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 "dsa_priv.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 ETH_P_DSA_A5PSW 0xE001
26 #define A5PSW_TAG_LEN 8
27 #define A5PSW_CTRL_DATA_FORCE_FORWARD BIT(0)
28 /* This is both used for xmit tag and rcv tagging */
29 #define A5PSW_CTRL_DATA_PORT GENMASK(3, 0)
30
31 struct a5psw_tag {
32 __be16 ctrl_tag;
33 __be16 ctrl_data;
34 __be16 ctrl_data2_hi;
35 __be16 ctrl_data2_lo;
36 };
37
a5psw_tag_xmit(struct sk_buff * skb,struct net_device * dev)38 static struct sk_buff *a5psw_tag_xmit(struct sk_buff *skb, struct net_device *dev)
39 {
40 struct dsa_port *dp = dsa_slave_to_port(dev);
41 struct a5psw_tag *ptag;
42 u32 data2_val;
43
44 BUILD_BUG_ON(sizeof(*ptag) != A5PSW_TAG_LEN);
45
46 /* The Ethernet switch we are interfaced with needs packets to be at
47 * least 60 bytes otherwise they will be discarded when they enter the
48 * switch port logic.
49 */
50 if (__skb_put_padto(skb, ETH_ZLEN, false))
51 return NULL;
52
53 /* provide 'A5PSW_TAG_LEN' bytes additional space */
54 skb_push(skb, A5PSW_TAG_LEN);
55
56 /* make room between MACs and Ether-Type to insert tag */
57 dsa_alloc_etype_header(skb, A5PSW_TAG_LEN);
58
59 ptag = dsa_etype_header_pos_tx(skb);
60
61 data2_val = FIELD_PREP(A5PSW_CTRL_DATA_PORT, BIT(dp->index));
62 ptag->ctrl_tag = htons(ETH_P_DSA_A5PSW);
63 ptag->ctrl_data = htons(A5PSW_CTRL_DATA_FORCE_FORWARD);
64 ptag->ctrl_data2_lo = htons(data2_val);
65 ptag->ctrl_data2_hi = 0;
66
67 return skb;
68 }
69
a5psw_tag_rcv(struct sk_buff * skb,struct net_device * dev)70 static struct sk_buff *a5psw_tag_rcv(struct sk_buff *skb,
71 struct net_device *dev)
72 {
73 struct a5psw_tag *tag;
74 int port;
75
76 if (unlikely(!pskb_may_pull(skb, A5PSW_TAG_LEN))) {
77 dev_warn_ratelimited(&dev->dev,
78 "Dropping packet, cannot pull\n");
79 return NULL;
80 }
81
82 tag = dsa_etype_header_pos_rx(skb);
83
84 if (tag->ctrl_tag != htons(ETH_P_DSA_A5PSW)) {
85 dev_warn_ratelimited(&dev->dev, "Dropping packet due to invalid TAG marker\n");
86 return NULL;
87 }
88
89 port = FIELD_GET(A5PSW_CTRL_DATA_PORT, ntohs(tag->ctrl_data));
90
91 skb->dev = dsa_master_find_slave(dev, 0, port);
92 if (!skb->dev)
93 return NULL;
94
95 skb_pull_rcsum(skb, A5PSW_TAG_LEN);
96 dsa_strip_etype_header(skb, A5PSW_TAG_LEN);
97
98 dsa_default_offload_fwd_mark(skb);
99
100 return skb;
101 }
102
103 static const struct dsa_device_ops a5psw_netdev_ops = {
104 .name = "a5psw",
105 .proto = DSA_TAG_PROTO_RZN1_A5PSW,
106 .xmit = a5psw_tag_xmit,
107 .rcv = a5psw_tag_rcv,
108 .needed_headroom = A5PSW_TAG_LEN,
109 };
110
111 MODULE_LICENSE("GPL v2");
112 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_A5PSW);
113 module_dsa_tag_driver(a5psw_netdev_ops);
114
