Commit | Line | Data |
---|---|---|
2e0bc452 ZB |
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/netdevice.h> | |
16 | ||
17 | static struct phy_led_trigger *phy_speed_to_led_trigger(struct phy_device *phy, | |
18 | unsigned int speed) | |
19 | { | |
20 | unsigned int i; | |
21 | ||
22 | for (i = 0; i < phy->phy_num_led_triggers; i++) { | |
23 | if (phy->phy_led_triggers[i].speed == speed) | |
24 | return &phy->phy_led_triggers[i]; | |
25 | } | |
26 | return NULL; | |
27 | } | |
28 | ||
29 | void phy_led_trigger_change_speed(struct phy_device *phy) | |
30 | { | |
31 | struct phy_led_trigger *plt; | |
32 | ||
33 | if (!phy->link) | |
34 | goto out_change_speed; | |
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 | goto out_change_speed; | |
45 | } | |
46 | ||
47 | if (plt != phy->last_triggered) { | |
48 | led_trigger_event(&phy->last_triggered->trigger, LED_OFF); | |
49 | led_trigger_event(&plt->trigger, LED_FULL); | |
50 | phy->last_triggered = plt; | |
51 | } | |
52 | return; | |
53 | ||
54 | out_change_speed: | |
55 | if (phy->last_triggered) { | |
56 | led_trigger_event(&phy->last_triggered->trigger, | |
57 | LED_OFF); | |
58 | phy->last_triggered = NULL; | |
59 | } | |
60 | } | |
61 | EXPORT_SYMBOL_GPL(phy_led_trigger_change_speed); | |
62 | ||
63 | static int phy_led_trigger_register(struct phy_device *phy, | |
64 | struct phy_led_trigger *plt, | |
65 | unsigned int speed) | |
66 | { | |
67 | char name_suffix[PHY_LED_TRIGGER_SPEED_SUFFIX_SIZE]; | |
68 | ||
69 | plt->speed = speed; | |
70 | ||
71 | if (speed < SPEED_1000) | |
72 | snprintf(name_suffix, sizeof(name_suffix), "%dMbps", speed); | |
73 | else if (speed == SPEED_2500) | |
74 | snprintf(name_suffix, sizeof(name_suffix), "2.5Gbps"); | |
75 | else | |
76 | snprintf(name_suffix, sizeof(name_suffix), "%dGbps", | |
77 | DIV_ROUND_CLOSEST(speed, 1000)); | |
78 | ||
79 | snprintf(plt->name, sizeof(plt->name), PHY_ID_FMT ":%s", | |
80 | phy->mdio.bus->id, phy->mdio.addr, name_suffix); | |
81 | plt->trigger.name = plt->name; | |
82 | ||
83 | return led_trigger_register(&plt->trigger); | |
84 | } | |
85 | ||
86 | static void phy_led_trigger_unregister(struct phy_led_trigger *plt) | |
87 | { | |
88 | led_trigger_unregister(&plt->trigger); | |
89 | } | |
90 | ||
91 | int phy_led_triggers_register(struct phy_device *phy) | |
92 | { | |
93 | int i, err; | |
94 | unsigned int speeds[50]; | |
95 | ||
96 | phy->phy_num_led_triggers = phy_supported_speeds(phy, speeds, | |
97 | ARRAY_SIZE(speeds)); | |
98 | if (!phy->phy_num_led_triggers) | |
99 | return 0; | |
100 | ||
101 | phy->phy_led_triggers = devm_kzalloc(&phy->mdio.dev, | |
102 | sizeof(struct phy_led_trigger) * | |
103 | phy->phy_num_led_triggers, | |
104 | GFP_KERNEL); | |
105 | if (!phy->phy_led_triggers) | |
106 | return -ENOMEM; | |
107 | ||
108 | for (i = 0; i < phy->phy_num_led_triggers; i++) { | |
109 | err = phy_led_trigger_register(phy, &phy->phy_led_triggers[i], | |
110 | speeds[i]); | |
111 | if (err) | |
112 | goto out_unreg; | |
113 | } | |
114 | ||
115 | phy->last_triggered = NULL; | |
116 | phy_led_trigger_change_speed(phy); | |
117 | ||
118 | return 0; | |
119 | out_unreg: | |
120 | while (i--) | |
121 | phy_led_trigger_unregister(&phy->phy_led_triggers[i]); | |
122 | devm_kfree(&phy->mdio.dev, phy->phy_led_triggers); | |
123 | return err; | |
124 | } | |
125 | EXPORT_SYMBOL_GPL(phy_led_triggers_register); | |
126 | ||
127 | void phy_led_triggers_unregister(struct phy_device *phy) | |
128 | { | |
129 | int i; | |
130 | ||
131 | for (i = 0; i < phy->phy_num_led_triggers; i++) | |
132 | phy_led_trigger_unregister(&phy->phy_led_triggers[i]); | |
2e0bc452 ZB |
133 | } |
134 | EXPORT_SYMBOL_GPL(phy_led_triggers_unregister); |