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