[mirror_ubuntu-jammy-kernel.git] / net / dsa / tag_ocelot_8021q.c

// SPDX-License-Identifier: GPL-2.0
/* Copyright 2020-2021 NXP
 *
 * An implementation of the software-defined tag_8021q.c tagger format, which
 * also preserves full functionality under a vlan_filtering bridge. It does
 * this by using the TCAM engines for:
 * - pushing the RX VLAN as a second, outer tag, on egress towards the CPU port
 * - redirecting towards the correct front port based on TX VLAN and popping
 *   that on egress
 */
#include <linux/dsa/8021q.h>
#include <linux/dsa/ocelot.h>
#include "dsa_priv.h"

static struct sk_buff *ocelot_defer_xmit(struct dsa_port *dp,
					 struct sk_buff *skb)
{
	struct felix_deferred_xmit_work *xmit_work;
	struct felix_port *felix_port = dp->priv;

	xmit_work = kzalloc(sizeof(*xmit_work), GFP_ATOMIC);
	if (!xmit_work)
		return NULL;

	/* Calls felix_port_deferred_xmit in felix.c */
	kthread_init_work(&xmit_work->work, felix_port->xmit_work_fn);
	/* Increase refcount so the kfree_skb in dsa_slave_xmit
	 * won't really free the packet.
	 */
	xmit_work->dp = dp;
	xmit_work->skb = skb_get(skb);

	kthread_queue_work(felix_port->xmit_worker, &xmit_work->work);

	return NULL;
}

static struct sk_buff *ocelot_xmit(struct sk_buff *skb,
				   struct net_device *netdev)
{
	struct dsa_port *dp = dsa_slave_to_port(netdev);
	u16 tx_vid = dsa_8021q_tx_vid(dp->ds, dp->index);
	u16 queue_mapping = skb_get_queue_mapping(skb);
	u8 pcp = netdev_txq_to_tc(netdev, queue_mapping);
	struct ethhdr *hdr = eth_hdr(skb);

	if (ocelot_ptp_rew_op(skb) || is_link_local_ether_addr(hdr->h_dest))
		return ocelot_defer_xmit(dp, skb);

	return dsa_8021q_xmit(skb, netdev, ETH_P_8021Q,
			      ((pcp << VLAN_PRIO_SHIFT) | tx_vid));
}

static struct sk_buff *ocelot_rcv(struct sk_buff *skb,
				  struct net_device *netdev)
{
	int src_port, switch_id;

	dsa_8021q_rcv(skb, &src_port, &switch_id);

	skb->dev = dsa_master_find_slave(netdev, switch_id, src_port);
	if (!skb->dev)
		return NULL;

	dsa_default_offload_fwd_mark(skb);

	return skb;
}

static const struct dsa_device_ops ocelot_8021q_netdev_ops = {
	.name			= "ocelot-8021q",
	.proto			= DSA_TAG_PROTO_OCELOT_8021Q,
	.xmit			= ocelot_xmit,
	.rcv			= ocelot_rcv,
	.needed_headroom	= VLAN_HLEN,
	.promisc_on_master	= true,
};

MODULE_LICENSE("GPL v2");
MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_OCELOT_8021Q);

module_dsa_tag_driver(ocelot_8021q_netdev_ops);
Commit	Line	Data
7c83a7c5	1	// SPDX-License-Identifier: GPL-2.0
3c9cfb52	2	/* Copyright 2020-2021 NXP
7c83a7c5 VO	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>
deab6b1c	12	#include <linux/dsa/ocelot.h>
7c83a7c5 VO	13	#include "dsa_priv.h"
7c83a7c5 VO	14
49f885b2 VO	15	static struct sk_buff ocelot_defer_xmit(struct dsa_port dp,
	16	struct sk_buff *skb)
	17	{
	18	struct felix_deferred_xmit_work *xmit_work;
	19	struct felix_port *felix_port = dp->priv;
	20
	21	xmit_work = kzalloc(sizeof(*xmit_work), GFP_ATOMIC);
	22	if (!xmit_work)
	23	return NULL;
	24
	25	/* Calls felix_port_deferred_xmit in felix.c */
	26	kthread_init_work(&xmit_work->work, felix_port->xmit_work_fn);
	27	/* Increase refcount so the kfree_skb in dsa_slave_xmit
	28	* won't really free the packet.
	29	*/
	30	xmit_work->dp = dp;
	31	xmit_work->skb = skb_get(skb);
	32
	33	kthread_queue_work(felix_port->xmit_worker, &xmit_work->work);
	34
	35	return NULL;
	36	}
	37
7c83a7c5 VO	38	static struct sk_buff ocelot_xmit(struct sk_buff skb,
	39	struct net_device *netdev)
	40	{
	41	struct dsa_port *dp = dsa_slave_to_port(netdev);
	42	u16 tx_vid = dsa_8021q_tx_vid(dp->ds, dp->index);
	43	u16 queue_mapping = skb_get_queue_mapping(skb);
	44	u8 pcp = netdev_txq_to_tc(netdev, queue_mapping);
43ba33b4	45	struct ethhdr *hdr = eth_hdr(skb);
39e5308b	46
43ba33b4	47	if (ocelot_ptp_rew_op(skb) \|\| is_link_local_ether_addr(hdr->h_dest))
49f885b2	48	return ocelot_defer_xmit(dp, skb);
7c83a7c5 VO	49
	50	return dsa_8021q_xmit(skb, netdev, ETH_P_8021Q,
	51	((pcp << VLAN_PRIO_SHIFT) \| tx_vid));
	52	}
	53
	54	static struct sk_buff ocelot_rcv(struct sk_buff skb,
29a097b7	55	struct net_device *netdev)
7c83a7c5	56	{
0fac6aa0	57	int src_port, switch_id;
7c83a7c5	58
0fac6aa0	59	dsa_8021q_rcv(skb, &src_port, &switch_id);
7c83a7c5 VO	60
	61	skb->dev = dsa_master_find_slave(netdev, switch_id, src_port);
	62	if (!skb->dev)
	63	return NULL;
	64
bea79078	65	dsa_default_offload_fwd_mark(skb);
7c83a7c5 VO	66
	67	return skb;
	68	}
	69
	70	static const struct dsa_device_ops ocelot_8021q_netdev_ops = {
	71	.name = "ocelot-8021q",
	72	.proto = DSA_TAG_PROTO_OCELOT_8021Q,
	73	.xmit = ocelot_xmit,
	74	.rcv = ocelot_rcv,
4e500251	75	.needed_headroom = VLAN_HLEN,
c8c0ba4f	76	.promisc_on_master = true,
7c83a7c5 VO	77	};
	78
	79	MODULE_LICENSE("GPL v2");
	80	MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_OCELOT_8021Q);
	81
	82	module_dsa_tag_driver(ocelot_8021q_netdev_ops);