1*2e0bc452SZach Brown /* Copyright (C) 2016 National Instruments Corp. 2*2e0bc452SZach Brown * 3*2e0bc452SZach Brown * This program is free software; you can redistribute it and/or modify 4*2e0bc452SZach Brown * it under the terms of the GNU General Public License as published by 5*2e0bc452SZach Brown * the Free Software Foundation; either version 2 of the License, or 6*2e0bc452SZach Brown * (at your option) any later version. 7*2e0bc452SZach Brown * 8*2e0bc452SZach Brown * This program is distributed in the hope that it will be useful, 9*2e0bc452SZach Brown * but WITHOUT ANY WARRANTY; without even the implied warranty of 10*2e0bc452SZach Brown * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11*2e0bc452SZach Brown * GNU General Public License for more details. 12*2e0bc452SZach Brown */ 13*2e0bc452SZach Brown #include <linux/leds.h> 14*2e0bc452SZach Brown #include <linux/phy.h> 15*2e0bc452SZach Brown #include <linux/netdevice.h> 16*2e0bc452SZach Brown 17*2e0bc452SZach Brown static struct phy_led_trigger *phy_speed_to_led_trigger(struct phy_device *phy, 18*2e0bc452SZach Brown unsigned int speed) 19*2e0bc452SZach Brown { 20*2e0bc452SZach Brown unsigned int i; 21*2e0bc452SZach Brown 22*2e0bc452SZach Brown for (i = 0; i < phy->phy_num_led_triggers; i++) { 23*2e0bc452SZach Brown if (phy->phy_led_triggers[i].speed == speed) 24*2e0bc452SZach Brown return &phy->phy_led_triggers[i]; 25*2e0bc452SZach Brown } 26*2e0bc452SZach Brown return NULL; 27*2e0bc452SZach Brown } 28*2e0bc452SZach Brown 29*2e0bc452SZach Brown void phy_led_trigger_change_speed(struct phy_device *phy) 30*2e0bc452SZach Brown { 31*2e0bc452SZach Brown struct phy_led_trigger *plt; 32*2e0bc452SZach Brown 33*2e0bc452SZach Brown if (!phy->link) 34*2e0bc452SZach Brown goto out_change_speed; 35*2e0bc452SZach Brown 36*2e0bc452SZach Brown if (phy->speed == 0) 37*2e0bc452SZach Brown return; 38*2e0bc452SZach Brown 39*2e0bc452SZach Brown plt = phy_speed_to_led_trigger(phy, phy->speed); 40*2e0bc452SZach Brown if (!plt) { 41*2e0bc452SZach Brown netdev_alert(phy->attached_dev, 42*2e0bc452SZach Brown "No phy led trigger registered for speed(%d)\n", 43*2e0bc452SZach Brown phy->speed); 44*2e0bc452SZach Brown goto out_change_speed; 45*2e0bc452SZach Brown } 46*2e0bc452SZach Brown 47*2e0bc452SZach Brown if (plt != phy->last_triggered) { 48*2e0bc452SZach Brown led_trigger_event(&phy->last_triggered->trigger, LED_OFF); 49*2e0bc452SZach Brown led_trigger_event(&plt->trigger, LED_FULL); 50*2e0bc452SZach Brown phy->last_triggered = plt; 51*2e0bc452SZach Brown } 52*2e0bc452SZach Brown return; 53*2e0bc452SZach Brown 54*2e0bc452SZach Brown out_change_speed: 55*2e0bc452SZach Brown if (phy->last_triggered) { 56*2e0bc452SZach Brown led_trigger_event(&phy->last_triggered->trigger, 57*2e0bc452SZach Brown LED_OFF); 58*2e0bc452SZach Brown phy->last_triggered = NULL; 59*2e0bc452SZach Brown } 60*2e0bc452SZach Brown } 61*2e0bc452SZach Brown EXPORT_SYMBOL_GPL(phy_led_trigger_change_speed); 62*2e0bc452SZach Brown 63*2e0bc452SZach Brown static int phy_led_trigger_register(struct phy_device *phy, 64*2e0bc452SZach Brown struct phy_led_trigger *plt, 65*2e0bc452SZach Brown unsigned int speed) 66*2e0bc452SZach Brown { 67*2e0bc452SZach Brown char name_suffix[PHY_LED_TRIGGER_SPEED_SUFFIX_SIZE]; 68*2e0bc452SZach Brown 69*2e0bc452SZach Brown plt->speed = speed; 70*2e0bc452SZach Brown 71*2e0bc452SZach Brown if (speed < SPEED_1000) 72*2e0bc452SZach Brown snprintf(name_suffix, sizeof(name_suffix), "%dMbps", speed); 73*2e0bc452SZach Brown else if (speed == SPEED_2500) 74*2e0bc452SZach Brown snprintf(name_suffix, sizeof(name_suffix), "2.5Gbps"); 75*2e0bc452SZach Brown else 76*2e0bc452SZach Brown snprintf(name_suffix, sizeof(name_suffix), "%dGbps", 77*2e0bc452SZach Brown DIV_ROUND_CLOSEST(speed, 1000)); 78*2e0bc452SZach Brown 79*2e0bc452SZach Brown snprintf(plt->name, sizeof(plt->name), PHY_ID_FMT ":%s", 80*2e0bc452SZach Brown phy->mdio.bus->id, phy->mdio.addr, name_suffix); 81*2e0bc452SZach Brown plt->trigger.name = plt->name; 82*2e0bc452SZach Brown 83*2e0bc452SZach Brown return led_trigger_register(&plt->trigger); 84*2e0bc452SZach Brown } 85*2e0bc452SZach Brown 86*2e0bc452SZach Brown static void phy_led_trigger_unregister(struct phy_led_trigger *plt) 87*2e0bc452SZach Brown { 88*2e0bc452SZach Brown led_trigger_unregister(&plt->trigger); 89*2e0bc452SZach Brown } 90*2e0bc452SZach Brown 91*2e0bc452SZach Brown int phy_led_triggers_register(struct phy_device *phy) 92*2e0bc452SZach Brown { 93*2e0bc452SZach Brown int i, err; 94*2e0bc452SZach Brown unsigned int speeds[50]; 95*2e0bc452SZach Brown 96*2e0bc452SZach Brown phy->phy_num_led_triggers = phy_supported_speeds(phy, speeds, 97*2e0bc452SZach Brown ARRAY_SIZE(speeds)); 98*2e0bc452SZach Brown if (!phy->phy_num_led_triggers) 99*2e0bc452SZach Brown return 0; 100*2e0bc452SZach Brown 101*2e0bc452SZach Brown phy->phy_led_triggers = devm_kzalloc(&phy->mdio.dev, 102*2e0bc452SZach Brown sizeof(struct phy_led_trigger) * 103*2e0bc452SZach Brown phy->phy_num_led_triggers, 104*2e0bc452SZach Brown GFP_KERNEL); 105*2e0bc452SZach Brown if (!phy->phy_led_triggers) 106*2e0bc452SZach Brown return -ENOMEM; 107*2e0bc452SZach Brown 108*2e0bc452SZach Brown for (i = 0; i < phy->phy_num_led_triggers; i++) { 109*2e0bc452SZach Brown err = phy_led_trigger_register(phy, &phy->phy_led_triggers[i], 110*2e0bc452SZach Brown speeds[i]); 111*2e0bc452SZach Brown if (err) 112*2e0bc452SZach Brown goto out_unreg; 113*2e0bc452SZach Brown } 114*2e0bc452SZach Brown 115*2e0bc452SZach Brown phy->last_triggered = NULL; 116*2e0bc452SZach Brown phy_led_trigger_change_speed(phy); 117*2e0bc452SZach Brown 118*2e0bc452SZach Brown return 0; 119*2e0bc452SZach Brown out_unreg: 120*2e0bc452SZach Brown while (i--) 121*2e0bc452SZach Brown phy_led_trigger_unregister(&phy->phy_led_triggers[i]); 122*2e0bc452SZach Brown devm_kfree(&phy->mdio.dev, phy->phy_led_triggers); 123*2e0bc452SZach Brown return err; 124*2e0bc452SZach Brown } 125*2e0bc452SZach Brown EXPORT_SYMBOL_GPL(phy_led_triggers_register); 126*2e0bc452SZach Brown 127*2e0bc452SZach Brown void phy_led_triggers_unregister(struct phy_device *phy) 128*2e0bc452SZach Brown { 129*2e0bc452SZach Brown int i; 130*2e0bc452SZach Brown 131*2e0bc452SZach Brown for (i = 0; i < phy->phy_num_led_triggers; i++) 132*2e0bc452SZach Brown phy_led_trigger_unregister(&phy->phy_led_triggers[i]); 133*2e0bc452SZach Brown 134*2e0bc452SZach Brown devm_kfree(&phy->mdio.dev, phy->phy_led_triggers); 135*2e0bc452SZach Brown } 136*2e0bc452SZach Brown EXPORT_SYMBOL_GPL(phy_led_triggers_unregister); 137