diff --git a/drivers/net/phy/motorcomm.c b/drivers/net/phy/motorcomm.c
index a87e83fdf590..5a0cd4d7d633 100644
--- a/drivers/net/phy/motorcomm.c
+++ b/drivers/net/phy/motorcomm.c
@@ -271,6 +271,10 @@
*/
#define YTPHY_WCR_TYPE_PULSE BIT(0)
+#define YTPHY_LED_GENERAL_CFG_REG 0xA00B
+#define YTPHY_LED_FORCE_MODE(x) BIT((x) * 3)
+#define YTPHY_LED_FORCE_EN(x) BIT((x) * 3 + 2)
+
#define YTPHY_PAD_DRIVE_STRENGTH_REG 0xA010
#define YT8531_RGMII_RXC_DS_MASK GENMASK(15, 13)
#define YT8531_RGMII_RXD_DS_HI_MASK BIT(12) /* Bit 2 of rxd_ds */
@@ -1775,6 +1779,43 @@ static int yt8531s_config_init(struct phy_device *phydev)
return phy_restore_page(phydev, old_page, ret);
}
+static int yt8521_hw_led_on_set(struct phy_device *phydev, u8 index,
+ bool value)
+{
+ int mask, reg, val;
+
+ val = ytphy_read_ext(phydev, YTPHY_LED_GENERAL_CFG_REG);
+ if (val < 0)
+ return val;
+
+ mask = YTPHY_LED_FORCE_EN(index) | YTPHY_LED_FORCE_MODE(index);
+ if (value)
+ reg = val | mask;
+ else
+ reg = val & ~mask;
+
+ if (reg == val)
+ return 0;
+
+ return ytphy_write_ext(phydev, YTPHY_LED_GENERAL_CFG_REG, reg);
+}
+
+static int yt8521_led_brightness_set(struct phy_device *phydev, u8 index,
+ enum led_brightness value)
+{
+ int ret;
+
+ if (index >= YT8521_MAX_LEDS)
+ return -EINVAL;
+
+ /* clear HW LED setup */
+ ret = ytphy_write_ext(phydev, YT8521_LED0_CFG_REG + index, 0);
+ if (ret < 0)
+ return ret;
+
+ return yt8521_hw_led_on_set(phydev, index, (value != LED_OFF));
+}
+
static const unsigned long supported_trgs = (BIT(TRIGGER_NETDEV_FULL_DUPLEX) |
BIT(TRIGGER_NETDEV_HALF_DUPLEX) |
BIT(TRIGGER_NETDEV_LINK) |@@ -1801,10 +1842,15 @@ static int yt8521_led_hw_control_set(struct phy_device *phydev, u8 index,
unsigned long rules)
{
u16 val = 0;
+ int ret;
if (index >= YT8521_MAX_LEDS)
return -EINVAL;
+ ret = yt8521_hw_led_on_set(phydev, index, false);
+ if (ret < 0)
+ return ret;
+
if (test_bit(TRIGGER_NETDEV_LINK, &rules)) {
val |= YT8521_LED_10_ON_EN;
val |= YT8521_LED_100_ON_EN;@@ -3266,6 +3312,7 @@ static struct phy_driver motorcomm_phy_drvs[] = {
.soft_reset = yt8521_soft_reset,
.suspend = yt8521_suspend,
.resume = yt8521_resume,
+ .led_brightness_set = yt8521_led_brightness_set,
.led_hw_is_supported = yt8521_led_hw_is_supported,
.led_hw_control_set = yt8521_led_hw_control_set,
.led_hw_control_get = yt8521_led_hw_control_get,@@ -3288,6 +3335,7 @@ static struct phy_driver motorcomm_phy_drvs[] = {
.get_wol = ytphy_get_wol,
.set_wol = yt8531_set_wol,
.link_change_notify = yt8531_link_change_notify,
+ .led_brightness_set = yt8521_led_brightness_set,
.led_hw_is_supported = yt8521_led_hw_is_supported,
.led_hw_control_set = yt8521_led_hw_control_set,
.led_hw_control_get = yt8521_led_hw_control_get,--
2.34.1