[PATCH 7/8] mfd: ocelot: enable support for mdio management

[Rasmus Villemoes <ravi@prevas.dk>]

The implementation is rather straight-forward, following section
3.5.3 (MIIM interface in slave mode) in the data sheet for the
vsc7514.

Since each register access requires multiple MDIO accesses, keep the
parent mii_bus locked for the whole read/write in order that accesses
by different sub-devices do not end up corrupting each other. Since
the MFD among other things exposes an mdio bus to the switch's
internal PHYs, use MDIO_MUTEX_NESTED.

Looking through the data sheets of all of VSC7511, VSC7512, VSC7513,
VSC7514, I haven't seen any indication that they can be controlled
over I2C, so drop that mention from the Kconfig help text and add MDIO
in its place.

Signed-off-by: Rasmus Villemoes <ravi@prevas.dk>
---
 drivers/mfd/Kconfig       |   3 +-
 drivers/mfd/Makefile      |   2 +-
 drivers/mfd/ocelot-mdio.c | 161 ++++++++++++++++++++++++++++++++++++++
 3 files changed, 164 insertions(+), 2 deletions(-)
 create mode 100644 drivers/mfd/ocelot-mdio.c

diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index 4dc894061b62e..c062563794d9e 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -1048,6 +1048,7 @@ config MFD_MENF21BMC
 config MFD_OCELOT
 	tristate "Microsemi Ocelot External Control Support"
 	depends on SPI_MASTER
+	select PHYLIB
 	select MFD_CORE
 	select REGMAP
 	help
@@ -1056,7 +1057,7 @@ config MFD_OCELOT
 	  other functions, including pinctrl, MDIO, and communication with
 	  external chips. While some chips have an internal processor capable of
 	  running an OS, others don't. All chips can be controlled externally
-	  through different interfaces, including SPI, I2C, and PCIe.
+	  through different interfaces, including SPI, MDIO, and PCIe.
 
 	  Say yes here to add support for Ocelot chips (VSC7511, VSC7512,
 	  VSC7513, VSC7514) controlled externally.
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index 9220eaf7cf125..fc675ddd59f17 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -123,7 +123,7 @@ obj-$(CONFIG_MFD_MC13XXX_I2C)	+= mc13xxx-i2c.o
 
 obj-$(CONFIG_MFD_CORE)		+= mfd-core.o
 
-ocelot-soc-objs			:= ocelot-core.o ocelot-spi.o
+ocelot-soc-objs			:= ocelot-core.o ocelot-spi.o ocelot-mdio.o
 obj-$(CONFIG_MFD_OCELOT)	+= ocelot-soc.o
 
 obj-$(CONFIG_EZX_PCAP)		+= ezx-pcap.o
diff --git a/drivers/mfd/ocelot-mdio.c b/drivers/mfd/ocelot-mdio.c
new file mode 100644
index 0000000000000..7ac232a1ad6ad
--- /dev/null
+++ b/drivers/mfd/ocelot-mdio.c
@@ -0,0 +1,161 @@
+// SPDX-License-Identifier: (GPL-2.0 OR MIT)
+/*
+ * MIIM core driver for the Ocelot chip family.
+ */
+
+/*
+ * Each register access requires multiple MDIO accesses.
+ */
+
+#include <linux/device.h>
+#include <linux/mdio.h>
+#include <linux/phy.h>
+#include <linux/regmap.h>
+
+#include "ocelot.h"
+
+#define ADDR_REG0 0
+#define ADDR_REG1 1
+#define DATA_REG0 2
+#define DATA_REG1 3
+
+static int
+ocelot_mdio_write_addr(struct mdio_device *mdiodev, unsigned int addr)
+{
+	int ret;
+
+	addr &= 0x00ffffff;
+	addr >>= 2;
+
+	ret = __mdiodev_write(mdiodev, ADDR_REG0, addr & 0xffff);
+	if (ret)
+		return ret;
+
+	return __mdiodev_write(mdiodev, ADDR_REG1, addr >> 16);
+}
+
+static int
+__ocelot_mdio_write(void *context, unsigned int reg, unsigned int val)
+{
+	struct mdio_device *mdiodev = context;
+	int ret;
+
+	ret = ocelot_mdio_write_addr(mdiodev, reg);
+	if (ret)
+		return ret;
+
+	ret = __mdiodev_write(mdiodev, DATA_REG0, val & 0xffff);
+	if (ret)
+		return ret;
+
+	return __mdiodev_write(mdiodev, DATA_REG1, val >> 16);
+}
+
+static int
+__ocelot_mdio_read(struct mdio_device *mdiodev, unsigned int reg, unsigned int *val)
+{
+	int ret, lo, hi, i;
+
+	ret = ocelot_mdio_write_addr(mdiodev, reg);
+	if (ret)
+		return ret;
+
+	/*
+	 * The data registers must be read twice. Only after the first
+	 * read is the value of the register whose address was written
+	 * into the address registers latched into the data registers.
+	 */
+	for (i = 0; i < 2; ++i) {
+		lo = __mdiodev_read(mdiodev, DATA_REG0);
+		if (lo < 0)
+			return lo;
+		hi = __mdiodev_read(mdiodev, DATA_REG1);
+		if (hi < 0)
+			return hi;
+	}
+
+	*val = (hi << 16) | (lo & 0xffff);
+
+	return 0;
+}
+
+static int
+ocelot_mdio_write(void *context, unsigned int reg, unsigned int val)
+{
+	struct mdio_device *mdiodev = context;
+	int ret;
+
+	mutex_lock_nested(&mdiodev->bus->mdio_lock, MDIO_MUTEX_NESTED);
+	ret = __ocelot_mdio_write(mdiodev, reg, val);
+	mutex_unlock(&mdiodev->bus->mdio_lock);
+
+	return ret;
+}
+
+static int
+ocelot_mdio_read(void *context, unsigned int reg, unsigned int *val)
+{
+	struct mdio_device *mdiodev = context;
+	int ret;
+
+	mutex_lock_nested(&mdiodev->bus->mdio_lock, MDIO_MUTEX_NESTED);
+	ret = __ocelot_mdio_read(mdiodev, reg, val);
+	mutex_unlock(&mdiodev->bus->mdio_lock);
+
+	return ret;
+}
+
+static const struct regmap_bus ocelot_mdio_regmap_bus = {
+	.reg_write = ocelot_mdio_write,
+	.reg_read = ocelot_mdio_read,
+};
+
+static struct regmap *ocelot_mdio_init_regmap(struct device *dev, const struct resource *res)
+{
+	struct mdio_device *mdiodev = to_mdio_device(dev);
+	struct regmap_config regmap_config = {};
+
+	regmap_config.reg_bits = 32;
+	regmap_config.reg_stride = 4;
+	regmap_config.val_bits = 32;
+	regmap_config.name = res->name;
+	regmap_config.max_register = resource_size(res) - 1;
+	regmap_config.reg_base = res->start;
+
+	return devm_regmap_init(dev, &ocelot_mdio_regmap_bus, mdiodev, &regmap_config);
+}
+
+static int ocelot_mdio_probe(struct mdio_device *mdiodev)
+{
+	struct device *dev = &mdiodev->dev;
+	struct ocelot_ddata *ddata;
+
+	ddata = devm_kzalloc(dev, sizeof(*ddata), GFP_KERNEL);
+	if (!ddata)
+		return -ENOMEM;
+
+	dev_set_drvdata(dev, ddata);
+	ddata->init_regmap = ocelot_mdio_init_regmap;
+
+	return ocelot_core_init(dev);
+}
+
+static const struct of_device_id ocelot_mdio_of_match[] = {
+	{ .compatible = "mscc,vsc7512" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, ocelot_mdio_of_match);
+
+static struct mdio_driver ocelot_mdio_driver = {
+	.probe = ocelot_mdio_probe,
+	.mdiodrv.driver = {
+		.name = "ocelot-soc",
+		.of_match_table = ocelot_mdio_of_match,
+	},
+};
+mdio_module_driver(ocelot_mdio_driver);
+
+MODULE_DESCRIPTION("MDIO Controlled Ocelot Chip Driver");
+MODULE_AUTHOR("Rasmus Villemoes <ravi@prevas.dk>");
+MODULE_LICENSE("Dual MIT/GPL");
+MODULE_IMPORT_NS("MFD_OCELOT");
-- 
2.49.0