From 9e51a2fa9f7aa8bace041a943b517faa47f30624 Mon Sep 17 00:00:00 2001
From: Tobias Waldekranz <tobias@waldekranz.com>
Date: Wed, 15 Nov 2023 20:58:42 +0100
Subject: [PATCH 03/33] net: phy: marvell10g: Add LED support for 88X3310
Organization: Wires

Pickup the LEDs from the state in which the hardware reset or
bootloader left them, but also support further configuration via
device tree. This is primarily needed because the electrical polarity
and drive behavior is software controlled and not possible to set via
hardware strapping.

Trigger support:
- "none"
- "timer": Map 60-100 ms periods to the fast rate (81ms) and 1000-1600
  	   ms periods to the slow rate (1300ms). Defer everything else to
	   software blinking
- "netdev": Offload link or duplex information to the solid behavior;
  	    tx and/or rx activity to blink behavior.
---
 drivers/net/phy/marvell10g.c | 422 +++++++++++++++++++++++++++++++++++
 1 file changed, 422 insertions(+)

diff --git a/drivers/net/phy/marvell10g.c b/drivers/net/phy/marvell10g.c
index 40439db49601..7ae4744f147b 100644
--- a/drivers/net/phy/marvell10g.c
+++ b/drivers/net/phy/marvell10g.c
@@ -28,6 +28,7 @@
 #include <linux/firmware.h>
 #include <linux/hwmon.h>
 #include <linux/marvell_phy.h>
+#include <linux/of.h>
 #include <linux/phy.h>
 #include <linux/sfp.h>
 #include <linux/netdevice.h>
@@ -133,6 +134,16 @@ enum {
 	MV_V2_33X0_PORT_CTRL_MACTYPE_10GBASER_NO_SGMII_AN	= 0x5,
 	MV_V2_33X0_PORT_CTRL_MACTYPE_10GBASER_RATE_MATCH	= 0x6,
 	MV_V2_33X0_PORT_CTRL_MACTYPE_USXGMII			= 0x7,
+	MV_V2_LED0_CONTROL	= 0xf020,
+	MV_V2_LED_CONTROL_POLARITY_MASK		= 0x0003,
+	MV_V2_LED_CONTROL_POLARITY_SHIFT	= 0,
+	MV_V2_LED_CONTROL_ACTIVE_HIGH		= BIT(0),
+	MV_V2_LED_CONTROL_TRISTATE		= BIT(1),
+	MV_V2_LED_CONTROL_BLINK_RATE		= BIT(2),
+	MV_V2_LED_CONTROL_SOLID_FUNC_MASK	= 0x00f8,
+	MV_V2_LED_CONTROL_SOLID_FUNC_SHIFT	= 3,
+	MV_V2_LED_CONTROL_BLINK_FUNC_MASK	= 0x1f00,
+	MV_V2_LED_CONTROL_BLINK_FUNC_SHIFT	= 8,
 	MV_V2_PORT_INTR_STS		= 0xf040,
 	MV_V2_PORT_INTR_MASK		= 0xf043,
 	MV_V2_PORT_INTR_STS_WOL_EN	= BIT(8),
@@ -161,6 +172,7 @@ struct mv3310_mactype {
 struct mv3310_chip {
 	bool (*has_downshift)(struct phy_device *phydev);
 	void (*init_supported_interfaces)(unsigned long *mask);
+	int (*leds_probe)(struct phy_device *phydev);
 	int (*get_mactype)(struct phy_device *phydev);
 	int (*set_mactype)(struct phy_device *phydev, int mactype);
 	int (*select_mactype)(unsigned long *interfaces);
@@ -175,6 +187,13 @@ struct mv3310_chip {
 #endif
 };
 
+#define MV3310_N_LEDS 4
+
+struct mv3310_led {
+	u8 index;
+	u16 ctrl;
+};
+
 struct mv3310_priv {
 	DECLARE_BITMAP(supported_interfaces, PHY_INTERFACE_MODE_MAX);
 	const struct mv3310_mactype *mactype;
@@ -184,6 +203,8 @@ struct mv3310_priv {
 
 	struct device *hwmon_dev;
 	char *hwmon_name;
+
+	struct mv3310_led led[MV3310_N_LEDS];
 };
 
 static const struct mv3310_chip *to_mv3310_chip(struct phy_device *phydev)
@@ -191,6 +212,396 @@ static const struct mv3310_chip *to_mv3310_chip(struct phy_device *phydev)
 	return phydev->drv->driver_data;
 }
 
+enum mv3310_led_func {
+	MV3310_LED_FUNC_OFF		 = 0x00,
+	MV3310_LED_FUNC_TX_RX		 = 0x01,
+	MV3310_LED_FUNC_TX		 = 0x02,
+	MV3310_LED_FUNC_RX		 = 0x03,
+	MV3310_LED_FUNC_COLLISION	 = 0x04,
+	MV3310_LED_FUNC_LINK_COPPER	 = 0x05,
+	MV3310_LED_FUNC_LINK_FIBER	 = 0x06,
+	MV3310_LED_FUNC_LINK		 = 0x07,
+	MV3310_LED_FUNC_10M_LINK	 = 0x08,
+	MV3310_LED_FUNC_100M_LINK	 = 0x09,
+	MV3310_LED_FUNC_1G_LINK		 = 0x0a,
+	MV3310_LED_FUNC_10G_LINK	 = 0x0b,
+	MV3310_LED_FUNC_10M_100M_LINK	 = 0x0c,
+	MV3310_LED_FUNC_10M_100M_1G_LINK = 0x0d,
+	MV3310_LED_FUNC_100M_10G_LINK	 = 0x0e,
+	MV3310_LED_FUNC_1G_10G_LINK	 = 0x0f,
+	MV3310_LED_FUNC_1G_10G_SLAVE	 = 0x10,
+	MV3310_LED_FUNC_1G_10G_MASTER	 = 0x11,
+	MV3310_LED_FUNC_HALF_DUPLEX	 = 0x12,
+	MV3310_LED_FUNC_FULL_DUPLEX	 = 0x13,
+	MV3310_LED_FUNC_LINK_EEE	 = 0x14,
+	MV3310_LED_FUNC_2G5_LINK	 = 0x15,
+	MV3310_LED_FUNC_5G_LINK		 = 0x16,
+	MV3310_LED_FUNC_ON		 = 0x17,
+	MV3310_LED_FUNC_2G5_5G_SLAVE	 = 0x18,
+	MV3310_LED_FUNC_2G5_5G_MASTER	 = 0x19,
+
+	MV3310_LED_FUNC_SPEED_BLINK	 = 0x1f
+};
+
+static int mv3310_led_flags_from_funcs(struct mv3310_led *led,
+				       enum mv3310_led_func solid,
+				       enum mv3310_led_func blink,
+				       unsigned long *flagsp)
+{
+	unsigned long flags = 0;
+
+	switch (solid) {
+	case MV3310_LED_FUNC_OFF:
+		break;
+	case MV3310_LED_FUNC_LINK_COPPER:
+	case MV3310_LED_FUNC_LINK_FIBER:
+	case MV3310_LED_FUNC_LINK:
+		flags |= TRIGGER_NETDEV_LINK;
+		break;
+	case MV3310_LED_FUNC_HALF_DUPLEX:
+		flags |= TRIGGER_NETDEV_HALF_DUPLEX;
+		break;
+	case MV3310_LED_FUNC_FULL_DUPLEX:
+		flags |= TRIGGER_NETDEV_FULL_DUPLEX;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	switch (blink) {
+	case MV3310_LED_FUNC_OFF:
+		break;
+	case MV3310_LED_FUNC_TX:
+		flags |= TRIGGER_NETDEV_TX;
+		break;
+	case MV3310_LED_FUNC_RX:
+		flags |= TRIGGER_NETDEV_RX;
+		break;
+	case MV3310_LED_FUNC_TX_RX:
+		flags |= TRIGGER_NETDEV_TX | TRIGGER_NETDEV_RX;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	*flagsp = flags;
+	return 0;
+}
+
+static int mv3310_led_funcs_from_flags(struct mv3310_led *led,
+				       unsigned long flags,
+				       enum mv3310_led_func *solid,
+				       enum mv3310_led_func *blink)
+{
+	unsigned long activity, duplex, link;
+
+	if (flags & ~(BIT(TRIGGER_NETDEV_LINK) |
+		      BIT(TRIGGER_NETDEV_HALF_DUPLEX) |
+		      BIT(TRIGGER_NETDEV_FULL_DUPLEX) |
+		      BIT(TRIGGER_NETDEV_TX) |
+		      BIT(TRIGGER_NETDEV_RX)))
+		return -EINVAL;
+
+	link = flags & BIT(TRIGGER_NETDEV_LINK);
+
+	duplex = flags & (BIT(TRIGGER_NETDEV_HALF_DUPLEX) |
+			  BIT(TRIGGER_NETDEV_FULL_DUPLEX));
+
+	activity = flags & (BIT(TRIGGER_NETDEV_TX) |
+			    BIT(TRIGGER_NETDEV_RX));
+
+	if (link && duplex)
+		return -EINVAL;
+
+	if (solid) {
+		if (link) {
+			*solid = MV3310_LED_FUNC_LINK;
+		} else if (duplex) {
+			switch (duplex) {
+			case BIT(TRIGGER_NETDEV_HALF_DUPLEX):
+				*solid = MV3310_LED_FUNC_HALF_DUPLEX;
+				break;
+			case BIT(TRIGGER_NETDEV_FULL_DUPLEX):
+				*solid = MV3310_LED_FUNC_FULL_DUPLEX;
+				break;
+			default:
+				break;
+			}
+		} else {
+			*solid = MV3310_LED_FUNC_OFF;
+		}
+	}
+
+	if (blink) {
+		switch (activity) {
+		case 0:
+			*blink = MV3310_LED_FUNC_OFF;
+			break;
+		case BIT(TRIGGER_NETDEV_TX):
+			*blink = MV3310_LED_FUNC_TX;
+			break;
+		case BIT(TRIGGER_NETDEV_RX):
+			*blink = MV3310_LED_FUNC_RX;
+			break;
+		default:
+			*blink = MV3310_LED_FUNC_TX_RX;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+static int mv3310_led_get(struct phy_device *phydev,
+			  struct mv3310_led *led,
+			  enum mv3310_led_func *solid,
+			  enum mv3310_led_func *blink,
+			  bool *slow_blink)
+{
+	int ctrl;
+
+	ctrl = phy_read_mmd(phydev, MDIO_MMD_VEND2,
+			    MV_V2_LED0_CONTROL + led->index);
+	if (ctrl < 0)
+		return ctrl;
+
+	*solid = (ctrl & MV_V2_LED_CONTROL_SOLID_FUNC_MASK) >>
+		MV_V2_LED_CONTROL_SOLID_FUNC_SHIFT;
+	*blink = (ctrl & MV_V2_LED_CONTROL_BLINK_FUNC_MASK) >>
+		MV_V2_LED_CONTROL_BLINK_FUNC_SHIFT;
+
+	*slow_blink = !!(ctrl & MV_V2_LED_CONTROL_BLINK_RATE);
+	return 0;
+}
+
+static int mv3310_led_set(struct phy_device *phydev,
+			  struct mv3310_led *led,
+			  enum mv3310_led_func solid,
+			  enum mv3310_led_func blink,
+			  bool slow_blink)
+{
+	u16 ctrl = led->ctrl;
+
+	ctrl &= ~MV_V2_LED_CONTROL_SOLID_FUNC_MASK;
+	ctrl &= ~MV_V2_LED_CONTROL_BLINK_FUNC_MASK;
+	ctrl &= ~MV_V2_LED_CONTROL_BLINK_RATE;
+
+	ctrl |= solid << MV_V2_LED_CONTROL_SOLID_FUNC_SHIFT;
+	ctrl |= blink << MV_V2_LED_CONTROL_BLINK_FUNC_SHIFT;
+
+	if (slow_blink)
+		ctrl |= MV_V2_LED_CONTROL_BLINK_RATE;
+
+	return phy_write_mmd(phydev, MDIO_MMD_VEND2,
+			     MV_V2_LED0_CONTROL + led->index, ctrl);
+}
+
+static int mv3310_led_polarity_set(struct phy_device *phydev,
+				   int index, unsigned long modes)
+{
+	struct mv3310_priv *priv = dev_get_drvdata(&phydev->mdio.dev);
+	struct mv3310_led *led;
+	u16 ctrl;
+	int err;
+
+	if (index < 0 || index >= MV3310_N_LEDS)
+		return -ENODEV;
+
+	led = &priv->led[index];
+	ctrl = led->ctrl;
+
+	if (test_bit(PHY_LED_ACTIVE_LOW, &modes))
+		ctrl &= ~MV_V2_LED_CONTROL_ACTIVE_HIGH;
+	else
+		ctrl |= MV_V2_LED_CONTROL_ACTIVE_HIGH;
+
+	if (test_bit(PHY_LED_INACTIVE_HIGH_IMPEDANCE, &modes))
+		ctrl |= MV_V2_LED_CONTROL_TRISTATE;
+	else
+		ctrl &= ~MV_V2_LED_CONTROL_TRISTATE;
+
+	err = phy_write_mmd(phydev, MDIO_MMD_VEND2,
+			    MV_V2_LED0_CONTROL + index, ctrl);
+	if (err)
+		return err;
+
+	led->ctrl = ctrl;
+	return 0;
+}
+
+static int mv3310_led_brightness_set(struct phy_device *phydev,
+				     u8 index, enum led_brightness value)
+{
+	struct mv3310_priv *priv = dev_get_drvdata(&phydev->mdio.dev);
+	enum mv3310_led_func solid;
+	struct mv3310_led *led;
+
+	if (index >= MV3310_N_LEDS)
+		return -ENODEV;
+
+	led = &priv->led[index];
+
+	if (value == LED_OFF)
+		solid = MV3310_LED_FUNC_OFF;
+	else
+		solid = MV3310_LED_FUNC_ON;
+
+	return mv3310_led_set(phydev, led, solid, MV3310_LED_FUNC_OFF, false);
+}
+
+static int mv3310_led_blink_set(struct phy_device *phydev, u8 index,
+				unsigned long *delay_on,
+				unsigned long *delay_off)
+{
+	struct mv3310_priv *priv = dev_get_drvdata(&phydev->mdio.dev);
+	struct mv3310_led *led;
+	bool slow_blink;
+	int err;
+
+	if (index >= MV3310_N_LEDS)
+		return -ENODEV;
+
+	led = &priv->led[index];
+
+	if (*delay_on != *delay_off)
+		/* Defer anything other than 50% duty cycles to
+		 * software.
+		 */
+		return -EINVAL;
+
+	/* Accept values within ~20% of our supported rates (80ms or
+	 * 1300ms periods).
+	 */
+	if ((*delay_on >= 30) && (*delay_on <= 50))
+		slow_blink = false;
+	else if (((*delay_on >= 500) && (*delay_on <= 800)) || (*delay_on == 0))
+		slow_blink = true;
+	else
+		return -EINVAL;
+
+	err = mv3310_led_set(phydev, led, MV3310_LED_FUNC_OFF,
+			     MV3310_LED_FUNC_ON, slow_blink);
+	if (!err)
+		*delay_on = *delay_off = slow_blink ? 650 : 40;
+
+	return err;
+}
+
+static int mv3310_led_hw_is_supported(struct phy_device *phydev, u8 index,
+				      unsigned long flags)
+{
+	struct mv3310_priv *priv = dev_get_drvdata(&phydev->mdio.dev);
+	struct mv3310_led *led;
+
+	if (index >= MV3310_N_LEDS)
+		return -ENODEV;
+
+	led = &priv->led[index];
+
+	return mv3310_led_funcs_from_flags(led, flags, NULL, NULL);
+}
+
+static int mv3310_led_hw_control_set(struct phy_device *phydev, u8 index,
+				     unsigned long flags)
+{
+	struct mv3310_priv *priv = dev_get_drvdata(&phydev->mdio.dev);
+	enum mv3310_led_func solid, blink;
+	struct mv3310_led *led;
+	int err;
+
+	if (index >= MV3310_N_LEDS)
+		return -ENODEV;
+
+	led = &priv->led[index];
+
+	err = mv3310_led_funcs_from_flags(led, flags, &solid, &blink);
+	if (err)
+		return err;
+
+	return mv3310_led_set(phydev, led, solid, blink, false);
+}
+
+static int mv3310_led_hw_control_get(struct phy_device *phydev, u8 index,
+				     unsigned long *flags)
+{
+	struct mv3310_priv *priv = dev_get_drvdata(&phydev->mdio.dev);
+	enum mv3310_led_func solid, blink;
+	struct mv3310_led *led;
+	bool slow_blink;
+	int err;
+
+	if (index >= MV3310_N_LEDS)
+		return -ENODEV;
+
+	led = &priv->led[index];
+
+	err = mv3310_led_get(phydev, led, &solid, &blink, &slow_blink);
+	if (err)
+		return err;
+
+	return  mv3310_led_flags_from_funcs(led, solid, blink, flags);
+}
+
+static int mv3310_led_probe_of(struct phy_device *phydev,
+			       struct device_node *np)
+{
+	u32 index;
+	int err;
+
+	err = of_property_read_u32(np, "reg", &index);
+	if (err)
+		return err;
+
+	if (index >= MV3310_N_LEDS)
+		return -EINVAL;
+
+	/* mv3310_led_polarity_set() will not be called unless the
+	 * device tree specifies a mode that of_phy_led() considers to
+	 * be a non-default configuration. I.e., something other than
+	 * (active-high, drive low on inactive). But the 3310's HW
+	 * default is (active-low, drive low on inactive). Therefore,
+	 * configure all LEDs defined in the DT with the kernel
+	 * defaults, and then let of_phy_led() set the final value if
+	 * the config deviates from that.
+	 */
+	return mv3310_led_polarity_set(phydev, index, 0);
+}
+
+static int mv3310_leds_probe(struct phy_device *phydev)
+{
+	struct mv3310_priv *priv = dev_get_drvdata(&phydev->mdio.dev);
+	struct device_node *node = phydev->mdio.dev.of_node;
+	struct device_node *pnp, *np;
+	int err, val, index;
+
+	for (index = 0; index < MV3310_N_LEDS; index++) {
+		val = phy_read_mmd(phydev, MDIO_MMD_VEND2,
+				   MV_V2_LED0_CONTROL + index);
+		if (val < 0)
+			return val;
+
+		priv->led[index] = (struct mv3310_led) {
+			.index = index,
+			.ctrl = val,
+		};
+	}
+
+	if (!node)
+		return 0;
+
+	pnp = of_get_child_by_name(node, "leds");
+	if (!pnp)
+		return 0;
+
+	for_each_available_child_of_node(pnp, np) {
+		err = mv3310_led_probe_of(phydev, np);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
 #ifdef CONFIG_HWMON
 static umode_t mv3310_hwmon_is_visible(const void *data,
 				       enum hwmon_sensor_types type,
@@ -712,6 +1123,10 @@ static int mv3310_probe(struct phy_device *phydev)
 	if (ret)
 		return ret;
 
+	ret = chip->leds_probe ? chip->leds_probe(phydev) : 0;
+	if (ret)
+		return ret;
+
 	chip->init_supported_interfaces(priv->supported_interfaces);
 
 	return phy_sfp_probe(phydev, &mv3310_sfp_ops);
@@ -1364,6 +1779,7 @@ static void mv2111_init_supported_interfaces(unsigned long *mask)
 static const struct mv3310_chip mv3310_type = {
 	.has_downshift = mv3310_has_downshift,
 	.init_supported_interfaces = mv3310_init_supported_interfaces,
+	.leds_probe = mv3310_leds_probe,
 	.get_mactype = mv3310_get_mactype,
 	.set_mactype = mv3310_set_mactype,
 	.select_mactype = mv3310_select_mactype,
@@ -1577,6 +1993,12 @@ static struct phy_driver mv3310_drivers[] = {
 		.set_loopback	= genphy_c45_loopback,
 		.get_wol	= mv3110_get_wol,
 		.set_wol	= mv3110_set_wol,
+		.led_polarity_set = mv3310_led_polarity_set,
+		.led_brightness_set = mv3310_led_brightness_set,
+		.led_blink_set	= mv3310_led_blink_set,
+		.led_hw_is_supported = mv3310_led_hw_is_supported,
+		.led_hw_control_set = mv3310_led_hw_control_set,
+		.led_hw_control_get = mv3310_led_hw_control_get,
 	},
 	{
 		.phy_id		= MARVELL_PHY_ID_88X3310,
