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

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