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 --- a/include/net/dsa.h
  40 +++ b/include/net/dsa.h
  41 @@ -27,6 +27,7 @@ enum dsa_tag_protocol {
  42         DSA_TAG_PROTO_EDSA,
  43         DSA_TAG_PROTO_BRCM,
  44         DSA_TAG_PROTO_QCA,
  45 +       DSA_TAG_PROTO_MTK,
  46         DSA_TAG_LAST,           /* MUST BE LAST */
  47  };
  48
  49 --- a/net/dsa/Kconfig
  50 +++ b/net/dsa/Kconfig
  51 @@ -41,4 +41,6 @@ config NET_DSA_TAG_TRAILER
  52  config NET_DSA_TAG_QCA
  53         bool
  54
  55 +config NET_DSA_TAG_MTK
  56 +       bool
  57  endif
  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
  65 --- a/net/dsa/dsa.c
  66 +++ b/net/dsa/dsa.c
  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,
  70  #endif
  71 +#ifdef CONFIG_NET_DSA_TAG_MTK
  72 +       [DSA_TAG_PROTO_MTK] = &mtk_netdev_ops,
  73 +#endif
  74         [DSA_TAG_PROTO_NONE] = &none_ops,
  75  };
  76
  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_
  80  /* tag_qca.c */
  81  extern const struct dsa_device_ops qca_netdev_ops;
  82
  83 +/* tag_mtk.c */
  84 +extern const struct dsa_device_ops mtk_netdev_ops;
  85 +
  86  #endif
  87 --- /dev/null
  88 +++ b/net/dsa/tag_mtk.c
  89 @@ -0,0 +1,117 @@
  90 +/*
  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.
  97 + *
  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.
 102 + */
 103 +
 104 +#include <linux/etherdevice.h>
 105 +#include "dsa_priv.h"
 106 +
 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)
 110 +
 111 +static struct sk_buff *mtk_tag_xmit(struct sk_buff *skb,
 112 +                                   struct net_device *dev)
 113 +{
 114 +       struct dsa_slave_priv *p = netdev_priv(dev);
 115 +       u8 *mtk_tag;
 116 +
 117 +       if (skb_cow_head(skb, MTK_HDR_LEN) < 0)
 118 +               goto out_free;
 119 +
 120 +       skb_push(skb, MTK_HDR_LEN);
 121 +
 122 +       memmove(skb->data, skb->data + MTK_HDR_LEN, 2 * ETH_ALEN);
 123 +
 124 +       /* Build the tag after the MAC Source Address */
 125 +       mtk_tag = skb->data + 2 * ETH_ALEN;
 126 +       mtk_tag[0] = 0;
 127 +       mtk_tag[1] = (1 << p->dp->index) & MTK_HDR_XMIT_DP_BIT_MASK;
 128 +       mtk_tag[2] = 0;
 129 +       mtk_tag[3] = 0;
 130 +
 131 +       return skb;
 132 +
 133 +out_free:
 134 +       kfree_skb(skb);
 135 +       return NULL;
 136 +}
 137 +
 138 +static int mtk_tag_rcv(struct sk_buff *skb, struct net_device *dev,
 139 +                      struct packet_type *pt, struct net_device *orig_dev)
 140 +{
 141 +       struct dsa_switch_tree *dst = dev->dsa_ptr;
 142 +       struct dsa_switch *ds;
 143 +       int port;
 144 +       __be16 *phdr, hdr;
 145 +
 146 +       if (unlikely(!dst))
 147 +               goto out_drop;
 148 +
 149 +       skb = skb_unshare(skb, GFP_ATOMIC);
 150 +       if (!skb)
 151 +               goto out;
 152 +
 153 +       if (unlikely(!pskb_may_pull(skb, MTK_HDR_LEN)))
 154 +               goto out_drop;
 155 +
 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.
 159 +        */
 160 +       phdr = (__be16 *)(skb->data - 2);
 161 +       hdr = ntohs(*phdr);
 162 +
 163 +       /* Remove MTK tag and recalculate checksum. */
 164 +       skb_pull_rcsum(skb, MTK_HDR_LEN);
 165 +
 166 +       memmove(skb->data - ETH_HLEN,
 167 +               skb->data - ETH_HLEN - MTK_HDR_LEN,
 168 +               2 * ETH_ALEN);
 169 +
 170 +       /* This protocol doesn't support cascading multiple
 171 +        * switches so it's safe to assume the switch is first
 172 +        * in the tree.
 173 +        */
 174 +       ds = dst->ds[0];
 175 +       if (!ds)
 176 +               goto out_drop;
 177 +
 178 +       /* Get source port information */
 179 +       port = (hdr & MTK_HDR_RECV_SOURCE_PORT_MASK);
 180 +       if (!ds->ports[port].netdev)
 181 +               goto out_drop;
 182 +
 183 +       /* Update skb & forward the frame accordingly */
 184 +       skb_push(skb, ETH_HLEN);
 185 +
 186 +       skb->pkt_type = PACKET_HOST;
 187 +       skb->dev = ds->ports[port].netdev;
 188 +       skb->protocol = eth_type_trans(skb, skb->dev);
 189 +
 190 +       skb->dev->stats.rx_packets++;
 191 +       skb->dev->stats.rx_bytes += skb->len;
 192 +
 193 +       netif_receive_skb(skb);
 194 +
 195 +       return 0;
 196 +
 197 +out_drop:
 198 +       kfree_skb(skb);
 199 +out:
 200 +       return 0;
 201 +}
 202 +
 203 +const struct dsa_device_ops mtk_netdev_ops = {
 204 +       .xmit   = mtk_tag_xmit,
 205 +       .rcv    = mtk_tag_rcv,
 206 +};