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/netdevice.h> 16 17 static struct phy_led_trigger *phy_speed_to_led_trigger(struct phy_device *phy, 18 unsigned int speed) 19 { 20 unsigned int i; 21 22 for (i = 0; i < phy->phy_num_led_triggers; i++) { 23 if (phy->phy_led_triggers[i].speed == speed) 24 return &phy->phy_led_triggers[i]; 25 } 26 return NULL; 27 } 28 29 void phy_led_trigger_change_speed(struct phy_device *phy) 30 { 31 struct phy_led_trigger *plt; 32 33 if (!phy->link) 34 goto out_change_speed; 35 36 if (phy->speed == 0) 37 return; 38 39 plt = phy_speed_to_led_trigger(phy, phy->speed); 40 if (!plt) { 41 netdev_alert(phy->attached_dev, 42 "No phy led trigger registered for speed(%d)\n", 43 phy->speed); 44 goto out_change_speed; 45 } 46 47 if (plt != phy->last_triggered) { 48 led_trigger_event(&phy->last_triggered->trigger, LED_OFF); 49 led_trigger_event(&plt->trigger, LED_FULL); 50 phy->last_triggered = plt; 51 } 52 return; 53 54 out_change_speed: 55 if (phy->last_triggered) { 56 led_trigger_event(&phy->last_triggered->trigger, 57 LED_OFF); 58 phy->last_triggered = NULL; 59 } 60 } 61 EXPORT_SYMBOL_GPL(phy_led_trigger_change_speed); 62 63 static int phy_led_trigger_register(struct phy_device *phy, 64 struct phy_led_trigger *plt, 65 unsigned int speed) 66 { 67 char name_suffix[PHY_LED_TRIGGER_SPEED_SUFFIX_SIZE]; 68 69 plt->speed = speed; 70 71 if (speed < SPEED_1000) 72 snprintf(name_suffix, sizeof(name_suffix), "%dMbps", speed); 73 else if (speed == SPEED_2500) 74 snprintf(name_suffix, sizeof(name_suffix), "2.5Gbps"); 75 else 76 snprintf(name_suffix, sizeof(name_suffix), "%dGbps", 77 DIV_ROUND_CLOSEST(speed, 1000)); 78 79 snprintf(plt->name, sizeof(plt->name), PHY_ID_FMT ":%s", 80 phy->mdio.bus->id, phy->mdio.addr, name_suffix); 81 plt->trigger.name = plt->name; 82 83 return led_trigger_register(&plt->trigger); 84 } 85 86 static void phy_led_trigger_unregister(struct phy_led_trigger *plt) 87 { 88 led_trigger_unregister(&plt->trigger); 89 } 90 91 int phy_led_triggers_register(struct phy_device *phy) 92 { 93 int i, err; 94 unsigned int speeds[50]; 95 96 phy->phy_num_led_triggers = phy_supported_speeds(phy, speeds, 97 ARRAY_SIZE(speeds)); 98 if (!phy->phy_num_led_triggers) 99 return 0; 100 101 phy->phy_led_triggers = devm_kzalloc(&phy->mdio.dev, 102 sizeof(struct phy_led_trigger) * 103 phy->phy_num_led_triggers, 104 GFP_KERNEL); 105 if (!phy->phy_led_triggers) 106 return -ENOMEM; 107 108 for (i = 0; i < phy->phy_num_led_triggers; i++) { 109 err = phy_led_trigger_register(phy, &phy->phy_led_triggers[i], 110 speeds[i]); 111 if (err) 112 goto out_unreg; 113 } 114 115 phy->last_triggered = NULL; 116 phy_led_trigger_change_speed(phy); 117 118 return 0; 119 out_unreg: 120 while (i--) 121 phy_led_trigger_unregister(&phy->phy_led_triggers[i]); 122 devm_kfree(&phy->mdio.dev, phy->phy_led_triggers); 123 return err; 124 } 125 EXPORT_SYMBOL_GPL(phy_led_triggers_register); 126 127 void phy_led_triggers_unregister(struct phy_device *phy) 128 { 129 int i; 130 131 for (i = 0; i < phy->phy_num_led_triggers; i++) 132 phy_led_trigger_unregister(&phy->phy_led_triggers[i]); 133 } 134 EXPORT_SYMBOL_GPL(phy_led_triggers_unregister); 135