From b9867561de79ef76da7ccaa494199ecb1b1abef1 Mon Sep 17 00:00:00 2001
From: Benhur Tessele <benhur.tessele@satcube.com>
Date: Tue, 19 Sep 2023 15:55:59 +0200
Subject: [PATCH] drivers: usb: lan78xx: add support to control LED mode
 register

---
 drivers/net/usb/lan78xx.c | 22 ++++++++++++++++++++++
 drivers/net/usb/lan78xx.h |  3 +++
 2 files changed, 25 insertions(+)

diff --git a/drivers/net/usb/lan78xx.c b/drivers/net/usb/lan78xx.c
index c458c030fadf6..31a08710694da 100644
--- a/drivers/net/usb/lan78xx.c
+++ b/drivers/net/usb/lan78xx.c
@@ -2410,7 +2410,11 @@ static int lan78xx_phy_init(struct lan78xx_net *dev)
 
 	if (phydev->mdio.dev.of_node) {
 		u32 reg;
+		u32 led_modes[4];
+		u16 regval = 0;
 		int len;
+		int shifter = 0;
+		int i;
 
 		len = of_property_count_elems_of_size(phydev->mdio.dev.of_node,
 						      "microchip,led-modes",
@@ -2427,6 +2431,24 @@ static int lan78xx_phy_init(struct lan78xx_net *dev)
 				(len > 2) * HW_CFG_LED2_EN_ |
 				(len > 3) * HW_CFG_LED3_EN_;
 			lan78xx_write_reg(dev, HW_CFG, reg);
+
+			/* Sanity check */
+			len = len > 4 ? 4 : len;
+
+			ret = of_property_read_u32_array(phydev->mdio.dev.of_node,
+							 "microchip,led-modes",
+							 led_modes,
+							 len);
+			if (!ret) {
+				for(i = 0; i < len; i++) {
+					regval |= (led_modes[i] << shifter);
+					shifter += 4;
+				}
+				lan78xx_mdiobus_write(dev->mdiobus,
+						      MII_ACC_PHY_ADDR,
+						      MII_IDX_LED_MODE_REG,
+						      regval);
+			}
 		}
 	}
 
diff --git a/drivers/net/usb/lan78xx.h b/drivers/net/usb/lan78xx.h
index 968e5e5faee0a..b54395795e880 100644
--- a/drivers/net/usb/lan78xx.h
+++ b/drivers/net/usb/lan78xx.h
@@ -604,6 +604,9 @@
 #define MII_DATA			(0x124)
 #define MII_DATA_MASK_			(0x0000FFFF)
 
+#define MII_ACC_PHY_ADDR		(1)
+#define MII_IDX_LED_MODE_REG		(29)
+
 #define MAC_RGMII_ID			(0x128)
 #define MAC_RGMII_ID_TXC_DELAY_EN_	(0x00000002)
 #define MAC_RGMII_ID_RXC_DELAY_EN_	(0x00000001)