net/phy/phy_led_triggers.c

0001 // SPDX-License-Identifier: GPL-2.0+
0002 /* Copyright (C) 2016 National Instruments Corp. */
0003 #include <linux/leds.h>
0004 #include <linux/phy.h>
0005 #include <linux/phy_led_triggers.h>
0006 #include <linux/netdevice.h>
0007
0008 static struct phy_led_trigger *phy_speed_to_led_trigger(struct phy_device *phy,
0009                             unsigned int speed)
0010 {
0011     unsigned int i;
0012
0013     for (i = 0; i < phy->phy_num_led_triggers; i++) {
0014         if (phy->phy_led_triggers[i].speed == speed)
0015             return &phy->phy_led_triggers[i];
0016     }
0017     return NULL;
0018 }
0019
0020 static void phy_led_trigger_no_link(struct phy_device *phy)
0021 {
0022     if (phy->last_triggered) {
0023         led_trigger_event(&phy->last_triggered->trigger, LED_OFF);
0024         led_trigger_event(&phy->led_link_trigger->trigger, LED_OFF);
0025         phy->last_triggered = NULL;
0026     }
0027 }
0028
0029 void phy_led_trigger_change_speed(struct phy_device *phy)
0030 {
0031     struct phy_led_trigger *plt;
0032
0033     if (!phy->link)
0034         return phy_led_trigger_no_link(phy);
0035
0036     if (phy->speed == 0)
0037         return;
0038
0039     plt = phy_speed_to_led_trigger(phy, phy->speed);
0040     if (!plt) {
0041         netdev_alert(phy->attached_dev,
0042                  "No phy led trigger registered for speed(%d)\n",
0043                  phy->speed);
0044         return phy_led_trigger_no_link(phy);
0045     }
0046
0047     if (plt != phy->last_triggered) {
0048         if (!phy->last_triggered)
0049             led_trigger_event(&phy->led_link_trigger->trigger,
0050                       LED_FULL);
0051         else
0052             led_trigger_event(&phy->last_triggered->trigger, LED_OFF);
0053
0054         led_trigger_event(&plt->trigger, LED_FULL);
0055         phy->last_triggered = plt;
0056     }
0057 }
0058 EXPORT_SYMBOL_GPL(phy_led_trigger_change_speed);
0059
0060 static void phy_led_trigger_format_name(struct phy_device *phy, char *buf,
0061                     size_t size, const char *suffix)
0062 {
0063     snprintf(buf, size, PHY_ID_FMT ":%s",
0064          phy->mdio.bus->id, phy->mdio.addr, suffix);
0065 }
0066
0067 static int phy_led_trigger_register(struct phy_device *phy,
0068                     struct phy_led_trigger *plt,
0069                     unsigned int speed,
0070                     const char *suffix)
0071 {
0072     plt->speed = speed;
0073     phy_led_trigger_format_name(phy, plt->name, sizeof(plt->name), suffix);
0074     plt->trigger.name = plt->name;
0075
0076     return led_trigger_register(&plt->trigger);
0077 }
0078
0079 static void phy_led_trigger_unregister(struct phy_led_trigger *plt)
0080 {
0081     led_trigger_unregister(&plt->trigger);
0082 }
0083
0084 int phy_led_triggers_register(struct phy_device *phy)
0085 {
0086     int i, err;
0087     unsigned int speeds[50];
0088
0089     phy->phy_num_led_triggers = phy_supported_speeds(phy, speeds,
0090                              ARRAY_SIZE(speeds));
0091     if (!phy->phy_num_led_triggers)
0092         return 0;
0093
0094     phy->led_link_trigger = devm_kzalloc(&phy->mdio.dev,
0095                          sizeof(*phy->led_link_trigger),
0096                          GFP_KERNEL);
0097     if (!phy->led_link_trigger) {
0098         err = -ENOMEM;
0099         goto out_clear;
0100     }
0101
0102     err = phy_led_trigger_register(phy, phy->led_link_trigger, 0, "link");
0103     if (err)
0104         goto out_free_link;
0105
0106     phy->phy_led_triggers = devm_kcalloc(&phy->mdio.dev,
0107                         phy->phy_num_led_triggers,
0108                         sizeof(struct phy_led_trigger),
0109                         GFP_KERNEL);
0110     if (!phy->phy_led_triggers) {
0111         err = -ENOMEM;
0112         goto out_unreg_link;
0113     }
0114
0115     for (i = 0; i < phy->phy_num_led_triggers; i++) {
0116         err = phy_led_trigger_register(phy, &phy->phy_led_triggers[i],
0117                            speeds[i],
0118                            phy_speed_to_str(speeds[i]));
0119         if (err)
0120             goto out_unreg;
0121     }
0122
0123     phy->last_triggered = NULL;
0124     phy_led_trigger_change_speed(phy);
0125
0126     return 0;
0127 out_unreg:
0128     while (i--)
0129         phy_led_trigger_unregister(&phy->phy_led_triggers[i]);
0130     devm_kfree(&phy->mdio.dev, phy->phy_led_triggers);
0131 out_unreg_link:
0132     phy_led_trigger_unregister(phy->led_link_trigger);
0133 out_free_link:
0134     devm_kfree(&phy->mdio.dev, phy->led_link_trigger);
0135     phy->led_link_trigger = NULL;
0136 out_clear:
0137     phy->phy_num_led_triggers = 0;
0138     return err;
0139 }
0140 EXPORT_SYMBOL_GPL(phy_led_triggers_register);
0141
0142 void phy_led_triggers_unregister(struct phy_device *phy)
0143 {
0144     int i;
0145
0146     for (i = 0; i < phy->phy_num_led_triggers; i++)
0147         phy_led_trigger_unregister(&phy->phy_led_triggers[i]);
0148
0149     if (phy->led_link_trigger)
0150         phy_led_trigger_unregister(phy->led_link_trigger);
0151 }
0152 EXPORT_SYMBOL_GPL(phy_led_triggers_unregister);