target/linux/mediatek/patches-4.9/0101-net-mediatek-add-gsw-mt7530-driver.patch

   1 From 6b8a7257e7bcb56782c3f8048311670fe6a80209 Mon Sep 17 00:00:00 2001
   2 From: John Crispin <blogic@openwrt.org>
   3 Date: Mon, 11 Apr 2016 03:11:54 +0200
   4 Subject: [PATCH 101/102] net: mediatek add gsw/mt7530 driver
   5
   6 Signed-off-by: John Crispin <blogic@openwrt.org>
   7 ---
   8  drivers/net/ethernet/mediatek/Makefile      |    2 +-
   9  drivers/net/ethernet/mediatek/gsw_mt7620.h  |  251 +++++++
  10  drivers/net/ethernet/mediatek/gsw_mt7623.c  | 1084 +++++++++++++++++++++++++++
  11  drivers/net/ethernet/mediatek/mt7530.c      |  808 ++++++++++++++++++++
  12  drivers/net/ethernet/mediatek/mt7530.h      |   20 +
  13  drivers/net/ethernet/mediatek/mtk_eth_soc.c |   59 +-
  14  drivers/net/ethernet/mediatek/mtk_eth_soc.h |    5 +
  15  7 files changed, 2199 insertions(+), 30 deletions(-)
  16  create mode 100644 drivers/net/ethernet/mediatek/gsw_mt7620.h
  17  create mode 100644 drivers/net/ethernet/mediatek/gsw_mt7623.c
  18  create mode 100644 drivers/net/ethernet/mediatek/mt7530.c
  19  create mode 100644 drivers/net/ethernet/mediatek/mt7530.h
  20
  21 --- a/drivers/net/ethernet/mediatek/Makefile
  22 +++ b/drivers/net/ethernet/mediatek/Makefile
  23 @@ -2,4 +2,4 @@
  24  # Makefile for the Mediatek SoCs built-in ethernet macs
  25  #
  26
  27 -obj-$(CONFIG_NET_MEDIATEK_SOC)                 += mtk_eth_soc.o
  28 +obj-$(CONFIG_NET_MEDIATEK_SOC)                 += mt7530.o gsw_mt7623.o mtk_eth_soc.o
  29 --- /dev/null
  30 +++ b/drivers/net/ethernet/mediatek/gsw_mt7620.h
  31 @@ -0,0 +1,251 @@
  32 +/*   This program is free software; you can redistribute it and/or modify
  33 + *   it under the terms of the GNU General Public License as published by
  34 + *   the Free Software Foundation; version 2 of the License
  35 + *
  36 + *   This program is distributed in the hope that it will be useful,
  37 + *   but WITHOUT ANY WARRANTY; without even the implied warranty of
  38 + *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  39 + *   GNU General Public License for more details.
  40 + *
  41 + *   Copyright (C) 2009-2016 John Crispin <blogic@openwrt.org>
  42 + *   Copyright (C) 2009-2016 Felix Fietkau <nbd@nbd.name>
  43 + *   Copyright (C) 2013-2016 Michael Lee <igvtee@gmail.com>
  44 + */
  45 +
  46 +#ifndef _RALINK_GSW_MT7620_H__
  47 +#define _RALINK_GSW_MT7620_H__
  48 +
  49 +#define GSW_REG_PHY_TIMEOUT    (5 * HZ)
  50 +
  51 +#define MT7620_GSW_REG_PIAC    0x0004
  52 +
  53 +#define GSW_NUM_VLANS          16
  54 +#define GSW_NUM_VIDS           4096
  55 +#define GSW_NUM_PORTS          7
  56 +#define GSW_PORT6              6
  57 +
  58 +#define GSW_MDIO_ACCESS                BIT(31)
  59 +#define GSW_MDIO_READ          BIT(19)
  60 +#define GSW_MDIO_WRITE         BIT(18)
  61 +#define GSW_MDIO_START         BIT(16)
  62 +#define GSW_MDIO_ADDR_SHIFT    20
  63 +#define GSW_MDIO_REG_SHIFT     25
  64 +
  65 +#define GSW_REG_PORT_PMCR(x)   (0x3000 + (x * 0x100))
  66 +#define GSW_REG_PORT_STATUS(x) (0x3008 + (x * 0x100))
  67 +#define GSW_REG_SMACCR0                0x3fE4
  68 +#define GSW_REG_SMACCR1                0x3fE8
  69 +#define GSW_REG_CKGCR          0x3ff0
  70 +
  71 +#define GSW_REG_IMR            0x7008
  72 +#define GSW_REG_ISR            0x700c
  73 +#define GSW_REG_GPC1           0x7014
  74 +
  75 +#define SYSC_REG_CHIP_REV_ID   0x0c
  76 +#define SYSC_REG_CFG           0x10
  77 +#define SYSC_REG_CFG1          0x14
  78 +#define RST_CTRL_MCM           BIT(2)
  79 +#define SYSC_PAD_RGMII2_MDIO   0x58
  80 +#define SYSC_GPIO_MODE         0x60
  81 +
  82 +#define PORT_IRQ_ST_CHG                0x7f
  83 +
  84 +#define MT7621_ESW_PHY_POLLING 0x0000
  85 +#define MT7620_ESW_PHY_POLLING 0x7000
  86 +
  87 +#define        PMCR_IPG                BIT(18)
  88 +#define        PMCR_MAC_MODE           BIT(16)
  89 +#define        PMCR_FORCE              BIT(15)
  90 +#define        PMCR_TX_EN              BIT(14)
  91 +#define        PMCR_RX_EN              BIT(13)
  92 +#define        PMCR_BACKOFF            BIT(9)
  93 +#define        PMCR_BACKPRES           BIT(8)
  94 +#define        PMCR_RX_FC              BIT(5)
  95 +#define        PMCR_TX_FC              BIT(4)
  96 +#define        PMCR_SPEED(_x)          (_x << 2)
  97 +#define        PMCR_DUPLEX             BIT(1)
  98 +#define        PMCR_LINK               BIT(0)
  99 +
 100 +#define PHY_AN_EN              BIT(31)
 101 +#define PHY_PRE_EN             BIT(30)
 102 +#define PMY_MDC_CONF(_x)       ((_x & 0x3f) << 24)
 103 +
 104 +/* ethernet subsystem config register */
 105 +#define ETHSYS_SYSCFG0         0x14
 106 +/* ethernet subsystem clock register */
 107 +#define ETHSYS_CLKCFG0         0x2c
 108 +#define ETHSYS_TRGMII_CLK_SEL362_5     BIT(11)
 109 +
 110 +/* p5 RGMII wrapper TX clock control register */
 111 +#define MT7530_P5RGMIITXCR     0x7b04
 112 +/* p5 RGMII wrapper RX clock control register */
 113 +#define MT7530_P5RGMIIRXCR     0x7b00
 114 +/* TRGMII TDX ODT registers */
 115 +#define MT7530_TRGMII_TD0_ODT  0x7a54
 116 +#define MT7530_TRGMII_TD1_ODT  0x7a5c
 117 +#define MT7530_TRGMII_TD2_ODT  0x7a64
 118 +#define MT7530_TRGMII_TD3_ODT  0x7a6c
 119 +#define MT7530_TRGMII_TD4_ODT  0x7a74
 120 +#define MT7530_TRGMII_TD5_ODT  0x7a7c
 121 +/* TRGMII TCK ctrl register */
 122 +#define MT7530_TRGMII_TCK_CTRL 0x7a78
 123 +/* TRGMII Tx ctrl register */
 124 +#define MT7530_TRGMII_TXCTRL   0x7a40
 125 +/* port 6 extended control register */
 126 +#define MT7530_P6ECR            0x7830
 127 +/* IO driver control register */
 128 +#define MT7530_IO_DRV_CR       0x7810
 129 +/* top signal control register */
 130 +#define MT7530_TOP_SIG_CTRL    0x7808
 131 +/* modified hwtrap register */
 132 +#define MT7530_MHWTRAP         0x7804
 133 +/* hwtrap status register */
 134 +#define MT7530_HWTRAP          0x7800
 135 +/* status interrupt register */
 136 +#define MT7530_SYS_INT_STS     0x700c
 137 +/* system nterrupt register */
 138 +#define MT7530_SYS_INT_EN      0x7008
 139 +/* system control register */
 140 +#define MT7530_SYS_CTRL                0x7000
 141 +/* port MAC status register */
 142 +#define MT7530_PMSR_P(x)       (0x3008 + (x * 0x100))
 143 +/* port MAC control register */
 144 +#define MT7530_PMCR_P(x)       (0x3000 + (x * 0x100))
 145 +
 146 +#define MT7621_XTAL_SHIFT      6
 147 +#define MT7621_XTAL_MASK       0x7
 148 +#define MT7621_XTAL_25         6
 149 +#define MT7621_XTAL_40         3
 150 +#define MT7621_MDIO_DRV_MASK   (3 << 4)
 151 +#define MT7621_GE1_MODE_MASK   (3 << 12)
 152 +
 153 +#define TRGMII_TXCTRL_TXC_INV  BIT(30)
 154 +#define P6ECR_INTF_MODE_RGMII  BIT(1)
 155 +#define P5RGMIIRXCR_C_ALIGN    BIT(8)
 156 +#define P5RGMIIRXCR_DELAY_2    BIT(1)
 157 +#define P5RGMIITXCR_DELAY_2    (BIT(8) | BIT(2))
 158 +
 159 +/* TOP_SIG_CTRL bits */
 160 +#define TOP_SIG_CTRL_NORMAL    (BIT(17) | BIT(16))
 161 +
 162 +/* MHWTRAP bits */
 163 +#define MHWTRAP_MANUAL         BIT(16)
 164 +#define MHWTRAP_P5_MAC_SEL     BIT(13)
 165 +#define MHWTRAP_P6_DIS         BIT(8)
 166 +#define MHWTRAP_P5_RGMII_MODE  BIT(7)
 167 +#define MHWTRAP_P5_DIS         BIT(6)
 168 +#define MHWTRAP_PHY_ACCESS     BIT(5)
 169 +
 170 +/* HWTRAP bits */
 171 +#define HWTRAP_XTAL_SHIFT      9
 172 +#define HWTRAP_XTAL_MASK       0x3
 173 +
 174 +/* SYS_CTRL bits */
 175 +#define SYS_CTRL_SW_RST                BIT(1)
 176 +#define SYS_CTRL_REG_RST       BIT(0)
 177 +
 178 +/* PMCR bits */
 179 +#define PMCR_IFG_XMIT_96       BIT(18)
 180 +#define PMCR_MAC_MODE          BIT(16)
 181 +#define PMCR_FORCE_MODE                BIT(15)
 182 +#define PMCR_TX_EN             BIT(14)
 183 +#define PMCR_RX_EN             BIT(13)
 184 +#define PMCR_BACK_PRES_EN      BIT(9)
 185 +#define PMCR_BACKOFF_EN                BIT(8)
 186 +#define PMCR_TX_FC_EN          BIT(5)
 187 +#define PMCR_RX_FC_EN          BIT(4)
 188 +#define PMCR_FORCE_SPEED_1000  BIT(3)
 189 +#define PMCR_FORCE_FDX         BIT(1)
 190 +#define PMCR_FORCE_LNK         BIT(0)
 191 +#define PMCR_FIXED_LINK                (PMCR_IFG_XMIT_96 | PMCR_MAC_MODE | \
 192 +                                PMCR_FORCE_MODE | PMCR_TX_EN | PMCR_RX_EN | \
 193 +                                PMCR_BACK_PRES_EN | PMCR_BACKOFF_EN | \
 194 +                                PMCR_FORCE_SPEED_1000 | PMCR_FORCE_FDX | \
 195 +                                PMCR_FORCE_LNK)
 196 +
 197 +#define PMCR_FIXED_LINK_FC     (PMCR_FIXED_LINK | \
 198 +                                PMCR_TX_FC_EN | PMCR_RX_FC_EN)
 199 +
 200 +/* TRGMII control registers */
 201 +#define GSW_INTF_MODE          0x390
 202 +#define GSW_TRGMII_TD0_ODT     0x354
 203 +#define GSW_TRGMII_TD1_ODT     0x35c
 204 +#define GSW_TRGMII_TD2_ODT     0x364
 205 +#define GSW_TRGMII_TD3_ODT     0x36c
 206 +#define GSW_TRGMII_TXCTL_ODT   0x374
 207 +#define GSW_TRGMII_TCK_ODT     0x37c
 208 +#define GSW_TRGMII_RCK_CTRL    0x300
 209 +
 210 +#define INTF_MODE_TRGMII       BIT(1)
 211 +#define TRGMII_RCK_CTRL_RX_RST BIT(31)
 212 +
 213 +
 214 +/* possible XTAL speed */
 215 +#define        MT7623_XTAL_40          0
 216 +#define MT7623_XTAL_20         1
 217 +#define MT7623_XTAL_25         3
 218 +
 219 +/* GPIO port control registers */
 220 +#define        GPIO_OD33_CTRL8         0x4c0
 221 +#define        GPIO_BIAS_CTRL          0xed0
 222 +#define GPIO_DRV_SEL10         0xf00
 223 +
 224 +/* on MT7620 the functio of port 4 can be software configured */
 225 +enum {
 226 +       PORT4_EPHY = 0,
 227 +       PORT4_EXT,
 228 +};
 229 +
 230 +/* struct mt7620_gsw - the structure that holds the SoC specific data
 231 + * @dev:               The Device struct
 232 + * @base:              The base address
 233 + * @piac_offset:       The PIAC base may change depending on SoC
 234 + * @irq:               The IRQ we are using
 235 + * @port4:             The port4 mode on MT7620
 236 + * @autopoll:          Is MDIO autopolling enabled
 237 + * @ethsys:            The ethsys register map
 238 + * @pctl:              The pin control register map
 239 + * @clk_trgpll:                The trgmii pll clock
 240 + */
 241 +struct mt7620_gsw {
 242 +       struct mtk_eth          *eth;
 243 +       struct device           *dev;
 244 +       void __iomem            *base;
 245 +       u32                     piac_offset;
 246 +       int                     irq;
 247 +       int                     port4;
 248 +       unsigned long int       autopoll;
 249 +
 250 +       struct regmap           *ethsys;
 251 +       struct regmap           *pctl;
 252 +
 253 +       struct clk              *clk_trgpll;
 254 +
 255 +       int                     trgmii_force;
 256 +       bool                    wllll;
 257 +};
 258 +
 259 +/* switch register I/O wrappers */
 260 +void mtk_switch_w32(struct mt7620_gsw *gsw, u32 val, unsigned reg);
 261 +u32 mtk_switch_r32(struct mt7620_gsw *gsw, unsigned reg);
 262 +
 263 +/* the callback used by the driver core to bringup the switch */
 264 +int mtk_gsw_init(struct mtk_eth *eth);
 265 +
 266 +/* MDIO access wrappers */
 267 +int mt7620_mdio_write(struct mii_bus *bus, int phy_addr, int phy_reg, u16 val);
 268 +int mt7620_mdio_read(struct mii_bus *bus, int phy_addr, int phy_reg);
 269 +void mt7620_mdio_link_adjust(struct mtk_eth *eth, int port);
 270 +int mt7620_has_carrier(struct mtk_eth *eth);
 271 +void mt7620_print_link_state(struct mtk_eth *eth, int port, int link,
 272 +                            int speed, int duplex);
 273 +void mt7530_mdio_w32(struct mt7620_gsw *gsw, u32 reg, u32 val);
 274 +u32 mt7530_mdio_r32(struct mt7620_gsw *gsw, u32 reg);
 275 +void mt7530_mdio_m32(struct mt7620_gsw *gsw, u32 mask, u32 set, u32 reg);
 276 +
 277 +u32 _mtk_mdio_write(struct mtk_eth *eth, u32 phy_addr,
 278 +                     u32 phy_register, u32 write_data);
 279 +u32 _mtk_mdio_read(struct mtk_eth *eth, int phy_addr, int phy_reg);
 280 +void mt7620_handle_carrier(struct mtk_eth *eth);
 281 +
 282 +#endif
 283 --- /dev/null
 284 +++ b/drivers/net/ethernet/mediatek/gsw_mt7623.c
 285 @@ -0,0 +1,1084 @@
 286 +/*   This program is free software; you can redistribute it and/or modify
 287 + *   it under the terms of the GNU General Public License as published by
 288 + *   the Free Software Foundation; version 2 of the License
 289 + *
 290 + *   This program is distributed in the hope that it will be useful,
 291 + *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 292 + *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 293 + *   GNU General Public License for more details.
 294 + *
 295 + *   Copyright (C) 2009-2016 John Crispin <blogic@openwrt.org>
 296 + *   Copyright (C) 2009-2016 Felix Fietkau <nbd@nbd.name>
 297 + *   Copyright (C) 2013-2016 Michael Lee <igvtee@gmail.com>
 298 + */
 299 +
 300 +#include <linux/module.h>
 301 +#include <linux/kernel.h>
 302 +#include <linux/types.h>
 303 +#include <linux/platform_device.h>
 304 +#include <linux/of_device.h>
 305 +#include <linux/of_irq.h>
 306 +#include <linux/of_gpio.h>
 307 +#include <linux/of_mdio.h>
 308 +#include <linux/clk.h>
 309 +#include <linux/mfd/syscon.h>
 310 +#include <linux/regulator/consumer.h>
 311 +#include <linux/pm_runtime.h>
 312 +#include <linux/regmap.h>
 313 +#include <linux/reset.h>
 314 +#include <linux/mii.h>
 315 +#include <linux/interrupt.h>
 316 +#include <linux/netdevice.h>
 317 +#include <linux/dma-mapping.h>
 318 +#include <linux/phy.h>
 319 +#include <linux/ethtool.h>
 320 +#include <linux/version.h>
 321 +#include <linux/atomic.h>
 322 +
 323 +#include "mtk_eth_soc.h"
 324 +#include "gsw_mt7620.h"
 325 +#include "mt7530.h"
 326 +
 327 +#define ETHSYS_CLKCFG0                 0x2c
 328 +#define ETHSYS_TRGMII_CLK_SEL362_5     BIT(11)
 329 +
 330 +void mt7530_mdio_w32(struct mt7620_gsw *gsw, u32 reg, u32 val)
 331 +{
 332 +       _mtk_mdio_write(gsw->eth, 0x1f, 0x1f, (reg >> 6) & 0x3ff);
 333 +       _mtk_mdio_write(gsw->eth, 0x1f, (reg >> 2) & 0xf,  val & 0xffff);
 334 +       _mtk_mdio_write(gsw->eth, 0x1f, 0x10, val >> 16);
 335 +}
 336 +
 337 +u32 mt7530_mdio_r32(struct mt7620_gsw *gsw, u32 reg)
 338 +{
 339 +       u16 high, low;
 340 +
 341 +       _mtk_mdio_write(gsw->eth, 0x1f, 0x1f, (reg >> 6) & 0x3ff);
 342 +       low = _mtk_mdio_read(gsw->eth, 0x1f, (reg >> 2) & 0xf);
 343 +       high = _mtk_mdio_read(gsw->eth, 0x1f, 0x10);
 344 +
 345 +       return (high << 16) | (low & 0xffff);
 346 +}
 347 +
 348 +void mt7530_mdio_m32(struct mt7620_gsw *gsw, u32 mask, u32 set, u32 reg)
 349 +{
 350 +       u32 val = mt7530_mdio_r32(gsw, reg);
 351 +
 352 +       val &= mask;
 353 +       val |= set;
 354 +       mt7530_mdio_w32(gsw, reg, val);
 355 +}
 356 +
 357 +void mtk_switch_w32(struct mt7620_gsw *gsw, u32 val, unsigned reg)
 358 +{
 359 +       mtk_w32(gsw->eth, val, reg + 0x10000);
 360 +}
 361 +
 362 +u32 mtk_switch_r32(struct mt7620_gsw *gsw, unsigned reg)
 363 +{
 364 +       return mtk_r32(gsw->eth, reg + 0x10000);
 365 +}
 366 +
 367 +void mtk_switch_m32(struct mt7620_gsw *gsw, u32 mask, u32 set, unsigned reg)
 368 +{
 369 +       u32 val = mtk_switch_r32(gsw, reg);
 370 +
 371 +       val &= mask;
 372 +       val |= set;
 373 +
 374 +       mtk_switch_w32(gsw, val, reg);
 375 +}
 376 +
 377 +int mt7623_gsw_config(struct mtk_eth *eth)
 378 +{
 379 +       if (eth->mii_bus && mdiobus_get_phy(eth->mii_bus, 0x1f))
 380 +               mt7530_probe(eth->dev, NULL, eth->mii_bus, 1);
 381 +
 382 +       return 0;
 383 +}
 384 +
 385 +static irqreturn_t gsw_interrupt_mt7623(int irq, void *_eth)
 386 +{
 387 +       struct mtk_eth *eth = (struct mtk_eth *)_eth;
 388 +       struct mt7620_gsw *gsw = (struct mt7620_gsw *)eth->sw_priv;
 389 +       u32 reg, i;
 390 +
 391 +       reg = mt7530_mdio_r32(gsw, 0x700c);
 392 +
 393 +       for (i = 0; i < 5; i++)
 394 +               if (reg & BIT(i)) {
 395 +                       unsigned int link;
 396 +
 397 +                       link = mt7530_mdio_r32(gsw,
 398 +                                              0x3008 + (i * 0x100)) & 0x1;
 399 +
 400 +                       if (link)
 401 +                               dev_info(gsw->dev,
 402 +                                        "port %d link up\n", i);
 403 +                       else
 404 +                               dev_info(gsw->dev,
 405 +                                        "port %d link down\n", i);
 406 +               }
 407 +
 408 +//     mt7620_handle_carrier(eth);
 409 +       mt7530_mdio_w32(gsw, 0x700c, 0x1f);
 410 +
 411 +       return IRQ_HANDLED;
 412 +}
 413 +
 414 +static void wait_loop(struct mt7620_gsw *gsw)
 415 +{
 416 +       int i;
 417 +       int read_data;
 418 +
 419 +       for (i = 0; i < 320; i = i + 1)
 420 +               read_data = mtk_switch_r32(gsw, 0x610);
 421 +}
 422 +
 423 +static void trgmii_calibration_7623(struct mt7620_gsw *gsw)
 424 +{
 425 +
 426 +       unsigned int tap_a[5] = { 0, 0, 0, 0, 0 };      /* minumum delay for all correct */
 427 +       unsigned int tap_b[5] = { 0, 0, 0, 0, 0 };      /* maximum delay for all correct */
 428 +       unsigned int final_tap[5];
 429 +       unsigned int rxc_step_size;
 430 +       unsigned int rxd_step_size;
 431 +       unsigned int read_data;
 432 +       unsigned int tmp;
 433 +       unsigned int rd_wd;
 434 +       int i;
 435 +       unsigned int err_cnt[5];
 436 +       unsigned int init_toggle_data;
 437 +       unsigned int err_flag[5];
 438 +       unsigned int err_total_flag;
 439 +       unsigned int training_word;
 440 +       unsigned int rd_tap;
 441 +       u32 val;
 442 +
 443 +       u32 TRGMII_7623_base;
 444 +       u32 TRGMII_7623_RD_0;
 445 +       u32 TRGMII_RCK_CTRL;
 446 +
 447 +       TRGMII_7623_base = 0x300;       /* 0xFB110300 */
 448 +       TRGMII_7623_RD_0 = TRGMII_7623_base + 0x10;
 449 +       TRGMII_RCK_CTRL = TRGMII_7623_base;
 450 +       rxd_step_size = 0x1;
 451 +       rxc_step_size = 0x4;
 452 +       init_toggle_data = 0x00000055;
 453 +       training_word = 0x000000AC;
 454 +
 455 +       /* RX clock gating in MT7623 */
 456 +       mtk_switch_m32(gsw, 0x3fffffff, 0, TRGMII_7623_base + 0x04);
 457 +
 458 +       /* Assert RX  reset in MT7623 */
 459 +       mtk_switch_m32(gsw, 0, 0x80000000, TRGMII_7623_base + 0x00);
 460 +
 461 +       /* Set TX OE edge in  MT7623 */
 462 +       mtk_switch_m32(gsw, 0, 0x00002000, TRGMII_7623_base + 0x78);
 463 +
 464 +       /* Disable RX clock gating in MT7623 */
 465 +       mtk_switch_m32(gsw, 0, 0xC0000000, TRGMII_7623_base + 0x04);
 466 +
 467 +       /* Release RX reset in MT7623 */
 468 +       mtk_switch_m32(gsw, 0x7fffffff, 0, TRGMII_7623_base);
 469 +
 470 +       for (i = 0; i < 5; i++)
 471 +               mtk_switch_m32(gsw, 0, 0x80000000, TRGMII_7623_RD_0 + i * 8);
 472 +
 473 +       pr_err("Enable Training Mode in MT7530\n");
 474 +       read_data = mt7530_mdio_r32(gsw, 0x7A40);
 475 +       read_data |= 0xC0000000;
 476 +       mt7530_mdio_w32(gsw, 0x7A40, read_data);        /* Enable Training Mode in MT7530 */
 477 +       err_total_flag = 0;
 478 +       pr_err("Adjust RXC delay in MT7623\n");
 479 +       read_data = 0x0;
 480 +       while (err_total_flag == 0 && read_data != 0x68) {
 481 +               pr_err("2nd Enable EDGE CHK in MT7623\n");
 482 +               /* Enable EDGE CHK in MT7623 */
 483 +               for (i = 0; i < 5; i++)
 484 +                           mtk_switch_m32(gsw, 0x4fffffff, 0x40000000, TRGMII_7623_RD_0 + i * 8);
 485 +
 486 +               wait_loop(gsw);
 487 +               err_total_flag = 1;
 488 +               for (i = 0; i < 5; i++) {
 489 +                       err_cnt[i] =
 490 +                           mtk_switch_r32(gsw, TRGMII_7623_RD_0 + i * 8) >> 8;
 491 +                       err_cnt[i] &= 0x0000000f;
 492 +                       rd_wd = mtk_switch_r32(gsw, TRGMII_7623_RD_0 + i * 8) >> 16;
 493 +                       rd_wd &= 0x000000ff;
 494 +                       val = mtk_switch_r32(gsw, TRGMII_7623_RD_0 + i * 8);
 495 +                       pr_err("ERR_CNT = %d, RD_WD =%x, TRGMII_7623_RD_0=%x\n",
 496 +                              err_cnt[i], rd_wd, val);
 497 +                       if (err_cnt[i] != 0) {
 498 +                               err_flag[i] = 1;
 499 +                       } else if (rd_wd != 0x55) {
 500 +                               err_flag[i] = 1;
 501 +                       } else {
 502 +                               err_flag[i] = 0;
 503 +                       }
 504 +                       err_total_flag = err_flag[i] & err_total_flag;
 505 +               }
 506 +
 507 +               pr_err("2nd Disable EDGE CHK in MT7623\n");
 508 +               /* Disable EDGE CHK in MT7623 */
 509 +               for (i = 0; i < 5; i++)
 510 +                           mtk_switch_m32(gsw, 0x4fffffff, 0x40000000, TRGMII_7623_RD_0 + i * 8);
 511 +               wait_loop(gsw);
 512 +               pr_err("2nd Disable EDGE CHK in MT7623\n");
 513 +               /* Adjust RXC delay */
 514 +               /* RX clock gating in MT7623 */
 515 +               mtk_switch_m32(gsw, 0x3fffffff, 0, TRGMII_7623_base + 0x04);
 516 +               read_data = mtk_switch_r32(gsw, TRGMII_7623_base);
 517 +               if (err_total_flag == 0) {
 518 +                       tmp = (read_data & 0x0000007f) + rxc_step_size;
 519 +                       pr_err(" RXC delay = %d\n", tmp);
 520 +                       read_data >>= 8;
 521 +                       read_data &= 0xffffff80;
 522 +                       read_data |= tmp;
 523 +                       read_data <<= 8;
 524 +                       read_data &= 0xffffff80;
 525 +                       read_data |= tmp;
 526 +                       mtk_switch_w32(gsw, read_data, TRGMII_7623_base);
 527 +               } else {
 528 +                       tmp = (read_data & 0x0000007f) + 16;
 529 +                       pr_err(" RXC delay = %d\n", tmp);
 530 +                       read_data >>= 8;
 531 +                       read_data &= 0xffffff80;
 532 +                       read_data |= tmp;
 533 +                       read_data <<= 8;
 534 +                       read_data &= 0xffffff80;
 535 +                       read_data |= tmp;
 536 +                       mtk_switch_w32(gsw, read_data, TRGMII_7623_base);
 537 +               }
 538 +               read_data &= 0x000000ff;
 539 +
 540 +               /* Disable RX clock gating in MT7623 */
 541 +               mtk_switch_m32(gsw, 0, 0xC0000000, TRGMII_7623_base + 0x04);
 542 +               for (i = 0; i < 5; i++)
 543 +                       mtk_switch_m32(gsw, 0, 0x80000000, TRGMII_7623_RD_0 + i * 8);
 544 +       }
 545 +
 546 +       /* Read RD_WD MT7623 */
 547 +       for (i = 0; i < 5; i++) {
 548 +               rd_tap = 0;
 549 +               while (err_flag[i] != 0 && rd_tap != 128) {
 550 +                       /* Enable EDGE CHK in MT7623 */
 551 +                       mtk_switch_m32(gsw, 0x4fffffff, 0x40000000, TRGMII_7623_RD_0 + i * 8);
 552 +                       wait_loop(gsw);
 553 +
 554 +                       read_data = mtk_switch_r32(gsw, TRGMII_7623_RD_0 + i * 8);
 555 +                       err_cnt[i] = (read_data >> 8) & 0x0000000f;     /* Read MT7623 Errcnt */
 556 +                       rd_wd = (read_data >> 16) & 0x000000ff;
 557 +                       if (err_cnt[i] != 0 || rd_wd != 0x55) {
 558 +                               err_flag[i] = 1;
 559 +                       } else {
 560 +                               err_flag[i] = 0;
 561 +                       }
 562 +                       /* Disable EDGE CHK in MT7623 */
 563 +                       mtk_switch_m32(gsw, 0x4fffffff, 0x40000000, TRGMII_7623_RD_0 + i * 8);
 564 +                       wait_loop(gsw);
 565 +                       if (err_flag[i] != 0) {
 566 +                               rd_tap = (read_data & 0x0000007f) + rxd_step_size;      /* Add RXD delay in MT7623 */
 567 +                               read_data = (read_data & 0xffffff80) | rd_tap;
 568 +                               mtk_switch_w32(gsw, read_data,
 569 +                                       TRGMII_7623_RD_0 + i * 8);
 570 +                               tap_a[i] = rd_tap;
 571 +                       } else {
 572 +                               rd_tap = (read_data & 0x0000007f) + 48;
 573 +                               read_data = (read_data & 0xffffff80) | rd_tap;
 574 +                               mtk_switch_w32(gsw, read_data,
 575 +                                       TRGMII_7623_RD_0 + i * 8);
 576 +                       }
 577 +
 578 +               }
 579 +               pr_err("MT7623 %dth bit  Tap_a = %d\n", i, tap_a[i]);
 580 +       }
 581 +       /* pr_err("Last While Loop\n"); */
 582 +       for (i = 0; i < 5; i++) {
 583 +               while ((err_flag[i] == 0) && (rd_tap != 128)) {
 584 +                       read_data = mtk_switch_r32(gsw, TRGMII_7623_RD_0 + i * 8);
 585 +                       rd_tap = (read_data & 0x0000007f) + rxd_step_size;      /* Add RXD delay in MT7623 */
 586 +                       read_data = (read_data & 0xffffff80) | rd_tap;
 587 +                       mtk_switch_w32(gsw, read_data, TRGMII_7623_RD_0 + i * 8);
 588 +                       /* Enable EDGE CHK in MT7623 */
 589 +                       val =
 590 +                           mtk_switch_r32(gsw, TRGMII_7623_RD_0 + i * 8) | 0x40000000;
 591 +                       val &= 0x4fffffff;
 592 +                       mtk_switch_w32(gsw, val, TRGMII_7623_RD_0 + i * 8);
 593 +                       wait_loop(gsw);
 594 +                       read_data = mtk_switch_r32(gsw, TRGMII_7623_RD_0 + i * 8);
 595 +                       err_cnt[i] = (read_data >> 8) & 0x0000000f;     /* Read MT7623 Errcnt */
 596 +                       rd_wd = (read_data >> 16) & 0x000000ff;
 597 +                       if (err_cnt[i] != 0 || rd_wd != 0x55) {
 598 +                               err_flag[i] = 1;
 599 +                       } else {
 600 +                               err_flag[i] = 0;
 601 +                       }
 602 +
 603 +                       /* Disable EDGE CHK in MT7623 */
 604 +                       mtk_switch_m32(gsw, 0x4fffffff, 0x40000000, TRGMII_7623_RD_0 + i * 8);
 605 +                       wait_loop(gsw);
 606 +
 607 +               }
 608 +
 609 +               tap_b[i] = rd_tap;      /* -rxd_step_size; */
 610 +               pr_err("MT7623 %dth bit  Tap_b = %d\n", i, tap_b[i]);
 611 +               final_tap[i] = (tap_a[i] + tap_b[i]) / 2;       /* Calculate RXD delay = (TAP_A + TAP_B)/2 */
 612 +               read_data = (read_data & 0xffffff80) | final_tap[i];
 613 +               mtk_switch_w32(gsw, read_data, TRGMII_7623_RD_0 + i * 8);
 614 +       }
 615 +
 616 +       read_data = mt7530_mdio_r32(gsw, 0x7A40);
 617 +       read_data &= 0x3fffffff;
 618 +       mt7530_mdio_w32(gsw, 0x7A40, read_data);
 619 +}
 620 +
 621 +static void trgmii_calibration_7530(struct mt7620_gsw *gsw)
 622 +{
 623 +
 624 +       unsigned int tap_a[5] = { 0, 0, 0, 0, 0 };
 625 +       unsigned int tap_b[5] = { 0, 0, 0, 0, 0 };
 626 +       unsigned int final_tap[5];
 627 +       unsigned int rxc_step_size;
 628 +       unsigned int rxd_step_size;
 629 +       unsigned int read_data;
 630 +       unsigned int tmp = 0;
 631 +       int i;
 632 +       unsigned int err_cnt[5];
 633 +       unsigned int rd_wd;
 634 +       unsigned int init_toggle_data;
 635 +       unsigned int err_flag[5];
 636 +       unsigned int err_total_flag;
 637 +       unsigned int training_word;
 638 +       unsigned int rd_tap;
 639 +
 640 +       u32 TRGMII_7623_base;
 641 +       u32 TRGMII_7530_RD_0;
 642 +       u32 TRGMII_RCK_CTRL;
 643 +       u32 TRGMII_7530_base;
 644 +       u32 TRGMII_7530_TX_base;
 645 +       u32 val;
 646 +
 647 +       TRGMII_7623_base = 0x300;
 648 +       TRGMII_7530_base = 0x7A00;
 649 +       TRGMII_7530_RD_0 = TRGMII_7530_base + 0x10;
 650 +       TRGMII_RCK_CTRL = TRGMII_7623_base;
 651 +       rxd_step_size = 0x1;
 652 +       rxc_step_size = 0x8;
 653 +       init_toggle_data = 0x00000055;
 654 +       training_word = 0x000000AC;
 655 +
 656 +       TRGMII_7530_TX_base = TRGMII_7530_base + 0x50;
 657 +
 658 +       /* pr_err("Calibration begin ........\n"); */
 659 +       val = mtk_switch_r32(gsw, TRGMII_7623_base + 0x40) | 0x80000000;
 660 +       mtk_switch_w32(gsw, val, TRGMII_7623_base + 0x40);
 661 +       read_data = mt7530_mdio_r32(gsw, 0x7a10);
 662 +       /* pr_err("TRGMII_7530_RD_0 is %x\n", read_data); */
 663 +
 664 +       read_data = mt7530_mdio_r32(gsw, TRGMII_7530_base + 0x04);
 665 +       read_data &= 0x3fffffff;
 666 +       mt7530_mdio_w32(gsw, TRGMII_7530_base + 0x04, read_data);       /* RX clock gating in MT7530 */
 667 +
 668 +       read_data = mt7530_mdio_r32(gsw, TRGMII_7530_base + 0x78);
 669 +       read_data |= 0x00002000;
 670 +       mt7530_mdio_w32(gsw, TRGMII_7530_base + 0x78, read_data);       /* Set TX OE edge in  MT7530 */
 671 +
 672 +       read_data = mt7530_mdio_r32(gsw, TRGMII_7530_base);
 673 +       read_data |= 0x80000000;
 674 +       mt7530_mdio_w32(gsw, TRGMII_7530_base, read_data);      /* Assert RX  reset in MT7530 */
 675 +
 676 +       read_data = mt7530_mdio_r32(gsw, TRGMII_7530_base);
 677 +       read_data &= 0x7fffffff;
 678 +       mt7530_mdio_w32(gsw, TRGMII_7530_base, read_data);      /* Release RX reset in MT7530 */
 679 +
 680 +       read_data = mt7530_mdio_r32(gsw, TRGMII_7530_base + 0x04);
 681 +       read_data |= 0xC0000000;
 682 +       mt7530_mdio_w32(gsw, TRGMII_7530_base + 0x04, read_data);       /* Disable RX clock gating in MT7530 */
 683 +
 684 +       /* pr_err("Enable Training Mode in MT7623\n"); */
 685 +       /*Enable Training Mode in MT7623 */
 686 +       val = mtk_switch_r32(gsw, TRGMII_7623_base + 0x40) | 0x80000000;
 687 +       mtk_switch_w32(gsw, val, TRGMII_7623_base + 0x40);
 688 +       if (gsw->trgmii_force == 2000) {
 689 +               val = mtk_switch_r32(gsw, TRGMII_7623_base + 0x40) | 0xC0000000;
 690 +               mtk_switch_w32(gsw, val, TRGMII_7623_base + 0x40);
 691 +       } else {
 692 +               val = mtk_switch_r32(gsw, TRGMII_7623_base + 0x40) | 0x80000000;
 693 +               mtk_switch_w32(gsw, val, TRGMII_7623_base + 0x40);
 694 +       }
 695 +       val = mtk_switch_r32(gsw, TRGMII_7623_base + 0x078) & 0xfffff0ff;
 696 +       mtk_switch_w32(gsw, val, TRGMII_7623_base + 0x078);
 697 +       val = mtk_switch_r32(gsw, TRGMII_7623_base + 0x50) & 0xfffff0ff;
 698 +       mtk_switch_w32(gsw, val, TRGMII_7623_base + 0x50);
 699 +       val = mtk_switch_r32(gsw, TRGMII_7623_base + 0x58) & 0xfffff0ff;
 700 +       mtk_switch_w32(gsw, val, TRGMII_7623_base + 0x58);
 701 +       val = mtk_switch_r32(gsw, TRGMII_7623_base + 0x60) & 0xfffff0ff;
 702 +       mtk_switch_w32(gsw, val, TRGMII_7623_base + 0x60);
 703 +       val = mtk_switch_r32(gsw, TRGMII_7623_base + 0x68) & 0xfffff0ff;
 704 +       mtk_switch_w32(gsw, val, TRGMII_7623_base + 0x68);
 705 +       val = mtk_switch_r32(gsw, TRGMII_7623_base + 0x70) & 0xfffff0ff;
 706 +       mtk_switch_w32(gsw, val, TRGMII_7623_base + 0x70);
 707 +       val = mtk_switch_r32(gsw, TRGMII_7623_base + 0x78) & 0x00000800;
 708 +       mtk_switch_w32(gsw, val, TRGMII_7623_base + 0x78);
 709 +       err_total_flag = 0;
 710 +       /* pr_err("Adjust RXC delay in MT7530\n"); */
 711 +       read_data = 0x0;
 712 +       while (err_total_flag == 0 && (read_data != 0x68)) {
 713 +               /* pr_err("2nd Enable EDGE CHK in MT7530\n"); */
 714 +               /* Enable EDGE CHK in MT7530 */
 715 +               for (i = 0; i < 5; i++) {
 716 +                       read_data =
 717 +                           mt7530_mdio_r32(gsw, TRGMII_7530_RD_0 + i * 8);
 718 +                       read_data |= 0x40000000;
 719 +                       read_data &= 0x4fffffff;
 720 +                       mt7530_mdio_w32(gsw, TRGMII_7530_RD_0 + i * 8,
 721 +                                       read_data);
 722 +                       wait_loop(gsw);
 723 +                       /* pr_err("2nd Disable EDGE CHK in MT7530\n"); */
 724 +                       err_cnt[i] =
 725 +                           mt7530_mdio_r32(gsw, TRGMII_7530_RD_0 + i * 8);
 726 +                       /* pr_err("***** MT7530 %dth bit ERR_CNT =%x\n",i, err_cnt[i]); */
 727 +                       /* pr_err("MT7530 %dth bit ERR_CNT =%x\n",i, err_cnt[i]); */
 728 +                       err_cnt[i] >>= 8;
 729 +                       err_cnt[i] &= 0x0000ff0f;
 730 +                       rd_wd = err_cnt[i] >> 8;
 731 +                       rd_wd &= 0x000000ff;
 732 +                       err_cnt[i] &= 0x0000000f;
 733 +                       /* read_data = mt7530_mdio_r32(gsw,0x7a10,&read_data); */
 734 +                       if (err_cnt[i] != 0) {
 735 +                               err_flag[i] = 1;
 736 +                       } else if (rd_wd != 0x55) {
 737 +                               err_flag[i] = 1;
 738 +                       } else {
 739 +                               err_flag[i] = 0;
 740 +                       }
 741 +                       if (i == 0) {
 742 +                               err_total_flag = err_flag[i];
 743 +                       } else {
 744 +                               err_total_flag = err_flag[i] & err_total_flag;
 745 +                       }
 746 +                       /* Disable EDGE CHK in MT7530 */
 747 +                       read_data =
 748 +                           mt7530_mdio_r32(gsw, TRGMII_7530_RD_0 + i * 8);
 749 +                       read_data |= 0x40000000;
 750 +                       read_data &= 0x4fffffff;
 751 +                       mt7530_mdio_w32(gsw, TRGMII_7530_RD_0 + i * 8,
 752 +                                       read_data);
 753 +                       wait_loop(gsw);
 754 +               }
 755 +               /*Adjust RXC delay */
 756 +               if (err_total_flag == 0) {
 757 +                       read_data = mt7530_mdio_r32(gsw, TRGMII_7530_base);
 758 +                       read_data |= 0x80000000;
 759 +                       mt7530_mdio_w32(gsw, TRGMII_7530_base, read_data);      /* Assert RX  reset in MT7530 */
 760 +
 761 +                       read_data =
 762 +                           mt7530_mdio_r32(gsw, TRGMII_7530_base + 0x04);
 763 +                       read_data &= 0x3fffffff;
 764 +                       mt7530_mdio_w32(gsw, TRGMII_7530_base + 0x04, read_data);       /* RX clock gating in MT7530 */
 765 +
 766 +                       read_data = mt7530_mdio_r32(gsw, TRGMII_7530_base);
 767 +                       tmp = read_data;
 768 +                       tmp &= 0x0000007f;
 769 +                       tmp += rxc_step_size;
 770 +                       /* pr_err("Current rxc delay = %d\n", tmp); */
 771 +                       read_data &= 0xffffff80;
 772 +                       read_data |= tmp;
 773 +                       mt7530_mdio_w32(gsw, TRGMII_7530_base, read_data);
 774 +                       read_data = mt7530_mdio_r32(gsw, TRGMII_7530_base);
 775 +                       /* pr_err("Current RXC delay = %x\n", read_data); */
 776 +
 777 +                       read_data = mt7530_mdio_r32(gsw, TRGMII_7530_base);
 778 +                       read_data &= 0x7fffffff;
 779 +                       mt7530_mdio_w32(gsw, TRGMII_7530_base, read_data);      /* Release RX reset in MT7530 */
 780 +
 781 +                       read_data =
 782 +                           mt7530_mdio_r32(gsw, TRGMII_7530_base + 0x04);
 783 +                       read_data |= 0xc0000000;
 784 +                       mt7530_mdio_w32(gsw, TRGMII_7530_base + 0x04, read_data);       /* Disable RX clock gating in MT7530 */
 785 +                       pr_err("####### MT7530 RXC delay is %d\n", tmp);
 786 +               }
 787 +               read_data = tmp;
 788 +       }
 789 +       pr_err("Finish RXC Adjustment while loop\n");
 790 +
 791 +       /* pr_err("Read RD_WD MT7530\n"); */
 792 +       /* Read RD_WD MT7530 */
 793 +       for (i = 0; i < 5; i++) {
 794 +               rd_tap = 0;
 795 +               while (err_flag[i] != 0 && rd_tap != 128) {
 796 +                       /* Enable EDGE CHK in MT7530 */
 797 +                       read_data =
 798 +                           mt7530_mdio_r32(gsw, TRGMII_7530_RD_0 + i * 8);
 799 +                       read_data |= 0x40000000;
 800 +                       read_data &= 0x4fffffff;
 801 +                       mt7530_mdio_w32(gsw, TRGMII_7530_RD_0 + i * 8,
 802 +                                       read_data);
 803 +                       wait_loop(gsw);
 804 +                       err_cnt[i] = (read_data >> 8) & 0x0000000f;
 805 +                       rd_wd = (read_data >> 16) & 0x000000ff;
 806 +                       if (err_cnt[i] != 0 || rd_wd != 0x55) {
 807 +                               err_flag[i] = 1;
 808 +                       } else {
 809 +                               err_flag[i] = 0;
 810 +                       }
 811 +                       if (err_flag[i] != 0) {
 812 +                               rd_tap = (read_data & 0x0000007f) + rxd_step_size;      /* Add RXD delay in MT7530 */
 813 +                               read_data = (read_data & 0xffffff80) | rd_tap;
 814 +                               mt7530_mdio_w32(gsw, TRGMII_7530_RD_0 + i * 8,
 815 +                                               read_data);
 816 +                               tap_a[i] = rd_tap;
 817 +                       } else {
 818 +                               tap_a[i] = (read_data & 0x0000007f);    /* Record the min delay TAP_A */
 819 +                               rd_tap = tap_a[i] + 0x4;
 820 +                               read_data = (read_data & 0xffffff80) | rd_tap;
 821 +                               mt7530_mdio_w32(gsw, TRGMII_7530_RD_0 + i * 8,
 822 +                                               read_data);
 823 +                       }
 824 +
 825 +                       /* Disable EDGE CHK in MT7530 */
 826 +                       read_data =
 827 +                           mt7530_mdio_r32(gsw, TRGMII_7530_RD_0 + i * 8);
 828 +                       read_data |= 0x40000000;
 829 +                       read_data &= 0x4fffffff;
 830 +                       mt7530_mdio_w32(gsw, TRGMII_7530_RD_0 + i * 8,
 831 +                                       read_data);
 832 +                       wait_loop(gsw);
 833 +
 834 +               }
 835 +               pr_err("MT7530 %dth bit  Tap_a = %d\n", i, tap_a[i]);
 836 +       }
 837 +
 838 +       /* pr_err("Last While Loop\n"); */
 839 +       for (i = 0; i < 5; i++) {
 840 +               rd_tap = 0;
 841 +               while (err_flag[i] == 0 && (rd_tap != 128)) {
 842 +                       /* Enable EDGE CHK in MT7530 */
 843 +                       read_data = mt7530_mdio_r32(gsw, TRGMII_7530_RD_0 + i * 8);
 844 +                       read_data |= 0x40000000;
 845 +                       read_data &= 0x4fffffff;
 846 +                       mt7530_mdio_w32(gsw, TRGMII_7530_RD_0 + i * 8,
 847 +                                       read_data);
 848 +                       wait_loop(gsw);
 849 +                       err_cnt[i] = (read_data >> 8) & 0x0000000f;
 850 +                       rd_wd = (read_data >> 16) & 0x000000ff;
 851 +                       if (err_cnt[i] != 0 || rd_wd != 0x55)
 852 +                               err_flag[i] = 1;
 853 +                       else
 854 +                               err_flag[i] = 0;
 855 +
 856 +                       if (err_flag[i] == 0 && (rd_tap != 128)) {
 857 +                               /* Add RXD delay in MT7530 */
 858 +                               rd_tap = (read_data & 0x0000007f) + rxd_step_size;
 859 +                               read_data = (read_data & 0xffffff80) | rd_tap;
 860 +                               mt7530_mdio_w32(gsw, TRGMII_7530_RD_0 + i * 8,
 861 +                                               read_data);
 862 +                       }
 863 +                       /* Disable EDGE CHK in MT7530 */
 864 +                       read_data =
 865 +                           mt7530_mdio_r32(gsw, TRGMII_7530_RD_0 + i * 8);
 866 +                       read_data |= 0x40000000;
 867 +                       read_data &= 0x4fffffff;
 868 +                       mt7530_mdio_w32(gsw, TRGMII_7530_RD_0 + i * 8,
 869 +                                       read_data);
 870 +                       wait_loop(gsw);
 871 +               }
 872 +               tap_b[i] = rd_tap;      /* - rxd_step_size; */
 873 +               pr_err("MT7530 %dth bit  Tap_b = %d\n", i, tap_b[i]);
 874 +               /* Calculate RXD delay = (TAP_A + TAP_B)/2 */
 875 +               final_tap[i] = (tap_a[i] + tap_b[i]) / 2;
 876 +               /* pr_err("########****** MT7530 %dth bit Final Tap = %d\n", i, final_tap[i]); */
 877 +
 878 +               read_data = (read_data & 0xffffff80) | final_tap[i];
 879 +               mt7530_mdio_w32(gsw, TRGMII_7530_RD_0 + i * 8, read_data);
 880 +       }
 881 +
 882 +       if (gsw->trgmii_force == 2000)
 883 +               mtk_switch_m32(gsw, 0x7fffffff, 0, TRGMII_7623_base + 0x40);
 884 +       else
 885 +               mtk_switch_m32(gsw, 0x3fffffff, 0, TRGMII_7623_base + 0x40);
 886 +
 887 +}
 888 +
 889 +static void mt7530_trgmii_clock_setting(struct mt7620_gsw *gsw, u32 xtal_mode)
 890 +{
 891 +
 892 +       u32 regValue;
 893 +
 894 +       /* TRGMII Clock */
 895 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x1f);
 896 +       _mtk_mdio_write(gsw->eth, 0, 14, 0x410);
 897 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x401f);
 898 +       _mtk_mdio_write(gsw->eth, 0, 14, 0x1);
 899 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x1f);
 900 +       _mtk_mdio_write(gsw->eth, 0, 14, 0x404);
 901 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x401f);
 902 +
 903 +       if (xtal_mode == 1) {
 904 +               /* 25MHz */
 905 +               if (gsw->trgmii_force == 2600)
 906 +                       /* 325MHz */
 907 +                       _mtk_mdio_write(gsw->eth, 0, 14, 0x1a00);
 908 +               else if (gsw->trgmii_force == 2000)
 909 +                       /* 250MHz */
 910 +                       _mtk_mdio_write(gsw->eth, 0, 14, 0x1400);
 911 +       } else if (xtal_mode == 2) {
 912 +               /* 40MHz */
 913 +               if (gsw->trgmii_force == 2600)
 914 +                       /* 325MHz */
 915 +                       _mtk_mdio_write(gsw->eth, 0, 14, 0x1040);
 916 +               else if (gsw->trgmii_force == 2000)
 917 +                       /* 250MHz */
 918 +                       _mtk_mdio_write(gsw->eth, 0, 14, 0x0c80);
 919 +       }
 920 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x1f);
 921 +       _mtk_mdio_write(gsw->eth, 0, 14, 0x405);
 922 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x401f);
 923 +       _mtk_mdio_write(gsw->eth, 0, 14, 0x0);
 924 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x1f);
 925 +       _mtk_mdio_write(gsw->eth, 0, 14, 0x409);
 926 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x401f);
 927 +       if (xtal_mode == 1)
 928 +               /* 25MHz */
 929 +               _mtk_mdio_write(gsw->eth, 0, 14, 0x0057);
 930 +       else
 931 +               /* 40MHz */
 932 +               _mtk_mdio_write(gsw->eth, 0, 14, 0x0087);
 933 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x1f);
 934 +       _mtk_mdio_write(gsw->eth, 0, 14, 0x40a);
 935 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x401f);
 936 +       if (xtal_mode == 1)
 937 +               /* 25MHz */
 938 +               _mtk_mdio_write(gsw->eth, 0, 14, 0x0057);
 939 +       else
 940 +               /* 40MHz */
 941 +               _mtk_mdio_write(gsw->eth, 0, 14, 0x0087);
 942 +
 943 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x1f);
 944 +       _mtk_mdio_write(gsw->eth, 0, 14, 0x403);
 945 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x401f);
 946 +       _mtk_mdio_write(gsw->eth, 0, 14, 0x1800);
 947 +
 948 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x1f);
 949 +       _mtk_mdio_write(gsw->eth, 0, 14, 0x403);
 950 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x401f);
 951 +       _mtk_mdio_write(gsw->eth, 0, 14, 0x1c00);
 952 +
 953 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x1f);
 954 +       _mtk_mdio_write(gsw->eth, 0, 14, 0x401);
 955 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x401f);
 956 +       _mtk_mdio_write(gsw->eth, 0, 14, 0xc020);
 957 +
 958 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x1f);
 959 +       _mtk_mdio_write(gsw->eth, 0, 14, 0x406);
 960 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x401f);
 961 +       _mtk_mdio_write(gsw->eth, 0, 14, 0xa030);
 962 +
 963 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x1f);
 964 +       _mtk_mdio_write(gsw->eth, 0, 14, 0x406);
 965 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x401f);
 966 +       _mtk_mdio_write(gsw->eth, 0, 14, 0xa038);
 967 +
 968 +//     udelay(120);            /* for MT7623 bring up test */
 969 +
 970 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x1f);
 971 +       _mtk_mdio_write(gsw->eth, 0, 14, 0x410);
 972 +       _mtk_mdio_write(gsw->eth, 0, 13, 0x401f);
 973 +       _mtk_mdio_write(gsw->eth, 0, 14, 0x3);
 974 +
 975 +       regValue = mt7530_mdio_r32(gsw, 0x7830);
 976 +       regValue &= 0xFFFFFFFC;
 977 +       regValue |= 0x00000001;
 978 +       mt7530_mdio_w32(gsw, 0x7830, regValue);
 979 +
 980 +       regValue = mt7530_mdio_r32(gsw, 0x7a40);
 981 +       regValue &= ~(0x1 << 30);
 982 +       regValue &= ~(0x1 << 28);
 983 +       mt7530_mdio_w32(gsw, 0x7a40, regValue);
 984 +
 985 +       mt7530_mdio_w32(gsw, 0x7a78, 0x55);
 986 +//     udelay(100);            /* for mt7623 bring up test */
 987 +
 988 +       mtk_switch_m32(gsw, 0x7fffffff, 0, 0x300);
 989 +
 990 +       trgmii_calibration_7623(gsw);
 991 +       trgmii_calibration_7530(gsw);
 992 +
 993 +       mtk_switch_m32(gsw, 0, 0x80000000, 0x300);
 994 +       mtk_switch_m32(gsw, 0, 0x7fffffff, 0x300);
 995 +
 996 +       /*MT7530 RXC reset */
 997 +       regValue = mt7530_mdio_r32(gsw, 0x7a00);
 998 +       regValue |= (0x1 << 31);
 999 +       mt7530_mdio_w32(gsw, 0x7a00, regValue);
1000 +       mdelay(1);
1001 +       regValue &= ~(0x1 << 31);
1002 +       mt7530_mdio_w32(gsw, 0x7a00, regValue);
1003 +       mdelay(100);
1004 +}
1005 +
1006 +static void mt7623_hw_init(struct mtk_eth *eth, struct mt7620_gsw *gsw, struct device_node *np)
1007 +{
1008 +       u32     i;
1009 +       u32     val;
1010 +       u32     xtal_mode;
1011 +
1012 +       regmap_update_bits(gsw->ethsys, ETHSYS_CLKCFG0,
1013 +                          ETHSYS_TRGMII_CLK_SEL362_5,
1014 +                          ETHSYS_TRGMII_CLK_SEL362_5);
1015 +
1016 +       /* reset the TRGMII core */
1017 +       mtk_switch_m32(gsw, 0, INTF_MODE_TRGMII, GSW_INTF_MODE);
1018 +       /* Assert MT7623 RXC reset */
1019 +       mtk_switch_m32(gsw, 0, TRGMII_RCK_CTRL_RX_RST, GSW_TRGMII_RCK_CTRL);
1020 +
1021 +       /* Hardware reset Switch */
1022 +       device_reset(eth->dev);
1023 +
1024 +       /* Wait for Switch Reset Completed*/
1025 +       for (i = 0; i < 100; i++) {
1026 +               mdelay(10);
1027 +               if (mt7530_mdio_r32(gsw, MT7530_HWTRAP))
1028 +                       break;
1029 +       }
1030 +
1031 +       /* turn off all PHYs */
1032 +       for (i = 0; i <= 4; i++) {
1033 +               val = _mtk_mdio_read(gsw->eth, i, 0x0);
1034 +               val |= BIT(11);
1035 +               _mtk_mdio_write(gsw->eth, i, 0x0, val);
1036 +       }
1037 +
1038 +       /* reset the switch */
1039 +       mt7530_mdio_w32(gsw, MT7530_SYS_CTRL,
1040 +                       SYS_CTRL_SW_RST | SYS_CTRL_REG_RST);
1041 +       udelay(100);
1042 +
1043 +       /* GE1, Force 1000M/FD, FC ON */
1044 +       mt7530_mdio_w32(gsw, MT7530_PMCR_P(6), PMCR_FIXED_LINK_FC);
1045 +
1046 +       /* GE2, Force 1000M/FD, FC ON */
1047 +       mt7530_mdio_w32(gsw, MT7530_PMCR_P(5), PMCR_FIXED_LINK_FC);
1048 +
1049 +       /* Enable Port 6, P5 as GMAC5, P5 disable */
1050 +       val = mt7530_mdio_r32(gsw, MT7530_MHWTRAP);
1051 +       if (gsw->eth->mac[0] &&
1052 +           of_phy_is_fixed_link(gsw->eth->mac[0]->of_node))
1053 +               /* Enable Port 6 */
1054 +               val &= ~MHWTRAP_P6_DIS;
1055 +       else
1056 +               /* Disable Port 6 */
1057 +               val |= MHWTRAP_P6_DIS;
1058 +       if (gsw->eth->mac[1] &&
1059 +           of_phy_is_fixed_link(gsw->eth->mac[1]->of_node)) {
1060 +               /* Enable Port 5 */
1061 +               val &= ~MHWTRAP_P5_DIS;
1062 +               /* Port 5 as PHY */
1063 +               val &= ~MHWTRAP_P5_MAC_SEL;
1064 +       } else {
1065 +               /* Disable Port 5 */
1066 +               val |= MHWTRAP_P5_DIS;
1067 +               /* Port 5 as GMAC */
1068 +               val |= MHWTRAP_P5_MAC_SEL;
1069 +               val |= BIT(7);
1070 +               mt7530_mdio_w32(gsw, MT7530_PMCR_P(5), 0x8000);
1071 +       }
1072 +       /* gphy to port 0/4 */
1073 +       if (gsw->wllll)
1074 +               val |= BIT(20);
1075 +       else
1076 +               val &= ~BIT(20);
1077 +
1078 +       /* Set MT7530 phy direct access mode**/
1079 +       val &= ~MHWTRAP_PHY_ACCESS;
1080 +       /* manual override of HW-Trap */
1081 +       val |= MHWTRAP_MANUAL;
1082 +       mt7530_mdio_w32(gsw, MT7530_MHWTRAP, val);
1083 +       dev_info(gsw->dev, "Setting MHWTRAP to 0x%08x\n", val);
1084 +
1085 +       val = mt7530_mdio_r32(gsw, 0x7800);
1086 +       val = (val >> 9) & 0x3;
1087 +       if (val == 0x3) {
1088 +               xtal_mode = 1;
1089 +               /* 25Mhz Xtal - do nothing */
1090 +       } else if (val == 0x2) {
1091 +               /* 40Mhz */
1092 +               xtal_mode = 2;
1093 +
1094 +               /* disable MT7530 core clock */
1095 +               _mtk_mdio_write(gsw->eth, 0, 13, 0x1f);
1096 +               _mtk_mdio_write(gsw->eth, 0, 14, 0x410);
1097 +               _mtk_mdio_write(gsw->eth, 0, 13, 0x401f);
1098 +               _mtk_mdio_write(gsw->eth, 0, 14, 0x0);
1099 +
1100 +               /* disable MT7530 PLL */
1101 +               _mtk_mdio_write(gsw->eth, 0, 13, 0x1f);
1102 +               _mtk_mdio_write(gsw->eth, 0, 14, 0x40d);
1103 +               _mtk_mdio_write(gsw->eth, 0, 13, 0x401f);
1104 +               _mtk_mdio_write(gsw->eth, 0, 14, 0x2020);
1105 +
1106 +               /* for MT7530 core clock = 500Mhz */
1107 +               _mtk_mdio_write(gsw->eth, 0, 13, 0x1f);
1108 +               _mtk_mdio_write(gsw->eth, 0, 14, 0x40e);
1109 +               _mtk_mdio_write(gsw->eth, 0, 13, 0x401f);
1110 +               _mtk_mdio_write(gsw->eth, 0, 14, 0x119);
1111 +
1112 +               /* enable MT7530 PLL */
1113 +               _mtk_mdio_write(gsw->eth, 0, 13, 0x1f);
1114 +               _mtk_mdio_write(gsw->eth, 0, 14, 0x40d);
1115 +               _mtk_mdio_write(gsw->eth, 0, 13, 0x401f);
1116 +               _mtk_mdio_write(gsw->eth, 0, 14, 0x2820);
1117 +
1118 +               udelay(20);
1119 +
1120 +               /* enable MT7530 core clock */
1121 +               _mtk_mdio_write(gsw->eth, 0, 13, 0x1f);
1122 +               _mtk_mdio_write(gsw->eth, 0, 14, 0x410);
1123 +               _mtk_mdio_write(gsw->eth, 0, 13, 0x401f);
1124 +       } else {
1125 +               xtal_mode = 3;
1126 +               /* 20Mhz Xtal - TODO */
1127 +       }
1128 +
1129 +       /* RGMII */
1130 +       _mtk_mdio_write(gsw->eth, 0, 14, 0x1);
1131 +
1132 +       /* set MT7530 central align */
1133 +       val = mt7530_mdio_r32(gsw, 0x7830);
1134 +       val &= ~1;
1135 +       val |= 1<<1;
1136 +       mt7530_mdio_w32(gsw, 0x7830, val);
1137 +
1138 +       val = mt7530_mdio_r32(gsw, 0x7a40);
1139 +       val &= ~(1<<30);
1140 +       mt7530_mdio_w32(gsw, 0x7a40, val);
1141 +
1142 +       mt7530_mdio_w32(gsw, 0x7a78, 0x855);
1143 +
1144 +       /* delay setting for 10/1000M */
1145 +       mt7530_mdio_w32(gsw, 0x7b00, 0x104);
1146 +       mt7530_mdio_w32(gsw, 0x7b04, 0x10);
1147 +
1148 +       /* lower Tx Driving */
1149 +       mt7530_mdio_w32(gsw, 0x7a54, 0x88);
1150 +       mt7530_mdio_w32(gsw, 0x7a5c, 0x88);
1151 +       mt7530_mdio_w32(gsw, 0x7a64, 0x88);
1152 +       mt7530_mdio_w32(gsw, 0x7a6c, 0x88);
1153 +       mt7530_mdio_w32(gsw, 0x7a74, 0x88);
1154 +       mt7530_mdio_w32(gsw, 0x7a7c, 0x88);
1155 +       mt7530_mdio_w32(gsw, 0x7810, 0x11);
1156 +
1157 +       /* Set MT7623/MT7683 TX Driving */
1158 +       mtk_switch_w32(gsw, 0x88, 0x354);
1159 +       mtk_switch_w32(gsw, 0x88, 0x35c);
1160 +       mtk_switch_w32(gsw, 0x88, 0x364);
1161 +       mtk_switch_w32(gsw, 0x88, 0x36c);
1162 +       mtk_switch_w32(gsw, 0x88, 0x374);
1163 +       mtk_switch_w32(gsw, 0x88, 0x37c);
1164 +
1165 +       /* Set GE2 driving and slew rate */
1166 +       regmap_write(gsw->pctl, 0xF00, 0xe00);
1167 +       /* set GE2 TDSEL */
1168 +       regmap_write(gsw->pctl, 0x4C0, 0x5);
1169 +       /* set GE2 TUNE */
1170 +       regmap_write(gsw->pctl, 0xED0, 0x0);
1171 +
1172 +       regmap_write(gsw->pctl, 0xb70, 0);
1173 +       regmap_write(gsw->pctl, 0x250, 0xffff);
1174 +       regmap_write(gsw->pctl, 0x260, 0xff);
1175 +       regmap_write(gsw->pctl, 0x380, 0x37);
1176 +       regmap_write(gsw->pctl, 0x390, 0x40);
1177 +
1178 +       mt7530_trgmii_clock_setting(gsw, xtal_mode);
1179 +
1180 +       //LANWANPartition(gsw);
1181 +
1182 +       /* disable EEE */
1183 +       for (i = 0; i <= 4; i++) {
1184 +               _mtk_mdio_write(gsw->eth, i, 13, 0x7);
1185 +               _mtk_mdio_write(gsw->eth, i, 14, 0x3C);
1186 +               _mtk_mdio_write(gsw->eth, i, 13, 0x4007);
1187 +               _mtk_mdio_write(gsw->eth, i, 14, 0x0);
1188 +
1189 +               /* Increase SlvDPSready time */
1190 +               _mtk_mdio_write(gsw->eth, i, 31, 0x52b5);
1191 +               _mtk_mdio_write(gsw->eth, i, 16, 0xafae);
1192 +               _mtk_mdio_write(gsw->eth, i, 18, 0x2f);
1193 +               _mtk_mdio_write(gsw->eth, i, 16, 0x8fae);
1194 +
1195 +               /* Incease post_update_timer */
1196 +               _mtk_mdio_write(gsw->eth, i, 31, 0x3);
1197 +               _mtk_mdio_write(gsw->eth, i, 17, 0x4b);
1198 +
1199 +               /* Adjust 100_mse_threshold */
1200 +               _mtk_mdio_write(gsw->eth, i, 13, 0x1e);
1201 +               _mtk_mdio_write(gsw->eth, i, 14, 0x123);
1202 +               _mtk_mdio_write(gsw->eth, i, 13, 0x401e);
1203 +               _mtk_mdio_write(gsw->eth, i, 14, 0xffff);
1204 +
1205 +               /* Disable mcc */
1206 +               _mtk_mdio_write(gsw->eth, i, 13, 0x1e);
1207 +               _mtk_mdio_write(gsw->eth, i, 14, 0xa6);
1208 +               _mtk_mdio_write(gsw->eth, i, 13, 0x401e);
1209 +               _mtk_mdio_write(gsw->eth, i, 14, 0x300);
1210 +
1211 +               /* Disable HW auto downshift*/
1212 +               _mtk_mdio_write(gsw->eth, i, 31, 0x1);
1213 +               val = _mtk_mdio_read(gsw->eth, i, 0x14);
1214 +               val &= ~(1<<4);
1215 +               _mtk_mdio_write(gsw->eth, i, 0x14, val);
1216 +       }
1217 +
1218 +       /* turn on all PHYs */
1219 +       for (i = 0; i <= 4; i++) {
1220 +               val = _mtk_mdio_read(gsw->eth, i, 0);
1221 +               val &= ~BIT(11);
1222 +               _mtk_mdio_write(gsw->eth, i, 0, val);
1223 +       }
1224 +
1225 +       /* enable irq */
1226 +       mt7530_mdio_m32(gsw, 0, TOP_SIG_CTRL_NORMAL, MT7530_TOP_SIG_CTRL);
1227 +}
1228 +
1229 +static const struct of_device_id mediatek_gsw_match[] = {
1230 +       { .compatible = "mediatek,mt7623-gsw" },
1231 +       {},
1232 +};
1233 +MODULE_DEVICE_TABLE(of, mediatek_gsw_match);
1234 +
1235 +int mtk_gsw_init(struct mtk_eth *eth)
1236 +{
1237 +       struct device_node *np = eth->switch_np;
1238 +       struct platform_device *pdev = of_find_device_by_node(np);
1239 +       struct mt7620_gsw *gsw;
1240 +
1241 +       if (!pdev)
1242 +               return -ENODEV;
1243 +
1244 +       if (!of_device_is_compatible(np, mediatek_gsw_match->compatible))
1245 +               return -EINVAL;
1246 +
1247 +       gsw = platform_get_drvdata(pdev);
1248 +       if (!gsw)
1249 +               return -ENODEV;
1250 +       gsw->eth = eth;
1251 +       eth->sw_priv = gsw;
1252 +
1253 +       mt7623_hw_init(eth, gsw, np);
1254 +
1255 +       if (request_threaded_irq(gsw->irq, gsw_interrupt_mt7623, NULL, 0,
1256 +                                "gsw", eth))
1257 +               pr_err("fail to request irq\n");
1258 +       mt7530_mdio_w32(gsw, 0x7008, 0x1f);
1259 +
1260 +       return 0;
1261 +}
1262 +
1263 +static int mt7623_gsw_probe(struct platform_device *pdev)
1264 +{
1265 +       struct device_node *np = pdev->dev.of_node;
1266 +       struct device_node *pctl;
1267 +       int reset_pin, ret;
1268 +       struct mt7620_gsw *gsw;
1269 +       struct regulator *supply;
1270 +
1271 +       gsw = devm_kzalloc(&pdev->dev, sizeof(struct mt7620_gsw), GFP_KERNEL);
1272 +       if (!gsw)
1273 +               return -ENOMEM;
1274 +
1275 +       gsw->dev = &pdev->dev;
1276 +       gsw->trgmii_force = 2000;
1277 +       gsw->irq = irq_of_parse_and_map(np, 0);
1278 +       if (gsw->irq < 0)
1279 +               return -EINVAL;
1280 +
1281 +       gsw->ethsys = syscon_regmap_lookup_by_phandle(np, "mediatek,ethsys");
1282 +       if (IS_ERR(gsw->ethsys))
1283 +               return PTR_ERR(gsw->ethsys);
1284 +
1285 +       reset_pin = of_get_named_gpio(np, "mediatek,reset-pin", 0);
1286 +       if (reset_pin < 0)
1287 +               return reset_pin;
1288 +
1289 +       pctl = of_parse_phandle(np, "mediatek,pctl-regmap", 0);
1290 +       if (IS_ERR(pctl))
1291 +               return PTR_ERR(pctl);
1292 +
1293 +       gsw->pctl = syscon_node_to_regmap(pctl);
1294 +       if (IS_ERR(pctl))
1295 +               return PTR_ERR(pctl);
1296 +
1297 +       ret = devm_gpio_request(&pdev->dev, reset_pin, "mt7530-reset");
1298 +       if (ret)
1299 +               return ret;
1300 +
1301 +       gsw->clk_trgpll = devm_clk_get(&pdev->dev, "trgpll");
1302 +
1303 +       if (IS_ERR(gsw->clk_trgpll))
1304 +               return -ENODEV;
1305 +
1306 +       supply = devm_regulator_get(&pdev->dev, "mt7530");
1307 +       if (IS_ERR(supply))
1308 +               return PTR_ERR(supply);
1309 +
1310 +       regulator_set_voltage(supply, 1000000, 1000000);
1311 +       ret = regulator_enable(supply);
1312 +       if (ret) {
1313 +               dev_err(&pdev->dev, "Failed to enable reg-7530: %d\n", ret);
1314 +               return ret;
1315 +       }
1316 +
1317 +       gsw->wllll = of_property_read_bool(np, "mediatek,wllll");
1318 +
1319 +       pm_runtime_enable(&pdev->dev);
1320 +       pm_runtime_get_sync(&pdev->dev);
1321 +
1322 +       ret = clk_set_rate(gsw->clk_trgpll, 500000000);
1323 +       if (ret)
1324 +               return ret;
1325 +
1326 +       regmap_write(gsw->ethsys, 0x34, 0x800000);
1327 +       regmap_write(gsw->ethsys, 0x34, 0x0);
1328 +
1329 +       clk_prepare_enable(gsw->clk_trgpll);
1330 +
1331 +       gpio_direction_output(reset_pin, 0);
1332 +       udelay(1000);
1333 +       gpio_set_value(reset_pin, 1);
1334 +       mdelay(100);
1335 +
1336 +       platform_set_drvdata(pdev, gsw);
1337 +
1338 +       return 0;
1339 +}
1340 +
1341 +static int mt7623_gsw_remove(struct platform_device *pdev)
1342 +{
1343 +       struct mt7620_gsw *gsw = platform_get_drvdata(pdev);
1344 +
1345 +       clk_disable_unprepare(gsw->clk_trgpll);
1346 +
1347 +       pm_runtime_put_sync(&pdev->dev);
1348 +        pm_runtime_disable(&pdev->dev);
1349 +
1350 +       platform_set_drvdata(pdev, NULL);
1351 +
1352 +       return 0;
1353 +}
1354 +
1355 +static struct platform_driver gsw_driver = {
1356 +       .probe = mt7623_gsw_probe,
1357 +       .remove = mt7623_gsw_remove,
1358 +       .driver = {
1359 +               .name = "mt7623-gsw",
1360 +               .owner = THIS_MODULE,
1361 +               .of_match_table = mediatek_gsw_match,
1362 +       },
1363 +};
1364 +
1365 +module_platform_driver(gsw_driver);
1366 +
1367 +MODULE_LICENSE("GPL");
1368 +MODULE_AUTHOR("John Crispin <blogic@openwrt.org>");
1369 +MODULE_DESCRIPTION("GBit switch driver for Mediatek MT7623 SoC");
1370 --- /dev/null
1371 +++ b/drivers/net/ethernet/mediatek/mt7530.c
1372 @@ -0,0 +1,808 @@
1373 +/*
1374 + * This program is free software; you can redistribute it and/or
1375 + * modify it under the terms of the GNU General Public License
1376 + * as published by the Free Software Foundation; either version 2
1377 + * of the License, or (at your option) any later version.
1378 + *
1379 + * This program is distributed in the hope that it will be useful,
1380 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
1381 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
1382 + * GNU General Public License for more details.
1383 + *
1384 + * Copyright (C) 2013 John Crispin <blogic@openwrt.org>
1385 + */
1386 +
1387 +#include <linux/if.h>
1388 +#include <linux/module.h>
1389 +#include <linux/init.h>
1390 +#include <linux/list.h>
1391 +#include <linux/if_ether.h>
1392 +#include <linux/skbuff.h>
1393 +#include <linux/netdevice.h>
1394 +#include <linux/netlink.h>
1395 +#include <linux/bitops.h>
1396 +#include <net/genetlink.h>
1397 +#include <linux/switch.h>
1398 +#include <linux/delay.h>
1399 +#include <linux/phy.h>
1400 +#include <linux/netdevice.h>
1401 +#include <linux/etherdevice.h>
1402 +#include <linux/lockdep.h>
1403 +#include <linux/workqueue.h>
1404 +#include <linux/of_device.h>
1405 +
1406 +#include "mt7530.h"
1407 +
1408 +#define MT7530_CPU_PORT                6
1409 +#define MT7530_NUM_PORTS       8
1410 +#define MT7530_NUM_VLANS       16
1411 +#define MT7530_MAX_VID         4095
1412 +#define MT7530_MIN_VID         0
1413 +
1414 +/* registers */
1415 +#define REG_ESW_VLAN_VTCR              0x90
1416 +#define REG_ESW_VLAN_VAWD1             0x94
1417 +#define REG_ESW_VLAN_VAWD2             0x98
1418 +#define REG_ESW_VLAN_VTIM(x)   (0x100 + 4 * ((x) / 2))
1419 +
1420 +#define REG_ESW_VLAN_VAWD1_IVL_MAC     BIT(30)
1421 +#define REG_ESW_VLAN_VAWD1_VTAG_EN     BIT(28)
1422 +#define REG_ESW_VLAN_VAWD1_VALID       BIT(0)
1423 +
1424 +/* vlan egress mode */
1425 +enum {
1426 +       ETAG_CTRL_UNTAG = 0,
1427 +       ETAG_CTRL_TAG   = 2,
1428 +       ETAG_CTRL_SWAP  = 1,
1429 +       ETAG_CTRL_STACK = 3,
1430 +};
1431 +
1432 +#define REG_ESW_PORT_PCR(x)    (0x2004 | ((x) << 8))
1433 +#define REG_ESW_PORT_PVC(x)    (0x2010 | ((x) << 8))
1434 +#define REG_ESW_PORT_PPBV1(x)  (0x2014 | ((x) << 8))
1435 +
1436 +#define REG_HWTRAP             0x7804
1437 +
1438 +#define MIB_DESC(_s , _o, _n)   \
1439 +       {                       \
1440 +               .size = (_s),   \
1441 +               .offset = (_o), \
1442 +               .name = (_n),   \
1443 +       }
1444 +
1445 +struct mt7xxx_mib_desc {
1446 +       unsigned int size;
1447 +       unsigned int offset;
1448 +       const char *name;
1449 +};
1450 +
1451 +#define MT7621_MIB_COUNTER_BASE        0x4000
1452 +#define MT7621_MIB_COUNTER_PORT_OFFSET 0x100
1453 +#define MT7621_STATS_TDPC      0x00
1454 +#define MT7621_STATS_TCRC      0x04
1455 +#define MT7621_STATS_TUPC      0x08
1456 +#define MT7621_STATS_TMPC      0x0C
1457 +#define MT7621_STATS_TBPC      0x10
1458 +#define MT7621_STATS_TCEC      0x14
1459 +#define MT7621_STATS_TSCEC     0x18
1460 +#define MT7621_STATS_TMCEC     0x1C
1461 +#define MT7621_STATS_TDEC      0x20
1462 +#define MT7621_STATS_TLCEC     0x24
1463 +#define MT7621_STATS_TXCEC     0x28
1464 +#define MT7621_STATS_TPPC      0x2C
1465 +#define MT7621_STATS_TL64PC    0x30
1466 +#define MT7621_STATS_TL65PC    0x34
1467 +#define MT7621_STATS_TL128PC   0x38
1468 +#define MT7621_STATS_TL256PC   0x3C
1469 +#define MT7621_STATS_TL512PC   0x40
1470 +#define MT7621_STATS_TL1024PC  0x44
1471 +#define MT7621_STATS_TOC       0x48
1472 +#define MT7621_STATS_RDPC      0x60
1473 +#define MT7621_STATS_RFPC      0x64
1474 +#define MT7621_STATS_RUPC      0x68
1475 +#define MT7621_STATS_RMPC      0x6C
1476 +#define MT7621_STATS_RBPC      0x70
1477 +#define MT7621_STATS_RAEPC     0x74
1478 +#define MT7621_STATS_RCEPC     0x78
1479 +#define MT7621_STATS_RUSPC     0x7C
1480 +#define MT7621_STATS_RFEPC     0x80
1481 +#define MT7621_STATS_ROSPC     0x84
1482 +#define MT7621_STATS_RJEPC     0x88
1483 +#define MT7621_STATS_RPPC      0x8C
1484 +#define MT7621_STATS_RL64PC    0x90
1485 +#define MT7621_STATS_RL65PC    0x94
1486 +#define MT7621_STATS_RL128PC   0x98
1487 +#define MT7621_STATS_RL256PC   0x9C
1488 +#define MT7621_STATS_RL512PC   0xA0
1489 +#define MT7621_STATS_RL1024PC  0xA4
1490 +#define MT7621_STATS_ROC       0xA8
1491 +#define MT7621_STATS_RDPC_CTRL 0xB0
1492 +#define MT7621_STATS_RDPC_ING  0xB4
1493 +#define MT7621_STATS_RDPC_ARL  0xB8
1494 +
1495 +static const struct mt7xxx_mib_desc mt7621_mibs[] = {
1496 +       MIB_DESC(1, MT7621_STATS_TDPC, "TxDrop"),
1497 +       MIB_DESC(1, MT7621_STATS_TCRC, "TxCRC"),
1498 +       MIB_DESC(1, MT7621_STATS_TUPC, "TxUni"),
1499 +       MIB_DESC(1, MT7621_STATS_TMPC, "TxMulti"),
1500 +       MIB_DESC(1, MT7621_STATS_TBPC, "TxBroad"),
1501 +       MIB_DESC(1, MT7621_STATS_TCEC, "TxCollision"),
1502 +       MIB_DESC(1, MT7621_STATS_TSCEC, "TxSingleCol"),
1503 +       MIB_DESC(1, MT7621_STATS_TMCEC, "TxMultiCol"),
1504 +       MIB_DESC(1, MT7621_STATS_TDEC, "TxDefer"),
1505 +       MIB_DESC(1, MT7621_STATS_TLCEC, "TxLateCol"),
1506 +       MIB_DESC(1, MT7621_STATS_TXCEC, "TxExcCol"),
1507 +       MIB_DESC(1, MT7621_STATS_TPPC, "TxPause"),
1508 +       MIB_DESC(1, MT7621_STATS_TL64PC, "Tx64Byte"),
1509 +       MIB_DESC(1, MT7621_STATS_TL65PC, "Tx65Byte"),
1510 +       MIB_DESC(1, MT7621_STATS_TL128PC, "Tx128Byte"),
1511 +       MIB_DESC(1, MT7621_STATS_TL256PC, "Tx256Byte"),
1512 +       MIB_DESC(1, MT7621_STATS_TL512PC, "Tx512Byte"),
1513 +       MIB_DESC(1, MT7621_STATS_TL1024PC, "Tx1024Byte"),
1514 +       MIB_DESC(2, MT7621_STATS_TOC, "TxByte"),
1515 +       MIB_DESC(1, MT7621_STATS_RDPC, "RxDrop"),
1516 +       MIB_DESC(1, MT7621_STATS_RFPC, "RxFiltered"),
1517 +       MIB_DESC(1, MT7621_STATS_RUPC, "RxUni"),
1518 +       MIB_DESC(1, MT7621_STATS_RMPC, "RxMulti"),
1519 +       MIB_DESC(1, MT7621_STATS_RBPC, "RxBroad"),
1520 +       MIB_DESC(1, MT7621_STATS_RAEPC, "RxAlignErr"),
1521 +       MIB_DESC(1, MT7621_STATS_RCEPC, "RxCRC"),
1522 +       MIB_DESC(1, MT7621_STATS_RUSPC, "RxUnderSize"),
1523 +       MIB_DESC(1, MT7621_STATS_RFEPC, "RxFragment"),
1524 +       MIB_DESC(1, MT7621_STATS_ROSPC, "RxOverSize"),
1525 +       MIB_DESC(1, MT7621_STATS_RJEPC, "RxJabber"),
1526 +       MIB_DESC(1, MT7621_STATS_RPPC, "RxPause"),
1527 +       MIB_DESC(1, MT7621_STATS_RL64PC, "Rx64Byte"),
1528 +       MIB_DESC(1, MT7621_STATS_RL65PC, "Rx65Byte"),
1529 +       MIB_DESC(1, MT7621_STATS_RL128PC, "Rx128Byte"),
1530 +       MIB_DESC(1, MT7621_STATS_RL256PC, "Rx256Byte"),
1531 +       MIB_DESC(1, MT7621_STATS_RL512PC, "Rx512Byte"),
1532 +       MIB_DESC(1, MT7621_STATS_RL1024PC, "Rx1024Byte"),
1533 +       MIB_DESC(2, MT7621_STATS_ROC, "RxByte"),
1534 +       MIB_DESC(1, MT7621_STATS_RDPC_CTRL, "RxCtrlDrop"),
1535 +       MIB_DESC(1, MT7621_STATS_RDPC_ING, "RxIngDrop"),
1536 +       MIB_DESC(1, MT7621_STATS_RDPC_ARL, "RxARLDrop")
1537 +};
1538 +
1539 +enum {
1540 +       /* Global attributes. */
1541 +       MT7530_ATTR_ENABLE_VLAN,
1542 +};
1543 +
1544 +struct mt7530_port_entry {
1545 +       u16     pvid;
1546 +};
1547 +
1548 +struct mt7530_vlan_entry {
1549 +       u16     vid;
1550 +       u8      member;
1551 +       u8      etags;
1552 +};
1553 +
1554 +struct mt7530_priv {
1555 +       void __iomem            *base;
1556 +       struct mii_bus          *bus;
1557 +       struct switch_dev       swdev;
1558 +
1559 +       bool                    global_vlan_enable;
1560 +       struct mt7530_vlan_entry        vlan_entries[MT7530_NUM_VLANS];
1561 +       struct mt7530_port_entry        port_entries[MT7530_NUM_PORTS];
1562 +};
1563 +
1564 +struct mt7530_mapping {
1565 +       char    *name;
1566 +       u16     pvids[MT7530_NUM_PORTS];
1567 +       u8      members[MT7530_NUM_VLANS];
1568 +       u8      etags[MT7530_NUM_VLANS];
1569 +       u16     vids[MT7530_NUM_VLANS];
1570 +} mt7530_defaults[] = {
1571 +       {
1572 +               .name = "llllw",
1573 +               .pvids = { 1, 1, 1, 1, 2, 1, 1 },
1574 +               .members = { 0, 0x6f, 0x50 },
1575 +               .etags = { 0, 0x40, 0x40 },
1576 +               .vids = { 0, 1, 2 },
1577 +       }, {
1578 +               .name = "wllll",
1579 +               .pvids = { 2, 1, 1, 1, 1, 1, 1 },
1580 +               .members = { 0, 0x7e, 0x41 },
1581 +               .etags = { 0, 0x40, 0x40 },
1582 +               .vids = { 0, 1, 2 },
1583 +       },
1584 +};
1585 +
1586 +struct mt7530_mapping*
1587 +mt7530_find_mapping(struct device_node *np)
1588 +{
1589 +       const char *map;
1590 +       int i;
1591 +
1592 +       if (of_property_read_string(np, "mediatek,portmap", &map))
1593 +               return NULL;
1594 +
1595 +       for (i = 0; i < ARRAY_SIZE(mt7530_defaults); i++)
1596 +               if (!strcmp(map, mt7530_defaults[i].name))
1597 +                       return &mt7530_defaults[i];
1598 +
1599 +       return NULL;
1600 +}
1601 +
1602 +static void
1603 +mt7530_apply_mapping(struct mt7530_priv *mt7530, struct mt7530_mapping *map)
1604 +{
1605 +       int i = 0;
1606 +
1607 +       for (i = 0; i < MT7530_NUM_PORTS; i++)
1608 +               mt7530->port_entries[i].pvid = map->pvids[i];
1609 +
1610 +       for (i = 0; i < MT7530_NUM_VLANS; i++) {
1611 +               mt7530->vlan_entries[i].member = map->members[i];
1612 +               mt7530->vlan_entries[i].etags = map->etags[i];
1613 +               mt7530->vlan_entries[i].vid = map->vids[i];
1614 +       }
1615 +}
1616 +
1617 +static int
1618 +mt7530_reset_switch(struct switch_dev *dev)
1619 +{
1620 +       struct mt7530_priv *eth = container_of(dev, struct mt7530_priv, swdev);
1621 +       int i;
1622 +
1623 +       memset(eth->port_entries, 0, sizeof(eth->port_entries));
1624 +       memset(eth->vlan_entries, 0, sizeof(eth->vlan_entries));
1625 +
1626 +       /* set default vid of each vlan to the same number of vlan, so the vid
1627 +        * won't need be set explicitly.
1628 +        */
1629 +       for (i = 0; i < MT7530_NUM_VLANS; i++) {
1630 +               eth->vlan_entries[i].vid = i;
1631 +       }
1632 +
1633 +       return 0;
1634 +}
1635 +
1636 +static int
1637 +mt7530_get_vlan_enable(struct switch_dev *dev,
1638 +                          const struct switch_attr *attr,
1639 +                          struct switch_val *val)
1640 +{
1641 +       struct mt7530_priv *eth = container_of(dev, struct mt7530_priv, swdev);
1642 +
1643 +       val->value.i = eth->global_vlan_enable;
1644 +
1645 +       return 0;
1646 +}
1647 +
1648 +static int
1649 +mt7530_set_vlan_enable(struct switch_dev *dev,
1650 +                          const struct switch_attr *attr,
1651 +                          struct switch_val *val)
1652 +{
1653 +       struct mt7530_priv *eth = container_of(dev, struct mt7530_priv, swdev);
1654 +
1655 +       eth->global_vlan_enable = val->value.i != 0;
1656 +
1657 +       return 0;
1658 +}
1659 +
1660 +static u32
1661 +mt7530_r32(struct mt7530_priv *eth, u32 reg)
1662 +{
1663 +       u32 val;
1664 +       if (eth->bus) {
1665 +               u16 high, low;
1666 +
1667 +               mdiobus_write(eth->bus, 0x1f, 0x1f, (reg >> 6) & 0x3ff);
1668 +               low = mdiobus_read(eth->bus, 0x1f, (reg >> 2) & 0xf);
1669 +               high = mdiobus_read(eth->bus, 0x1f, 0x10);
1670 +
1671 +               return (high << 16) | (low & 0xffff);
1672 +       }
1673 +
1674 +       val = ioread32(eth->base + reg);
1675 +       pr_debug("MT7530 MDIO Read [%04x]=%08x\n", reg, val);
1676 +
1677 +       return val;
1678 +}
1679 +
1680 +static void
1681 +mt7530_w32(struct mt7530_priv *eth, u32 reg, u32 val)
1682 +{
1683 +       if (eth->bus) {
1684 +               mdiobus_write(eth->bus, 0x1f, 0x1f, (reg >> 6) & 0x3ff);
1685 +               mdiobus_write(eth->bus, 0x1f, (reg >> 2) & 0xf,  val & 0xffff);
1686 +               mdiobus_write(eth->bus, 0x1f, 0x10, val >> 16);
1687 +               return;
1688 +       }
1689 +
1690 +       pr_debug("MT7530 MDIO Write[%04x]=%08x\n", reg, val);
1691 +       iowrite32(val, eth->base + reg);
1692 +}
1693 +
1694 +static void
1695 +mt7530_vtcr(struct mt7530_priv *eth, u32 cmd, u32 val)
1696 +{
1697 +       int i;
1698 +
1699 +       mt7530_w32(eth, REG_ESW_VLAN_VTCR, BIT(31) | (cmd << 12) | val);
1700 +
1701 +       for (i = 0; i < 20; i++) {
1702 +               u32 val = mt7530_r32(eth, REG_ESW_VLAN_VTCR);
1703 +
1704 +               if ((val & BIT(31)) == 0)
1705 +                       break;
1706 +
1707 +               udelay(1000);
1708 +       }
1709 +       if (i == 20)
1710 +               printk("mt7530: vtcr timeout\n");
1711 +}
1712 +
1713 +static int
1714 +mt7530_get_port_pvid(struct switch_dev *dev, int port, int *val)
1715 +{
1716 +       struct mt7530_priv *eth = container_of(dev, struct mt7530_priv, swdev);
1717 +
1718 +       if (port >= MT7530_NUM_PORTS)
1719 +               return -EINVAL;
1720 +
1721 +       *val = mt7530_r32(eth, REG_ESW_PORT_PPBV1(port));
1722 +       *val &= 0xfff;
1723 +
1724 +       return 0;
1725 +}
1726 +
1727 +static int
1728 +mt7530_set_port_pvid(struct switch_dev *dev, int port, int pvid)
1729 +{
1730 +       struct mt7530_priv *eth = container_of(dev, struct mt7530_priv, swdev);
1731 +
1732 +       if (port >= MT7530_NUM_PORTS)
1733 +               return -EINVAL;
1734 +
1735 +       if (pvid < MT7530_MIN_VID || pvid > MT7530_MAX_VID)
1736 +               return -EINVAL;
1737 +
1738 +       eth->port_entries[port].pvid = pvid;
1739 +
1740 +       return 0;
1741 +}
1742 +
1743 +static int
1744 +mt7530_get_vlan_ports(struct switch_dev *dev, struct switch_val *val)
1745 +{
1746 +       struct mt7530_priv *eth = container_of(dev, struct mt7530_priv, swdev);
1747 +       u32 member;
1748 +       u32 etags;
1749 +       int i;
1750 +
1751 +       val->len = 0;
1752 +
1753 +       if (val->port_vlan < 0 || val->port_vlan >= MT7530_NUM_VLANS)
1754 +               return -EINVAL;
1755 +
1756 +       mt7530_vtcr(eth, 0, val->port_vlan);
1757 +
1758 +       member = mt7530_r32(eth, REG_ESW_VLAN_VAWD1);
1759 +       member >>= 16;
1760 +       member &= 0xff;
1761 +
1762 +       etags = mt7530_r32(eth, REG_ESW_VLAN_VAWD2);
1763 +
1764 +       for (i = 0; i < MT7530_NUM_PORTS; i++) {
1765 +               struct switch_port *p;
1766 +               int etag;
1767 +
1768 +               if (!(member & BIT(i)))
1769 +                       continue;
1770 +
1771 +               p = &val->value.ports[val->len++];
1772 +               p->id = i;
1773 +
1774 +               etag = (etags >> (i * 2)) & 0x3;
1775 +
1776 +               if (etag == ETAG_CTRL_TAG)
1777 +                       p->flags |= BIT(SWITCH_PORT_FLAG_TAGGED);
1778 +               else if (etag != ETAG_CTRL_UNTAG)
1779 +                       printk("vlan egress tag control neither untag nor tag.\n");
1780 +       }
1781 +
1782 +       return 0;
1783 +}
1784 +
1785 +static int
1786 +mt7530_set_vlan_ports(struct switch_dev *dev, struct switch_val *val)
1787 +{
1788 +       struct mt7530_priv *eth = container_of(dev, struct mt7530_priv, swdev);
1789 +       u8 member = 0;
1790 +       u8 etags = 0;
1791 +       int i;
1792 +
1793 +       if (val->port_vlan < 0 || val->port_vlan >= MT7530_NUM_VLANS ||
1794 +                       val->len > MT7530_NUM_PORTS)
1795 +               return -EINVAL;
1796 +
1797 +       for (i = 0; i < val->len; i++) {
1798 +               struct switch_port *p = &val->value.ports[i];
1799 +
1800 +               if (p->id >= MT7530_NUM_PORTS)
1801 +                       return -EINVAL;
1802 +
1803 +               member |= BIT(p->id);
1804 +
1805 +               if (p->flags & BIT(SWITCH_PORT_FLAG_TAGGED))
1806 +                       etags |= BIT(p->id);
1807 +       }
1808 +       eth->vlan_entries[val->port_vlan].member = member;
1809 +       eth->vlan_entries[val->port_vlan].etags = etags;
1810 +
1811 +       return 0;
1812 +}
1813 +
1814 +static int
1815 +mt7530_set_vid(struct switch_dev *dev, const struct switch_attr *attr,
1816 +               struct switch_val *val)
1817 +{
1818 +       struct mt7530_priv *eth = container_of(dev, struct mt7530_priv, swdev);
1819 +       int vlan;
1820 +       u16 vid;
1821 +
1822 +       vlan = val->port_vlan;
1823 +       vid = (u16)val->value.i;
1824 +
1825 +       if (vlan < 0 || vlan >= MT7530_NUM_VLANS)
1826 +               return -EINVAL;
1827 +
1828 +       if (vid < MT7530_MIN_VID || vid > MT7530_MAX_VID)
1829 +               return -EINVAL;
1830 +
1831 +       eth->vlan_entries[vlan].vid = vid;
1832 +       return 0;
1833 +}
1834 +
1835 +static int
1836 +mt7530_get_vid(struct switch_dev *dev, const struct switch_attr *attr,
1837 +               struct switch_val *val)
1838 +{
1839 +       struct mt7530_priv *eth = container_of(dev, struct mt7530_priv, swdev);
1840 +       u32 vid;
1841 +       int vlan;
1842 +
1843 +       vlan = val->port_vlan;
1844 +
1845 +       vid = mt7530_r32(eth, REG_ESW_VLAN_VTIM(vlan));
1846 +       if (vlan & 1)
1847 +               vid = vid >> 12;
1848 +       vid &= 0xfff;
1849 +
1850 +       val->value.i = vid;
1851 +       return 0;
1852 +}
1853 +
1854 +static int
1855 +mt7530_apply_config(struct switch_dev *dev)
1856 +{
1857 +       struct mt7530_priv *eth = container_of(dev, struct mt7530_priv, swdev);
1858 +       int i, j;
1859 +       u8 tag_ports;
1860 +       u8 untag_ports;
1861 +
1862 +       if (!eth->global_vlan_enable) {
1863 +               for (i = 0; i < MT7530_NUM_PORTS; i++)
1864 +                       mt7530_w32(eth, REG_ESW_PORT_PCR(i), 0x00ff0000);
1865 +
1866 +               for (i = 0; i < MT7530_NUM_PORTS; i++)
1867 +                       mt7530_w32(eth, REG_ESW_PORT_PVC(i), 0x810000c0);
1868 +
1869 +               return 0;
1870 +       }
1871 +
1872 +       /* set all ports as security mode */
1873 +       for (i = 0; i < MT7530_NUM_PORTS; i++)
1874 +               mt7530_w32(eth, REG_ESW_PORT_PCR(i), 0x00ff0003);
1875 +
1876 +       /* check if a port is used in tag/untag vlan egress mode */
1877 +       tag_ports = 0;
1878 +       untag_ports = 0;
1879 +
1880 +       for (i = 0; i < MT7530_NUM_VLANS; i++) {
1881 +               u8 member = eth->vlan_entries[i].member;
1882 +               u8 etags = eth->vlan_entries[i].etags;
1883 +
1884 +               if (!member)
1885 +                       continue;
1886 +
1887 +               for (j = 0; j < MT7530_NUM_PORTS; j++) {
1888 +                       if (!(member & BIT(j)))
1889 +                               continue;
1890 +
1891 +                       if (etags & BIT(j))
1892 +                               tag_ports |= 1u << j;
1893 +                       else
1894 +                               untag_ports |= 1u << j;
1895 +               }
1896 +       }
1897 +
1898 +       /* set all untag-only ports as transparent and the rest as user port */
1899 +       for (i = 0; i < MT7530_NUM_PORTS; i++) {
1900 +               u32 pvc_mode = 0x81000000;
1901 +
1902 +               if (untag_ports & BIT(i) && !(tag_ports & BIT(i)))
1903 +                       pvc_mode = 0x810000c0;
1904 +
1905 +               mt7530_w32(eth, REG_ESW_PORT_PVC(i), pvc_mode);
1906 +       }
1907 +
1908 +       for (i = 0; i < MT7530_NUM_VLANS; i++) {
1909 +               u16 vid = eth->vlan_entries[i].vid;
1910 +               u8 member = eth->vlan_entries[i].member;
1911 +               u8 etags = eth->vlan_entries[i].etags;
1912 +               u32 val;
1913 +
1914 +               /* vid of vlan */
1915 +               val = mt7530_r32(eth, REG_ESW_VLAN_VTIM(i));
1916 +               if (i % 2 == 0) {
1917 +                       val &= 0xfff000;
1918 +                       val |= vid;
1919 +               } else {
1920 +                       val &= 0xfff;
1921 +                       val |= (vid << 12);
1922 +               }
1923 +               mt7530_w32(eth, REG_ESW_VLAN_VTIM(i), val);
1924 +
1925 +               /* vlan port membership */
1926 +               if (member)
1927 +                       mt7530_w32(eth, REG_ESW_VLAN_VAWD1, REG_ESW_VLAN_VAWD1_IVL_MAC |
1928 +                               REG_ESW_VLAN_VAWD1_VTAG_EN | (member << 16) |
1929 +                               REG_ESW_VLAN_VAWD1_VALID);
1930 +               else
1931 +                       mt7530_w32(eth, REG_ESW_VLAN_VAWD1, 0);
1932 +
1933 +               /* egress mode */
1934 +               val = 0;
1935 +               for (j = 0; j < MT7530_NUM_PORTS; j++) {
1936 +                       if (etags & BIT(j))
1937 +                               val |= ETAG_CTRL_TAG << (j * 2);
1938 +                       else
1939 +                               val |= ETAG_CTRL_UNTAG << (j * 2);
1940 +               }
1941 +               mt7530_w32(eth, REG_ESW_VLAN_VAWD2, val);
1942 +
1943 +               /* write to vlan table */
1944 +               mt7530_vtcr(eth, 1, i);
1945 +       }
1946 +
1947 +       /* Port Default PVID */
1948 +       for (i = 0; i < MT7530_NUM_PORTS; i++) {
1949 +               u32 val;
1950 +               val = mt7530_r32(eth, REG_ESW_PORT_PPBV1(i));
1951 +               val &= ~0xfff;
1952 +               val |= eth->port_entries[i].pvid;
1953 +               mt7530_w32(eth, REG_ESW_PORT_PPBV1(i), val);
1954 +       }
1955 +
1956 +       return 0;
1957 +}
1958 +
1959 +static int
1960 +mt7530_get_port_link(struct switch_dev *dev,  int port,
1961 +                       struct switch_port_link *link)
1962 +{
1963 +       struct mt7530_priv *eth = container_of(dev, struct mt7530_priv, swdev);
1964 +       u32 speed, pmsr;
1965 +
1966 +       if (port < 0 || port >= MT7530_NUM_PORTS)
1967 +               return -EINVAL;
1968 +
1969 +       pmsr = mt7530_r32(eth, 0x3008 + (0x100 * port));
1970 +
1971 +       link->link = pmsr & 1;
1972 +       link->duplex = (pmsr >> 1) & 1;
1973 +       speed = (pmsr >> 2) & 3;
1974 +
1975 +       switch (speed) {
1976 +       case 0:
1977 +               link->speed = SWITCH_PORT_SPEED_10;
1978 +               break;
1979 +       case 1:
1980 +               link->speed = SWITCH_PORT_SPEED_100;
1981 +               break;
1982 +       case 2:
1983 +       case 3: /* forced gige speed can be 2 or 3 */
1984 +               link->speed = SWITCH_PORT_SPEED_1000;
1985 +               break;
1986 +       default:
1987 +               link->speed = SWITCH_PORT_SPEED_UNKNOWN;
1988 +               break;
1989 +       }
1990 +
1991 +       return 0;
1992 +}
1993 +
1994 +static const struct switch_attr mt7530_global[] = {
1995 +       {
1996 +               .type = SWITCH_TYPE_INT,
1997 +               .name = "enable_vlan",
1998 +               .description = "VLAN mode (1:enabled)",
1999 +               .max = 1,
2000 +               .id = MT7530_ATTR_ENABLE_VLAN,
2001 +               .get = mt7530_get_vlan_enable,
2002 +               .set = mt7530_set_vlan_enable,
2003 +       },
2004 +};
2005 +
2006 +static u64 get_mib_counter(struct mt7530_priv *eth, int i, int port)
2007 +{
2008 +       unsigned int port_base;
2009 +       u64 t;
2010 +
2011 +       port_base = MT7621_MIB_COUNTER_BASE +
2012 +                   MT7621_MIB_COUNTER_PORT_OFFSET * port;
2013 +
2014 +       t = mt7530_r32(eth, port_base + mt7621_mibs[i].offset);
2015 +       if (mt7621_mibs[i].size == 2) {
2016 +               u64 hi;
2017 +
2018 +               hi = mt7530_r32(eth, port_base + mt7621_mibs[i].offset + 4);
2019 +               t |= hi << 32;
2020 +       }
2021 +
2022 +       return t;
2023 +}
2024 +
2025 +static int mt7621_sw_get_port_mib(struct switch_dev *dev,
2026 +                                 const struct switch_attr *attr,
2027 +                                 struct switch_val *val)
2028 +{
2029 +       static char buf[4096];
2030 +       struct mt7530_priv *eth = container_of(dev, struct mt7530_priv, swdev);
2031 +       int i, len = 0;
2032 +
2033 +       if (val->port_vlan >= MT7530_NUM_PORTS)
2034 +               return -EINVAL;
2035 +
2036 +       len += snprintf(buf + len, sizeof(buf) - len,
2037 +                       "Port %d MIB counters\n", val->port_vlan);
2038 +
2039 +       for (i = 0; i < sizeof(mt7621_mibs) / sizeof(*mt7621_mibs); ++i) {
2040 +               u64 counter;
2041 +               len += snprintf(buf + len, sizeof(buf) - len,
2042 +                               "%-11s: ", mt7621_mibs[i].name);
2043 +               counter = get_mib_counter(eth, i, val->port_vlan);
2044 +               len += snprintf(buf + len, sizeof(buf) - len, "%llu\n",
2045 +                               counter);
2046 +       }
2047 +
2048 +       val->value.s = buf;
2049 +       val->len = len;
2050 +       return 0;
2051 +}
2052 +
2053 +static const struct switch_attr mt7621_port[] = {
2054 +       {
2055 +               .type = SWITCH_TYPE_STRING,
2056 +               .name = "mib",
2057 +               .description = "Get MIB counters for port",
2058 +               .get = mt7621_sw_get_port_mib,
2059 +               .set = NULL,
2060 +       },
2061 +};
2062 +
2063 +static const struct switch_attr mt7530_port[] = {
2064 +};
2065 +
2066 +static const struct switch_attr mt7530_vlan[] = {
2067 +       {
2068 +               .type = SWITCH_TYPE_INT,
2069 +               .name = "vid",
2070 +               .description = "VLAN ID (0-4094)",
2071 +               .set = mt7530_set_vid,
2072 +               .get = mt7530_get_vid,
2073 +               .max = 4094,
2074 +       },
2075 +};
2076 +
2077 +static const struct switch_dev_ops mt7621_ops = {
2078 +       .attr_global = {
2079 +               .attr = mt7530_global,
2080 +               .n_attr = ARRAY_SIZE(mt7530_global),
2081 +       },
2082 +/*     .attr_port = {
2083 +               .attr = mt7621_port,
2084 +               .n_attr = ARRAY_SIZE(mt7621_port),
2085 +       },*/
2086 +       .attr_vlan = {
2087 +               .attr = mt7530_vlan,
2088 +               .n_attr = ARRAY_SIZE(mt7530_vlan),
2089 +       },
2090 +       .get_vlan_ports = mt7530_get_vlan_ports,
2091 +       .set_vlan_ports = mt7530_set_vlan_ports,
2092 +       .get_port_pvid = mt7530_get_port_pvid,
2093 +       .set_port_pvid = mt7530_set_port_pvid,
2094 +       .get_port_link = mt7530_get_port_link,
2095 +       .apply_config = mt7530_apply_config,
2096 +       .reset_switch = mt7530_reset_switch,
2097 +};
2098 +
2099 +static const struct switch_dev_ops mt7530_ops = {
2100 +       .attr_global = {
2101 +               .attr = mt7530_global,
2102 +               .n_attr = ARRAY_SIZE(mt7530_global),
2103 +       },
2104 +       .attr_port = {
2105 +               .attr = mt7530_port,
2106 +               .n_attr = ARRAY_SIZE(mt7530_port),
2107 +       },
2108 +       .attr_vlan = {
2109 +               .attr = mt7530_vlan,
2110 +               .n_attr = ARRAY_SIZE(mt7530_vlan),
2111 +       },
2112 +       .get_vlan_ports = mt7530_get_vlan_ports,
2113 +       .set_vlan_ports = mt7530_set_vlan_ports,
2114 +       .get_port_pvid = mt7530_get_port_pvid,
2115 +       .set_port_pvid = mt7530_set_port_pvid,
2116 +       .get_port_link = mt7530_get_port_link,
2117 +       .apply_config = mt7530_apply_config,
2118 +       .reset_switch = mt7530_reset_switch,
2119 +};
2120 +
2121 +int
2122 +mt7530_probe(struct device *dev, void __iomem *base, struct mii_bus *bus, int vlan)
2123 +{
2124 +       struct switch_dev *swdev;
2125 +       struct mt7530_priv *mt7530;
2126 +       struct mt7530_mapping *map;
2127 +       int ret;
2128 +
2129 +       mt7530 = devm_kzalloc(dev, sizeof(struct mt7530_priv), GFP_KERNEL);
2130 +       if (!mt7530)
2131 +               return -ENOMEM;
2132 +
2133 +       mt7530->base = base;
2134 +       mt7530->bus = bus;
2135 +       mt7530->global_vlan_enable = vlan;
2136 +
2137 +       swdev = &mt7530->swdev;
2138 +       if (bus) {
2139 +               swdev->alias = "mt7530";
2140 +               swdev->name = "mt7530";
2141 +       } else if (IS_ENABLED(CONFIG_MACH_MT7623)) {
2142 +               swdev->alias = "mt7623";
2143 +               swdev->name = "mt7623";
2144 +       } else if (IS_ENABLED(CONFIG_SOC_MT7621)) {
2145 +               swdev->alias = "mt7621";
2146 +               swdev->name = "mt7621";
2147 +       } else {
2148 +               swdev->alias = "mt7620";
2149 +               swdev->name = "mt7620";
2150 +       }
2151 +       swdev->cpu_port = MT7530_CPU_PORT;
2152 +       swdev->ports = MT7530_NUM_PORTS;
2153 +       swdev->vlans = MT7530_NUM_VLANS;
2154 +       if (IS_ENABLED(CONFIG_SOC_MT7621) || IS_ENABLED(CONFIG_MACH_MT7623))
2155 +               swdev->ops = &mt7621_ops;
2156 +       else
2157 +               swdev->ops = &mt7530_ops;
2158 +
2159 +       ret = register_switch(swdev, NULL);
2160 +       if (ret) {
2161 +               dev_err(dev, "failed to register mt7530\n");
2162 +               return ret;
2163 +       }
2164 +
2165 +       mt7530_reset_switch(swdev);
2166 +
2167 +       map = mt7530_find_mapping(dev->of_node);
2168 +       if (map)
2169 +               mt7530_apply_mapping(mt7530, map);
2170 +       mt7530_apply_config(swdev);
2171 +
2172 +       /* magic vodoo */
2173 +       if (!(IS_ENABLED(CONFIG_SOC_MT7621) || IS_ENABLED(CONFIG_MACH_MT7623)) && bus && mt7530_r32(mt7530, REG_HWTRAP) !=  0x1117edf) {
2174 +               dev_info(dev, "fixing up MHWTRAP register - bootloader probably played with it\n");
2175 +               mt7530_w32(mt7530, REG_HWTRAP, 0x1117edf);
2176 +       }
2177 +       dev_info(dev, "loaded %s driver\n", swdev->name);
2178 +
2179 +       return 0;
2180 +}
2181 --- /dev/null
2182 +++ b/drivers/net/ethernet/mediatek/mt7530.h
2183 @@ -0,0 +1,20 @@
2184 +/*
2185 + * This program is free software; you can redistribute it and/or
2186 + * modify it under the terms of the GNU General Public License
2187 + * as published by the Free Software Foundation; either version 2
2188 + * of the License, or (at your option) any later version.
2189 + *
2190 + * This program is distributed in the hope that it will be useful,
2191 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
2192 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
2193 + * GNU General Public License for more details.
2194 + *
2195 + * Copyright (C) 2013 John Crispin <blogic@openwrt.org>
2196 + */
2197 +
2198 +#ifndef _MT7530_H__
2199 +#define _MT7530_H__
2200 +
2201 +int mt7530_probe(struct device *dev, void __iomem *base, struct mii_bus *bus, int vlan);
2202 +
2203 +#endif
2204 --- a/drivers/net/ethernet/mediatek/mtk_eth_soc.c
2205 +++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.c
2206 @@ -25,6 +25,9 @@
2207
2208  #include "mtk_eth_soc.h"
2209
2210 +/* the callback used by the driver core to bringup the switch */
2211 +int mtk_gsw_init(struct mtk_eth *eth);
2212 +
2213  static int mtk_msg_level = -1;
2214  module_param_named(msg_level, mtk_msg_level, int, 0);
2215  MODULE_PARM_DESC(msg_level, "Message level (-1=defaults,0=none,...,16=all)");
2216 @@ -74,14 +77,14 @@
2217                         return 0;
2218                 if (time_after(jiffies, t_start + PHY_IAC_TIMEOUT))
2219                         break;
2220 -               usleep_range(10, 20);
2221 +//             usleep_range(10, 20);
2222         }
2223
2224         dev_err(eth->dev, "mdio: MDIO timeout\n");
2225         return -1;
2226  }
2227
2228 -static u32 _mtk_mdio_write(struct mtk_eth *eth, u32 phy_addr,
2229 +u32 _mtk_mdio_write(struct mtk_eth *eth, u32 phy_addr,
2230                            u32 phy_register, u32 write_data)
2231  {
2232         if (mtk_mdio_busy_wait(eth))
2233 @@ -100,7 +103,7 @@
2234         return 0;
2235  }
2236
2237 -static u32 _mtk_mdio_read(struct mtk_eth *eth, int phy_addr, int phy_reg)
2238 +u32 _mtk_mdio_read(struct mtk_eth *eth, int phy_addr, int phy_reg)
2239  {
2240         u32 d;
2241
2242 @@ -155,7 +158,7 @@
2243
2244         val = (speed == SPEED_1000) ?
2245                 RCK_CTRL_RGMII_1000 : RCK_CTRL_RGMII_10_100;
2246 -       mtk_w32(eth, val, TRGMII_RCK_CTRL);
2247 +       mtk_w32(eth, val, _TRGMII_RCK_CTRL);
2248
2249         val = (speed == SPEED_1000) ?
2250                 TCK_CTRL_RGMII_1000 : TCK_CTRL_RGMII_10_100;
2251 @@ -1833,15 +1836,6 @@
2252         }
2253         regmap_write(eth->ethsys, ETHSYS_SYSCFG0, val);
2254
2255 -       /* Set GE2 driving and slew rate */
2256 -       regmap_write(eth->pctl, GPIO_DRV_SEL10, 0xa00);
2257 -
2258 -       /* set GE2 TDSEL */
2259 -       regmap_write(eth->pctl, GPIO_OD33_CTRL8, 0x5);
2260 -
2261 -       /* set GE2 TUNE */
2262 -       regmap_write(eth->pctl, GPIO_BIAS_CTRL, 0x0);
2263 -
2264         /* GE1, Force 1000M/FD, FC ON */
2265         mtk_w32(eth, MAC_MCR_FIXED_LINK, MTK_MAC_MCR(0));
2266
2267 @@ -1851,6 +1845,8 @@
2268         /* Enable RX VLan Offloading */
2269         mtk_w32(eth, 1, MTK_CDMP_EG_CTRL);
2270
2271 +       mtk_gsw_init(eth);
2272 +
2273         /* disable delay and normal interrupt */
2274         mtk_w32(eth, 0, MTK_QDMA_DELAY_INT);
2275         mtk_w32(eth, 0, MTK_PDMA_DELAY_INT);
2276 @@ -1879,6 +1875,8 @@
2277                 mtk_w32(eth, val, MTK_GDMA_FWD_CFG(i));
2278         }
2279
2280 +       mt7623_gsw_config(eth);
2281 +
2282         return 0;
2283  }
2284
2285 @@ -2379,6 +2377,9 @@
2286         if (!eth)
2287                 return -ENOMEM;
2288
2289 +       eth->switch_np = of_parse_phandle(pdev->dev.of_node,
2290 +                                         "mediatek,switch", 0);
2291 +
2292         eth->dev = &pdev->dev;
2293         eth->base = devm_ioremap_resource(&pdev->dev, res);
2294         if (IS_ERR(eth->base))
2295 --- a/drivers/net/ethernet/mediatek/mtk_eth_soc.h
2296 +++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.h
2297 @@ -314,7 +314,7 @@
2298                                  MAC_MCR_FORCE_DPX | MAC_MCR_FORCE_LINK)
2299
2300  /* TRGMII RXC control register */
2301 -#define TRGMII_RCK_CTRL                0x10300
2302 +#define _TRGMII_RCK_CTRL               0x10300
2303  #define DQSI0(x)               ((x << 0) & GENMASK(6, 0))
2304  #define DQSI1(x)               ((x << 8) & GENMASK(14, 8))
2305  #define RXCTL_DMWTLAT(x)       ((x << 16) & GENMASK(18, 16))
2306 @@ -554,6 +554,9 @@
2307         struct mii_bus                  *mii_bus;
2308         struct work_struct              pending_work;
2309         unsigned long                   state;
2310 +
2311 +       struct device_node              *switch_np;
2312 +       void                            *sw_priv;
2313  };
2314
2315  /* struct mtk_mac -    the structure that holds the info about the MACs of the
2316 @@ -586,4 +589,6 @@
2317  void mtk_w32(struct mtk_eth *eth, u32 val, unsigned reg);
2318  u32 mtk_r32(struct mtk_eth *eth, unsigned reg);
2319
2320 +int mt7623_gsw_config(struct mtk_eth *eth);
2321 +
2322  #endif /* MTK_ETH_H */