net/dsa/tag_ocelot_8021q.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* Copyright 2020-2021 NXP
   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 <linux/dsa/ocelot.h>
  13 #include "dsa_priv.h"
  14
  15 struct ocelot_8021q_tagger_private {
  16         struct ocelot_8021q_tagger_data data; /* Must be first */
  17         struct kthread_worker *xmit_worker;
  18 };
  19
  20 static struct sk_buff *ocelot_defer_xmit(struct dsa_port *dp,
  21                                          struct sk_buff *skb)
  22 {
  23         struct ocelot_8021q_tagger_private *priv = dp->ds->tagger_data;
  24         struct ocelot_8021q_tagger_data *data = &priv->data;
  25         void (*xmit_work_fn)(struct kthread_work *work);
  26         struct felix_deferred_xmit_work *xmit_work;
  27         struct kthread_worker *xmit_worker;
  28
  29         xmit_work_fn = data->xmit_work_fn;
  30         xmit_worker = priv->xmit_worker;
  31
  32         if (!xmit_work_fn || !xmit_worker)
  33                 return NULL;
  34
  35         xmit_work = kzalloc(sizeof(*xmit_work), GFP_ATOMIC);
  36         if (!xmit_work)
  37                 return NULL;
  38
  39         /* Calls felix_port_deferred_xmit in felix.c */
  40         kthread_init_work(&xmit_work->work, xmit_work_fn);
  41         /* Increase refcount so the kfree_skb in dsa_slave_xmit
  42          * won't really free the packet.
  43          */
  44         xmit_work->dp = dp;
  45         xmit_work->skb = skb_get(skb);
  46
  47         kthread_queue_work(xmit_worker, &xmit_work->work);
  48
  49         return NULL;
  50 }
  51
  52 static struct sk_buff *ocelot_xmit(struct sk_buff *skb,
  53                                    struct net_device *netdev)
  54 {
  55         struct dsa_port *dp = dsa_slave_to_port(netdev);
  56         u16 queue_mapping = skb_get_queue_mapping(skb);
  57         u8 pcp = netdev_txq_to_tc(netdev, queue_mapping);
  58         u16 tx_vid = dsa_tag_8021q_tx_vid(dp);
  59         struct ethhdr *hdr = eth_hdr(skb);
  60
  61         if (ocelot_ptp_rew_op(skb) || is_link_local_ether_addr(hdr->h_dest))
  62                 return ocelot_defer_xmit(dp, skb);
  63
  64         return dsa_8021q_xmit(skb, netdev, ETH_P_8021Q,
  65                               ((pcp << VLAN_PRIO_SHIFT) | tx_vid));
  66 }
  67
  68 static struct sk_buff *ocelot_rcv(struct sk_buff *skb,
  69                                   struct net_device *netdev)
  70 {
  71         int src_port, switch_id;
  72
  73         dsa_8021q_rcv(skb, &src_port, &switch_id);
  74
  75         skb->dev = dsa_master_find_slave(netdev, switch_id, src_port);
  76         if (!skb->dev)
  77                 return NULL;
  78
  79         dsa_default_offload_fwd_mark(skb);
  80
  81         return skb;
  82 }
  83
  84 static void ocelot_disconnect(struct dsa_switch *ds)
  85 {
  86         struct ocelot_8021q_tagger_private *priv = ds->tagger_data;
  87
  88         kthread_destroy_worker(priv->xmit_worker);
  89         kfree(priv);
  90         ds->tagger_data = NULL;
  91 }
  92
  93 static int ocelot_connect(struct dsa_switch *ds)
  94 {
  95         struct ocelot_8021q_tagger_private *priv;
  96         int err;
  97
  98         priv = kzalloc(sizeof(*priv), GFP_KERNEL);
  99         if (!priv)
 100                 return -ENOMEM;
 101
 102         priv->xmit_worker = kthread_create_worker(0, "felix_xmit");
 103         if (IS_ERR(priv->xmit_worker)) {
 104                 err = PTR_ERR(priv->xmit_worker);
 105                 kfree(priv);
 106                 return err;
 107         }
 108
 109         ds->tagger_data = priv;
 110
 111         return 0;
 112 }
 113
 114 static const struct dsa_device_ops ocelot_8021q_netdev_ops = {
 115         .name                   = "ocelot-8021q",
 116         .proto                  = DSA_TAG_PROTO_OCELOT_8021Q,
 117         .xmit                   = ocelot_xmit,
 118         .rcv                    = ocelot_rcv,
 119         .connect                = ocelot_connect,
 120         .disconnect             = ocelot_disconnect,
 121         .needed_headroom        = VLAN_HLEN,
 122         .promisc_on_master      = true,
 123 };
 124
 125 MODULE_LICENSE("GPL v2");
 126 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_OCELOT_8021Q);
 127
 128 module_dsa_tag_driver(ocelot_8021q_netdev_ops);