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