[v3,2/2] net: phy: add marvell usb to mdio controller

diff --git a/MAINTAINERS b/MAINTAINERS
index ff35669f8712..f36023d4ee44 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -10105,6 +10105,7 @@  MARVELL USB MDIO CONTROLLER DRIVER
 M:	Tobias Waldekranz <tobias@waldekranz.com>
 L:	netdev@vger.kernel.org
 S:	Maintained
+F:	drivers/net/phy/mdio-mvusb.c
 F:	Documentation/devicetree/bindings/net/marvell,mvusb.yaml
 
 MARVELL XENON MMC/SD/SDIO HOST CONTROLLER DRIVER
diff --git a/drivers/net/phy/Kconfig b/drivers/net/phy/Kconfig
index cc7f1df855da..be9ae8620cd1 100644
--- a/drivers/net/phy/Kconfig
+++ b/drivers/net/phy/Kconfig
@@ -179,6 +179,13 @@  config MDIO_MSCC_MIIM
 	  This driver supports the MIIM (MDIO) interface found in the network
 	  switches of the Microsemi SoCs
 
+config MDIO_MVUSB
+	tristate "Marvell USB to MDIO Adapter"
+	depends on OF_MDIO && USB
+	help
+	  A USB to MDIO converter present on development boards for
+	  Marvell's Link Street family of Ethernet switches.
+
 config MDIO_OCTEON
 	tristate "Octeon and some ThunderX SOCs MDIO buses"
 	depends on (64BIT && OF_MDIO) || COMPILE_TEST
diff --git a/drivers/net/phy/Makefile b/drivers/net/phy/Makefile
index 70774ab474e6..2f5c7093a65b 100644
--- a/drivers/net/phy/Makefile
+++ b/drivers/net/phy/Makefile
@@ -40,6 +40,7 @@  obj-$(CONFIG_MDIO_I2C)		+= mdio-i2c.o
 obj-$(CONFIG_MDIO_IPQ8064)	+= mdio-ipq8064.o
 obj-$(CONFIG_MDIO_MOXART)	+= mdio-moxart.o
 obj-$(CONFIG_MDIO_MSCC_MIIM)	+= mdio-mscc-miim.o
+obj-$(CONFIG_MDIO_MVUSB)	+= mdio-mvusb.o
 obj-$(CONFIG_MDIO_OCTEON)	+= mdio-octeon.o
 obj-$(CONFIG_MDIO_SUN4I)	+= mdio-sun4i.o
 obj-$(CONFIG_MDIO_THUNDER)	+= mdio-thunder.o
diff --git a/drivers/net/phy/mdio-mvusb.c b/drivers/net/phy/mdio-mvusb.c
new file mode 100644
index 000000000000..d5eabddfdf51
--- /dev/null
+++ b/drivers/net/phy/mdio-mvusb.c
@@ -0,0 +1,120 @@ 
+// SPDX-License-Identifier: GPL-2.0
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_mdio.h>
+#include <linux/phy.h>
+#include <linux/usb.h>
+
+#define USB_MARVELL_VID	0x1286
+
+static const struct usb_device_id mvusb_mdio_table[] = {
+	{ USB_DEVICE(USB_MARVELL_VID, 0x1fa4) },
+
+	{}
+};
+MODULE_DEVICE_TABLE(usb, mvusb_mdio_table);
+
+enum {
+	MVUSB_CMD_PREAMBLE0,
+	MVUSB_CMD_PREAMBLE1,
+	MVUSB_CMD_ADDR,
+	MVUSB_CMD_VAL,
+};
+
+struct mvusb_mdio {
+	struct usb_device *udev;
+	struct mii_bus *mdio;
+
+	__le16 buf[4];
+};
+
+static int mvusb_mdio_read(struct mii_bus *mdio, int dev, int reg)
+{
+	struct mvusb_mdio *mvusb = mdio->priv;
+	int err, alen;
+
+	if (dev & MII_ADDR_C45)
+		return -EOPNOTSUPP;
+
+	mvusb->buf[MVUSB_CMD_ADDR] = cpu_to_le16(0xa400 | (dev << 5) | reg);
+
+	err = usb_bulk_msg(mvusb->udev, usb_sndbulkpipe(mvusb->udev, 2),
+			   mvusb->buf, 6, &alen, 100);
+	if (err)
+		return err;
+
+	err = usb_bulk_msg(mvusb->udev, usb_rcvbulkpipe(mvusb->udev, 6),
+			   &mvusb->buf[MVUSB_CMD_VAL], 2, &alen, 100);
+	if (err)
+		return err;
+
+	return le16_to_cpu(mvusb->buf[MVUSB_CMD_VAL]);
+}
+
+static int mvusb_mdio_write(struct mii_bus *mdio, int dev, int reg, u16 val)
+{
+	struct mvusb_mdio *mvusb = mdio->priv;
+	int alen;
+
+	if (dev & MII_ADDR_C45)
+		return -EOPNOTSUPP;
+
+	mvusb->buf[MVUSB_CMD_ADDR] = cpu_to_le16(0x8000 | (dev << 5) | reg);
+	mvusb->buf[MVUSB_CMD_VAL]  = cpu_to_le16(val);
+
+	return usb_bulk_msg(mvusb->udev, usb_sndbulkpipe(mvusb->udev, 2),
+			    mvusb->buf, 8, &alen, 100);
+}
+
+static int mvusb_mdio_probe(struct usb_interface *interface,
+			    const struct usb_device_id *id)
+{
+	struct device *dev = &interface->dev;
+	struct mvusb_mdio *mvusb;
+	struct mii_bus *mdio;
+
+	mdio = devm_mdiobus_alloc_size(dev, sizeof(*mvusb));
+	if (!mdio)
+		return -ENOMEM;
+
+	mvusb = mdio->priv;
+	mvusb->mdio = mdio;
+	mvusb->udev = usb_get_dev(interface_to_usbdev(interface));
+
+	/* Reversed from USB PCAPs, no idea what these mean. */
+	mvusb->buf[MVUSB_CMD_PREAMBLE0] = cpu_to_le16(0xe800);
+	mvusb->buf[MVUSB_CMD_PREAMBLE1] = cpu_to_le16(0x0001);
+
+	snprintf(mdio->id, MII_BUS_ID_SIZE, "mvusb-%s", dev_name(dev));
+	mdio->name = mdio->id;
+	mdio->parent = dev;
+	mdio->read = mvusb_mdio_read;
+	mdio->write = mvusb_mdio_write;
+
+	usb_set_intfdata(interface, mvusb);
+	return of_mdiobus_register(mdio, dev->of_node);
+}
+
+static void mvusb_mdio_disconnect(struct usb_interface *interface)
+{
+	struct mvusb_mdio *mvusb = usb_get_intfdata(interface);
+	struct usb_device *udev = mvusb->udev;
+
+	mdiobus_unregister(mvusb->mdio);
+	usb_set_intfdata(interface, NULL);
+	usb_put_dev(udev);
+}
+
+static struct usb_driver mvusb_mdio_driver = {
+	.name       = "mvusb_mdio",
+	.id_table   = mvusb_mdio_table,
+	.probe      = mvusb_mdio_probe,
+	.disconnect = mvusb_mdio_disconnect,
+};
+
+module_usb_driver(mvusb_mdio_driver);
+
+MODULE_AUTHOR("Tobias Waldekranz <tobias@waldekranz.com>");
+MODULE_DESCRIPTION("Marvell USB MDIO Adapter");
+MODULE_LICENSE("GPL");