target/linux/mediatek/patches-4.9/0028-net-next-dsa-add-Mediatek-tag-RX-TX-handler.patch

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