1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  * Copyright (C) 2022 Schneider Electric
4  *
5  * Clément Léger <clement.leger@bootlin.com>
6  */
7 
8 #include <linux/bitfield.h>
9 #include <linux/etherdevice.h>
10 #include <linux/if_ether.h>
11 #include <net/dsa.h>
12 
13 #include "tag.h"
14 
15 /* To define the outgoing port and to discover the incoming port a TAG is
16  * inserted after Src MAC :
17  *
18  *       Dest MAC       Src MAC           TAG         Type
19  * ...| 1 2 3 4 5 6 | 1 2 3 4 5 6 | 1 2 3 4 5 6 7 8 | 1 2 |...
20  *                                |<--------------->|
21  *
22  * See struct a5psw_tag for layout
23  */
24 
25 #define A5PSW_NAME			"a5psw"
26 
27 #define ETH_P_DSA_A5PSW			0xE001
28 #define A5PSW_TAG_LEN			8
29 #define A5PSW_CTRL_DATA_FORCE_FORWARD	BIT(0)
30 /* This is both used for xmit tag and rcv tagging */
31 #define A5PSW_CTRL_DATA_PORT		GENMASK(3, 0)
32 
33 struct a5psw_tag {
34 	__be16 ctrl_tag;
35 	__be16 ctrl_data;
36 	__be16 ctrl_data2_hi;
37 	__be16 ctrl_data2_lo;
38 };
39 
a5psw_tag_xmit(struct sk_buff * skb,struct net_device * dev)40 static struct sk_buff *a5psw_tag_xmit(struct sk_buff *skb, struct net_device *dev)
41 {
42 	struct dsa_port *dp = dsa_user_to_port(dev);
43 	struct a5psw_tag *ptag;
44 	u32 data2_val;
45 
46 	BUILD_BUG_ON(sizeof(*ptag) != A5PSW_TAG_LEN);
47 
48 	/* The Ethernet switch we are interfaced with needs packets to be at
49 	 * least 60 bytes otherwise they will be discarded when they enter the
50 	 * switch port logic.
51 	 */
52 	if (__skb_put_padto(skb, ETH_ZLEN, false))
53 		return NULL;
54 
55 	/* provide 'A5PSW_TAG_LEN' bytes additional space */
56 	skb_push(skb, A5PSW_TAG_LEN);
57 
58 	/* make room between MACs and Ether-Type to insert tag */
59 	dsa_alloc_etype_header(skb, A5PSW_TAG_LEN);
60 
61 	ptag = dsa_etype_header_pos_tx(skb);
62 
63 	data2_val = FIELD_PREP(A5PSW_CTRL_DATA_PORT, BIT(dp->index));
64 	ptag->ctrl_tag = htons(ETH_P_DSA_A5PSW);
65 	ptag->ctrl_data = htons(A5PSW_CTRL_DATA_FORCE_FORWARD);
66 	ptag->ctrl_data2_lo = htons(data2_val);
67 	ptag->ctrl_data2_hi = 0;
68 
69 	return skb;
70 }
71 
a5psw_tag_rcv(struct sk_buff * skb,struct net_device * dev)72 static struct sk_buff *a5psw_tag_rcv(struct sk_buff *skb,
73 				     struct net_device *dev)
74 {
75 	struct a5psw_tag *tag;
76 	int port;
77 
78 	if (unlikely(!pskb_may_pull(skb, A5PSW_TAG_LEN))) {
79 		dev_warn_ratelimited(&dev->dev,
80 				     "Dropping packet, cannot pull\n");
81 		return NULL;
82 	}
83 
84 	tag = dsa_etype_header_pos_rx(skb);
85 
86 	if (tag->ctrl_tag != htons(ETH_P_DSA_A5PSW)) {
87 		dev_warn_ratelimited(&dev->dev, "Dropping packet due to invalid TAG marker\n");
88 		return NULL;
89 	}
90 
91 	port = FIELD_GET(A5PSW_CTRL_DATA_PORT, ntohs(tag->ctrl_data));
92 
93 	skb->dev = dsa_conduit_find_user(dev, 0, port);
94 	if (!skb->dev)
95 		return NULL;
96 
97 	skb_pull_rcsum(skb, A5PSW_TAG_LEN);
98 	dsa_strip_etype_header(skb, A5PSW_TAG_LEN);
99 
100 	dsa_default_offload_fwd_mark(skb);
101 
102 	return skb;
103 }
104 
105 static const struct dsa_device_ops a5psw_netdev_ops = {
106 	.name	= A5PSW_NAME,
107 	.proto	= DSA_TAG_PROTO_RZN1_A5PSW,
108 	.xmit	= a5psw_tag_xmit,
109 	.rcv	= a5psw_tag_rcv,
110 	.needed_headroom = A5PSW_TAG_LEN,
111 };
112 
113 MODULE_DESCRIPTION("DSA tag driver for Renesas RZ/N1 A5PSW switch");
114 MODULE_LICENSE("GPL v2");
115 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_A5PSW, A5PSW_NAME);
116 module_dsa_tag_driver(a5psw_netdev_ops);
117