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, const 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 plt->speed = speed; 81 phy_led_trigger_format_name(phy, plt->name, sizeof(plt->name), 82 phy_speed_to_str(speed)); 83 plt->trigger.name = plt->name; 84 85 return led_trigger_register(&plt->trigger); 86 } 87 88 static void phy_led_trigger_unregister(struct phy_led_trigger *plt) 89 { 90 led_trigger_unregister(&plt->trigger); 91 } 92 93 int phy_led_triggers_register(struct phy_device *phy) 94 { 95 int i, err; 96 unsigned int speeds[50]; 97 98 phy->phy_num_led_triggers = phy_supported_speeds(phy, speeds, 99 ARRAY_SIZE(speeds)); 100 if (!phy->phy_num_led_triggers) 101 return 0; 102 103 phy->led_link_trigger = devm_kzalloc(&phy->mdio.dev, 104 sizeof(*phy->led_link_trigger), 105 GFP_KERNEL); 106 if (!phy->led_link_trigger) { 107 err = -ENOMEM; 108 goto out_clear; 109 } 110 111 phy_led_trigger_format_name(phy, phy->led_link_trigger->name, 112 sizeof(phy->led_link_trigger->name), 113 "link"); 114 phy->led_link_trigger->trigger.name = phy->led_link_trigger->name; 115 116 err = led_trigger_register(&phy->led_link_trigger->trigger); 117 if (err) 118 goto out_free_link; 119 120 phy->phy_led_triggers = devm_kcalloc(&phy->mdio.dev, 121 phy->phy_num_led_triggers, 122 sizeof(struct phy_led_trigger), 123 GFP_KERNEL); 124 if (!phy->phy_led_triggers) { 125 err = -ENOMEM; 126 goto out_unreg_link; 127 } 128 129 for (i = 0; i < phy->phy_num_led_triggers; i++) { 130 err = phy_led_trigger_register(phy, &phy->phy_led_triggers[i], 131 speeds[i]); 132 if (err) 133 goto out_unreg; 134 } 135 136 phy->last_triggered = NULL; 137 phy_led_trigger_change_speed(phy); 138 139 return 0; 140 out_unreg: 141 while (i--) 142 phy_led_trigger_unregister(&phy->phy_led_triggers[i]); 143 devm_kfree(&phy->mdio.dev, phy->phy_led_triggers); 144 out_unreg_link: 145 phy_led_trigger_unregister(phy->led_link_trigger); 146 out_free_link: 147 devm_kfree(&phy->mdio.dev, phy->led_link_trigger); 148 phy->led_link_trigger = NULL; 149 out_clear: 150 phy->phy_num_led_triggers = 0; 151 return err; 152 } 153 EXPORT_SYMBOL_GPL(phy_led_triggers_register); 154 155 void phy_led_triggers_unregister(struct phy_device *phy) 156 { 157 int i; 158 159 for (i = 0; i < phy->phy_num_led_triggers; i++) 160 phy_led_trigger_unregister(&phy->phy_led_triggers[i]); 161 162 if (phy->led_link_trigger) 163 phy_led_trigger_unregister(phy->led_link_trigger); 164 } 165 EXPORT_SYMBOL_GPL(phy_led_triggers_unregister); 166