1
2
3
4
5
6
7
8
9
10
11#include <linux/device.h>
12#include <linux/leds.h>
13#include <linux/module.h>
14#include <linux/of.h>
15#include <linux/slab.h>
16#include <linux/usb.h>
17#include <linux/usb/of.h>
18
19struct usbport_trig_data {
20 struct led_classdev *led_cdev;
21 struct list_head ports;
22 struct notifier_block nb;
23 int count;
24};
25
26struct usbport_trig_port {
27 struct usbport_trig_data *data;
28 struct usb_device *hub;
29 int portnum;
30 char *port_name;
31 bool observed;
32 struct device_attribute attr;
33 struct list_head list;
34};
35
36
37
38
39
40
41
42
43static bool usbport_trig_usb_dev_observed(struct usbport_trig_data *usbport_data,
44 struct usb_device *usb_dev)
45{
46 struct usbport_trig_port *port;
47
48 if (!usb_dev->parent)
49 return false;
50
51 list_for_each_entry(port, &usbport_data->ports, list) {
52 if (usb_dev->parent == port->hub &&
53 usb_dev->portnum == port->portnum)
54 return port->observed;
55 }
56
57 return false;
58}
59
60static int usbport_trig_usb_dev_check(struct usb_device *usb_dev, void *data)
61{
62 struct usbport_trig_data *usbport_data = data;
63
64 if (usbport_trig_usb_dev_observed(usbport_data, usb_dev))
65 usbport_data->count++;
66
67 return 0;
68}
69
70
71
72
73static void usbport_trig_update_count(struct usbport_trig_data *usbport_data)
74{
75 struct led_classdev *led_cdev = usbport_data->led_cdev;
76
77 usbport_data->count = 0;
78 usb_for_each_dev(usbport_data, usbport_trig_usb_dev_check);
79 led_set_brightness(led_cdev, usbport_data->count ? LED_FULL : LED_OFF);
80}
81
82
83
84
85
86static ssize_t usbport_trig_port_show(struct device *dev,
87 struct device_attribute *attr, char *buf)
88{
89 struct usbport_trig_port *port = container_of(attr,
90 struct usbport_trig_port,
91 attr);
92
93 return sprintf(buf, "%d\n", port->observed) + 1;
94}
95
96static ssize_t usbport_trig_port_store(struct device *dev,
97 struct device_attribute *attr,
98 const char *buf, size_t size)
99{
100 struct usbport_trig_port *port = container_of(attr,
101 struct usbport_trig_port,
102 attr);
103
104 if (!strcmp(buf, "0") || !strcmp(buf, "0\n"))
105 port->observed = 0;
106 else if (!strcmp(buf, "1") || !strcmp(buf, "1\n"))
107 port->observed = 1;
108 else
109 return -EINVAL;
110
111 usbport_trig_update_count(port->data);
112
113 return size;
114}
115
116static struct attribute *ports_attrs[] = {
117 NULL,
118};
119static const struct attribute_group ports_group = {
120 .name = "ports",
121 .attrs = ports_attrs,
122};
123
124
125
126
127
128
129
130
131static bool usbport_trig_port_observed(struct usbport_trig_data *usbport_data,
132 struct usb_device *usb_dev, int port1)
133{
134 struct device *dev = usbport_data->led_cdev->dev;
135 struct device_node *led_np = dev->of_node;
136 struct of_phandle_args args;
137 struct device_node *port_np;
138 int count, i;
139
140 if (!led_np)
141 return false;
142
143
144 port_np = usb_of_get_child_node(usb_dev->dev.of_node, port1);
145 if (!port_np)
146 return false;
147
148
149 count = of_count_phandle_with_args(led_np, "trigger-sources",
150 "#trigger-source-cells");
151 if (count < 0) {
152 dev_warn(dev, "Failed to get trigger sources for %pOF\n",
153 led_np);
154 return false;
155 }
156
157
158 for (i = 0; i < count; i++) {
159 int err;
160
161 err = of_parse_phandle_with_args(led_np, "trigger-sources",
162 "#trigger-source-cells", i,
163 &args);
164 if (err) {
165 dev_err(dev, "Failed to get trigger source phandle at index %d: %d\n",
166 i, err);
167 continue;
168 }
169
170 of_node_put(args.np);
171
172 if (args.np == port_np)
173 return true;
174 }
175
176 return false;
177}
178
179static int usbport_trig_add_port(struct usbport_trig_data *usbport_data,
180 struct usb_device *usb_dev,
181 const char *hub_name, int portnum)
182{
183 struct led_classdev *led_cdev = usbport_data->led_cdev;
184 struct usbport_trig_port *port;
185 size_t len;
186 int err;
187
188 port = kzalloc(sizeof(*port), GFP_KERNEL);
189 if (!port) {
190 err = -ENOMEM;
191 goto err_out;
192 }
193
194 port->data = usbport_data;
195 port->hub = usb_dev;
196 port->portnum = portnum;
197 port->observed = usbport_trig_port_observed(usbport_data, usb_dev,
198 portnum);
199
200 len = strlen(hub_name) + 8;
201 port->port_name = kzalloc(len, GFP_KERNEL);
202 if (!port->port_name) {
203 err = -ENOMEM;
204 goto err_free_port;
205 }
206 snprintf(port->port_name, len, "%s-port%d", hub_name, portnum);
207
208 sysfs_attr_init(&port->attr.attr);
209 port->attr.attr.name = port->port_name;
210 port->attr.attr.mode = S_IRUSR | S_IWUSR;
211 port->attr.show = usbport_trig_port_show;
212 port->attr.store = usbport_trig_port_store;
213
214 err = sysfs_add_file_to_group(&led_cdev->dev->kobj, &port->attr.attr,
215 ports_group.name);
216 if (err)
217 goto err_free_port_name;
218
219 list_add_tail(&port->list, &usbport_data->ports);
220
221 return 0;
222
223err_free_port_name:
224 kfree(port->port_name);
225err_free_port:
226 kfree(port);
227err_out:
228 return err;
229}
230
231static int usbport_trig_add_usb_dev_ports(struct usb_device *usb_dev,
232 void *data)
233{
234 struct usbport_trig_data *usbport_data = data;
235 int i;
236
237 for (i = 1; i <= usb_dev->maxchild; i++)
238 usbport_trig_add_port(usbport_data, usb_dev,
239 dev_name(&usb_dev->dev), i);
240
241 return 0;
242}
243
244static void usbport_trig_remove_port(struct usbport_trig_data *usbport_data,
245 struct usbport_trig_port *port)
246{
247 struct led_classdev *led_cdev = usbport_data->led_cdev;
248
249 list_del(&port->list);
250 sysfs_remove_file_from_group(&led_cdev->dev->kobj, &port->attr.attr,
251 ports_group.name);
252 kfree(port->port_name);
253 kfree(port);
254}
255
256static void usbport_trig_remove_usb_dev_ports(struct usbport_trig_data *usbport_data,
257 struct usb_device *usb_dev)
258{
259 struct usbport_trig_port *port, *tmp;
260
261 list_for_each_entry_safe(port, tmp, &usbport_data->ports, list) {
262 if (port->hub == usb_dev)
263 usbport_trig_remove_port(usbport_data, port);
264 }
265}
266
267
268
269
270
271static int usbport_trig_notify(struct notifier_block *nb, unsigned long action,
272 void *data)
273{
274 struct usbport_trig_data *usbport_data =
275 container_of(nb, struct usbport_trig_data, nb);
276 struct led_classdev *led_cdev = usbport_data->led_cdev;
277 struct usb_device *usb_dev = data;
278 bool observed;
279
280 observed = usbport_trig_usb_dev_observed(usbport_data, usb_dev);
281
282 switch (action) {
283 case USB_DEVICE_ADD:
284 usbport_trig_add_usb_dev_ports(usb_dev, usbport_data);
285 if (observed && usbport_data->count++ == 0)
286 led_set_brightness(led_cdev, LED_FULL);
287 return NOTIFY_OK;
288 case USB_DEVICE_REMOVE:
289 usbport_trig_remove_usb_dev_ports(usbport_data, usb_dev);
290 if (observed && --usbport_data->count == 0)
291 led_set_brightness(led_cdev, LED_OFF);
292 return NOTIFY_OK;
293 }
294
295 return NOTIFY_DONE;
296}
297
298static void usbport_trig_activate(struct led_classdev *led_cdev)
299{
300 struct usbport_trig_data *usbport_data;
301 int err;
302
303 usbport_data = kzalloc(sizeof(*usbport_data), GFP_KERNEL);
304 if (!usbport_data)
305 return;
306 usbport_data->led_cdev = led_cdev;
307
308
309 INIT_LIST_HEAD(&usbport_data->ports);
310 err = sysfs_create_group(&led_cdev->dev->kobj, &ports_group);
311 if (err)
312 goto err_free;
313 usb_for_each_dev(usbport_data, usbport_trig_add_usb_dev_ports);
314 usbport_trig_update_count(usbport_data);
315
316
317 usbport_data->nb.notifier_call = usbport_trig_notify,
318 led_cdev->trigger_data = usbport_data;
319 usb_register_notify(&usbport_data->nb);
320
321 led_cdev->activated = true;
322 return;
323
324err_free:
325 kfree(usbport_data);
326}
327
328static void usbport_trig_deactivate(struct led_classdev *led_cdev)
329{
330 struct usbport_trig_data *usbport_data = led_cdev->trigger_data;
331 struct usbport_trig_port *port, *tmp;
332
333 if (!led_cdev->activated)
334 return;
335
336 list_for_each_entry_safe(port, tmp, &usbport_data->ports, list) {
337 usbport_trig_remove_port(usbport_data, port);
338 }
339
340 usb_unregister_notify(&usbport_data->nb);
341
342 sysfs_remove_group(&led_cdev->dev->kobj, &ports_group);
343
344 kfree(usbport_data);
345
346 led_cdev->activated = false;
347}
348
349static struct led_trigger usbport_led_trigger = {
350 .name = "usbport",
351 .activate = usbport_trig_activate,
352 .deactivate = usbport_trig_deactivate,
353};
354
355static int __init usbport_trig_init(void)
356{
357 return led_trigger_register(&usbport_led_trigger);
358}
359
360static void __exit usbport_trig_exit(void)
361{
362 led_trigger_unregister(&usbport_led_trigger);
363}
364
365module_init(usbport_trig_init);
366module_exit(usbport_trig_exit);
367
368MODULE_AUTHOR("Rafał Miłecki <rafal@milecki.pl>");
369MODULE_DESCRIPTION("USB port trigger");
370MODULE_LICENSE("GPL v2");
371
