target/linux/mediatek/patches-4.4/0057-net-mediatek-out-of-tree-fixes.patch

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