1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
|
From 5c01c03920c63630864d2b8641924a8c7c6cb62f Mon Sep 17 00:00:00 2001
From: Sean Wang <sean.wang@mediatek.com>
Date: Wed, 29 Mar 2017 17:38:20 +0800
Subject: [PATCH 28/57] net-next: dsa: add Mediatek tag RX/TX handler
Add the support for the 4-bytes tag for DSA port distinguishing inserted
allowing receiving and transmitting the packet via the particular port.
The tag is being added after the source MAC address in the ethernet
header.
Signed-off-by: Sean Wang <sean.wang@mediatek.com>
Signed-off-by: Landen Chao <Landen.Chao@mediatek.com>
Reviewed-by: Andrew Lunn <andrew@lunn.ch>
Reviewed-by: Florian Fainelli <f.fainelli@gmail.com>
---
include/net/dsa.h | 1 +
net/dsa/Kconfig | 2 +
net/dsa/Makefile | 1 +
net/dsa/dsa.c | 3 ++
net/dsa/dsa_priv.h | 3 ++
net/dsa/tag_mtk.c | 117 +++++++++++++++++++++++++++++++++++++++++++++++++++++
6 files changed, 127 insertions(+)
create mode 100644 net/dsa/tag_mtk.c
--- a/include/net/dsa.h
+++ b/include/net/dsa.h
@@ -27,6 +27,7 @@ enum dsa_tag_protocol {
DSA_TAG_PROTO_EDSA,
DSA_TAG_PROTO_BRCM,
DSA_TAG_PROTO_QCA,
+ DSA_TAG_PROTO_MTK,
DSA_TAG_LAST, /* MUST BE LAST */
};
--- a/net/dsa/Kconfig
+++ b/net/dsa/Kconfig
@@ -42,4 +42,6 @@ config NET_DSA_TAG_TRAILER
config NET_DSA_TAG_QCA
bool
+config NET_DSA_TAG_MTK
+ bool
endif
--- a/net/dsa/Makefile
+++ b/net/dsa/Makefile
@@ -8,3 +8,4 @@ dsa_core-$(CONFIG_NET_DSA_TAG_DSA) += ta
dsa_core-$(CONFIG_NET_DSA_TAG_EDSA) += tag_edsa.o
dsa_core-$(CONFIG_NET_DSA_TAG_TRAILER) += tag_trailer.o
dsa_core-$(CONFIG_NET_DSA_TAG_QCA) += tag_qca.o
+dsa_core-$(CONFIG_NET_DSA_TAG_MTK) += tag_mtk.o
--- a/net/dsa/dsa.c
+++ b/net/dsa/dsa.c
@@ -57,6 +57,9 @@ const struct dsa_device_ops *dsa_device_
#ifdef CONFIG_NET_DSA_TAG_QCA
[DSA_TAG_PROTO_QCA] = &qca_netdev_ops,
#endif
+#ifdef CONFIG_NET_DSA_TAG_MTK
+ [DSA_TAG_PROTO_MTK] = &mtk_netdev_ops,
+#endif
[DSA_TAG_PROTO_NONE] = &none_ops,
};
--- a/net/dsa/dsa_priv.h
+++ b/net/dsa/dsa_priv.h
@@ -84,4 +84,7 @@ extern const struct dsa_device_ops brcm_
/* tag_qca.c */
extern const struct dsa_device_ops qca_netdev_ops;
+/* tag_mtk.c */
+extern const struct dsa_device_ops mtk_netdev_ops;
+
#endif
--- /dev/null
+++ b/net/dsa/tag_mtk.c
@@ -0,0 +1,117 @@
+/*
+ * Mediatek DSA Tag support
+ * Copyright (C) 2017 Landen Chao <landen.chao@mediatek.com>
+ * Sean Wang <sean.wang@mediatek.com>
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/etherdevice.h>
+#include "dsa_priv.h"
+
+#define MTK_HDR_LEN 4
+#define MTK_HDR_RECV_SOURCE_PORT_MASK GENMASK(2, 0)
+#define MTK_HDR_XMIT_DP_BIT_MASK GENMASK(5, 0)
+
+static struct sk_buff *mtk_tag_xmit(struct sk_buff *skb,
+ struct net_device *dev)
+{
+ struct dsa_slave_priv *p = netdev_priv(dev);
+ u8 *mtk_tag;
+
+ if (skb_cow_head(skb, MTK_HDR_LEN) < 0)
+ goto out_free;
+
+ skb_push(skb, MTK_HDR_LEN);
+
+ memmove(skb->data, skb->data + MTK_HDR_LEN, 2 * ETH_ALEN);
+
+ /* Build the tag after the MAC Source Address */
+ mtk_tag = skb->data + 2 * ETH_ALEN;
+ mtk_tag[0] = 0;
+ mtk_tag[1] = (1 << p->dp->index) & MTK_HDR_XMIT_DP_BIT_MASK;
+ mtk_tag[2] = 0;
+ mtk_tag[3] = 0;
+
+ return skb;
+
+out_free:
+ kfree_skb(skb);
+ return NULL;
+}
+
+static int mtk_tag_rcv(struct sk_buff *skb, struct net_device *dev,
+ struct packet_type *pt, struct net_device *orig_dev)
+{
+ struct dsa_switch_tree *dst = dev->dsa_ptr;
+ struct dsa_switch *ds;
+ int port;
+ __be16 *phdr, hdr;
+
+ if (unlikely(!dst))
+ goto out_drop;
+
+ skb = skb_unshare(skb, GFP_ATOMIC);
+ if (!skb)
+ goto out;
+
+ if (unlikely(!pskb_may_pull(skb, MTK_HDR_LEN)))
+ goto out_drop;
+
+ /* The MTK header is added by the switch between src addr
+ * and ethertype at this point, skb->data points to 2 bytes
+ * after src addr so header should be 2 bytes right before.
+ */
+ phdr = (__be16 *)(skb->data - 2);
+ hdr = ntohs(*phdr);
+
+ /* Remove MTK tag and recalculate checksum. */
+ skb_pull_rcsum(skb, MTK_HDR_LEN);
+
+ memmove(skb->data - ETH_HLEN,
+ skb->data - ETH_HLEN - MTK_HDR_LEN,
+ 2 * ETH_ALEN);
+
+ /* This protocol doesn't support cascading multiple
+ * switches so it's safe to assume the switch is first
+ * in the tree.
+ */
+ ds = dst->ds[0];
+ if (!ds)
+ goto out_drop;
+
+ /* Get source port information */
+ port = (hdr & MTK_HDR_RECV_SOURCE_PORT_MASK);
+ if (!ds->ports[port].netdev)
+ goto out_drop;
+
+ /* Update skb & forward the frame accordingly */
+ skb_push(skb, ETH_HLEN);
+
+ skb->pkt_type = PACKET_HOST;
+ skb->dev = ds->ports[port].netdev;
+ skb->protocol = eth_type_trans(skb, skb->dev);
+
+ skb->dev->stats.rx_packets++;
+ skb->dev->stats.rx_bytes += skb->len;
+
+ netif_receive_skb(skb);
+
+ return 0;
+
+out_drop:
+ kfree_skb(skb);
+out:
+ return 0;
+}
+
+const struct dsa_device_ops mtk_netdev_ops = {
+ .xmit = mtk_tag_xmit,
+ .rcv = mtk_tag_rcv,
+};
|
