net/dsa/tag_sja1105.c

   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                 return true;
  33         if (!dsa_port_is_vlan_filtering(dev->dsa_ptr))
  34                 return true;
  35         return false;
  36 }
  37
  38 static struct sk_buff *sja1105_xmit(struct sk_buff *skb,
  39                                     struct net_device *netdev)
  40 {
  41         struct dsa_port *dp = dsa_slave_to_port(netdev);
  42         struct dsa_switch *ds = dp->ds;
  43         u16 tx_vid = dsa_8021q_tx_vid(ds, dp->index);
  44         u8 pcp = skb->priority;
  45
  46         /* Transmitting management traffic does not rely upon switch tagging,
  47          * but instead SPI-installed management routes. Part 2 of this
  48          * is the .port_deferred_xmit driver callback.
  49          */
  50         if (unlikely(sja1105_is_link_local(skb)))
  51                 return dsa_defer_xmit(skb, netdev);
  52
  53         /* If we are under a vlan_filtering bridge, IP termination on
  54          * switch ports based on 802.1Q tags is simply too brittle to
  55          * be passable. So just defer to the dsa_slave_notag_xmit
  56          * implementation.
  57          */
  58         if (dsa_port_is_vlan_filtering(dp))
  59                 return skb;
  60
  61         return dsa_8021q_xmit(skb, netdev, ETH_P_SJA1105,
  62                              ((pcp << VLAN_PRIO_SHIFT) | tx_vid));
  63 }
  64
  65 static struct sk_buff *sja1105_rcv(struct sk_buff *skb,
  66                                    struct net_device *netdev,
  67                                    struct packet_type *pt)
  68 {
  69         struct ethhdr *hdr = eth_hdr(skb);
  70         u64 source_port, switch_id;
  71         struct sk_buff *nskb;
  72         u16 tpid, vid, tci;
  73         bool is_tagged;
  74
  75         nskb = dsa_8021q_rcv(skb, netdev, pt, &tpid, &tci);
  76         is_tagged = (nskb && tpid == ETH_P_SJA1105);
  77
  78         skb->priority = (tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT;
  79         vid = tci & VLAN_VID_MASK;
  80
  81         skb->offload_fwd_mark = 1;
  82
  83         if (sja1105_is_link_local(skb)) {
  84                 /* Management traffic path. Switch embeds the switch ID and
  85                  * port ID into bytes of the destination MAC, courtesy of
  86                  * the incl_srcpt options.
  87                  */
  88                 source_port = hdr->h_dest[3];
  89                 switch_id = hdr->h_dest[4];
  90                 /* Clear the DMAC bytes that were mangled by the switch */
  91                 hdr->h_dest[3] = 0;
  92                 hdr->h_dest[4] = 0;
  93         } else {
  94                 /* Normal traffic path. */
  95                 source_port = dsa_8021q_rx_source_port(vid);
  96                 switch_id = dsa_8021q_rx_switch_id(vid);
  97         }
  98
  99         skb->dev = dsa_master_find_slave(netdev, switch_id, source_port);
 100         if (!skb->dev) {
 101                 netdev_warn(netdev, "Couldn't decode source port\n");
 102                 return NULL;
 103         }
 104
 105         /* Delete/overwrite fake VLAN header, DSA expects to not find
 106          * it there, see dsa_switch_rcv: skb_push(skb, ETH_HLEN).
 107          */
 108         if (is_tagged)
 109                 memmove(skb->data - ETH_HLEN, skb->data - ETH_HLEN - VLAN_HLEN,
 110                         ETH_HLEN - VLAN_HLEN);
 111
 112         return skb;
 113 }
 114
 115 static struct dsa_device_ops sja1105_netdev_ops = {
 116         .name = "sja1105",
 117         .proto = DSA_TAG_PROTO_SJA1105,
 118         .xmit = sja1105_xmit,
 119         .rcv = sja1105_rcv,
 120         .filter = sja1105_filter,
 121         .overhead = VLAN_HLEN,
 122 };
 123
 124 MODULE_LICENSE("GPL v2");
 125 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_SJA1105);
 126
 127 module_dsa_tag_driver(sja1105_netdev_ops);