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         /* PTP over IP packets need UDP checksumming. We may have inherited
  36          * NETIF_F_HW_CSUM from the DSA master, but these packets are not sent
  37          * through the DSA master, so calculate the checksum here.
  38          */
  39         if (skb->ip_summed == CHECKSUM_PARTIAL && skb_checksum_help(skb))
  40                 return NULL;
  41
  42         xmit_work = kzalloc(sizeof(*xmit_work), GFP_ATOMIC);
  43         if (!xmit_work)
  44                 return NULL;
  45
  46         /* Calls felix_port_deferred_xmit in felix.c */
  47         kthread_init_work(&xmit_work->work, xmit_work_fn);
  48         /* Increase refcount so the kfree_skb in dsa_slave_xmit
  49          * won't really free the packet.
  50          */
  51         xmit_work->dp = dp;
  52         xmit_work->skb = skb_get(skb);
  53
  54         kthread_queue_work(xmit_worker, &xmit_work->work);
  55
  56         return NULL;
  57 }
  58
  59 static struct sk_buff *ocelot_xmit(struct sk_buff *skb,
  60                                    struct net_device *netdev)
  61 {
  62         struct dsa_port *dp = dsa_slave_to_port(netdev);
  63         u16 queue_mapping = skb_get_queue_mapping(skb);
  64         u8 pcp = netdev_txq_to_tc(netdev, queue_mapping);
  65         u16 tx_vid = dsa_tag_8021q_standalone_vid(dp);
  66         struct ethhdr *hdr = eth_hdr(skb);
  67
  68         if (ocelot_ptp_rew_op(skb) || is_link_local_ether_addr(hdr->h_dest))
  69                 return ocelot_defer_xmit(dp, skb);
  70
  71         return dsa_8021q_xmit(skb, netdev, ETH_P_8021Q,
  72                               ((pcp << VLAN_PRIO_SHIFT) | tx_vid));
  73 }
  74
  75 static struct sk_buff *ocelot_rcv(struct sk_buff *skb,
  76                                   struct net_device *netdev)
  77 {
  78         int src_port, switch_id;
  79
  80         dsa_8021q_rcv(skb, &src_port, &switch_id, NULL);
  81
  82         skb->dev = dsa_master_find_slave(netdev, switch_id, src_port);
  83         if (!skb->dev)
  84                 return NULL;
  85
  86         dsa_default_offload_fwd_mark(skb);
  87
  88         return skb;
  89 }
  90
  91 static void ocelot_disconnect(struct dsa_switch *ds)
  92 {
  93         struct ocelot_8021q_tagger_private *priv = ds->tagger_data;
  94
  95         kthread_destroy_worker(priv->xmit_worker);
  96         kfree(priv);
  97         ds->tagger_data = NULL;
  98 }
  99
 100 static int ocelot_connect(struct dsa_switch *ds)
 101 {
 102         struct ocelot_8021q_tagger_private *priv;
 103         int err;
 104
 105         priv = kzalloc(sizeof(*priv), GFP_KERNEL);
 106         if (!priv)
 107                 return -ENOMEM;
 108
 109         priv->xmit_worker = kthread_create_worker(0, "felix_xmit");
 110         if (IS_ERR(priv->xmit_worker)) {
 111                 err = PTR_ERR(priv->xmit_worker);
 112                 kfree(priv);
 113                 return err;
 114         }
 115
 116         ds->tagger_data = priv;
 117
 118         return 0;
 119 }
 120
 121 static const struct dsa_device_ops ocelot_8021q_netdev_ops = {
 122         .name                   = "ocelot-8021q",
 123         .proto                  = DSA_TAG_PROTO_OCELOT_8021Q,
 124         .xmit                   = ocelot_xmit,
 125         .rcv                    = ocelot_rcv,
 126         .connect                = ocelot_connect,
 127         .disconnect             = ocelot_disconnect,
 128         .needed_headroom        = VLAN_HLEN,
 129         .promisc_on_master      = true,
 130 };
 131
 132 MODULE_LICENSE("GPL v2");
 133 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_OCELOT_8021Q);
 134
 135 module_dsa_tag_driver(ocelot_8021q_netdev_ops);