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