target/linux/mediatek/patches-4.4/0089-net-mediatek-add-gsw-mt7530-driver.patch

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