target/linux/ramips/patches-4.3/0513-net-mediatek-add-swconfig-driver-for-gsw_mt762x.patch

   1 From cf5a08f1f16913da8bb24a96afaa2969b29d0827 Mon Sep 17 00:00:00 2001
   2 From: John Crispin <blogic@openwrt.org>
   3 Date: Mon, 14 Dec 2015 22:25:57 +0100
   4 Subject: [PATCH 513/513] net: mediatek: add swconfig driver for gsw_mt762x
   5
   6 Signed-off-by: John Crispin <blogic@openwrt.org>
   7 ---
   8  drivers/net/ethernet/mediatek/Makefile      |    4 +-
   9  drivers/net/ethernet/mediatek/mt7530.c      |  804 +++++++++++++++++++++++++++
  10  drivers/net/ethernet/mediatek/mt7530.h      |   20 +
  11  drivers/net/ethernet/mediatek/mtk_eth_soc.c |    9 +-
  12  drivers/net/ethernet/mediatek/mtk_eth_soc.h |    1 +
  13  drivers/net/ethernet/mediatek/soc_mt7620.c  |    1 +
  14  6 files changed, 835 insertions(+), 4 deletions(-)
  15  create mode 100644 drivers/net/ethernet/mediatek/mt7530.c
  16  create mode 100644 drivers/net/ethernet/mediatek/mt7530.h
  17
  18 diff --git a/drivers/net/ethernet/mediatek/Makefile b/drivers/net/ethernet/mediatek/Makefile
  19 index c4d2dfb..07ba4c2 100644
  20 --- a/drivers/net/ethernet/mediatek/Makefile
  21 +++ b/drivers/net/ethernet/mediatek/Makefile
  22 @@ -15,6 +15,6 @@ mtk-eth-soc-$(CONFIG_NET_MEDIATEK_MT7620)     += soc_mt7620.o
  23  mtk-eth-soc-$(CONFIG_NET_MEDIATEK_MT7621)      += soc_mt7621.o
  24
  25  obj-$(CONFIG_NET_MEDIATEK_ESW_RT3050)          += esw_rt3050.o
  26 -obj-$(CONFIG_NET_MEDIATEK_GSW_MT7620)          += gsw_mt7620.o
  27 -obj-$(CONFIG_NET_MEDIATEK_GSW_MT7621)          += gsw_mt7621.o
  28 +obj-$(CONFIG_NET_MEDIATEK_GSW_MT7620)          += gsw_mt7620.o mt7530.o
  29 +obj-$(CONFIG_NET_MEDIATEK_GSW_MT7621)          += gsw_mt7621.o mt7530.o
  30  obj-$(CONFIG_NET_MEDIATEK_SOC)                 += mtk-eth-soc.o
  31 diff --git a/drivers/net/ethernet/mediatek/mt7530.c b/drivers/net/ethernet/mediatek/mt7530.c
  32 new file mode 100644
  33 index 0000000..4d9980d
  34 --- /dev/null
  35 +++ b/drivers/net/ethernet/mediatek/mt7530.c
  36 @@ -0,0 +1,804 @@
  37 +/*
  38 + * This program is free software; you can redistribute it and/or
  39 + * modify it under the terms of the GNU General Public License
  40 + * as published by the Free Software Foundation; either version 2
  41 + * of the License, or (at your option) any later version.
  42 + *
  43 + * This program is distributed in the hope that it will be useful,
  44 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
  45 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  46 + * GNU General Public License for more details.
  47 + *
  48 + * Copyright (C) 2013 John Crispin <blogic@openwrt.org>
  49 + */
  50 +
  51 +#include <linux/if.h>
  52 +#include <linux/module.h>
  53 +#include <linux/init.h>
  54 +#include <linux/list.h>
  55 +#include <linux/if_ether.h>
  56 +#include <linux/skbuff.h>
  57 +#include <linux/netdevice.h>
  58 +#include <linux/netlink.h>
  59 +#include <linux/bitops.h>
  60 +#include <net/genetlink.h>
  61 +#include <linux/switch.h>
  62 +#include <linux/delay.h>
  63 +#include <linux/phy.h>
  64 +#include <linux/netdevice.h>
  65 +#include <linux/etherdevice.h>
  66 +#include <linux/lockdep.h>
  67 +#include <linux/workqueue.h>
  68 +#include <linux/of_device.h>
  69 +
  70 +#include "mt7530.h"
  71 +
  72 +#define MT7530_CPU_PORT                6
  73 +#define MT7530_NUM_PORTS       8
  74 +#define MT7530_NUM_VLANS       16
  75 +#define MT7530_MAX_VID         4095
  76 +#define MT7530_MIN_VID         0
  77 +
  78 +/* registers */
  79 +#define REG_ESW_VLAN_VTCR              0x90
  80 +#define REG_ESW_VLAN_VAWD1             0x94
  81 +#define REG_ESW_VLAN_VAWD2             0x98
  82 +#define REG_ESW_VLAN_VTIM(x)   (0x100 + 4 * ((x) / 2))
  83 +
  84 +#define REG_ESW_VLAN_VAWD1_IVL_MAC     BIT(30)
  85 +#define REG_ESW_VLAN_VAWD1_VTAG_EN     BIT(28)
  86 +#define REG_ESW_VLAN_VAWD1_VALID       BIT(0)
  87 +
  88 +/* vlan egress mode */
  89 +enum {
  90 +       ETAG_CTRL_UNTAG = 0,
  91 +       ETAG_CTRL_TAG   = 2,
  92 +       ETAG_CTRL_SWAP  = 1,
  93 +       ETAG_CTRL_STACK = 3,
  94 +};
  95 +
  96 +#define REG_ESW_PORT_PCR(x)    (0x2004 | ((x) << 8))
  97 +#define REG_ESW_PORT_PVC(x)    (0x2010 | ((x) << 8))
  98 +#define REG_ESW_PORT_PPBV1(x)  (0x2014 | ((x) << 8))
  99 +
 100 +#define REG_HWTRAP             0x7804
 101 +
 102 +#define MIB_DESC(_s , _o, _n)   \
 103 +       {                       \
 104 +               .size = (_s),   \
 105 +               .offset = (_o), \
 106 +               .name = (_n),   \
 107 +       }
 108 +
 109 +struct mt7xxx_mib_desc {
 110 +       unsigned int size;
 111 +       unsigned int offset;
 112 +       const char *name;
 113 +};
 114 +
 115 +#define MT7621_MIB_COUNTER_BASE        0x4000
 116 +#define MT7621_MIB_COUNTER_PORT_OFFSET 0x100
 117 +#define MT7621_STATS_TDPC      0x00
 118 +#define MT7621_STATS_TCRC      0x04
 119 +#define MT7621_STATS_TUPC      0x08
 120 +#define MT7621_STATS_TMPC      0x0C
 121 +#define MT7621_STATS_TBPC      0x10
 122 +#define MT7621_STATS_TCEC      0x14
 123 +#define MT7621_STATS_TSCEC     0x18
 124 +#define MT7621_STATS_TMCEC     0x1C
 125 +#define MT7621_STATS_TDEC      0x20
 126 +#define MT7621_STATS_TLCEC     0x24
 127 +#define MT7621_STATS_TXCEC     0x28
 128 +#define MT7621_STATS_TPPC      0x2C
 129 +#define MT7621_STATS_TL64PC    0x30
 130 +#define MT7621_STATS_TL65PC    0x34
 131 +#define MT7621_STATS_TL128PC   0x38
 132 +#define MT7621_STATS_TL256PC   0x3C
 133 +#define MT7621_STATS_TL512PC   0x40
 134 +#define MT7621_STATS_TL1024PC  0x44
 135 +#define MT7621_STATS_TOC       0x48
 136 +#define MT7621_STATS_RDPC      0x60
 137 +#define MT7621_STATS_RFPC      0x64
 138 +#define MT7621_STATS_RUPC      0x68
 139 +#define MT7621_STATS_RMPC      0x6C
 140 +#define MT7621_STATS_RBPC      0x70
 141 +#define MT7621_STATS_RAEPC     0x74
 142 +#define MT7621_STATS_RCEPC     0x78
 143 +#define MT7621_STATS_RUSPC     0x7C
 144 +#define MT7621_STATS_RFEPC     0x80
 145 +#define MT7621_STATS_ROSPC     0x84
 146 +#define MT7621_STATS_RJEPC     0x88
 147 +#define MT7621_STATS_RPPC      0x8C
 148 +#define MT7621_STATS_RL64PC    0x90
 149 +#define MT7621_STATS_RL65PC    0x94
 150 +#define MT7621_STATS_RL128PC   0x98
 151 +#define MT7621_STATS_RL256PC   0x9C
 152 +#define MT7621_STATS_RL512PC   0xA0
 153 +#define MT7621_STATS_RL1024PC  0xA4
 154 +#define MT7621_STATS_ROC       0xA8
 155 +#define MT7621_STATS_RDPC_CTRL 0xB0
 156 +#define MT7621_STATS_RDPC_ING  0xB4
 157 +#define MT7621_STATS_RDPC_ARL  0xB8
 158 +
 159 +static const struct mt7xxx_mib_desc mt7621_mibs[] = {
 160 +       MIB_DESC(1, MT7621_STATS_TDPC, "TxDrop"),
 161 +       MIB_DESC(1, MT7621_STATS_TCRC, "TxCRC"),
 162 +       MIB_DESC(1, MT7621_STATS_TUPC, "TxUni"),
 163 +       MIB_DESC(1, MT7621_STATS_TMPC, "TxMulti"),
 164 +       MIB_DESC(1, MT7621_STATS_TBPC, "TxBroad"),
 165 +       MIB_DESC(1, MT7621_STATS_TCEC, "TxCollision"),
 166 +       MIB_DESC(1, MT7621_STATS_TSCEC, "TxSingleCol"),
 167 +       MIB_DESC(1, MT7621_STATS_TMCEC, "TxMultiCol"),
 168 +       MIB_DESC(1, MT7621_STATS_TDEC, "TxDefer"),
 169 +       MIB_DESC(1, MT7621_STATS_TLCEC, "TxLateCol"),
 170 +       MIB_DESC(1, MT7621_STATS_TXCEC, "TxExcCol"),
 171 +       MIB_DESC(1, MT7621_STATS_TPPC, "TxPause"),
 172 +       MIB_DESC(1, MT7621_STATS_TL64PC, "Tx64Byte"),
 173 +       MIB_DESC(1, MT7621_STATS_TL65PC, "Tx65Byte"),
 174 +       MIB_DESC(1, MT7621_STATS_TL128PC, "Tx128Byte"),
 175 +       MIB_DESC(1, MT7621_STATS_TL256PC, "Tx256Byte"),
 176 +       MIB_DESC(1, MT7621_STATS_TL512PC, "Tx512Byte"),
 177 +       MIB_DESC(1, MT7621_STATS_TL1024PC, "Tx1024Byte"),
 178 +       MIB_DESC(2, MT7621_STATS_TOC, "TxByte"),
 179 +       MIB_DESC(1, MT7621_STATS_RDPC, "RxDrop"),
 180 +       MIB_DESC(1, MT7621_STATS_RFPC, "RxFiltered"),
 181 +       MIB_DESC(1, MT7621_STATS_RUPC, "RxUni"),
 182 +       MIB_DESC(1, MT7621_STATS_RMPC, "RxMulti"),
 183 +       MIB_DESC(1, MT7621_STATS_RBPC, "RxBroad"),
 184 +       MIB_DESC(1, MT7621_STATS_RAEPC, "RxAlignErr"),
 185 +       MIB_DESC(1, MT7621_STATS_RCEPC, "RxCRC"),
 186 +       MIB_DESC(1, MT7621_STATS_RUSPC, "RxUnderSize"),
 187 +       MIB_DESC(1, MT7621_STATS_RFEPC, "RxFragment"),
 188 +       MIB_DESC(1, MT7621_STATS_ROSPC, "RxOverSize"),
 189 +       MIB_DESC(1, MT7621_STATS_RJEPC, "RxJabber"),
 190 +       MIB_DESC(1, MT7621_STATS_RPPC, "RxPause"),
 191 +       MIB_DESC(1, MT7621_STATS_RL64PC, "Rx64Byte"),
 192 +       MIB_DESC(1, MT7621_STATS_RL65PC, "Rx65Byte"),
 193 +       MIB_DESC(1, MT7621_STATS_RL128PC, "Rx128Byte"),
 194 +       MIB_DESC(1, MT7621_STATS_RL256PC, "Rx256Byte"),
 195 +       MIB_DESC(1, MT7621_STATS_RL512PC, "Rx512Byte"),
 196 +       MIB_DESC(1, MT7621_STATS_RL1024PC, "Rx1024Byte"),
 197 +       MIB_DESC(2, MT7621_STATS_ROC, "RxByte"),
 198 +       MIB_DESC(1, MT7621_STATS_RDPC_CTRL, "RxCtrlDrop"),
 199 +       MIB_DESC(1, MT7621_STATS_RDPC_ING, "RxIngDrop"),
 200 +       MIB_DESC(1, MT7621_STATS_RDPC_ARL, "RxARLDrop")
 201 +};
 202 +
 203 +enum {
 204 +       /* Global attributes. */
 205 +       MT7530_ATTR_ENABLE_VLAN,
 206 +};
 207 +
 208 +struct mt7530_port_entry {
 209 +       u16     pvid;
 210 +};
 211 +
 212 +struct mt7530_vlan_entry {
 213 +       u16     vid;
 214 +       u8      member;
 215 +       u8      etags;
 216 +};
 217 +
 218 +struct mt7530_priv {
 219 +       void __iomem            *base;
 220 +       struct mii_bus          *bus;
 221 +       struct switch_dev       swdev;
 222 +
 223 +       bool                    global_vlan_enable;
 224 +       struct mt7530_vlan_entry        vlan_entries[MT7530_NUM_VLANS];
 225 +       struct mt7530_port_entry        port_entries[MT7530_NUM_PORTS];
 226 +};
 227 +
 228 +struct mt7530_mapping {
 229 +       char    *name;
 230 +       u16     pvids[MT7530_NUM_PORTS];
 231 +       u8      members[MT7530_NUM_VLANS];
 232 +       u8      etags[MT7530_NUM_VLANS];
 233 +       u16     vids[MT7530_NUM_VLANS];
 234 +} mt7530_defaults[] = {
 235 +       {
 236 +               .name = "llllw",
 237 +               .pvids = { 1, 1, 1, 1, 2, 1, 1 },
 238 +               .members = { 0, 0x6f, 0x50 },
 239 +               .etags = { 0, 0x40, 0x40 },
 240 +               .vids = { 0, 1, 2 },
 241 +       }, {
 242 +               .name = "wllll",
 243 +               .pvids = { 2, 1, 1, 1, 1, 1, 1 },
 244 +               .members = { 0, 0x7e, 0x41 },
 245 +               .etags = { 0, 0x40, 0x40 },
 246 +               .vids = { 0, 1, 2 },
 247 +       },
 248 +};
 249 +
 250 +struct mt7530_mapping*
 251 +mt7530_find_mapping(struct device_node *np)
 252 +{
 253 +       const char *map;
 254 +       int i;
 255 +
 256 +       if (of_property_read_string(np, "mediatek,portmap", &map))
 257 +               return NULL;
 258 +
 259 +       for (i = 0; i < ARRAY_SIZE(mt7530_defaults); i++)
 260 +               if (!strcmp(map, mt7530_defaults[i].name))
 261 +                       return &mt7530_defaults[i];
 262 +
 263 +       return NULL;
 264 +}
 265 +
 266 +static void
 267 +mt7530_apply_mapping(struct mt7530_priv *mt7530, struct mt7530_mapping *map)
 268 +{
 269 +       int i = 0;
 270 +
 271 +       for (i = 0; i < MT7530_NUM_PORTS; i++)
 272 +               mt7530->port_entries[i].pvid = map->pvids[i];
 273 +
 274 +       for (i = 0; i < MT7530_NUM_VLANS; i++) {
 275 +               mt7530->vlan_entries[i].member = map->members[i];
 276 +               mt7530->vlan_entries[i].etags = map->etags[i];
 277 +               mt7530->vlan_entries[i].vid = map->vids[i];
 278 +       }
 279 +}
 280 +
 281 +static int
 282 +mt7530_reset_switch(struct switch_dev *dev)
 283 +{
 284 +       struct mt7530_priv *priv = container_of(dev, struct mt7530_priv, swdev);
 285 +       int i;
 286 +
 287 +       memset(priv->port_entries, 0, sizeof(priv->port_entries));
 288 +       memset(priv->vlan_entries, 0, sizeof(priv->vlan_entries));
 289 +
 290 +       /* set default vid of each vlan to the same number of vlan, so the vid
 291 +        * won't need be set explicitly.
 292 +        */
 293 +       for (i = 0; i < MT7530_NUM_VLANS; i++) {
 294 +               priv->vlan_entries[i].vid = i;
 295 +       }
 296 +
 297 +       return 0;
 298 +}
 299 +
 300 +static int
 301 +mt7530_get_vlan_enable(struct switch_dev *dev,
 302 +                          const struct switch_attr *attr,
 303 +                          struct switch_val *val)
 304 +{
 305 +       struct mt7530_priv *priv = container_of(dev, struct mt7530_priv, swdev);
 306 +
 307 +       val->value.i = priv->global_vlan_enable;
 308 +
 309 +       return 0;
 310 +}
 311 +
 312 +static int
 313 +mt7530_set_vlan_enable(struct switch_dev *dev,
 314 +                          const struct switch_attr *attr,
 315 +                          struct switch_val *val)
 316 +{
 317 +       struct mt7530_priv *priv = container_of(dev, struct mt7530_priv, swdev);
 318 +
 319 +       priv->global_vlan_enable = val->value.i != 0;
 320 +
 321 +       return 0;
 322 +}
 323 +
 324 +static u32
 325 +mt7530_r32(struct mt7530_priv *priv, u32 reg)
 326 +{
 327 +       u32 val;
 328 +       if (priv->bus) {
 329 +               u16 high, low;
 330 +
 331 +               mdiobus_write(priv->bus, 0x1f, 0x1f, (reg >> 6) & 0x3ff);
 332 +               low = mdiobus_read(priv->bus, 0x1f, (reg >> 2) & 0xf);
 333 +               high = mdiobus_read(priv->bus, 0x1f, 0x10);
 334 +
 335 +               return (high << 16) | (low & 0xffff);
 336 +       }
 337 +
 338 +       val = ioread32(priv->base + reg);
 339 +       pr_debug("MT7530 MDIO Read [%04x]=%08x\n", reg, val);
 340 +
 341 +       return val;
 342 +}
 343 +
 344 +static void
 345 +mt7530_w32(struct mt7530_priv *priv, u32 reg, u32 val)
 346 +{
 347 +       if (priv->bus) {
 348 +               mdiobus_write(priv->bus, 0x1f, 0x1f, (reg >> 6) & 0x3ff);
 349 +               mdiobus_write(priv->bus, 0x1f, (reg >> 2) & 0xf,  val & 0xffff);
 350 +               mdiobus_write(priv->bus, 0x1f, 0x10, val >> 16);
 351 +               return;
 352 +       }
 353 +
 354 +       pr_debug("MT7530 MDIO Write[%04x]=%08x\n", reg, val);
 355 +       iowrite32(val, priv->base + reg);
 356 +}
 357 +
 358 +static void
 359 +mt7530_vtcr(struct mt7530_priv *priv, u32 cmd, u32 val)
 360 +{
 361 +       int i;
 362 +
 363 +       mt7530_w32(priv, REG_ESW_VLAN_VTCR, BIT(31) | (cmd << 12) | val);
 364 +
 365 +       for (i = 0; i < 20; i++) {
 366 +               u32 val = mt7530_r32(priv, REG_ESW_VLAN_VTCR);
 367 +
 368 +               if ((val & BIT(31)) == 0)
 369 +                       break;
 370 +
 371 +               udelay(1000);
 372 +       }
 373 +       if (i == 20)
 374 +               printk("mt7530: vtcr timeout\n");
 375 +}
 376 +
 377 +static int
 378 +mt7530_get_port_pvid(struct switch_dev *dev, int port, int *val)
 379 +{
 380 +       struct mt7530_priv *priv = container_of(dev, struct mt7530_priv, swdev);
 381 +
 382 +       if (port >= MT7530_NUM_PORTS)
 383 +               return -EINVAL;
 384 +
 385 +       *val = mt7530_r32(priv, REG_ESW_PORT_PPBV1(port));
 386 +       *val &= 0xfff;
 387 +
 388 +       return 0;
 389 +}
 390 +
 391 +static int
 392 +mt7530_set_port_pvid(struct switch_dev *dev, int port, int pvid)
 393 +{
 394 +       struct mt7530_priv *priv = container_of(dev, struct mt7530_priv, swdev);
 395 +
 396 +       if (port >= MT7530_NUM_PORTS)
 397 +               return -EINVAL;
 398 +
 399 +       if (pvid < MT7530_MIN_VID || pvid > MT7530_MAX_VID)
 400 +               return -EINVAL;
 401 +
 402 +       priv->port_entries[port].pvid = pvid;
 403 +
 404 +       return 0;
 405 +}
 406 +
 407 +static int
 408 +mt7530_get_vlan_ports(struct switch_dev *dev, struct switch_val *val)
 409 +{
 410 +       struct mt7530_priv *priv = container_of(dev, struct mt7530_priv, swdev);
 411 +       u32 member;
 412 +       u32 etags;
 413 +       int i;
 414 +
 415 +       val->len = 0;
 416 +
 417 +       if (val->port_vlan < 0 || val->port_vlan >= MT7530_NUM_VLANS)
 418 +               return -EINVAL;
 419 +
 420 +       mt7530_vtcr(priv, 0, val->port_vlan);
 421 +
 422 +       member = mt7530_r32(priv, REG_ESW_VLAN_VAWD1);
 423 +       member >>= 16;
 424 +       member &= 0xff;
 425 +
 426 +       etags = mt7530_r32(priv, REG_ESW_VLAN_VAWD2);
 427 +
 428 +       for (i = 0; i < MT7530_NUM_PORTS; i++) {
 429 +               struct switch_port *p;
 430 +               int etag;
 431 +
 432 +               if (!(member & BIT(i)))
 433 +                       continue;
 434 +
 435 +               p = &val->value.ports[val->len++];
 436 +               p->id = i;
 437 +
 438 +               etag = (etags >> (i * 2)) & 0x3;
 439 +
 440 +               if (etag == ETAG_CTRL_TAG)
 441 +                       p->flags |= BIT(SWITCH_PORT_FLAG_TAGGED);
 442 +               else if (etag != ETAG_CTRL_UNTAG)
 443 +                       printk("vlan egress tag control neither untag nor tag.\n");
 444 +       }
 445 +
 446 +       return 0;
 447 +}
 448 +
 449 +static int
 450 +mt7530_set_vlan_ports(struct switch_dev *dev, struct switch_val *val)
 451 +{
 452 +       struct mt7530_priv *priv = container_of(dev, struct mt7530_priv, swdev);
 453 +       u8 member = 0;
 454 +       u8 etags = 0;
 455 +       int i;
 456 +
 457 +       if (val->port_vlan < 0 || val->port_vlan >= MT7530_NUM_VLANS ||
 458 +                       val->len > MT7530_NUM_PORTS)
 459 +               return -EINVAL;
 460 +
 461 +       for (i = 0; i < val->len; i++) {
 462 +               struct switch_port *p = &val->value.ports[i];
 463 +
 464 +               if (p->id >= MT7530_NUM_PORTS)
 465 +                       return -EINVAL;
 466 +
 467 +               member |= BIT(p->id);
 468 +
 469 +               if (p->flags & BIT(SWITCH_PORT_FLAG_TAGGED))
 470 +                       etags |= BIT(p->id);
 471 +       }
 472 +       priv->vlan_entries[val->port_vlan].member = member;
 473 +       priv->vlan_entries[val->port_vlan].etags = etags;
 474 +
 475 +       return 0;
 476 +}
 477 +
 478 +static int
 479 +mt7530_set_vid(struct switch_dev *dev, const struct switch_attr *attr,
 480 +               struct switch_val *val)
 481 +{
 482 +       struct mt7530_priv *priv = container_of(dev, struct mt7530_priv, swdev);
 483 +       int vlan;
 484 +       u16 vid;
 485 +
 486 +       vlan = val->port_vlan;
 487 +       vid = (u16)val->value.i;
 488 +
 489 +       if (vlan < 0 || vlan >= MT7530_NUM_VLANS)
 490 +               return -EINVAL;
 491 +
 492 +       if (vid < MT7530_MIN_VID || vid > MT7530_MAX_VID)
 493 +               return -EINVAL;
 494 +
 495 +       priv->vlan_entries[vlan].vid = vid;
 496 +       return 0;
 497 +}
 498 +
 499 +static int
 500 +mt7530_get_vid(struct switch_dev *dev, const struct switch_attr *attr,
 501 +               struct switch_val *val)
 502 +{
 503 +       struct mt7530_priv *priv = container_of(dev, struct mt7530_priv, swdev);
 504 +       u32 vid;
 505 +       int vlan;
 506 +
 507 +       vlan = val->port_vlan;
 508 +
 509 +       vid = mt7530_r32(priv, REG_ESW_VLAN_VTIM(vlan));
 510 +       if (vlan & 1)
 511 +               vid = vid >> 12;
 512 +       vid &= 0xfff;
 513 +
 514 +       val->value.i = vid;
 515 +       return 0;
 516 +}
 517 +
 518 +static int
 519 +mt7530_apply_config(struct switch_dev *dev)
 520 +{
 521 +       struct mt7530_priv *priv = container_of(dev, struct mt7530_priv, swdev);
 522 +       int i, j;
 523 +       u8 tag_ports;
 524 +       u8 untag_ports;
 525 +
 526 +       if (!priv->global_vlan_enable) {
 527 +               for (i = 0; i < MT7530_NUM_PORTS; i++)
 528 +                       mt7530_w32(priv, REG_ESW_PORT_PCR(i), 0x00ff0000);
 529 +
 530 +               for (i = 0; i < MT7530_NUM_PORTS; i++)
 531 +                       mt7530_w32(priv, REG_ESW_PORT_PVC(i), 0x810000c0);
 532 +
 533 +               return 0;
 534 +       }
 535 +
 536 +       /* set all ports as security mode */
 537 +       for (i = 0; i < MT7530_NUM_PORTS; i++)
 538 +               mt7530_w32(priv, REG_ESW_PORT_PCR(i), 0x00ff0003);
 539 +
 540 +       /* check if a port is used in tag/untag vlan egress mode */
 541 +       tag_ports = 0;
 542 +       untag_ports = 0;
 543 +
 544 +       for (i = 0; i < MT7530_NUM_VLANS; i++) {
 545 +               u8 member = priv->vlan_entries[i].member;
 546 +               u8 etags = priv->vlan_entries[i].etags;
 547 +
 548 +               if (!member)
 549 +                       continue;
 550 +
 551 +               for (j = 0; j < MT7530_NUM_PORTS; j++) {
 552 +                       if (!(member & BIT(j)))
 553 +                               continue;
 554 +
 555 +                       if (etags & BIT(j))
 556 +                               tag_ports |= 1u << j;
 557 +                       else
 558 +                               untag_ports |= 1u << j;
 559 +               }
 560 +       }
 561 +
 562 +       /* set all untag-only ports as transparent and the rest as user port */
 563 +       for (i = 0; i < MT7530_NUM_PORTS; i++) {
 564 +               u32 pvc_mode = 0x81000000;
 565 +
 566 +               if (untag_ports & BIT(i) && !(tag_ports & BIT(i)))
 567 +                       pvc_mode = 0x810000c0;
 568 +
 569 +               mt7530_w32(priv, REG_ESW_PORT_PVC(i), pvc_mode);
 570 +       }
 571 +
 572 +       for (i = 0; i < MT7530_NUM_VLANS; i++) {
 573 +               u16 vid = priv->vlan_entries[i].vid;
 574 +               u8 member = priv->vlan_entries[i].member;
 575 +               u8 etags = priv->vlan_entries[i].etags;
 576 +               u32 val;
 577 +
 578 +               /* vid of vlan */
 579 +               val = mt7530_r32(priv, REG_ESW_VLAN_VTIM(i));
 580 +               if (i % 2 == 0) {
 581 +                       val &= 0xfff000;
 582 +                       val |= vid;
 583 +               } else {
 584 +                       val &= 0xfff;
 585 +                       val |= (vid << 12);
 586 +               }
 587 +               mt7530_w32(priv, REG_ESW_VLAN_VTIM(i), val);
 588 +
 589 +               /* vlan port membership */
 590 +               if (member)
 591 +                       mt7530_w32(priv, REG_ESW_VLAN_VAWD1, REG_ESW_VLAN_VAWD1_IVL_MAC |
 592 +                               REG_ESW_VLAN_VAWD1_VTAG_EN | (member << 16) |
 593 +                               REG_ESW_VLAN_VAWD1_VALID);
 594 +               else
 595 +                       mt7530_w32(priv, REG_ESW_VLAN_VAWD1, 0);
 596 +
 597 +               /* egress mode */
 598 +               val = 0;
 599 +               for (j = 0; j < MT7530_NUM_PORTS; j++) {
 600 +                       if (etags & BIT(j))
 601 +                               val |= ETAG_CTRL_TAG << (j * 2);
 602 +                       else
 603 +                               val |= ETAG_CTRL_UNTAG << (j * 2);
 604 +               }
 605 +               mt7530_w32(priv, REG_ESW_VLAN_VAWD2, val);
 606 +
 607 +               /* write to vlan table */
 608 +               mt7530_vtcr(priv, 1, i);
 609 +       }
 610 +
 611 +       /* Port Default PVID */
 612 +       for (i = 0; i < MT7530_NUM_PORTS; i++) {
 613 +               u32 val;
 614 +               val = mt7530_r32(priv, REG_ESW_PORT_PPBV1(i));
 615 +               val &= ~0xfff;
 616 +               val |= priv->port_entries[i].pvid;
 617 +               mt7530_w32(priv, REG_ESW_PORT_PPBV1(i), val);
 618 +       }
 619 +
 620 +       return 0;
 621 +}
 622 +
 623 +static int
 624 +mt7530_get_port_link(struct switch_dev *dev,  int port,
 625 +                       struct switch_port_link *link)
 626 +{
 627 +       struct mt7530_priv *priv = container_of(dev, struct mt7530_priv, swdev);
 628 +       u32 speed, pmsr;
 629 +
 630 +       if (port < 0 || port >= MT7530_NUM_PORTS)
 631 +               return -EINVAL;
 632 +
 633 +       pmsr = mt7530_r32(priv, 0x3008 + (0x100 * port));
 634 +
 635 +       link->link = pmsr & 1;
 636 +       link->duplex = (pmsr >> 1) & 1;
 637 +       speed = (pmsr >> 2) & 3;
 638 +
 639 +       switch (speed) {
 640 +       case 0:
 641 +               link->speed = SWITCH_PORT_SPEED_10;
 642 +               break;
 643 +       case 1:
 644 +               link->speed = SWITCH_PORT_SPEED_100;
 645 +               break;
 646 +       case 2:
 647 +       case 3: /* forced gige speed can be 2 or 3 */
 648 +               link->speed = SWITCH_PORT_SPEED_1000;
 649 +               break;
 650 +       default:
 651 +               link->speed = SWITCH_PORT_SPEED_UNKNOWN;
 652 +               break;
 653 +       }
 654 +
 655 +       return 0;
 656 +}
 657 +
 658 +static const struct switch_attr mt7530_global[] = {
 659 +       {
 660 +               .type = SWITCH_TYPE_INT,
 661 +               .name = "enable_vlan",
 662 +               .description = "VLAN mode (1:enabled)",
 663 +               .max = 1,
 664 +               .id = MT7530_ATTR_ENABLE_VLAN,
 665 +               .get = mt7530_get_vlan_enable,
 666 +               .set = mt7530_set_vlan_enable,
 667 +       },
 668 +};
 669 +
 670 +static u64 get_mib_counter(struct mt7530_priv *priv, int i, int port)
 671 +{
 672 +       unsigned int port_base;
 673 +       u64 t;
 674 +
 675 +       port_base = MT7621_MIB_COUNTER_BASE +
 676 +                   MT7621_MIB_COUNTER_PORT_OFFSET * port;
 677 +
 678 +       t = mt7530_r32(priv, port_base + mt7621_mibs[i].offset);
 679 +       if (mt7621_mibs[i].size == 2) {
 680 +               u64 hi;
 681 +
 682 +               hi = mt7530_r32(priv, port_base + mt7621_mibs[i].offset + 4);
 683 +               t |= hi << 32;
 684 +       }
 685 +
 686 +       return t;
 687 +}
 688 +
 689 +static int mt7621_sw_get_port_mib(struct switch_dev *dev,
 690 +                                 const struct switch_attr *attr,
 691 +                                 struct switch_val *val)
 692 +{
 693 +       static char buf[4096];
 694 +       struct mt7530_priv *priv = container_of(dev, struct mt7530_priv, swdev);
 695 +       int i, len = 0;
 696 +
 697 +       if (val->port_vlan >= MT7530_NUM_PORTS)
 698 +               return -EINVAL;
 699 +
 700 +       len += snprintf(buf + len, sizeof(buf) - len,
 701 +                       "Port %d MIB counters\n", val->port_vlan);
 702 +
 703 +       for (i = 0; i < sizeof(mt7621_mibs) / sizeof(*mt7621_mibs); ++i) {
 704 +               u64 counter;
 705 +               len += snprintf(buf + len, sizeof(buf) - len,
 706 +                               "%-11s: ", mt7621_mibs[i].name);
 707 +               counter = get_mib_counter(priv, i, val->port_vlan);
 708 +               len += snprintf(buf + len, sizeof(buf) - len, "%llu\n",
 709 +                               counter);
 710 +       }
 711 +
 712 +       val->value.s = buf;
 713 +       val->len = len;
 714 +       return 0;
 715 +}
 716 +
 717 +static const struct switch_attr mt7621_port[] = {
 718 +       {
 719 +               .type = SWITCH_TYPE_STRING,
 720 +               .name = "mib",
 721 +               .description = "Get MIB counters for port",
 722 +               .get = mt7621_sw_get_port_mib,
 723 +               .set = NULL,
 724 +       },
 725 +};
 726 +
 727 +static const struct switch_attr mt7530_port[] = {
 728 +};
 729 +
 730 +static const struct switch_attr mt7530_vlan[] = {
 731 +       {
 732 +               .type = SWITCH_TYPE_INT,
 733 +               .name = "vid",
 734 +               .description = "VLAN ID (0-4094)",
 735 +               .set = mt7530_set_vid,
 736 +               .get = mt7530_get_vid,
 737 +               .max = 4094,
 738 +       },
 739 +};
 740 +
 741 +static const struct switch_dev_ops mt7621_ops = {
 742 +       .attr_global = {
 743 +               .attr = mt7530_global,
 744 +               .n_attr = ARRAY_SIZE(mt7530_global),
 745 +       },
 746 +       .attr_port = {
 747 +               .attr = mt7621_port,
 748 +               .n_attr = ARRAY_SIZE(mt7621_port),
 749 +       },
 750 +       .attr_vlan = {
 751 +               .attr = mt7530_vlan,
 752 +               .n_attr = ARRAY_SIZE(mt7530_vlan),
 753 +       },
 754 +       .get_vlan_ports = mt7530_get_vlan_ports,
 755 +       .set_vlan_ports = mt7530_set_vlan_ports,
 756 +       .get_port_pvid = mt7530_get_port_pvid,
 757 +       .set_port_pvid = mt7530_set_port_pvid,
 758 +       .get_port_link = mt7530_get_port_link,
 759 +       .apply_config = mt7530_apply_config,
 760 +       .reset_switch = mt7530_reset_switch,
 761 +};
 762 +
 763 +static const struct switch_dev_ops mt7530_ops = {
 764 +       .attr_global = {
 765 +               .attr = mt7530_global,
 766 +               .n_attr = ARRAY_SIZE(mt7530_global),
 767 +       },
 768 +       .attr_port = {
 769 +               .attr = mt7530_port,
 770 +               .n_attr = ARRAY_SIZE(mt7530_port),
 771 +       },
 772 +       .attr_vlan = {
 773 +               .attr = mt7530_vlan,
 774 +               .n_attr = ARRAY_SIZE(mt7530_vlan),
 775 +       },
 776 +       .get_vlan_ports = mt7530_get_vlan_ports,
 777 +       .set_vlan_ports = mt7530_set_vlan_ports,
 778 +       .get_port_pvid = mt7530_get_port_pvid,
 779 +       .set_port_pvid = mt7530_set_port_pvid,
 780 +       .get_port_link = mt7530_get_port_link,
 781 +       .apply_config = mt7530_apply_config,
 782 +       .reset_switch = mt7530_reset_switch,
 783 +};
 784 +
 785 +int
 786 +mt7530_probe(struct device *dev, void __iomem *base, struct mii_bus *bus, int vlan)
 787 +{
 788 +       struct switch_dev *swdev;
 789 +       struct mt7530_priv *mt7530;
 790 +       struct mt7530_mapping *map;
 791 +       int ret;
 792 +
 793 +       mt7530 = devm_kzalloc(dev, sizeof(struct mt7530_priv), GFP_KERNEL);
 794 +       if (!mt7530)
 795 +               return -ENOMEM;
 796 +
 797 +       mt7530->base = base;
 798 +       mt7530->bus = bus;
 799 +       mt7530->global_vlan_enable = vlan;
 800 +
 801 +       swdev = &mt7530->swdev;
 802 +       if (bus) {
 803 +               swdev->alias = "mt7530";
 804 +               swdev->name = "mt7530";
 805 +       } else if (IS_ENABLED(CONFIG_SOC_MT7621)) {
 806 +               swdev->alias = "mt7621";
 807 +               swdev->name = "mt7621";
 808 +       } else {
 809 +               swdev->alias = "mt7620";
 810 +               swdev->name = "mt7620";
 811 +       }
 812 +       swdev->cpu_port = MT7530_CPU_PORT;
 813 +       swdev->ports = MT7530_NUM_PORTS;
 814 +       swdev->vlans = MT7530_NUM_VLANS;
 815 +       if (IS_ENABLED(CONFIG_SOC_MT7621))
 816 +               swdev->ops = &mt7621_ops;
 817 +       else
 818 +               swdev->ops = &mt7530_ops;
 819 +
 820 +       ret = register_switch(swdev, NULL);
 821 +       if (ret) {
 822 +               dev_err(dev, "failed to register mt7530\n");
 823 +               return ret;
 824 +       }
 825 +
 826 +
 827 +       map = mt7530_find_mapping(dev->of_node);
 828 +       if (map)
 829 +               mt7530_apply_mapping(mt7530, map);
 830 +       mt7530_apply_config(swdev);
 831 +
 832 +       /* magic vodoo */
 833 +       if (!IS_ENABLED(CONFIG_SOC_MT7621) && bus && mt7530_r32(mt7530, REG_HWTRAP) !=  0x1117edf) {
 834 +               dev_info(dev, "fixing up MHWTRAP register - bootloader probably played with it\n");
 835 +               mt7530_w32(mt7530, REG_HWTRAP, 0x1117edf);
 836 +       }
 837 +       dev_info(dev, "loaded %s driver\n", swdev->name);
 838 +
 839 +       return 0;
 840 +}
 841 diff --git a/drivers/net/ethernet/mediatek/mt7530.h b/drivers/net/ethernet/mediatek/mt7530.h
 842 new file mode 100644
 843 index 0000000..1fc8c62
 844 --- /dev/null
 845 +++ b/drivers/net/ethernet/mediatek/mt7530.h
 846 @@ -0,0 +1,20 @@
 847 +/*
 848 + * This program is free software; you can redistribute it and/or
 849 + * modify it under the terms of the GNU General Public License
 850 + * as published by the Free Software Foundation; either version 2
 851 + * of the License, or (at your option) any later version.
 852 + *
 853 + * This program is distributed in the hope that it will be useful,
 854 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
 855 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 856 + * GNU General Public License for more details.
 857 + *
 858 + * Copyright (C) 2013 John Crispin <blogic@openwrt.org>
 859 + */
 860 +
 861 +#ifndef _MT7530_H__
 862 +#define _MT7530_H__
 863 +
 864 +int mt7530_probe(struct device *dev, void __iomem *base, struct mii_bus *bus, int vlan);
 865 +
 866 +#endif
 867 diff --git a/drivers/net/ethernet/mediatek/mtk_eth_soc.c b/drivers/net/ethernet/mediatek/mtk_eth_soc.c
 868 index dae7147..6299f87 100644
 869 --- a/drivers/net/ethernet/mediatek/mtk_eth_soc.c
 870 +++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.c
 871 @@ -1308,8 +1308,13 @@ static int __init fe_init(struct net_device *dev)
 872         }
 873
 874         err = fe_hw_init(dev);
 875 -       if (!err)
 876 -               return 0;
 877 +       if (err)
 878 +               goto err_phy_disconnect;
 879 +
 880 +       if ((priv->flags & FE_FLAG_HAS_SWITCH) && priv->soc->switch_config)
 881 +               priv->soc->switch_config(priv);
 882 +
 883 +       return 0;
 884
 885  err_phy_disconnect:
 886         if (priv->phy)
 887 diff --git a/drivers/net/ethernet/mediatek/mtk_eth_soc.h b/drivers/net/ethernet/mediatek/mtk_eth_soc.h
 888 index ba5ba07..d5f8b87 100644
 889 --- a/drivers/net/ethernet/mediatek/mtk_eth_soc.h
 890 +++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.h
 891 @@ -383,6 +383,7 @@ struct fe_soc_data {
 892         int (*fwd_config)(struct fe_priv *priv);
 893         void (*tx_dma)(struct fe_tx_dma *txd);
 894         int (*switch_init)(struct fe_priv *priv);
 895 +       int (*switch_config)(struct fe_priv *priv);
 896         void (*port_init)(struct fe_priv *priv, struct device_node *port);
 897         int (*has_carrier)(struct fe_priv *priv);
 898         int (*mdio_init)(struct fe_priv *priv);
 899 diff --git a/drivers/net/ethernet/mediatek/soc_mt7620.c b/drivers/net/ethernet/mediatek/soc_mt7620.c
 900 index 9ad6bc9..740dd90 100644
 901 --- a/drivers/net/ethernet/mediatek/soc_mt7620.c
 902 +++ b/drivers/net/ethernet/mediatek/soc_mt7620.c
 903 @@ -313,6 +313,7 @@ static struct fe_soc_data mt7620_data = {
 904         .fwd_config = mt7620_fwd_config,
 905         .tx_dma = mt7620_tx_dma,
 906         .switch_init = mtk_gsw_init,
 907 +       .switch_config = mt7620_gsw_config,
 908         .port_init = mt7620_port_init,
 909         .reg_table = mt7620_reg_table,
 910         .pdma_glo_cfg = FE_PDMA_SIZE_16DWORDS,
 911 --
 912 1.7.10.4
 913