From:   =?UTF-8?q?=C3=81lvaro=20Fern=C3=A1ndez=20Rojas?= 
        <noltari@gmail.com>
To:     davem@davemloft.net, edumazet@google.com, kuba@kernel.org,
        pabeni@redhat.com, robh+dt@kernel.org,
        krzysztof.kozlowski+dt@linaro.org, f.fainelli@gmail.com,
        jonas.gorski@gmail.com, andrew@lunn.ch, olteanv@gmail.com,
        hkallweit1@gmail.com, linux@armlinux.org.uk,
        netdev@vger.kernel.org, devicetree@vger.kernel.org,
        linux-kernel@vger.kernel.org
Cc:     =?UTF-8?q?=C3=81lvaro=20Fern=C3=A1ndez=20Rojas?= 
        <noltari@gmail.com>
Subject: [PATCH 2/3] net: dsa: b53: mmap: register MDIO Mux bus controller
Date:   Fri, 17 Mar 2023 12:34:26 +0100
Message-Id: <20230317113427.302162-3-noltari@gmail.com>
In-Reply-To: <20230317113427.302162-1-noltari@gmail.com>
References: <20230317113427.302162-1-noltari@gmail.com>
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
Precedence: bulk

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