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>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/include/net/dsa.h b/include/net/dsa.h
index ffe56cc338feb9618191bdfcee44201d8139c72d..7ba9b1fb565ca17696ab3e5bcd08855d733281ab 100644 (file)
--- a/include/net/dsa.h
+++ b/include/net/dsa.h
@@ -32,6 +32,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 */
 };
 

diff --git a/net/dsa/Kconfig b/net/dsa/Kconfig
index da4d64f432db8ae4407990801f07786da4f1acc5..aa21f49f12156a403086e1625a95829fdf9513f5 100644 (file)
--- a/net/dsa/Kconfig
+++ b/net/dsa/Kconfig
@@ -31,4 +31,6 @@ config NET_DSA_TAG_TRAILER
 config NET_DSA_TAG_QCA
        bool
 
+config NET_DSA_TAG_MTK
+       bool
 endif

diff --git a/net/dsa/Makefile b/net/dsa/Makefile
index 31d343796251da06c979b3428f275f5911dfb2ab..9b1d478f3713fef59c12de3d578ca62c7e881819 100644 (file)
--- a/net/dsa/Makefile
+++ b/net/dsa/Makefile
@@ -8,3 +8,4 @@ dsa_core-$(CONFIG_NET_DSA_TAG_DSA) += tag_dsa.o
 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

diff --git a/net/dsa/dsa.c b/net/dsa/dsa.c
index 95d1a756202c45b6b1332163fd83d2c3e0c123c4..6cad15da58922ad06aa663d54aebdd6fff92e0ae 100644 (file)
--- a/net/dsa/dsa.c
+++ b/net/dsa/dsa.c
@@ -53,6 +53,9 @@ const struct dsa_device_ops *dsa_device_ops[DSA_TAG_LAST] = {
 #endif
 #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,
 };

diff --git a/net/dsa/dsa_priv.h b/net/dsa/dsa_priv.h
index 0706a511244e92ff0174173eb388a41ff59141f4..2a31399218110c07984e9c6f723ec13dd4a0f5f0 100644 (file)
--- a/net/dsa/dsa_priv.h
+++ b/net/dsa/dsa_priv.h
@@ -85,4 +85,7 @@ extern const struct dsa_device_ops brcm_netdev_ops;
 /* 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

diff --git a/net/dsa/tag_mtk.c b/net/dsa/tag_mtk.c
new file mode 100644 (file)
index 0000000..44ae635
--- /dev/null
+++ b/net/dsa/tag_mtk.c
@@ -0,0 +1,118 @@
+/*
+ * 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 <net/dsa.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,
+};