net: stmmac: dwmac-rk: Set XPCS port1 speed for GMAC1 SGMII mode

When gmac1 is working in SGMII mode, need to additionally
configure the speed of XPCS port1 according to the current
link information, otherwise it will not work.

Signed-off-by: David Wu <david.wu@rock-chips.com>
Change-Id: Ie9407e1566e5ef2804ed50ee203d277f652aca23

drivers/net/ethernet/stmicro/stmmac/dwmac-rk.c

@@ -47,6 +47,7 @@ struct rk_gmac_ops {
 	void (*set_to_qsgmii)(struct rk_priv_data *bsp_priv);
 	void (*set_rgmii_speed)(struct rk_priv_data *bsp_priv, int speed);
 	void (*set_rmii_speed)(struct rk_priv_data *bsp_priv, int speed);
+	void (*set_sgmii_speed)(struct rk_priv_data *bsp_priv, int speed);
 	void (*integrated_phy_powerup)(struct rk_priv_data *bsp_priv);
 };
 
@@ -195,10 +196,10 @@ static int xpcs_setup(struct rk_priv_data *bsp_priv, int mode)
 				   SR_MII_CTRL_AN_ENABLE);
 		}
 	} else {
-		val = xpcs_read(bsp_priv, SR_MII_OFFSET(id) + VR_MII_DIG_CTRL1);
-		xpcs_write(bsp_priv, SR_MII_OFFSET(id) + VR_MII_DIG_CTRL1,
+		val = xpcs_read(bsp_priv, SR_MII_OFFSET(0) + VR_MII_DIG_CTRL1);
+		xpcs_write(bsp_priv, SR_MII_OFFSET(0) + VR_MII_DIG_CTRL1,
 			   val | MII_MAC_AUTO_SW);
-		xpcs_write(bsp_priv, SR_MII_OFFSET(id) + MII_BMCR,
+		xpcs_write(bsp_priv, SR_MII_OFFSET(0) + MII_BMCR,
 			   SR_MII_CTRL_AN_ENABLE);
 	}
 
@@ -1400,13 +1401,42 @@ static void rk3568_set_gmac_speed(struct rk_priv_data *bsp_priv, int speed)
 			__func__, rate, ret);
 }
 
+static void rk3568_set_gmac_sgmii_speed(struct rk_priv_data *bsp_priv, int speed)
+{
+	struct device *dev = &bsp_priv->pdev->dev;
+	unsigned int ctrl;
+
+	/* Only gmac1 set the speed for port1 */
+	if (!bsp_priv->bus_id)
+		return;
+
+	switch (speed) {
+	case 10:
+		ctrl = BMCR_SPEED10;
+		break;
+	case 100:
+		ctrl = BMCR_SPEED100;
+		break;
+	case 1000:
+		ctrl = BMCR_SPEED1000;
+		break;
+	default:
+		dev_err(dev, "unknown speed value for GMAC speed=%d", speed);
+		return;
+	}
+
+	xpcs_write(bsp_priv, SR_MII_OFFSET(bsp_priv->bus_id) + MII_BMCR,
+		   ctrl | BMCR_FULLDPLX);
+}
+
 static const struct rk_gmac_ops rk3568_ops = {
 	.set_to_rgmii = rk3568_set_to_rgmii,
 	.set_to_rmii = rk3568_set_to_rmii,
 	.set_to_sgmii = rk3568_set_to_sgmii,
 	.set_to_qsgmii = rk3568_set_to_qsgmii,
 	.set_rgmii_speed = rk3568_set_gmac_speed,
 	.set_rmii_speed = rk3568_set_gmac_speed,
+	.set_sgmii_speed = rk3568_set_gmac_sgmii_speed,
 };
 
 #define RV1108_GRF_GMAC_CON0		0X0900
@@ -2005,6 +2035,8 @@ static void rk_fix_speed(void *priv, unsigned int speed)
 			bsp_priv->ops->set_rmii_speed(bsp_priv, speed);
 		break;
 	case PHY_INTERFACE_MODE_SGMII:
+		if (bsp_priv->ops && bsp_priv->ops->set_sgmii_speed)
+			bsp_priv->ops->set_sgmii_speed(bsp_priv, speed);
 	case PHY_INTERFACE_MODE_QSGMII:
 		break;
 	default: