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 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) 12 { 13 const struct ethhdr *hdr = eth_hdr(skb); 14 u64 dmac = ether_addr_to_u64(hdr->h_dest); 15 16 if ((dmac & SJA1105_LINKLOCAL_FILTER_A_MASK) == 17 SJA1105_LINKLOCAL_FILTER_A) 18 return true; 19 if ((dmac & SJA1105_LINKLOCAL_FILTER_B_MASK) == 20 SJA1105_LINKLOCAL_FILTER_B) 21 return true; 22 return false; 23 } 24 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. 28 */ 29 static bool sja1105_filter(const struct sk_buff *skb, struct net_device *dev) 30 { 31 if (sja1105_is_link_local(skb)) { 32 SJA1105_SKB_CB(skb)->type = SJA1105_FRAME_TYPE_LINK_LOCAL; 33 return true; 34 } 35 if (!dsa_port_is_vlan_filtering(dev->dsa_ptr)) { 36 SJA1105_SKB_CB(skb)->type = SJA1105_FRAME_TYPE_NORMAL; 37 return true; 38 } 39 return false; 40 } 41 42 static struct sk_buff *sja1105_xmit(struct sk_buff *skb, 43 struct net_device *netdev) 44 { 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; 49 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. 53 */ 54 if (unlikely(sja1105_is_link_local(skb))) 55 return dsa_defer_xmit(skb, netdev); 56 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 60 * implementation. 61 */ 62 if (dsa_port_is_vlan_filtering(dp)) 63 return skb; 64 65 return dsa_8021q_xmit(skb, netdev, ETH_P_SJA1105, 66 ((pcp << VLAN_PRIO_SHIFT) | tx_vid)); 67 } 68 69 static struct sk_buff *sja1105_rcv(struct sk_buff *skb, 70 struct net_device *netdev, 71 struct packet_type *pt) 72 { 73 struct ethhdr *hdr = eth_hdr(skb); 74 u64 source_port, switch_id; 75 struct sk_buff *nskb; 76 u16 tpid, vid, tci; 77 bool is_tagged; 78 79 nskb = dsa_8021q_rcv(skb, netdev, pt, &tpid, &tci); 80 is_tagged = (nskb && tpid == ETH_P_SJA1105); 81 82 skb->priority = (tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT; 83 vid = tci & VLAN_VID_MASK; 84 85 skb->offload_fwd_mark = 1; 86 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. 91 */ 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 */ 95 hdr->h_dest[3] = 0; 96 hdr->h_dest[4] = 0; 97 } else { 98 /* Normal traffic path. */ 99 source_port = dsa_8021q_rx_source_port(vid); 100 switch_id = dsa_8021q_rx_switch_id(vid); 101 } 102 103 skb->dev = dsa_master_find_slave(netdev, switch_id, source_port); 104 if (!skb->dev) { 105 netdev_warn(netdev, "Couldn't decode source port\n"); 106 return NULL; 107 } 108 109 /* Delete/overwrite fake VLAN header, DSA expects to not find 110 * it there, see dsa_switch_rcv: skb_push(skb, ETH_HLEN). 111 */ 112 if (is_tagged) 113 memmove(skb->data - ETH_HLEN, skb->data - ETH_HLEN - VLAN_HLEN, 114 ETH_HLEN - VLAN_HLEN); 115 116 return skb; 117 } 118 119 static struct dsa_device_ops sja1105_netdev_ops = { 120 .name = "sja1105", 121 .proto = DSA_TAG_PROTO_SJA1105, 122 .xmit = sja1105_xmit, 123 .rcv = sja1105_rcv, 124 .filter = sja1105_filter, 125 .overhead = VLAN_HLEN, 126 }; 127 128 MODULE_LICENSE("GPL v2"); 129 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_SJA1105); 130 131 module_dsa_tag_driver(sja1105_netdev_ops); 132