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