1 // SPDX-License-Identifier: GPL-2.0
2 /* Copyright (c) 2019, Vladimir Oltean <olteanv@gmail.com>
4 #include <linux/if_vlan.h>
5 #include <linux/dsa/sja1105.h>
6 #include <linux/dsa/8021q.h>
7 #include <linux/packing.h>
10 /* Similar to is_link_local_ether_addr(hdr->h_dest) but also covers PTP */
11 static inline bool sja1105_is_link_local(const struct sk_buff *skb)
13 const struct ethhdr *hdr = eth_hdr(skb);
14 u64 dmac = ether_addr_to_u64(hdr->h_dest);
16 if ((dmac & SJA1105_LINKLOCAL_FILTER_A_MASK) ==
17 SJA1105_LINKLOCAL_FILTER_A)
19 if ((dmac & SJA1105_LINKLOCAL_FILTER_B_MASK) ==
20 SJA1105_LINKLOCAL_FILTER_B)
25 /* This is the first time the tagger sees the frame on RX.
26 * Figure out if we can decode it, and if we can, annotate skb->cb with how we
27 * plan to do that, so we don't need to check again in the rcv function.
29 static bool sja1105_filter(const struct sk_buff *skb, struct net_device *dev)
31 if (sja1105_is_link_local(skb)) {
32 SJA1105_SKB_CB(skb)->type = SJA1105_FRAME_TYPE_LINK_LOCAL;
35 if (!dsa_port_is_vlan_filtering(dev->dsa_ptr)) {
36 SJA1105_SKB_CB(skb)->type = SJA1105_FRAME_TYPE_NORMAL;
42 static struct sk_buff *sja1105_xmit(struct sk_buff *skb,
43 struct net_device *netdev)
45 struct dsa_port *dp = dsa_slave_to_port(netdev);
46 struct dsa_switch *ds = dp->ds;
47 u16 tx_vid = dsa_8021q_tx_vid(ds, dp->index);
48 u8 pcp = skb->priority;
50 /* Transmitting management traffic does not rely upon switch tagging,
51 * but instead SPI-installed management routes. Part 2 of this
52 * is the .port_deferred_xmit driver callback.
54 if (unlikely(sja1105_is_link_local(skb)))
55 return dsa_defer_xmit(skb, netdev);
57 /* If we are under a vlan_filtering bridge, IP termination on
58 * switch ports based on 802.1Q tags is simply too brittle to
59 * be passable. So just defer to the dsa_slave_notag_xmit
62 if (dsa_port_is_vlan_filtering(dp))
65 return dsa_8021q_xmit(skb, netdev, ETH_P_SJA1105,
66 ((pcp << VLAN_PRIO_SHIFT) | tx_vid));
69 static struct sk_buff *sja1105_rcv(struct sk_buff *skb,
70 struct net_device *netdev,
71 struct packet_type *pt)
73 struct ethhdr *hdr = eth_hdr(skb);
74 u64 source_port, switch_id;
79 nskb = dsa_8021q_rcv(skb, netdev, pt, &tpid, &tci);
80 is_tagged = (nskb && tpid == ETH_P_SJA1105);
82 skb->priority = (tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT;
83 vid = tci & VLAN_VID_MASK;
85 skb->offload_fwd_mark = 1;
87 if (SJA1105_SKB_CB(skb)->type == SJA1105_FRAME_TYPE_LINK_LOCAL) {
88 /* Management traffic path. Switch embeds the switch ID and
89 * port ID into bytes of the destination MAC, courtesy of
90 * the incl_srcpt options.
92 source_port = hdr->h_dest[3];
93 switch_id = hdr->h_dest[4];
94 /* Clear the DMAC bytes that were mangled by the switch */
98 /* Normal traffic path. */
99 source_port = dsa_8021q_rx_source_port(vid);
100 switch_id = dsa_8021q_rx_switch_id(vid);
103 skb->dev = dsa_master_find_slave(netdev, switch_id, source_port);
105 netdev_warn(netdev, "Couldn't decode source port\n");
109 /* Delete/overwrite fake VLAN header, DSA expects to not find
110 * it there, see dsa_switch_rcv: skb_push(skb, ETH_HLEN).
113 memmove(skb->data - ETH_HLEN, skb->data - ETH_HLEN - VLAN_HLEN,
114 ETH_HLEN - VLAN_HLEN);
119 static struct dsa_device_ops sja1105_netdev_ops = {
121 .proto = DSA_TAG_PROTO_SJA1105,
122 .xmit = sja1105_xmit,
124 .filter = sja1105_filter,
125 .overhead = VLAN_HLEN,
128 MODULE_LICENSE("GPL v2");
129 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_SJA1105);
131 module_dsa_tag_driver(sja1105_netdev_ops);