diff --git a/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/Makefile b/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/Makefile
new file mode 100755
index 0000000..e304fcb
--- /dev/null
+++ b/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/Makefile
@@ -0,0 +1,11 @@
+#
+# Makefile for MediaTek MT753x gigabit switch
+#
+
+obj-$(CONFIG_MT753X_GSW)	+= mt753x.o
+
+mt753x-$(CONFIG_SWCONFIG)	+= mt753x_swconfig.o
+
+mt753x-y			+= mt753x_mdio.o mt7530.o mt7531.o \
+					mt753x_common.o mt753x_vlan.o mt753x_nl.o
+
diff --git a/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt7530.c b/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt7530.c
new file mode 100755
index 0000000..7853e27
--- /dev/null
+++ b/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt7530.c
@@ -0,0 +1,644 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2018 MediaTek Inc.
+ * Author: Weijie Gao <weijie.gao@mediatek.com>
+ */
+
+#include <linux/kernel.h>
+#include <linux/delay.h>
+
+#include "mt753x.h"
+#include "mt753x_regs.h"
+
+/* MT7530 registers */
+
+/* Unique fields of PMCR for MT7530 */
+#define FORCE_MODE			BIT(15)
+
+/* Unique fields of GMACCR for MT7530 */
+#define VLAN_SUPT_NO_S			14
+#define VLAN_SUPT_NO_M			0x1c000
+#define LATE_COL_DROP			BIT(13)
+
+/* Unique fields of (M)HWSTRAP for MT7530 */
+#define BOND_OPTION			BIT(24)
+#define P5_PHY0_SEL			BIT(20)
+#define CHG_TRAP			BIT(16)
+#define LOOPDET_DIS			BIT(14)
+#define P5_INTF_SEL_GMAC5		BIT(13)
+#define SMI_ADDR_S			11
+#define SMI_ADDR_M			0x1800
+#define XTAL_FSEL_S			9
+#define XTAL_FSEL_M			0x600
+#define P6_INTF_DIS			BIT(8)
+#define P5_INTF_MODE_RGMII		BIT(7)
+#define P5_INTF_DIS_S			BIT(6)
+#define C_MDIO_BPS_S			BIT(5)
+#define EEPROM_EN_S			BIT(4)
+
+/* PHY EEE Register bitmap of define */
+#define PHY_DEV07			0x07
+#define PHY_DEV07_REG_03C		0x3c
+
+/* PHY Extend Register 0x14 bitmap of define */
+#define PHY_EXT_REG_14			0x14
+
+/* Fields of PHY_EXT_REG_14 */
+#define PHY_EN_DOWN_SHFIT		BIT(4)
+
+/* PHY Token Ring Register 0x10 bitmap of define */
+#define PHY_TR_REG_10			0x10
+
+/* PHY Token Ring Register 0x12 bitmap of define */
+#define PHY_TR_REG_12			0x12
+
+/* PHY LPI PCS/DSP Control Register bitmap of define */
+#define PHY_LPI_REG_11			0x11
+
+/* PHY DEV 0x1e Register bitmap of define */
+#define PHY_DEV1E			0x1e
+#define PHY_DEV1E_REG_123		0x123
+#define PHY_DEV1E_REG_A6		0xa6
+
+/* Values of XTAL_FSEL */
+#define XTAL_20MHZ			1
+#define XTAL_40MHZ			2
+#define XTAL_25MHZ			3
+
+/* Top single control CR define */
+#define TOP_SIG_CTRL			0x7808
+
+/* TOP_SIG_CTRL Register bitmap of define */
+#define OUTPUT_INTR_S			16
+#define OUTPUT_INTR_M			0x30000
+
+#define P6ECR				0x7830
+#define P6_INTF_MODE_TRGMII		BIT(0)
+
+#define TRGMII_TXCTRL			0x7a40
+#define TRAIN_TXEN			BIT(31)
+#define TXC_INV				BIT(30)
+#define TX_DOEO				BIT(29)
+#define TX_RST				BIT(28)
+
+#define TRGMII_TD0_CTRL			0x7a50
+#define TRGMII_TD1_CTRL			0x7a58
+#define TRGMII_TD2_CTRL			0x7a60
+#define TRGMII_TD3_CTRL			0x7a68
+#define TRGMII_TXCTL_CTRL		0x7a70
+#define TRGMII_TCK_CTRL			0x7a78
+#define TRGMII_TD_CTRL(n)		(0x7a50 + (n) * 8)
+#define NUM_TRGMII_CTRL			6
+#define TX_DMPEDRV			BIT(31)
+#define TX_DM_SR			BIT(15)
+#define TX_DMERODT			BIT(14)
+#define TX_DMOECTL			BIT(13)
+#define TX_TAP_S			8
+#define TX_TAP_M			0xf00
+#define TX_TRAIN_WD_S			0
+#define TX_TRAIN_WD_M			0xff
+
+#define TRGMII_TD0_ODT			0x7a54
+#define TRGMII_TD1_ODT			0x7a5c
+#define TRGMII_TD2_ODT			0x7a64
+#define TRGMII_TD3_ODT			0x7a6c
+#define TRGMII_TXCTL_ODT		0x7574
+#define TRGMII_TCK_ODT			0x757c
+#define TRGMII_TD_ODT(n)		(0x7a54 + (n) * 8)
+#define NUM_TRGMII_ODT			6
+#define TX_DM_DRVN_PRE_S		30
+#define TX_DM_DRVN_PRE_M		0xc0000000
+#define TX_DM_DRVP_PRE_S		28
+#define TX_DM_DRVP_PRE_M		0x30000000
+#define TX_DM_TDSEL_S			24
+#define TX_DM_TDSEL_M			0xf000000
+#define TX_ODTEN			BIT(23)
+#define TX_DME_PRE			BIT(20)
+#define TX_DM_DRVNT0			BIT(19)
+#define TX_DM_DRVPT0			BIT(18)
+#define TX_DM_DRVNTE			BIT(17)
+#define TX_DM_DRVPTE			BIT(16)
+#define TX_DM_ODTN_S			12
+#define TX_DM_ODTN_M			0x7000
+#define TX_DM_ODTP_S			8
+#define TX_DM_ODTP_M			0x700
+#define TX_DM_DRVN_S			4
+#define TX_DM_DRVN_M			0xf0
+#define TX_DM_DRVP_S			0
+#define TX_DM_DRVP_M			0x0f
+
+#define P5RGMIIRXCR			0x7b00
+#define CSR_RGMII_RCTL_CFG_S		24
+#define CSR_RGMII_RCTL_CFG_M		0x7000000
+#define CSR_RGMII_RXD_CFG_S		16
+#define CSR_RGMII_RXD_CFG_M		0x70000
+#define CSR_RGMII_EDGE_ALIGN		BIT(8)
+#define CSR_RGMII_RXC_90DEG_CFG_S	4
+#define CSR_RGMII_RXC_90DEG_CFG_M	0xf0
+#define CSR_RGMII_RXC_0DEG_CFG_S	0
+#define CSR_RGMII_RXC_0DEG_CFG_M	0x0f
+
+#define P5RGMIITXCR			0x7b04
+#define CSR_RGMII_TXEN_CFG_S		16
+#define CSR_RGMII_TXEN_CFG_M		0x70000
+#define CSR_RGMII_TXD_CFG_S		8
+#define CSR_RGMII_TXD_CFG_M		0x700
+#define CSR_RGMII_TXC_CFG_S		0
+#define CSR_RGMII_TXC_CFG_M		0x1f
+
+#define CHIP_REV			0x7ffc
+#define CHIP_NAME_S			16
+#define CHIP_NAME_M			0xffff0000
+#define CHIP_REV_S			0
+#define CHIP_REV_M			0x0f
+
+/* MMD registers */
+#define CORE_PLL_GROUP2			0x401
+#define RG_SYSPLL_EN_NORMAL		BIT(15)
+#define RG_SYSPLL_VODEN			BIT(14)
+#define RG_SYSPLL_POSDIV_S		5
+#define RG_SYSPLL_POSDIV_M		0x60
+
+#define CORE_PLL_GROUP4			0x403
+#define RG_SYSPLL_DDSFBK_EN		BIT(12)
+#define RG_SYSPLL_BIAS_EN		BIT(11)
+#define RG_SYSPLL_BIAS_LPF_EN		BIT(10)
+
+#define CORE_PLL_GROUP5			0x404
+#define RG_LCDDS_PCW_NCPO1_S		0
+#define RG_LCDDS_PCW_NCPO1_M		0xffff
+
+#define CORE_PLL_GROUP6			0x405
+#define RG_LCDDS_PCW_NCPO0_S		0
+#define RG_LCDDS_PCW_NCPO0_M		0xffff
+
+#define CORE_PLL_GROUP7			0x406
+#define RG_LCDDS_PWDB			BIT(15)
+#define RG_LCDDS_ISO_EN			BIT(13)
+#define RG_LCCDS_C_S			4
+#define RG_LCCDS_C_M			0x70
+#define RG_LCDDS_PCW_NCPO_CHG		BIT(3)
+
+#define CORE_PLL_GROUP10		0x409
+#define RG_LCDDS_SSC_DELTA_S		0
+#define RG_LCDDS_SSC_DELTA_M		0xfff
+
+#define CORE_PLL_GROUP11		0x40a
+#define RG_LCDDS_SSC_DELTA1_S		0
+#define RG_LCDDS_SSC_DELTA1_M		0xfff
+
+#define CORE_GSWPLL_GCR_1		0x040d
+#define GSWPLL_PREDIV_S			14
+#define GSWPLL_PREDIV_M			0xc000
+#define GSWPLL_POSTDIV_200M_S		12
+#define GSWPLL_POSTDIV_200M_M		0x3000
+#define GSWPLL_EN_PRE			BIT(11)
+#define GSWPLL_FBKSEL			BIT(10)
+#define GSWPLL_BP			BIT(9)
+#define GSWPLL_BR			BIT(8)
+#define GSWPLL_FBKDIV_200M_S		0
+#define GSWPLL_FBKDIV_200M_M		0xff
+
+#define CORE_GSWPLL_GCR_2		0x040e
+#define GSWPLL_POSTDIV_500M_S		8
+#define GSWPLL_POSTDIV_500M_M		0x300
+#define GSWPLL_FBKDIV_500M_S		0
+#define GSWPLL_FBKDIV_500M_M		0xff
+
+#define TRGMII_GSW_CLK_CG		0x0410
+#define TRGMIICK_EN			BIT(1)
+#define GSWCK_EN			BIT(0)
+
+static int mt7530_mii_read(struct gsw_mt753x *gsw, int phy, int reg)
+{
+	if (phy < MT753X_NUM_PHYS)
+		phy = (gsw->phy_base + phy) & MT753X_SMI_ADDR_MASK;
+
+	return mdiobus_read(gsw->host_bus, phy, reg);
+}
+
+static void mt7530_mii_write(struct gsw_mt753x *gsw, int phy, int reg, u16 val)
+{
+	if (phy < MT753X_NUM_PHYS)
+		phy = (gsw->phy_base + phy) & MT753X_SMI_ADDR_MASK;
+
+	mdiobus_write(gsw->host_bus, phy, reg, val);
+}
+
+static int mt7530_mmd_read(struct gsw_mt753x *gsw, int addr, int devad, u16 reg)
+{
+	u16 val;
+
+	if (addr < MT753X_NUM_PHYS)
+		addr = (gsw->phy_base + addr) & MT753X_SMI_ADDR_MASK;
+
+	mutex_lock(&gsw->host_bus->mdio_lock);
+
+	gsw->host_bus->write(gsw->host_bus, addr, MII_MMD_ACC_CTL_REG,
+			     (MMD_ADDR << MMD_CMD_S) |
+			     ((devad << MMD_DEVAD_S) & MMD_DEVAD_M));
+
+	gsw->host_bus->write(gsw->host_bus, addr, MII_MMD_ADDR_DATA_REG, reg);
+
+	gsw->host_bus->write(gsw->host_bus, addr, MII_MMD_ACC_CTL_REG,
+			     (MMD_DATA << MMD_CMD_S) |
+			     ((devad << MMD_DEVAD_S) & MMD_DEVAD_M));
+
+	val = gsw->host_bus->read(gsw->host_bus, addr, MII_MMD_ADDR_DATA_REG);
+
+	mutex_unlock(&gsw->host_bus->mdio_lock);
+
+	return val;
+}
+
+static void mt7530_mmd_write(struct gsw_mt753x *gsw, int addr, int devad,
+			     u16 reg, u16 val)
+{
+	if (addr < MT753X_NUM_PHYS)
+		addr = (gsw->phy_base + addr) & MT753X_SMI_ADDR_MASK;
+
+	mutex_lock(&gsw->host_bus->mdio_lock);
+
+	gsw->host_bus->write(gsw->host_bus, addr, MII_MMD_ACC_CTL_REG,
+		      (MMD_ADDR << MMD_CMD_S) |
+		      ((devad << MMD_DEVAD_S) & MMD_DEVAD_M));
+
+	gsw->host_bus->write(gsw->host_bus, addr, MII_MMD_ADDR_DATA_REG, reg);
+
+	gsw->host_bus->write(gsw->host_bus, addr, MII_MMD_ACC_CTL_REG,
+		      (MMD_DATA << MMD_CMD_S) |
+		      ((devad << MMD_DEVAD_S) & MMD_DEVAD_M));
+
+	gsw->host_bus->write(gsw->host_bus, addr, MII_MMD_ADDR_DATA_REG, val);
+
+	mutex_unlock(&gsw->host_bus->mdio_lock);
+}
+
+static void mt7530_core_reg_write(struct gsw_mt753x *gsw, u32 reg, u32 val)
+{
+	gsw->mmd_write(gsw, 0, 0x1f, reg, val);
+}
+
+static void mt7530_trgmii_setting(struct gsw_mt753x *gsw)
+{
+	u16 i;
+
+	mt7530_core_reg_write(gsw, CORE_PLL_GROUP5, 0x0780);
+	mdelay(1);
+	mt7530_core_reg_write(gsw, CORE_PLL_GROUP6, 0);
+	mt7530_core_reg_write(gsw, CORE_PLL_GROUP10, 0x87);
+	mdelay(1);
+	mt7530_core_reg_write(gsw, CORE_PLL_GROUP11, 0x87);
+
+	/* PLL BIAS enable */
+	mt7530_core_reg_write(gsw, CORE_PLL_GROUP4,
+			      RG_SYSPLL_DDSFBK_EN | RG_SYSPLL_BIAS_EN);
+	mdelay(1);
+
+	/* PLL LPF enable */
+	mt7530_core_reg_write(gsw, CORE_PLL_GROUP4,
+			      RG_SYSPLL_DDSFBK_EN |
+			      RG_SYSPLL_BIAS_EN | RG_SYSPLL_BIAS_LPF_EN);
+
+	/* sys PLL enable */
+	mt7530_core_reg_write(gsw, CORE_PLL_GROUP2,
+			      RG_SYSPLL_EN_NORMAL | RG_SYSPLL_VODEN |
+			      (1 << RG_SYSPLL_POSDIV_S));
+
+	/* LCDDDS PWDS */
+	mt7530_core_reg_write(gsw, CORE_PLL_GROUP7,
+			      (3 << RG_LCCDS_C_S) |
+			      RG_LCDDS_PWDB | RG_LCDDS_ISO_EN);
+	mdelay(1);
+
+	/* Enable MT7530 TRGMII clock */
+	mt7530_core_reg_write(gsw, TRGMII_GSW_CLK_CG, GSWCK_EN | TRGMIICK_EN);
+
+	/* lower Tx Driving */
+	for (i = 0 ; i < NUM_TRGMII_ODT; i++)
+		mt753x_reg_write(gsw, TRGMII_TD_ODT(i),
+				 (4 << TX_DM_DRVP_S) | (4 << TX_DM_DRVN_S));
+}
+
+static void mt7530_rgmii_setting(struct gsw_mt753x *gsw)
+{
+	u32 val;
+
+	mt7530_core_reg_write(gsw, CORE_PLL_GROUP5, 0x0c80);
+	mdelay(1);
+	mt7530_core_reg_write(gsw, CORE_PLL_GROUP6, 0);
+	mt7530_core_reg_write(gsw, CORE_PLL_GROUP10, 0x87);
+	mdelay(1);
+	mt7530_core_reg_write(gsw, CORE_PLL_GROUP11, 0x87);
+
+	val = mt753x_reg_read(gsw, TRGMII_TXCTRL);
+	val &= ~TXC_INV;
+	mt753x_reg_write(gsw, TRGMII_TXCTRL, val);
+
+	mt753x_reg_write(gsw, TRGMII_TCK_CTRL,
+			 (8 << TX_TAP_S) | (0x55 << TX_TRAIN_WD_S));
+}
+
+static int mt7530_mac_port_setup(struct gsw_mt753x *gsw)
+{
+	u32 hwstrap, p6ecr = 0, p5mcr, p6mcr, phyad;
+
+	hwstrap = mt753x_reg_read(gsw, MHWSTRAP);
+	hwstrap &= ~(P6_INTF_DIS | P5_INTF_MODE_RGMII | P5_INTF_DIS_S);
+	hwstrap |= P5_INTF_SEL_GMAC5;
+	if (!gsw->port5_cfg.enabled) {
+		p5mcr = FORCE_MODE;
+		hwstrap |= P5_INTF_DIS_S;
+	} else {
+		p5mcr = (IPG_96BIT_WITH_SHORT_IPG << IPG_CFG_S) |
+			MAC_MODE | MAC_TX_EN | MAC_RX_EN |
+			BKOFF_EN | BACKPR_EN;
+
+		if (gsw->port5_cfg.force_link) {
+			p5mcr |= FORCE_MODE | FORCE_LINK | FORCE_RX_FC |
+				 FORCE_TX_FC;
+			p5mcr |= gsw->port5_cfg.speed << FORCE_SPD_S;
+
+			if (gsw->port5_cfg.duplex)
+				p5mcr |= FORCE_DPX;
+		}
+
+		switch (gsw->port5_cfg.phy_mode) {
+		case PHY_INTERFACE_MODE_MII:
+		case PHY_INTERFACE_MODE_GMII:
+			break;
+		case PHY_INTERFACE_MODE_RGMII:
+			hwstrap |= P5_INTF_MODE_RGMII;
+			break;
+		default:
+			dev_info(gsw->dev, "%s is not supported by port5\n",
+				 phy_modes(gsw->port5_cfg.phy_mode));
+			p5mcr = FORCE_MODE;
+			hwstrap |= P5_INTF_DIS_S;
+		}
+
+		/* Port5 to PHY direct mode */
+		if (of_property_read_u32(gsw->port5_cfg.np, "phy-address",
+					 &phyad))
+			goto parse_p6;
+
+		if (phyad != 0 && phyad != 4) {
+			dev_info(gsw->dev,
+				 "Only PHY 0/4 can be connected to Port 5\n");
+			goto parse_p6;
+		}
+
+		hwstrap &= ~P5_INTF_SEL_GMAC5;
+		if (phyad == 0)
+			hwstrap |= P5_PHY0_SEL;
+		else
+			hwstrap &= ~P5_PHY0_SEL;
+	}
+
+parse_p6:
+	if (!gsw->port6_cfg.enabled) {
+		p6mcr = FORCE_MODE;
+		hwstrap |= P6_INTF_DIS;
+	} else {
+		p6mcr = (IPG_96BIT_WITH_SHORT_IPG << IPG_CFG_S) |
+			MAC_MODE | MAC_TX_EN | MAC_RX_EN |
+			BKOFF_EN | BACKPR_EN;
+
+		if (gsw->port6_cfg.force_link) {
+			p6mcr |= FORCE_MODE | FORCE_LINK | FORCE_RX_FC |
+				 FORCE_TX_FC;
+			p6mcr |= gsw->port6_cfg.speed << FORCE_SPD_S;
+
+			if (gsw->port6_cfg.duplex)
+				p6mcr |= FORCE_DPX;
+		}
+
+		switch (gsw->port6_cfg.phy_mode) {
+		case PHY_INTERFACE_MODE_RGMII:
+			p6ecr = BIT(1);
+			break;
+		case PHY_INTERFACE_MODE_TRGMII:
+			/* set MT7530 central align */
+			p6ecr = BIT(0);
+			break;
+		default:
+			dev_info(gsw->dev, "%s is not supported by port6\n",
+				 phy_modes(gsw->port6_cfg.phy_mode));
+			p6mcr = FORCE_MODE;
+			hwstrap |= P6_INTF_DIS;
+		}
+	}
+
+	mt753x_reg_write(gsw, MHWSTRAP, hwstrap);
+	mt753x_reg_write(gsw, P6ECR, p6ecr);
+
+	mt753x_reg_write(gsw, PMCR(5), p5mcr);
+	mt753x_reg_write(gsw, PMCR(6), p6mcr);
+
+	return 0;
+}
+
+static void mt7530_core_pll_setup(struct gsw_mt753x *gsw)
+{
+	u32 hwstrap;
+
+	hwstrap = mt753x_reg_read(gsw, HWSTRAP);
+
+	switch ((hwstrap & XTAL_FSEL_M) >> XTAL_FSEL_S) {
+	case XTAL_40MHZ:
+		/* Disable MT7530 core clock */
+		mt7530_core_reg_write(gsw, TRGMII_GSW_CLK_CG, 0);
+
+		/* disable MT7530 PLL */
+		mt7530_core_reg_write(gsw, CORE_GSWPLL_GCR_1,
+				      (2 << GSWPLL_POSTDIV_200M_S) |
+				      (32 << GSWPLL_FBKDIV_200M_S));
+
+		/* For MT7530 core clock = 500Mhz */
+		mt7530_core_reg_write(gsw, CORE_GSWPLL_GCR_2,
+				      (1 << GSWPLL_POSTDIV_500M_S) |
+				      (25 << GSWPLL_FBKDIV_500M_S));
+
+		/* Enable MT7530 PLL */
+		mt7530_core_reg_write(gsw, CORE_GSWPLL_GCR_1,
+				      (2 << GSWPLL_POSTDIV_200M_S) |
+				      (32 << GSWPLL_FBKDIV_200M_S) |
+				      GSWPLL_EN_PRE);
+
+		usleep_range(20, 40);
+
+		/* Enable MT7530 core clock */
+		mt7530_core_reg_write(gsw, TRGMII_GSW_CLK_CG, GSWCK_EN);
+		break;
+	default:
+		/* TODO: PLL settings for 20/25MHz */
+		break;
+	}
+
+	hwstrap = mt753x_reg_read(gsw, HWSTRAP);
+	hwstrap |= CHG_TRAP;
+	if (gsw->direct_phy_access)
+		hwstrap &= ~C_MDIO_BPS_S;
+	else
+		hwstrap |= C_MDIO_BPS_S;
+
+	mt753x_reg_write(gsw, MHWSTRAP, hwstrap);
+
+	if (gsw->port6_cfg.enabled &&
+	    gsw->port6_cfg.phy_mode == PHY_INTERFACE_MODE_TRGMII) {
+		mt7530_trgmii_setting(gsw);
+	} else {
+		/* RGMII */
+		mt7530_rgmii_setting(gsw);
+	}
+
+	/* delay setting for 10/1000M */
+	mt753x_reg_write(gsw, P5RGMIIRXCR,
+			 CSR_RGMII_EDGE_ALIGN |
+			 (2 << CSR_RGMII_RXC_0DEG_CFG_S));
+	mt753x_reg_write(gsw, P5RGMIITXCR, 0x14 << CSR_RGMII_TXC_CFG_S);
+}
+
+static int mt7530_sw_detect(struct gsw_mt753x *gsw, struct chip_rev *crev)
+{
+	u32 rev;
+
+	rev = mt753x_reg_read(gsw, CHIP_REV);
+
+	if (((rev & CHIP_NAME_M) >> CHIP_NAME_S) == MT7530) {
+		if (crev) {
+			crev->rev = rev & CHIP_REV_M;
+			crev->name = "MT7530";
+		}
+
+		return 0;
+	}
+
+	return -ENODEV;
+}
+
+static void mt7530_phy_setting(struct gsw_mt753x *gsw)
+{
+	int i;
+	u32 val;
+
+	for (i = 0; i < MT753X_NUM_PHYS; i++) {
+		/* Disable EEE */
+		gsw->mmd_write(gsw, i, PHY_DEV07, PHY_DEV07_REG_03C, 0);
+
+		/* Enable HW auto downshift */
+		gsw->mii_write(gsw, i, 0x1f, 0x1);
+		val = gsw->mii_read(gsw, i, PHY_EXT_REG_14);
+		val |= PHY_EN_DOWN_SHFIT;
+		gsw->mii_write(gsw, i, PHY_EXT_REG_14, val);
+
+		/* Increase SlvDPSready time */
+		gsw->mii_write(gsw, i, 0x1f, 0x52b5);
+		gsw->mii_write(gsw, i, PHY_TR_REG_10, 0xafae);
+		gsw->mii_write(gsw, i, PHY_TR_REG_12, 0x2f);
+		gsw->mii_write(gsw, i, PHY_TR_REG_10, 0x8fae);
+
+		/* Increase post_update_timer */
+		gsw->mii_write(gsw, i, 0x1f, 0x3);
+		gsw->mii_write(gsw, i, PHY_LPI_REG_11, 0x4b);
+		gsw->mii_write(gsw, i, 0x1f, 0);
+
+		/* Adjust 100_mse_threshold */
+		gsw->mmd_write(gsw, i, PHY_DEV1E, PHY_DEV1E_REG_123, 0xffff);
+
+		/* Disable mcc */
+		gsw->mmd_write(gsw, i, PHY_DEV1E, PHY_DEV1E_REG_A6, 0x300);
+	}
+}
+
+static inline bool get_phy_access_mode(const struct device_node *np)
+{
+	return of_property_read_bool(np, "mt7530,direct-phy-access");
+}
+
+static int mt7530_sw_init(struct gsw_mt753x *gsw)
+{
+	int i;
+	u32 val;
+
+	gsw->direct_phy_access = get_phy_access_mode(gsw->dev->of_node);
+
+	/* Force MT7530 to use (in)direct PHY access */
+	val = mt753x_reg_read(gsw, HWSTRAP);
+	val |= CHG_TRAP;
+	if (gsw->direct_phy_access)
+		val &= ~C_MDIO_BPS_S;
+	else
+		val |= C_MDIO_BPS_S;
+	mt753x_reg_write(gsw, MHWSTRAP, val);
+
+	/* Read PHY address base from HWSTRAP */
+	gsw->phy_base  = (((val & SMI_ADDR_M) >> SMI_ADDR_S) << 3) + 8;
+	gsw->phy_base &= MT753X_SMI_ADDR_MASK;
+
+	if (gsw->direct_phy_access) {
+		gsw->mii_read = mt7530_mii_read;
+		gsw->mii_write = mt7530_mii_write;
+		gsw->mmd_read = mt7530_mmd_read;
+		gsw->mmd_write = mt7530_mmd_write;
+	} else {
+		gsw->mii_read = mt753x_mii_read;
+		gsw->mii_write = mt753x_mii_write;
+		gsw->mmd_read = mt753x_mmd_ind_read;
+		gsw->mmd_write = mt753x_mmd_ind_write;
+	}
+
+	for (i = 0; i < MT753X_NUM_PHYS; i++) {
+		val = gsw->mii_read(gsw, i, MII_BMCR);
+		val |= BMCR_PDOWN;
+		gsw->mii_write(gsw, i, MII_BMCR, val);
+	}
+
+	/* Force MAC link down before reset */
+	mt753x_reg_write(gsw, PMCR(5), FORCE_MODE);
+	mt753x_reg_write(gsw, PMCR(6), FORCE_MODE);
+
+	/* Switch soft reset */
+	/* BUG: sw reset causes gsw int flooding */
+	mt753x_reg_write(gsw, SYS_CTRL, SW_PHY_RST | SW_SYS_RST | SW_REG_RST);
+	usleep_range(10, 20);
+
+	/* global mac control settings configuration */
+	mt753x_reg_write(gsw, GMACCR,
+			 LATE_COL_DROP | (15 << MTCC_LMT_S) |
+			 (2 << MAX_RX_JUMBO_S) | RX_PKT_LEN_MAX_JUMBO);
+
+	/* Output INTR selected */
+	val = mt753x_reg_read(gsw, TOP_SIG_CTRL);
+	val &= ~OUTPUT_INTR_M;
+	val |= (3 << OUTPUT_INTR_S);
+	mt753x_reg_write(gsw, TOP_SIG_CTRL, val);
+
+	mt7530_core_pll_setup(gsw);
+	mt7530_mac_port_setup(gsw);
+
+	return 0;
+}
+
+static int mt7530_sw_post_init(struct gsw_mt753x *gsw)
+{
+	int i;
+	u32 val;
+
+	mt7530_phy_setting(gsw);
+
+	for (i = 0; i < MT753X_NUM_PHYS; i++) {
+		val = gsw->mii_read(gsw, i, MII_BMCR);
+		val &= ~BMCR_PDOWN;
+		gsw->mii_write(gsw, i, MII_BMCR, val);
+	}
+
+	return 0;
+}
+
+struct mt753x_sw_id mt7530_id = {
+	.model = MT7530,
+	.detect = mt7530_sw_detect,
+	.init = mt7530_sw_init,
+	.post_init = mt7530_sw_post_init
+};
diff --git a/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt7531.c b/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt7531.c
new file mode 100755
index 0000000..7253042
--- /dev/null
+++ b/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt7531.c
@@ -0,0 +1,1058 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2018 MediaTek Inc.
+ * Author: Zhanguo Ju <zhanguo.ju@mediatek.com>
+ */
+
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/hrtimer.h>
+
+#include "mt753x.h"
+#include "mt753x_regs.h"
+
+/* MT7531 registers */
+#define SGMII_REG_BASE			0x5000
+#define SGMII_REG_PORT_BASE		0x1000
+#define SGMII_REG(p, r)			(SGMII_REG_BASE + \
+					(p) * SGMII_REG_PORT_BASE + (r))
+#define PCS_CONTROL_1(p)		SGMII_REG(p, 0x00)
+#define SGMII_MODE(p)			SGMII_REG(p, 0x20)
+#define QPHY_PWR_STATE_CTRL(p)		SGMII_REG(p, 0xe8)
+#define ANA_CKBG(p)			SGMII_REG(p, 0x100)
+#define ANA_DA_FORCE_MODE1(p)		SGMII_REG(p, 0x110)
+#define PHYA_CTRL_SIGNAL3(p)		SGMII_REG(p, 0x128)
+#define PHYA_ANA_SYSPLL(p)		SGMII_REG(p, 0x158)
+
+/* Fields of PCS_CONTROL_1 */
+#define SGMII_LINK_STATUS		BIT(18)
+#define SGMII_AN_ENABLE			BIT(12)
+#define SGMII_AN_RESTART		BIT(9)
+
+/* Fields of SGMII_MODE */
+#define SGMII_REMOTE_FAULT_DIS		BIT(8)
+#define SGMII_IF_MODE_FORCE_DUPLEX	BIT(4)
+#define SGMII_IF_MODE_FORCE_SPEED_S	0x2
+#define SGMII_IF_MODE_FORCE_SPEED_M	0x0c
+#define SGMII_IF_MODE_ADVERT_AN		BIT(1)
+
+/* Values of SGMII_IF_MODE_FORCE_SPEED */
+#define SGMII_IF_MODE_FORCE_SPEED_10	0
+#define SGMII_IF_MODE_FORCE_SPEED_100	1
+#define SGMII_IF_MODE_FORCE_SPEED_1000	2
+
+/* Fields of QPHY_PWR_STATE_CTRL */
+#define PHYA_PWD			BIT(4)
+
+/* Fields of ANA_CKBG */
+#define SSUSB_PLL_SSC_EN		BIT(21)
+
+/* Fields of ANA_DA_FORCE_MODE1 */
+#define FORCE_PLL_SSC_EN		BIT(30)
+
+/* Fields of PHYA_CTRL_SIGNAL3 */
+#define RG_TPHY_SPEED_S			2
+#define RG_TPHY_SPEED_M			0x0c
+
+/* Values of RG_TPHY_SPEED */
+#define RG_TPHY_SPEED_1000		0
+#define RG_TPHY_SPEED_2500		1
+
+/* Fields of PHYA_ANA_SYSPLL */
+#define RG_VUSB10_ON			BIT(29)
+
+/* Unique fields of (M)HWSTRAP for MT7531 */
+#define XTAL_FSEL_S			7
+#define XTAL_FSEL_M			BIT(7)
+#define PHY_EN				BIT(6)
+#define CHG_STRAP			BIT(8)
+
+/* Efuse Register Define */
+#define GBE_EFUSE			0x7bc8
+#define GBE_SEL_EFUSE_EN		BIT(0)
+
+/* PHY ENABLE Register bitmap define */
+#define PHY_DEV1F			0x1f
+#define PHY_DEV1F_REG_44		0x44
+#define PHY_DEV1F_REG_104		0x104
+#define PHY_DEV1F_REG_10A		0x10a
+#define PHY_DEV1F_REG_10B		0x10b
+#define PHY_DEV1F_REG_10C		0x10c
+#define PHY_DEV1F_REG_10D		0x10d
+#define PHY_DEV1F_REG_268		0x268
+#define PHY_DEV1F_REG_269		0x269
+#define PHY_DEV1F_REG_26A		0x26A
+#define PHY_DEV1F_REG_403		0x403
+
+/* Fields of PHY_DEV1F_REG_403 */
+#define GBE_EFUSE_SETTING		BIT(3)
+#define PHY_EN_BYPASS_MODE		BIT(4)
+#define POWER_ON_OFF			BIT(5)
+#define PHY_PLL_M			GENMASK(9, 8)
+#define PHY_PLL_SEL(x)			(((x) << 8) & GENMASK(9, 8))
+
+/* PHY EEE Register bitmap of define */
+#define PHY_DEV07			0x07
+#define PHY_DEV07_REG_03C		0x3c
+
+/* PHY Extend Register 0x14 bitmap of define */
+#define PHY_EXT_REG_14			0x14
+
+/* Fields of PHY_EXT_REG_14 */
+#define PHY_EN_DOWN_SHFIT		BIT(4)
+
+/* PHY Extend Register 0x17 bitmap of define */
+#define PHY_EXT_REG_17			0x17
+
+/* Fields of PHY_EXT_REG_17 */
+#define PHY_LINKDOWN_POWER_SAVING_EN	BIT(4)
+
+/* PHY PMA Register 0x17 bitmap of define */
+#define SLV_DSP_READY_TIME_S		15
+#define SLV_DSP_READY_TIME_M		(0xff << SLV_DSP_READY_TIME_S)
+
+/* PHY PMA Register 0x18 bitmap of define */
+#define ENABLE_RANDOM_UPDATE_TRIGGER	BIT(8)
+
+/* PHY DEV 0x1e Register bitmap of define */
+#define PHY_DEV1E			0x1e
+#define PHY_TX_MLT3_BASE		0x0
+#define PHY_DEV1E_REG_13		0x13
+#define PHY_DEV1E_REG_14		0x14
+#define PHY_DEV1E_REG_41		0x41
+#define PHY_DEV1E_REG_A6		0xa6
+#define PHY_DEV1E_REG_0C6		0x0c6
+#define PHY_DEV1E_REG_0FE		0x0fe
+#define PHY_DEV1E_REG_123		0x123
+#define PHY_DEV1E_REG_141		0x141
+#define PHY_DEV1E_REG_189		0x189
+#define PHY_DEV1E_REG_234		0x234
+
+/* Fields of PHY_DEV1E_REG_0C6 */
+#define PHY_POWER_SAVING_S		8
+#define PHY_POWER_SAVING_M		0x300
+#define PHY_POWER_SAVING_TX		0x0
+
+/* Fields of PHY_DEV1E_REG_189 */
+#define DESCRAMBLER_CLEAR_EN		0x1
+
+/* Fields of PHY_DEV1E_REG_234 */
+#define TR_OPEN_LOOP_EN			BIT(0)
+
+/* Port debug count register */
+#define DBG_CNT_BASE			0x3018
+#define DBG_CNT_PORT_BASE		0x100
+#define DBG_CNT(p)			(DBG_CNT_BASE + \
+					(p) * DBG_CNT_PORT_BASE)
+#define DIS_CLR				BIT(31)
+
+/* Values of XTAL_FSEL_S */
+#define XTAL_40MHZ			0
+#define XTAL_25MHZ			1
+
+#define PLLGP_EN			0x7820
+#define EN_COREPLL			BIT(2)
+#define SW_CLKSW			BIT(1)
+#define SW_PLLGP			BIT(0)
+
+#define PLLGP_CR0			0x78a8
+#define RG_COREPLL_EN			BIT(22)
+#define RG_COREPLL_POSDIV_S		23
+#define RG_COREPLL_POSDIV_M		0x3800000
+#define RG_COREPLL_SDM_PCW_S		1
+#define RG_COREPLL_SDM_PCW_M		0x3ffffe
+#define RG_COREPLL_SDM_PCW_CHG		BIT(0)
+
+/* TOP Signals Status Register */
+#define TOP_SIG_SR			0x780c
+#define PAD_MCM_SMI_EN			BIT(0)
+#define PAD_DUAL_SGMII_EN		BIT(1)
+
+/* RGMII and SGMII PLL clock */
+#define ANA_PLLGP_CR2			0x78b0
+#define ANA_PLLGP_CR5			0x78bc
+
+/* GPIO mode define */
+#define GPIO_MODE_REGS(x)		(0x7c0c + (((x) / 8) * 4))
+#define GPIO_MODE_S			4
+
+/* GPIO GROUP IOLB SMT0 Control */
+#define SMT0_IOLB			0x7f04
+#define SMT_IOLB_5_SMI_MDC_EN		BIT(5)
+
+/* Unique fields of PMCR for MT7531 */
+#define FORCE_MODE_EEE1G		BIT(25)
+#define FORCE_MODE_EEE100		BIT(26)
+#define FORCE_MODE_TX_FC		BIT(27)
+#define FORCE_MODE_RX_FC		BIT(28)
+#define FORCE_MODE_DPX			BIT(29)
+#define FORCE_MODE_SPD			BIT(30)
+#define FORCE_MODE_LNK			BIT(31)
+#define FORCE_MODE			BIT(15)
+
+#define CHIP_REV			0x781C
+#define CHIP_NAME_S			16
+#define CHIP_NAME_M			0xffff0000
+#define CHIP_REV_S			0
+#define CHIP_REV_M			0x0f
+#define CHIP_REV_E1			0x0
+
+#define CLKGEN_CTRL			0x7500
+#define CLK_SKEW_OUT_S			8
+#define CLK_SKEW_OUT_M			0x300
+#define CLK_SKEW_IN_S			6
+#define CLK_SKEW_IN_M			0xc0
+#define RXCLK_NO_DELAY			BIT(5)
+#define TXCLK_NO_REVERSE		BIT(4)
+#define GP_MODE_S			1
+#define GP_MODE_M			0x06
+#define GP_CLK_EN			BIT(0)
+
+#define CPGC_CTRL			0xB0
+#define COL_EN				BIT(0)
+#define COL_CLK_EN			BIT(1)
+#define COL_RST_N			BIT(2)
+#define COL_BUSY			BIT(3)
+
+/* Values of GP_MODE */
+#define GP_MODE_RGMII			0
+#define GP_MODE_MII			1
+#define GP_MODE_REV_MII			2
+
+/* Values of CLK_SKEW_IN */
+#define CLK_SKEW_IN_NO_CHANGE		0
+#define CLK_SKEW_IN_DELAY_100PPS	1
+#define CLK_SKEW_IN_DELAY_200PPS	2
+#define CLK_SKEW_IN_REVERSE		3
+
+/* Values of CLK_SKEW_OUT */
+#define CLK_SKEW_OUT_NO_CHANGE		0
+#define CLK_SKEW_OUT_DELAY_100PPS	1
+#define CLK_SKEW_OUT_DELAY_200PPS	2
+#define CLK_SKEW_OUT_REVERSE		3
+
+/* Proprietory Control Register of Internal Phy device 0x1e */
+#define RXADC_CONTROL_3			0xc2
+#define RXADC_LDO_CONTROL_2		0xd3
+
+/* Proprietory Control Register of Internal Phy device 0x1f */
+#define TXVLD_DA_271			0x271
+#define TXVLD_DA_272			0x272
+#define TXVLD_DA_273			0x273
+
+/* gpio pinmux pins and functions define */
+static int gpio_int_pins[] = {0};
+static int gpio_int_funcs[] = {1};
+static int gpio_mdc_pins[] = {11, 20};
+static int gpio_mdc_funcs[] = {2, 2};
+static int gpio_mdio_pins[] = {12, 21};
+static int gpio_mdio_funcs[] = {2, 2};
+
+static int mt7531_set_port_sgmii_force_mode(struct gsw_mt753x *gsw, u32 port,
+					    struct mt753x_port_cfg *port_cfg)
+{
+	u32 speed, port_base, val;
+	ktime_t timeout;
+	u32 timeout_us;
+
+	if (port < 5 || port >= MT753X_NUM_PORTS) {
+		dev_info(gsw->dev, "port %d is not a SGMII port\n", port);
+		return -EINVAL;
+	}
+
+	port_base = port - 5;
+
+	switch (port_cfg->speed) {
+	case MAC_SPD_1000:
+		speed = RG_TPHY_SPEED_1000;
+		break;
+	case MAC_SPD_2500:
+		speed = RG_TPHY_SPEED_2500;
+		break;
+	default:
+		dev_info(gsw->dev, "invalid SGMII speed idx %d for port %d\n",
+			 port_cfg->speed, port);
+
+		speed = RG_TPHY_SPEED_1000;
+	}
+
+	/* Step 1: Speed select register setting */
+	val = mt753x_reg_read(gsw, PHYA_CTRL_SIGNAL3(port_base));
+	val &= ~RG_TPHY_SPEED_M;
+	val |= speed << RG_TPHY_SPEED_S;
+	mt753x_reg_write(gsw, PHYA_CTRL_SIGNAL3(port_base), val);
+
+	/* Step 2 : Disable AN */
+	val = mt753x_reg_read(gsw, PCS_CONTROL_1(port_base));
+	val &= ~SGMII_AN_ENABLE;
+	mt753x_reg_write(gsw, PCS_CONTROL_1(port_base), val);
+
+	/* Step 3: SGMII force mode setting */
+	val = mt753x_reg_read(gsw, SGMII_MODE(port_base));
+	val &= ~SGMII_IF_MODE_ADVERT_AN;
+	val &= ~SGMII_IF_MODE_FORCE_SPEED_M;
+	val |= SGMII_IF_MODE_FORCE_SPEED_1000 << SGMII_IF_MODE_FORCE_SPEED_S;
+	val |= SGMII_IF_MODE_FORCE_DUPLEX;
+	/* For sgmii force mode, 0 is full duplex and 1 is half duplex */
+	if (port_cfg->duplex)
+		val &= ~SGMII_IF_MODE_FORCE_DUPLEX;
+
+	mt753x_reg_write(gsw, SGMII_MODE(port_base), val);
+
+	/* Step 4: XXX: Disable Link partner's AN and set force mode */
+
+	/* Step 5: XXX: Special setting for PHYA ==> reserved for flexible */
+
+	/* Step 6 : Release PHYA power down state */
+	val = mt753x_reg_read(gsw, QPHY_PWR_STATE_CTRL(port_base));
+	val &= ~PHYA_PWD;
+	mt753x_reg_write(gsw, QPHY_PWR_STATE_CTRL(port_base), val);
+
+	/* Step 7 : Polling SGMII_LINK_STATUS */
+	timeout_us = 2000000;
+	timeout = ktime_add_us(ktime_get(), timeout_us);
+	while (1) {
+		val = mt753x_reg_read(gsw, PCS_CONTROL_1(port_base));
+		val &= SGMII_LINK_STATUS;
+
+		if (val)
+			break;
+
+		if (ktime_compare(ktime_get(), timeout) > 0)
+			return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int mt7531_set_port_sgmii_an_mode(struct gsw_mt753x *gsw, u32 port,
+					 struct mt753x_port_cfg *port_cfg)
+{
+	u32 speed, port_base, val;
+	ktime_t timeout;
+	u32 timeout_us;
+
+	if (port < 5 || port >= MT753X_NUM_PORTS) {
+		dev_info(gsw->dev, "port %d is not a SGMII port\n", port);
+		return -EINVAL;
+	}
+
+	port_base = port - 5;
+
+	switch (port_cfg->speed) {
+	case MAC_SPD_1000:
+		speed = RG_TPHY_SPEED_1000;
+		break;
+	case MAC_SPD_2500:
+		speed = RG_TPHY_SPEED_2500;
+		break;
+	default:
+		dev_info(gsw->dev, "invalid SGMII speed idx %d for port %d\n",
+			 port_cfg->speed, port);
+
+		speed = RG_TPHY_SPEED_1000;
+	}
+
+	/* Step 1: Speed select register setting */
+	val = mt753x_reg_read(gsw, PHYA_CTRL_SIGNAL3(port_base));
+	val &= ~RG_TPHY_SPEED_M;
+	val |= speed << RG_TPHY_SPEED_S;
+	mt753x_reg_write(gsw, PHYA_CTRL_SIGNAL3(port_base), val);
+
+	/* Step 2: Remote fault disable */
+	val = mt753x_reg_read(gsw, SGMII_MODE(port));
+	val |= SGMII_REMOTE_FAULT_DIS;
+	mt753x_reg_write(gsw, SGMII_MODE(port), val);
+
+	/* Step 3: Setting Link partner's AN enable = 1 */
+
+	/* Step 4: Setting Link partner's device ability for speed/duplex */
+
+	/* Step 5: AN re-start */
+	val = mt753x_reg_read(gsw, PCS_CONTROL_1(port));
+	val |= SGMII_AN_RESTART;
+	mt753x_reg_write(gsw, PCS_CONTROL_1(port), val);
+
+	/* Step 6: Special setting for PHYA ==> reserved for flexible */
+
+	/* Step 7 : Polling SGMII_LINK_STATUS */
+	timeout_us = 2000000;
+	timeout = ktime_add_us(ktime_get(), timeout_us);
+	while (1) {
+		val = mt753x_reg_read(gsw, PCS_CONTROL_1(port_base));
+		val &= SGMII_LINK_STATUS;
+
+		if (val)
+			break;
+
+		if (ktime_compare(ktime_get(), timeout) > 0)
+			return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static void mt7531_sgmii_ssc(struct gsw_mt753x *gsw, u32 port, int enable)
+{
+	u32 val;
+	u32 port_base = port - 5;
+
+	if (enable) {
+		val = mt753x_reg_read(gsw, ANA_CKBG(port_base));
+		val |= SSUSB_PLL_SSC_EN;
+		mt753x_reg_write(gsw, ANA_CKBG(port_base), val);
+
+		val = mt753x_reg_read(gsw, ANA_DA_FORCE_MODE1(port_base));
+		val |= FORCE_PLL_SSC_EN;
+		mt753x_reg_write(gsw, ANA_DA_FORCE_MODE1(port_base), val);
+	} else {
+		val = mt753x_reg_read(gsw, ANA_CKBG(port_base));
+		val &= ~SSUSB_PLL_SSC_EN;
+		mt753x_reg_write(gsw, ANA_CKBG(port_base), val);
+
+		val = mt753x_reg_read(gsw, ANA_DA_FORCE_MODE1(port_base));
+		val &= ~FORCE_PLL_SSC_EN;
+		mt753x_reg_write(gsw, ANA_DA_FORCE_MODE1(port_base), val);
+	}
+}
+
+static int mt7531_set_port_rgmii(struct gsw_mt753x *gsw, u32 port)
+{
+	u32 val;
+
+	if (port != 5) {
+		dev_info(gsw->dev, "RGMII mode is not available for port %d\n",
+			 port);
+		return -EINVAL;
+	}
+
+	val = mt753x_reg_read(gsw, CLKGEN_CTRL);
+	val |= GP_CLK_EN;
+	val &= ~GP_MODE_M;
+	val |= GP_MODE_RGMII << GP_MODE_S;
+	val |= TXCLK_NO_REVERSE;
+	val |= RXCLK_NO_DELAY;
+	val &= ~CLK_SKEW_IN_M;
+	val |= CLK_SKEW_IN_NO_CHANGE << CLK_SKEW_IN_S;
+	val &= ~CLK_SKEW_OUT_M;
+	val |= CLK_SKEW_OUT_NO_CHANGE << CLK_SKEW_OUT_S;
+	mt753x_reg_write(gsw, CLKGEN_CTRL, val);
+
+	return 0;
+}
+
+static int mt7531_mac_port_setup(struct gsw_mt753x *gsw, u32 port,
+				 struct mt753x_port_cfg *port_cfg)
+{
+	u32 pmcr;
+	u32 speed;
+
+	if (port < 5 || port >= MT753X_NUM_PORTS) {
+		dev_info(gsw->dev, "port %d is not a MAC port\n", port);
+		return -EINVAL;
+	}
+
+	if (port_cfg->enabled) {
+		pmcr = (IPG_96BIT_WITH_SHORT_IPG << IPG_CFG_S) |
+		       MAC_MODE | MAC_TX_EN | MAC_RX_EN |
+		       BKOFF_EN | BACKPR_EN;
+
+		if (port_cfg->force_link) {
+			/* PMCR's speed field 0x11 is reserved,
+			 * sw should set 0x10
+			 */
+			speed = port_cfg->speed;
+			if (port_cfg->speed == MAC_SPD_2500)
+				speed = MAC_SPD_1000;
+
+			pmcr |= FORCE_MODE_LNK | FORCE_LINK |
+				FORCE_MODE_SPD | FORCE_MODE_DPX |
+				FORCE_MODE_RX_FC | FORCE_MODE_TX_FC |
+				FORCE_RX_FC | FORCE_TX_FC |
+				(speed << FORCE_SPD_S);
+
+			if (port_cfg->duplex)
+				pmcr |= FORCE_DPX;
+		}
+	} else {
+		pmcr = FORCE_MODE_LNK;
+	}
+
+	switch (port_cfg->phy_mode) {
+	case PHY_INTERFACE_MODE_RGMII:
+		mt7531_set_port_rgmii(gsw, port);
+		break;
+	case PHY_INTERFACE_MODE_SGMII:
+		if (port_cfg->force_link)
+			mt7531_set_port_sgmii_force_mode(gsw, port, port_cfg);
+		else
+			mt7531_set_port_sgmii_an_mode(gsw, port, port_cfg);
+
+		mt7531_sgmii_ssc(gsw, port, port_cfg->ssc_on);
+		break;
+	default:
+		if (port_cfg->enabled)
+			dev_info(gsw->dev, "%s is not supported by port %d\n",
+				 phy_modes(port_cfg->phy_mode), port);
+
+		pmcr = FORCE_MODE_LNK;
+	}
+
+	mt753x_reg_write(gsw, PMCR(port), pmcr);
+
+	return 0;
+}
+
+static void mt7531_core_pll_setup(struct gsw_mt753x *gsw)
+{
+	u32 val;
+	u32 top_sig;
+	u32 hwstrap;
+	u32 xtal;
+
+	val = mt753x_reg_read(gsw, CHIP_REV);
+	top_sig = mt753x_reg_read(gsw, TOP_SIG_SR);
+	hwstrap = mt753x_reg_read(gsw, HWSTRAP);
+	if ((val & CHIP_REV_M) > 0)
+		xtal = (top_sig & PAD_MCM_SMI_EN) ? XTAL_40MHZ : XTAL_25MHZ;
+	else
+		xtal = (hwstrap & XTAL_FSEL_M) >> XTAL_FSEL_S;
+
+	/* dump HW strap and XTAL */
+	dev_info(gsw->dev, "HWSTRAP=0x%x XTAL=%dMHz\n", hwstrap,
+		 (xtal == XTAL_25MHZ) ? 25 : 40);
+
+	/* Only BE needs additional setting */
+	if (top_sig & PAD_DUAL_SGMII_EN)
+		return;
+
+	/* Disable Port5 SGMII clearly */
+	val = mt753x_reg_read(gsw, PHYA_ANA_SYSPLL(0));
+	val &= ~RG_VUSB10_ON;
+	mt753x_reg_write(gsw, PHYA_ANA_SYSPLL(0), val);
+
+	switch (xtal) {
+	case XTAL_25MHZ:
+		/* Step 1 : Disable MT7531 COREPLL */
+		val = mt753x_reg_read(gsw, PLLGP_EN);
+		val &= ~EN_COREPLL;
+		mt753x_reg_write(gsw, PLLGP_EN, val);
+
+		/* Step 2: switch to XTAL output */
+		val = mt753x_reg_read(gsw, PLLGP_EN);
+		val |= SW_CLKSW;
+		mt753x_reg_write(gsw, PLLGP_EN, val);
+
+		val = mt753x_reg_read(gsw, PLLGP_CR0);
+		val &= ~RG_COREPLL_EN;
+		mt753x_reg_write(gsw, PLLGP_CR0, val);
+
+		/* Step 3: disable PLLGP and enable program PLLGP */
+		val = mt753x_reg_read(gsw, PLLGP_EN);
+		val |= SW_PLLGP;
+		mt753x_reg_write(gsw, PLLGP_EN, val);
+
+		/* Step 4: program COREPLL output frequency to 500MHz */
+		val = mt753x_reg_read(gsw, PLLGP_CR0);
+		val &= ~RG_COREPLL_POSDIV_M;
+		val |= 2 << RG_COREPLL_POSDIV_S;
+		mt753x_reg_write(gsw, PLLGP_CR0, val);
+		usleep_range(25, 35);
+
+		val = mt753x_reg_read(gsw, PLLGP_CR0);
+		val &= ~RG_COREPLL_SDM_PCW_M;
+		val |= 0x140000 << RG_COREPLL_SDM_PCW_S;
+		mt753x_reg_write(gsw, PLLGP_CR0, val);
+
+		/* Set feedback divide ratio update signal to high */
+		val = mt753x_reg_read(gsw, PLLGP_CR0);
+		val |= RG_COREPLL_SDM_PCW_CHG;
+		mt753x_reg_write(gsw, PLLGP_CR0, val);
+		/* Wait for at least 16 XTAL clocks */
+		usleep_range(10, 20);
+
+		/* Step 5: set feedback divide ratio update signal to low */
+		val = mt753x_reg_read(gsw, PLLGP_CR0);
+		val &= ~RG_COREPLL_SDM_PCW_CHG;
+		mt753x_reg_write(gsw, PLLGP_CR0, val);
+
+		/* Enable 325M clock for SGMII */
+		mt753x_reg_write(gsw, ANA_PLLGP_CR5, 0xad0000);
+
+		/* Enable 250SSC clock for RGMII */
+		mt753x_reg_write(gsw, ANA_PLLGP_CR2, 0x4f40000);
+
+		/* Step 6: Enable MT7531 PLL */
+		val = mt753x_reg_read(gsw, PLLGP_CR0);
+		val |= RG_COREPLL_EN;
+		mt753x_reg_write(gsw, PLLGP_CR0, val);
+
+		val = mt753x_reg_read(gsw, PLLGP_EN);
+		val |= EN_COREPLL;
+		mt753x_reg_write(gsw, PLLGP_EN, val);
+		usleep_range(25, 35);
+
+		break;
+	case XTAL_40MHZ:
+		/* Step 1 : Disable MT7531 COREPLL */
+		val = mt753x_reg_read(gsw, PLLGP_EN);
+		val &= ~EN_COREPLL;
+		mt753x_reg_write(gsw, PLLGP_EN, val);
+
+		/* Step 2: switch to XTAL output */
+		val = mt753x_reg_read(gsw, PLLGP_EN);
+		val |= SW_CLKSW;
+		mt753x_reg_write(gsw, PLLGP_EN, val);
+
+		val = mt753x_reg_read(gsw, PLLGP_CR0);
+		val &= ~RG_COREPLL_EN;
+		mt753x_reg_write(gsw, PLLGP_CR0, val);
+
+		/* Step 3: disable PLLGP and enable program PLLGP */
+		val = mt753x_reg_read(gsw, PLLGP_EN);
+		val |= SW_PLLGP;
+		mt753x_reg_write(gsw, PLLGP_EN, val);
+
+		/* Step 4: program COREPLL output frequency to 500MHz */
+		val = mt753x_reg_read(gsw, PLLGP_CR0);
+		val &= ~RG_COREPLL_POSDIV_M;
+		val |= 2 << RG_COREPLL_POSDIV_S;
+		mt753x_reg_write(gsw, PLLGP_CR0, val);
+		usleep_range(25, 35);
+
+		val = mt753x_reg_read(gsw, PLLGP_CR0);
+		val &= ~RG_COREPLL_SDM_PCW_M;
+		val |= 0x190000 << RG_COREPLL_SDM_PCW_S;
+		mt753x_reg_write(gsw, PLLGP_CR0, val);
+
+		/* Set feedback divide ratio update signal to high */
+		val = mt753x_reg_read(gsw, PLLGP_CR0);
+		val |= RG_COREPLL_SDM_PCW_CHG;
+		mt753x_reg_write(gsw, PLLGP_CR0, val);
+		/* Wait for at least 16 XTAL clocks */
+		usleep_range(10, 20);
+
+		/* Step 5: set feedback divide ratio update signal to low */
+		val = mt753x_reg_read(gsw, PLLGP_CR0);
+		val &= ~RG_COREPLL_SDM_PCW_CHG;
+		mt753x_reg_write(gsw, PLLGP_CR0, val);
+
+		/* Enable 325M clock for SGMII */
+		mt753x_reg_write(gsw, ANA_PLLGP_CR5, 0xad0000);
+
+		/* Enable 250SSC clock for RGMII */
+		mt753x_reg_write(gsw, ANA_PLLGP_CR2, 0x4f40000);
+
+		/* Step 6: Enable MT7531 PLL */
+		val = mt753x_reg_read(gsw, PLLGP_CR0);
+		val |= RG_COREPLL_EN;
+		mt753x_reg_write(gsw, PLLGP_CR0, val);
+
+		val = mt753x_reg_read(gsw, PLLGP_EN);
+		val |= EN_COREPLL;
+		mt753x_reg_write(gsw, PLLGP_EN, val);
+		usleep_range(25, 35);
+		break;
+	}
+}
+
+static int mt7531_internal_phy_calibration(struct gsw_mt753x *gsw)
+{
+	return 0;
+}
+
+static int mt7531_sw_detect(struct gsw_mt753x *gsw, struct chip_rev *crev)
+{
+	u32 rev, topsig;
+
+	rev = mt753x_reg_read(gsw, CHIP_REV);
+
+	if (((rev & CHIP_NAME_M) >> CHIP_NAME_S) == MT7531) {
+		if (crev) {
+			topsig = mt753x_reg_read(gsw, TOP_SIG_SR);
+
+			crev->rev = rev & CHIP_REV_M;
+			crev->name = topsig & PAD_DUAL_SGMII_EN ?
+				     "MT7531AE" : "MT7531BE";
+		}
+
+		return 0;
+	}
+
+	return -ENODEV;
+}
+
+static void pinmux_set_mux_7531(struct gsw_mt753x *gsw, u32 pin, u32 mode)
+{
+	u32 val;
+
+	val = mt753x_reg_read(gsw, GPIO_MODE_REGS(pin));
+	val &= ~(0xf << (pin & 7) * GPIO_MODE_S);
+	val |= mode << (pin & 7) * GPIO_MODE_S;
+	mt753x_reg_write(gsw, GPIO_MODE_REGS(pin), val);
+}
+
+static int mt7531_set_gpio_pinmux(struct gsw_mt753x *gsw)
+{
+	u32 group = 0;
+	struct device_node *np = gsw->dev->of_node;
+
+	/* Set GPIO 0 interrupt mode */
+	pinmux_set_mux_7531(gsw, gpio_int_pins[0], gpio_int_funcs[0]);
+
+	of_property_read_u32(np, "mediatek,mdio_master_pinmux", &group);
+
+	/* group = 0: do nothing, 1: 1st group (AE), 2: 2nd group (BE) */
+	if (group > 0 && group <= 2) {
+		group--;
+		pinmux_set_mux_7531(gsw, gpio_mdc_pins[group],
+				    gpio_mdc_funcs[group]);
+		pinmux_set_mux_7531(gsw, gpio_mdio_pins[group],
+				    gpio_mdio_funcs[group]);
+	}
+
+	return 0;
+}
+
+static void mt7531_phy_pll_setup(struct gsw_mt753x *gsw)
+{
+	u32 hwstrap;
+	u32 val;
+
+	val = mt753x_reg_read(gsw, CHIP_REV);
+	if ((val & CHIP_REV_M) > 0)
+		return;
+
+	hwstrap = mt753x_reg_read(gsw, HWSTRAP);
+
+	switch ((hwstrap & XTAL_FSEL_M) >> XTAL_FSEL_S) {
+	case XTAL_25MHZ:
+		/* disable pll auto calibration */
+		gsw->mmd_write(gsw, 0, PHY_DEV1F, PHY_DEV1F_REG_104, 0x608);
+
+		/* change pll sel */
+		val = gsw->mmd_read(gsw, 0, PHY_DEV1F,
+				     PHY_DEV1F_REG_403);
+		val &= ~(PHY_PLL_M);
+		val |= PHY_PLL_SEL(3);
+		gsw->mmd_write(gsw, 0, PHY_DEV1F, PHY_DEV1F_REG_403, val);
+
+		/* set divider ratio */
+		gsw->mmd_write(gsw, 0, PHY_DEV1F,
+			       PHY_DEV1F_REG_10A, 0x1009);
+
+		/* set divider ratio */
+		gsw->mmd_write(gsw, 0, PHY_DEV1F, PHY_DEV1F_REG_10B, 0x7c6);
+
+		/* capacitance and resistance adjustment */
+		gsw->mmd_write(gsw, 0, PHY_DEV1F,
+			       PHY_DEV1F_REG_10C, 0xa8be);
+
+		break;
+	case XTAL_40MHZ:
+		/* disable pll auto calibration */
+		gsw->mmd_write(gsw, 0, PHY_DEV1F, PHY_DEV1F_REG_104, 0x608);
+
+		/* change pll sel */
+		val = gsw->mmd_read(gsw, 0, PHY_DEV1F,
+				     PHY_DEV1F_REG_403);
+		val &= ~(PHY_PLL_M);
+		val |= PHY_PLL_SEL(3);
+		gsw->mmd_write(gsw, 0, PHY_DEV1F, PHY_DEV1F_REG_403, val);
+
+		/* set divider ratio */
+		gsw->mmd_write(gsw, 0, PHY_DEV1F,
+			       PHY_DEV1F_REG_10A, 0x1018);
+
+		/* set divider ratio */
+		gsw->mmd_write(gsw, 0, PHY_DEV1F, PHY_DEV1F_REG_10B, 0xc676);
+
+		/* capacitance and resistance adjustment */
+		gsw->mmd_write(gsw, 0, PHY_DEV1F,
+			       PHY_DEV1F_REG_10C, 0xd8be);
+		break;
+	}
+
+	/* power down pll. additional delay is not required via mdio access */
+	gsw->mmd_write(gsw, 0, PHY_DEV1F, PHY_DEV1F_REG_10D, 0x10);
+
+	/* power up pll */
+	gsw->mmd_write(gsw, 0, PHY_DEV1F, PHY_DEV1F_REG_10D, 0x14);
+}
+
+/* 12 registers for TX_MLT3 waveform tuning.
+ *    012 345 678 9ab
+ *  1    __
+ *     _/  \_
+ *  0_/      \
+ *            \_    _/
+ * -1           \__/
+ */
+static void mt7531_phy_100m_eye_diag_setting(struct gsw_mt753x *gsw, u32 port)
+{
+	gsw->mmd_write(gsw, port, PHY_DEV1E, PHY_TX_MLT3_BASE + 0x0, 0x187);
+	gsw->mmd_write(gsw, port, PHY_DEV1E, PHY_TX_MLT3_BASE + 0x1, 0x1c9);
+	gsw->mmd_write(gsw, port, PHY_DEV1E, PHY_TX_MLT3_BASE + 0x2, 0x1c6);
+	gsw->mmd_write(gsw, port, PHY_DEV1E, PHY_TX_MLT3_BASE + 0x3, 0x182);
+	gsw->mmd_write(gsw, port, PHY_DEV1E, PHY_TX_MLT3_BASE + 0x4, 0x208);
+	gsw->mmd_write(gsw, port, PHY_DEV1E, PHY_TX_MLT3_BASE + 0x5, 0x205);
+	gsw->mmd_write(gsw, port, PHY_DEV1E, PHY_TX_MLT3_BASE + 0x6, 0x384);
+	gsw->mmd_write(gsw, port, PHY_DEV1E, PHY_TX_MLT3_BASE + 0x7, 0x3cb);
+	gsw->mmd_write(gsw, port, PHY_DEV1E, PHY_TX_MLT3_BASE + 0x8, 0x3c4);
+	gsw->mmd_write(gsw, port, PHY_DEV1E, PHY_TX_MLT3_BASE + 0x9, 0x30a);
+	gsw->mmd_write(gsw, port, PHY_DEV1E, PHY_TX_MLT3_BASE + 0xa, 0x00b);
+	gsw->mmd_write(gsw, port, PHY_DEV1E, PHY_TX_MLT3_BASE + 0xb, 0x002);
+}
+
+static void mt7531_phy_setting(struct gsw_mt753x *gsw)
+{
+	int i;
+	u32 val;
+
+	for (i = 0; i < MT753X_NUM_PHYS; i++) {
+		mt7531_phy_100m_eye_diag_setting(gsw, i);
+
+		/* Enable HW auto downshift */
+		gsw->mii_write(gsw, i, 0x1f, 0x1);
+		val = gsw->mii_read(gsw, i, PHY_EXT_REG_14);
+		val |= PHY_EN_DOWN_SHFIT;
+		gsw->mii_write(gsw, i, PHY_EXT_REG_14, val);
+
+		/* Decrease SlvDPSready time */
+		val = mt753x_tr_read(gsw, i, PMA_CH, PMA_NOD, PMA_17);
+		val &= ~SLV_DSP_READY_TIME_M;
+		val |= 0xc << SLV_DSP_READY_TIME_S;
+		mt753x_tr_write(gsw, i, PMA_CH, PMA_NOD, PMA_17, val);
+
+		/* Enable Random Update Mechanism */
+		val = mt753x_tr_read(gsw, i, PMA_CH, PMA_NOD, PMA_18);
+		val |= ENABLE_RANDOM_UPDATE_TRIGGER;
+		mt753x_tr_write(gsw, i, PMA_CH, PMA_NOD, PMA_18, val);
+
+		/* PHY link down power saving enable */
+		val = gsw->mii_read(gsw, i, PHY_EXT_REG_17);
+		val |= PHY_LINKDOWN_POWER_SAVING_EN;
+		gsw->mii_write(gsw, i, PHY_EXT_REG_17, val);
+
+		val = gsw->mmd_read(gsw, i, PHY_DEV1E, PHY_DEV1E_REG_0C6);
+		val &= ~PHY_POWER_SAVING_M;
+		val |= PHY_POWER_SAVING_TX << PHY_POWER_SAVING_S;
+		gsw->mmd_write(gsw, i, PHY_DEV1E, PHY_DEV1E_REG_0C6, val);
+
+		/* Timing Recovery for GbE slave mode */
+		mt753x_tr_write(gsw, i, PMA_CH, PMA_NOD, PMA_01, 0x6fb90a);
+		mt753x_tr_write(gsw, i, DSP_CH, DSP_NOD, DSP_06, 0x2ebaef);
+		val = gsw->mmd_read(gsw, i, PHY_DEV1E, PHY_DEV1E_REG_234);
+		val |= TR_OPEN_LOOP_EN;
+		gsw->mmd_write(gsw, i, PHY_DEV1E, PHY_DEV1E_REG_234, val);
+
+		/* Enable Asymmetric Pause Capability */
+		val = gsw->mii_read(gsw, i, MII_ADVERTISE);
+		val |= ADVERTISE_PAUSE_ASYM;
+		gsw->mii_write(gsw, i, MII_ADVERTISE, val);
+	}
+}
+
+static void mt7531_adjust_line_driving(struct gsw_mt753x *gsw, u32 port)
+{
+	/* For ADC timing margin window for LDO calibration */
+	gsw->mmd_write(gsw, port, PHY_DEV1E, RXADC_LDO_CONTROL_2, 0x2222);
+
+	/* Adjust AD sample timing */
+	gsw->mmd_write(gsw, port, PHY_DEV1E, RXADC_CONTROL_3, 0x4444);
+
+	/* Adjust Line driver current for different mode */
+	gsw->mmd_write(gsw, port, PHY_DEV1F, TXVLD_DA_271, 0x2ca5);
+
+	/* Adjust Line driver current for different mode */
+	gsw->mmd_write(gsw, port, PHY_DEV1F, TXVLD_DA_272, 0xc6b);
+
+	/* Adjust Line driver gain for 10BT from 1000BT calibration result */
+	gsw->mmd_write(gsw, port, PHY_DEV1F, TXVLD_DA_273, 0x3000);
+
+	/* Adjust RX Echo path filter */
+	gsw->mmd_write(gsw, port, PHY_DEV1E, PHY_DEV1E_REG_0FE, 0x2);
+
+	/* Adjust RX HVGA bias current */
+	gsw->mmd_write(gsw, port, PHY_DEV1E, PHY_DEV1E_REG_41, 0x3333);
+
+	/* Adjust TX class AB driver 1 */
+	gsw->mmd_write(gsw, port, PHY_DEV1F, PHY_DEV1F_REG_268, 0x384);
+
+	/* Adjust TX class AB driver 2 */
+	gsw->mmd_write(gsw, port, PHY_DEV1F, PHY_DEV1F_REG_269, 0x1114);
+
+	/* Adjust DAC delay for TX Pairs */
+	gsw->mmd_write(gsw, port, PHY_DEV1E, PHY_DEV1E_REG_13, 0x404);
+	gsw->mmd_write(gsw, port, PHY_DEV1E, PHY_DEV1E_REG_14, 0x404);
+
+	/* Adjust DAC digital delay for TX Delay */
+	gsw->mmd_write(gsw, port, PHY_DEV1F, PHY_DEV1F_REG_44, 0xc0);
+
+	/* Adjust Line driver compensation cap for stability concern due to
+	 * increase current.
+	 */
+	gsw->mmd_write(gsw, port, PHY_DEV1F, PHY_DEV1F_REG_26A, 0x3333);
+}
+
+static void mt7531_eee_setting(struct gsw_mt753x *gsw, u32 port)
+{
+	u32 val;
+
+	/* Disable EEE */
+	gsw->mmd_write(gsw, port, PHY_DEV07, PHY_DEV07_REG_03C, 0);
+
+	/* Disable generate signal to clear the scramble_lock when lpi mode */
+	val = gsw->mmd_read(gsw, port, PHY_DEV1E, PHY_DEV1E_REG_189);
+	val &= ~DESCRAMBLER_CLEAR_EN;
+	gsw->mmd_write(gsw, port, PHY_DEV1E, PHY_DEV1E_REG_189, val);
+
+	/* Roll back EEE Slave Mode */
+	gsw->mmd_write(gsw, port, 0x1e, 0x2d1, 0);
+	mt753x_tr_write(gsw, port, DSP_CH, DSP_NOD, DSP_08, 0x1b);
+	mt753x_tr_write(gsw, port, DSP_CH, DSP_NOD, DSP_0f, 0);
+	mt753x_tr_write(gsw, port, DSP_CH, DSP_NOD, DSP_10, 0x5000);
+
+	/* Adjust 100_mse_threshold */
+	gsw->mmd_write(gsw, port, PHY_DEV1E, PHY_DEV1E_REG_123, 0xffff);
+
+	/* Disable mcc */
+	gsw->mmd_write(gsw, port, PHY_DEV1E, PHY_DEV1E_REG_A6, 0x300);
+}
+
+static void mt7531_afifo_reset(struct gsw_mt753x *gsw, int enable)
+{
+	int p;
+	u32 val;
+
+	if (enable) {
+		for (p = 0; p < MT753X_NUM_PORTS; p++) {
+			val = mt753x_reg_read(gsw, DBG_CNT(p));
+			val &= ~DIS_CLR;
+			mt753x_reg_write(gsw, DBG_CNT(p), val);
+		}
+	} else {
+		for (p = 0; p < MT753X_NUM_PORTS; p++) {
+			val = mt753x_reg_read(gsw, DBG_CNT(p));
+			val |= DIS_CLR;
+			mt753x_reg_write(gsw, DBG_CNT(p), val);
+		}
+	}
+}
+
+static int mt7531_sw_init(struct gsw_mt753x *gsw)
+{
+	int i;
+	u32 val;
+
+	gsw->phy_base = (gsw->smi_addr + 1) & MT753X_SMI_ADDR_MASK;
+
+	gsw->mii_read = mt753x_mii_read;
+	gsw->mii_write = mt753x_mii_write;
+	gsw->mmd_read = mt753x_mmd_read;
+	gsw->mmd_write = mt753x_mmd_write;
+
+	gsw->hw_phy_cal = of_property_read_bool(gsw->dev->of_node, "mediatek,hw_phy_cal");
+
+	for (i = 0; i < MT753X_NUM_PHYS; i++) {
+		val = gsw->mii_read(gsw, i, MII_BMCR);
+		val |= BMCR_ISOLATE;
+		gsw->mii_write(gsw, i, MII_BMCR, val);
+	}
+
+	/* Force MAC link down before reset */
+	mt753x_reg_write(gsw, PMCR(5), FORCE_MODE_LNK);
+	mt753x_reg_write(gsw, PMCR(6), FORCE_MODE_LNK);
+
+	/* Switch soft reset */
+	mt753x_reg_write(gsw, SYS_CTRL, SW_SYS_RST | SW_REG_RST);
+	usleep_range(10, 20);
+
+	/* Enable MDC input Schmitt Trigger */
+	val = mt753x_reg_read(gsw, SMT0_IOLB);
+	mt753x_reg_write(gsw, SMT0_IOLB, val | SMT_IOLB_5_SMI_MDC_EN);
+
+	/* Set 7531 gpio pinmux */
+	mt7531_set_gpio_pinmux(gsw);
+
+	mt7531_core_pll_setup(gsw);
+	mt7531_mac_port_setup(gsw, 5, &gsw->port5_cfg);
+	mt7531_mac_port_setup(gsw, 6, &gsw->port6_cfg);
+
+	/* Global mac control settings */
+	mt753x_reg_write(gsw, GMACCR,
+			 (15 << MTCC_LMT_S) | (15 << MAX_RX_JUMBO_S) |
+			 RX_PKT_LEN_MAX_JUMBO);
+
+	/* Enable Collision Poll */
+	val = mt753x_reg_read(gsw, CPGC_CTRL);
+	val |= COL_CLK_EN;
+	mt753x_reg_write(gsw, CPGC_CTRL, val);
+	val |= COL_RST_N;
+	mt753x_reg_write(gsw, CPGC_CTRL, val);
+	val |= COL_EN;
+	mt753x_reg_write(gsw, CPGC_CTRL, val);
+
+	/* Disable AFIFO reset for extra short IPG */
+	mt7531_afifo_reset(gsw, 0);
+
+	return 0;
+}
+
+static int mt7531_sw_post_init(struct gsw_mt753x *gsw)
+{
+	int i;
+	u32 val;
+
+	/* Let internal PHYs only Tx constant data in configure stage. */
+	for (i = 0; i < MT753X_NUM_PHYS; i++)
+		gsw->mmd_write(gsw, i, PHY_DEV1E, PHY_DEV1E_REG_141, 0x200);
+
+	/* Internal PHYs might be enabled by HW Bootstrapping, or bootloader.
+	 * Turn off PHYs before setup PHY PLL.
+	 */
+	val = gsw->mmd_read(gsw, 0, PHY_DEV1F, PHY_DEV1F_REG_403);
+	val |= PHY_EN_BYPASS_MODE;
+	val |= POWER_ON_OFF;
+	gsw->mmd_write(gsw, 0, PHY_DEV1F, PHY_DEV1F_REG_403, val);
+
+	mt7531_phy_pll_setup(gsw);
+
+	/* Enable Internal PHYs before phy setting */
+	val = gsw->mmd_read(gsw, 0, PHY_DEV1F, PHY_DEV1F_REG_403);
+	val |= PHY_EN_BYPASS_MODE;
+	val &= ~POWER_ON_OFF;
+	gsw->mmd_write(gsw, 0, PHY_DEV1F, PHY_DEV1F_REG_403, val);
+
+	mt7531_phy_setting(gsw);
+
+	for (i = 0; i < MT753X_NUM_PHYS; i++) {
+		val = gsw->mii_read(gsw, i, MII_BMCR);
+		val &= ~BMCR_ISOLATE;
+		gsw->mii_write(gsw, i, MII_BMCR, val);
+	}
+
+	for (i = 0; i < MT753X_NUM_PHYS; i++) {
+		mt7531_adjust_line_driving(gsw, i);
+		mt7531_eee_setting(gsw, i);
+	}
+
+	/* Restore internal PHYs normal Tx function after configure stage. */
+	for (i = 0; i < MT753X_NUM_PHYS; i++)
+		gsw->mmd_write(gsw, i, PHY_DEV1E, PHY_DEV1E_REG_141, 0x0);
+
+	mt7531_internal_phy_calibration(gsw);
+
+	return 0;
+}
+
+struct mt753x_sw_id mt7531_id = {
+	.model = MT7531,
+	.detect = mt7531_sw_detect,
+	.init = mt7531_sw_init,
+	.post_init = mt7531_sw_post_init
+};
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Zhanguo Ju <zhanguo.ju@mediatek.com>");
+MODULE_DESCRIPTION("Driver for MediaTek MT753x Gigabit Switch");
diff --git a/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt753x.h b/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt753x.h
new file mode 100755
index 0000000..732bda1
--- /dev/null
+++ b/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt753x.h
@@ -0,0 +1,224 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2018 MediaTek Inc.
+ * Author: Weijie Gao <weijie.gao@mediatek.com>
+ */
+
+#ifndef _MT753X_H_
+#define _MT753X_H_
+
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/netdevice.h>
+#include <linux/of_mdio.h>
+#include <linux/workqueue.h>
+#include <linux/gpio/consumer.h>
+
+#ifdef CONFIG_SWCONFIG
+#include <linux/switch.h>
+#endif
+
+#include "mt753x_vlan.h"
+
+#define MT753X_DFL_CPU_PORT	6
+#define MT753X_NUM_PHYS		5
+
+#define MT753X_DFL_SMI_ADDR	0x1f
+#define MT753X_SMI_ADDR_MASK	0x1f
+
+struct gsw_mt753x;
+
+enum mt753x_model {
+	MT7530 = 0x7530,
+	MT7531 = 0x7531
+};
+
+struct mt753x_port_cfg {
+	struct device_node *np;
+	int phy_mode;
+	u32 enabled: 1;
+	u32 force_link: 1;
+	u32 speed: 2;
+	u32 duplex: 1;
+	bool ssc_on;
+	bool stag_on;
+};
+
+struct mt753x_phy {
+	struct gsw_mt753x *gsw;
+	struct net_device netdev;
+	struct phy_device *phydev;
+};
+
+struct gsw_mt753x {
+	u32 id;
+
+	struct device *dev;
+	struct mii_bus *host_bus;
+	struct mii_bus *gphy_bus;
+	struct mutex mii_lock;	/* MII access lock */
+	u32 smi_addr;
+	u32 phy_base;
+	int direct_phy_access;
+
+	enum mt753x_model model;
+	const char *name;
+
+	struct mt753x_port_cfg port5_cfg;
+	struct mt753x_port_cfg port6_cfg;
+
+	bool hw_phy_cal;
+	bool phy_status_poll;
+	struct mt753x_phy phys[MT753X_NUM_PHYS];
+//	int phy_irqs[PHY_MAX_ADDR]; //FIXME 
+
+	int phy_link_sts;
+
+	int irq;
+	int reset_pin;
+	struct work_struct irq_worker;
+
+#ifdef CONFIG_SWCONFIG
+	struct switch_dev swdev;
+	u32 cpu_port;
+#endif
+
+	int global_vlan_enable;
+	struct mt753x_vlan_entry vlan_entries[MT753X_NUM_VLANS];
+	struct mt753x_port_entry port_entries[MT753X_NUM_PORTS];
+
+	int (*mii_read)(struct gsw_mt753x *gsw, int phy, int reg);
+	void (*mii_write)(struct gsw_mt753x *gsw, int phy, int reg, u16 val);
+
+	int (*mmd_read)(struct gsw_mt753x *gsw, int addr, int devad, u16 reg);
+	void (*mmd_write)(struct gsw_mt753x *gsw, int addr, int devad, u16 reg,
+			  u16 val);
+
+	struct list_head list;
+};
+
+struct chip_rev {
+	const char *name;
+	u32 rev;
+};
+
+struct mt753x_sw_id {
+	enum mt753x_model model;
+	int (*detect)(struct gsw_mt753x *gsw, struct chip_rev *crev);
+	int (*init)(struct gsw_mt753x *gsw);
+	int (*post_init)(struct gsw_mt753x *gsw);
+};
+
+extern struct list_head mt753x_devs;
+
+struct gsw_mt753x *mt753x_get_gsw(u32 id);
+struct gsw_mt753x *mt753x_get_first_gsw(void);
+void mt753x_put_gsw(void);
+void mt753x_lock_gsw(void);
+
+u32 mt753x_reg_read(struct gsw_mt753x *gsw, u32 reg);
+void mt753x_reg_write(struct gsw_mt753x *gsw, u32 reg, u32 val);
+
+int mt753x_mii_read(struct gsw_mt753x *gsw, int phy, int reg);
+void mt753x_mii_write(struct gsw_mt753x *gsw, int phy, int reg, u16 val);
+
+int mt753x_mmd_read(struct gsw_mt753x *gsw, int addr, int devad, u16 reg);
+void mt753x_mmd_write(struct gsw_mt753x *gsw, int addr, int devad, u16 reg,
+		      u16 val);
+
+int mt753x_mmd_ind_read(struct gsw_mt753x *gsw, int addr, int devad, u16 reg);
+void mt753x_mmd_ind_write(struct gsw_mt753x *gsw, int addr, int devad, u16 reg,
+			  u16 val);
+
+int mt753x_tr_read(struct gsw_mt753x *gsw, int addr, u8 ch, u8 node, u8 daddr);
+void mt753x_tr_write(struct gsw_mt753x *gsw, int addr, u8 ch, u8 node, u8 daddr,
+		     u32 data);
+
+void mt753x_irq_worker(struct work_struct *work);
+void mt753x_irq_enable(struct gsw_mt753x *gsw);
+
+int mt753x_phy_calibration(struct gsw_mt753x *gsw, u8 phyaddr);
+int extphy_init(struct gsw_mt753x *gsw, int addr);
+
+/* MDIO Indirect Access Registers */
+#define MII_MMD_ACC_CTL_REG		0x0d
+#define MMD_CMD_S			14
+#define MMD_CMD_M			0xc000
+#define MMD_DEVAD_S			0
+#define MMD_DEVAD_M			0x1f
+
+/* MMD_CMD: MMD commands */
+#define MMD_ADDR			0
+#define MMD_DATA			1
+
+#define MII_MMD_ADDR_DATA_REG		0x0e
+
+/* Procedure of MT753x Internal Register Access
+ *
+ * 1. Internal Register Address
+ *
+ *    The MT753x has a 16-bit register address and each register is 32-bit.
+ *    This means the lowest two bits are not used as the register address is
+ *    4-byte aligned.
+ *
+ *    Rest of the valid bits are divided into two parts:
+ *      Bit 15..6 is the Page address
+ *      Bit 5..2 is the low address
+ *
+ *    -------------------------------------------------------------------
+ *    | 15  14  13  12  11  10   9   8   7   6 | 5   4   3   2 | 1   0  |
+ *    |----------------------------------------|---------------|--------|
+ *    |              Page Address              |    Address    | Unused |
+ *    -------------------------------------------------------------------
+ *
+ * 2. MDIO access timing
+ *
+ *    The MT753x uses the following MDIO timing for a single register read
+ *
+ *      Phase 1: Write Page Address
+ *    -------------------------------------------------------------------
+ *    | ST | OP | PHY_ADDR | TYPE | RSVD | TA |  RSVD |    PAGE_ADDR    |
+ *    -------------------------------------------------------------------
+ *    | 01 | 01 |   11111  |   1  | 1111 | xx | 00000 | REG_ADDR[15..6] |
+ *    -------------------------------------------------------------------
+ *
+ *      Phase 2: Write low Address & Read low word
+ *    -------------------------------------------------------------------
+ *    | ST | OP | PHY_ADDR | TYPE |    LOW_ADDR    | TA |      DATA     |
+ *    -------------------------------------------------------------------
+ *    | 01 | 10 |   11111  |   0  | REG_ADDR[5..2] | xx |  DATA[15..0]  |
+ *    -------------------------------------------------------------------
+ *
+ *      Phase 3: Read high word
+ *    -------------------------------------------------------------------
+ *    | ST | OP | PHY_ADDR | TYPE | RSVD | TA |           DATA          |
+ *    -------------------------------------------------------------------
+ *    | 01 | 10 |   11111  |   1  | 0000 | xx |       DATA[31..16]      |
+ *    -------------------------------------------------------------------
+ *
+ *    The MT753x uses the following MDIO timing for a single register write
+ *
+ *      Phase 1: Write Page Address (The same as read)
+ *
+ *      Phase 2: Write low Address and low word
+ *    -------------------------------------------------------------------
+ *    | ST | OP | PHY_ADDR | TYPE |    LOW_ADDR    | TA |      DATA     |
+ *    -------------------------------------------------------------------
+ *    | 01 | 01 |   11111  |   0  | REG_ADDR[5..2] | xx |  DATA[15..0]  |
+ *    -------------------------------------------------------------------
+ *
+ *      Phase 3: write high word
+ *    -------------------------------------------------------------------
+ *    | ST | OP | PHY_ADDR | TYPE | RSVD | TA |           DATA          |
+ *    -------------------------------------------------------------------
+ *    | 01 | 01 |   11111  |   1  | 0000 | xx |       DATA[31..16]      |
+ *    -------------------------------------------------------------------
+ *
+ */
+
+/* Internal Register Address fields */
+#define MT753X_REG_PAGE_ADDR_S		6
+#define MT753X_REG_PAGE_ADDR_M		0xffc0
+#define MT753X_REG_ADDR_S		2
+#define MT753X_REG_ADDR_M		0x3c
+#endif /* _MT753X_H_ */
diff --git a/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt753x_mdio.c b/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt753x_mdio.c
new file mode 100755
index 0000000..06a1114
--- /dev/null
+++ b/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt753x_mdio.c
@@ -0,0 +1,861 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2018 MediaTek Inc.
+ * Author: Weijie Gao <weijie.gao@mediatek.com>
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/reset.h>
+#include <linux/hrtimer.h>
+#include <linux/mii.h>
+#include <linux/of_mdio.h>
+#include <linux/of_platform.h>
+#include <linux/of_gpio.h>
+#include <linux/of_net.h>
+#include <linux/of_irq.h>
+#include <linux/phy.h>
+
+#include "mt753x.h"
+#include "mt753x_swconfig.h"
+#include "mt753x_regs.h"
+#include "mt753x_nl.h"
+#include "mt7530.h"
+#include "mt7531.h"
+
+static u32 mt753x_id;
+struct list_head mt753x_devs;
+static DEFINE_MUTEX(mt753x_devs_lock);
+
+static struct mt753x_sw_id *mt753x_sw_ids[] = {
+	&mt7530_id,
+	&mt7531_id,
+};
+
+u32 mt753x_reg_read(struct gsw_mt753x *gsw, u32 reg)
+{
+	u32 high, low;
+
+	mutex_lock(&gsw->host_bus->mdio_lock);
+
+	gsw->host_bus->write(gsw->host_bus, gsw->smi_addr, 0x1f,
+		(reg & MT753X_REG_PAGE_ADDR_M) >> MT753X_REG_PAGE_ADDR_S);
+
+	low = gsw->host_bus->read(gsw->host_bus, gsw->smi_addr,
+		(reg & MT753X_REG_ADDR_M) >> MT753X_REG_ADDR_S);
+
+	high = gsw->host_bus->read(gsw->host_bus, gsw->smi_addr, 0x10);
+
+	mutex_unlock(&gsw->host_bus->mdio_lock);
+
+	return (high << 16) | (low & 0xffff);
+}
+
+void mt753x_reg_write(struct gsw_mt753x *gsw, u32 reg, u32 val)
+{
+	mutex_lock(&gsw->host_bus->mdio_lock);
+
+	gsw->host_bus->write(gsw->host_bus, gsw->smi_addr, 0x1f,
+		(reg & MT753X_REG_PAGE_ADDR_M) >> MT753X_REG_PAGE_ADDR_S);
+
+	gsw->host_bus->write(gsw->host_bus, gsw->smi_addr,
+		(reg & MT753X_REG_ADDR_M) >> MT753X_REG_ADDR_S, val & 0xffff);
+
+	gsw->host_bus->write(gsw->host_bus, gsw->smi_addr, 0x10, val >> 16);
+
+	mutex_unlock(&gsw->host_bus->mdio_lock);
+}
+
+/* Indirect MDIO clause 22/45 access */
+static int mt753x_mii_rw(struct gsw_mt753x *gsw, int phy, int reg, u16 data,
+			 u32 cmd, u32 st)
+{
+	ktime_t timeout;
+	u32 val, timeout_us;
+	int ret = 0;
+
+	timeout_us = 100000;
+	timeout = ktime_add_us(ktime_get(), timeout_us);
+	while (1) {
+		val = mt753x_reg_read(gsw, PHY_IAC);
+
+		if ((val & PHY_ACS_ST) == 0)
+			break;
+
+		if (ktime_compare(ktime_get(), timeout) > 0)
+			return -ETIMEDOUT;
+	}
+
+	val = (st << MDIO_ST_S) |
+	      ((cmd << MDIO_CMD_S) & MDIO_CMD_M) |
+	      ((phy << MDIO_PHY_ADDR_S) & MDIO_PHY_ADDR_M) |
+	      ((reg << MDIO_REG_ADDR_S) & MDIO_REG_ADDR_M);
+
+	if (cmd == MDIO_CMD_WRITE || cmd == MDIO_CMD_ADDR)
+		val |= data & MDIO_RW_DATA_M;
+
+	mt753x_reg_write(gsw, PHY_IAC, val | PHY_ACS_ST);
+
+	timeout_us = 100000;
+	timeout = ktime_add_us(ktime_get(), timeout_us);
+	while (1) {
+		val = mt753x_reg_read(gsw, PHY_IAC);
+
+		if ((val & PHY_ACS_ST) == 0)
+			break;
+
+		if (ktime_compare(ktime_get(), timeout) > 0)
+			return -ETIMEDOUT;
+	}
+
+	if (cmd == MDIO_CMD_READ || cmd == MDIO_CMD_READ_C45) {
+		val = mt753x_reg_read(gsw, PHY_IAC);
+		ret = val & MDIO_RW_DATA_M;
+	}
+
+	return ret;
+}
+
+int mt753x_mii_read(struct gsw_mt753x *gsw, int phy, int reg)
+{
+	int val;
+
+	if (phy < MT753X_NUM_PHYS)
+		phy = (gsw->phy_base + phy) & MT753X_SMI_ADDR_MASK;
+
+	mutex_lock(&gsw->mii_lock);
+	val = mt753x_mii_rw(gsw, phy, reg, 0, MDIO_CMD_READ, MDIO_ST_C22);
+	mutex_unlock(&gsw->mii_lock);
+
+	return val;
+}
+
+void mt753x_mii_write(struct gsw_mt753x *gsw, int phy, int reg, u16 val)
+{
+	if (phy < MT753X_NUM_PHYS)
+		phy = (gsw->phy_base + phy) & MT753X_SMI_ADDR_MASK;
+
+	mutex_lock(&gsw->mii_lock);
+	mt753x_mii_rw(gsw, phy, reg, val, MDIO_CMD_WRITE, MDIO_ST_C22);
+	mutex_unlock(&gsw->mii_lock);
+}
+
+int mt753x_mmd_read(struct gsw_mt753x *gsw, int addr, int devad, u16 reg)
+{
+	int val;
+
+	if (addr < MT753X_NUM_PHYS)
+		addr = (gsw->phy_base + addr) & MT753X_SMI_ADDR_MASK;
+
+	mutex_lock(&gsw->mii_lock);
+	mt753x_mii_rw(gsw, addr, devad, reg, MDIO_CMD_ADDR, MDIO_ST_C45);
+	val = mt753x_mii_rw(gsw, addr, devad, 0, MDIO_CMD_READ_C45,
+			    MDIO_ST_C45);
+	mutex_unlock(&gsw->mii_lock);
+
+	return val;
+}
+
+void mt753x_mmd_write(struct gsw_mt753x *gsw, int addr, int devad, u16 reg,
+		      u16 val)
+{
+	if (addr < MT753X_NUM_PHYS)
+		addr = (gsw->phy_base + addr) & MT753X_SMI_ADDR_MASK;
+
+	mutex_lock(&gsw->mii_lock);
+	mt753x_mii_rw(gsw, addr, devad, reg, MDIO_CMD_ADDR, MDIO_ST_C45);
+	mt753x_mii_rw(gsw, addr, devad, val, MDIO_CMD_WRITE, MDIO_ST_C45);
+	mutex_unlock(&gsw->mii_lock);
+}
+
+int mt753x_mmd_ind_read(struct gsw_mt753x *gsw, int addr, int devad, u16 reg)
+{
+	u16 val;
+
+	if (addr < MT753X_NUM_PHYS)
+		addr = (gsw->phy_base + addr) & MT753X_SMI_ADDR_MASK;
+
+	mutex_lock(&gsw->mii_lock);
+
+	mt753x_mii_rw(gsw, addr, MII_MMD_ACC_CTL_REG,
+		      (MMD_ADDR << MMD_CMD_S) |
+		      ((devad << MMD_DEVAD_S) & MMD_DEVAD_M),
+		      MDIO_CMD_WRITE, MDIO_ST_C22);
+
+	mt753x_mii_rw(gsw, addr, MII_MMD_ADDR_DATA_REG, reg,
+		      MDIO_CMD_WRITE, MDIO_ST_C22);
+
+	mt753x_mii_rw(gsw, addr, MII_MMD_ACC_CTL_REG,
+		      (MMD_DATA << MMD_CMD_S) |
+		      ((devad << MMD_DEVAD_S) & MMD_DEVAD_M),
+		      MDIO_CMD_WRITE, MDIO_ST_C22);
+
+	val = mt753x_mii_rw(gsw, addr, MII_MMD_ADDR_DATA_REG, 0,
+			    MDIO_CMD_READ, MDIO_ST_C22);
+
+	mutex_unlock(&gsw->mii_lock);
+
+	return val;
+}
+
+void mt753x_mmd_ind_write(struct gsw_mt753x *gsw, int addr, int devad, u16 reg,
+			  u16 val)
+{
+	if (addr < MT753X_NUM_PHYS)
+		addr = (gsw->phy_base + addr) & MT753X_SMI_ADDR_MASK;
+
+	mutex_lock(&gsw->mii_lock);
+
+	mt753x_mii_rw(gsw, addr, MII_MMD_ACC_CTL_REG,
+		      (MMD_ADDR << MMD_CMD_S) |
+		      ((devad << MMD_DEVAD_S) & MMD_DEVAD_M),
+		      MDIO_CMD_WRITE, MDIO_ST_C22);
+
+	mt753x_mii_rw(gsw, addr, MII_MMD_ADDR_DATA_REG, reg,
+		      MDIO_CMD_WRITE, MDIO_ST_C22);
+
+	mt753x_mii_rw(gsw, addr, MII_MMD_ACC_CTL_REG,
+		      (MMD_DATA << MMD_CMD_S) |
+		      ((devad << MMD_DEVAD_S) & MMD_DEVAD_M),
+		      MDIO_CMD_WRITE, MDIO_ST_C22);
+
+	mt753x_mii_rw(gsw, addr, MII_MMD_ADDR_DATA_REG, val,
+		      MDIO_CMD_WRITE, MDIO_ST_C22);
+
+	mutex_unlock(&gsw->mii_lock);
+}
+
+static inline int mt753x_get_duplex(const struct device_node *np)
+{
+	return of_property_read_bool(np, "full-duplex");
+}
+
+static void mt753x_load_port_cfg(struct gsw_mt753x *gsw)
+{
+	struct device_node *port_np;
+	struct device_node *fixed_link_node;
+	struct mt753x_port_cfg *port_cfg;
+	u32 port;
+
+	for_each_child_of_node(gsw->dev->of_node, port_np) {
+		if (!of_device_is_compatible(port_np, "mediatek,mt753x-port"))
+			continue;
+
+		if (!of_device_is_available(port_np))
+			continue;
+
+		if (of_property_read_u32(port_np, "reg", &port))
+			continue;
+
+		switch (port) {
+		case 5:
+			port_cfg = &gsw->port5_cfg;
+			break;
+		case 6:
+			port_cfg = &gsw->port6_cfg;
+			break;
+		default:
+			continue;
+		}
+
+		if (port_cfg->enabled) {
+			dev_info(gsw->dev, "duplicated node for port%d\n",
+				 port_cfg->phy_mode);
+			continue;
+		}
+
+		port_cfg->np = port_np;
+
+		port_cfg->phy_mode = of_get_phy_mode(port_np);
+		if (port_cfg->phy_mode < 0) {
+			dev_info(gsw->dev, "incorrect phy-mode %d\n", port);
+			continue;
+		}
+
+		fixed_link_node = of_get_child_by_name(port_np, "fixed-link");
+		if (fixed_link_node) {
+			u32 speed;
+
+			port_cfg->force_link = 1;
+			port_cfg->duplex = mt753x_get_duplex(fixed_link_node);
+
+			if (of_property_read_u32(fixed_link_node, "speed",
+						 &speed)) {
+				speed = 0;
+				continue;
+			}
+
+			of_node_put(fixed_link_node);
+
+			switch (speed) {
+			case 10:
+				port_cfg->speed = MAC_SPD_10;
+				break;
+			case 100:
+				port_cfg->speed = MAC_SPD_100;
+				break;
+			case 1000:
+				port_cfg->speed = MAC_SPD_1000;
+				break;
+			case 2500:
+				port_cfg->speed = MAC_SPD_2500;
+				break;
+			default:
+				dev_info(gsw->dev, "incorrect speed %d\n",
+					 speed);
+				continue;
+			}
+		}
+
+		port_cfg->ssc_on = of_property_read_bool(port_cfg->np,
+							 "mediatek,ssc-on");
+		port_cfg->stag_on = of_property_read_bool(port_cfg->np,
+							  "mediatek,stag-on");
+		port_cfg->enabled = 1;
+	}
+}
+
+void mt753x_tr_write(struct gsw_mt753x *gsw, int addr, u8 ch, u8 node, u8 daddr,
+		     u32 data)
+{
+	ktime_t timeout;
+	u32 timeout_us;
+	u32 val;
+
+	if (addr < MT753X_NUM_PHYS)
+		addr = (gsw->phy_base + addr) & MT753X_SMI_ADDR_MASK;
+
+	gsw->mii_write(gsw, addr, PHY_CL22_PAGE_CTRL, PHY_TR_PAGE);
+
+	val = gsw->mii_read(gsw, addr, PHY_TR_CTRL);
+
+	timeout_us = 100000;
+	timeout = ktime_add_us(ktime_get(), timeout_us);
+	while (1) {
+		val = gsw->mii_read(gsw, addr, PHY_TR_CTRL);
+
+		if (!!(val & PHY_TR_PKT_XMT_STA))
+			break;
+
+		if (ktime_compare(ktime_get(), timeout) > 0)
+			goto out;
+	}
+
+	gsw->mii_write(gsw, addr, PHY_TR_LOW_DATA, PHY_TR_LOW_VAL(data));
+	gsw->mii_write(gsw, addr, PHY_TR_HIGH_DATA, PHY_TR_HIGH_VAL(data));
+	val = PHY_TR_PKT_XMT_STA | (PHY_TR_WRITE << PHY_TR_WR_S) |
+	      (ch << PHY_TR_CH_ADDR_S) | (node << PHY_TR_NODE_ADDR_S) |
+	      (daddr << PHY_TR_DATA_ADDR_S);
+	gsw->mii_write(gsw, addr, PHY_TR_CTRL, val);
+
+	timeout_us = 100000;
+	timeout = ktime_add_us(ktime_get(), timeout_us);
+	while (1) {
+		val = gsw->mii_read(gsw, addr, PHY_TR_CTRL);
+
+		if (!!(val & PHY_TR_PKT_XMT_STA))
+			break;
+
+		if (ktime_compare(ktime_get(), timeout) > 0)
+			goto out;
+	}
+out:
+	gsw->mii_write(gsw, addr, PHY_CL22_PAGE_CTRL, 0);
+}
+
+int mt753x_tr_read(struct gsw_mt753x *gsw, int addr, u8 ch, u8 node, u8 daddr)
+{
+	ktime_t timeout;
+	u32 timeout_us;
+	u32 val;
+	u8 val_h;
+
+	if (addr < MT753X_NUM_PHYS)
+		addr = (gsw->phy_base + addr) & MT753X_SMI_ADDR_MASK;
+
+	gsw->mii_write(gsw, addr, PHY_CL22_PAGE_CTRL, PHY_TR_PAGE);
+
+	val = gsw->mii_read(gsw, addr, PHY_TR_CTRL);
+
+	timeout_us = 100000;
+	timeout = ktime_add_us(ktime_get(), timeout_us);
+	while (1) {
+		val = gsw->mii_read(gsw, addr, PHY_TR_CTRL);
+
+		if (!!(val & PHY_TR_PKT_XMT_STA))
+			break;
+
+		if (ktime_compare(ktime_get(), timeout) > 0) {
+			gsw->mii_write(gsw, addr, PHY_CL22_PAGE_CTRL, 0);
+			return -ETIMEDOUT;
+		}
+	}
+
+	val = PHY_TR_PKT_XMT_STA | (PHY_TR_READ << PHY_TR_WR_S) |
+	      (ch << PHY_TR_CH_ADDR_S) | (node << PHY_TR_NODE_ADDR_S) |
+	      (daddr << PHY_TR_DATA_ADDR_S);
+	gsw->mii_write(gsw, addr, PHY_TR_CTRL, val);
+
+	timeout_us = 100000;
+	timeout = ktime_add_us(ktime_get(), timeout_us);
+	while (1) {
+		val = gsw->mii_read(gsw, addr, PHY_TR_CTRL);
+
+		if (!!(val & PHY_TR_PKT_XMT_STA))
+			break;
+
+		if (ktime_compare(ktime_get(), timeout) > 0) {
+			gsw->mii_write(gsw, addr, PHY_CL22_PAGE_CTRL, 0);
+			return -ETIMEDOUT;
+		}
+	}
+
+	val = gsw->mii_read(gsw, addr, PHY_TR_LOW_DATA);
+	val_h = gsw->mii_read(gsw, addr, PHY_TR_HIGH_DATA);
+	val |= (val_h << 16);
+
+	gsw->mii_write(gsw, addr, PHY_CL22_PAGE_CTRL, 0);
+
+	return val;
+}
+
+static void mt753x_add_gsw(struct gsw_mt753x *gsw)
+{
+	mutex_lock(&mt753x_devs_lock);
+	gsw->id = mt753x_id++;
+	INIT_LIST_HEAD(&gsw->list);
+	list_add_tail(&gsw->list, &mt753x_devs);
+	mutex_unlock(&mt753x_devs_lock);
+}
+
+static void mt753x_remove_gsw(struct gsw_mt753x *gsw)
+{
+	mutex_lock(&mt753x_devs_lock);
+	list_del(&gsw->list);
+	mutex_unlock(&mt753x_devs_lock);
+}
+
+
+struct gsw_mt753x *mt753x_get_gsw(u32 id)
+{
+	struct gsw_mt753x *dev;
+
+	mutex_lock(&mt753x_devs_lock);
+
+	list_for_each_entry(dev, &mt753x_devs, list) {
+		if (dev->id == id)
+			return dev;
+	}
+
+	mutex_unlock(&mt753x_devs_lock);
+
+	return NULL;
+}
+
+struct gsw_mt753x *mt753x_get_first_gsw(void)
+{
+	struct gsw_mt753x *dev;
+
+	mutex_lock(&mt753x_devs_lock);
+
+	list_for_each_entry(dev, &mt753x_devs, list)
+		return dev;
+
+	mutex_unlock(&mt753x_devs_lock);
+
+	return NULL;
+}
+
+void mt753x_put_gsw(void)
+{
+	mutex_unlock(&mt753x_devs_lock);
+}
+
+void mt753x_lock_gsw(void)
+{
+	mutex_lock(&mt753x_devs_lock);
+}
+
+static int mt753x_hw_reset(struct gsw_mt753x *gsw)
+{
+	struct device_node *np = gsw->dev->of_node;
+	struct reset_control *rstc;
+	int mcm;
+	int ret = -EINVAL;
+
+	mcm = of_property_read_bool(np, "mediatek,mcm");
+	if (mcm) {
+		rstc = devm_reset_control_get(gsw->dev, "mcm");
+		ret = IS_ERR(rstc);
+		if (IS_ERR(rstc)) {
+			dev_err(gsw->dev, "Missing reset ctrl of switch\n");
+			return ret;
+		}
+
+		reset_control_assert(rstc);
+		msleep(30);
+		reset_control_deassert(rstc);
+
+		gsw->reset_pin = -1;
+		return 0;
+	}
+
+	gsw->reset_pin = of_get_named_gpio(np, "reset-gpios", 0);
+	if (gsw->reset_pin < 0) {
+		dev_err(gsw->dev, "Missing reset pin of switch\n");
+		return ret;
+	}
+
+	ret = devm_gpio_request(gsw->dev, gsw->reset_pin, "mt753x-reset");
+	if (ret) {
+		dev_info(gsw->dev, "Failed to request gpio %d\n",
+			 gsw->reset_pin);
+		return ret;
+	}
+
+	gpio_direction_output(gsw->reset_pin, 0);
+	msleep(30);
+	gpio_set_value(gsw->reset_pin, 1);
+	msleep(500);
+
+	return 0;
+}
+#if 1 //XDXDXDXD
+static int mt753x_mdio_read(struct mii_bus *bus, int addr, int reg)
+{
+	struct gsw_mt753x *gsw = bus->priv;
+
+	return gsw->mii_read(gsw, addr, reg);
+}
+
+static int mt753x_mdio_write(struct mii_bus *bus, int addr, int reg, u16 val)
+{
+	struct gsw_mt753x *gsw = bus->priv;
+
+	gsw->mii_write(gsw, addr, reg, val);
+
+	return 0;
+}
+
+static const struct net_device_ops mt753x_dummy_netdev_ops = {
+};
+
+static void mt753x_phy_link_handler(struct net_device *dev)
+{
+	struct mt753x_phy *phy = container_of(dev, struct mt753x_phy, netdev);
+	struct phy_device *phydev = phy->phydev;
+	struct gsw_mt753x *gsw = phy->gsw;
+	u32 port = phy - gsw->phys;
+
+	if (phydev->link) {
+		dev_info(gsw->dev,
+			 "Port %d Link is Up - %s/%s - flow control %s\n",
+			 port, phy_speed_to_str(phydev->speed),
+			 (phydev->duplex == DUPLEX_FULL) ? "Full" : "Half",
+			 phydev->pause ? "rx/tx" : "off");
+	} else {
+		dev_info(gsw->dev, "Port %d Link is Down\n", port);
+	}
+}
+
+static void mt753x_connect_internal_phys(struct gsw_mt753x *gsw,
+					 struct device_node *mii_np)
+{
+	struct device_node *phy_np;
+	struct mt753x_phy *phy;
+	int phy_mode;
+	u32 phyad;
+
+	if (!mii_np)
+		return;
+
+	for_each_child_of_node(mii_np, phy_np) {
+		if (of_property_read_u32(phy_np, "reg", &phyad))
+			continue;
+
+		if (phyad >= MT753X_NUM_PHYS)
+			continue;
+
+		phy_mode = of_get_phy_mode(phy_np);
+		if (phy_mode < 0) {
+			dev_info(gsw->dev, "incorrect phy-mode %d for PHY %d\n",
+				 phy_mode, phyad);
+			continue;
+		}
+
+		phy = &gsw->phys[phyad];
+		phy->gsw = gsw;
+
+		init_dummy_netdev(&phy->netdev);
+		phy->netdev.netdev_ops = &mt753x_dummy_netdev_ops;
+
+		phy->phydev = of_phy_connect(&phy->netdev, phy_np,
+					mt753x_phy_link_handler, 0, phy_mode);
+		if (!phy->phydev) {
+			dev_info(gsw->dev, "could not connect to PHY %d\n",
+				 phyad);
+			continue;
+		}
+
+		phy_start(phy->phydev);
+	}
+}
+
+static void mt753x_disconnect_internal_phys(struct gsw_mt753x *gsw)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(gsw->phys); i++) {
+		if (gsw->phys[i].phydev) {
+			phy_stop(gsw->phys[i].phydev);
+			phy_disconnect(gsw->phys[i].phydev);
+			gsw->phys[i].phydev = NULL;
+		}
+	}
+}
+
+static int mt753x_mdio_register(struct gsw_mt753x *gsw)
+{
+	struct device_node *mii_np;
+	int i, ret;
+
+	mii_np = of_get_child_by_name(gsw->dev->of_node, "mdio-bus");
+	if (mii_np && !of_device_is_available(mii_np)) {
+		ret = -ENODEV;
+		goto err_put_node;
+	}
+
+	gsw->gphy_bus = devm_mdiobus_alloc(gsw->dev);
+	if (!gsw->gphy_bus) {
+		ret = -ENOMEM;
+		goto err_put_node;
+	}
+
+	gsw->gphy_bus->name = "mt753x_mdio";
+	gsw->gphy_bus->read = mt753x_mdio_read;
+	gsw->gphy_bus->write = mt753x_mdio_write;
+	gsw->gphy_bus->priv = gsw;
+	gsw->gphy_bus->parent = gsw->dev;
+	gsw->gphy_bus->phy_mask = BIT(MT753X_NUM_PHYS) - 1;
+//	gsw->gphy_bus->irq = gsw->phy_irqs;
+
+	for (i = 0; i < PHY_MAX_ADDR; i++)
+		gsw->gphy_bus->irq[i] = PHY_POLL;
+
+	if (mii_np)
+		snprintf(gsw->gphy_bus->id, MII_BUS_ID_SIZE, "%s@%s",
+			 mii_np->name, gsw->dev->of_node->name);
+	else
+		snprintf(gsw->gphy_bus->id, MII_BUS_ID_SIZE, "mdio@%s",
+			 gsw->dev->of_node->name);
+
+	ret = of_mdiobus_register(gsw->gphy_bus, mii_np);
+
+	if (ret) {
+		devm_mdiobus_free(gsw->dev, gsw->gphy_bus);
+		gsw->gphy_bus = NULL;
+	} else {
+		if (gsw->phy_status_poll)
+			mt753x_connect_internal_phys(gsw, mii_np);
+	}
+
+err_put_node:
+	if (mii_np)
+		of_node_put(mii_np);
+
+	return ret;
+}
+#endif
+
+static irqreturn_t mt753x_irq_handler(int irq, void *dev)
+{
+	struct gsw_mt753x *gsw = dev;
+
+	disable_irq_nosync(gsw->irq);
+
+	schedule_work(&gsw->irq_worker);
+
+	return IRQ_HANDLED;
+}
+
+static int mt753x_probe(struct platform_device *pdev)
+{
+	struct gsw_mt753x *gsw;
+	struct mt753x_sw_id *sw;
+	struct device_node *np = pdev->dev.of_node;
+	struct device_node *mdio;
+	struct mii_bus *mdio_bus;
+	int ret = -EINVAL;
+	struct chip_rev rev;
+	struct mt753x_mapping *map;
+	int i;
+
+	mdio = of_parse_phandle(np, "mediatek,mdio", 0);
+	if (!mdio)
+		return -EINVAL;
+
+	mdio_bus = of_mdio_find_bus(mdio);
+	if (!mdio_bus)
+		return -EPROBE_DEFER;
+
+	gsw = devm_kzalloc(&pdev->dev, sizeof(struct gsw_mt753x), GFP_KERNEL);
+	if (!gsw)
+		return -ENOMEM;
+
+	gsw->host_bus = mdio_bus;
+	gsw->dev = &pdev->dev;
+	mutex_init(&gsw->mii_lock);
+
+	/* Switch hard reset */
+	if (mt753x_hw_reset(gsw))
+		goto fail;
+
+	/* Fetch the SMI address dirst */
+	if (of_property_read_u32(np, "mediatek,smi-addr", &gsw->smi_addr))
+		gsw->smi_addr = MT753X_DFL_SMI_ADDR;
+
+	/* Get LAN/WAN port mapping */
+	map = mt753x_find_mapping(np);
+	if (map) {
+		mt753x_apply_mapping(gsw, map);
+		gsw->global_vlan_enable = 1;
+		dev_info(gsw->dev, "LAN/WAN VLAN setting=%s\n", map->name);
+	}
+
+	/* Load MAC port configurations */
+	mt753x_load_port_cfg(gsw);
+
+	/* Check for valid switch and then initialize */
+	for (i = 0; i < ARRAY_SIZE(mt753x_sw_ids); i++) {
+		if (!mt753x_sw_ids[i]->detect(gsw, &rev)) {
+			sw = mt753x_sw_ids[i];
+
+			gsw->name = rev.name;
+			gsw->model = sw->model;
+
+			dev_info(gsw->dev, "Switch is MediaTek %s rev %d",
+				 gsw->name, rev.rev);
+
+			/* Initialize the switch */
+			ret = sw->init(gsw);
+			if (ret)
+				goto fail;
+
+			break;
+		}
+	}
+
+	if (i >= ARRAY_SIZE(mt753x_sw_ids)) {
+		dev_err(gsw->dev, "No mt753x switch found\n");
+		goto fail;
+	}
+
+	gsw->irq = platform_get_irq(pdev, 0);
+	if (gsw->irq >= 0) {
+		ret = devm_request_irq(gsw->dev, gsw->irq, mt753x_irq_handler,
+				       0, dev_name(gsw->dev), gsw);
+		if (ret) {
+			dev_err(gsw->dev, "Failed to request irq %d\n",
+				gsw->irq);
+			goto fail;
+		}
+
+		INIT_WORK(&gsw->irq_worker, mt753x_irq_worker);
+	}
+
+	platform_set_drvdata(pdev, gsw);
+
+	gsw->phy_status_poll = of_property_read_bool(gsw->dev->of_node,
+						     "mediatek,phy-poll");
+
+	mt753x_add_gsw(gsw);
+#if 1 //XDXD
+	mt753x_mdio_register(gsw);
+#endif
+
+	mt753x_swconfig_init(gsw);
+
+	if (sw->post_init)
+		sw->post_init(gsw);
+
+	if (gsw->irq >= 0)
+		mt753x_irq_enable(gsw);
+
+	return 0;
+
+fail:
+	devm_kfree(&pdev->dev, gsw);
+
+	return ret;
+}
+
+static int mt753x_remove(struct platform_device *pdev)
+{
+	struct gsw_mt753x *gsw = platform_get_drvdata(pdev);
+
+	if (gsw->irq >= 0)
+		cancel_work_sync(&gsw->irq_worker);
+
+	if (gsw->reset_pin >= 0)
+		devm_gpio_free(&pdev->dev, gsw->reset_pin);
+
+#ifdef CONFIG_SWCONFIG
+	mt753x_swconfig_destroy(gsw);
+#endif
+
+#if 1 //XDXD
+	mt753x_disconnect_internal_phys(gsw);
+
+	mdiobus_unregister(gsw->gphy_bus);
+#endif
+
+	mt753x_remove_gsw(gsw);
+
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static const struct of_device_id mt753x_ids[] = {
+	{ .compatible = "mediatek,mt753x" },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(of, mt753x_ids);
+
+static struct platform_driver mt753x_driver = {
+	.probe = mt753x_probe,
+	.remove = mt753x_remove,
+	.driver = {
+		.name = "mt753x",
+		.of_match_table = mt753x_ids,
+	},
+};
+
+static int __init mt753x_init(void)
+{
+	int ret;
+
+	INIT_LIST_HEAD(&mt753x_devs);
+	ret = platform_driver_register(&mt753x_driver);
+
+	mt753x_nl_init();
+
+	return ret;
+}
+module_init(mt753x_init);
+
+static void __exit mt753x_exit(void)
+{
+	mt753x_nl_exit();
+
+	platform_driver_unregister(&mt753x_driver);
+}
+module_exit(mt753x_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Weijie Gao <weijie.gao@mediatek.com>");
+MODULE_DESCRIPTION("Driver for MediaTek MT753x Gigabit Switch");
diff --git a/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt753x_nl.c b/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt753x_nl.c
new file mode 100755
index 0000000..a04c701
--- /dev/null
+++ b/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt753x_nl.c
@@ -0,0 +1,381 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2018 MediaTek Inc.
+ * Author: Sirui Zhao <Sirui.Zhao@mediatek.com>
+ */
+
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <net/genetlink.h>
+
+#include "mt753x.h"
+#include "mt753x_nl.h"
+
+struct mt753x_nl_cmd_item {
+	enum mt753x_cmd cmd;
+	bool require_dev;
+	int (*process)(struct genl_info *info, struct gsw_mt753x *gsw);
+	u32 nr_required_attrs;
+	const enum mt753x_attr *required_attrs;
+};
+
+static int mt753x_nl_response(struct sk_buff *skb, struct genl_info *info);
+
+static const struct nla_policy mt753x_nl_cmd_policy[] = {
+	[MT753X_ATTR_TYPE_MESG] = { .type = NLA_STRING },
+	[MT753X_ATTR_TYPE_PHY] = { .type = NLA_S32 },
+	[MT753X_ATTR_TYPE_REG] = { .type = NLA_S32 },
+	[MT753X_ATTR_TYPE_VAL] = { .type = NLA_S32 },
+	[MT753X_ATTR_TYPE_DEV_NAME] = { .type = NLA_S32 },
+	[MT753X_ATTR_TYPE_DEV_ID] = { .type = NLA_S32 },
+	[MT753X_ATTR_TYPE_DEVAD] = { .type = NLA_S32 },
+};
+
+static const struct genl_ops mt753x_nl_ops[] = {
+	{
+		.cmd = MT753X_CMD_REQUEST,
+		.doit = mt753x_nl_response,
+//		.policy = mt753x_nl_cmd_policy,
+		.flags = GENL_ADMIN_PERM,
+	}, {
+		.cmd = MT753X_CMD_READ,
+		.doit = mt753x_nl_response,
+//		.policy = mt753x_nl_cmd_policy,
+		.flags = GENL_ADMIN_PERM,
+	}, {
+		.cmd = MT753X_CMD_WRITE,
+		.doit = mt753x_nl_response,
+//		.policy = mt753x_nl_cmd_policy,
+		.flags = GENL_ADMIN_PERM,
+	},
+};
+
+static struct genl_family mt753x_nl_family = {
+	.name =		MT753X_GENL_NAME,
+	.version =	MT753X_GENL_VERSION,
+	.maxattr =	MT753X_NR_ATTR_TYPE,
+	.ops =		mt753x_nl_ops,
+	.n_ops =	ARRAY_SIZE(mt753x_nl_ops),
+	.policy =	mt753x_nl_cmd_policy,
+};
+
+static int mt753x_nl_list_devs(char *buff, int size)
+{
+	struct gsw_mt753x *gsw;
+	int len, total = 0;
+	char buf[80];
+
+	memset(buff, 0, size);
+
+	mt753x_lock_gsw();
+
+	list_for_each_entry(gsw, &mt753x_devs, list) {
+		len = snprintf(buf, sizeof(buf),
+			       "id: %d, model: %s, node: %s\n",
+			       gsw->id, gsw->name, gsw->dev->of_node->name);
+		strncat(buff, buf, size - total);
+		total += len;
+	}
+
+	mt753x_put_gsw();
+
+	return total;
+}
+
+static int mt753x_nl_prepare_reply(struct genl_info *info, u8 cmd,
+				   struct sk_buff **skbp)
+{
+	struct sk_buff *msg;
+	void *reply;
+
+	if (!info)
+		return -EINVAL;
+
+	msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
+
+	/* Construct send-back message header */
+	reply = genlmsg_put(msg, info->snd_portid, info->snd_seq,
+			    &mt753x_nl_family, 0, cmd);
+	if (!reply) {
+		nlmsg_free(msg);
+		return -EINVAL;
+	}
+
+	*skbp = msg;
+	return 0;
+}
+
+static int mt753x_nl_send_reply(struct sk_buff *skb, struct genl_info *info)
+{
+	struct genlmsghdr *genlhdr = nlmsg_data(nlmsg_hdr(skb));
+	void *reply = genlmsg_data(genlhdr);
+
+	/* Finalize a generic netlink message (update message header) */
+	genlmsg_end(skb, reply);
+
+	/* reply to a request */
+	return genlmsg_reply(skb, info);
+}
+
+static s32 mt753x_nl_get_s32(struct genl_info *info, enum mt753x_attr attr,
+			     s32 defval)
+{
+	struct nlattr *na;
+
+	na = info->attrs[attr];
+	if (na)
+		return nla_get_s32(na);
+
+	return defval;
+}
+
+static int mt753x_nl_get_u32(struct genl_info *info, enum mt753x_attr attr,
+			     u32 *val)
+{
+	struct nlattr *na;
+
+	na = info->attrs[attr];
+	if (na) {
+		*val = nla_get_u32(na);
+		return 0;
+	}
+
+	return -1;
+}
+
+static struct gsw_mt753x *mt753x_nl_parse_find_gsw(struct genl_info *info)
+{
+	struct gsw_mt753x *gsw;
+	struct nlattr *na;
+	int gsw_id;
+
+	na = info->attrs[MT753X_ATTR_TYPE_DEV_ID];
+	if (na) {
+		gsw_id = nla_get_s32(na);
+		if (gsw_id >= 0)
+			gsw = mt753x_get_gsw(gsw_id);
+		else
+			gsw = mt753x_get_first_gsw();
+	} else {
+		gsw = mt753x_get_first_gsw();
+	}
+
+	return gsw;
+}
+
+static int mt753x_nl_get_swdevs(struct genl_info *info, struct gsw_mt753x *gsw)
+{
+	struct sk_buff *rep_skb = NULL;
+	char dev_info[512];
+	int ret;
+
+	ret = mt753x_nl_list_devs(dev_info, sizeof(dev_info));
+	if (!ret) {
+		pr_info("No switch registered\n");
+		return -EINVAL;
+	}
+
+	ret = mt753x_nl_prepare_reply(info, MT753X_CMD_REPLY, &rep_skb);
+	if (ret < 0)
+		goto err;
+
+	ret = nla_put_string(rep_skb, MT753X_ATTR_TYPE_MESG, dev_info);
+	if (ret < 0)
+		goto err;
+
+	return mt753x_nl_send_reply(rep_skb, info);
+
+err:
+	if (rep_skb)
+		nlmsg_free(rep_skb);
+
+	return ret;
+}
+
+static int mt753x_nl_reply_read(struct genl_info *info, struct gsw_mt753x *gsw)
+{
+	struct sk_buff *rep_skb = NULL;
+	s32 phy, devad, reg;
+	int value;
+	int ret = 0;
+
+	phy = mt753x_nl_get_s32(info, MT753X_ATTR_TYPE_PHY, -1);
+	devad = mt753x_nl_get_s32(info, MT753X_ATTR_TYPE_DEVAD, -1);
+	reg = mt753x_nl_get_s32(info, MT753X_ATTR_TYPE_REG, -1);
+
+	if (reg < 0)
+		goto err;
+
+	ret = mt753x_nl_prepare_reply(info, MT753X_CMD_READ, &rep_skb);
+	if (ret < 0)
+		goto err;
+
+	if (phy >= 0) {
+		if (devad < 0)
+			value = gsw->mii_read(gsw, phy, reg);
+		else
+			value = gsw->mmd_read(gsw, phy, devad, reg);
+	} else {
+		value = mt753x_reg_read(gsw, reg);
+	}
+
+	ret = nla_put_s32(rep_skb, MT753X_ATTR_TYPE_REG, reg);
+	if (ret < 0)
+		goto err;
+
+	ret = nla_put_s32(rep_skb, MT753X_ATTR_TYPE_VAL, value);
+	if (ret < 0)
+		goto err;
+
+	return mt753x_nl_send_reply(rep_skb, info);
+
+err:
+	if (rep_skb)
+		nlmsg_free(rep_skb);
+
+	return ret;
+}
+
+static int mt753x_nl_reply_write(struct genl_info *info, struct gsw_mt753x *gsw)
+{
+	struct sk_buff *rep_skb = NULL;
+	s32 phy, devad, reg;
+	u32 value;
+	int ret = 0;
+
+	phy = mt753x_nl_get_s32(info, MT753X_ATTR_TYPE_PHY, -1);
+	devad = mt753x_nl_get_s32(info, MT753X_ATTR_TYPE_DEVAD, -1);
+	reg = mt753x_nl_get_s32(info, MT753X_ATTR_TYPE_REG, -1);
+
+	if (mt753x_nl_get_u32(info, MT753X_ATTR_TYPE_VAL, &value))
+		goto err;
+
+	if (reg < 0)
+		goto err;
+
+	ret = mt753x_nl_prepare_reply(info, MT753X_CMD_WRITE, &rep_skb);
+	if (ret < 0)
+		goto err;
+
+	if (phy >= 0) {
+		if (devad < 0)
+			gsw->mii_write(gsw, phy, reg, value);
+		else
+			gsw->mmd_write(gsw, phy, devad, reg, value);
+	} else {
+		mt753x_reg_write(gsw, reg, value);
+	}
+
+	ret = nla_put_s32(rep_skb, MT753X_ATTR_TYPE_REG, reg);
+	if (ret < 0)
+		goto err;
+
+	ret = nla_put_s32(rep_skb, MT753X_ATTR_TYPE_VAL, value);
+	if (ret < 0)
+		goto err;
+
+	return mt753x_nl_send_reply(rep_skb, info);
+
+err:
+	if (rep_skb)
+		nlmsg_free(rep_skb);
+
+	return ret;
+}
+
+static const enum mt753x_attr mt753x_nl_cmd_read_attrs[] = {
+	MT753X_ATTR_TYPE_REG
+};
+
+static const enum mt753x_attr mt753x_nl_cmd_write_attrs[] = {
+	MT753X_ATTR_TYPE_REG,
+	MT753X_ATTR_TYPE_VAL
+};
+
+static const struct mt753x_nl_cmd_item mt753x_nl_cmds[] = {
+	{
+		.cmd = MT753X_CMD_REQUEST,
+		.require_dev = false,
+		.process = mt753x_nl_get_swdevs
+	}, {
+		.cmd = MT753X_CMD_READ,
+		.require_dev = true,
+		.process = mt753x_nl_reply_read,
+		.required_attrs = mt753x_nl_cmd_read_attrs,
+		.nr_required_attrs = ARRAY_SIZE(mt753x_nl_cmd_read_attrs),
+	}, {
+		.cmd = MT753X_CMD_WRITE,
+		.require_dev = true,
+		.process = mt753x_nl_reply_write,
+		.required_attrs = mt753x_nl_cmd_write_attrs,
+		.nr_required_attrs = ARRAY_SIZE(mt753x_nl_cmd_write_attrs),
+	}
+};
+
+static int mt753x_nl_response(struct sk_buff *skb, struct genl_info *info)
+{
+	struct genlmsghdr *hdr = nlmsg_data(info->nlhdr);
+	const struct mt753x_nl_cmd_item *cmditem = NULL;
+	struct gsw_mt753x *gsw = NULL;
+	u32 sat_req_attrs = 0;
+	int i, ret;
+
+	for (i = 0; i < ARRAY_SIZE(mt753x_nl_cmds); i++) {
+		if (hdr->cmd == mt753x_nl_cmds[i].cmd) {
+			cmditem = &mt753x_nl_cmds[i];
+			break;
+		}
+	}
+
+	if (!cmditem) {
+		pr_info("mt753x-nl: unknown cmd %u\n", hdr->cmd);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < cmditem->nr_required_attrs; i++) {
+		if (info->attrs[cmditem->required_attrs[i]])
+			sat_req_attrs++;
+	}
+
+	if (sat_req_attrs != cmditem->nr_required_attrs) {
+		pr_info("mt753x-nl: missing required attr(s) for cmd %u\n",
+			hdr->cmd);
+		return -EINVAL;
+	}
+
+	if (cmditem->require_dev) {
+		gsw = mt753x_nl_parse_find_gsw(info);
+		if (!gsw) {
+			pr_info("mt753x-nl: failed to find switch dev\n");
+			return -EINVAL;
+		}
+	}
+
+	ret = cmditem->process(info, gsw);
+
+	mt753x_put_gsw();
+
+	return ret;
+}
+
+int __init mt753x_nl_init(void)
+{
+	int ret;
+
+	ret = genl_register_family(&mt753x_nl_family);
+	if (ret) {
+		pr_info("mt753x-nl: genl_register_family_with_ops failed\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+void __exit mt753x_nl_exit(void)
+{
+	genl_unregister_family(&mt753x_nl_family);
+}
diff --git a/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt753x_regs.h b/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt753x_regs.h
new file mode 100755
index 0000000..1784873
--- /dev/null
+++ b/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt753x_regs.h
@@ -0,0 +1,344 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2018 MediaTek Inc.
+ * Author: Weijie Gao <weijie.gao@mediatek.com>
+ */
+
+#ifndef _MT753X_REGS_H_
+#define _MT753X_REGS_H_
+
+#include <linux/bitops.h>
+
+/* Values of Egress TAG Control */
+#define ETAG_CTRL_UNTAG			0
+#define ETAG_CTRL_TAG			2
+#define ETAG_CTRL_SWAP			1
+#define ETAG_CTRL_STACK			3
+
+#define VTCR				0x90
+#define VAWD1				0x94
+#define VAWD2				0x98
+
+/* Fields of VTCR */
+#define VTCR_BUSY			BIT(31)
+#define IDX_INVLD			BIT(16)
+#define VTCR_FUNC_S			12
+#define VTCR_FUNC_M			0xf000
+#define VTCR_VID_S			0
+#define VTCR_VID_M			0xfff
+
+/* Values of VTCR_FUNC */
+#define VTCR_READ_VLAN_ENTRY		0
+#define VTCR_WRITE_VLAN_ENTRY		1
+#define VTCR_INVD_VLAN_ENTRY		2
+#define VTCR_ENABLE_VLAN_ENTRY		3
+#define VTCR_READ_ACL_ENTRY		4
+#define VTCR_WRITE_ACL_ENTRY		5
+#define VTCR_READ_TRTCM_TABLE		6
+#define VTCR_WRITE_TRTCM_TABLE		7
+#define VTCR_READ_ACL_MASK_ENTRY	8
+#define VTCR_WRITE_ACL_MASK_ENTRY	9
+#define VTCR_READ_ACL_RULE_ENTRY	10
+#define VTCR_WRITE_ACL_RULE_ENTRY	11
+#define VTCR_READ_ACL_RATE_ENTRY	12
+#define VTCR_WRITE_ACL_RATE_ENTRY	13
+
+/* VLAN entry fields */
+/* VAWD1 */
+#define PORT_STAG			BIT(31)
+#define IVL_MAC				BIT(30)
+#define EG_CON				BIT(29)
+#define VTAG_EN				BIT(28)
+#define COPY_PRI			BIT(27)
+#define USER_PRI_S			24
+#define USER_PRI_M			0x7000000
+#define PORT_MEM_S			16
+#define PORT_MEM_M			0xff0000
+#define S_TAG1_S			4
+#define S_TAG1_M			0xfff0
+#define FID_S				1
+#define FID_M				0x0e
+#define VENTRY_VALID			BIT(0)
+
+/* VAWD2 */
+#define S_TAG2_S			16
+#define S_TAG2_M			0xffff0000
+#define PORT_ETAG_S(p)			((p) * 2)
+#define PORT_ETAG_M			0x03
+
+#define PORT_CTRL_BASE			0x2000
+#define PORT_CTRL_PORT_OFFSET		0x100
+#define PORT_CTRL_REG(p, r)		(PORT_CTRL_BASE + \
+					(p) * PORT_CTRL_PORT_OFFSET +  (r))
+#define CKGCR(p)			PORT_CTRL_REG(p, 0x00)
+#define PCR(p)				PORT_CTRL_REG(p, 0x04)
+#define PIC(p)				PORT_CTRL_REG(p, 0x08)
+#define PSC(p)				PORT_CTRL_REG(p, 0x0c)
+#define PVC(p)				PORT_CTRL_REG(p, 0x10)
+#define PPBV1(p)			PORT_CTRL_REG(p, 0x14)
+#define PPBV2(p)			PORT_CTRL_REG(p, 0x18)
+#define BSR(p)				PORT_CTRL_REG(p, 0x1c)
+#define STAG01				PORT_CTRL_REG(p, 0x20)
+#define STAG23				PORT_CTRL_REG(p, 0x24)
+#define STAG45				PORT_CTRL_REG(p, 0x28)
+#define STAG67				PORT_CTRL_REG(p, 0x2c)
+
+#define PPBV(p, g)			(PPBV1(p) + ((g) / 2) * 4)
+
+/* Fields of PCR */
+#define MLDV2_EN			BIT(30)
+#define EG_TAG_S			28
+#define EG_TAG_M			0x30000000
+#define PORT_PRI_S			24
+#define PORT_PRI_M			0x7000000
+#define PORT_MATRIX_S			16
+#define PORT_MATRIX_M			0xff0000
+#define UP2DSCP_EN			BIT(12)
+#define UP2TAG_EN			BIT(11)
+#define ACL_EN				BIT(10)
+#define PORT_TX_MIR			BIT(9)
+#define PORT_RX_MIR			BIT(8)
+#define ACL_MIR				BIT(7)
+#define MIS_PORT_FW_S			4
+#define MIS_PORT_FW_M			0x70
+#define VLAN_MIS			BIT(2)
+#define PORT_VLAN_S			0
+#define PORT_VLAN_M			0x03
+
+/* Values of PORT_VLAN */
+#define PORT_MATRIX_MODE		0
+#define FALLBACK_MODE			1
+#define CHECK_MODE			2
+#define SECURITY_MODE			3
+
+/* Fields of PVC */
+#define STAG_VPID_S			16
+#define STAG_VPID_M			0xffff0000
+#define DIS_PVID			BIT(15)
+#define FORCE_PVID			BIT(14)
+#define PT_VPM				BIT(12)
+#define PT_OPTION			BIT(11)
+#define PVC_EG_TAG_S			8
+#define PVC_EG_TAG_M			0x700
+#define VLAN_ATTR_S			6
+#define VLAN_ATTR_M			0xc0
+#define PVC_PORT_STAG			BIT(5)
+#define BC_LKYV_EN			BIT(4)
+#define MC_LKYV_EN			BIT(3)
+#define UC_LKYV_EN			BIT(2)
+#define ACC_FRM_S			0
+#define ACC_FRM_M			0x03
+
+/* Values of VLAN_ATTR */
+#define VA_USER_PORT			0
+#define VA_STACK_PORT			1
+#define VA_TRANSLATION_PORT		2
+#define VA_TRANSPARENT_PORT		3
+
+/* Fields of PPBV */
+#define GRP_PORT_PRI_S(g)		(((g) % 2) * 16 + 13)
+#define GRP_PORT_PRI_M			0x07
+#define GRP_PORT_VID_S(g)		(((g) % 2) * 16)
+#define GRP_PORT_VID_M			0xfff
+
+#define PORT_MAC_CTRL_BASE		0x3000
+#define PORT_MAC_CTRL_PORT_OFFSET	0x100
+#define PORT_MAC_CTRL_REG(p, r)		(PORT_MAC_CTRL_BASE + \
+					(p) * PORT_MAC_CTRL_PORT_OFFSET + (r))
+#define PMCR(p)				PORT_MAC_CTRL_REG(p, 0x00)
+#define PMEEECR(p)			PORT_MAC_CTRL_REG(p, 0x04)
+#define PMSR(p)				PORT_MAC_CTRL_REG(p, 0x08)
+#define PINT_EN(p)			PORT_MAC_CTRL_REG(p, 0x10)
+#define PINT_STS(p)			PORT_MAC_CTRL_REG(p, 0x14)
+
+#define GMACCR				(PORT_MAC_CTRL_BASE + 0xe0)
+#define TXCRC_EN			BIT(19)
+#define RXCRC_EN			BIT(18)
+#define PRMBL_LMT_EN			BIT(17)
+#define MTCC_LMT_S			9
+#define MTCC_LMT_M			0x1e00
+#define MAX_RX_JUMBO_S			2
+#define MAX_RX_JUMBO_M			0x3c
+#define MAX_RX_PKT_LEN_S		0
+#define MAX_RX_PKT_LEN_M		0x3
+
+/* Values of MAX_RX_PKT_LEN */
+#define RX_PKT_LEN_1518			0
+#define RX_PKT_LEN_1536			1
+#define RX_PKT_LEN_1522			2
+#define RX_PKT_LEN_MAX_JUMBO		3
+
+/* Fields of PMCR */
+#define IPG_CFG_S			18
+#define IPG_CFG_M			0xc0000
+#define EXT_PHY				BIT(17)
+#define MAC_MODE			BIT(16)
+#define MAC_TX_EN			BIT(14)
+#define MAC_RX_EN			BIT(13)
+#define MAC_PRE				BIT(11)
+#define BKOFF_EN			BIT(9)
+#define BACKPR_EN			BIT(8)
+#define FORCE_EEE1G			BIT(7)
+#define FORCE_EEE1000			BIT(6)
+#define FORCE_RX_FC			BIT(5)
+#define FORCE_TX_FC			BIT(4)
+#define FORCE_SPD_S			2
+#define FORCE_SPD_M			0x0c
+#define FORCE_DPX			BIT(1)
+#define FORCE_LINK			BIT(0)
+
+/* Fields of PMSR */
+#define EEE1G_STS			BIT(7)
+#define EEE100_STS			BIT(6)
+#define RX_FC_STS			BIT(5)
+#define TX_FC_STS			BIT(4)
+#define MAC_SPD_STS_S			2
+#define MAC_SPD_STS_M			0x0c
+#define MAC_DPX_STS			BIT(1)
+#define MAC_LNK_STS			BIT(0)
+
+/* Values of MAC_SPD_STS */
+#define MAC_SPD_10			0
+#define MAC_SPD_100			1
+#define MAC_SPD_1000			2
+#define MAC_SPD_2500			3
+
+/* Values of IPG_CFG */
+#define IPG_96BIT			0
+#define IPG_96BIT_WITH_SHORT_IPG	1
+#define IPG_64BIT			2
+
+#define MIB_COUNTER_BASE		0x4000
+#define MIB_COUNTER_PORT_OFFSET		0x100
+#define MIB_COUNTER_REG(p, r)		(MIB_COUNTER_BASE + \
+					(p) * MIB_COUNTER_PORT_OFFSET + (r))
+#define STATS_TDPC			0x00
+#define STATS_TCRC			0x04
+#define STATS_TUPC			0x08
+#define STATS_TMPC			0x0C
+#define STATS_TBPC			0x10
+#define STATS_TCEC			0x14
+#define STATS_TSCEC			0x18
+#define STATS_TMCEC			0x1C
+#define STATS_TDEC			0x20
+#define STATS_TLCEC			0x24
+#define STATS_TXCEC			0x28
+#define STATS_TPPC			0x2C
+#define STATS_TL64PC			0x30
+#define STATS_TL65PC			0x34
+#define STATS_TL128PC			0x38
+#define STATS_TL256PC			0x3C
+#define STATS_TL512PC			0x40
+#define STATS_TL1024PC			0x44
+#define STATS_TOC			0x48
+#define STATS_RDPC			0x60
+#define STATS_RFPC			0x64
+#define STATS_RUPC			0x68
+#define STATS_RMPC			0x6C
+#define STATS_RBPC			0x70
+#define STATS_RAEPC			0x74
+#define STATS_RCEPC			0x78
+#define STATS_RUSPC			0x7C
+#define STATS_RFEPC			0x80
+#define STATS_ROSPC			0x84
+#define STATS_RJEPC			0x88
+#define STATS_RPPC			0x8C
+#define STATS_RL64PC			0x90
+#define STATS_RL65PC			0x94
+#define STATS_RL128PC			0x98
+#define STATS_RL256PC			0x9C
+#define STATS_RL512PC			0xA0
+#define STATS_RL1024PC			0xA4
+#define STATS_ROC			0xA8
+#define STATS_RDPC_CTRL			0xB0
+#define STATS_RDPC_ING			0xB4
+#define STATS_RDPC_ARL			0xB8
+
+#define SYS_CTRL			0x7000
+#define SW_PHY_RST			BIT(2)
+#define SW_SYS_RST			BIT(1)
+#define SW_REG_RST			BIT(0)
+
+#define SYS_INT_EN			0x7008
+#define SYS_INT_STS			0x700c
+#define MAC_PC_INT			BIT(16)
+#define PHY_INT(p)			BIT((p) + 8)
+#define PHY_LC_INT(p)			BIT(p)
+
+#define PHY_IAC				0x701c
+#define PHY_ACS_ST			BIT(31)
+#define MDIO_REG_ADDR_S			25
+#define MDIO_REG_ADDR_M			0x3e000000
+#define MDIO_PHY_ADDR_S			20
+#define MDIO_PHY_ADDR_M			0x1f00000
+#define MDIO_CMD_S			18
+#define MDIO_CMD_M			0xc0000
+#define MDIO_ST_S			16
+#define MDIO_ST_M			0x30000
+#define MDIO_RW_DATA_S			0
+#define MDIO_RW_DATA_M			0xffff
+
+/* MDIO_CMD: MDIO commands */
+#define MDIO_CMD_ADDR			0
+#define MDIO_CMD_WRITE			1
+#define MDIO_CMD_READ			2
+#define MDIO_CMD_READ_C45		3
+
+/* MDIO_ST: MDIO start field */
+#define MDIO_ST_C45			0
+#define MDIO_ST_C22			1
+
+#define HWSTRAP				0x7800
+#define MHWSTRAP			0x7804
+
+/* Internal GPHY Page Control Register */
+#define PHY_CL22_PAGE_CTRL		0x1f
+#define PHY_TR_PAGE			0x52b5
+
+/* Internal GPHY Token Ring Access Registers */
+#define PHY_TR_CTRL			0x10
+#define PHY_TR_LOW_DATA			0x11
+#define PHY_TR_HIGH_DATA		0x12
+
+/* Fields of PHY_TR_CTRL */
+#define PHY_TR_PKT_XMT_STA		BIT(15)
+#define PHY_TR_WR_S			13
+#define PHY_TR_CH_ADDR_S		11
+#define PHY_TR_NODE_ADDR_S		7
+#define PHY_TR_DATA_ADDR_S		1
+
+enum phy_tr_wr {
+	PHY_TR_WRITE = 0,
+	PHY_TR_READ = 1,
+};
+
+/* Helper macro for GPHY Token Ring Access */
+#define PHY_TR_LOW_VAL(x)		((x) & 0xffff)
+#define PHY_TR_HIGH_VAL(x)		(((x) & 0xff0000) >> 16)
+
+/* Token Ring Channels */
+#define PMA_CH				0x1
+#define DSP_CH				0x2
+
+/* Token Ring Nodes */
+#define PMA_NOD				0xf
+#define DSP_NOD				0xd
+
+/* Token Ring register range */
+enum tr_pma_reg_addr {
+	PMA_MIN = 0x0,
+	PMA_01  = 0x1,
+	PMA_17  = 0x17,
+	PMA_18  = 0x18,
+	PMA_MAX = 0x3d,
+};
+
+enum tr_dsp_reg_addr {
+	DSP_MIN = 0x0,
+	DSP_06  = 0x6,
+	DSP_08  = 0x8,
+	DSP_0f  = 0xf,
+	DSP_10  = 0x10,
+	DSP_MAX = 0x3e,
+};
+#endif /* _MT753X_REGS_H_ */
diff --git a/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt753x_swconfig.c b/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt753x_swconfig.c
new file mode 100755
index 0000000..7a05952
--- /dev/null
+++ b/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt753x_swconfig.c
@@ -0,0 +1,517 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2018 MediaTek Inc.
+ * Author: Weijie Gao <weijie.gao@mediatek.com>
+ */
+
+#include <linux/if.h>
+#include <linux/list.h>
+#include <linux/if_ether.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/netlink.h>
+#include <linux/bitops.h>
+#include <net/genetlink.h>
+#include <linux/delay.h>
+#include <linux/phy.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/lockdep.h>
+#include <linux/workqueue.h>
+#include <linux/of_device.h>
+
+#include "mt753x.h"
+#include "mt753x_swconfig.h"
+#include "mt753x_regs.h"
+
+#define MT753X_PORT_MIB_TXB_ID	18	/* TxByte */
+#define MT753X_PORT_MIB_RXB_ID	37	/* RxByte */
+
+#define MIB_DESC(_s, _o, _n)   \
+	{                       \
+		.size = (_s),   \
+		.offset = (_o), \
+		.name = (_n),   \
+	}
+
+struct mt753x_mib_desc {
+	unsigned int size;
+	unsigned int offset;
+	const char *name;
+};
+
+static const struct mt753x_mib_desc mt753x_mibs[] = {
+	MIB_DESC(1, STATS_TDPC, "TxDrop"),
+	MIB_DESC(1, STATS_TCRC, "TxCRC"),
+	MIB_DESC(1, STATS_TUPC, "TxUni"),
+	MIB_DESC(1, STATS_TMPC, "TxMulti"),
+	MIB_DESC(1, STATS_TBPC, "TxBroad"),
+	MIB_DESC(1, STATS_TCEC, "TxCollision"),
+	MIB_DESC(1, STATS_TSCEC, "TxSingleCol"),
+	MIB_DESC(1, STATS_TMCEC, "TxMultiCol"),
+	MIB_DESC(1, STATS_TDEC, "TxDefer"),
+	MIB_DESC(1, STATS_TLCEC, "TxLateCol"),
+	MIB_DESC(1, STATS_TXCEC, "TxExcCol"),
+	MIB_DESC(1, STATS_TPPC, "TxPause"),
+	MIB_DESC(1, STATS_TL64PC, "Tx64Byte"),
+	MIB_DESC(1, STATS_TL65PC, "Tx65Byte"),
+	MIB_DESC(1, STATS_TL128PC, "Tx128Byte"),
+	MIB_DESC(1, STATS_TL256PC, "Tx256Byte"),
+	MIB_DESC(1, STATS_TL512PC, "Tx512Byte"),
+	MIB_DESC(1, STATS_TL1024PC, "Tx1024Byte"),
+	MIB_DESC(2, STATS_TOC, "TxByte"),
+	MIB_DESC(1, STATS_RDPC, "RxDrop"),
+	MIB_DESC(1, STATS_RFPC, "RxFiltered"),
+	MIB_DESC(1, STATS_RUPC, "RxUni"),
+	MIB_DESC(1, STATS_RMPC, "RxMulti"),
+	MIB_DESC(1, STATS_RBPC, "RxBroad"),
+	MIB_DESC(1, STATS_RAEPC, "RxAlignErr"),
+	MIB_DESC(1, STATS_RCEPC, "RxCRC"),
+	MIB_DESC(1, STATS_RUSPC, "RxUnderSize"),
+	MIB_DESC(1, STATS_RFEPC, "RxFragment"),
+	MIB_DESC(1, STATS_ROSPC, "RxOverSize"),
+	MIB_DESC(1, STATS_RJEPC, "RxJabber"),
+	MIB_DESC(1, STATS_RPPC, "RxPause"),
+	MIB_DESC(1, STATS_RL64PC, "Rx64Byte"),
+	MIB_DESC(1, STATS_RL65PC, "Rx65Byte"),
+	MIB_DESC(1, STATS_RL128PC, "Rx128Byte"),
+	MIB_DESC(1, STATS_RL256PC, "Rx256Byte"),
+	MIB_DESC(1, STATS_RL512PC, "Rx512Byte"),
+	MIB_DESC(1, STATS_RL1024PC, "Rx1024Byte"),
+	MIB_DESC(2, STATS_ROC, "RxByte"),
+	MIB_DESC(1, STATS_RDPC_CTRL, "RxCtrlDrop"),
+	MIB_DESC(1, STATS_RDPC_ING, "RxIngDrop"),
+	MIB_DESC(1, STATS_RDPC_ARL, "RxARLDrop")
+};
+
+enum {
+	/* Global attributes. */
+	MT753X_ATTR_ENABLE_VLAN,
+};
+
+static int mt753x_get_vlan_enable(struct switch_dev *dev,
+				  const struct switch_attr *attr,
+				  struct switch_val *val)
+{
+	struct gsw_mt753x *gsw = container_of(dev, struct gsw_mt753x, swdev);
+
+	val->value.i = gsw->global_vlan_enable;
+
+	return 0;
+}
+
+static int mt753x_set_vlan_enable(struct switch_dev *dev,
+				  const struct switch_attr *attr,
+				  struct switch_val *val)
+{
+	struct gsw_mt753x *gsw = container_of(dev, struct gsw_mt753x, swdev);
+
+	gsw->global_vlan_enable = val->value.i != 0;
+
+	return 0;
+}
+
+static int mt753x_get_port_pvid(struct switch_dev *dev, int port, int *val)
+{
+	struct gsw_mt753x *gsw = container_of(dev, struct gsw_mt753x, swdev);
+
+	if (port >= MT753X_NUM_PORTS)
+		return -EINVAL;
+
+	*val = mt753x_reg_read(gsw, PPBV1(port));
+	*val &= GRP_PORT_VID_M;
+
+	return 0;
+}
+
+static int mt753x_set_port_pvid(struct switch_dev *dev, int port, int pvid)
+{
+	struct gsw_mt753x *gsw = container_of(dev, struct gsw_mt753x, swdev);
+
+	if (port >= MT753X_NUM_PORTS)
+		return -EINVAL;
+
+	if (pvid < MT753X_MIN_VID || pvid > MT753X_MAX_VID)
+		return -EINVAL;
+
+	gsw->port_entries[port].pvid = pvid;
+
+	return 0;
+}
+
+static int mt753x_get_vlan_ports(struct switch_dev *dev, struct switch_val *val)
+{
+	struct gsw_mt753x *gsw = container_of(dev, struct gsw_mt753x, swdev);
+	u32 member;
+	u32 etags;
+	int i;
+
+	val->len = 0;
+
+	if (val->port_vlan < 0 || val->port_vlan >= MT753X_NUM_VLANS)
+		return -EINVAL;
+
+	mt753x_vlan_ctrl(gsw, VTCR_READ_VLAN_ENTRY, val->port_vlan);
+
+	member = mt753x_reg_read(gsw, VAWD1);
+	member &= PORT_MEM_M;
+	member >>= PORT_MEM_S;
+
+	etags = mt753x_reg_read(gsw, VAWD2);
+
+	for (i = 0; i < MT753X_NUM_PORTS; i++) {
+		struct switch_port *p;
+		int etag;
+
+		if (!(member & BIT(i)))
+			continue;
+
+		p = &val->value.ports[val->len++];
+		p->id = i;
+
+		etag = (etags >> PORT_ETAG_S(i)) & PORT_ETAG_M;
+
+		if (etag == ETAG_CTRL_TAG)
+			p->flags |= BIT(SWITCH_PORT_FLAG_TAGGED);
+		else if (etag != ETAG_CTRL_UNTAG)
+			dev_info(gsw->dev,
+				 "vlan egress tag control neither untag nor tag.\n");
+	}
+
+	return 0;
+}
+
+static int mt753x_set_vlan_ports(struct switch_dev *dev, struct switch_val *val)
+{
+	struct gsw_mt753x *gsw = container_of(dev, struct gsw_mt753x, swdev);
+	u8 member = 0;
+	u8 etags = 0;
+	int i;
+
+	if (val->port_vlan < 0 || val->port_vlan >= MT753X_NUM_VLANS ||
+	    val->len > MT753X_NUM_PORTS)
+		return -EINVAL;
+
+	for (i = 0; i < val->len; i++) {
+		struct switch_port *p = &val->value.ports[i];
+
+		if (p->id >= MT753X_NUM_PORTS)
+			return -EINVAL;
+
+		member |= BIT(p->id);
+
+		if (p->flags & BIT(SWITCH_PORT_FLAG_TAGGED))
+			etags |= BIT(p->id);
+	}
+
+	gsw->vlan_entries[val->port_vlan].member = member;
+	gsw->vlan_entries[val->port_vlan].etags = etags;
+
+	return 0;
+}
+
+static int mt753x_set_vid(struct switch_dev *dev,
+			  const struct switch_attr *attr,
+			  struct switch_val *val)
+{
+	struct gsw_mt753x *gsw = container_of(dev, struct gsw_mt753x, swdev);
+	int vlan;
+	u16 vid;
+
+	vlan = val->port_vlan;
+	vid = (u16)val->value.i;
+
+	if (vlan < 0 || vlan >= MT753X_NUM_VLANS)
+		return -EINVAL;
+
+	if (vid < MT753X_MIN_VID || vid > MT753X_MAX_VID)
+		return -EINVAL;
+
+	gsw->vlan_entries[vlan].vid = vid;
+	return 0;
+}
+
+static int mt753x_get_vid(struct switch_dev *dev,
+			  const struct switch_attr *attr,
+			  struct switch_val *val)
+{
+	val->value.i = val->port_vlan;
+	return 0;
+}
+
+static int mt753x_get_port_link(struct switch_dev *dev, int port,
+				struct switch_port_link *link)
+{
+	struct gsw_mt753x *gsw = container_of(dev, struct gsw_mt753x, swdev);
+	u32 speed, pmsr;
+
+	if (port < 0 || port >= MT753X_NUM_PORTS)
+		return -EINVAL;
+
+	pmsr = mt753x_reg_read(gsw, PMSR(port));
+
+	link->link = pmsr & MAC_LNK_STS;
+	link->duplex = pmsr & MAC_DPX_STS;
+	speed = (pmsr & MAC_SPD_STS_M) >> MAC_SPD_STS_S;
+
+	switch (speed) {
+	case MAC_SPD_10:
+		link->speed = SWITCH_PORT_SPEED_10;
+		break;
+	case MAC_SPD_100:
+		link->speed = SWITCH_PORT_SPEED_100;
+		break;
+	case MAC_SPD_1000:
+		link->speed = SWITCH_PORT_SPEED_1000;
+		break;
+	case MAC_SPD_2500:
+		/* TODO: swconfig has no support for 2500 now */
+		link->speed = SWITCH_PORT_SPEED_UNKNOWN;
+		break;
+	}
+
+	return 0;
+}
+
+static int mt753x_set_port_link(struct switch_dev *dev, int port,
+				struct switch_port_link *link)
+{
+#ifndef MODULE
+	if (port >= MT753X_NUM_PHYS)
+		return -EINVAL;
+
+	return switch_generic_set_link(dev, port, link);
+#else
+	return -ENOTSUPP;
+#endif
+}
+
+static u64 get_mib_counter(struct gsw_mt753x *gsw, int i, int port)
+{
+	unsigned int offset;
+	u64 lo, hi, hi2;
+
+	offset = mt753x_mibs[i].offset;
+
+	if (mt753x_mibs[i].size == 1)
+		return mt753x_reg_read(gsw, MIB_COUNTER_REG(port, offset));
+
+	do {
+		hi = mt753x_reg_read(gsw, MIB_COUNTER_REG(port, offset + 4));
+		lo = mt753x_reg_read(gsw, MIB_COUNTER_REG(port, offset));
+		hi2 = mt753x_reg_read(gsw, MIB_COUNTER_REG(port, offset + 4));
+	} while (hi2 != hi);
+
+	return (hi << 32) | lo;
+}
+
+static int mt753x_get_port_mib(struct switch_dev *dev,
+			       const struct switch_attr *attr,
+			       struct switch_val *val)
+{
+	static char buf[4096];
+	struct gsw_mt753x *gsw = container_of(dev, struct gsw_mt753x, swdev);
+	int i, len = 0;
+
+	if (val->port_vlan >= MT753X_NUM_PORTS)
+		return -EINVAL;
+
+	len += snprintf(buf + len, sizeof(buf) - len,
+			"Port %d MIB counters\n", val->port_vlan);
+
+	for (i = 0; i < ARRAY_SIZE(mt753x_mibs); ++i) {
+		u64 counter;
+
+		len += snprintf(buf + len, sizeof(buf) - len,
+				"%-11s: ", mt753x_mibs[i].name);
+		counter = get_mib_counter(gsw, i, val->port_vlan);
+		len += snprintf(buf + len, sizeof(buf) - len, "%llu\n",
+				counter);
+	}
+
+	val->value.s = buf;
+	val->len = len;
+	return 0;
+}
+
+static int mt753x_get_port_stats(struct switch_dev *dev, int port,
+				 struct switch_port_stats *stats)
+{
+	struct gsw_mt753x *gsw = container_of(dev, struct gsw_mt753x, swdev);
+
+	if (port < 0 || port >= MT753X_NUM_PORTS)
+		return -EINVAL;
+
+	stats->tx_bytes = get_mib_counter(gsw, MT753X_PORT_MIB_TXB_ID, port);
+	stats->rx_bytes = get_mib_counter(gsw, MT753X_PORT_MIB_RXB_ID, port);
+
+	return 0;
+}
+
+static void mt753x_port_isolation(struct gsw_mt753x *gsw)
+{
+	int i;
+
+	for (i = 0; i < MT753X_NUM_PORTS; i++)
+		mt753x_reg_write(gsw, PCR(i),
+				 BIT(gsw->cpu_port) << PORT_MATRIX_S);
+
+	mt753x_reg_write(gsw, PCR(gsw->cpu_port), PORT_MATRIX_M);
+
+	for (i = 0; i < MT753X_NUM_PORTS; i++) {
+		u32 pvc_mode = 0x8100 << STAG_VPID_S;
+
+		if ((gsw->port5_cfg.stag_on && i == 5) ||
+		    (gsw->port6_cfg.stag_on && i == 6))
+			pvc_mode |= PVC_PORT_STAG;
+		else
+			pvc_mode |= (VA_TRANSPARENT_PORT << VLAN_ATTR_S);
+
+		mt753x_reg_write(gsw, PVC(i), pvc_mode);
+	}
+}
+
+static int mt753x_apply_config(struct switch_dev *dev)
+{
+	struct gsw_mt753x *gsw = container_of(dev, struct gsw_mt753x, swdev);
+
+	if (!gsw->global_vlan_enable) {
+		mt753x_port_isolation(gsw);
+		return 0;
+	}
+
+	mt753x_apply_vlan_config(gsw);
+
+	return 0;
+}
+
+static int mt753x_reset_switch(struct switch_dev *dev)
+{
+	struct gsw_mt753x *gsw = container_of(dev, struct gsw_mt753x, swdev);
+	int i;
+
+	memset(gsw->port_entries, 0, sizeof(gsw->port_entries));
+	memset(gsw->vlan_entries, 0, sizeof(gsw->vlan_entries));
+
+	/* set default vid of each vlan to the same number of vlan, so the vid
+	 * won't need be set explicitly.
+	 */
+	for (i = 0; i < MT753X_NUM_VLANS; i++)
+		gsw->vlan_entries[i].vid = i;
+
+	return 0;
+}
+
+static int mt753x_phy_read16(struct switch_dev *dev, int addr, u8 reg,
+			     u16 *value)
+{
+	struct gsw_mt753x *gsw = container_of(dev, struct gsw_mt753x, swdev);
+
+	*value = gsw->mii_read(gsw, addr, reg);
+
+	return 0;
+}
+
+static int mt753x_phy_write16(struct switch_dev *dev, int addr, u8 reg,
+			      u16 value)
+{
+	struct gsw_mt753x *gsw = container_of(dev, struct gsw_mt753x, swdev);
+
+	gsw->mii_write(gsw, addr, reg, value);
+
+	return 0;
+}
+
+static const struct switch_attr mt753x_global[] = {
+	{
+		.type = SWITCH_TYPE_INT,
+		.name = "enable_vlan",
+		.description = "VLAN mode (1:enabled)",
+		.max = 1,
+		.id = MT753X_ATTR_ENABLE_VLAN,
+		.get = mt753x_get_vlan_enable,
+		.set = mt753x_set_vlan_enable,
+	}
+};
+
+static const struct switch_attr mt753x_port[] = {
+	{
+		.type = SWITCH_TYPE_STRING,
+		.name = "mib",
+		.description = "Get MIB counters for port",
+		.get = mt753x_get_port_mib,
+		.set = NULL,
+	},
+};
+
+static const struct switch_attr mt753x_vlan[] = {
+	{
+		.type = SWITCH_TYPE_INT,
+		.name = "vid",
+		.description = "VLAN ID (0-4094)",
+		.set = mt753x_set_vid,
+		.get = mt753x_get_vid,
+		.max = 4094,
+	},
+};
+
+static const struct switch_dev_ops mt753x_swdev_ops = {
+	.attr_global = {
+		.attr = mt753x_global,
+		.n_attr = ARRAY_SIZE(mt753x_global),
+	},
+	.attr_port = {
+		.attr = mt753x_port,
+		.n_attr = ARRAY_SIZE(mt753x_port),
+	},
+	.attr_vlan = {
+		.attr = mt753x_vlan,
+		.n_attr = ARRAY_SIZE(mt753x_vlan),
+	},
+	.get_vlan_ports = mt753x_get_vlan_ports,
+	.set_vlan_ports = mt753x_set_vlan_ports,
+	.get_port_pvid = mt753x_get_port_pvid,
+	.set_port_pvid = mt753x_set_port_pvid,
+	.get_port_link = mt753x_get_port_link,
+	.set_port_link = mt753x_set_port_link,
+	.get_port_stats = mt753x_get_port_stats,
+	.apply_config = mt753x_apply_config,
+	.reset_switch = mt753x_reset_switch,
+	.phy_read16 = mt753x_phy_read16,
+	.phy_write16 = mt753x_phy_write16,
+};
+
+int mt753x_swconfig_init(struct gsw_mt753x *gsw)
+{
+	struct device_node *np = gsw->dev->of_node;
+	struct switch_dev *swdev;
+	int ret;
+
+	if (of_property_read_u32(np, "mediatek,cpuport", &gsw->cpu_port))
+		gsw->cpu_port = MT753X_DFL_CPU_PORT;
+
+	swdev = &gsw->swdev;
+
+	swdev->name = gsw->name;
+	swdev->alias = gsw->name;
+	swdev->cpu_port = gsw->cpu_port;
+	swdev->ports = MT753X_NUM_PORTS;
+	swdev->vlans = MT753X_NUM_VLANS;
+	swdev->ops = &mt753x_swdev_ops;
+
+	ret = register_switch(swdev, NULL);
+	if (ret) {
+		dev_notice(gsw->dev, "Failed to register switch %s\n",
+			   swdev->name);
+		return ret;
+	}
+
+	mt753x_apply_config(swdev);
+
+	return 0;
+}
+
+void mt753x_swconfig_destroy(struct gsw_mt753x *gsw)
+{
+	unregister_switch(&gsw->swdev);
+}
diff --git a/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt753x_vlan.c b/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt753x_vlan.c
new file mode 100755
index 0000000..c806566
--- /dev/null
+++ b/target/linux/mediatek/files-5.4/drivers/net/phy/mtk/mt753x/mt753x_vlan.c
@@ -0,0 +1,187 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2018 MediaTek Inc.
+ */
+
+#include "mt753x.h"
+#include "mt753x_regs.h"
+
+struct mt753x_mapping mt753x_def_mapping[] = {
+	{
+		.name = "llllw",
+		.pvids = { 1, 1, 1, 1, 2, 2, 1 },
+		.members = { 0, 0x4f, 0x30 },
+		.etags = { 0, 0, 0 },
+		.vids = { 0, 1, 2 },
+	}, {
+		.name = "wllll",
+		.pvids = { 2, 1, 1, 1, 1, 2, 1 },
+		.members = { 0, 0x5e, 0x21 },
+		.etags = { 0, 0, 0 },
+		.vids = { 0, 1, 2 },
+	}, {
+		.name = "lwlll",
+		.pvids = { 1, 2, 1, 1, 1, 2, 1 },
+		.members = { 0, 0x5d, 0x22 },
+		.etags = { 0, 0, 0 },
+		.vids = { 0, 1, 2 },
+	},
+};
+
+void mt753x_vlan_ctrl(struct gsw_mt753x *gsw, u32 cmd, u32 val)
+{
+	int i;
+
+	mt753x_reg_write(gsw, VTCR,
+			 VTCR_BUSY | ((cmd << VTCR_FUNC_S) & VTCR_FUNC_M) |
+			 (val & VTCR_VID_M));
+
+	for (i = 0; i < 300; i++) {
+		u32 val = mt753x_reg_read(gsw, VTCR);
+
+		if ((val & VTCR_BUSY) == 0)
+			break;
+
+		usleep_range(1000, 1100);
+	}
+
+	if (i == 300)
+		dev_info(gsw->dev, "vtcr timeout\n");
+}
+
+static void mt753x_write_vlan_entry(struct gsw_mt753x *gsw, int vlan, u16 vid,
+				    u8 ports, u8 etags)
+{
+	int port;
+	u32 val;
+
+	/* vlan port membership */
+	if (ports)
+		mt753x_reg_write(gsw, VAWD1,
+				 IVL_MAC | VTAG_EN | VENTRY_VALID |
+				 ((ports << PORT_MEM_S) & PORT_MEM_M));
+	else
+		mt753x_reg_write(gsw, VAWD1, 0);
+
+	/* egress mode */
+	val = 0;
+	for (port = 0; port < MT753X_NUM_PORTS; port++) {
+		if (etags & BIT(port))
+			val |= ETAG_CTRL_TAG << PORT_ETAG_S(port);
+		else
+			val |= ETAG_CTRL_UNTAG << PORT_ETAG_S(port);
+	}
+	mt753x_reg_write(gsw, VAWD2, val);
+
+	/* write to vlan table */
+	mt753x_vlan_ctrl(gsw, VTCR_WRITE_VLAN_ENTRY, vid);
+}
+
+void mt753x_apply_vlan_config(struct gsw_mt753x *gsw)
+{
+	int i, j;
+	u8 tag_ports;
+	u8 untag_ports;
+
+	/* set all ports as security mode */
+	for (i = 0; i < MT753X_NUM_PORTS; i++)
+		mt753x_reg_write(gsw, PCR(i),
+				 PORT_MATRIX_M | SECURITY_MODE);
+
+	/* check if a port is used in tag/untag vlan egress mode */
+	tag_ports = 0;
+	untag_ports = 0;
+
+	for (i = 0; i < MT753X_NUM_VLANS; i++) {
+		u8 member = gsw->vlan_entries[i].member;
+		u8 etags = gsw->vlan_entries[i].etags;
+
+		if (!member)
+			continue;
+
+		for (j = 0; j < MT753X_NUM_PORTS; j++) {
+			if (!(member & BIT(j)))
+				continue;
+
+			if (etags & BIT(j))
+				tag_ports |= 1u << j;
+			else
+				untag_ports |= 1u << j;
+		}
+	}
+
+	/* set all untag-only ports as transparent and the rest as user port */
+	for (i = 0; i < MT753X_NUM_PORTS; i++) {
+		u32 pvc_mode = 0x8100 << STAG_VPID_S;
+
+		if (untag_ports & BIT(i) && !(tag_ports & BIT(i)))
+			pvc_mode = (0x8100 << STAG_VPID_S) |
+				(VA_TRANSPARENT_PORT << VLAN_ATTR_S);
+
+		if ((gsw->port5_cfg.stag_on && i == 5) ||
+		    (gsw->port6_cfg.stag_on && i == 6))
+			pvc_mode = (0x8100 << STAG_VPID_S) | PVC_PORT_STAG;
+
+		mt753x_reg_write(gsw, PVC(i), pvc_mode);
+	}
+
+	/* first clear the switch vlan table */
+	for (i = 0; i < MT753X_NUM_VLANS; i++)
+		mt753x_write_vlan_entry(gsw, i, i, 0, 0);
+
+	/* now program only vlans with members to avoid
+	 * clobbering remapped entries in later iterations
+	 */
+	for (i = 0; i < MT753X_NUM_VLANS; i++) {
+		u16 vid = gsw->vlan_entries[i].vid;
+		u8 member = gsw->vlan_entries[i].member;
+		u8 etags = gsw->vlan_entries[i].etags;
+
+		if (member)
+			mt753x_write_vlan_entry(gsw, i, vid, member, etags);
+	}
+
+	/* Port Default PVID */
+	for (i = 0; i < MT753X_NUM_PORTS; i++) {
+		int vlan = gsw->port_entries[i].pvid;
+		u16 pvid = 0;
+		u32 val;
+
+		if (vlan < MT753X_NUM_VLANS && gsw->vlan_entries[vlan].member)
+			pvid = gsw->vlan_entries[vlan].vid;
+
+		val = mt753x_reg_read(gsw, PPBV1(i));
+		val &= ~GRP_PORT_VID_M;
+		val |= pvid;
+		mt753x_reg_write(gsw, PPBV1(i), val);
+	}
+}
+
+struct mt753x_mapping *mt753x_find_mapping(struct device_node *np)
+{
+	const char *map;
+	int i;
+
+	if (of_property_read_string(np, "mediatek,portmap", &map))
+		return NULL;
+
+	for (i = 0; i < ARRAY_SIZE(mt753x_def_mapping); i++)
+		if (!strcmp(map, mt753x_def_mapping[i].name))
+			return &mt753x_def_mapping[i];
+
+	return NULL;
+}
+
+void mt753x_apply_mapping(struct gsw_mt753x *gsw, struct mt753x_mapping *map)
+{
+	int i = 0;
+
+	for (i = 0; i < MT753X_NUM_PORTS; i++)
+		gsw->port_entries[i].pvid = map->pvids[i];
+
+	for (i = 0; i < MT753X_NUM_VLANS; i++) {
+		gsw->vlan_entries[i].member = map->members[i];
+		gsw->vlan_entries[i].etags = map->etags[i];
+		gsw->vlan_entries[i].vid = map->vids[i];
+	}
+}