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>
13bd954b82SVladimir Oltean
14bd954b82SVladimir Oltean #include "tag.h"
15*19d05ea7SVladimir Oltean #include "tag_8021q.h"
167c83a7c5SVladimir Oltean
1794793a56SVladimir Oltean #define OCELOT_8021Q_NAME "ocelot-8021q"
1894793a56SVladimir Oltean
1935d97680SVladimir Oltean struct ocelot_8021q_tagger_private {
2035d97680SVladimir Oltean struct ocelot_8021q_tagger_data data; /* Must be first */
2135d97680SVladimir Oltean struct kthread_worker *xmit_worker;
2235d97680SVladimir Oltean };
2335d97680SVladimir Oltean
ocelot_defer_xmit(struct dsa_port * dp,struct sk_buff * skb)2449f885b2SVladimir Oltean static struct sk_buff *ocelot_defer_xmit(struct dsa_port *dp,
2549f885b2SVladimir Oltean struct sk_buff *skb)
2649f885b2SVladimir Oltean {
2735d97680SVladimir Oltean struct ocelot_8021q_tagger_private *priv = dp->ds->tagger_data;
2835d97680SVladimir Oltean struct ocelot_8021q_tagger_data *data = &priv->data;
2935d97680SVladimir Oltean void (*xmit_work_fn)(struct kthread_work *work);
3049f885b2SVladimir Oltean struct felix_deferred_xmit_work *xmit_work;
3135d97680SVladimir Oltean struct kthread_worker *xmit_worker;
3235d97680SVladimir Oltean
3335d97680SVladimir Oltean xmit_work_fn = data->xmit_work_fn;
3435d97680SVladimir Oltean xmit_worker = priv->xmit_worker;
3535d97680SVladimir Oltean
3635d97680SVladimir Oltean if (!xmit_work_fn || !xmit_worker)
3735d97680SVladimir Oltean return NULL;
3849f885b2SVladimir Oltean
3929940ce3SVladimir Oltean /* PTP over IP packets need UDP checksumming. We may have inherited
4029940ce3SVladimir Oltean * NETIF_F_HW_CSUM from the DSA master, but these packets are not sent
4129940ce3SVladimir Oltean * through the DSA master, so calculate the checksum here.
4229940ce3SVladimir Oltean */
4329940ce3SVladimir Oltean if (skb->ip_summed == CHECKSUM_PARTIAL && skb_checksum_help(skb))
4429940ce3SVladimir Oltean return NULL;
4529940ce3SVladimir Oltean
4649f885b2SVladimir Oltean xmit_work = kzalloc(sizeof(*xmit_work), GFP_ATOMIC);
4749f885b2SVladimir Oltean if (!xmit_work)
4849f885b2SVladimir Oltean return NULL;
4949f885b2SVladimir Oltean
5049f885b2SVladimir Oltean /* Calls felix_port_deferred_xmit in felix.c */
5135d97680SVladimir Oltean kthread_init_work(&xmit_work->work, xmit_work_fn);
5249f885b2SVladimir Oltean /* Increase refcount so the kfree_skb in dsa_slave_xmit
5349f885b2SVladimir Oltean * won't really free the packet.
5449f885b2SVladimir Oltean */
5549f885b2SVladimir Oltean xmit_work->dp = dp;
5649f885b2SVladimir Oltean xmit_work->skb = skb_get(skb);
5749f885b2SVladimir Oltean
5835d97680SVladimir Oltean kthread_queue_work(xmit_worker, &xmit_work->work);
5949f885b2SVladimir Oltean
6049f885b2SVladimir Oltean return NULL;
6149f885b2SVladimir Oltean }
6249f885b2SVladimir Oltean
ocelot_xmit(struct sk_buff * skb,struct net_device * netdev)637c83a7c5SVladimir Oltean static struct sk_buff *ocelot_xmit(struct sk_buff *skb,
647c83a7c5SVladimir Oltean struct net_device *netdev)
657c83a7c5SVladimir Oltean {
667c83a7c5SVladimir Oltean struct dsa_port *dp = dsa_slave_to_port(netdev);
677c83a7c5SVladimir Oltean u16 queue_mapping = skb_get_queue_mapping(skb);
687c83a7c5SVladimir Oltean u8 pcp = netdev_txq_to_tc(netdev, queue_mapping);
6904b67e18SVladimir Oltean u16 tx_vid = dsa_tag_8021q_standalone_vid(dp);
7043ba33b4SVladimir Oltean struct ethhdr *hdr = eth_hdr(skb);
710a6f17c6SVladimir Oltean
7243ba33b4SVladimir Oltean if (ocelot_ptp_rew_op(skb) || is_link_local_ether_addr(hdr->h_dest))
7349f885b2SVladimir Oltean return ocelot_defer_xmit(dp, skb);
747c83a7c5SVladimir Oltean
757c83a7c5SVladimir Oltean return dsa_8021q_xmit(skb, netdev, ETH_P_8021Q,
767c83a7c5SVladimir Oltean ((pcp << VLAN_PRIO_SHIFT) | tx_vid));
777c83a7c5SVladimir Oltean }
787c83a7c5SVladimir Oltean
ocelot_rcv(struct sk_buff * skb,struct net_device * netdev)797c83a7c5SVladimir Oltean static struct sk_buff *ocelot_rcv(struct sk_buff *skb,
8029a097b7SVladimir Oltean struct net_device *netdev)
817c83a7c5SVladimir Oltean {
820fac6aa0SVladimir Oltean int src_port, switch_id;
837c83a7c5SVladimir Oltean
84d7f9787aSVladimir Oltean dsa_8021q_rcv(skb, &src_port, &switch_id, NULL);
857c83a7c5SVladimir Oltean
867c83a7c5SVladimir Oltean skb->dev = dsa_master_find_slave(netdev, switch_id, src_port);
877c83a7c5SVladimir Oltean if (!skb->dev)
887c83a7c5SVladimir Oltean return NULL;
897c83a7c5SVladimir Oltean
90bea79078SVladimir Oltean dsa_default_offload_fwd_mark(skb);
917c83a7c5SVladimir Oltean
927c83a7c5SVladimir Oltean return skb;
937c83a7c5SVladimir Oltean }
947c83a7c5SVladimir Oltean
ocelot_disconnect(struct dsa_switch * ds)957f297314SVladimir Oltean static void ocelot_disconnect(struct dsa_switch *ds)
9635d97680SVladimir Oltean {
977f297314SVladimir Oltean struct ocelot_8021q_tagger_private *priv = ds->tagger_data;
9835d97680SVladimir Oltean
9935d97680SVladimir Oltean kthread_destroy_worker(priv->xmit_worker);
10035d97680SVladimir Oltean kfree(priv);
1017f297314SVladimir Oltean ds->tagger_data = NULL;
10235d97680SVladimir Oltean }
10335d97680SVladimir Oltean
ocelot_connect(struct dsa_switch * ds)1047f297314SVladimir Oltean static int ocelot_connect(struct dsa_switch *ds)
10535d97680SVladimir Oltean {
10635d97680SVladimir Oltean struct ocelot_8021q_tagger_private *priv;
10735d97680SVladimir Oltean int err;
10835d97680SVladimir Oltean
10935d97680SVladimir Oltean priv = kzalloc(sizeof(*priv), GFP_KERNEL);
1107f297314SVladimir Oltean if (!priv)
1117f297314SVladimir Oltean return -ENOMEM;
11235d97680SVladimir Oltean
11335d97680SVladimir Oltean priv->xmit_worker = kthread_create_worker(0, "felix_xmit");
11435d97680SVladimir Oltean if (IS_ERR(priv->xmit_worker)) {
11535d97680SVladimir Oltean err = PTR_ERR(priv->xmit_worker);
1167f297314SVladimir Oltean kfree(priv);
1177f297314SVladimir Oltean return err;
11835d97680SVladimir Oltean }
11935d97680SVladimir Oltean
1207f297314SVladimir Oltean ds->tagger_data = priv;
12135d97680SVladimir Oltean
12235d97680SVladimir Oltean return 0;
12335d97680SVladimir Oltean }
12435d97680SVladimir Oltean
1257c83a7c5SVladimir Oltean static const struct dsa_device_ops ocelot_8021q_netdev_ops = {
12694793a56SVladimir Oltean .name = OCELOT_8021Q_NAME,
1277c83a7c5SVladimir Oltean .proto = DSA_TAG_PROTO_OCELOT_8021Q,
1287c83a7c5SVladimir Oltean .xmit = ocelot_xmit,
1297c83a7c5SVladimir Oltean .rcv = ocelot_rcv,
13035d97680SVladimir Oltean .connect = ocelot_connect,
13135d97680SVladimir Oltean .disconnect = ocelot_disconnect,
1324e500251SVladimir Oltean .needed_headroom = VLAN_HLEN,
133c8c0ba4fSVladimir Oltean .promisc_on_master = true,
1347c83a7c5SVladimir Oltean };
1357c83a7c5SVladimir Oltean
1367c83a7c5SVladimir Oltean MODULE_LICENSE("GPL v2");
13794793a56SVladimir Oltean MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_OCELOT_8021Q, OCELOT_8021Q_NAME);
1387c83a7c5SVladimir Oltean
1397c83a7c5SVladimir Oltean module_dsa_tag_driver(ocelot_8021q_netdev_ops);
140