From 28baa5e2635285b178326b301f534ed95c65dd01 Mon Sep 17 00:00:00 2001
From: Jonas Gorski <jonas.gorski@gmail.com>
Date: Thu, 29 Sep 2016 11:44:39 +0200
Subject: [PATCH] sfp: retry phy probe if unsuccessful

Some phys seem to take longer than 50 ms to come out of reset, so retry
until we find a phy.

Signed-off-by: Jonas Gorski <jonas.gorski@gmail.com>
---
 drivers/net/phy/sfp.c | 38 +++++++++++++++++++++++++-------------
 1 file changed, 25 insertions(+), 13 deletions(-)

--- a/drivers/net/phy/sfp.c
+++ b/drivers/net/phy/sfp.c
@@ -491,7 +491,7 @@ static void sfp_sm_phy_detach(struct sfp
 	sfp->mod_phy = NULL;
 }
 
-static void sfp_sm_probe_phy(struct sfp *sfp)
+static int sfp_sm_probe_phy(struct sfp *sfp)
 {
 	struct phy_device *phy;
 	int err;
@@ -500,12 +500,12 @@ static void sfp_sm_probe_phy(struct sfp
 
 	phy = mdiobus_scan(sfp->i2c_mii, SFP_PHY_ADDR);
 	if (IS_ERR(phy)) {
+		if (PTR_ERR(phy) == -ENODEV) {
+			dev_dbg(sfp->dev, "no PHY detected\n");
+			return -EAGAIN;
+		}
 		dev_err(sfp->dev, "mdiobus scan returned %ld\n", PTR_ERR(phy));
-		return;
-	}
-	if (!phy) {
-		dev_info(sfp->dev, "no PHY detected\n");
-		return;
+		return PTR_ERR(phy);
 	}
 
 	if (sfp->phylink) {
@@ -515,12 +515,14 @@ static void sfp_sm_probe_phy(struct sfp
 			phy_device_free(phy);
 			dev_err(sfp->dev, "phylink_connect_phy failed: %d\n",
 				err);
-			return;
+			return err;
 		}
 	}
 
 	sfp->mod_phy = phy;
 	phy_start(phy);
+
+	return 0;
 }
 
 static void sfp_sm_link_up(struct sfp *sfp)
@@ -569,30 +571,41 @@ static void sfp_sm_fault(struct sfp *sfp
 
 static void sfp_sm_mod_init(struct sfp *sfp)
 {
+	int ret = 0;
+
 	sfp_module_tx_enable(sfp);
 
-	/* Wait t_init before indicating that the link is up, provided the
-	 * current state indicates no TX_FAULT.  If TX_FAULT clears before
-	 * this time, that's fine too.
-	 */
-	sfp_sm_next(sfp, SFP_S_INIT, T_INIT_JIFFIES);
-	sfp->sm_retries = 5;
+	if (!sfp->phylink)
+		return;
 
-	if (sfp->phylink) {
-		/* Setting the serdes link mode is guesswork: there's no
-		 * field in the EEPROM which indicates what mode should
-		 * be used.
-		 *
-		 * If it's a gigabit-only fiber module, it probably does
-		 * not have a PHY, so switch to 802.3z negotiation mode.
-		 * Otherwise, switch to SGMII mode (which is required to
-		 * support non-gigabit speeds) and probe for a PHY.
+	/* Setting the serdes link mode is guesswork: there's no
+	 * field in the EEPROM which indicates what mode should
+	 * be used.
+	 *
+	 * If it's a gigabit-only fiber module, it probably does
+	 * not have a PHY, so switch to 802.3z negotiation mode.
+	 * Otherwise, switch to SGMII mode (which is required to
+	 * support non-gigabit speeds) and probe for a PHY.
+	 */
+	if (sfp->id.base.e1000_base_t ||
+	    sfp->id.base.e100_base_lx ||
+	    sfp->id.base.e100_base_fx)
+		ret = sfp_sm_probe_phy(sfp);
+
+	if (!ret) {
+		/* Wait t_init before indicating that the link is up, provided
+		 * the current state indicates no TX_FAULT.  If TX_FAULT clears
+		 * this time, that's fine too.
 		 */
-		if (sfp->id.base.e1000_base_t ||
-		    sfp->id.base.e100_base_lx ||
-		    sfp->id.base.e100_base_fx)
-			sfp_sm_probe_phy(sfp);
+		sfp_sm_next(sfp, SFP_S_INIT, T_INIT_JIFFIES);
+		sfp->sm_retries = 5;
+		return;
 	}
+
+	if (ret == -EAGAIN)
+		sfp_sm_set_timer(sfp, T_PROBE_RETRY);
+	else
+		sfp_sm_next(sfp, SFP_S_TX_DISABLE, 0);
 }
 
 static int sfp_sm_mod_probe(struct sfp *sfp)