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> 15*d6f8cfa3SGeert Uytterhoeven #include <linux/phy_led_triggers.h> 162e0bc452SZach Brown #include <linux/netdevice.h> 172e0bc452SZach Brown 182e0bc452SZach Brown static struct phy_led_trigger *phy_speed_to_led_trigger(struct phy_device *phy, 192e0bc452SZach Brown unsigned int speed) 202e0bc452SZach Brown { 212e0bc452SZach Brown unsigned int i; 222e0bc452SZach Brown 232e0bc452SZach Brown for (i = 0; i < phy->phy_num_led_triggers; i++) { 242e0bc452SZach Brown if (phy->phy_led_triggers[i].speed == speed) 252e0bc452SZach Brown return &phy->phy_led_triggers[i]; 262e0bc452SZach Brown } 272e0bc452SZach Brown return NULL; 282e0bc452SZach Brown } 292e0bc452SZach Brown 302e0bc452SZach Brown void phy_led_trigger_change_speed(struct phy_device *phy) 312e0bc452SZach Brown { 322e0bc452SZach Brown struct phy_led_trigger *plt; 332e0bc452SZach Brown 342e0bc452SZach Brown if (!phy->link) 352e0bc452SZach Brown goto out_change_speed; 362e0bc452SZach Brown 372e0bc452SZach Brown if (phy->speed == 0) 382e0bc452SZach Brown return; 392e0bc452SZach Brown 402e0bc452SZach Brown plt = phy_speed_to_led_trigger(phy, phy->speed); 412e0bc452SZach Brown if (!plt) { 422e0bc452SZach Brown netdev_alert(phy->attached_dev, 432e0bc452SZach Brown "No phy led trigger registered for speed(%d)\n", 442e0bc452SZach Brown phy->speed); 452e0bc452SZach Brown goto out_change_speed; 462e0bc452SZach Brown } 472e0bc452SZach Brown 482e0bc452SZach Brown if (plt != phy->last_triggered) { 492e0bc452SZach Brown led_trigger_event(&phy->last_triggered->trigger, LED_OFF); 502e0bc452SZach Brown led_trigger_event(&plt->trigger, LED_FULL); 512e0bc452SZach Brown phy->last_triggered = plt; 522e0bc452SZach Brown } 532e0bc452SZach Brown return; 542e0bc452SZach Brown 552e0bc452SZach Brown out_change_speed: 562e0bc452SZach Brown if (phy->last_triggered) { 572e0bc452SZach Brown led_trigger_event(&phy->last_triggered->trigger, 582e0bc452SZach Brown LED_OFF); 592e0bc452SZach Brown phy->last_triggered = NULL; 602e0bc452SZach Brown } 612e0bc452SZach Brown } 622e0bc452SZach Brown EXPORT_SYMBOL_GPL(phy_led_trigger_change_speed); 632e0bc452SZach Brown 642e0bc452SZach Brown static int phy_led_trigger_register(struct phy_device *phy, 652e0bc452SZach Brown struct phy_led_trigger *plt, 662e0bc452SZach Brown unsigned int speed) 672e0bc452SZach Brown { 682e0bc452SZach Brown char name_suffix[PHY_LED_TRIGGER_SPEED_SUFFIX_SIZE]; 692e0bc452SZach Brown 702e0bc452SZach Brown plt->speed = speed; 712e0bc452SZach Brown 722e0bc452SZach Brown if (speed < SPEED_1000) 732e0bc452SZach Brown snprintf(name_suffix, sizeof(name_suffix), "%dMbps", speed); 742e0bc452SZach Brown else if (speed == SPEED_2500) 752e0bc452SZach Brown snprintf(name_suffix, sizeof(name_suffix), "2.5Gbps"); 762e0bc452SZach Brown else 772e0bc452SZach Brown snprintf(name_suffix, sizeof(name_suffix), "%dGbps", 782e0bc452SZach Brown DIV_ROUND_CLOSEST(speed, 1000)); 792e0bc452SZach Brown 802e0bc452SZach Brown snprintf(plt->name, sizeof(plt->name), PHY_ID_FMT ":%s", 812e0bc452SZach Brown phy->mdio.bus->id, phy->mdio.addr, name_suffix); 822e0bc452SZach Brown plt->trigger.name = plt->name; 832e0bc452SZach Brown 842e0bc452SZach Brown return led_trigger_register(&plt->trigger); 852e0bc452SZach Brown } 862e0bc452SZach Brown 872e0bc452SZach Brown static void phy_led_trigger_unregister(struct phy_led_trigger *plt) 882e0bc452SZach Brown { 892e0bc452SZach Brown led_trigger_unregister(&plt->trigger); 902e0bc452SZach Brown } 912e0bc452SZach Brown 922e0bc452SZach Brown int phy_led_triggers_register(struct phy_device *phy) 932e0bc452SZach Brown { 942e0bc452SZach Brown int i, err; 952e0bc452SZach Brown unsigned int speeds[50]; 962e0bc452SZach Brown 972e0bc452SZach Brown phy->phy_num_led_triggers = phy_supported_speeds(phy, speeds, 982e0bc452SZach Brown ARRAY_SIZE(speeds)); 992e0bc452SZach Brown if (!phy->phy_num_led_triggers) 1002e0bc452SZach Brown return 0; 1012e0bc452SZach Brown 1022e0bc452SZach Brown phy->phy_led_triggers = devm_kzalloc(&phy->mdio.dev, 1032e0bc452SZach Brown sizeof(struct phy_led_trigger) * 1042e0bc452SZach Brown phy->phy_num_led_triggers, 1052e0bc452SZach Brown GFP_KERNEL); 1068a87fca8SGeert Uytterhoeven if (!phy->phy_led_triggers) { 1078a87fca8SGeert Uytterhoeven err = -ENOMEM; 1088a87fca8SGeert Uytterhoeven goto out_clear; 1098a87fca8SGeert Uytterhoeven } 1102e0bc452SZach Brown 1112e0bc452SZach Brown for (i = 0; i < phy->phy_num_led_triggers; i++) { 1122e0bc452SZach Brown err = phy_led_trigger_register(phy, &phy->phy_led_triggers[i], 1132e0bc452SZach Brown speeds[i]); 1142e0bc452SZach Brown if (err) 1152e0bc452SZach Brown goto out_unreg; 1162e0bc452SZach Brown } 1172e0bc452SZach Brown 1182e0bc452SZach Brown phy->last_triggered = NULL; 1192e0bc452SZach Brown phy_led_trigger_change_speed(phy); 1202e0bc452SZach Brown 1212e0bc452SZach Brown return 0; 1222e0bc452SZach Brown out_unreg: 1232e0bc452SZach Brown while (i--) 1242e0bc452SZach Brown phy_led_trigger_unregister(&phy->phy_led_triggers[i]); 1252e0bc452SZach Brown devm_kfree(&phy->mdio.dev, phy->phy_led_triggers); 1268a87fca8SGeert Uytterhoeven out_clear: 1278a87fca8SGeert Uytterhoeven phy->phy_num_led_triggers = 0; 1282e0bc452SZach Brown return err; 1292e0bc452SZach Brown } 1302e0bc452SZach Brown EXPORT_SYMBOL_GPL(phy_led_triggers_register); 1312e0bc452SZach Brown 1322e0bc452SZach Brown void phy_led_triggers_unregister(struct phy_device *phy) 1332e0bc452SZach Brown { 1342e0bc452SZach Brown int i; 1352e0bc452SZach Brown 1362e0bc452SZach Brown for (i = 0; i < phy->phy_num_led_triggers; i++) 1372e0bc452SZach Brown phy_led_trigger_unregister(&phy->phy_led_triggers[i]); 1382e0bc452SZach Brown } 1392e0bc452SZach Brown EXPORT_SYMBOL_GPL(phy_led_triggers_unregister); 140