mediatek: backport the latest version of the mt7531 support patches

Fixes unknown unicast flooding issue

Signed-off-by: Felix Fietkau <nbd@nbd.name>

1 files changed, 1510 insertions, 0 deletions

diff --git a/target/linux/mediatek/patches-5.4/0604-net-dsa-mt7530-Add-the-support-of-MT7531-switch.patch b/target/linux/mediatek/patches-5.4/0604-net-dsa-mt7530-Add-the-support-of-MT7531-switch.patch
new file mode 100644
index 0000000000..1e6126eb7f
--- /dev/null
+++ b/target/linux/mediatek/patches-5.4/0604-net-dsa-mt7530-Add-the-support-of-MT7531-switch.patch
@@ -0,0 +1,1510 @@
+From: Landen Chao <landen.chao@mediatek.com>
+Date: Fri, 4 Sep 2020 22:21:59 +0800
+Subject: [PATCH] net: dsa: mt7530: Add the support of MT7531 switch
+
+Add new support for MT7531:
+
+MT7531 is the next generation of MT7530. It is also a 7-ports switch with
+5 giga embedded phys, 2 cpu ports, and the same MAC logic of MT7530. Cpu
+port 6 only supports SGMII interface. Cpu port 5 supports either RGMII
+or SGMII in different HW sku, but cannot be muxed to PHY of port 0/4 like
+mt7530. Due to SGMII interface support, pll, and pad setting are different
+from MT7530. This patch adds different initial setting, and SGMII phylink
+handlers of MT7531.
+
+MT7531 SGMII interface can be configured in following mode:
+- 'SGMII AN mode' with in-band negotiation capability
+    which is compatible with PHY_INTERFACE_MODE_SGMII.
+- 'SGMII force mode' without in-band negotiation
+    which is compatible with 10B/8B encoding of
+    PHY_INTERFACE_MODE_1000BASEX with fixed full-duplex and fixed pause.
+- 2.5 times faster clocked 'SGMII force mode' without in-band negotiation
+    which is compatible with 10B/8B encoding of
+    PHY_INTERFACE_MODE_2500BASEX with fixed full-duplex and fixed pause.
+
+Signed-off-by: Landen Chao <landen.chao@mediatek.com>
+Signed-off-by: Sean Wang <sean.wang@mediatek.com>
+---
+
+--- a/drivers/net/dsa/mt7530.c
++++ b/drivers/net/dsa/mt7530.c
+@@ -234,6 +234,12 @@ mt7530_write(struct mt7530_priv *priv, u
+ }
+ 
+ static u32
++_mt7530_unlocked_read(struct mt7530_dummy_poll *p)
++{
++	return mt7530_mii_read(p->priv, p->reg);
++}
++
++static u32
+ _mt7530_read(struct mt7530_dummy_poll *p)
+ {
+ 	struct mii_bus		*bus = p->priv->bus;
+@@ -483,6 +489,108 @@ mt7530_pad_clk_setup(struct dsa_switch *
+ 	return 0;
+ }
+ 
++static bool mt7531_dual_sgmii_supported(struct mt7530_priv *priv)
++{
++	u32 val;
++
++	val = mt7530_read(priv, MT7531_TOP_SIG_SR);
++
++	return (val & PAD_DUAL_SGMII_EN) != 0;
++}
++
++static int
++mt7531_pad_setup(struct dsa_switch *ds, phy_interface_t interface)
++{
++	struct mt7530_priv *priv = ds->priv;
++	u32 val;
++	u32 top_sig;
++	u32 hwstrap;
++	u32 xtal;
++
++	if (mt7531_dual_sgmii_supported(priv))
++		return 0;
++
++	val = mt7530_read(priv, MT7531_CREV);
++	top_sig = mt7530_read(priv, MT7531_TOP_SIG_SR);
++	hwstrap = mt7530_read(priv, MT7531_HWTRAP);
++	if ((val & CHIP_REV_M) > 0)
++		xtal = (top_sig & PAD_MCM_SMI_EN) ? HWTRAP_XTAL_FSEL_40MHZ :
++						    HWTRAP_XTAL_FSEL_25MHZ;
++	else
++		xtal = hwstrap & HWTRAP_XTAL_FSEL_MASK;
++
++	/* Step 1 : Disable MT7531 COREPLL */
++	val = mt7530_read(priv, MT7531_PLLGP_EN);
++	val &= ~EN_COREPLL;
++	mt7530_write(priv, MT7531_PLLGP_EN, val);
++
++	/* Step 2: switch to XTAL output */
++	val = mt7530_read(priv, MT7531_PLLGP_EN);
++	val |= SW_CLKSW;
++	mt7530_write(priv, MT7531_PLLGP_EN, val);
++
++	val = mt7530_read(priv, MT7531_PLLGP_CR0);
++	val &= ~RG_COREPLL_EN;
++	mt7530_write(priv, MT7531_PLLGP_CR0, val);
++
++	/* Step 3: disable PLLGP and enable program PLLGP */
++	val = mt7530_read(priv, MT7531_PLLGP_EN);
++	val |= SW_PLLGP;
++	mt7530_write(priv, MT7531_PLLGP_EN, val);
++
++	/* Step 4: program COREPLL output frequency to 500MHz */
++	val = mt7530_read(priv, MT7531_PLLGP_CR0);
++	val &= ~RG_COREPLL_POSDIV_M;
++	val |= 2 << RG_COREPLL_POSDIV_S;
++	mt7530_write(priv, MT7531_PLLGP_CR0, val);
++	usleep_range(25, 35);
++
++	switch (xtal) {
++	case HWTRAP_XTAL_FSEL_25MHZ:
++		val = mt7530_read(priv, MT7531_PLLGP_CR0);
++		val &= ~RG_COREPLL_SDM_PCW_M;
++		val |= 0x140000 << RG_COREPLL_SDM_PCW_S;
++		mt7530_write(priv, MT7531_PLLGP_CR0, val);
++		break;
++	case HWTRAP_XTAL_FSEL_40MHZ:
++		val = mt7530_read(priv, MT7531_PLLGP_CR0);
++		val &= ~RG_COREPLL_SDM_PCW_M;
++		val |= 0x190000 << RG_COREPLL_SDM_PCW_S;
++		mt7530_write(priv, MT7531_PLLGP_CR0, val);
++		break;
++	};
++
++	/* Set feedback divide ratio update signal to high */
++	val = mt7530_read(priv, MT7531_PLLGP_CR0);
++	val |= RG_COREPLL_SDM_PCW_CHG;
++	mt7530_write(priv, MT7531_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 = mt7530_read(priv, MT7531_PLLGP_CR0);
++	val &= ~RG_COREPLL_SDM_PCW_CHG;
++	mt7530_write(priv, MT7531_PLLGP_CR0, val);
++
++	/* Enable 325M clock for SGMII */
++	mt7530_write(priv, MT7531_ANA_PLLGP_CR5, 0xad0000);
++
++	/* Enable 250SSC clock for RGMII */
++	mt7530_write(priv, MT7531_ANA_PLLGP_CR2, 0x4f40000);
++
++	/* Step 6: Enable MT7531 PLL */
++	val = mt7530_read(priv, MT7531_PLLGP_CR0);
++	val |= RG_COREPLL_EN;
++	mt7530_write(priv, MT7531_PLLGP_CR0, val);
++
++	val = mt7530_read(priv, MT7531_PLLGP_EN);
++	val |= EN_COREPLL;
++	mt7530_write(priv, MT7531_PLLGP_EN, val);
++	usleep_range(25, 35);
++
++	return 0;
++}
++
+ static void
+ mt7530_mib_reset(struct dsa_switch *ds)
+ {
+@@ -507,6 +615,217 @@ static int mt7530_phy_write(struct dsa_s
+ 	return mdiobus_write_nested(priv->bus, port, regnum, val);
+ }
+ 
++static int
++mt7531_ind_c45_phy_read(struct mt7530_priv *priv, int port, int devad,
++			int regnum)
++{
++	struct mii_bus *bus = priv->bus;
++	struct mt7530_dummy_poll p;
++	u32 reg, val;
++	int ret;
++
++	INIT_MT7530_DUMMY_POLL(&p, priv, MT7531_PHY_IAC);
++
++	mutex_lock_nested(&bus->mdio_lock, MDIO_MUTEX_NESTED);
++
++	ret = readx_poll_timeout(_mt7530_unlocked_read, &p, val,
++				 !(val & MT7531_PHY_ACS_ST), 20, 100000);
++	if (ret < 0) {
++		dev_err(priv->dev, "poll timeout\n");
++		goto out;
++	}
++
++	reg = MT7531_MDIO_CL45_ADDR | MT7531_MDIO_PHY_ADDR(port) |
++	      MT7531_MDIO_DEV_ADDR(devad) | regnum;
++	mt7530_mii_write(priv, MT7531_PHY_IAC, reg | MT7531_PHY_ACS_ST);
++
++	ret = readx_poll_timeout(_mt7530_unlocked_read, &p, val,
++				 !(val & MT7531_PHY_ACS_ST), 20, 100000);
++	if (ret < 0) {
++		dev_err(priv->dev, "poll timeout\n");
++		goto out;
++	}
++
++	reg = MT7531_MDIO_CL45_READ | MT7531_MDIO_PHY_ADDR(port) |
++	      MT7531_MDIO_DEV_ADDR(devad);
++	mt7530_mii_write(priv, MT7531_PHY_IAC, reg | MT7531_PHY_ACS_ST);
++
++	ret = readx_poll_timeout(_mt7530_unlocked_read, &p, val,
++				 !(val & MT7531_PHY_ACS_ST), 20, 100000);
++	if (ret < 0) {
++		dev_err(priv->dev, "poll timeout\n");
++		goto out;
++	}
++
++	ret = val & MT7531_MDIO_RW_DATA_MASK;
++out:
++	mutex_unlock(&bus->mdio_lock);
++
++	return ret;
++}
++
++static int
++mt7531_ind_c45_phy_write(struct mt7530_priv *priv, int port, int devad,
++			 int regnum, u32 data)
++{
++	struct mii_bus *bus = priv->bus;
++	struct mt7530_dummy_poll p;
++	u32 val, reg;
++	int ret;
++
++	INIT_MT7530_DUMMY_POLL(&p, priv, MT7531_PHY_IAC);
++
++	mutex_lock_nested(&bus->mdio_lock, MDIO_MUTEX_NESTED);
++
++	ret = readx_poll_timeout(_mt7530_unlocked_read, &p, val,
++				 !(val & MT7531_PHY_ACS_ST), 20, 100000);
++	if (ret < 0) {
++		dev_err(priv->dev, "poll timeout\n");
++		goto out;
++	}
++
++	reg = MT7531_MDIO_CL45_ADDR | MT7531_MDIO_PHY_ADDR(port) |
++	      MT7531_MDIO_DEV_ADDR(devad) | regnum;
++	mt7530_mii_write(priv, MT7531_PHY_IAC, reg | MT7531_PHY_ACS_ST);
++
++	ret = readx_poll_timeout(_mt7530_unlocked_read, &p, val,
++				 !(val & MT7531_PHY_ACS_ST), 20, 100000);
++	if (ret < 0) {
++		dev_err(priv->dev, "poll timeout\n");
++		goto out;
++	}
++
++	reg = MT7531_MDIO_CL45_WRITE | MT7531_MDIO_PHY_ADDR(port) |
++	      MT7531_MDIO_DEV_ADDR(devad) | data;
++	mt7530_mii_write(priv, MT7531_PHY_IAC, reg | MT7531_PHY_ACS_ST);
++
++	ret = readx_poll_timeout(_mt7530_unlocked_read, &p, val,
++				 !(val & MT7531_PHY_ACS_ST), 20, 100000);
++	if (ret < 0) {
++		dev_err(priv->dev, "poll timeout\n");
++		goto out;
++	}
++
++out:
++	mutex_unlock(&bus->mdio_lock);
++
++	return ret;
++}
++
++static int
++mt7531_ind_c22_phy_read(struct mt7530_priv *priv, int port, int regnum)
++{
++	struct mii_bus *bus = priv->bus;
++	struct mt7530_dummy_poll p;
++	int ret;
++	u32 val;
++
++	INIT_MT7530_DUMMY_POLL(&p, priv, MT7531_PHY_IAC);
++
++	mutex_lock_nested(&bus->mdio_lock, MDIO_MUTEX_NESTED);
++
++	ret = readx_poll_timeout(_mt7530_unlocked_read, &p, val,
++				 !(val & MT7531_PHY_ACS_ST), 20, 100000);
++	if (ret < 0) {
++		dev_err(priv->dev, "poll timeout\n");
++		goto out;
++	}
++
++	val = MT7531_MDIO_CL22_READ | MT7531_MDIO_PHY_ADDR(port) |
++	      MT7531_MDIO_REG_ADDR(regnum);
++
++	mt7530_mii_write(priv, MT7531_PHY_IAC, val | MT7531_PHY_ACS_ST);
++
++	ret = readx_poll_timeout(_mt7530_unlocked_read, &p, val,
++				 !(val & MT7531_PHY_ACS_ST), 20, 100000);
++	if (ret < 0) {
++		dev_err(priv->dev, "poll timeout\n");
++		goto out;
++	}
++
++	ret = val & MT7531_MDIO_RW_DATA_MASK;
++out:
++	mutex_unlock(&bus->mdio_lock);
++
++	return ret;
++}
++
++static int
++mt7531_ind_c22_phy_write(struct mt7530_priv *priv, int port, int regnum,
++			 u16 data)
++{
++	struct mii_bus *bus = priv->bus;
++	struct mt7530_dummy_poll p;
++	int ret;
++	u32 reg;
++
++	INIT_MT7530_DUMMY_POLL(&p, priv, MT7531_PHY_IAC);
++
++	mutex_lock_nested(&bus->mdio_lock, MDIO_MUTEX_NESTED);
++
++	ret = readx_poll_timeout(_mt7530_unlocked_read, &p, reg,
++				 !(reg & MT7531_PHY_ACS_ST), 20, 100000);
++	if (ret < 0) {
++		dev_err(priv->dev, "poll timeout\n");
++		goto out;
++	}
++
++	reg = MT7531_MDIO_CL22_WRITE | MT7531_MDIO_PHY_ADDR(port) |
++	      MT7531_MDIO_REG_ADDR(regnum) | data;
++
++	mt7530_mii_write(priv, MT7531_PHY_IAC, reg | MT7531_PHY_ACS_ST);
++
++	ret = readx_poll_timeout(_mt7530_unlocked_read, &p, reg,
++				 !(reg & MT7531_PHY_ACS_ST), 20, 100000);
++	if (ret < 0) {
++		dev_err(priv->dev, "poll timeout\n");
++		goto out;
++	}
++
++out:
++	mutex_unlock(&bus->mdio_lock);
++
++	return ret;
++}
++
++static int
++mt7531_ind_phy_read(struct dsa_switch *ds, int port, int regnum)
++{
++	struct mt7530_priv *priv = ds->priv;
++	int devad;
++	int ret;
++
++	if (regnum & MII_ADDR_C45) {
++		devad = (regnum >> MII_DEVADDR_C45_SHIFT) & 0x1f;
++		ret = mt7531_ind_c45_phy_read(priv, port, devad,
++					      regnum & MII_REGADDR_C45_MASK);
++	} else {
++		ret = mt7531_ind_c22_phy_read(priv, port, regnum);
++	}
++
++	return ret;
++}
++
++static int
++mt7531_ind_phy_write(struct dsa_switch *ds, int port, int regnum,
++		     u16 data)
++{
++	struct mt7530_priv *priv = ds->priv;
++	int devad;
++	int ret;
++
++	if (regnum & MII_ADDR_C45) {
++		devad = (regnum >> MII_DEVADDR_C45_SHIFT) & 0x1f;
++		ret = mt7531_ind_c45_phy_write(priv, port, devad,
++					       regnum & MII_REGADDR_C45_MASK,
++					       data);
++	} else {
++		ret = mt7531_ind_c22_phy_write(priv, port, regnum, data);
++	}
++
++	return ret;
++}
++
+ static void
+ mt7530_get_strings(struct dsa_switch *ds, int port, u32 stringset,
+ 		   uint8_t *data)
+@@ -623,9 +942,14 @@ unlock_exit:
+ }
+ 
+ static int
+-mt7530_cpu_port_enable(struct mt7530_priv *priv,
+-		       int port)
++mt753x_cpu_port_enable(struct dsa_switch *ds, int port)
+ {
++	struct mt7530_priv *priv = ds->priv;
++
++	/* Setup max capability of CPU port at first */
++	if (priv->info->cpu_port_config)
++		priv->info->cpu_port_config(ds, port);
++
+ 	/* Enable Mediatek header mode on the cpu port */
+ 	mt7530_write(priv, MT7530_PVC_P(port),
+ 		     PORT_SPEC_TAG);
+@@ -638,7 +962,7 @@ mt7530_cpu_port_enable(struct mt7530_pri
+ 		mt7530_rmw(priv, MT7530_MFC, CPU_MASK, CPU_EN | CPU_PORT(port));
+ 
+ 	/* CPU port gets connected to all user ports of
+-	 * the switch
++	 * the switch.
+ 	 */
+ 	mt7530_write(priv, MT7530_PCR_P(port),
+ 		     PCR_MATRIX(dsa_user_ports(priv->ds)));
+@@ -1132,27 +1456,42 @@ mt7530_port_vlan_del(struct dsa_switch *
+ 	return 0;
+ }
+ 
+-static int mt7530_port_mirror_add(struct dsa_switch *ds, int port,
++static int mt753x_mirror_port_get(unsigned int id, u32 val)
++{
++	return (id == ID_MT7531) ? MT7531_MIRROR_PORT_GET(val) :
++				   MIRROR_PORT(val);
++}
++
++static int mt753x_mirror_port_set(unsigned int id, u32 val)
++{
++	return (id == ID_MT7531) ? MT7531_MIRROR_PORT_SET(val) :
++				   MIRROR_PORT(val);
++}
++
++static int mt753x_port_mirror_add(struct dsa_switch *ds, int port,
+ 				  struct dsa_mall_mirror_tc_entry *mirror,
+ 				  bool ingress)
+ {
+ 	struct mt7530_priv *priv = ds->priv;
++	int monitor_port;
+ 	u32 val;
+ 
+ 	/* Check for existent entry */
+ 	if ((ingress ? priv->mirror_rx : priv->mirror_tx) & BIT(port))
+ 		return -EEXIST;
+ 
+-	val = mt7530_read(priv, MT7530_MFC);
++	val = mt7530_read(priv, MT753X_MIRROR_REG(priv->id));
+ 
+ 	/* MT7530 only supports one monitor port */
+-	if (val & MIRROR_EN && MIRROR_PORT(val) != mirror->to_local_port)
++	monitor_port = mt753x_mirror_port_get(priv->id, val);
++	if (val & MT753X_MIRROR_EN(priv->id) &&
++	    monitor_port != mirror->to_local_port)
+ 		return -EEXIST;
+ 
+-	val |= MIRROR_EN;
+-	val &= ~MIRROR_MASK;
+-	val |= mirror->to_local_port;
+-	mt7530_write(priv, MT7530_MFC, val);
++	val |= MT753X_MIRROR_EN(priv->id);
++	val &= ~MT753X_MIRROR_MASK(priv->id);
++	val |= mt753x_mirror_port_set(priv->id, mirror->to_local_port);
++	mt7530_write(priv, MT753X_MIRROR_REG(priv->id), val);
+ 
+ 	val = mt7530_read(priv, MT7530_PCR_P(port));
+ 	if (ingress) {
+@@ -1167,7 +1506,7 @@ static int mt7530_port_mirror_add(struct
+ 	return 0;
+ }
+ 
+-static void mt7530_port_mirror_del(struct dsa_switch *ds, int port,
++static void mt753x_port_mirror_del(struct dsa_switch *ds, int port,
+ 				   struct dsa_mall_mirror_tc_entry *mirror)
+ {
+ 	struct mt7530_priv *priv = ds->priv;
+@@ -1184,9 +1523,9 @@ static void mt7530_port_mirror_del(struc
+ 	mt7530_write(priv, MT7530_PCR_P(port), val);
+ 
+ 	if (!priv->mirror_rx && !priv->mirror_tx) {
+-		val = mt7530_read(priv, MT7530_MFC);
+-		val &= ~MIRROR_EN;
+-		mt7530_write(priv, MT7530_MFC, val);
++		val = mt7530_read(priv, MT753X_MIRROR_REG(priv->id));
++		val &= ~MT753X_MIRROR_EN(priv->id);
++		mt7530_write(priv, MT753X_MIRROR_REG(priv->id), val);
+ 	}
+ }
+ 
+@@ -1292,7 +1631,7 @@ mt7530_setup(struct dsa_switch *ds)
+ 			   PCR_MATRIX_CLR);
+ 
+ 		if (dsa_is_cpu_port(ds, i))
+-			mt7530_cpu_port_enable(priv, i);
++			mt753x_cpu_port_enable(ds, i);
+ 		else
+ 			mt7530_port_disable(ds, i);
+ 
+@@ -1346,6 +1685,118 @@ mt7530_setup(struct dsa_switch *ds)
+ 	return 0;
+ }
+ 
++static int
++mt7531_setup(struct dsa_switch *ds)
++{
++	struct mt7530_priv *priv = ds->priv;
++	struct mt7530_dummy_poll p;
++	u32 val, id;
++	int ret, i;
++
++	/* Reset whole chip through gpio pin or memory-mapped registers for
++	 * different type of hardware
++	 */
++	if (priv->mcm) {
++		reset_control_assert(priv->rstc);
++		usleep_range(1000, 1100);
++		reset_control_deassert(priv->rstc);
++	} else {
++		gpiod_set_value_cansleep(priv->reset, 0);
++		usleep_range(1000, 1100);
++		gpiod_set_value_cansleep(priv->reset, 1);
++	}
++
++	/* Waiting for MT7530 got to stable */
++	INIT_MT7530_DUMMY_POLL(&p, priv, MT7530_HWTRAP);
++	ret = readx_poll_timeout(_mt7530_read, &p, val, val != 0,
++				 20, 1000000);
++	if (ret < 0) {
++		dev_err(priv->dev, "reset timeout\n");
++		return ret;
++	}
++
++	id = mt7530_read(priv, MT7531_CREV);
++	id >>= CHIP_NAME_SHIFT;
++
++	if (id != MT7531_ID) {
++		dev_err(priv->dev, "chip %x can't be supported\n", id);
++		return -ENODEV;
++	}
++
++	/* Reset the switch through internal reset */
++	mt7530_write(priv, MT7530_SYS_CTRL,
++		     SYS_CTRL_PHY_RST | SYS_CTRL_SW_RST |
++		     SYS_CTRL_REG_RST);
++
++	if (mt7531_dual_sgmii_supported(priv)) {
++		priv->p5_intf_sel = P5_INTF_SEL_GMAC5_SGMII;
++
++		/* Let ds->slave_mii_bus be able to access external phy. */
++		mt7530_rmw(priv, MT7531_GPIO_MODE1, MT7531_GPIO11_RG_RXD2_MASK,
++			   MT7531_EXT_P_MDC_11);
++		mt7530_rmw(priv, MT7531_GPIO_MODE1, MT7531_GPIO12_RG_RXD3_MASK,
++			   MT7531_EXT_P_MDIO_12);
++	} else {
++		priv->p5_intf_sel = P5_INTF_SEL_GMAC5;
++	}
++	dev_dbg(ds->dev, "P5 support %s interface\n",
++		p5_intf_modes(priv->p5_intf_sel));
++
++	mt7530_rmw(priv, MT7531_GPIO_MODE0, MT7531_GPIO0_MASK,
++		   MT7531_GPIO0_INTERRUPT);
++
++	/* Let phylink decide the interface later. */
++	priv->p5_interface = PHY_INTERFACE_MODE_NA;
++	priv->p6_interface = PHY_INTERFACE_MODE_NA;
++
++	/* Enable PHY core PLL, since phy_device has not yet been created
++	 * provided for phy_[read,write]_mmd_indirect is called, we provide
++	 * our own mt7531_ind_mmd_phy_[read,write] to complete this
++	 * function.
++	 */
++	val = mt7531_ind_c45_phy_read(priv, MT753X_CTRL_PHY_ADDR,
++				      MDIO_MMD_VEND2, CORE_PLL_GROUP4);
++	val |= MT7531_PHY_PLL_BYPASS_MODE;
++	val &= ~MT7531_PHY_PLL_OFF;
++	mt7531_ind_c45_phy_write(priv, MT753X_CTRL_PHY_ADDR, MDIO_MMD_VEND2,
++				 CORE_PLL_GROUP4, val);
++
++	/* BPDU to CPU port */
++	mt7530_rmw(priv, MT7531_CFC, MT7531_CPU_PMAP_MASK,
++		   BIT(MT7530_CPU_PORT));
++	mt7530_rmw(priv, MT753X_BPC, MT753X_BPDU_PORT_FW_MASK,
++		   MT753X_BPDU_CPU_ONLY);
++
++	/* Enable and reset MIB counters */
++	mt7530_mib_reset(ds);
++
++	for (i = 0; i < MT7530_NUM_PORTS; i++) {
++		/* Disable forwarding by default on all ports */
++		mt7530_rmw(priv, MT7530_PCR_P(i), PCR_MATRIX_MASK,
++			   PCR_MATRIX_CLR);
++
++		mt7530_set(priv, MT7531_DBG_CNT(i), MT7531_DIS_CLR);
++
++		if (dsa_is_cpu_port(ds, i))
++			mt753x_cpu_port_enable(ds, i);
++		else
++			mt7530_port_disable(ds, i);
++
++		/* Enable consistent egress tag */
++		mt7530_rmw(priv, MT7530_PVC_P(i), PVC_EG_TAG_MASK,
++			   PVC_EG_TAG(MT7530_VLAN_EG_CONSISTENT));
++	}
++
++	ds->configure_vlan_while_not_filtering = true;
++
++	/* Flush the FDB table */
++	ret = mt7530_fdb_cmd(priv, MT7530_FDB_FLUSH, NULL);
++	if (ret < 0)
++		return ret;
++
++	return 0;
++}
++
+ static bool
+ mt7530_phy_mode_supported(struct dsa_switch *ds, int port,
+ 			  const struct phylink_link_state *state)
+@@ -1384,6 +1835,47 @@ unsupported:
+ 	return false;
+ }
+ 
++static bool mt7531_is_rgmii_port(struct mt7530_priv *priv, u32 port)
++{
++	return (port == 5) && (priv->p5_intf_sel != P5_INTF_SEL_GMAC5_SGMII);
++}
++
++static bool
++mt7531_phy_supported(struct dsa_switch *ds, int port,
++		     const struct phylink_link_state *state)
++{
++	struct mt7530_priv *priv = ds->priv;
++
++	switch (port) {
++	case 0: /* Internal phy */
++	case 1:
++	case 2:
++	case 3:
++	case 4:
++		if (state->interface != PHY_INTERFACE_MODE_GMII)
++			goto unsupported;
++		break;
++	case 5: /* 2nd cpu port supports either rgmii or sgmii/8023z */
++		if (mt7531_is_rgmii_port(priv, port))
++			return phy_interface_mode_is_rgmii(state->interface);
++		fallthrough;
++	case 6: /* 1st cpu port supports sgmii/8023z only */
++		if (state->interface != PHY_INTERFACE_MODE_SGMII &&
++		    !phy_interface_mode_is_8023z(state->interface))
++			goto unsupported;
++		break;
++	default:
++		dev_err(priv->dev, "%s: unsupported port: %i\n", __func__,
++			port);
++		goto unsupported;
++	}
++
++	return true;
++
++unsupported:
++	return false;
++}
++
+ static bool
+ mt753x_phy_mode_supported(struct dsa_switch *ds, int port,
+ 			  const struct phylink_link_state *state)
+@@ -1416,6 +1908,227 @@ mt7530_mac_config(struct dsa_switch *ds,
+ 	return 0;
+ }
+ 
++static int mt7531_rgmii_setup(struct mt7530_priv *priv, u32 port,
++			      phy_interface_t interface,
++			      struct phy_device *phydev)
++{
++	u32 val;
++
++	if (!mt7531_is_rgmii_port(priv, port)) {
++		dev_err(priv->dev, "RGMII mode is not available for port %d\n",
++			port);
++		return -EINVAL;
++	}
++
++	val = mt7530_read(priv, MT7531_CLKGEN_CTRL);
++	val |= GP_CLK_EN;
++	val &= ~GP_MODE_MASK;
++	val |= GP_MODE(MT7531_GP_MODE_RGMII);
++	val &= ~CLK_SKEW_IN_MASK;
++	val |= CLK_SKEW_IN(MT7531_CLK_SKEW_NO_CHG);
++	val &= ~CLK_SKEW_OUT_MASK;
++	val |= CLK_SKEW_OUT(MT7531_CLK_SKEW_NO_CHG);
++	val |= TXCLK_NO_REVERSE | RXCLK_NO_DELAY;
++
++	/* Do not adjust rgmii delay when vendor phy driver presents. */
++	if (!phydev || phy_driver_is_genphy(phydev)) {
++		val &= ~(TXCLK_NO_REVERSE | RXCLK_NO_DELAY);
++		switch (interface) {
++		case PHY_INTERFACE_MODE_RGMII:
++			val |= TXCLK_NO_REVERSE;
++			val |= RXCLK_NO_DELAY;
++			break;
++		case PHY_INTERFACE_MODE_RGMII_RXID:
++			val |= TXCLK_NO_REVERSE;
++			break;
++		case PHY_INTERFACE_MODE_RGMII_TXID:
++			val |= RXCLK_NO_DELAY;
++			break;
++		case PHY_INTERFACE_MODE_RGMII_ID:
++			break;
++		default:
++			return -EINVAL;
++		}
++	}
++	mt7530_write(priv, MT7531_CLKGEN_CTRL, val);
++
++	return 0;
++}
++
++static void mt7531_sgmii_validate(struct mt7530_priv *priv, int port,
++				  unsigned long *supported)
++{
++	/* Port5 supports ethier RGMII or SGMII.
++	 * Port6 supports SGMII only.
++	 */
++	switch (port) {
++	case 5:
++		if (mt7531_is_rgmii_port(priv, port))
++			break;
++		fallthrough;
++	case 6:
++		phylink_set(supported, 1000baseX_Full);
++		phylink_set(supported, 2500baseX_Full);
++		phylink_set(supported, 2500baseT_Full);
++	}
++}
++
++static void
++mt7531_sgmii_link_up_force(struct dsa_switch *ds, int port,
++			   unsigned int mode, phy_interface_t interface,
++			   int speed, int duplex)
++{
++	struct mt7530_priv *priv = ds->priv;
++	unsigned int val;
++
++	/* For adjusting speed and duplex of SGMII force mode. */
++	if (interface != PHY_INTERFACE_MODE_SGMII ||
++	    phylink_autoneg_inband(mode))
++		return;
++
++	/* SGMII force mode setting */
++	val = mt7530_read(priv, MT7531_SGMII_MODE(port));
++	val &= ~MT7531_SGMII_IF_MODE_MASK;
++
++	switch (speed) {
++	case SPEED_10:
++		val |= MT7531_SGMII_FORCE_SPEED_10;
++		break;
++	case SPEED_100:
++		val |= MT7531_SGMII_FORCE_SPEED_100;
++		break;
++	case SPEED_1000:
++		val |= MT7531_SGMII_FORCE_SPEED_1000;
++		break;
++	}
++
++	/* MT7531 SGMII 1G force mode can only work in full duplex mode,
++	 * no matter MT7531_SGMII_FORCE_HALF_DUPLEX is set or not.
++	 */
++	if ((speed == SPEED_10 || speed == SPEED_100) &&
++	    duplex != DUPLEX_FULL)
++		val |= MT7531_SGMII_FORCE_HALF_DUPLEX;
++
++	mt7530_write(priv, MT7531_SGMII_MODE(port), val);
++}
++
++static bool mt753x_is_mac_port(u32 port)
++{
++	return (port == 5 || port == 6);
++}
++
++static int mt7531_sgmii_setup_mode_force(struct mt7530_priv *priv, u32 port,
++					 phy_interface_t interface)
++{
++	u32 val;
++
++	if (!mt753x_is_mac_port(port))
++		return -EINVAL;
++
++	mt7530_set(priv, MT7531_QPHY_PWR_STATE_CTRL(port),
++		   MT7531_SGMII_PHYA_PWD);
++
++	val = mt7530_read(priv, MT7531_PHYA_CTRL_SIGNAL3(port));
++	val &= ~MT7531_RG_TPHY_SPEED_MASK;
++	/* Setup 2.5 times faster clock for 2.5Gbps data speeds with 10B/8B
++	 * encoding.
++	 */
++	val |= (interface == PHY_INTERFACE_MODE_2500BASEX) ?
++		MT7531_RG_TPHY_SPEED_3_125G : MT7531_RG_TPHY_SPEED_1_25G;
++	mt7530_write(priv, MT7531_PHYA_CTRL_SIGNAL3(port), val);
++
++	mt7530_clear(priv, MT7531_PCS_CONTROL_1(port), MT7531_SGMII_AN_ENABLE);
++
++	/* MT7531 SGMII 1G and 2.5G force mode can only work in full duplex
++	 * mode, no matter MT7531_SGMII_FORCE_HALF_DUPLEX is set or not.
++	 */
++	mt7530_rmw(priv, MT7531_SGMII_MODE(port),
++		   MT7531_SGMII_IF_MODE_MASK | MT7531_SGMII_REMOTE_FAULT_DIS,
++		   MT7531_SGMII_FORCE_SPEED_1000);
++
++	mt7530_write(priv, MT7531_QPHY_PWR_STATE_CTRL(port), 0);
++
++	return 0;
++}
++
++static int mt7531_sgmii_setup_mode_an(struct mt7530_priv *priv, int port,
++				      phy_interface_t interface)
++{
++	if (!mt753x_is_mac_port(port))
++		return -EINVAL;
++
++	mt7530_set(priv, MT7531_QPHY_PWR_STATE_CTRL(port),
++		   MT7531_SGMII_PHYA_PWD);
++
++	mt7530_rmw(priv, MT7531_PHYA_CTRL_SIGNAL3(port),
++		   MT7531_RG_TPHY_SPEED_MASK, MT7531_RG_TPHY_SPEED_1_25G);
++
++	mt7530_set(priv, MT7531_SGMII_MODE(port),
++		   MT7531_SGMII_REMOTE_FAULT_DIS |
++		   MT7531_SGMII_SPEED_DUPLEX_AN);
++
++	mt7530_rmw(priv, MT7531_PCS_SPEED_ABILITY(port),
++		   MT7531_SGMII_TX_CONFIG_MASK, 1);
++
++	mt7530_set(priv, MT7531_PCS_CONTROL_1(port), MT7531_SGMII_AN_ENABLE);
++
++	mt7530_set(priv, MT7531_PCS_CONTROL_1(port), MT7531_SGMII_AN_RESTART);
++
++	mt7530_write(priv, MT7531_QPHY_PWR_STATE_CTRL(port), 0);
++
++	return 0;
++}
++
++static void mt7531_sgmii_restart_an(struct dsa_switch *ds, int port)
++{
++	struct mt7530_priv *priv = ds->priv;
++	u32 val;
++
++	/* Only restart AN when AN is enabled */
++	val = mt7530_read(priv, MT7531_PCS_CONTROL_1(port));
++	if (val & MT7531_SGMII_AN_ENABLE) {
++		val |= MT7531_SGMII_AN_RESTART;
++		mt7530_write(priv, MT7531_PCS_CONTROL_1(port), val);
++	}
++}
++
++static int
++mt7531_mac_config(struct dsa_switch *ds, int port, unsigned int mode,
++		  phy_interface_t interface)
++{
++	struct mt7530_priv *priv = ds->priv;
++	struct phy_device *phydev;
++	const struct dsa_port *dp;
++
++	if (!mt753x_is_mac_port(port)) {
++		dev_err(priv->dev, "port %d is not a MAC port\n", port);
++		return -EINVAL;
++	}
++
++	switch (interface) {
++	case PHY_INTERFACE_MODE_RGMII:
++	case PHY_INTERFACE_MODE_RGMII_ID:
++	case PHY_INTERFACE_MODE_RGMII_RXID:
++	case PHY_INTERFACE_MODE_RGMII_TXID:
++		dp = dsa_to_port(ds, port);
++		phydev = dp->slave->phydev;
++		return mt7531_rgmii_setup(priv, port, interface, phydev);
++	case PHY_INTERFACE_MODE_SGMII:
++		return mt7531_sgmii_setup_mode_an(priv, port, interface);
++	case PHY_INTERFACE_MODE_NA:
++	case PHY_INTERFACE_MODE_1000BASEX:
++	case PHY_INTERFACE_MODE_2500BASEX:
++		if (phylink_autoneg_inband(mode))
++			return -EINVAL;
++
++		return mt7531_sgmii_setup_mode_force(priv, port, interface);
++	default:
++		return -EINVAL;
++	}
++
++	return -EINVAL;
++}
++
+ static int
+ mt753x_mac_config(struct dsa_switch *ds, int port, unsigned int mode,
+ 		  const struct phylink_link_state *state)
+@@ -1451,6 +2164,8 @@ mt753x_phylink_mac_config(struct dsa_swi
+ 		if (mt753x_mac_config(ds, port, mode, state) < 0)
+ 			goto unsupported;
+ 
++		if (priv->p5_intf_sel != P5_DISABLED)
++			priv->p5_interface = state->interface;
+ 		break;
+ 	case 6: /* 1st cpu port */
+ 		if (priv->p6_interface == state->interface)
+@@ -1470,7 +2185,8 @@ unsupported:
+ 		return;
+ 	}
+ 
+-	if (phylink_autoneg_inband(mode)) {
++	if (phylink_autoneg_inband(mode) &&
++	    state->interface != PHY_INTERFACE_MODE_SGMII) {
+ 		dev_err(ds->dev, "%s: in-band negotiation unsupported\n",
+ 			__func__);
+ 		return;
+@@ -1480,7 +2196,7 @@ unsupported:
+ 	mcr_new = mcr_cur;
+ 	mcr_new &= ~PMCR_LINK_SETTINGS_MASK;
+ 	mcr_new |= PMCR_IFG_XMIT(1) | PMCR_MAC_MODE | PMCR_BACKOFF_EN |
+-		   PMCR_BACKPR_EN | PMCR_FORCE_MODE;
++		   PMCR_BACKPR_EN | PMCR_FORCE_MODE_ID(priv->id);
+ 
+ 	/* Are we connected to external phy */
+ 	if (port == 5 && dsa_is_user_port(ds, 5))
+@@ -1490,7 +2206,18 @@ unsupported:
+ 		mt7530_write(priv, MT7530_PMCR_P(port), mcr_new);
+ }
+ 
+-static void mt7530_phylink_mac_link_down(struct dsa_switch *ds, int port,
++static void
++mt753x_phylink_mac_an_restart(struct dsa_switch *ds, int port)
++{
++	struct mt7530_priv *priv = ds->priv;
++
++	if (!priv->info->mac_pcs_an_restart)
++		return;
++
++	priv->info->mac_pcs_an_restart(ds, port);
++}
++
++static void mt753x_phylink_mac_link_down(struct dsa_switch *ds, int port,
+ 					 unsigned int mode,
+ 					 phy_interface_t interface)
+ {
+@@ -1499,7 +2226,19 @@ static void mt7530_phylink_mac_link_down
+ 	mt7530_clear(priv, MT7530_PMCR_P(port), PMCR_LINK_SETTINGS_MASK);
+ }
+ 
+-static void mt7530_phylink_mac_link_up(struct dsa_switch *ds, int port,
++static void mt753x_mac_pcs_link_up(struct dsa_switch *ds, int port,
++				   unsigned int mode, phy_interface_t interface,
++				   int speed, int duplex)
++{
++	struct mt7530_priv *priv = ds->priv;
++
++	if (!priv->info->mac_pcs_link_up)
++		return;
++
++	priv->info->mac_pcs_link_up(ds, port, mode, interface, speed, duplex);
++}
++
++static void mt753x_phylink_mac_link_up(struct dsa_switch *ds, int port,
+ 				       unsigned int mode,
+ 				       phy_interface_t interface,
+ 				       struct phy_device *phydev,
+@@ -1509,18 +2248,29 @@ static void mt7530_phylink_mac_link_up(s
+ 	struct mt7530_priv *priv = ds->priv;
+ 	u32 mcr;
+ 
++	mt753x_mac_pcs_link_up(ds, port, mode, interface, speed, duplex);
++
+ 	mcr = PMCR_RX_EN | PMCR_TX_EN | PMCR_FORCE_LNK;
+ 
++	/* MT753x MAC works in 1G full duplex mode for all up-clocked
++	 * variants.
++	 */
++	if (interface == PHY_INTERFACE_MODE_TRGMII ||
++	    (phy_interface_mode_is_8023z(interface))) {
++		speed = SPEED_1000;
++		duplex = DUPLEX_FULL;
++	}
++
+ 	switch (speed) {
+ 	case SPEED_1000:
+ 		mcr |= PMCR_FORCE_SPEED_1000;
+ 		if (priv->eee_enable & BIT(port))
+-			mcr_new |= PMCR_FORCE_EEE1G;
++			mcr |= PMCR_FORCE_EEE1G;
+ 		break;
+ 	case SPEED_100:
+ 		mcr |= PMCR_FORCE_SPEED_100;
+ 		if (priv->eee_enable & BIT(port))
+-			mcr_new |= PMCR_FORCE_EEE100;
++			mcr |= PMCR_FORCE_EEE100;
+ 		break;
+ 	}
+ 	if (duplex == DUPLEX_FULL) {
+@@ -1534,6 +2284,45 @@ static void mt7530_phylink_mac_link_up(s
+ 	mt7530_set(priv, MT7530_PMCR_P(port), mcr);
+ }
+ 
++static int
++mt7531_cpu_port_config(struct dsa_switch *ds, int port)
++{
++	struct mt7530_priv *priv = ds->priv;
++	phy_interface_t interface;
++	int speed;
++
++	switch (port) {
++	case 5:
++		if (mt7531_is_rgmii_port(priv, port))
++			interface = PHY_INTERFACE_MODE_RGMII;
++		else
++			interface = PHY_INTERFACE_MODE_2500BASEX;
++
++		priv->p5_interface = interface;
++		break;
++	case 6:
++		interface = PHY_INTERFACE_MODE_2500BASEX;
++
++		mt7531_pad_setup(ds, interface);
++
++		priv->p6_interface = interface;
++		break;
++	};
++
++	if (interface == PHY_INTERFACE_MODE_2500BASEX)
++		speed = SPEED_2500;
++	else
++		speed = SPEED_1000;
++
++	mt7531_mac_config(ds, port, MLO_AN_FIXED, interface);
++	mt7530_write(priv, MT7530_PMCR_P(port),
++		     PMCR_CPU_PORT_SETTING(priv->id));
++	mt753x_phylink_mac_link_up(ds, port, MLO_AN_FIXED, interface, NULL,
++				   speed, DUPLEX_FULL, true, true);
++
++	return 0;
++}
++
+ static void
+ mt7530_mac_port_validate(struct dsa_switch *ds, int port,
+ 			 unsigned long *supported)
+@@ -1542,6 +2331,14 @@ mt7530_mac_port_validate(struct dsa_swit
+ 		phylink_set(supported, 1000baseX_Full);
+ }
+ 
++static void mt7531_mac_port_validate(struct dsa_switch *ds, int port,
++				     unsigned long *supported)
++{
++	struct mt7530_priv *priv = ds->priv;
++
++	mt7531_sgmii_validate(priv, port, supported);
++}
++
+ static void
+ mt753x_phylink_validate(struct dsa_switch *ds, int port,
+ 			unsigned long *supported,
+@@ -1558,7 +2355,8 @@ mt753x_phylink_validate(struct dsa_switc
+ 
+ 	phylink_set_port_modes(mask);
+ 
+-	if (state->interface != PHY_INTERFACE_MODE_TRGMII) {
++	if (state->interface != PHY_INTERFACE_MODE_TRGMII ||
++	    !phy_interface_mode_is_8023z(state->interface)) {
+ 		phylink_set(mask, 10baseT_Half);
+ 		phylink_set(mask, 10baseT_Full);
+ 		phylink_set(mask, 100baseT_Half);
+@@ -1577,6 +2375,11 @@ mt753x_phylink_validate(struct dsa_switc
+ 
+ 	linkmode_and(supported, supported, mask);
+ 	linkmode_and(state->advertising, state->advertising, mask);
++
++	/* We can only operate at 2500BaseX or 1000BaseX.  If requested
++	 * to advertise both, only report advertising at 2500BaseX.
++	 */
++	phylink_helper_basex_speed(state);
+ }
+ 
+ static int
+@@ -1667,6 +2470,63 @@ static int mt7530_set_mac_eee(struct dsa
+ 	return 0;
+ }
+ 
++#ifdef notyet
++static int
++mt7531_sgmii_pcs_get_state_an(struct mt7530_priv *priv, int port,
++			      struct phylink_link_state *state)
++{
++	u32 status, val;
++	u16 config_reg;
++
++	status = mt7530_read(priv, MT7531_PCS_CONTROL_1(port));
++	state->link = !!(status & MT7531_SGMII_LINK_STATUS);
++	if (state->interface == PHY_INTERFACE_MODE_SGMII &&
++	    (status & MT7531_SGMII_AN_ENABLE)) {
++		val = mt7530_read(priv, MT7531_PCS_SPEED_ABILITY(port));
++		config_reg = val >> 16;
++
++		switch (config_reg & LPA_SGMII_SPD_MASK) {
++		case LPA_SGMII_1000:
++			state->speed = SPEED_1000;
++			break;
++		case LPA_SGMII_100:
++			state->speed = SPEED_100;
++			break;
++		case LPA_SGMII_10:
++			state->speed = SPEED_10;
++			break;
++		default:
++			dev_err(priv->dev, "invalid sgmii PHY speed\n");
++			state->link = false;
++			return -EINVAL;
++		}
++
++		if (config_reg & LPA_SGMII_FULL_DUPLEX)
++			state->duplex = DUPLEX_FULL;
++		else
++			state->duplex = DUPLEX_HALF;
++	}
++
++	return 0;
++}
++#endif
++
++static int
++mt7531_phylink_mac_link_state(struct dsa_switch *ds, int port,
++			      struct phylink_link_state *state)
++{
++#ifdef notyet
++	struct mt7530_priv *priv = ds->priv;
++
++	if (state->interface == PHY_INTERFACE_MODE_SGMII)
++		return mt7531_sgmii_pcs_get_state_an(priv, port, state);
++#else
++	return mt7530_phylink_mac_link_state(ds, port, state);
++#endif
++
++	return -EOPNOTSUPP;
++}
++
+ static int
+ mt753x_phylink_mac_link_state(struct dsa_switch *ds, int port,
+ 			      struct phylink_link_state *state)
+@@ -1720,13 +2580,14 @@ static const struct dsa_switch_ops mt753
+ 	.port_vlan_prepare	= mt7530_port_vlan_prepare,
+ 	.port_vlan_add		= mt7530_port_vlan_add,
+ 	.port_vlan_del		= mt7530_port_vlan_del,
+-	.port_mirror_add	= mt7530_port_mirror_add,
+-	.port_mirror_del	= mt7530_port_mirror_del,
++	.port_mirror_add	= mt753x_port_mirror_add,
++	.port_mirror_del	= mt753x_port_mirror_del,
+ 	.phylink_validate	= mt753x_phylink_validate,
+ 	.phylink_mac_link_state	= mt753x_phylink_mac_link_state,
+ 	.phylink_mac_config	= mt753x_phylink_mac_config,
+-	.phylink_mac_link_down	= mt7530_phylink_mac_link_down,
+-	.phylink_mac_link_up	= mt7530_phylink_mac_link_up,
++	.phylink_mac_an_restart	= mt753x_phylink_mac_an_restart,
++	.phylink_mac_link_down	= mt753x_phylink_mac_link_down,
++	.phylink_mac_link_up	= mt753x_phylink_mac_link_up,
+ 	.get_mac_eee		= mt7530_get_mac_eee,
+ 	.set_mac_eee		= mt7530_set_mac_eee,
+ };
+@@ -1754,11 +2615,26 @@ static const struct mt753x_info mt753x_t
+ 		.mac_port_get_state = mt7530_phylink_mac_link_state,
+ 		.mac_port_config = mt7530_mac_config,
+ 	},
++	[ID_MT7531] = {
++		.id = ID_MT7531,
++		.sw_setup = mt7531_setup,
++		.phy_read = mt7531_ind_phy_read,
++		.phy_write = mt7531_ind_phy_write,
++		.pad_setup = mt7531_pad_setup,
++		.cpu_port_config = mt7531_cpu_port_config,
++		.phy_mode_supported = mt7531_phy_supported,
++		.mac_port_validate = mt7531_mac_port_validate,
++		.mac_port_get_state = mt7531_phylink_mac_link_state,
++		.mac_port_config = mt7531_mac_config,
++		.mac_pcs_an_restart = mt7531_sgmii_restart_an,
++		.mac_pcs_link_up = mt7531_sgmii_link_up_force,
++	},
+ };
+ 
+ static const struct of_device_id mt7530_of_match[] = {
+ 	{ .compatible = "mediatek,mt7621", .data = &mt753x_table[ID_MT7621], },
+ 	{ .compatible = "mediatek,mt7530", .data = &mt753x_table[ID_MT7530], },
++	{ .compatible = "mediatek,mt7531", .data = &mt753x_table[ID_MT7531], },
+ 	{ /* sentinel */ },
+ };
+ MODULE_DEVICE_TABLE(of, mt7530_of_match);
+--- a/drivers/net/dsa/mt7530.h
++++ b/drivers/net/dsa/mt7530.h
+@@ -14,6 +14,7 @@
+ enum mt753x_id {
+ 	ID_MT7530 = 0,
+ 	ID_MT7621 = 1,
++	ID_MT7531 = 2,
+ };
+ 
+ #define	NUM_TRGMII_CTRL			5
+@@ -41,6 +42,33 @@ enum mt753x_id {
+ #define  MIRROR_PORT(x)			((x) & 0x7)
+ #define  MIRROR_MASK			0x7
+ 
++/* Registers for CPU forward control */
++#define MT7531_CFC			0x4
++#define  MT7531_MIRROR_EN		BIT(19)
++#define  MT7531_MIRROR_MASK		(MIRROR_MASK << 16)
++#define  MT7531_MIRROR_PORT_GET(x)	(((x) >> 16) & MIRROR_MASK)
++#define  MT7531_MIRROR_PORT_SET(x)	(((x) & MIRROR_MASK) << 16)
++#define  MT7531_CPU_PMAP_MASK		GENMASK(7, 0)
++
++#define MT753X_MIRROR_REG(id)		(((id) == ID_MT7531) ? \
++					 MT7531_CFC : MT7530_MFC)
++#define MT753X_MIRROR_EN(id)		(((id) == ID_MT7531) ? \
++					 MT7531_MIRROR_EN : MIRROR_EN)
++#define MT753X_MIRROR_MASK(id)		(((id) == ID_MT7531) ? \
++					 MT7531_MIRROR_MASK : MIRROR_MASK)
++
++/* Registers for BPDU and PAE frame control*/
++#define MT753X_BPC			0x24
++#define  MT753X_BPDU_PORT_FW_MASK	GENMASK(2, 0)
++
++enum mt753x_bpdu_port_fw {
++	MT753X_BPDU_FOLLOW_MFC,
++	MT753X_BPDU_CPU_EXCLUDE = 4,
++	MT753X_BPDU_CPU_INCLUDE = 5,
++	MT753X_BPDU_CPU_ONLY = 6,
++	MT753X_BPDU_DROP = 7,
++};
++
+ /* Registers for address table access */
+ #define MT7530_ATA1			0x74
+ #define  STATIC_EMP			0
+@@ -222,10 +250,30 @@ enum mt7530_vlan_port_attr {
+ #define  PMCR_FORCE_LNK			BIT(0)
+ #define  PMCR_SPEED_MASK		(PMCR_FORCE_SPEED_100 | \
+ 					 PMCR_FORCE_SPEED_1000)
++#define  MT7531_FORCE_LNK		BIT(31)
++#define  MT7531_FORCE_SPD		BIT(30)
++#define  MT7531_FORCE_DPX		BIT(29)
++#define  MT7531_FORCE_RX_FC		BIT(28)
++#define  MT7531_FORCE_TX_FC		BIT(27)
++#define  MT7531_FORCE_MODE		(MT7531_FORCE_LNK | \
++					 MT7531_FORCE_SPD | \
++					 MT7531_FORCE_DPX | \
++					 MT7531_FORCE_RX_FC | \
++					 MT7531_FORCE_TX_FC)
++#define  PMCR_FORCE_MODE_ID(id)		(((id) == ID_MT7531) ? \
++					 MT7531_FORCE_MODE : \
++					 PMCR_FORCE_MODE)
+ #define  PMCR_LINK_SETTINGS_MASK	(PMCR_TX_EN | PMCR_FORCE_SPEED_1000 | \
+ 					 PMCR_RX_EN | PMCR_FORCE_SPEED_100 | \
+ 					 PMCR_TX_FC_EN | PMCR_RX_FC_EN | \
+ 					 PMCR_FORCE_FDX | PMCR_FORCE_LNK)
++#define  PMCR_CPU_PORT_SETTING(id)	(PMCR_FORCE_MODE_ID((id)) | \
++					 PMCR_IFG_XMIT(1) | PMCR_MAC_MODE | \
++					 PMCR_BACKOFF_EN | PMCR_BACKPR_EN | \
++					 PMCR_TX_EN | PMCR_RX_EN | \
++					 PMCR_TX_FC_EN | PMCR_RX_FC_EN | \
++					 PMCR_FORCE_SPEED_1000 | \
++					 PMCR_FORCE_FDX | PMCR_FORCE_LNK)
+ 
+ #define MT7530_PMSR_P(x)		(0x3008 + (x) * 0x100)
+ #define  PMSR_EEE1G			BIT(7)
+@@ -245,6 +293,10 @@ enum mt7530_vlan_port_attr {
+ #define  LPI_THRESH(x)			((x & 0xFFF) << 4)
+ #define  LPI_MODE_EN			BIT(0)
+ 
++/* Register for port debug count */
++#define MT7531_DBG_CNT(x)		(0x3018 + (x) * 0x100)
++#define  MT7531_DIS_CLR			BIT(31)
++
+ /* Register for MIB */
+ #define MT7530_PORT_MIB_COUNTER(x)	(0x4000 + (x) * 0x100)
+ #define MT7530_MIB_CCR			0x4fe0
+@@ -262,12 +314,118 @@ enum mt7530_vlan_port_attr {
+ 					 CCR_RX_OCT_CNT_BAD | \
+ 					 CCR_TX_OCT_CNT_GOOD | \
+ 					 CCR_TX_OCT_CNT_BAD)
++
++/* MT7531 SGMII register group */
++#define MT7531_SGMII_REG_BASE		0x5000
++#define MT7531_SGMII_REG(p, r)		(MT7531_SGMII_REG_BASE + \
++					((p) - 5) * 0x1000 + (r))
++
++/* Register forSGMII PCS_CONTROL_1 */
++#define MT7531_PCS_CONTROL_1(p)		MT7531_SGMII_REG(p, 0x00)
++#define  MT7531_SGMII_LINK_STATUS	BIT(18)
++#define  MT7531_SGMII_AN_ENABLE		BIT(12)
++#define  MT7531_SGMII_AN_RESTART	BIT(9)
++
++/* Register for SGMII PCS_SPPED_ABILITY */
++#define MT7531_PCS_SPEED_ABILITY(p)	MT7531_SGMII_REG(p, 0x08)
++#define  MT7531_SGMII_TX_CONFIG_MASK	GENMASK(15, 0)
++#define  MT7531_SGMII_TX_CONFIG		BIT(0)
++
++/* Register for SGMII_MODE */
++#define MT7531_SGMII_MODE(p)		MT7531_SGMII_REG(p, 0x20)
++#define  MT7531_SGMII_REMOTE_FAULT_DIS	BIT(8)
++#define  MT7531_SGMII_IF_MODE_MASK	GENMASK(5, 1)
++#define  MT7531_SGMII_FORCE_DUPLEX	BIT(4)
++#define  MT7531_SGMII_FORCE_SPEED_MASK	GENMASK(3, 2)
++#define  MT7531_SGMII_FORCE_SPEED_1000	BIT(3)
++#define  MT7531_SGMII_FORCE_SPEED_100	BIT(2)
++#define  MT7531_SGMII_FORCE_SPEED_10	0
++#define  MT7531_SGMII_SPEED_DUPLEX_AN	BIT(1)
++
++enum mt7531_sgmii_force_duplex {
++	MT7531_SGMII_FORCE_FULL_DUPLEX = 0,
++	MT7531_SGMII_FORCE_HALF_DUPLEX = 0x10,
++};
++
++/* Fields of QPHY_PWR_STATE_CTRL */
++#define MT7531_QPHY_PWR_STATE_CTRL(p)	MT7531_SGMII_REG(p, 0xe8)
++#define  MT7531_SGMII_PHYA_PWD		BIT(4)
++
++/* Values of SGMII SPEED */
++#define MT7531_PHYA_CTRL_SIGNAL3(p)	MT7531_SGMII_REG(p, 0x128)
++#define  MT7531_RG_TPHY_SPEED_MASK	(BIT(2) | BIT(3))
++#define  MT7531_RG_TPHY_SPEED_1_25G	0x0
++#define  MT7531_RG_TPHY_SPEED_3_125G	BIT(2)
++
+ /* Register for system reset */
+ #define MT7530_SYS_CTRL			0x7000
+ #define  SYS_CTRL_PHY_RST		BIT(2)
+ #define  SYS_CTRL_SW_RST		BIT(1)
+ #define  SYS_CTRL_REG_RST		BIT(0)
+ 
++/* Register for PHY Indirect Access Control */
++#define MT7531_PHY_IAC			0x701C
++#define  MT7531_PHY_ACS_ST		BIT(31)
++#define  MT7531_MDIO_REG_ADDR_MASK	(0x1f << 25)
++#define  MT7531_MDIO_PHY_ADDR_MASK	(0x1f << 20)
++#define  MT7531_MDIO_CMD_MASK		(0x3 << 18)
++#define  MT7531_MDIO_ST_MASK		(0x3 << 16)
++#define  MT7531_MDIO_RW_DATA_MASK	(0xffff)
++#define  MT7531_MDIO_REG_ADDR(x)	(((x) & 0x1f) << 25)
++#define  MT7531_MDIO_DEV_ADDR(x)	(((x) & 0x1f) << 25)
++#define  MT7531_MDIO_PHY_ADDR(x)	(((x) & 0x1f) << 20)
++#define  MT7531_MDIO_CMD(x)		(((x) & 0x3) << 18)
++#define  MT7531_MDIO_ST(x)		(((x) & 0x3) << 16)
++
++enum mt7531_phy_iac_cmd {
++	MT7531_MDIO_ADDR = 0,
++	MT7531_MDIO_WRITE = 1,
++	MT7531_MDIO_READ = 2,
++	MT7531_MDIO_READ_CL45 = 3,
++};
++
++/* MDIO_ST: MDIO start field */
++enum mt7531_mdio_st {
++	MT7531_MDIO_ST_CL45 = 0,
++	MT7531_MDIO_ST_CL22 = 1,
++};
++
++#define  MT7531_MDIO_CL22_READ		(MT7531_MDIO_ST(MT7531_MDIO_ST_CL22) | \
++					 MT7531_MDIO_CMD(MT7531_MDIO_READ))
++#define  MT7531_MDIO_CL22_WRITE		(MT7531_MDIO_ST(MT7531_MDIO_ST_CL22) | \
++					 MT7531_MDIO_CMD(MT7531_MDIO_WRITE))
++#define  MT7531_MDIO_CL45_ADDR		(MT7531_MDIO_ST(MT7531_MDIO_ST_CL45) | \
++					 MT7531_MDIO_CMD(MT7531_MDIO_ADDR))
++#define  MT7531_MDIO_CL45_READ		(MT7531_MDIO_ST(MT7531_MDIO_ST_CL45) | \
++					 MT7531_MDIO_CMD(MT7531_MDIO_READ))
++#define  MT7531_MDIO_CL45_WRITE		(MT7531_MDIO_ST(MT7531_MDIO_ST_CL45) | \
++					 MT7531_MDIO_CMD(MT7531_MDIO_WRITE))
++
++/* Register for RGMII clock phase */
++#define MT7531_CLKGEN_CTRL		0x7500
++#define  CLK_SKEW_OUT(x)		(((x) & 0x3) << 8)
++#define  CLK_SKEW_OUT_MASK		GENMASK(9, 8)
++#define  CLK_SKEW_IN(x)			(((x) & 0x3) << 6)
++#define  CLK_SKEW_IN_MASK		GENMASK(7, 6)
++#define  RXCLK_NO_DELAY			BIT(5)
++#define  TXCLK_NO_REVERSE		BIT(4)
++#define  GP_MODE(x)			(((x) & 0x3) << 1)
++#define  GP_MODE_MASK			GENMASK(2, 1)
++#define  GP_CLK_EN			BIT(0)
++
++enum mt7531_gp_mode {
++	MT7531_GP_MODE_RGMII = 0,
++	MT7531_GP_MODE_MII = 1,
++	MT7531_GP_MODE_REV_MII = 2
++};
++
++enum mt7531_clk_skew {
++	MT7531_CLK_SKEW_NO_CHG = 0,
++	MT7531_CLK_SKEW_DLY_100PPS = 1,
++	MT7531_CLK_SKEW_DLY_200PPS = 2,
++	MT7531_CLK_SKEW_REVERSE = 3,
++};
++
+ /* Register for hw trap status */
+ #define MT7530_HWTRAP			0x7800
+ #define  HWTRAP_XTAL_MASK		(BIT(10) | BIT(9))
+@@ -275,6 +433,16 @@ enum mt7530_vlan_port_attr {
+ #define  HWTRAP_XTAL_40MHZ		(BIT(10))
+ #define  HWTRAP_XTAL_20MHZ		(BIT(9))
+ 
++#define MT7531_HWTRAP			0x7800
++#define  HWTRAP_XTAL_FSEL_MASK		BIT(7)
++#define  HWTRAP_XTAL_FSEL_25MHZ		BIT(7)
++#define  HWTRAP_XTAL_FSEL_40MHZ		0
++/* 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)
++
+ /* Register for hw trap modification */
+ #define MT7530_MHWTRAP			0x7804
+ #define  MHWTRAP_PHY0_SEL		BIT(20)
+@@ -289,14 +457,37 @@ enum mt7530_vlan_port_attr {
+ #define MT7530_TOP_SIG_CTRL		0x7808
+ #define  TOP_SIG_CTRL_NORMAL		(BIT(17) | BIT(16))
+ 
++#define MT7531_TOP_SIG_SR		0x780c
++#define  PAD_DUAL_SGMII_EN		BIT(1)
++#define  PAD_MCM_SMI_EN			BIT(0)
++
+ #define MT7530_IO_DRV_CR		0x7810
+ #define  P5_IO_CLK_DRV(x)		((x) & 0x3)
+ #define  P5_IO_DATA_DRV(x)		(((x) & 0x3) << 4)
+ 
++#define MT7531_CHIP_REV			0x781C
++
++#define MT7531_PLLGP_EN			0x7820
++#define  EN_COREPLL			BIT(2)
++#define  SW_CLKSW			BIT(1)
++#define  SW_PLLGP			BIT(0)
++
+ #define MT7530_P6ECR			0x7830
+ #define  P6_INTF_MODE_MASK		0x3
+ #define  P6_INTF_MODE(x)		((x) & 0x3)
+ 
++#define MT7531_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)
++
++/* Registers for RGMII and SGMII PLL clock */
++#define MT7531_ANA_PLLGP_CR2		0x78b0
++#define MT7531_ANA_PLLGP_CR5		0x78bc
++
+ /* Registers for TRGMII on the both side */
+ #define MT7530_TRGMII_RCK_CTRL		0x7a00
+ #define  RX_RST				BIT(31)
+@@ -335,10 +526,25 @@ enum mt7530_vlan_port_attr {
+ #define MT7530_P5RGMIITXCR		0x7b04
+ #define  CSR_RGMII_TXC_CFG(x)		((x) & 0x1f)
+ 
++/* Registers for GPIO mode */
++#define MT7531_GPIO_MODE0		0x7c0c
++#define  MT7531_GPIO0_MASK		GENMASK(3, 0)
++#define  MT7531_GPIO0_INTERRUPT		1
++
++#define MT7531_GPIO_MODE1		0x7c10
++#define  MT7531_GPIO11_RG_RXD2_MASK	GENMASK(15, 12)
++#define  MT7531_EXT_P_MDC_11		(2 << 12)
++#define  MT7531_GPIO12_RG_RXD3_MASK	GENMASK(19, 16)
++#define  MT7531_EXT_P_MDIO_12		(2 << 16)
++
+ #define MT7530_CREV			0x7ffc
+ #define  CHIP_NAME_SHIFT		16
+ #define  MT7530_ID			0x7530
+ 
++#define MT7531_CREV			0x781C
++#define  CHIP_REV_M			0x0f
++#define  MT7531_ID			0x7531
++
+ /* Registers for core PLL access through mmd indirect */
+ #define CORE_PLL_GROUP2			0x401
+ #define  RG_SYSPLL_EN_NORMAL		BIT(15)
+@@ -355,6 +561,10 @@ enum mt7530_vlan_port_attr {
+ #define  RG_SYSPLL_DDSFBK_EN		BIT(12)
+ #define  RG_SYSPLL_BIAS_EN		BIT(11)
+ #define  RG_SYSPLL_BIAS_LPF_EN		BIT(10)
++#define  MT7531_PHY_PLL_OFF		BIT(5)
++#define  MT7531_PHY_PLL_BYPASS_MODE	BIT(4)
++
++#define MT753X_CTRL_PHY_ADDR		0
+ 
+ #define CORE_PLL_GROUP5			0x404
+ #define  RG_LCDDS_PCW_NCPO1(x)		((x) & 0xffff)
+@@ -433,6 +643,7 @@ enum p5_interface_select {
+ 	P5_INTF_SEL_PHY_P0,
+ 	P5_INTF_SEL_PHY_P4,
+ 	P5_INTF_SEL_GMAC5,
++	P5_INTF_SEL_GMAC5_SGMII,
+ };
+ 
+ static const char *p5_intf_modes(unsigned int p5_interface)
+@@ -446,6 +657,8 @@ static const char *p5_intf_modes(unsigne
+ 		return "PHY P4";
+ 	case P5_INTF_SEL_GMAC5:
+ 		return "GMAC5";
++	case P5_INTF_SEL_GMAC5_SGMII:
++		return "GMAC5_SGMII";
+ 	default:
+ 		return "unknown";
+ 	}
+@@ -466,6 +679,10 @@ static const char *p5_intf_modes(unsigne
+  *			MAC port
+  * @mac_port_config:	Holding the way setting up the PHY attribute to a
+  *			certain MAC port
++ * @mac_pcs_an_restart	Holding the way restarting PCS autonegotiation for a
++ *			certain MAC port
++ * @mac_pcs_link_up:	Holding the way setting up the PHY attribute to the pcs
++ *			of the certain MAC port
+  */
+ struct mt753x_info {
+ 	enum mt753x_id id;
+@@ -474,6 +691,7 @@ struct mt753x_info {
+ 	int (*phy_read)(struct dsa_switch *ds, int port, int regnum);
+ 	int (*phy_write)(struct dsa_switch *ds, int port, int regnum, u16 val);
+ 	int (*pad_setup)(struct dsa_switch *ds, phy_interface_t interface);
++	int (*cpu_port_config)(struct dsa_switch *ds, int port);
+ 	bool (*phy_mode_supported)(struct dsa_switch *ds, int port,
+ 				   const struct phylink_link_state *state);
+ 	void (*mac_port_validate)(struct dsa_switch *ds, int port,
+@@ -483,6 +701,10 @@ struct mt753x_info {
+ 	int (*mac_port_config)(struct dsa_switch *ds, int port,
+ 			       unsigned int mode,
+ 			       phy_interface_t interface);
++	void (*mac_pcs_an_restart)(struct dsa_switch *ds, int port);
++	void (*mac_pcs_link_up)(struct dsa_switch *ds, int port,
++				unsigned int mode, phy_interface_t interface,
++				int speed, int duplex);
+ };
+ 
+ /* struct mt7530_priv -	This is the main data structure for holding the state