1 /* Copyright (C) 2016 National Instruments Corp. 2 * 3 * This program is free software; you can redistribute it and/or modify 4 * it under the terms of the GNU General Public License as published by 5 * the Free Software Foundation; either version 2 of the License, or 6 * (at your option) any later version. 7 * 8 * This program is distributed in the hope that it will be useful, 9 * but WITHOUT ANY WARRANTY; without even the implied warranty of 10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 * GNU General Public License for more details. 12 */ 13 #include <linux/leds.h> 14 #include <linux/phy.h> 15 #include <linux/phy_led_triggers.h> 16 #include <linux/netdevice.h> 17 18 static struct phy_led_trigger *phy_speed_to_led_trigger(struct phy_device *phy, 19 unsigned int speed) 20 { 21 unsigned int i; 22 23 for (i = 0; i < phy->phy_num_led_triggers; i++) { 24 if (phy->phy_led_triggers[i].speed == speed) 25 return &phy->phy_led_triggers[i]; 26 } 27 return NULL; 28 } 29 30 static void phy_led_trigger_no_link(struct phy_device *phy) 31 { 32 if (phy->last_triggered) { 33 led_trigger_event(&phy->last_triggered->trigger, LED_OFF); 34 led_trigger_event(&phy->led_link_trigger->trigger, LED_OFF); 35 phy->last_triggered = NULL; 36 } 37 } 38 39 void phy_led_trigger_change_speed(struct phy_device *phy) 40 { 41 struct phy_led_trigger *plt; 42 43 if (!phy->link) 44 return phy_led_trigger_no_link(phy); 45 46 if (phy->speed == 0) 47 return; 48 49 plt = phy_speed_to_led_trigger(phy, phy->speed); 50 if (!plt) { 51 netdev_alert(phy->attached_dev, 52 "No phy led trigger registered for speed(%d)\n", 53 phy->speed); 54 return phy_led_trigger_no_link(phy); 55 } 56 57 if (plt != phy->last_triggered) { 58 if (!phy->last_triggered) 59 led_trigger_event(&phy->led_link_trigger->trigger, 60 LED_FULL); 61 62 led_trigger_event(&phy->last_triggered->trigger, LED_OFF); 63 led_trigger_event(&plt->trigger, LED_FULL); 64 phy->last_triggered = plt; 65 } 66 } 67 EXPORT_SYMBOL_GPL(phy_led_trigger_change_speed); 68 69 static void phy_led_trigger_format_name(struct phy_device *phy, char *buf, 70 size_t size, char *suffix) 71 { 72 snprintf(buf, size, PHY_ID_FMT ":%s", 73 phy->mdio.bus->id, phy->mdio.addr, suffix); 74 } 75 76 static int phy_led_trigger_register(struct phy_device *phy, 77 struct phy_led_trigger *plt, 78 unsigned int speed) 79 { 80 char name_suffix[PHY_LED_TRIGGER_SPEED_SUFFIX_SIZE]; 81 82 plt->speed = speed; 83 84 if (speed < SPEED_1000) 85 snprintf(name_suffix, sizeof(name_suffix), "%dMbps", speed); 86 else if (speed == SPEED_2500) 87 snprintf(name_suffix, sizeof(name_suffix), "2.5Gbps"); 88 else 89 snprintf(name_suffix, sizeof(name_suffix), "%dGbps", 90 DIV_ROUND_CLOSEST(speed, 1000)); 91 92 phy_led_trigger_format_name(phy, plt->name, sizeof(plt->name), 93 name_suffix); 94 plt->trigger.name = plt->name; 95 96 return led_trigger_register(&plt->trigger); 97 } 98 99 static void phy_led_trigger_unregister(struct phy_led_trigger *plt) 100 { 101 led_trigger_unregister(&plt->trigger); 102 } 103 104 int phy_led_triggers_register(struct phy_device *phy) 105 { 106 int i, err; 107 unsigned int speeds[50]; 108 109 phy->phy_num_led_triggers = phy_supported_speeds(phy, speeds, 110 ARRAY_SIZE(speeds)); 111 if (!phy->phy_num_led_triggers) 112 return 0; 113 114 phy->led_link_trigger = devm_kzalloc(&phy->mdio.dev, 115 sizeof(*phy->led_link_trigger), 116 GFP_KERNEL); 117 if (!phy->led_link_trigger) { 118 err = -ENOMEM; 119 goto out_clear; 120 } 121 122 phy_led_trigger_format_name(phy, phy->led_link_trigger->name, 123 sizeof(phy->led_link_trigger->name), 124 "link"); 125 phy->led_link_trigger->trigger.name = phy->led_link_trigger->name; 126 127 err = led_trigger_register(&phy->led_link_trigger->trigger); 128 if (err) 129 goto out_free_link; 130 131 phy->phy_led_triggers = devm_kzalloc(&phy->mdio.dev, 132 sizeof(struct phy_led_trigger) * 133 phy->phy_num_led_triggers, 134 GFP_KERNEL); 135 if (!phy->phy_led_triggers) { 136 err = -ENOMEM; 137 goto out_unreg_link; 138 } 139 140 for (i = 0; i < phy->phy_num_led_triggers; i++) { 141 err = phy_led_trigger_register(phy, &phy->phy_led_triggers[i], 142 speeds[i]); 143 if (err) 144 goto out_unreg; 145 } 146 147 phy->last_triggered = NULL; 148 phy_led_trigger_change_speed(phy); 149 150 return 0; 151 out_unreg: 152 while (i--) 153 phy_led_trigger_unregister(&phy->phy_led_triggers[i]); 154 devm_kfree(&phy->mdio.dev, phy->phy_led_triggers); 155 out_unreg_link: 156 phy_led_trigger_unregister(phy->led_link_trigger); 157 out_free_link: 158 devm_kfree(&phy->mdio.dev, phy->led_link_trigger); 159 phy->led_link_trigger = NULL; 160 out_clear: 161 phy->phy_num_led_triggers = 0; 162 return err; 163 } 164 EXPORT_SYMBOL_GPL(phy_led_triggers_register); 165 166 void phy_led_triggers_unregister(struct phy_device *phy) 167 { 168 int i; 169 170 for (i = 0; i < phy->phy_num_led_triggers; i++) 171 phy_led_trigger_unregister(&phy->phy_led_triggers[i]); 172 173 if (phy->led_link_trigger) 174 phy_led_trigger_unregister(phy->led_link_trigger); 175 } 176 EXPORT_SYMBOL_GPL(phy_led_triggers_unregister); 177