Meta frame reception relies on the hardware keeping its promise that it will send no other traffic towards the CPU port between a link-local frame and a meta frame. Otherwise there is no other way to associate the meta frame with the link-local frame it's holding a timestamp of. The receive function is made stateful, and buffers a timestampable frame until its meta frame arrives, then merges the two, drops the meta and releases the link-local frame up the stack.
Unified · Split
| @@ -1600,6 +1600,14 @@ static int sja1105_setup(struct dsa_switch *ds) · 1599 unmodified lines | |||
| 1600 | return sja1105_setup_8021q_tagging(ds, true); | 1600 | return sja1105_setup_8021q_tagging(ds, true); |
| 1601 | } | 1601 | } |
| 1602 | 1602 | ||
| 1603 | static void sja1105_teardown(struct dsa_switch *ds) | ||
| 1604 | { | ||
| 1605 | struct sja1105_private *priv = ds->priv; | ||
| 1606 | |||
| 1607 | cancel_work_sync(&priv->tagger_data.rxtstamp_work); | ||
| 1608 | skb_queue_purge(&priv->tagger_data.skb_rxtstamp_queue); | ||
| 1609 | } | ||
| 1610 | |||
| 1603 | static int sja1105_mgmt_xmit(struct dsa_switch *ds, int port, int slot, | 1611 | static int sja1105_mgmt_xmit(struct dsa_switch *ds, int port, int slot, |
| 1604 | struct sk_buff *skb, bool takets) | 1612 | struct sk_buff *skb, bool takets) |
| 1605 | { | 1613 | { |
| @@ -1747,6 +1755,57 @@ static int sja1105_set_ageing_time(struct dsa_switch *ds, · 141 unmodified lines | |||
| 1747 | return sja1105_static_config_reload(priv); | 1755 | return sja1105_static_config_reload(priv); |
| 1748 | } | 1756 | } |
| 1749 | 1757 | ||
| 1758 | #define to_tagger(d) \ | ||
| 1759 | container_of((d), struct sja1105_tagger_data, rxtstamp_work) | ||
| 1760 | #define to_sja1105(d) \ | ||
| 1761 | container_of((d), struct sja1105_private, tagger_data) | ||
| 1762 | |||
| 1763 | static void sja1105_rxtstamp_work(struct work_struct *work) | ||
| 1764 | { | ||
| 1765 | struct sja1105_tagger_data *data = to_tagger(work); | ||
| 1766 | struct sja1105_private *priv = to_sja1105(data); | ||
| 1767 | struct sk_buff *skb; | ||
| 1768 | u64 now; | ||
| 1769 | |||
| 1770 | mutex_lock(&priv->ptp_lock); | ||
| 1771 | |||
| 1772 | now = priv->tstamp_cc.read(&priv->tstamp_cc); | ||
| 1773 | |||
| 1774 | while ((skb = skb_dequeue(&data->skb_rxtstamp_queue)) != NULL) { | ||
| 1775 | struct skb_shared_hwtstamps *shwt = skb_hwtstamps(skb); | ||
| 1776 | u64 ts; | ||
| 1777 | |||
| 1778 | *shwt = (struct skb_shared_hwtstamps) {0}; | ||
| 1779 | |||
| 1780 | ts = SJA1105_SKB_CB(skb)->meta_tstamp; | ||
| 1781 | ts = sja1105_tstamp_reconstruct(priv, now, ts); | ||
| 1782 | ts = timecounter_cyc2time(&priv->tstamp_tc, ts); | ||
| 1783 | |||
| 1784 | shwt->hwtstamp = ns_to_ktime(ts); | ||
| 1785 | netif_rx_ni(skb); | ||
| 1786 | } | ||
| 1787 | |||
| 1788 | mutex_unlock(&priv->ptp_lock); | ||
| 1789 | } | ||
| 1790 | |||
| 1791 | /* Called from dsa_skb_defer_rx_timestamp */ | ||
| 1792 | bool sja1105_port_rxtstamp(struct dsa_switch *ds, int port, | ||
| 1793 | struct sk_buff *skb, unsigned int type) | ||
| 1794 | { | ||
| 1795 | struct sja1105_private *priv = ds->priv; | ||
| 1796 | struct sja1105_tagger_data *data = &priv->tagger_data; | ||
| 1797 | |||
| 1798 | if (!data->hwts_rx_en) | ||
| 1799 | return false; | ||
| 1800 | |||
| 1801 | /* We need to read the full PTP clock to reconstruct the Rx | ||
| 1802 | * timestamp. For that we need a sleepable context. | ||
| 1803 | */ | ||
| 1804 | skb_queue_tail(&data->skb_rxtstamp_queue, skb); | ||
| 1805 | schedule_work(&data->rxtstamp_work); | ||
| 1806 | return true; | ||
| 1807 | } | ||
| 1808 | |||
| 1750 | /* Called from dsa_skb_tx_timestamp. This callback is just to make DSA clone | 1809 | /* Called from dsa_skb_tx_timestamp. This callback is just to make DSA clone |
| 1751 | * the skb and have it available in DSA_SKB_CB in the .port_deferred_xmit | 1810 | * the skb and have it available in DSA_SKB_CB in the .port_deferred_xmit |
| 1752 | * callback, where we will timestamp it synchronously. | 1811 | * callback, where we will timestamp it synchronously. |
| @@ -1766,6 +1825,7 @@ bool sja1105_port_txtstamp(struct dsa_switch *ds, int port, · 13 unmodified lines | |||
| 1766 | static const struct dsa_switch_ops sja1105_switch_ops = { | 1825 | static const struct dsa_switch_ops sja1105_switch_ops = { |
| 1767 | .get_tag_protocol = sja1105_get_tag_protocol, | 1826 | .get_tag_protocol = sja1105_get_tag_protocol, |
| 1768 | .setup = sja1105_setup, | 1827 | .setup = sja1105_setup, |
| 1828 | .teardown = sja1105_teardown, | ||
| 1769 | .set_ageing_time = sja1105_set_ageing_time, | 1829 | .set_ageing_time = sja1105_set_ageing_time, |
| 1770 | .phylink_validate = sja1105_phylink_validate, | 1830 | .phylink_validate = sja1105_phylink_validate, |
| 1771 | .phylink_mac_config = sja1105_mac_config, | 1831 | .phylink_mac_config = sja1105_mac_config, |
| @@ -1787,6 +1847,7 @@ static const struct dsa_switch_ops sja1105_switch_ops = { · 15 unmodified lines | |||
| 1787 | .port_mdb_add = sja1105_mdb_add, | 1847 | .port_mdb_add = sja1105_mdb_add, |
| 1788 | .port_mdb_del = sja1105_mdb_del, | 1848 | .port_mdb_del = sja1105_mdb_del, |
| 1789 | .port_deferred_xmit = sja1105_port_deferred_xmit, | 1849 | .port_deferred_xmit = sja1105_port_deferred_xmit, |
| 1850 | .port_rxtstamp = sja1105_port_rxtstamp, | ||
| 1790 | .port_txtstamp = sja1105_port_txtstamp, | 1851 | .port_txtstamp = sja1105_port_txtstamp, |
| 1791 | }; | 1852 | }; |
| 1792 | 1853 | ||
| @@ -1885,6 +1946,7 @@ static int sja1105_probe(struct spi_device *spi) · 92 unmodified lines | |||
| 1885 | 1946 | ||
| 1886 | tagger_data = &priv->tagger_data; | 1947 | tagger_data = &priv->tagger_data; |
| 1887 | skb_queue_head_init(&tagger_data->skb_rxtstamp_queue); | 1948 | skb_queue_head_init(&tagger_data->skb_rxtstamp_queue); |
| 1949 | INIT_WORK(&tagger_data->rxtstamp_work, sja1105_rxtstamp_work); | ||
| 1888 | 1950 | ||
| 1889 | /* Connections between dsa_port and sja1105_port */ | 1951 | /* Connections between dsa_port and sja1105_port */ |
| 1890 | for (i = 0; i < SJA1105_NUM_PORTS; i++) { | 1952 | for (i = 0; i < SJA1105_NUM_PORTS; i++) { |
| @@ -45,6 +45,13 @@ struct sja1105_tagger_data { · 44 unmodified lines | |||
| 45 | bool hwts_rx_en; | 45 | bool hwts_rx_en; |
| 46 | }; | 46 | }; |
| 47 | 47 | ||
| 48 | struct sja1105_skb_cb { | ||
| 49 | u32 meta_tstamp; | ||
| 50 | }; | ||
| 51 | |||
| 52 | #define SJA1105_SKB_CB(skb) \ | ||
| 53 | ((struct sja1105_skb_cb *)DSA_SKB_CB_PRIV(skb)) | ||
| 54 | |||
| 48 | struct sja1105_port { | 55 | struct sja1105_port { |
| 49 | struct sja1105_tagger_data *data; | 56 | struct sja1105_tagger_data *data; |
| 50 | struct dsa_port *dp; | 57 | struct dsa_port *dp; |
| @@ -110,6 +110,124 @@ static struct sk_buff *sja1105_xmit(struct sk_buff *skb, · 109 unmodified lines | |||
| 110 | ((pcp << VLAN_PRIO_SHIFT) | tx_vid)); | 110 | ((pcp << VLAN_PRIO_SHIFT) | tx_vid)); |
| 111 | } | 111 | } |
| 112 | 112 | ||
| 113 | static void sja1105_transfer_meta(struct sk_buff *skb, | ||
| 114 | const struct sja1105_meta *meta) | ||
| 115 | { | ||
| 116 | struct ethhdr *hdr = eth_hdr(skb); | ||
| 117 | |||
| 118 | hdr->h_dest[3] = meta->dmac_byte_3; | ||
| 119 | hdr->h_dest[4] = meta->dmac_byte_4; | ||
| 120 | SJA1105_SKB_CB(skb)->meta_tstamp = meta->tstamp; | ||
| 121 | } | ||
| 122 | |||
| 123 | /* This is a simple state machine which follows the hardware mechanism of | ||
| 124 | * generating RX timestamps: | ||
| 125 | * | ||
| 126 | * After each timestampable skb (all traffic for which send_meta1 and | ||
| 127 | * send_meta0 is true, aka all MAC-filtered link-local traffic) a meta frame | ||
| 128 | * containing a partial timestamp is immediately generated by the switch and | ||
| 129 | * sent as a follow-up to the link-local frame on the CPU port. | ||
| 130 | * | ||
| 131 | * The meta frames have no unique identifier (such as sequence number) by which | ||
| 132 | * one may pair them to the correct timestampable frame. | ||
| 133 | * Instead, the switch has internal logic that ensures no frames are sent on | ||
| 134 | * the CPU port between a link-local timestampable frame and its corresponding | ||
| 135 | * meta follow-up. It also ensures strict ordering between ports (lower ports | ||
| 136 | * have higher priority towards the CPU port). For this reason, a per-port | ||
| 137 | * data structure is not needed/desirable. | ||
| 138 | * | ||
| 139 | * This function pairs the link-local frame with its partial timestamp from the | ||
| 140 | * meta follow-up frame. The full timestamp will be reconstructed later in a | ||
| 141 | * work queue. | ||
| 142 | */ | ||
| 143 | static struct sk_buff | ||
| 144 | *sja1105_rcv_meta_state_machine(struct sk_buff *skb, | ||
| 145 | struct sja1105_meta *meta, | ||
| 146 | bool is_link_local, | ||
| 147 | bool is_meta) | ||
| 148 | { | ||
| 149 | struct sja1105_port *sp; | ||
| 150 | struct dsa_port *dp; | ||
| 151 | |||
| 152 | dp = dsa_slave_to_port(skb->dev); | ||
| 153 | sp = dp->priv; | ||
| 154 | |||
| 155 | /* Step 1: A timestampable frame was received. | ||
| 156 | * Buffer it until we get its meta frame. | ||
| 157 | */ | ||
| 158 | if (is_link_local && sp->data->hwts_rx_en) { | ||
| 159 | spin_lock(&sp->data->meta_lock); | ||
| 160 | /* Was this a link-local frame instead of the meta | ||
| 161 | * that we were expecting? | ||
| 162 | */ | ||
| 163 | if (sp->data->stampable_skb) { | ||
| 164 | dev_err_ratelimited(dp->ds->dev, | ||
| 165 | "Expected meta frame, is %12llx " | ||
| 166 | "in the DSA master multicast filter?\n", | ||
| 167 | SJA1105_META_DMAC); | ||
| 168 | } | ||
| 169 | |||
| 170 | /* Hold a reference to avoid dsa_switch_rcv | ||
| 171 | * from freeing the skb. | ||
| 172 | */ | ||
| 173 | sp->data->stampable_skb = skb_get(skb); | ||
| 174 | spin_unlock(&sp->data->meta_lock); | ||
| 175 | |||
| 176 | /* Tell DSA we got nothing */ | ||
| 177 | return NULL; | ||
| 178 | |||
| 179 | /* Step 2: The meta frame arrived. | ||
| 180 | * Time to take the stampable skb out of the closet, annotate it | ||
| 181 | * with the partial timestamp, and pretend that we received it | ||
| 182 | * just now (basically masquerade the buffered frame as the meta | ||
| 183 | * frame, which serves no further purpose). | ||
| 184 | */ | ||
| 185 | } else if (is_meta) { | ||
| 186 | struct sk_buff *stampable_skb; | ||
| 187 | |||
| 188 | spin_lock(&sp->data->meta_lock); | ||
| 189 | |||
| 190 | stampable_skb = sp->data->stampable_skb; | ||
| 191 | sp->data->stampable_skb = NULL; | ||
| 192 | |||
| 193 | /* Was this a meta frame instead of the link-local | ||
| 194 | * that we were expecting? | ||
| 195 | */ | ||
| 196 | if (!stampable_skb) { | ||
| 197 | dev_err_ratelimited(dp->ds->dev, | ||
| 198 | "Unexpected meta frame\n"); | ||
| 199 | spin_unlock(&sp->data->meta_lock); | ||
| 200 | return NULL; | ||
| 201 | } | ||
| 202 | |||
| 203 | if (stampable_skb->dev != skb->dev) { | ||
| 204 | dev_err_ratelimited(dp->ds->dev, | ||
| 205 | "Meta frame on wrong port\n"); | ||
| 206 | spin_unlock(&sp->data->meta_lock); | ||
| 207 | return NULL; | ||
| 208 | } | ||
| 209 | |||
| 210 | /* Free the meta frame and give DSA the buffered stampable_skb | ||
| 211 | * for further processing up the network stack. | ||
| 212 | */ | ||
| 213 | kfree_skb(skb); | ||
| 214 | |||
| 215 | skb = skb_copy(stampable_skb, GFP_ATOMIC); | ||
| 216 | if (!skb) { | ||
| 217 | dev_err_ratelimited(dp->ds->dev, | ||
| 218 | "Failed to copy stampable skb\n"); | ||
| 219 | return NULL; | ||
| 220 | } | ||
| 221 | sja1105_transfer_meta(skb, meta); | ||
| 222 | /* The cached copy will be freed now */ | ||
| 223 | skb_unref(stampable_skb); | ||
| 224 | |||
| 225 | spin_unlock(&sp->data->meta_lock); | ||
| 226 | } | ||
| 227 | |||
| 228 | return skb; | ||
| 229 | } | ||
| 230 | |||
| 113 | static struct sk_buff *sja1105_rcv(struct sk_buff *skb, | 231 | static struct sk_buff *sja1105_rcv(struct sk_buff *skb, |
| 114 | struct net_device *netdev, | 232 | struct net_device *netdev, |
| 115 | struct packet_type *pt) | 233 | struct packet_type *pt) |
| @@ -167,7 +285,8 @@ static struct sk_buff *sja1105_rcv(struct sk_buff *skb, · 51 unmodified lines | |||
| 167 | if (is_tagged) | 285 | if (is_tagged) |
| 168 | skb = dsa_8021q_remove_header(skb); | 286 | skb = dsa_8021q_remove_header(skb); |
| 169 | 287 | ||
| 170 | return skb; | 288 | return sja1105_rcv_meta_state_machine(skb, &meta, is_link_local, |
| 289 | is_meta); | ||
| 171 | } | 290 | } |
| 172 | 291 | ||
| 173 | static struct dsa_device_ops sja1105_netdev_ops = { | 292 | static struct dsa_device_ops sja1105_netdev_ops = { |