[PATCH net-next 5/9] net: dsa: vsc73xx: use mutex to mdio operations

[Pawel Dembicki <paweldembicki@gmail.com>]

vsc73xx needs mutex during mdio bus access to avoid races. Without it,
phys are misconfigured and bus operations aren't work as expected.

Signed-off-by: Pawel Dembicki <paweldembicki@gmail.com>
---
 drivers/net/dsa/vitesse-vsc73xx-core.c | 59 ++++++++++++++++++++------
 drivers/net/dsa/vitesse-vsc73xx.h      |  2 +
 2 files changed, 47 insertions(+), 14 deletions(-)

diff --git a/drivers/net/dsa/vitesse-vsc73xx-core.c b/drivers/net/dsa/vitesse-vsc73xx-core.c
index 5eb37dee2261..40c64ef7e729 100644
--- a/drivers/net/dsa/vitesse-vsc73xx-core.c
+++ b/drivers/net/dsa/vitesse-vsc73xx-core.c
@@ -545,6 +545,18 @@ static int vsc73xx_detect(struct vsc73xx *vsc)
 	return 0;
 }
 
+static int vsc73xx_mdio_busy_check(struct vsc73xx *vsc)
+{
+	int val, ret;
+
+	ret = vsc73xx_read(vsc, VSC73XX_BLOCK_MII, VSC73XX_BLOCK_MII_INTERNAL,
+			   VSC73XX_MII_STAT, &val);
+	if (ret)
+		return ret;
+
+	return (val & VSC73XX_MII_STAT_BUSY) ? -EBUSY : 0;
+}
+
 static int vsc73xx_phy_read(struct dsa_switch *ds, int phy, int regnum)
 {
 	struct vsc73xx *vsc = ds->priv;
@@ -552,6 +564,12 @@ static int vsc73xx_phy_read(struct dsa_switch *ds, int phy, int regnum)
 	u32 val;
 	int ret;
 
+	mutex_lock(&vsc->mdio_lock);
+
+	ret = vsc73xx_mdio_busy_check(vsc);
+	if (ret)
+		goto err;
+
 	/* Setting bit 26 means "read" */
 	cmd = VSC73XX_MII_CMD_OPERATION |
 	      FIELD_PREP(VSC73XX_MII_CMD_PHY_ADDR, phy) |
@@ -559,23 +577,27 @@ static int vsc73xx_phy_read(struct dsa_switch *ds, int phy, int regnum)
 	ret = vsc73xx_write(vsc, VSC73XX_BLOCK_MII, VSC73XX_BLOCK_MII_INTERNAL,
 			    VSC73XX_MII_CMD, cmd);
 	if (ret)
-		return ret;
-	msleep(2);
+		goto err;
+	usleep_range(100, 200);
 	ret = vsc73xx_read(vsc, VSC73XX_BLOCK_MII, VSC73XX_BLOCK_MII_INTERNAL,
 			   VSC73XX_MII_DATA, &val);
 	if (ret)
-		return ret;
+		goto err;
+
 	if (val & VSC73XX_MII_DATA_FAILURE) {
 		dev_err(vsc->dev, "reading reg %02x from phy%d failed\n",
 			regnum, phy);
-		return -EIO;
+		ret = -EIO;
+		goto err;
 	}
-	val &= VSC73XX_MII_DATA_READ_DATA;
+	ret = val & VSC73XX_MII_DATA_READ_DATA;
 
 	dev_dbg(vsc->dev, "read reg %02x from phy%d = %04x\n",
-		regnum, phy, val);
+		regnum, phy, ret);
 
-	return val;
+err:
+	mutex_unlock(&vsc->mdio_lock);
+	return ret;
 }
 
 static int vsc73xx_phy_write(struct dsa_switch *ds, int phy, int regnum,
@@ -583,7 +605,13 @@ static int vsc73xx_phy_write(struct dsa_switch *ds, int phy, int regnum,
 {
 	struct vsc73xx *vsc = ds->priv;
 	u32 cmd;
-	int ret;
+	int ret = 0;
+
+	mutex_lock(&vsc->mdio_lock);
+
+	ret = vsc73xx_mdio_busy_check(vsc);
+	if (ret)
+		goto err;
 
 	/* It was found through tedious experiments that this router
 	 * chip really hates to have it's PHYs reset. They
@@ -601,12 +629,13 @@ static int vsc73xx_phy_write(struct dsa_switch *ds, int phy, int regnum,
 	      FIELD_PREP(VSC73XX_MII_CMD_WRITE_DATA, val);
 	ret = vsc73xx_write(vsc, VSC73XX_BLOCK_MII, VSC73XX_BLOCK_MII_INTERNAL,
 			    VSC73XX_MII_CMD, cmd);
-	if (ret)
-		return ret;
-
-	dev_dbg(vsc->dev, "write %04x to reg %02x in phy%d\n",
-		val, regnum, phy);
-	return 0;
+	if (!ret)
+		dev_dbg(vsc->dev, "write %04x to reg %02x in phy%d\n",
+			val, regnum, phy);
+	usleep_range(100, 200);
+err:
+	mutex_unlock(&vsc->mdio_lock);
+	return ret;
 }
 
 static enum dsa_tag_protocol vsc73xx_get_tag_protocol(struct dsa_switch *ds,
@@ -1973,6 +2002,8 @@ int vsc73xx_probe(struct vsc73xx *vsc)
 		return -ENODEV;
 	}
 
+	mutex_init(&vsc->mdio_lock);
+
 	eth_random_addr(vsc->addr);
 	dev_info(vsc->dev,
 		 "MAC for control frames: %02X:%02X:%02X:%02X:%02X:%02X\n",
diff --git a/drivers/net/dsa/vitesse-vsc73xx.h b/drivers/net/dsa/vitesse-vsc73xx.h
index 3ca579acc798..fc0fa8f5f57c 100644
--- a/drivers/net/dsa/vitesse-vsc73xx.h
+++ b/drivers/net/dsa/vitesse-vsc73xx.h
@@ -45,6 +45,7 @@ struct vsc73xx_portinfo {
  * @vlans: List of configured vlans. Contains port mask and untagged status of
  *	every vlan configured in port vlan operation. It doesn't cover tag_8021q
  *	vlans.
+ * @mdio_lock: Mutex used in mdio operations
  */
 struct vsc73xx {
 	struct device			*dev;
@@ -57,6 +58,7 @@ struct vsc73xx {
 	void				*priv;
 	struct vsc73xx_portinfo		portinfo[VSC73XX_MAX_NUM_PORTS];
 	struct list_head		vlans;
+	struct mutex			mdio_lock;
 };
 
 /**
-- 
2.34.1