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 | |
8 | static 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 | |
20 | static void phy_led_trigger_no_link(struct phy_device *phy) |
21 | { |
22 | if (phy->last_triggered) { |
23 | led_trigger_event(trigger: &phy->last_triggered->trigger, event: LED_OFF); |
24 | led_trigger_event(trigger: &phy->led_link_trigger->trigger, event: LED_OFF); |
25 | phy->last_triggered = NULL; |
26 | } |
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 | return phy_led_trigger_no_link(phy); |
35 | |
36 | if (phy->speed == 0) |
37 | return; |
38 | |
39 | plt = phy_speed_to_led_trigger(phy, speed: phy->speed); |
40 | if (!plt) { |
41 | netdev_alert(dev: phy->attached_dev, |
42 | format: "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(trigger: &phy->led_link_trigger->trigger, |
50 | event: LED_FULL); |
51 | else |
52 | led_trigger_event(trigger: &phy->last_triggered->trigger, event: LED_OFF); |
53 | |
54 | led_trigger_event(trigger: &plt->trigger, event: LED_FULL); |
55 | phy->last_triggered = plt; |
56 | } |
57 | } |
58 | EXPORT_SYMBOL_GPL(phy_led_trigger_change_speed); |
59 | |
60 | static 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 | |
67 | static 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, buf: plt->name, size: sizeof(plt->name), suffix); |
74 | plt->trigger.name = plt->name; |
75 | |
76 | return led_trigger_register(trigger: &plt->trigger); |
77 | } |
78 | |
79 | static void phy_led_trigger_unregister(struct phy_led_trigger *plt) |
80 | { |
81 | led_trigger_unregister(trigger: &plt->trigger); |
82 | } |
83 | |
84 | int 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(dev: &phy->mdio.dev, |
95 | size: 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, plt: phy->led_link_trigger, speed: 0, suffix: "link" ); |
103 | if (err) |
104 | goto out_free_link; |
105 | |
106 | phy->phy_led_triggers = devm_kcalloc(dev: &phy->mdio.dev, |
107 | n: phy->phy_num_led_triggers, |
108 | size: 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, plt: &phy->phy_led_triggers[i], |
117 | speed: speeds[i], |
118 | suffix: phy_speed_to_str(speed: 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; |
127 | out_unreg: |
128 | while (i--) |
129 | phy_led_trigger_unregister(plt: &phy->phy_led_triggers[i]); |
130 | devm_kfree(dev: &phy->mdio.dev, p: phy->phy_led_triggers); |
131 | out_unreg_link: |
132 | phy_led_trigger_unregister(plt: phy->led_link_trigger); |
133 | out_free_link: |
134 | devm_kfree(dev: &phy->mdio.dev, p: phy->led_link_trigger); |
135 | phy->led_link_trigger = NULL; |
136 | out_clear: |
137 | phy->phy_num_led_triggers = 0; |
138 | return err; |
139 | } |
140 | EXPORT_SYMBOL_GPL(phy_led_triggers_register); |
141 | |
142 | void 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(plt: &phy->phy_led_triggers[i]); |
148 | |
149 | if (phy->led_link_trigger) |
150 | phy_led_trigger_unregister(plt: phy->led_link_trigger); |
151 | } |
152 | EXPORT_SYMBOL_GPL(phy_led_triggers_unregister); |
153 | |