1 From patchwork Wed Mar 29 09:38:20 2017
2 Content-Type: text/plain; charset="utf-8"
4 Content-Transfer-Encoding: 7bit
5 Subject: [net-next,v3,2/5] net-next: dsa: add Mediatek tag RX/TX handler
6 From: sean.wang@mediatek.com
7 X-Patchwork-Id: 9651099
8 Message-Id: <1490780303-18598-3-git-send-email-sean.wang@mediatek.com>
9 To: <andrew@lunn.ch>, <f.fainelli@gmail.com>,
10 <vivien.didelot@savoirfairelinux.com>, <matthias.bgg@gmail.com>,
11 <robh+dt@kernel.org>, <mark.rutland@arm.com>
12 Cc: devicetree@vger.kernel.org, Landen.Chao@mediatek.com, keyhaede@gmail.com,
13 netdev@vger.kernel.org, sean.wang@mediatek.com,
14 linux-kernel@vger.kernel.org,
15 linux-mediatek@lists.infradead.org, objelf@gmail.com, davem@davemloft.net
16 Date: Wed, 29 Mar 2017 17:38:20 +0800
18 From: Sean Wang <sean.wang@mediatek.com>
20 Add the support for the 4-bytes tag for DSA port distinguishing inserted
21 allowing receiving and transmitting the packet via the particular port.
22 The tag is being added after the source MAC address in the ethernet
25 Signed-off-by: Sean Wang <sean.wang@mediatek.com>
26 Signed-off-by: Landen Chao <Landen.Chao@mediatek.com>
27 Reviewed-by: Andrew Lunn <andrew@lunn.ch>
28 Reviewed-by: Florian Fainelli <f.fainelli@gmail.com>
30 include/net/dsa.h | 1 +
32 net/dsa/Makefile | 1 +
34 net/dsa/dsa_priv.h | 3 ++
35 net/dsa/tag_mtk.c | 117 +++++++++++++++++++++++++++++++++++++++++++++++++++++
36 6 files changed, 127 insertions(+)
37 create mode 100644 net/dsa/tag_mtk.c
39 --- a/include/net/dsa.h
40 +++ b/include/net/dsa.h
41 @@ -27,6 +27,7 @@ enum dsa_tag_protocol {
46 DSA_TAG_LAST, /* MUST BE LAST */
51 @@ -41,4 +41,6 @@ config NET_DSA_TAG_TRAILER
52 config NET_DSA_TAG_QCA
55 +config NET_DSA_TAG_MTK
58 --- a/net/dsa/Makefile
59 +++ b/net/dsa/Makefile
60 @@ -8,3 +8,4 @@ dsa_core-$(CONFIG_NET_DSA_TAG_DSA) += ta
61 dsa_core-$(CONFIG_NET_DSA_TAG_EDSA) += tag_edsa.o
62 dsa_core-$(CONFIG_NET_DSA_TAG_TRAILER) += tag_trailer.o
63 dsa_core-$(CONFIG_NET_DSA_TAG_QCA) += tag_qca.o
64 +dsa_core-$(CONFIG_NET_DSA_TAG_MTK) += tag_mtk.o
67 @@ -57,6 +57,9 @@ const struct dsa_device_ops *dsa_device_
68 #ifdef CONFIG_NET_DSA_TAG_QCA
69 [DSA_TAG_PROTO_QCA] = &qca_netdev_ops,
71 +#ifdef CONFIG_NET_DSA_TAG_MTK
72 + [DSA_TAG_PROTO_MTK] = &mtk_netdev_ops,
74 [DSA_TAG_PROTO_NONE] = &none_ops,
77 --- a/net/dsa/dsa_priv.h
78 +++ b/net/dsa/dsa_priv.h
79 @@ -84,4 +84,7 @@ extern const struct dsa_device_ops brcm_
81 extern const struct dsa_device_ops qca_netdev_ops;
84 +extern const struct dsa_device_ops mtk_netdev_ops;
88 +++ b/net/dsa/tag_mtk.c
91 + * Mediatek DSA Tag support
92 + * Copyright (C) 2017 Landen Chao <landen.chao@mediatek.com>
93 + * Sean Wang <sean.wang@mediatek.com>
94 + * This program is free software; you can redistribute it and/or modify
95 + * it under the terms of the GNU General Public License version 2 and
96 + * only version 2 as published by the Free Software Foundation.
98 + * This program is distributed in the hope that it will be useful,
99 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
100 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
101 + * GNU General Public License for more details.
104 +#include <linux/etherdevice.h>
105 +#include "dsa_priv.h"
107 +#define MTK_HDR_LEN 4
108 +#define MTK_HDR_RECV_SOURCE_PORT_MASK GENMASK(2, 0)
109 +#define MTK_HDR_XMIT_DP_BIT_MASK GENMASK(5, 0)
111 +static struct sk_buff *mtk_tag_xmit(struct sk_buff *skb,
112 + struct net_device *dev)
114 + struct dsa_slave_priv *p = netdev_priv(dev);
117 + if (skb_cow_head(skb, MTK_HDR_LEN) < 0)
120 + skb_push(skb, MTK_HDR_LEN);
122 + memmove(skb->data, skb->data + MTK_HDR_LEN, 2 * ETH_ALEN);
124 + /* Build the tag after the MAC Source Address */
125 + mtk_tag = skb->data + 2 * ETH_ALEN;
127 + mtk_tag[1] = (1 << p->dp->index) & MTK_HDR_XMIT_DP_BIT_MASK;
138 +static int mtk_tag_rcv(struct sk_buff *skb, struct net_device *dev,
139 + struct packet_type *pt, struct net_device *orig_dev)
141 + struct dsa_switch_tree *dst = dev->dsa_ptr;
142 + struct dsa_switch *ds;
146 + if (unlikely(!dst))
149 + skb = skb_unshare(skb, GFP_ATOMIC);
153 + if (unlikely(!pskb_may_pull(skb, MTK_HDR_LEN)))
156 + /* The MTK header is added by the switch between src addr
157 + * and ethertype at this point, skb->data points to 2 bytes
158 + * after src addr so header should be 2 bytes right before.
160 + phdr = (__be16 *)(skb->data - 2);
161 + hdr = ntohs(*phdr);
163 + /* Remove MTK tag and recalculate checksum. */
164 + skb_pull_rcsum(skb, MTK_HDR_LEN);
166 + memmove(skb->data - ETH_HLEN,
167 + skb->data - ETH_HLEN - MTK_HDR_LEN,
170 + /* This protocol doesn't support cascading multiple
171 + * switches so it's safe to assume the switch is first
178 + /* Get source port information */
179 + port = (hdr & MTK_HDR_RECV_SOURCE_PORT_MASK);
180 + if (!ds->ports[port].netdev)
183 + /* Update skb & forward the frame accordingly */
184 + skb_push(skb, ETH_HLEN);
186 + skb->pkt_type = PACKET_HOST;
187 + skb->dev = ds->ports[port].netdev;
188 + skb->protocol = eth_type_trans(skb, skb->dev);
190 + skb->dev->stats.rx_packets++;
191 + skb->dev->stats.rx_bytes += skb->len;
193 + netif_receive_skb(skb);
203 +const struct dsa_device_ops mtk_netdev_ops = {
204 + .xmit = mtk_tag_xmit,
205 + .rcv = mtk_tag_rcv,