[PATCH 2/2] net:xilinx_axi: check PCS/PMA PHY status in setup_phy

[Andy Chiu]

Both PCS/PMA PHY and the external PHY need to have a valid link status
in order to have Ethernet traffic. Check and wait this status at
setup_phy() so that we could diagnose if there is a PHY issue.

Signed-off-by: Andy Chiu <andy.chiu at sifive.com>
Reviewed-by: Greentime Hu <greentime.hu at sifive.com>
---
 drivers/net/xilinx_axi_emac.c | 43 +++++++++++++++++++++++++++++++++++
 1 file changed, 43 insertions(+)

diff --git a/drivers/net/xilinx_axi_emac.c b/drivers/net/xilinx_axi_emac.c
index 447e1d4c9c..257ab4661e 100644
--- a/drivers/net/xilinx_axi_emac.c
+++ b/drivers/net/xilinx_axi_emac.c
@@ -336,6 +336,44 @@ static int axiemac_phy_init(struct udevice *dev)
 	return 0;
 }
 
+static int pcs_pma_startup(struct axidma_priv *priv)
+{
+	u32 rc;
+	u32 retry_cnt = 0;
+	u16 mii_reg;
+
+	rc = phyread(priv, priv->pcsaddr, MII_BMCR, &mii_reg);
+	if (rc)
+		goto failed_mdio;
+
+	if (!(mii_reg & BMCR_ANENABLE)) {
+		mii_reg |= BMCR_ANENABLE;
+		if (phywrite(priv, priv->pcsaddr, MII_BMCR, mii_reg))
+			goto failed_mdio;
+	}
+
+	/* Check the internal PHY status and warn user if the internal PHY is
+	 * not resetted
+	 */
+	printf("axiemac: waiting for link status of the PCS/PMA PHY");
+	while (retry_cnt * 10 < PHY_ANEG_TIMEOUT) {
+		rc = phyread(priv, priv->pcsaddr, MII_BMSR, &mii_reg);
+		if ((mii_reg & BMSR_LSTATUS) && mii_reg != 0xffff && !rc) {
+			printf(".Done\n");
+			return 0;
+		}
+
+		printf(".");
+		retry_cnt++;
+		mdelay(10);
+	}
+	printf("\n\tWarning, PCS/PMA PHY is not ready, link is down\n");
+	return 1;
+failed_mdio:
+	printf("axiemac: MDIO to the PCS/PMA PHY has failed\n");
+	return 1;
+}
+
 /* Setting axi emac and phy to proper setting */
 static int setup_phy(struct udevice *dev)
 {
@@ -367,6 +405,11 @@ static int setup_phy(struct udevice *dev)
 		       phydev->dev->name);
 		return 0;
 	}
+	if (priv->interface == PHY_INTERFACE_MODE_SGMII ||
+	    priv->interface == PHY_INTERFACE_MODE_1000BASEX) {
+		if (pcs_pma_startup(priv))
+			return 0;
+	}
 	if (!phydev->link) {
 		printf("%s: No link.\n", phydev->dev->name);
 		return 0;
-- 
2.36.0