net/dsa/tag_ocelot_8021q.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* Copyright 2020-2021 NXP Semiconductors
   3  *
   4  * An implementation of the software-defined tag_8021q.c tagger format, which
   5  * also preserves full functionality under a vlan_filtering bridge. It does
   6  * this by using the TCAM engines for:
   7  * - pushing the RX VLAN as a second, outer tag, on egress towards the CPU port
   8  * - redirecting towards the correct front port based on TX VLAN and popping
   9  *   that on egress
  10  */
  11 #include <linux/dsa/8021q.h>
  12 #include <soc/mscc/ocelot.h>
  13 #include <soc/mscc/ocelot_ptp.h>
  14 #include "dsa_priv.h"
  15
  16 static struct sk_buff *ocelot_xmit_ptp(struct dsa_port *dp,
  17                                        struct sk_buff *skb,
  18                                        struct sk_buff *clone)
  19 {
  20         struct ocelot *ocelot = dp->ds->priv;
  21         struct ocelot_port *ocelot_port;
  22         int port = dp->index;
  23         u32 rew_op;
  24
  25         if (!ocelot_can_inject(ocelot, 0))
  26                 return NULL;
  27
  28         ocelot_port = ocelot->ports[port];
  29         rew_op = ocelot_port->ptp_cmd;
  30
  31         /* Retrieve timestamp ID populated inside skb->cb[0] of the
  32          * clone by ocelot_port_add_txtstamp_skb
  33          */
  34         if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP)
  35                 rew_op |= clone->cb[0] << 3;
  36
  37         ocelot_port_inject_frame(ocelot, port, 0, rew_op, skb);
  38
  39         return NULL;
  40 }
  41
  42 static struct sk_buff *ocelot_xmit(struct sk_buff *skb,
  43                                    struct net_device *netdev)
  44 {
  45         struct dsa_port *dp = dsa_slave_to_port(netdev);
  46         u16 tx_vid = dsa_8021q_tx_vid(dp->ds, dp->index);
  47         u16 queue_mapping = skb_get_queue_mapping(skb);
  48         u8 pcp = netdev_txq_to_tc(netdev, queue_mapping);
  49         struct sk_buff *clone = DSA_SKB_CB(skb)->clone;
  50
  51         /* TX timestamping was requested, so inject through MMIO */
  52         if (clone)
  53                 return ocelot_xmit_ptp(dp, skb, clone);
  54
  55         return dsa_8021q_xmit(skb, netdev, ETH_P_8021Q,
  56                               ((pcp << VLAN_PRIO_SHIFT) | tx_vid));
  57 }
  58
  59 static struct sk_buff *ocelot_rcv(struct sk_buff *skb,
  60                                   struct net_device *netdev,
  61                                   struct packet_type *pt)
  62 {
  63         int src_port, switch_id, qos_class;
  64         u16 vid, tci;
  65
  66         skb_push_rcsum(skb, ETH_HLEN);
  67         if (skb_vlan_tag_present(skb)) {
  68                 tci = skb_vlan_tag_get(skb);
  69                 __vlan_hwaccel_clear_tag(skb);
  70         } else {
  71                 __skb_vlan_pop(skb, &tci);
  72         }
  73         skb_pull_rcsum(skb, ETH_HLEN);
  74
  75         vid = tci & VLAN_VID_MASK;
  76         src_port = dsa_8021q_rx_source_port(vid);
  77         switch_id = dsa_8021q_rx_switch_id(vid);
  78         qos_class = (tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT;
  79
  80         skb->dev = dsa_master_find_slave(netdev, switch_id, src_port);
  81         if (!skb->dev)
  82                 return NULL;
  83
  84         skb->offload_fwd_mark = 1;
  85         skb->priority = qos_class;
  86
  87         return skb;
  88 }
  89
  90 static const struct dsa_device_ops ocelot_8021q_netdev_ops = {
  91         .name                   = "ocelot-8021q",
  92         .proto                  = DSA_TAG_PROTO_OCELOT_8021Q,
  93         .xmit                   = ocelot_xmit,
  94         .rcv                    = ocelot_rcv,
  95         .overhead               = VLAN_HLEN,
  96         .promisc_on_master      = true,
  97 };
  98
  99 MODULE_LICENSE("GPL v2");
 100 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_OCELOT_8021Q);
 101
 102 module_dsa_tag_driver(ocelot_8021q_netdev_ops);