153 lines · c
1// SPDX-License-Identifier: GPL-2.0+2/* Copyright (C) 2016 National Instruments Corp. */3#include <linux/leds.h>4#include <linux/phy.h>5#include <linux/phy_led_triggers.h>6#include <linux/netdevice.h>7 8static struct phy_led_trigger *phy_speed_to_led_trigger(struct phy_device *phy,9 unsigned int speed)10{11 unsigned int i;12 13 for (i = 0; i < phy->phy_num_led_triggers; i++) {14 if (phy->phy_led_triggers[i].speed == speed)15 return &phy->phy_led_triggers[i];16 }17 return NULL;18}19 20static void phy_led_trigger_no_link(struct phy_device *phy)21{22 if (phy->last_triggered) {23 led_trigger_event(&phy->last_triggered->trigger, LED_OFF);24 led_trigger_event(&phy->led_link_trigger->trigger, LED_OFF);25 phy->last_triggered = NULL;26 }27}28 29void phy_led_trigger_change_speed(struct phy_device *phy)30{31 struct phy_led_trigger *plt;32 33 if (!phy->link)34 return phy_led_trigger_no_link(phy);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 return phy_led_trigger_no_link(phy);45 }46 47 if (plt != phy->last_triggered) {48 if (!phy->last_triggered)49 led_trigger_event(&phy->led_link_trigger->trigger,50 LED_FULL);51 else52 led_trigger_event(&phy->last_triggered->trigger, LED_OFF);53 54 led_trigger_event(&plt->trigger, LED_FULL);55 phy->last_triggered = plt;56 }57}58EXPORT_SYMBOL_GPL(phy_led_trigger_change_speed);59 60static void phy_led_trigger_format_name(struct phy_device *phy, char *buf,61 size_t size, const char *suffix)62{63 snprintf(buf, size, PHY_ID_FMT ":%s",64 phy->mdio.bus->id, phy->mdio.addr, suffix);65}66 67static int phy_led_trigger_register(struct phy_device *phy,68 struct phy_led_trigger *plt,69 unsigned int speed,70 const char *suffix)71{72 plt->speed = speed;73 phy_led_trigger_format_name(phy, plt->name, sizeof(plt->name), suffix);74 plt->trigger.name = plt->name;75 76 return led_trigger_register(&plt->trigger);77}78 79static void phy_led_trigger_unregister(struct phy_led_trigger *plt)80{81 led_trigger_unregister(&plt->trigger);82}83 84int phy_led_triggers_register(struct phy_device *phy)85{86 int i, err;87 unsigned int speeds[50];88 89 phy->phy_num_led_triggers = phy_supported_speeds(phy, speeds,90 ARRAY_SIZE(speeds));91 if (!phy->phy_num_led_triggers)92 return 0;93 94 phy->led_link_trigger = devm_kzalloc(&phy->mdio.dev,95 sizeof(*phy->led_link_trigger),96 GFP_KERNEL);97 if (!phy->led_link_trigger) {98 err = -ENOMEM;99 goto out_clear;100 }101 102 err = phy_led_trigger_register(phy, phy->led_link_trigger, 0, "link");103 if (err)104 goto out_free_link;105 106 phy->phy_led_triggers = devm_kcalloc(&phy->mdio.dev,107 phy->phy_num_led_triggers,108 sizeof(struct phy_led_trigger),109 GFP_KERNEL);110 if (!phy->phy_led_triggers) {111 err = -ENOMEM;112 goto out_unreg_link;113 }114 115 for (i = 0; i < phy->phy_num_led_triggers; i++) {116 err = phy_led_trigger_register(phy, &phy->phy_led_triggers[i],117 speeds[i],118 phy_speed_to_str(speeds[i]));119 if (err)120 goto out_unreg;121 }122 123 phy->last_triggered = NULL;124 phy_led_trigger_change_speed(phy);125 126 return 0;127out_unreg:128 while (i--)129 phy_led_trigger_unregister(&phy->phy_led_triggers[i]);130 devm_kfree(&phy->mdio.dev, phy->phy_led_triggers);131out_unreg_link:132 phy_led_trigger_unregister(phy->led_link_trigger);133out_free_link:134 devm_kfree(&phy->mdio.dev, phy->led_link_trigger);135 phy->led_link_trigger = NULL;136out_clear:137 phy->phy_num_led_triggers = 0;138 return err;139}140EXPORT_SYMBOL_GPL(phy_led_triggers_register);141 142void phy_led_triggers_unregister(struct phy_device *phy)143{144 int i;145 146 for (i = 0; i < phy->phy_num_led_triggers; i++)147 phy_led_trigger_unregister(&phy->phy_led_triggers[i]);148 149 if (phy->led_link_trigger)150 phy_led_trigger_unregister(phy->led_link_trigger);151}152EXPORT_SYMBOL_GPL(phy_led_triggers_unregister);153