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