12e0bc452SZach Brown /* Copyright (C) 2016 National Instruments Corp. 22e0bc452SZach Brown * 32e0bc452SZach Brown * This program is free software; you can redistribute it and/or modify 42e0bc452SZach Brown * it under the terms of the GNU General Public License as published by 52e0bc452SZach Brown * the Free Software Foundation; either version 2 of the License, or 62e0bc452SZach Brown * (at your option) any later version. 72e0bc452SZach Brown * 82e0bc452SZach Brown * This program is distributed in the hope that it will be useful, 92e0bc452SZach Brown * but WITHOUT ANY WARRANTY; without even the implied warranty of 102e0bc452SZach Brown * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 112e0bc452SZach Brown * GNU General Public License for more details. 122e0bc452SZach Brown */ 132e0bc452SZach Brown #include <linux/leds.h> 142e0bc452SZach Brown #include <linux/phy.h> 152e0bc452SZach Brown #include <linux/netdevice.h> 162e0bc452SZach Brown 172e0bc452SZach Brown static struct phy_led_trigger *phy_speed_to_led_trigger(struct phy_device *phy, 182e0bc452SZach Brown unsigned int speed) 192e0bc452SZach Brown { 202e0bc452SZach Brown unsigned int i; 212e0bc452SZach Brown 222e0bc452SZach Brown for (i = 0; i < phy->phy_num_led_triggers; i++) { 232e0bc452SZach Brown if (phy->phy_led_triggers[i].speed == speed) 242e0bc452SZach Brown return &phy->phy_led_triggers[i]; 252e0bc452SZach Brown } 262e0bc452SZach Brown return NULL; 272e0bc452SZach Brown } 282e0bc452SZach Brown 292e0bc452SZach Brown void phy_led_trigger_change_speed(struct phy_device *phy) 302e0bc452SZach Brown { 312e0bc452SZach Brown struct phy_led_trigger *plt; 322e0bc452SZach Brown 332e0bc452SZach Brown if (!phy->link) 342e0bc452SZach Brown goto out_change_speed; 352e0bc452SZach Brown 362e0bc452SZach Brown if (phy->speed == 0) 372e0bc452SZach Brown return; 382e0bc452SZach Brown 392e0bc452SZach Brown plt = phy_speed_to_led_trigger(phy, phy->speed); 402e0bc452SZach Brown if (!plt) { 412e0bc452SZach Brown netdev_alert(phy->attached_dev, 422e0bc452SZach Brown "No phy led trigger registered for speed(%d)\n", 432e0bc452SZach Brown phy->speed); 442e0bc452SZach Brown goto out_change_speed; 452e0bc452SZach Brown } 462e0bc452SZach Brown 472e0bc452SZach Brown if (plt != phy->last_triggered) { 482e0bc452SZach Brown led_trigger_event(&phy->last_triggered->trigger, LED_OFF); 492e0bc452SZach Brown led_trigger_event(&plt->trigger, LED_FULL); 502e0bc452SZach Brown phy->last_triggered = plt; 512e0bc452SZach Brown } 522e0bc452SZach Brown return; 532e0bc452SZach Brown 542e0bc452SZach Brown out_change_speed: 552e0bc452SZach Brown if (phy->last_triggered) { 562e0bc452SZach Brown led_trigger_event(&phy->last_triggered->trigger, 572e0bc452SZach Brown LED_OFF); 582e0bc452SZach Brown phy->last_triggered = NULL; 592e0bc452SZach Brown } 602e0bc452SZach Brown } 612e0bc452SZach Brown EXPORT_SYMBOL_GPL(phy_led_trigger_change_speed); 622e0bc452SZach Brown 632e0bc452SZach Brown static int phy_led_trigger_register(struct phy_device *phy, 642e0bc452SZach Brown struct phy_led_trigger *plt, 652e0bc452SZach Brown unsigned int speed) 662e0bc452SZach Brown { 672e0bc452SZach Brown char name_suffix[PHY_LED_TRIGGER_SPEED_SUFFIX_SIZE]; 682e0bc452SZach Brown 692e0bc452SZach Brown plt->speed = speed; 702e0bc452SZach Brown 712e0bc452SZach Brown if (speed < SPEED_1000) 722e0bc452SZach Brown snprintf(name_suffix, sizeof(name_suffix), "%dMbps", speed); 732e0bc452SZach Brown else if (speed == SPEED_2500) 742e0bc452SZach Brown snprintf(name_suffix, sizeof(name_suffix), "2.5Gbps"); 752e0bc452SZach Brown else 762e0bc452SZach Brown snprintf(name_suffix, sizeof(name_suffix), "%dGbps", 772e0bc452SZach Brown DIV_ROUND_CLOSEST(speed, 1000)); 782e0bc452SZach Brown 792e0bc452SZach Brown snprintf(plt->name, sizeof(plt->name), PHY_ID_FMT ":%s", 802e0bc452SZach Brown phy->mdio.bus->id, phy->mdio.addr, name_suffix); 812e0bc452SZach Brown plt->trigger.name = plt->name; 822e0bc452SZach Brown 832e0bc452SZach Brown return led_trigger_register(&plt->trigger); 842e0bc452SZach Brown } 852e0bc452SZach Brown 862e0bc452SZach Brown static void phy_led_trigger_unregister(struct phy_led_trigger *plt) 872e0bc452SZach Brown { 882e0bc452SZach Brown led_trigger_unregister(&plt->trigger); 892e0bc452SZach Brown } 902e0bc452SZach Brown 912e0bc452SZach Brown int phy_led_triggers_register(struct phy_device *phy) 922e0bc452SZach Brown { 932e0bc452SZach Brown int i, err; 942e0bc452SZach Brown unsigned int speeds[50]; 952e0bc452SZach Brown 962e0bc452SZach Brown phy->phy_num_led_triggers = phy_supported_speeds(phy, speeds, 972e0bc452SZach Brown ARRAY_SIZE(speeds)); 982e0bc452SZach Brown if (!phy->phy_num_led_triggers) 992e0bc452SZach Brown return 0; 1002e0bc452SZach Brown 1012e0bc452SZach Brown phy->phy_led_triggers = devm_kzalloc(&phy->mdio.dev, 1022e0bc452SZach Brown sizeof(struct phy_led_trigger) * 1032e0bc452SZach Brown phy->phy_num_led_triggers, 1042e0bc452SZach Brown GFP_KERNEL); 105*8a87fca8SGeert Uytterhoeven if (!phy->phy_led_triggers) { 106*8a87fca8SGeert Uytterhoeven err = -ENOMEM; 107*8a87fca8SGeert Uytterhoeven goto out_clear; 108*8a87fca8SGeert Uytterhoeven } 1092e0bc452SZach Brown 1102e0bc452SZach Brown for (i = 0; i < phy->phy_num_led_triggers; i++) { 1112e0bc452SZach Brown err = phy_led_trigger_register(phy, &phy->phy_led_triggers[i], 1122e0bc452SZach Brown speeds[i]); 1132e0bc452SZach Brown if (err) 1142e0bc452SZach Brown goto out_unreg; 1152e0bc452SZach Brown } 1162e0bc452SZach Brown 1172e0bc452SZach Brown phy->last_triggered = NULL; 1182e0bc452SZach Brown phy_led_trigger_change_speed(phy); 1192e0bc452SZach Brown 1202e0bc452SZach Brown return 0; 1212e0bc452SZach Brown out_unreg: 1222e0bc452SZach Brown while (i--) 1232e0bc452SZach Brown phy_led_trigger_unregister(&phy->phy_led_triggers[i]); 1242e0bc452SZach Brown devm_kfree(&phy->mdio.dev, phy->phy_led_triggers); 125*8a87fca8SGeert Uytterhoeven out_clear: 126*8a87fca8SGeert Uytterhoeven phy->phy_num_led_triggers = 0; 1272e0bc452SZach Brown return err; 1282e0bc452SZach Brown } 1292e0bc452SZach Brown EXPORT_SYMBOL_GPL(phy_led_triggers_register); 1302e0bc452SZach Brown 1312e0bc452SZach Brown void phy_led_triggers_unregister(struct phy_device *phy) 1322e0bc452SZach Brown { 1332e0bc452SZach Brown int i; 1342e0bc452SZach Brown 1352e0bc452SZach Brown for (i = 0; i < phy->phy_num_led_triggers; i++) 1362e0bc452SZach Brown phy_led_trigger_unregister(&phy->phy_led_triggers[i]); 1372e0bc452SZach Brown } 1382e0bc452SZach Brown EXPORT_SYMBOL_GPL(phy_led_triggers_unregister); 139