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
33struct a5psw_tag {
34	__be16 ctrl_tag;
35	__be16 ctrl_data;
36	__be16 ctrl_data2_hi;
37	__be16 ctrl_data2_lo;
38};
39
40static 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
72static 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
105static 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
113MODULE_DESCRIPTION("DSA tag driver for Renesas RZ/N1 A5PSW switch");
114MODULE_LICENSE("GPL v2");
115MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_A5PSW, A5PSW_NAME);
116module_dsa_tag_driver(a5psw_netdev_ops);
117