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 | 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 | 1611 | static int sja1105_mgmt_xmit(struct dsa_switch *ds, int port, int slot, |
| 1604 | 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 | 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 | 1809 | /* Called from dsa_skb_tx_timestamp. This callback is just to make DSA clone |
| 1751 | 1810 | * the skb and have it available in DSA_SKB_CB in the .port_deferred_xmit |
| 1752 | 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 | 1825 | static const struct dsa_switch_ops sja1105_switch_ops = { |
| 1767 | 1826 | .get_tag_protocol = sja1105_get_tag_protocol, |
| 1768 | 1827 | .setup = sja1105_setup, |
| 1828 | + .teardown = sja1105_teardown, | |
| 1769 | 1829 | .set_ageing_time = sja1105_set_ageing_time, |
| 1770 | 1830 | .phylink_validate = sja1105_phylink_validate, |
| 1771 | 1831 | .phylink_mac_config = sja1105_mac_config, |
| @@ -1787,6 +1847,7 @@ static const struct dsa_switch_ops sja1105_switch_ops = { · 15 unmodified lines | ||
| 1787 | 1847 | .port_mdb_add = sja1105_mdb_add, |
| 1788 | 1848 | .port_mdb_del = sja1105_mdb_del, |
| 1789 | 1849 | .port_deferred_xmit = sja1105_port_deferred_xmit, |
| 1850 | + .port_rxtstamp = sja1105_port_rxtstamp, | |
| 1790 | 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 | 1947 | tagger_data = &priv->tagger_data; |
| 1887 | 1948 | skb_queue_head_init(&tagger_data->skb_rxtstamp_queue); |
| 1949 | + INIT_WORK(&tagger_data->rxtstamp_work, sja1105_rxtstamp_work); | |
| 1888 | 1950 | |
| 1889 | 1951 | /* Connections between dsa_port and sja1105_port */ |
| 1890 | 1952 | for (i = 0; i < SJA1105_NUM_PORTS; i++) { |
| @@ -45,6 +45,13 @@ struct sja1105_tagger_data { · 44 unmodified lines | ||
| 45 | 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 | 55 | struct sja1105_port { |
| 49 | 56 | struct sja1105_tagger_data *data; |
| 50 | 57 | struct dsa_port *dp; |
| @@ -110,6 +110,124 @@ static struct sk_buff *sja1105_xmit(struct sk_buff *skb, · 109 unmodified lines | ||
| 110 | 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 | 231 | static struct sk_buff *sja1105_rcv(struct sk_buff *skb, |
| 114 | 232 | struct net_device *netdev, |
| 115 | 233 | struct packet_type *pt) |
| @@ -167,7 +285,8 @@ static struct sk_buff *sja1105_rcv(struct sk_buff *skb, · 51 unmodified lines | ||
| 167 | 285 | if (is_tagged) |
| 168 | 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 | 292 | static struct dsa_device_ops sja1105_netdev_ops = { |