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 */
usbport_trig_usb_dev_observed(struct usbport_trig_data * usbport_data,struct usb_device * usb_dev)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
usbport_trig_usb_dev_check(struct usb_device * usb_dev,void * data)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 */
usbport_trig_update_count(struct usbport_trig_data * usbport_data)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
usbport_trig_port_show(struct device * dev,struct device_attribute * attr,char * buf)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
usbport_trig_port_store(struct device * dev,struct device_attribute * attr,const char * buf,size_t size)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 static const struct attribute_group *ports_groups[] = {
123 &ports_group,
124 NULL
125 };
126
127 /***************************************
128 * Adding & removing ports
129 ***************************************/
130
131 /**
132 * usbport_trig_port_observed - Check if port should be observed
133 */
usbport_trig_port_observed(struct usbport_trig_data * usbport_data,struct usb_device * usb_dev,int port1)134 static bool usbport_trig_port_observed(struct usbport_trig_data *usbport_data,
135 struct usb_device *usb_dev, int port1)
136 {
137 struct device *dev = usbport_data->led_cdev->dev;
138 struct device_node *led_np = dev->of_node;
139 struct of_phandle_args args;
140 struct device_node *port_np;
141 int count, i;
142
143 if (!led_np)
144 return false;
145
146 /*
147 * Get node of port being added
148 *
149 * FIXME: This is really the device node of the connected device
150 */
151 port_np = usb_of_get_device_node(usb_dev, port1);
152 if (!port_np)
153 return false;
154
155 of_node_put(port_np);
156
157 /* Amount of trigger sources for this LED */
158 count = of_count_phandle_with_args(led_np, "trigger-sources",
159 "#trigger-source-cells");
160 if (count < 0) {
161 dev_warn(dev, "Failed to get trigger sources for %pOF\n",
162 led_np);
163 return false;
164 }
165
166 /* Check list of sources for this specific port */
167 for (i = 0; i < count; i++) {
168 int err;
169
170 err = of_parse_phandle_with_args(led_np, "trigger-sources",
171 "#trigger-source-cells", i,
172 &args);
173 if (err) {
174 dev_err(dev, "Failed to get trigger source phandle at index %d: %d\n",
175 i, err);
176 continue;
177 }
178
179 of_node_put(args.np);
180
181 if (args.np == port_np)
182 return true;
183 }
184
185 return false;
186 }
187
usbport_trig_add_port(struct usbport_trig_data * usbport_data,struct usb_device * usb_dev,const char * hub_name,int portnum)188 static int usbport_trig_add_port(struct usbport_trig_data *usbport_data,
189 struct usb_device *usb_dev,
190 const char *hub_name, int portnum)
191 {
192 struct led_classdev *led_cdev = usbport_data->led_cdev;
193 struct usbport_trig_port *port;
194 size_t len;
195 int err;
196
197 port = kzalloc(sizeof(*port), GFP_KERNEL);
198 if (!port) {
199 err = -ENOMEM;
200 goto err_out;
201 }
202
203 port->data = usbport_data;
204 port->hub = usb_dev;
205 port->portnum = portnum;
206 port->observed = usbport_trig_port_observed(usbport_data, usb_dev,
207 portnum);
208
209 len = strlen(hub_name) + 8;
210 port->port_name = kzalloc(len, GFP_KERNEL);
211 if (!port->port_name) {
212 err = -ENOMEM;
213 goto err_free_port;
214 }
215 snprintf(port->port_name, len, "%s-port%d", hub_name, portnum);
216
217 sysfs_attr_init(&port->attr.attr);
218 port->attr.attr.name = port->port_name;
219 port->attr.attr.mode = S_IRUSR | S_IWUSR;
220 port->attr.show = usbport_trig_port_show;
221 port->attr.store = usbport_trig_port_store;
222
223 err = sysfs_add_file_to_group(&led_cdev->dev->kobj, &port->attr.attr,
224 ports_group.name);
225 if (err)
226 goto err_free_port_name;
227
228 list_add_tail(&port->list, &usbport_data->ports);
229
230 return 0;
231
232 err_free_port_name:
233 kfree(port->port_name);
234 err_free_port:
235 kfree(port);
236 err_out:
237 return err;
238 }
239
usbport_trig_add_usb_dev_ports(struct usb_device * usb_dev,void * data)240 static int usbport_trig_add_usb_dev_ports(struct usb_device *usb_dev,
241 void *data)
242 {
243 struct usbport_trig_data *usbport_data = data;
244 int i;
245
246 for (i = 1; i <= usb_dev->maxchild; i++)
247 usbport_trig_add_port(usbport_data, usb_dev,
248 dev_name(&usb_dev->dev), i);
249
250 return 0;
251 }
252
usbport_trig_remove_port(struct usbport_trig_data * usbport_data,struct usbport_trig_port * port)253 static void usbport_trig_remove_port(struct usbport_trig_data *usbport_data,
254 struct usbport_trig_port *port)
255 {
256 struct led_classdev *led_cdev = usbport_data->led_cdev;
257
258 list_del(&port->list);
259 sysfs_remove_file_from_group(&led_cdev->dev->kobj, &port->attr.attr,
260 ports_group.name);
261 kfree(port->port_name);
262 kfree(port);
263 }
264
usbport_trig_remove_usb_dev_ports(struct usbport_trig_data * usbport_data,struct usb_device * usb_dev)265 static void usbport_trig_remove_usb_dev_ports(struct usbport_trig_data *usbport_data,
266 struct usb_device *usb_dev)
267 {
268 struct usbport_trig_port *port, *tmp;
269
270 list_for_each_entry_safe(port, tmp, &usbport_data->ports, list) {
271 if (port->hub == usb_dev)
272 usbport_trig_remove_port(usbport_data, port);
273 }
274 }
275
276 /***************************************
277 * Init, exit, etc.
278 ***************************************/
279
usbport_trig_notify(struct notifier_block * nb,unsigned long action,void * data)280 static int usbport_trig_notify(struct notifier_block *nb, unsigned long action,
281 void *data)
282 {
283 struct usbport_trig_data *usbport_data =
284 container_of(nb, struct usbport_trig_data, nb);
285 struct led_classdev *led_cdev = usbport_data->led_cdev;
286 struct usb_device *usb_dev = data;
287 bool observed;
288
289 observed = usbport_trig_usb_dev_observed(usbport_data, usb_dev);
290
291 switch (action) {
292 case USB_DEVICE_ADD:
293 usbport_trig_add_usb_dev_ports(usb_dev, usbport_data);
294 if (observed && usbport_data->count++ == 0)
295 led_set_brightness(led_cdev, LED_FULL);
296 return NOTIFY_OK;
297 case USB_DEVICE_REMOVE:
298 usbport_trig_remove_usb_dev_ports(usbport_data, usb_dev);
299 if (observed && --usbport_data->count == 0)
300 led_set_brightness(led_cdev, LED_OFF);
301 return NOTIFY_OK;
302 }
303
304 return NOTIFY_DONE;
305 }
306
usbport_trig_activate(struct led_classdev * led_cdev)307 static int usbport_trig_activate(struct led_classdev *led_cdev)
308 {
309 struct usbport_trig_data *usbport_data;
310
311 usbport_data = kzalloc(sizeof(*usbport_data), GFP_KERNEL);
312 if (!usbport_data)
313 return -ENOMEM;
314 usbport_data->led_cdev = led_cdev;
315
316 /* List of ports */
317 INIT_LIST_HEAD(&usbport_data->ports);
318 usb_for_each_dev(usbport_data, usbport_trig_add_usb_dev_ports);
319 usbport_trig_update_count(usbport_data);
320
321 /* Notifications */
322 usbport_data->nb.notifier_call = usbport_trig_notify;
323 led_set_trigger_data(led_cdev, usbport_data);
324 usb_register_notify(&usbport_data->nb);
325
326 return 0;
327 }
328
usbport_trig_deactivate(struct led_classdev * led_cdev)329 static void usbport_trig_deactivate(struct led_classdev *led_cdev)
330 {
331 struct usbport_trig_data *usbport_data = led_get_trigger_data(led_cdev);
332 struct usbport_trig_port *port, *tmp;
333
334 list_for_each_entry_safe(port, tmp, &usbport_data->ports, list) {
335 usbport_trig_remove_port(usbport_data, port);
336 }
337
338 usb_unregister_notify(&usbport_data->nb);
339
340 kfree(usbport_data);
341 }
342
343 static struct led_trigger usbport_led_trigger = {
344 .name = "usbport",
345 .activate = usbport_trig_activate,
346 .deactivate = usbport_trig_deactivate,
347 .groups = ports_groups,
348 };
349
usbport_trig_init(void)350 static int __init usbport_trig_init(void)
351 {
352 return led_trigger_register(&usbport_led_trigger);
353 }
354
usbport_trig_exit(void)355 static void __exit usbport_trig_exit(void)
356 {
357 led_trigger_unregister(&usbport_led_trigger);
358 }
359
360 module_init(usbport_trig_init);
361 module_exit(usbport_trig_exit);
362
363 MODULE_AUTHOR("Rafał Miłecki <rafal@milecki.pl>");
364 MODULE_DESCRIPTION("USB port trigger");
365 MODULE_LICENSE("GPL v2");
366
