]>
Commit | Line | Data |
---|---|---|
a08d6a6d CL |
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 | ||
bd954b82 | 13 | #include "tag.h" |
a08d6a6d CL |
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 | ||
94793a56 VO |
25 | #define A5PSW_NAME "a5psw" |
26 | ||
a08d6a6d CL |
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 | ||
40 | static struct sk_buff *a5psw_tag_xmit(struct sk_buff *skb, struct net_device *dev) | |
41 | { | |
42 | struct dsa_port *dp = dsa_slave_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 | ||
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_master_find_slave(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 = { | |
94793a56 | 106 | .name = A5PSW_NAME, |
a08d6a6d CL |
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_LICENSE("GPL v2"); | |
94793a56 | 114 | MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_A5PSW, A5PSW_NAME); |
a08d6a6d | 115 | module_dsa_tag_driver(a5psw_netdev_ops); |