]>
Commit | Line | Data |
---|---|---|
227d07a0 VO |
1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* Copyright (c) 2019, Vladimir Oltean <olteanv@gmail.com> | |
3 | */ | |
4 | #include <linux/if_vlan.h> | |
5 | #include <linux/dsa/sja1105.h> | |
6 | #include <linux/dsa/8021q.h> | |
28da0555 | 7 | #include <linux/skbuff.h> |
227d07a0 VO |
8 | #include <linux/packing.h> |
9 | #include "dsa_priv.h" | |
10 | ||
4913b8eb VO |
11 | /* Is this a TX or an RX header? */ |
12 | #define SJA1110_HEADER_HOST_TO_SWITCH BIT(15) | |
13 | ||
14 | /* RX header */ | |
15 | #define SJA1110_RX_HEADER_IS_METADATA BIT(14) | |
16 | #define SJA1110_RX_HEADER_HOST_ONLY BIT(13) | |
17 | #define SJA1110_RX_HEADER_HAS_TRAILER BIT(12) | |
18 | ||
19 | /* Trap-to-host format (no trailer present) */ | |
20 | #define SJA1110_RX_HEADER_SRC_PORT(x) (((x) & GENMASK(7, 4)) >> 4) | |
21 | #define SJA1110_RX_HEADER_SWITCH_ID(x) ((x) & GENMASK(3, 0)) | |
22 | ||
23 | /* Timestamp format (trailer present) */ | |
24 | #define SJA1110_RX_HEADER_TRAILER_POS(x) ((x) & GENMASK(11, 0)) | |
25 | ||
26 | #define SJA1110_RX_TRAILER_SWITCH_ID(x) (((x) & GENMASK(7, 4)) >> 4) | |
27 | #define SJA1110_RX_TRAILER_SRC_PORT(x) ((x) & GENMASK(3, 0)) | |
28 | ||
566b18c8 VO |
29 | /* Meta frame format (for 2-step TX timestamps) */ |
30 | #define SJA1110_RX_HEADER_N_TS(x) (((x) & GENMASK(8, 4)) >> 4) | |
31 | ||
4913b8eb VO |
32 | /* TX header */ |
33 | #define SJA1110_TX_HEADER_UPDATE_TC BIT(14) | |
34 | #define SJA1110_TX_HEADER_TAKE_TS BIT(13) | |
35 | #define SJA1110_TX_HEADER_TAKE_TS_CASC BIT(12) | |
36 | #define SJA1110_TX_HEADER_HAS_TRAILER BIT(11) | |
37 | ||
38 | /* Only valid if SJA1110_TX_HEADER_HAS_TRAILER is false */ | |
39 | #define SJA1110_TX_HEADER_PRIO(x) (((x) << 7) & GENMASK(10, 7)) | |
40 | #define SJA1110_TX_HEADER_TSTAMP_ID(x) ((x) & GENMASK(7, 0)) | |
41 | ||
42 | /* Only valid if SJA1110_TX_HEADER_HAS_TRAILER is true */ | |
43 | #define SJA1110_TX_HEADER_TRAILER_POS(x) ((x) & GENMASK(10, 0)) | |
44 | ||
45 | #define SJA1110_TX_TRAILER_TSTAMP_ID(x) (((x) << 24) & GENMASK(31, 24)) | |
46 | #define SJA1110_TX_TRAILER_PRIO(x) (((x) << 21) & GENMASK(23, 21)) | |
47 | #define SJA1110_TX_TRAILER_SWITCHID(x) (((x) << 12) & GENMASK(15, 12)) | |
48 | #define SJA1110_TX_TRAILER_DESTPORTS(x) (((x) << 1) & GENMASK(11, 1)) | |
49 | ||
566b18c8 VO |
50 | #define SJA1110_META_TSTAMP_SIZE 10 |
51 | ||
4913b8eb VO |
52 | #define SJA1110_HEADER_LEN 4 |
53 | #define SJA1110_RX_TRAILER_LEN 13 | |
54 | #define SJA1110_TX_TRAILER_LEN 4 | |
55 | #define SJA1110_MAX_PADDING_LEN 15 | |
56 | ||
28da0555 VO |
57 | enum sja1110_meta_tstamp { |
58 | SJA1110_META_TSTAMP_TX = 0, | |
59 | SJA1110_META_TSTAMP_RX = 1, | |
60 | }; | |
61 | ||
227d07a0 VO |
62 | /* Similar to is_link_local_ether_addr(hdr->h_dest) but also covers PTP */ |
63 | static inline bool sja1105_is_link_local(const struct sk_buff *skb) | |
64 | { | |
65 | const struct ethhdr *hdr = eth_hdr(skb); | |
66 | u64 dmac = ether_addr_to_u64(hdr->h_dest); | |
67 | ||
79fa7061 VO |
68 | if (ntohs(hdr->h_proto) == ETH_P_SJA1105_META) |
69 | return false; | |
227d07a0 VO |
70 | if ((dmac & SJA1105_LINKLOCAL_FILTER_A_MASK) == |
71 | SJA1105_LINKLOCAL_FILTER_A) | |
72 | return true; | |
73 | if ((dmac & SJA1105_LINKLOCAL_FILTER_B_MASK) == | |
74 | SJA1105_LINKLOCAL_FILTER_B) | |
75 | return true; | |
76 | return false; | |
77 | } | |
78 | ||
e53e18a6 VO |
79 | struct sja1105_meta { |
80 | u64 tstamp; | |
81 | u64 dmac_byte_4; | |
82 | u64 dmac_byte_3; | |
83 | u64 source_port; | |
84 | u64 switch_id; | |
85 | }; | |
86 | ||
87 | static void sja1105_meta_unpack(const struct sk_buff *skb, | |
88 | struct sja1105_meta *meta) | |
89 | { | |
90 | u8 *buf = skb_mac_header(skb) + ETH_HLEN; | |
91 | ||
92 | /* UM10944.pdf section 4.2.17 AVB Parameters: | |
93 | * Structure of the meta-data follow-up frame. | |
94 | * It is in network byte order, so there are no quirks | |
95 | * while unpacking the meta frame. | |
96 | * | |
97 | * Also SJA1105 E/T only populates bits 23:0 of the timestamp | |
98 | * whereas P/Q/R/S does 32 bits. Since the structure is the | |
99 | * same and the E/T puts zeroes in the high-order byte, use | |
100 | * a unified unpacking command for both device series. | |
101 | */ | |
102 | packing(buf, &meta->tstamp, 31, 0, 4, UNPACK, 0); | |
103 | packing(buf + 4, &meta->dmac_byte_4, 7, 0, 1, UNPACK, 0); | |
104 | packing(buf + 5, &meta->dmac_byte_3, 7, 0, 1, UNPACK, 0); | |
105 | packing(buf + 6, &meta->source_port, 7, 0, 1, UNPACK, 0); | |
106 | packing(buf + 7, &meta->switch_id, 7, 0, 1, UNPACK, 0); | |
107 | } | |
108 | ||
d3f9b90b VO |
109 | static inline bool sja1105_is_meta_frame(const struct sk_buff *skb) |
110 | { | |
111 | const struct ethhdr *hdr = eth_hdr(skb); | |
112 | u64 smac = ether_addr_to_u64(hdr->h_source); | |
113 | u64 dmac = ether_addr_to_u64(hdr->h_dest); | |
114 | ||
115 | if (smac != SJA1105_META_SMAC) | |
116 | return false; | |
117 | if (dmac != SJA1105_META_DMAC) | |
118 | return false; | |
119 | if (ntohs(hdr->h_proto) != ETH_P_SJA1105_META) | |
120 | return false; | |
121 | return true; | |
122 | } | |
123 | ||
a68578c2 | 124 | /* Calls sja1105_port_deferred_xmit in sja1105_main.c */ |
994d2cbb | 125 | static struct sk_buff *sja1105_defer_xmit(struct dsa_port *dp, |
a68578c2 VO |
126 | struct sk_buff *skb) |
127 | { | |
994d2cbb VO |
128 | struct sja1105_port *sp = dp->priv; |
129 | ||
130 | if (!dsa_port_is_sja1105(dp)) | |
131 | return skb; | |
132 | ||
a68578c2 VO |
133 | /* Increase refcount so the kfree_skb in dsa_slave_xmit |
134 | * won't really free the packet. | |
135 | */ | |
136 | skb_queue_tail(&sp->xmit_queue, skb_get(skb)); | |
137 | kthread_queue_work(sp->xmit_worker, &sp->xmit_work); | |
138 | ||
139 | return NULL; | |
140 | } | |
141 | ||
8ded9160 VO |
142 | /* Send VLAN tags with a TPID that blends in with whatever VLAN protocol a |
143 | * bridge spanning ports of this switch might have. | |
144 | */ | |
994d2cbb | 145 | static u16 sja1105_xmit_tpid(struct dsa_port *dp) |
38b5beea | 146 | { |
8ded9160 VO |
147 | struct dsa_switch *ds = dp->ds; |
148 | struct dsa_port *other_dp; | |
149 | u16 proto; | |
150 | ||
151 | /* Since VLAN awareness is global, then if this port is VLAN-unaware, | |
152 | * all ports are. Use the VLAN-unaware TPID used for tag_8021q. | |
153 | */ | |
154 | if (!dsa_port_is_vlan_filtering(dp)) | |
155 | return ETH_P_SJA1105; | |
156 | ||
157 | /* Port is VLAN-aware, so there is a bridge somewhere (a single one, | |
158 | * we're sure about that). It may not be on this port though, so we | |
159 | * need to find it. | |
160 | */ | |
161 | list_for_each_entry(other_dp, &ds->dst->ports, list) { | |
162 | if (other_dp->ds != ds) | |
163 | continue; | |
164 | ||
165 | if (!other_dp->bridge_dev) | |
166 | continue; | |
167 | ||
168 | /* Error is returned only if CONFIG_BRIDGE_VLAN_FILTERING, | |
169 | * which seems pointless to handle, as our port cannot become | |
170 | * VLAN-aware in that case. | |
171 | */ | |
172 | br_vlan_get_proto(other_dp->bridge_dev, &proto); | |
173 | ||
174 | return proto; | |
175 | } | |
994d2cbb | 176 | |
8ded9160 | 177 | WARN_ONCE(1, "Port is VLAN-aware but cannot find associated bridge!\n"); |
994d2cbb | 178 | |
8ded9160 | 179 | return ETH_P_SJA1105; |
38b5beea VO |
180 | } |
181 | ||
b6ad86e6 VO |
182 | static struct sk_buff *sja1105_imprecise_xmit(struct sk_buff *skb, |
183 | struct net_device *netdev) | |
184 | { | |
185 | struct dsa_port *dp = dsa_slave_to_port(netdev); | |
186 | struct net_device *br = dp->bridge_dev; | |
187 | u16 tx_vid; | |
188 | ||
189 | /* If the port is under a VLAN-aware bridge, just slide the | |
190 | * VLAN-tagged packet into the FDB and hope for the best. | |
191 | * This works because we support a single VLAN-aware bridge | |
192 | * across the entire dst, and its VLANs cannot be shared with | |
193 | * any standalone port. | |
194 | */ | |
195 | if (br_vlan_enabled(br)) | |
196 | return skb; | |
197 | ||
198 | /* If the port is under a VLAN-unaware bridge, use an imprecise | |
199 | * TX VLAN that targets the bridge's entire broadcast domain, | |
200 | * instead of just the specific port. | |
201 | */ | |
202 | tx_vid = dsa_8021q_bridge_tx_fwd_offload_vid(dp->bridge_num); | |
203 | ||
994d2cbb | 204 | return dsa_8021q_xmit(skb, netdev, sja1105_xmit_tpid(dp), tx_vid); |
b6ad86e6 VO |
205 | } |
206 | ||
b0b8c67e VO |
207 | /* Transform untagged control packets into pvid-tagged control packets so that |
208 | * all packets sent by this tagger are VLAN-tagged and we can configure the | |
209 | * switch to drop untagged packets coming from the DSA master. | |
210 | */ | |
211 | static struct sk_buff *sja1105_pvid_tag_control_pkt(struct dsa_port *dp, | |
212 | struct sk_buff *skb, u8 pcp) | |
213 | { | |
214 | __be16 xmit_tpid = htons(sja1105_xmit_tpid(dp)); | |
215 | struct vlan_ethhdr *hdr; | |
216 | ||
217 | /* If VLAN tag is in hwaccel area, move it to the payload | |
218 | * to deal with both cases uniformly and to ensure that | |
219 | * the VLANs are added in the right order. | |
220 | */ | |
221 | if (unlikely(skb_vlan_tag_present(skb))) { | |
222 | skb = __vlan_hwaccel_push_inside(skb); | |
223 | if (!skb) | |
224 | return NULL; | |
225 | } | |
226 | ||
227 | hdr = (struct vlan_ethhdr *)skb_mac_header(skb); | |
228 | ||
229 | /* If skb is already VLAN-tagged, leave that VLAN ID in place */ | |
230 | if (hdr->h_vlan_proto == xmit_tpid) | |
231 | return skb; | |
232 | ||
233 | return vlan_insert_tag(skb, xmit_tpid, (pcp << VLAN_PRIO_SHIFT) | | |
234 | SJA1105_DEFAULT_VLAN); | |
235 | } | |
236 | ||
227d07a0 VO |
237 | static struct sk_buff *sja1105_xmit(struct sk_buff *skb, |
238 | struct net_device *netdev) | |
239 | { | |
240 | struct dsa_port *dp = dsa_slave_to_port(netdev); | |
2821d50f | 241 | u16 tx_vid = dsa_8021q_tx_vid(dp->ds, dp->index); |
5f06c63b VO |
242 | u16 queue_mapping = skb_get_queue_mapping(skb); |
243 | u8 pcp = netdev_txq_to_tc(netdev, queue_mapping); | |
227d07a0 | 244 | |
b6ad86e6 VO |
245 | if (skb->offload_fwd_mark) |
246 | return sja1105_imprecise_xmit(skb, netdev); | |
247 | ||
227d07a0 VO |
248 | /* Transmitting management traffic does not rely upon switch tagging, |
249 | * but instead SPI-installed management routes. Part 2 of this | |
250 | * is the .port_deferred_xmit driver callback. | |
251 | */ | |
b0b8c67e VO |
252 | if (unlikely(sja1105_is_link_local(skb))) { |
253 | skb = sja1105_pvid_tag_control_pkt(dp, skb, pcp); | |
254 | if (!skb) | |
255 | return NULL; | |
256 | ||
994d2cbb | 257 | return sja1105_defer_xmit(dp, skb); |
b0b8c67e | 258 | } |
227d07a0 | 259 | |
994d2cbb | 260 | return dsa_8021q_xmit(skb, netdev, sja1105_xmit_tpid(dp), |
227d07a0 VO |
261 | ((pcp << VLAN_PRIO_SHIFT) | tx_vid)); |
262 | } | |
263 | ||
4913b8eb VO |
264 | static struct sk_buff *sja1110_xmit(struct sk_buff *skb, |
265 | struct net_device *netdev) | |
266 | { | |
566b18c8 | 267 | struct sk_buff *clone = SJA1105_SKB_CB(skb)->clone; |
4913b8eb VO |
268 | struct dsa_port *dp = dsa_slave_to_port(netdev); |
269 | u16 tx_vid = dsa_8021q_tx_vid(dp->ds, dp->index); | |
270 | u16 queue_mapping = skb_get_queue_mapping(skb); | |
271 | u8 pcp = netdev_txq_to_tc(netdev, queue_mapping); | |
4913b8eb VO |
272 | __be32 *tx_trailer; |
273 | __be16 *tx_header; | |
274 | int trailer_pos; | |
275 | ||
b6ad86e6 VO |
276 | if (skb->offload_fwd_mark) |
277 | return sja1105_imprecise_xmit(skb, netdev); | |
278 | ||
4913b8eb VO |
279 | /* Transmitting control packets is done using in-band control |
280 | * extensions, while data packets are transmitted using | |
281 | * tag_8021q TX VLANs. | |
282 | */ | |
283 | if (likely(!sja1105_is_link_local(skb))) | |
994d2cbb | 284 | return dsa_8021q_xmit(skb, netdev, sja1105_xmit_tpid(dp), |
4913b8eb VO |
285 | ((pcp << VLAN_PRIO_SHIFT) | tx_vid)); |
286 | ||
b0b8c67e VO |
287 | skb = sja1105_pvid_tag_control_pkt(dp, skb, pcp); |
288 | if (!skb) | |
289 | return NULL; | |
290 | ||
4913b8eb VO |
291 | skb_push(skb, SJA1110_HEADER_LEN); |
292 | ||
6bef794d | 293 | dsa_alloc_etype_header(skb, SJA1110_HEADER_LEN); |
4913b8eb VO |
294 | |
295 | trailer_pos = skb->len; | |
296 | ||
a72808b6 | 297 | tx_header = dsa_etype_header_pos_tx(skb); |
4913b8eb VO |
298 | tx_trailer = skb_put(skb, SJA1110_TX_TRAILER_LEN); |
299 | ||
a72808b6 VO |
300 | tx_header[0] = htons(ETH_P_SJA1110); |
301 | tx_header[1] = htons(SJA1110_HEADER_HOST_TO_SWITCH | | |
302 | SJA1110_TX_HEADER_HAS_TRAILER | | |
303 | SJA1110_TX_HEADER_TRAILER_POS(trailer_pos)); | |
4913b8eb VO |
304 | *tx_trailer = cpu_to_be32(SJA1110_TX_TRAILER_PRIO(pcp) | |
305 | SJA1110_TX_TRAILER_SWITCHID(dp->ds->index) | | |
306 | SJA1110_TX_TRAILER_DESTPORTS(BIT(dp->index))); | |
566b18c8 VO |
307 | if (clone) { |
308 | u8 ts_id = SJA1105_SKB_CB(clone)->ts_id; | |
309 | ||
a72808b6 | 310 | tx_header[1] |= htons(SJA1110_TX_HEADER_TAKE_TS); |
566b18c8 VO |
311 | *tx_trailer |= cpu_to_be32(SJA1110_TX_TRAILER_TSTAMP_ID(ts_id)); |
312 | } | |
4913b8eb VO |
313 | |
314 | return skb; | |
315 | } | |
316 | ||
f3097be2 VO |
317 | static void sja1105_transfer_meta(struct sk_buff *skb, |
318 | const struct sja1105_meta *meta) | |
319 | { | |
320 | struct ethhdr *hdr = eth_hdr(skb); | |
321 | ||
322 | hdr->h_dest[3] = meta->dmac_byte_3; | |
323 | hdr->h_dest[4] = meta->dmac_byte_4; | |
617ef8d9 | 324 | SJA1105_SKB_CB(skb)->tstamp = meta->tstamp; |
f3097be2 VO |
325 | } |
326 | ||
327 | /* This is a simple state machine which follows the hardware mechanism of | |
328 | * generating RX timestamps: | |
329 | * | |
330 | * After each timestampable skb (all traffic for which send_meta1 and | |
331 | * send_meta0 is true, aka all MAC-filtered link-local traffic) a meta frame | |
332 | * containing a partial timestamp is immediately generated by the switch and | |
333 | * sent as a follow-up to the link-local frame on the CPU port. | |
334 | * | |
335 | * The meta frames have no unique identifier (such as sequence number) by which | |
336 | * one may pair them to the correct timestampable frame. | |
337 | * Instead, the switch has internal logic that ensures no frames are sent on | |
338 | * the CPU port between a link-local timestampable frame and its corresponding | |
339 | * meta follow-up. It also ensures strict ordering between ports (lower ports | |
340 | * have higher priority towards the CPU port). For this reason, a per-port | |
341 | * data structure is not needed/desirable. | |
342 | * | |
343 | * This function pairs the link-local frame with its partial timestamp from the | |
344 | * meta follow-up frame. The full timestamp will be reconstructed later in a | |
345 | * work queue. | |
346 | */ | |
347 | static struct sk_buff | |
348 | *sja1105_rcv_meta_state_machine(struct sk_buff *skb, | |
349 | struct sja1105_meta *meta, | |
350 | bool is_link_local, | |
351 | bool is_meta) | |
352 | { | |
f3097be2 VO |
353 | /* Step 1: A timestampable frame was received. |
354 | * Buffer it until we get its meta frame. | |
355 | */ | |
3e8db7e5 | 356 | if (is_link_local) { |
994d2cbb VO |
357 | struct dsa_port *dp = dsa_slave_to_port(skb->dev); |
358 | struct sja1105_port *sp = dp->priv; | |
359 | ||
360 | if (unlikely(!dsa_port_is_sja1105(dp))) | |
361 | return skb; | |
362 | ||
3e8db7e5 VO |
363 | if (!test_bit(SJA1105_HWTS_RX_EN, &sp->data->state)) |
364 | /* Do normal processing. */ | |
365 | return skb; | |
366 | ||
f3097be2 VO |
367 | spin_lock(&sp->data->meta_lock); |
368 | /* Was this a link-local frame instead of the meta | |
369 | * that we were expecting? | |
370 | */ | |
371 | if (sp->data->stampable_skb) { | |
372 | dev_err_ratelimited(dp->ds->dev, | |
373 | "Expected meta frame, is %12llx " | |
374 | "in the DSA master multicast filter?\n", | |
375 | SJA1105_META_DMAC); | |
93fa8587 | 376 | kfree_skb(sp->data->stampable_skb); |
f3097be2 VO |
377 | } |
378 | ||
379 | /* Hold a reference to avoid dsa_switch_rcv | |
380 | * from freeing the skb. | |
381 | */ | |
382 | sp->data->stampable_skb = skb_get(skb); | |
383 | spin_unlock(&sp->data->meta_lock); | |
384 | ||
385 | /* Tell DSA we got nothing */ | |
386 | return NULL; | |
387 | ||
388 | /* Step 2: The meta frame arrived. | |
389 | * Time to take the stampable skb out of the closet, annotate it | |
390 | * with the partial timestamp, and pretend that we received it | |
391 | * just now (basically masquerade the buffered frame as the meta | |
392 | * frame, which serves no further purpose). | |
393 | */ | |
394 | } else if (is_meta) { | |
994d2cbb VO |
395 | struct dsa_port *dp = dsa_slave_to_port(skb->dev); |
396 | struct sja1105_port *sp = dp->priv; | |
f3097be2 VO |
397 | struct sk_buff *stampable_skb; |
398 | ||
994d2cbb VO |
399 | if (unlikely(!dsa_port_is_sja1105(dp))) |
400 | return skb; | |
401 | ||
3e8db7e5 VO |
402 | /* Drop the meta frame if we're not in the right state |
403 | * to process it. | |
404 | */ | |
405 | if (!test_bit(SJA1105_HWTS_RX_EN, &sp->data->state)) | |
406 | return NULL; | |
407 | ||
f3097be2 VO |
408 | spin_lock(&sp->data->meta_lock); |
409 | ||
410 | stampable_skb = sp->data->stampable_skb; | |
411 | sp->data->stampable_skb = NULL; | |
412 | ||
413 | /* Was this a meta frame instead of the link-local | |
414 | * that we were expecting? | |
415 | */ | |
416 | if (!stampable_skb) { | |
417 | dev_err_ratelimited(dp->ds->dev, | |
418 | "Unexpected meta frame\n"); | |
419 | spin_unlock(&sp->data->meta_lock); | |
420 | return NULL; | |
421 | } | |
422 | ||
423 | if (stampable_skb->dev != skb->dev) { | |
424 | dev_err_ratelimited(dp->ds->dev, | |
425 | "Meta frame on wrong port\n"); | |
426 | spin_unlock(&sp->data->meta_lock); | |
427 | return NULL; | |
428 | } | |
429 | ||
430 | /* Free the meta frame and give DSA the buffered stampable_skb | |
431 | * for further processing up the network stack. | |
432 | */ | |
433 | kfree_skb(skb); | |
f163fed2 | 434 | skb = stampable_skb; |
f3097be2 | 435 | sja1105_transfer_meta(skb, meta); |
f3097be2 VO |
436 | |
437 | spin_unlock(&sp->data->meta_lock); | |
438 | } | |
439 | ||
440 | return skb; | |
441 | } | |
442 | ||
233697b3 VO |
443 | static bool sja1105_skb_has_tag_8021q(const struct sk_buff *skb) |
444 | { | |
445 | u16 tpid = ntohs(eth_hdr(skb)->h_proto); | |
446 | ||
447 | return tpid == ETH_P_SJA1105 || tpid == ETH_P_8021Q || | |
448 | skb_vlan_tag_present(skb); | |
449 | } | |
450 | ||
4913b8eb VO |
451 | static bool sja1110_skb_has_inband_control_extension(const struct sk_buff *skb) |
452 | { | |
453 | return ntohs(eth_hdr(skb)->h_proto) == ETH_P_SJA1110; | |
454 | } | |
455 | ||
04a17583 VO |
456 | /* If the VLAN in the packet is a tag_8021q one, set @source_port and |
457 | * @switch_id and strip the header. Otherwise set @vid and keep it in the | |
458 | * packet. | |
884be12f | 459 | */ |
04a17583 | 460 | static void sja1105_vlan_rcv(struct sk_buff *skb, int *source_port, |
884be12f VO |
461 | int *switch_id, u16 *vid) |
462 | { | |
463 | struct vlan_ethhdr *hdr = (struct vlan_ethhdr *)skb_mac_header(skb); | |
464 | u16 vlan_tci; | |
465 | ||
466 | if (skb_vlan_tag_present(skb)) | |
467 | vlan_tci = skb_vlan_tag_get(skb); | |
468 | else | |
469 | vlan_tci = ntohs(hdr->h_vlan_TCI); | |
470 | ||
04a17583 VO |
471 | if (vid_is_dsa_8021q_rxvlan(vlan_tci & VLAN_VID_MASK)) |
472 | return dsa_8021q_rcv(skb, source_port, switch_id); | |
884be12f VO |
473 | |
474 | /* Try our best with imprecise RX */ | |
475 | *vid = vlan_tci & VLAN_VID_MASK; | |
884be12f VO |
476 | } |
477 | ||
227d07a0 | 478 | static struct sk_buff *sja1105_rcv(struct sk_buff *skb, |
29a097b7 | 479 | struct net_device *netdev) |
227d07a0 | 480 | { |
884be12f | 481 | int source_port = -1, switch_id = -1; |
e53e18a6 | 482 | struct sja1105_meta meta = {0}; |
e80f40cb | 483 | struct ethhdr *hdr; |
42824463 | 484 | bool is_link_local; |
e53e18a6 | 485 | bool is_meta; |
884be12f | 486 | u16 vid; |
227d07a0 | 487 | |
e80f40cb | 488 | hdr = eth_hdr(skb); |
42824463 | 489 | is_link_local = sja1105_is_link_local(skb); |
e53e18a6 | 490 | is_meta = sja1105_is_meta_frame(skb); |
227d07a0 | 491 | |
233697b3 | 492 | if (sja1105_skb_has_tag_8021q(skb)) { |
42824463 | 493 | /* Normal traffic path. */ |
04a17583 | 494 | sja1105_vlan_rcv(skb, &source_port, &switch_id, &vid); |
42824463 | 495 | } else if (is_link_local) { |
227d07a0 VO |
496 | /* Management traffic path. Switch embeds the switch ID and |
497 | * port ID into bytes of the destination MAC, courtesy of | |
498 | * the incl_srcpt options. | |
499 | */ | |
500 | source_port = hdr->h_dest[3]; | |
501 | switch_id = hdr->h_dest[4]; | |
502 | /* Clear the DMAC bytes that were mangled by the switch */ | |
503 | hdr->h_dest[3] = 0; | |
504 | hdr->h_dest[4] = 0; | |
e53e18a6 VO |
505 | } else if (is_meta) { |
506 | sja1105_meta_unpack(skb, &meta); | |
507 | source_port = meta.source_port; | |
508 | switch_id = meta.switch_id; | |
227d07a0 | 509 | } else { |
42824463 | 510 | return NULL; |
227d07a0 VO |
511 | } |
512 | ||
04a17583 | 513 | if (source_port == -1 || switch_id == -1) |
884be12f VO |
514 | skb->dev = dsa_find_designated_bridge_port_by_vid(netdev, vid); |
515 | else | |
516 | skb->dev = dsa_master_find_slave(netdev, switch_id, source_port); | |
227d07a0 VO |
517 | if (!skb->dev) { |
518 | netdev_warn(netdev, "Couldn't decode source port\n"); | |
519 | return NULL; | |
520 | } | |
521 | ||
bea79078 VO |
522 | if (!is_link_local) |
523 | dsa_default_offload_fwd_mark(skb); | |
524 | ||
f3097be2 VO |
525 | return sja1105_rcv_meta_state_machine(skb, &meta, is_link_local, |
526 | is_meta); | |
227d07a0 VO |
527 | } |
528 | ||
28da0555 VO |
529 | static void sja1110_process_meta_tstamp(struct dsa_switch *ds, int port, |
530 | u8 ts_id, enum sja1110_meta_tstamp dir, | |
531 | u64 tstamp) | |
532 | { | |
533 | struct sk_buff *skb, *skb_tmp, *skb_match = NULL; | |
534 | struct dsa_port *dp = dsa_to_port(ds, port); | |
535 | struct skb_shared_hwtstamps shwt = {0}; | |
536 | struct sja1105_port *sp = dp->priv; | |
537 | ||
538 | if (!dsa_port_is_sja1105(dp)) | |
539 | return; | |
540 | ||
541 | /* We don't care about RX timestamps on the CPU port */ | |
542 | if (dir == SJA1110_META_TSTAMP_RX) | |
543 | return; | |
544 | ||
545 | spin_lock(&sp->data->skb_txtstamp_queue.lock); | |
546 | ||
547 | skb_queue_walk_safe(&sp->data->skb_txtstamp_queue, skb, skb_tmp) { | |
548 | if (SJA1105_SKB_CB(skb)->ts_id != ts_id) | |
549 | continue; | |
550 | ||
551 | __skb_unlink(skb, &sp->data->skb_txtstamp_queue); | |
552 | skb_match = skb; | |
553 | ||
554 | break; | |
555 | } | |
556 | ||
557 | spin_unlock(&sp->data->skb_txtstamp_queue.lock); | |
558 | ||
559 | if (WARN_ON(!skb_match)) | |
560 | return; | |
561 | ||
562 | shwt.hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(tstamp)); | |
563 | skb_complete_tx_timestamp(skb_match, &shwt); | |
564 | } | |
565 | ||
566b18c8 VO |
566 | static struct sk_buff *sja1110_rcv_meta(struct sk_buff *skb, u16 rx_header) |
567 | { | |
5d928ff4 | 568 | u8 *buf = dsa_etype_header_pos_rx(skb) + SJA1110_HEADER_LEN; |
566b18c8 VO |
569 | int switch_id = SJA1110_RX_HEADER_SWITCH_ID(rx_header); |
570 | int n_ts = SJA1110_RX_HEADER_N_TS(rx_header); | |
571 | struct net_device *master = skb->dev; | |
572 | struct dsa_port *cpu_dp; | |
566b18c8 VO |
573 | struct dsa_switch *ds; |
574 | int i; | |
575 | ||
576 | cpu_dp = master->dsa_ptr; | |
577 | ds = dsa_switch_find(cpu_dp->dst->index, switch_id); | |
578 | if (!ds) { | |
579 | net_err_ratelimited("%s: cannot find switch id %d\n", | |
580 | master->name, switch_id); | |
581 | return NULL; | |
582 | } | |
583 | ||
584 | for (i = 0; i <= n_ts; i++) { | |
585 | u8 ts_id, source_port, dir; | |
586 | u64 tstamp; | |
587 | ||
588 | ts_id = buf[0]; | |
589 | source_port = (buf[1] & GENMASK(7, 4)) >> 4; | |
590 | dir = (buf[1] & BIT(3)) >> 3; | |
591 | tstamp = be64_to_cpu(*(__be64 *)(buf + 2)); | |
592 | ||
593 | sja1110_process_meta_tstamp(ds, source_port, ts_id, dir, | |
594 | tstamp); | |
595 | ||
596 | buf += SJA1110_META_TSTAMP_SIZE; | |
597 | } | |
598 | ||
599 | /* Discard the meta frame, we've consumed the timestamps it contained */ | |
600 | return NULL; | |
601 | } | |
602 | ||
4913b8eb VO |
603 | static struct sk_buff *sja1110_rcv_inband_control_extension(struct sk_buff *skb, |
604 | int *source_port, | |
bea79078 VO |
605 | int *switch_id, |
606 | bool *host_only) | |
4913b8eb VO |
607 | { |
608 | u16 rx_header; | |
609 | ||
610 | if (unlikely(!pskb_may_pull(skb, SJA1110_HEADER_LEN))) | |
611 | return NULL; | |
612 | ||
613 | /* skb->data points to skb_mac_header(skb) + ETH_HLEN, which is exactly | |
614 | * what we need because the caller has checked the EtherType (which is | |
615 | * located 2 bytes back) and we just need a pointer to the header that | |
616 | * comes afterwards. | |
617 | */ | |
618 | rx_header = ntohs(*(__be16 *)skb->data); | |
566b18c8 | 619 | |
bea79078 VO |
620 | if (rx_header & SJA1110_RX_HEADER_HOST_ONLY) |
621 | *host_only = true; | |
622 | ||
566b18c8 VO |
623 | if (rx_header & SJA1110_RX_HEADER_IS_METADATA) |
624 | return sja1110_rcv_meta(skb, rx_header); | |
4913b8eb VO |
625 | |
626 | /* Timestamp frame, we have a trailer */ | |
627 | if (rx_header & SJA1110_RX_HEADER_HAS_TRAILER) { | |
628 | int start_of_padding = SJA1110_RX_HEADER_TRAILER_POS(rx_header); | |
629 | u8 *rx_trailer = skb_tail_pointer(skb) - SJA1110_RX_TRAILER_LEN; | |
630 | u64 *tstamp = &SJA1105_SKB_CB(skb)->tstamp; | |
631 | u8 last_byte = rx_trailer[12]; | |
632 | ||
633 | /* The timestamp is unaligned, so we need to use packing() | |
634 | * to get it | |
635 | */ | |
636 | packing(rx_trailer, tstamp, 63, 0, 8, UNPACK, 0); | |
637 | ||
638 | *source_port = SJA1110_RX_TRAILER_SRC_PORT(last_byte); | |
639 | *switch_id = SJA1110_RX_TRAILER_SWITCH_ID(last_byte); | |
640 | ||
641 | /* skb->len counts from skb->data, while start_of_padding | |
642 | * counts from the destination MAC address. Right now skb->data | |
643 | * is still as set by the DSA master, so to trim away the | |
644 | * padding and trailer we need to account for the fact that | |
645 | * skb->data points to skb_mac_header(skb) + ETH_HLEN. | |
646 | */ | |
647 | pskb_trim_rcsum(skb, start_of_padding - ETH_HLEN); | |
648 | /* Trap-to-host frame, no timestamp trailer */ | |
649 | } else { | |
650 | *source_port = SJA1110_RX_HEADER_SRC_PORT(rx_header); | |
651 | *switch_id = SJA1110_RX_HEADER_SWITCH_ID(rx_header); | |
652 | } | |
653 | ||
654 | /* Advance skb->data past the DSA header */ | |
655 | skb_pull_rcsum(skb, SJA1110_HEADER_LEN); | |
656 | ||
f1dacd7a | 657 | dsa_strip_etype_header(skb, SJA1110_HEADER_LEN); |
4913b8eb VO |
658 | |
659 | /* With skb->data in its final place, update the MAC header | |
660 | * so that eth_hdr() continues to works properly. | |
661 | */ | |
662 | skb_set_mac_header(skb, -ETH_HLEN); | |
663 | ||
664 | return skb; | |
665 | } | |
666 | ||
667 | static struct sk_buff *sja1110_rcv(struct sk_buff *skb, | |
29a097b7 | 668 | struct net_device *netdev) |
4913b8eb | 669 | { |
0fac6aa0 | 670 | int source_port = -1, switch_id = -1; |
bea79078 | 671 | bool host_only = false; |
421297ef | 672 | u16 vid = 0; |
4913b8eb | 673 | |
4913b8eb VO |
674 | if (sja1110_skb_has_inband_control_extension(skb)) { |
675 | skb = sja1110_rcv_inband_control_extension(skb, &source_port, | |
bea79078 VO |
676 | &switch_id, |
677 | &host_only); | |
4913b8eb VO |
678 | if (!skb) |
679 | return NULL; | |
680 | } | |
681 | ||
682 | /* Packets with in-band control extensions might still have RX VLANs */ | |
683 | if (likely(sja1105_skb_has_tag_8021q(skb))) | |
04a17583 | 684 | sja1105_vlan_rcv(skb, &source_port, &switch_id, &vid); |
4913b8eb | 685 | |
04a17583 | 686 | if (source_port == -1 || switch_id == -1) |
884be12f VO |
687 | skb->dev = dsa_find_designated_bridge_port_by_vid(netdev, vid); |
688 | else | |
689 | skb->dev = dsa_master_find_slave(netdev, switch_id, source_port); | |
4913b8eb | 690 | if (!skb->dev) { |
884be12f | 691 | netdev_warn(netdev, "Couldn't decode source port\n"); |
4913b8eb VO |
692 | return NULL; |
693 | } | |
694 | ||
bea79078 VO |
695 | if (!host_only) |
696 | dsa_default_offload_fwd_mark(skb); | |
697 | ||
4913b8eb VO |
698 | return skb; |
699 | } | |
700 | ||
e6652979 VO |
701 | static void sja1105_flow_dissect(const struct sk_buff *skb, __be16 *proto, |
702 | int *offset) | |
703 | { | |
704 | /* No tag added for management frames, all ok */ | |
705 | if (unlikely(sja1105_is_link_local(skb))) | |
706 | return; | |
707 | ||
708 | dsa_tag_generic_flow_dissect(skb, proto, offset); | |
709 | } | |
710 | ||
4913b8eb VO |
711 | static void sja1110_flow_dissect(const struct sk_buff *skb, __be16 *proto, |
712 | int *offset) | |
713 | { | |
714 | /* Management frames have 2 DSA tags on RX, so the needed_headroom we | |
715 | * declared is fine for the generic dissector adjustment procedure. | |
716 | */ | |
717 | if (unlikely(sja1105_is_link_local(skb))) | |
718 | return dsa_tag_generic_flow_dissect(skb, proto, offset); | |
719 | ||
720 | /* For the rest, there is a single DSA tag, the tag_8021q one */ | |
721 | *offset = VLAN_HLEN; | |
722 | *proto = ((__be16 *)skb->data)[(VLAN_HLEN / 2) - 1]; | |
723 | } | |
724 | ||
097f0244 | 725 | static const struct dsa_device_ops sja1105_netdev_ops = { |
227d07a0 VO |
726 | .name = "sja1105", |
727 | .proto = DSA_TAG_PROTO_SJA1105, | |
728 | .xmit = sja1105_xmit, | |
729 | .rcv = sja1105_rcv, | |
4e500251 | 730 | .needed_headroom = VLAN_HLEN, |
e6652979 | 731 | .flow_dissect = sja1105_flow_dissect, |
707091eb | 732 | .promisc_on_master = true, |
227d07a0 VO |
733 | }; |
734 | ||
4913b8eb | 735 | DSA_TAG_DRIVER(sja1105_netdev_ops); |
227d07a0 VO |
736 | MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_SJA1105); |
737 | ||
4913b8eb VO |
738 | static const struct dsa_device_ops sja1110_netdev_ops = { |
739 | .name = "sja1110", | |
740 | .proto = DSA_TAG_PROTO_SJA1110, | |
741 | .xmit = sja1110_xmit, | |
742 | .rcv = sja1110_rcv, | |
4913b8eb VO |
743 | .flow_dissect = sja1110_flow_dissect, |
744 | .needed_headroom = SJA1110_HEADER_LEN + VLAN_HLEN, | |
745 | .needed_tailroom = SJA1110_RX_TRAILER_LEN + SJA1110_MAX_PADDING_LEN, | |
746 | }; | |
747 | ||
748 | DSA_TAG_DRIVER(sja1110_netdev_ops); | |
749 | MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_SJA1110); | |
750 | ||
751 | static struct dsa_tag_driver *sja1105_tag_driver_array[] = { | |
752 | &DSA_TAG_DRIVER_NAME(sja1105_netdev_ops), | |
753 | &DSA_TAG_DRIVER_NAME(sja1110_netdev_ops), | |
754 | }; | |
755 | ||
756 | module_dsa_tag_drivers(sja1105_tag_driver_array); | |
757 | ||
758 | MODULE_LICENSE("GPL v2"); |