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