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