17c83a7c5SVladimir Oltean // SPDX-License-Identifier: GPL-2.0 23c9cfb52SVladimir Oltean /* Copyright 2020-2021 NXP 37c83a7c5SVladimir Oltean * 47c83a7c5SVladimir Oltean * An implementation of the software-defined tag_8021q.c tagger format, which 57c83a7c5SVladimir Oltean * also preserves full functionality under a vlan_filtering bridge. It does 67c83a7c5SVladimir Oltean * this by using the TCAM engines for: 77c83a7c5SVladimir Oltean * - pushing the RX VLAN as a second, outer tag, on egress towards the CPU port 87c83a7c5SVladimir Oltean * - redirecting towards the correct front port based on TX VLAN and popping 97c83a7c5SVladimir Oltean * that on egress 107c83a7c5SVladimir Oltean */ 117c83a7c5SVladimir Oltean #include <linux/dsa/8021q.h> 12deab6b1cSVladimir Oltean #include <linux/dsa/ocelot.h> 13*bd954b82SVladimir Oltean 14*bd954b82SVladimir Oltean #include "tag.h" 157c83a7c5SVladimir Oltean 1694793a56SVladimir Oltean #define OCELOT_8021Q_NAME "ocelot-8021q" 1794793a56SVladimir Oltean 1835d97680SVladimir Oltean struct ocelot_8021q_tagger_private { 1935d97680SVladimir Oltean struct ocelot_8021q_tagger_data data; /* Must be first */ 2035d97680SVladimir Oltean struct kthread_worker *xmit_worker; 2135d97680SVladimir Oltean }; 2235d97680SVladimir Oltean 2349f885b2SVladimir Oltean static struct sk_buff *ocelot_defer_xmit(struct dsa_port *dp, 2449f885b2SVladimir Oltean struct sk_buff *skb) 2549f885b2SVladimir Oltean { 2635d97680SVladimir Oltean struct ocelot_8021q_tagger_private *priv = dp->ds->tagger_data; 2735d97680SVladimir Oltean struct ocelot_8021q_tagger_data *data = &priv->data; 2835d97680SVladimir Oltean void (*xmit_work_fn)(struct kthread_work *work); 2949f885b2SVladimir Oltean struct felix_deferred_xmit_work *xmit_work; 3035d97680SVladimir Oltean struct kthread_worker *xmit_worker; 3135d97680SVladimir Oltean 3235d97680SVladimir Oltean xmit_work_fn = data->xmit_work_fn; 3335d97680SVladimir Oltean xmit_worker = priv->xmit_worker; 3435d97680SVladimir Oltean 3535d97680SVladimir Oltean if (!xmit_work_fn || !xmit_worker) 3635d97680SVladimir Oltean return NULL; 3749f885b2SVladimir Oltean 3829940ce3SVladimir Oltean /* PTP over IP packets need UDP checksumming. We may have inherited 3929940ce3SVladimir Oltean * NETIF_F_HW_CSUM from the DSA master, but these packets are not sent 4029940ce3SVladimir Oltean * through the DSA master, so calculate the checksum here. 4129940ce3SVladimir Oltean */ 4229940ce3SVladimir Oltean if (skb->ip_summed == CHECKSUM_PARTIAL && skb_checksum_help(skb)) 4329940ce3SVladimir Oltean return NULL; 4429940ce3SVladimir Oltean 4549f885b2SVladimir Oltean xmit_work = kzalloc(sizeof(*xmit_work), GFP_ATOMIC); 4649f885b2SVladimir Oltean if (!xmit_work) 4749f885b2SVladimir Oltean return NULL; 4849f885b2SVladimir Oltean 4949f885b2SVladimir Oltean /* Calls felix_port_deferred_xmit in felix.c */ 5035d97680SVladimir Oltean kthread_init_work(&xmit_work->work, xmit_work_fn); 5149f885b2SVladimir Oltean /* Increase refcount so the kfree_skb in dsa_slave_xmit 5249f885b2SVladimir Oltean * won't really free the packet. 5349f885b2SVladimir Oltean */ 5449f885b2SVladimir Oltean xmit_work->dp = dp; 5549f885b2SVladimir Oltean xmit_work->skb = skb_get(skb); 5649f885b2SVladimir Oltean 5735d97680SVladimir Oltean kthread_queue_work(xmit_worker, &xmit_work->work); 5849f885b2SVladimir Oltean 5949f885b2SVladimir Oltean return NULL; 6049f885b2SVladimir Oltean } 6149f885b2SVladimir Oltean 627c83a7c5SVladimir Oltean static struct sk_buff *ocelot_xmit(struct sk_buff *skb, 637c83a7c5SVladimir Oltean struct net_device *netdev) 647c83a7c5SVladimir Oltean { 657c83a7c5SVladimir Oltean struct dsa_port *dp = dsa_slave_to_port(netdev); 667c83a7c5SVladimir Oltean u16 queue_mapping = skb_get_queue_mapping(skb); 677c83a7c5SVladimir Oltean u8 pcp = netdev_txq_to_tc(netdev, queue_mapping); 6804b67e18SVladimir Oltean u16 tx_vid = dsa_tag_8021q_standalone_vid(dp); 6943ba33b4SVladimir Oltean struct ethhdr *hdr = eth_hdr(skb); 700a6f17c6SVladimir Oltean 7143ba33b4SVladimir Oltean if (ocelot_ptp_rew_op(skb) || is_link_local_ether_addr(hdr->h_dest)) 7249f885b2SVladimir Oltean return ocelot_defer_xmit(dp, skb); 737c83a7c5SVladimir Oltean 747c83a7c5SVladimir Oltean return dsa_8021q_xmit(skb, netdev, ETH_P_8021Q, 757c83a7c5SVladimir Oltean ((pcp << VLAN_PRIO_SHIFT) | tx_vid)); 767c83a7c5SVladimir Oltean } 777c83a7c5SVladimir Oltean 787c83a7c5SVladimir Oltean static struct sk_buff *ocelot_rcv(struct sk_buff *skb, 7929a097b7SVladimir Oltean struct net_device *netdev) 807c83a7c5SVladimir Oltean { 810fac6aa0SVladimir Oltean int src_port, switch_id; 827c83a7c5SVladimir Oltean 83d7f9787aSVladimir Oltean dsa_8021q_rcv(skb, &src_port, &switch_id, NULL); 847c83a7c5SVladimir Oltean 857c83a7c5SVladimir Oltean skb->dev = dsa_master_find_slave(netdev, switch_id, src_port); 867c83a7c5SVladimir Oltean if (!skb->dev) 877c83a7c5SVladimir Oltean return NULL; 887c83a7c5SVladimir Oltean 89bea79078SVladimir Oltean dsa_default_offload_fwd_mark(skb); 907c83a7c5SVladimir Oltean 917c83a7c5SVladimir Oltean return skb; 927c83a7c5SVladimir Oltean } 937c83a7c5SVladimir Oltean 947f297314SVladimir Oltean static void ocelot_disconnect(struct dsa_switch *ds) 9535d97680SVladimir Oltean { 967f297314SVladimir Oltean struct ocelot_8021q_tagger_private *priv = ds->tagger_data; 9735d97680SVladimir Oltean 9835d97680SVladimir Oltean kthread_destroy_worker(priv->xmit_worker); 9935d97680SVladimir Oltean kfree(priv); 1007f297314SVladimir Oltean ds->tagger_data = NULL; 10135d97680SVladimir Oltean } 10235d97680SVladimir Oltean 1037f297314SVladimir Oltean static int ocelot_connect(struct dsa_switch *ds) 10435d97680SVladimir Oltean { 10535d97680SVladimir Oltean struct ocelot_8021q_tagger_private *priv; 10635d97680SVladimir Oltean int err; 10735d97680SVladimir Oltean 10835d97680SVladimir Oltean priv = kzalloc(sizeof(*priv), GFP_KERNEL); 1097f297314SVladimir Oltean if (!priv) 1107f297314SVladimir Oltean return -ENOMEM; 11135d97680SVladimir Oltean 11235d97680SVladimir Oltean priv->xmit_worker = kthread_create_worker(0, "felix_xmit"); 11335d97680SVladimir Oltean if (IS_ERR(priv->xmit_worker)) { 11435d97680SVladimir Oltean err = PTR_ERR(priv->xmit_worker); 1157f297314SVladimir Oltean kfree(priv); 1167f297314SVladimir Oltean return err; 11735d97680SVladimir Oltean } 11835d97680SVladimir Oltean 1197f297314SVladimir Oltean ds->tagger_data = priv; 12035d97680SVladimir Oltean 12135d97680SVladimir Oltean return 0; 12235d97680SVladimir Oltean } 12335d97680SVladimir Oltean 1247c83a7c5SVladimir Oltean static const struct dsa_device_ops ocelot_8021q_netdev_ops = { 12594793a56SVladimir Oltean .name = OCELOT_8021Q_NAME, 1267c83a7c5SVladimir Oltean .proto = DSA_TAG_PROTO_OCELOT_8021Q, 1277c83a7c5SVladimir Oltean .xmit = ocelot_xmit, 1287c83a7c5SVladimir Oltean .rcv = ocelot_rcv, 12935d97680SVladimir Oltean .connect = ocelot_connect, 13035d97680SVladimir Oltean .disconnect = ocelot_disconnect, 1314e500251SVladimir Oltean .needed_headroom = VLAN_HLEN, 132c8c0ba4fSVladimir Oltean .promisc_on_master = true, 1337c83a7c5SVladimir Oltean }; 1347c83a7c5SVladimir Oltean 1357c83a7c5SVladimir Oltean MODULE_LICENSE("GPL v2"); 13694793a56SVladimir Oltean MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_OCELOT_8021Q, OCELOT_8021Q_NAME); 1377c83a7c5SVladimir Oltean 1387c83a7c5SVladimir Oltean module_dsa_tag_driver(ocelot_8021q_netdev_ops); 139