LXR linux/net/dsa/tag

   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 */
  11static 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 */
  29static 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
  38static 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
  65static 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
 115static 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
 124MODULE_LICENSE("GPL v2");
 125MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_SJA1105);
 126
 127module_dsa_tag_driver(sja1105_netdev_ops);
 128