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); 129