GNU Linux-libre 4.19.286-gnu1
[releases.git] / drivers / usb / core / ledtrig-usbport.c
1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * USB port LED trigger
4  *
5  * Copyright (C) 2016 Rafał Miłecki <rafal@milecki.pl>
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
16 struct usbport_trig_data {
17         struct led_classdev *led_cdev;
18         struct list_head ports;
19         struct notifier_block nb;
20         int count; /* Amount of connected matching devices */
21 };
22
23 struct 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  * Helpers
35  ***************************************/
36
37 /**
38  * usbport_trig_usb_dev_observed - Check if dev is connected to observed port
39  */
40 static 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
57 static 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  * usbport_trig_update_count - Recalculate amount of connected matching devices
69  */
70 static 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  * Device attr
81  ***************************************/
82
83 static 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
93 static 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
113 static struct attribute *ports_attrs[] = {
114         NULL,
115 };
116
117 static const struct attribute_group ports_group = {
118         .name = "ports",
119         .attrs = ports_attrs,
120 };
121
122 /***************************************
123  * Adding & removing ports
124  ***************************************/
125
126 /**
127  * usbport_trig_port_observed - Check if port should be observed
128  */
129 static 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          * Get node of port being added
143          *
144          * FIXME: This is really the device node of the connected device
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         /* Amount of trigger sources for this LED */
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         /* Check list of sources for this specific port */
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
183 static 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
227 err_free_port_name:
228         kfree(port->port_name);
229 err_free_port:
230         kfree(port);
231 err_out:
232         return err;
233 }
234
235 static 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
248 static 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
260 static 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  * Init, exit, etc.
273  ***************************************/
274
275 static 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
302 static 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         /* List of ports */
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         /* Notifications */
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
326 err_free:
327         kfree(usbport_data);
328         return err;
329 }
330
331 static 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
347 static struct led_trigger usbport_led_trigger = {
348         .name     = "usbport",
349         .activate = usbport_trig_activate,
350         .deactivate = usbport_trig_deactivate,
351 };
352
353 static int __init usbport_trig_init(void)
354 {
355         return led_trigger_register(&usbport_led_trigger);
356 }
357
358 static void __exit usbport_trig_exit(void)
359 {
360         led_trigger_unregister(&usbport_led_trigger);
361 }
362
363 module_init(usbport_trig_init);
364 module_exit(usbport_trig_exit);
365
366 MODULE_AUTHOR("Rafał Miłecki <rafal@milecki.pl>");
367 MODULE_DESCRIPTION("USB port trigger");
368 MODULE_LICENSE("GPL v2");