target/linux/mediatek/patches-4.9/0092-dsa2.patch

   1 From patchwork Wed Mar 29 09:38:20 2017
   2 Content-Type: text/plain; charset="utf-8"
   3 MIME-Version: 1.0
   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
  17
  18 From: Sean Wang <sean.wang@mediatek.com>
  19
  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
  23 header.
  24
  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>
  29 ---
  30  include/net/dsa.h  |   1 +
  31  net/dsa/Kconfig    |   2 +
  32  net/dsa/Makefile   |   1 +
  33  net/dsa/dsa.c      |   3 ++
  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
  38
  39 diff --git a/include/net/dsa.h b/include/net/dsa.h
  40 index 4e13e69..3276547 100644
  41 --- a/include/net/dsa.h
  42 +++ b/include/net/dsa.h
  43 @@ -31,6 +31,7 @@ enum dsa_tag_protocol {
  44         DSA_TAG_PROTO_EDSA,
  45         DSA_TAG_PROTO_BRCM,
  46         DSA_TAG_PROTO_QCA,
  47 +       DSA_TAG_PROTO_MTK,
  48         DSA_TAG_LAST,           /* MUST BE LAST */
  49  };
  50
  51 diff --git a/net/dsa/Kconfig b/net/dsa/Kconfig
  52 index 9649238..d78789b 100644
  53 --- a/net/dsa/Kconfig
  54 +++ b/net/dsa/Kconfig
  55 @@ -31,4 +31,6 @@ config NET_DSA_TAG_TRAILER
  56  config NET_DSA_TAG_QCA
  57         bool
  58
  59 +config NET_DSA_TAG_MTK
  60 +       bool
  61  endif
  62 diff --git a/net/dsa/Makefile b/net/dsa/Makefile
  63 index 31d3437..9b1d478 100644
  64 --- a/net/dsa/Makefile
  65 +++ b/net/dsa/Makefile
  66 @@ -8,3 +8,4 @@ dsa_core-$(CONFIG_NET_DSA_TAG_DSA) += tag_dsa.o
  67  dsa_core-$(CONFIG_NET_DSA_TAG_EDSA) += tag_edsa.o
  68  dsa_core-$(CONFIG_NET_DSA_TAG_TRAILER) += tag_trailer.o
  69  dsa_core-$(CONFIG_NET_DSA_TAG_QCA) += tag_qca.o
  70 +dsa_core-$(CONFIG_NET_DSA_TAG_MTK) += tag_mtk.o
  71 diff --git a/net/dsa/dsa.c b/net/dsa/dsa.c
  72 index b6d4f6a..617f736 100644
  73 --- a/net/dsa/dsa.c
  74 +++ b/net/dsa/dsa.c
  75 @@ -53,6 +53,9 @@ static struct sk_buff *dsa_slave_notag_xmit(struct sk_buff *skb,
  76  #ifdef CONFIG_NET_DSA_TAG_QCA
  77         [DSA_TAG_PROTO_QCA] = &qca_netdev_ops,
  78  #endif
  79 +#ifdef CONFIG_NET_DSA_TAG_MTK
  80 +       [DSA_TAG_PROTO_MTK] = &mtk_netdev_ops,
  81 +#endif
  82         [DSA_TAG_PROTO_NONE] = &none_ops,
  83  };
  84
  85 diff --git a/net/dsa/dsa_priv.h b/net/dsa/dsa_priv.h
  86 index 0706a51..2a31399 100644
  87 --- a/net/dsa/dsa_priv.h
  88 +++ b/net/dsa/dsa_priv.h
  89 @@ -85,4 +85,7 @@ int dsa_slave_create(struct dsa_switch *ds, struct device *parent,
  90  /* tag_qca.c */
  91  extern const struct dsa_device_ops qca_netdev_ops;
  92
  93 +/* tag_mtk.c */
  94 +extern const struct dsa_device_ops mtk_netdev_ops;
  95 +
  96  #endif
  97 diff --git a/net/dsa/tag_mtk.c b/net/dsa/tag_mtk.c
  98 new file mode 100644
  99 index 0000000..833a9d6
 100 --- /dev/null
 101 +++ b/net/dsa/tag_mtk.c
 102 @@ -0,0 +1,117 @@
 103 +/*
 104 + * Mediatek DSA Tag support
 105 + * Copyright (C) 2017 Landen Chao <landen.chao@mediatek.com>
 106 + *                   Sean Wang <sean.wang@mediatek.com>
 107 + * This program is free software; you can redistribute it and/or modify
 108 + * it under the terms of the GNU General Public License version 2 and
 109 + * only version 2 as published by the Free Software Foundation.
 110 + *
 111 + * This program is distributed in the hope that it will be useful,
 112 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
 113 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 114 + * GNU General Public License for more details.
 115 + */
 116 +
 117 +#include <linux/etherdevice.h>
 118 +#include "dsa_priv.h"
 119 +
 120 +#define MTK_HDR_LEN            4
 121 +#define MTK_HDR_RECV_SOURCE_PORT_MASK  GENMASK(2, 0)
 122 +#define MTK_HDR_XMIT_DP_BIT_MASK       GENMASK(5, 0)
 123 +
 124 +static struct sk_buff *mtk_tag_xmit(struct sk_buff *skb,
 125 +                                   struct net_device *dev)
 126 +{
 127 +       struct dsa_slave_priv *p = netdev_priv(dev);
 128 +       u8 *mtk_tag;
 129 +
 130 +       if (skb_cow_head(skb, MTK_HDR_LEN) < 0)
 131 +               goto out_free;
 132 +
 133 +       skb_push(skb, MTK_HDR_LEN);
 134 +
 135 +       memmove(skb->data, skb->data + MTK_HDR_LEN, 2 * ETH_ALEN);
 136 +
 137 +       /* Build the tag after the MAC Source Address */
 138 +       mtk_tag = skb->data + 2 * ETH_ALEN;
 139 +       mtk_tag[0] = 0;
 140 +       mtk_tag[1] = (1 << p->dp->index) & MTK_HDR_XMIT_DP_BIT_MASK;
 141 +       mtk_tag[2] = 0;
 142 +       mtk_tag[3] = 0;
 143 +
 144 +       return skb;
 145 +
 146 +out_free:
 147 +       kfree_skb(skb);
 148 +       return NULL;
 149 +}
 150 +
 151 +static int mtk_tag_rcv(struct sk_buff *skb, struct net_device *dev,
 152 +                      struct packet_type *pt, struct net_device *orig_dev)
 153 +{
 154 +       struct dsa_switch_tree *dst = dev->dsa_ptr;
 155 +       struct dsa_switch *ds;
 156 +       int port;
 157 +       __be16 *phdr, hdr;
 158 +
 159 +       if (unlikely(!dst))
 160 +               goto out_drop;
 161 +
 162 +       skb = skb_unshare(skb, GFP_ATOMIC);
 163 +       if (!skb)
 164 +               goto out;
 165 +
 166 +       if (unlikely(!pskb_may_pull(skb, MTK_HDR_LEN)))
 167 +               goto out_drop;
 168 +
 169 +       /* The MTK header is added by the switch between src addr
 170 +        * and ethertype at this point, skb->data points to 2 bytes
 171 +        * after src addr so header should be 2 bytes right before.
 172 +        */
 173 +       phdr = (__be16 *)(skb->data - 2);
 174 +       hdr = ntohs(*phdr);
 175 +
 176 +       /* Remove MTK tag and recalculate checksum. */
 177 +       skb_pull_rcsum(skb, MTK_HDR_LEN);
 178 +
 179 +       memmove(skb->data - ETH_HLEN,
 180 +               skb->data - ETH_HLEN - MTK_HDR_LEN,
 181 +               2 * ETH_ALEN);
 182 +
 183 +       /* This protocol doesn't support cascading multiple
 184 +        * switches so it's safe to assume the switch is first
 185 +        * in the tree.
 186 +        */
 187 +       ds = dst->ds[0];
 188 +       if (!ds)
 189 +               goto out_drop;
 190 +
 191 +       /* Get source port information */
 192 +       port = (hdr & MTK_HDR_RECV_SOURCE_PORT_MASK);
 193 +       if (!ds->ports[port].netdev)
 194 +               goto out_drop;
 195 +
 196 +       /* Update skb & forward the frame accordingly */
 197 +       skb_push(skb, ETH_HLEN);
 198 +
 199 +       skb->pkt_type = PACKET_HOST;
 200 +       skb->dev = ds->ports[port].netdev;
 201 +       skb->protocol = eth_type_trans(skb, skb->dev);
 202 +
 203 +       skb->dev->stats.rx_packets++;
 204 +       skb->dev->stats.rx_bytes += skb->len;
 205 +
 206 +       netif_receive_skb(skb);
 207 +
 208 +       return 0;
 209 +
 210 +out_drop:
 211 +       kfree_skb(skb);
 212 +out:
 213 +       return 0;
 214 +}
 215 +
 216 +const struct dsa_device_ops mtk_netdev_ops = {
 217 +       .xmit   = mtk_tag_xmit,
 218 +       .rcv    = mtk_tag_rcv,
 219 +};