[PATCH 2/3] net: dsa: b53: mmap: register MDIO Mux bus controller

["Álvaro Fernández Rojas" <noltari@gmail.com>]

b53 MMAP devices have a MDIO Mux bus controller that must be registered after
properly initializing the switch. If the MDIO Mux controller is registered
from a separate driver and the device has an external switch present, it will
cause a race condition which will hang the device.

Signed-off-by: Álvaro Fernández Rojas <noltari@gmail.com>
---
 drivers/net/dsa/b53/Kconfig    |   1 +
 drivers/net/dsa/b53/b53_mmap.c | 127 ++++++++++++++++++++++++++++++++-
 2 files changed, 127 insertions(+), 1 deletion(-)

diff --git a/drivers/net/dsa/b53/Kconfig b/drivers/net/dsa/b53/Kconfig
index ebaa4a80d544..04450ee1ba82 100644
--- a/drivers/net/dsa/b53/Kconfig
+++ b/drivers/net/dsa/b53/Kconfig
@@ -26,6 +26,7 @@ config B53_MDIO_DRIVER
 config B53_MMAP_DRIVER
 	tristate "B53 MMAP connected switch driver"
 	depends on B53 && HAS_IOMEM
+	select MDIO_BUS_MUX
 	default BCM63XX || BMIPS_GENERIC
 	help
 	  Select to enable support for memory-mapped switches like the BCM63XX
diff --git a/drivers/net/dsa/b53/b53_mmap.c b/drivers/net/dsa/b53/b53_mmap.c
index e968322dfbf0..44becbb12bb5 100644
--- a/drivers/net/dsa/b53/b53_mmap.c
+++ b/drivers/net/dsa/b53/b53_mmap.c
@@ -18,15 +18,31 @@
 
 #include <linux/bits.h>
 #include <linux/kernel.h>
+#include <linux/mdio-mux.h>
 #include <linux/module.h>
 #include <linux/io.h>
+#include <linux/of_mdio.h>
 #include <linux/platform_device.h>
 #include <linux/platform_data/b53.h>
 
 #include "b53_priv.h"
 
+#define REG_MDIOC		0xb0
+#define  REG_MDIOC_EXT_MASK	BIT(16)
+#define  REG_MDIOC_REG_SHIFT	20
+#define  REG_MDIOC_PHYID_SHIFT	25
+#define  REG_MDIOC_RD_MASK	BIT(30)
+#define  REG_MDIOC_WR_MASK	BIT(31)
+
+#define REG_MDIOD		0xb4
+
 struct b53_mmap_priv {
 	void __iomem *regs;
+
+	/* Internal MDIO Mux bus */
+	struct mii_bus *mbus;
+	int ext_phy;
+	void *mux_handle;
 };
 
 static int b53_mmap_read8(struct b53_device *dev, u8 page, u8 reg, u8 *val)
@@ -229,6 +245,111 @@ static const struct b53_io_ops b53_mmap_ops = {
 	.write64 = b53_mmap_write64,
 };
 
+static int b53_mmap_mdiomux_read(struct mii_bus *bus, int phy_id, int loc)
+{
+	struct b53_device *dev = bus->priv;
+	struct b53_mmap_priv *priv = dev->priv;
+	uint32_t reg;
+	uint16_t val;
+
+	b53_mmap_write32(dev, 0, REG_MDIOC, 0);
+
+	reg = REG_MDIOC_RD_MASK |
+	      (phy_id << REG_MDIOC_PHYID_SHIFT) |
+	      (loc << REG_MDIOC_REG_SHIFT);
+	if (priv->ext_phy)
+		reg |= REG_MDIOC_EXT_MASK;
+
+	b53_mmap_write32(dev, 0, REG_MDIOC, reg);
+	udelay(50);
+	b53_mmap_read16(dev, 0, REG_MDIOD, &val);
+
+	return (int) val;
+}
+
+static int b53_mmap_mdiomux_write(struct mii_bus *bus, int phy_id, int loc,
+				  uint16_t val)
+{
+	struct b53_device *dev = bus->priv;
+	struct b53_mmap_priv *priv = dev->priv;
+	uint32_t reg;
+
+	b53_mmap_write32(dev, 0, REG_MDIOC, 0);
+
+	reg = REG_MDIOC_WR_MASK |
+	      (phy_id << REG_MDIOC_PHYID_SHIFT) |
+	      (loc << REG_MDIOC_REG_SHIFT);
+	if (priv->ext_phy)
+		reg |= REG_MDIOC_EXT_MASK;
+	reg |= val;
+
+	b53_mmap_write32(dev, 0, REG_MDIOC, reg);
+	udelay(50);
+
+	return 0;
+}
+
+static int b53_mmap_mdiomux_switch_fn(int current_child, int desired_child,
+				      void *data)
+{
+	struct b53_device *dev = data;
+	struct b53_mmap_priv *priv = dev->priv;
+
+	priv->ext_phy = desired_child;
+
+	return 0;
+}
+
+static int b53_mmap_mdiomux_init(struct b53_device *priv)
+{
+	struct b53_mmap_priv *mpriv = priv->priv;
+	struct device *dev = priv->dev;
+	struct device_node *np = dev->of_node;
+	struct device_node *mnp;
+	struct mii_bus *mbus;
+	int ret;
+
+	mnp = of_get_child_by_name(np, "mdio-mux");
+	if (!mnp)
+		return 0;
+
+	mbus = devm_mdiobus_alloc(dev);
+	if (!mbus) {
+		of_node_put(mnp);
+		return -ENOMEM;
+	}
+
+	mbus->priv = priv;
+	mbus->name = np->full_name;
+	snprintf(mbus->id, MII_BUS_ID_SIZE, "%pOF", np);
+	mbus->parent = dev;
+	mbus->read = b53_mmap_mdiomux_read;
+	mbus->write = b53_mmap_mdiomux_write;
+	mbus->phy_mask = 0x3f;
+
+	ret = devm_of_mdiobus_register(dev, mbus, mnp);
+	if (ret) {
+		of_node_put(mnp);
+		dev_err(dev, "MDIO mux registration failed\n");
+		return ret;
+	}
+
+	ret = mdio_mux_init(dev, mnp, b53_mmap_mdiomux_switch_fn,
+			    &mpriv->mux_handle, priv, mbus);
+	of_node_put(mnp);
+	if (ret) {
+		mdiobus_unregister(mbus);
+		dev_err(dev, "MDIO mux initialization failed\n");
+		return ret;
+	}
+
+	dev_info(dev, "MDIO mux bus init\n");
+
+	mpriv->mbus = mbus;
+
+	return 0;
+}
+
 static int b53_mmap_probe_of(struct platform_device *pdev,
 			     struct b53_platform_data **ppdata)
 {
@@ -306,7 +427,11 @@ static int b53_mmap_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, dev);
 
-	return b53_switch_register(dev);
+	ret = b53_switch_register(dev);
+	if (ret)
+		return ret;
+
+	return b53_mmap_mdiomux_init(dev);
 }
 
 static int b53_mmap_remove(struct platform_device *pdev)
-- 
2.30.2