1 files changed, 62 insertions, 44 deletions

diff --git a/drivers/net/phy/phy_led_triggers.c b/drivers/net/phy/phy_led_triggers.c
index 94ca42e630bb..60893691d4c3 100644
--- a/drivers/net/phy/phy_led_triggers.c
+++ b/drivers/net/phy/phy_led_triggers.c
@@ -1,20 +1,12 @@
-/* Copyright (C) 2016 National Instruments Corp.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
-  * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
+// SPDX-License-Identifier: GPL-2.0+
+/* Copyright (C) 2016 National Instruments Corp. */
 #include <linux/leds.h>
 #include <linux/phy.h>
 #include <linux/phy_led_triggers.h>
 #include <linux/netdevice.h>
 
+#include "phylib-internal.h"
+
 static struct phy_led_trigger *phy_speed_to_led_trigger(struct phy_device *phy,
 							unsigned int speed)
 {
@@ -27,12 +19,21 @@ static struct phy_led_trigger *phy_speed_to_led_trigger(struct phy_device *phy,
 	return NULL;
 }
 
+static void phy_led_trigger_no_link(struct phy_device *phy)
+{
+	if (phy->last_triggered) {
+		led_trigger_event(&phy->last_triggered->trigger, LED_OFF);
+		led_trigger_event(&phy->led_link_trigger->trigger, LED_OFF);
+		phy->last_triggered = NULL;
+	}
+}
+
 void phy_led_trigger_change_speed(struct phy_device *phy)
 {
 	struct phy_led_trigger *plt;
 
 	if (!phy->link)
-		goto out_change_speed;
+		return phy_led_trigger_no_link(phy);
 
 	if (phy->speed == 0)
 		return;
@@ -42,43 +43,36 @@ void phy_led_trigger_change_speed(struct phy_device *phy)
 		netdev_alert(phy->attached_dev,
 			     "No phy led trigger registered for speed(%d)\n",
 			     phy->speed);
-		goto out_change_speed;
+		return phy_led_trigger_no_link(phy);
 	}
 
 	if (plt != phy->last_triggered) {
-		led_trigger_event(&phy->last_triggered->trigger, LED_OFF);
+		if (!phy->last_triggered)
+			led_trigger_event(&phy->led_link_trigger->trigger,
+					  LED_FULL);
+		else
+			led_trigger_event(&phy->last_triggered->trigger, LED_OFF);
+
 		led_trigger_event(&plt->trigger, LED_FULL);
 		phy->last_triggered = plt;
 	}
-	return;
-
-out_change_speed:
-	if (phy->last_triggered) {
-		led_trigger_event(&phy->last_triggered->trigger,
-				  LED_OFF);
-		phy->last_triggered = NULL;
-	}
 }
 EXPORT_SYMBOL_GPL(phy_led_trigger_change_speed);
 
+static void phy_led_trigger_format_name(struct phy_device *phy, char *buf,
+					size_t size, const char *suffix)
+{
+	snprintf(buf, size, PHY_ID_FMT ":%s",
+		 phy->mdio.bus->id, phy->mdio.addr, suffix);
+}
+
 static int phy_led_trigger_register(struct phy_device *phy,
 				    struct phy_led_trigger *plt,
-				    unsigned int speed)
+				    unsigned int speed,
+				    const char *suffix)
 {
-	char name_suffix[PHY_LED_TRIGGER_SPEED_SUFFIX_SIZE];
-
 	plt->speed = speed;
-
-	if (speed < SPEED_1000)
-		snprintf(name_suffix, sizeof(name_suffix), "%dMbps", speed);
-	else if (speed == SPEED_2500)
-		snprintf(name_suffix, sizeof(name_suffix), "2.5Gbps");
-	else
-		snprintf(name_suffix, sizeof(name_suffix), "%dGbps",
-			 DIV_ROUND_CLOSEST(speed, 1000));
-
-	snprintf(plt->name, sizeof(plt->name), PHY_ID_FMT ":%s",
-		 phy->mdio.bus->id, phy->mdio.addr, name_suffix);
+	phy_led_trigger_format_name(phy, plt->name, sizeof(plt->name), suffix);
 	plt->trigger.name = plt->name;
 
 	return led_trigger_register(&plt->trigger);
@@ -99,18 +93,29 @@ int phy_led_triggers_register(struct phy_device *phy)
 	if (!phy->phy_num_led_triggers)
 		return 0;
 
-	phy->phy_led_triggers = devm_kzalloc(&phy->mdio.dev,
-					    sizeof(struct phy_led_trigger) *
-						   phy->phy_num_led_triggers,
-					    GFP_KERNEL);
-	if (!phy->phy_led_triggers) {
+	phy->led_link_trigger = kzalloc(sizeof(*phy->led_link_trigger),
+					GFP_KERNEL);
+	if (!phy->led_link_trigger) {
 		err = -ENOMEM;
 		goto out_clear;
 	}
 
+	err = phy_led_trigger_register(phy, phy->led_link_trigger, 0, "link");
+	if (err)
+		goto out_free_link;
+
+	phy->phy_led_triggers = kcalloc(phy->phy_num_led_triggers,
+					sizeof(struct phy_led_trigger),
+					GFP_KERNEL);
+	if (!phy->phy_led_triggers) {
+		err = -ENOMEM;
+		goto out_unreg_link;
+	}
+
 	for (i = 0; i < phy->phy_num_led_triggers; i++) {
 		err = phy_led_trigger_register(phy, &phy->phy_led_triggers[i],
-					       speeds[i]);
+					       speeds[i],
+					       phy_speed_to_str(speeds[i]));
 		if (err)
 			goto out_unreg;
 	}
@@ -122,7 +127,12 @@ int phy_led_triggers_register(struct phy_device *phy)
 out_unreg:
 	while (i--)
 		phy_led_trigger_unregister(&phy->phy_led_triggers[i]);
-	devm_kfree(&phy->mdio.dev, phy->phy_led_triggers);
+	kfree(phy->phy_led_triggers);
+out_unreg_link:
+	phy_led_trigger_unregister(phy->led_link_trigger);
+out_free_link:
+	kfree(phy->led_link_trigger);
+	phy->led_link_trigger = NULL;
 out_clear:
 	phy->phy_num_led_triggers = 0;
 	return err;
@@ -135,5 +145,13 @@ void phy_led_triggers_unregister(struct phy_device *phy)
 
 	for (i = 0; i < phy->phy_num_led_triggers; i++)
 		phy_led_trigger_unregister(&phy->phy_led_triggers[i]);
+	kfree(phy->phy_led_triggers);
+	phy->phy_led_triggers = NULL;
+
+	if (phy->led_link_trigger) {
+		phy_led_trigger_unregister(phy->led_link_trigger);
+		kfree(phy->led_link_trigger);
+		phy->led_link_trigger = NULL;
+	}
 }
 EXPORT_SYMBOL_GPL(phy_led_triggers_unregister);