[PATCH net-next 2/3] net: phy: marvell10g: add support for configuring LEDs

[Russell King <rmk+kernel@xxxxxxxxxxxxxxx>]

Add support for configuring the LEDs. Macchiatobin has an oddity in
that the left LED goes out when the cable is connected, and flashes
when there's link activity. This is because the reset default for
the LED outputs assume that the LED is connected to supply, not to
ground. Add support for configuring the LED modes and polarities.

Signed-off-by: Russell King <rmk+kernel@xxxxxxxxxxxxxxx>
---
 drivers/net/phy/marvell10g.c | 62 ++++++++++++++++++++++++++++++++----
 1 file changed, 55 insertions(+), 7 deletions(-)

diff --git a/drivers/net/phy/marvell10g.c b/drivers/net/phy/marvell10g.c
index 9a4e12a2af07..88720fa5c611 100644
--- a/drivers/net/phy/marvell10g.c
+++ b/drivers/net/phy/marvell10g.c
@@ -25,6 +25,7 @@
 #include <linux/ctype.h>
 #include <linux/hwmon.h>
 #include <linux/marvell_phy.h>
+#include <linux/of.h>
 #include <linux/phy.h>
 #include <linux/sfp.h>
 
@@ -74,6 +75,8 @@ enum {
 struct mv3310_priv {
 	struct device *hwmon_dev;
 	char *hwmon_name;
+	u8 num_leds;
+	u16 led_mode[4];
 };
 
 #ifdef CONFIG_HWMON
@@ -238,6 +241,43 @@ static const struct sfp_upstream_ops mv3310_sfp_ops = {
 	.module_insert = mv3310_sfp_insert,
 };
 
+static int mv3310_leds_write(struct phy_device *phydev)
+{
+	struct mv3310_priv *priv = dev_get_drvdata(&phydev->mdio.dev);
+	int i, ret;
+
+	for (i = 0; i < priv->num_leds; i++) {
+		ret = phy_write_mmd(phydev, MDIO_MMD_VEND2, 0xf020 + i,
+				    priv->led_mode[i]);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int mv3310_fw_config(struct phy_device *phydev)
+{
+	struct mv3310_priv *priv = dev_get_drvdata(&phydev->mdio.dev);
+	struct device_node *node;
+	int ret;
+
+	node = phydev->mdio.dev.of_node;
+	if (!node)
+		return 0;
+
+	ret = of_property_read_variable_u16_array(node, "marvell,led-mode",
+				priv->led_mode, 1, ARRAY_SIZE(priv->led_mode));
+	if (ret == -EINVAL)
+		ret = 0;
+	if (ret < 0)
+		return ret;
+
+	priv->num_leds = ret;
+
+	return 0;
+}
+
 static int mv3310_probe(struct phy_device *phydev)
 {
 	struct mv3310_priv *priv;
@@ -248,6 +288,20 @@ static int mv3310_probe(struct phy_device *phydev)
 	    (phydev->c45_ids.devices_in_package & mmd_mask) != mmd_mask)
 		return -ENODEV;
 
+	priv = devm_kzalloc(&phydev->mdio.dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	dev_set_drvdata(&phydev->mdio.dev, priv);
+
+	ret = mv3310_fw_config(phydev);
+	if (ret < 0)
+		return ret;
+
+	ret = mv3310_leds_write(phydev);
+	if (ret < 0)
+		return ret;
+
 	ret = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MV_PMA_BOOT);
 	if (ret < 0)
 		return ret;
@@ -258,12 +312,6 @@ static int mv3310_probe(struct phy_device *phydev)
 		return -ENODEV;
 	}
 
-	priv = devm_kzalloc(&phydev->mdio.dev, sizeof(*priv), GFP_KERNEL);
-	if (!priv)
-		return -ENOMEM;
-
-	dev_set_drvdata(&phydev->mdio.dev, priv);
-
 	ret = mv3310_hwmon_probe(phydev);
 	if (ret)
 		return ret;
@@ -316,7 +364,7 @@ static int mv3310_config_init(struct phy_device *phydev)
 	    phydev->interface != PHY_INTERFACE_MODE_10GBASER)
 		return -ENODEV;
 
-	return 0;
+	return mv3310_leds_write(phydev);
 }
 
 static int mv3310_get_features(struct phy_device *phydev)
-- 
2.20.1