mvsw61xx: enable SerDes on 6176 if required

If the cpu port is connected through SGMII we need to enable SerDes for
it to work.

Signed-off-by: Jonas Gorski <jonas.gorski@gmail.com>
Acked-by: Felix Fietkau <nbd@nbd.name>

target/linux/generic/files/drivers/net/phy/mvsw61xx.c

@@ -172,6 +172,27 @@ mvsw61xx_mdio_write(struct switch_dev *dev, int addr, int reg, u16 val)
 				    MV_INDIRECT_INPROGRESS, 0) < 0;
 }
 
+static int
+mvsw61xx_mdio_page_read(struct switch_dev *dev, int port, int page, int reg)
+{
+	int ret;
+
+	mvsw61xx_mdio_write(dev, port, MII_MV_PAGE, page);
+	ret = mvsw61xx_mdio_read(dev, port, reg);
+	mvsw61xx_mdio_write(dev, port, MII_MV_PAGE, 0);
+
+	return ret;
+}
+
+static void
+mvsw61xx_mdio_page_write(struct switch_dev *dev, int port, int page, int reg,
+			 u16 val)
+{
+	mvsw61xx_mdio_write(dev, port, MII_MV_PAGE, page);
+	mvsw61xx_mdio_write(dev, port, reg, val);
+	mvsw61xx_mdio_write(dev, port, MII_MV_PAGE, 0);
+}
+
 static int
 mvsw61xx_get_port_mask(struct switch_dev *dev,
 		const struct switch_attr *attr, struct switch_val *val)
@@ -591,6 +612,19 @@ static int mvsw61xx_apply(struct switch_dev *dev)
 	return mvsw61xx_update_state(dev);
 }
 
+static void mvsw61xx_enable_serdes(struct switch_dev *dev)
+{
+	int bmcr = mvsw61xx_mdio_page_read(dev, MV_REG_FIBER_SERDES,
+					   MV_PAGE_FIBER_SERDES, MII_BMCR);
+	if (bmcr < 0)
+		return;
+
+	if (bmcr & BMCR_PDOWN)
+		mvsw61xx_mdio_page_write(dev, MV_REG_FIBER_SERDES,
+					 MV_PAGE_FIBER_SERDES, MII_BMCR,
+					 bmcr & ~BMCR_PDOWN);
+}
+
 static int _mvsw61xx_reset(struct switch_dev *dev, bool full)
 {
 	struct mvsw61xx_state *state = get_state(dev);
@@ -635,6 +669,18 @@ static int _mvsw61xx_reset(struct switch_dev *dev, bool full)
 					    BMCR_ANENABLE | BMCR_FULLDPLX |
 					    BMCR_SPEED1000);
 		}
+
+		/* enable SerDes if necessary */
+		if (full && i >= 5 && state->model == MV_IDENT_VALUE_6176) {
+			u16 sts = sr16(dev, MV_PORTREG(STATUS, i));
+			u16 mode = sts & MV_PORT_STATUS_CMODE_MASK;
+
+			if (mode == MV_PORT_STATUS_CMODE_100BASE_X ||
+			    mode == MV_PORT_STATUS_CMODE_1000BASE_X ||
+			    mode == MV_PORT_STATUS_CMODE_SGMII) {
+				mvsw61xx_enable_serdes(dev);
+			}
+		}
 	}
 
 	for (i = 0; i < dev->vlans; i++) {

target/linux/generic/files/drivers/net/phy/mvsw61xx.h

@@ -48,6 +48,14 @@ enum {
 	MV_PORT_STATUS_LINK		= (1 << 11),
 };
 
+enum {
+	MV_PORT_STATUS_CMODE_100BASE_X	= 0x8,
+	MV_PORT_STATUS_CMODE_1000BASE_X	= 0x9,
+	MV_PORT_STATUS_CMODE_SGMII	= 0xa,
+};
+
+#define MV_PORT_STATUS_CMODE_MASK	0xf
+
 enum {
 	MV_PORT_STATUS_SPEED_10		= 0x00,
 	MV_PORT_STATUS_SPEED_100	= 0x01,
@@ -239,6 +247,11 @@ enum {
 	MV_SPEC_DOWNSHIFT_COUNTER	= (0x3 << 12),
 };
 
+#define MII_MV_PAGE			22
+
+#define MV_REG_FIBER_SERDES		0xf
+#define MV_PAGE_FIBER_SERDES		0x1
+
 struct mvsw61xx_state {
 	struct switch_dev dev;
 	struct mii_bus *bus;