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(data: usbport_data, fn: usbport_trig_usb_dev_check); |
76 | led_set_brightness(led_cdev, brightness: 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, fmt: "%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(usbport_data: 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(hub: usb_dev, port1); |
147 | if (!port_np) |
148 | return false; |
149 | |
150 | of_node_put(node: port_np); |
151 | |
152 | /* Amount of trigger sources for this LED */ |
153 | count = of_count_phandle_with_args(np: led_np, list_name: "trigger-sources" , |
154 | cells_name: "#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(np: led_np, list_name: "trigger-sources" , |
166 | cells_name: "#trigger-source-cells" , index: i, |
167 | out_args: &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(node: 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(size: 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 | port1: portnum); |
203 | |
204 | len = strlen(hub_name) + 8; |
205 | port->port_name = kzalloc(size: len, GFP_KERNEL); |
206 | if (!port->port_name) { |
207 | err = -ENOMEM; |
208 | goto err_free_port; |
209 | } |
210 | snprintf(buf: port->port_name, size: len, fmt: "%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(kobj: &led_cdev->dev->kobj, attr: &port->attr.attr, |
219 | group: ports_group.name); |
220 | if (err) |
221 | goto err_free_port_name; |
222 | |
223 | list_add_tail(new: &port->list, head: &usbport_data->ports); |
224 | |
225 | return 0; |
226 | |
227 | err_free_port_name: |
228 | kfree(objp: port->port_name); |
229 | err_free_port: |
230 | kfree(objp: 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 | hub_name: dev_name(dev: &usb_dev->dev), portnum: 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(entry: &port->list); |
254 | sysfs_remove_file_from_group(kobj: &led_cdev->dev->kobj, attr: &port->attr.attr, |
255 | group: ports_group.name); |
256 | kfree(objp: port->port_name); |
257 | kfree(objp: 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, data: usbport_data); |
289 | if (observed && usbport_data->count++ == 0) |
290 | led_set_brightness(led_cdev, brightness: 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, brightness: 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(size: 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(list: &usbport_data->ports); |
314 | err = sysfs_create_group(kobj: &led_cdev->dev->kobj, grp: &ports_group); |
315 | if (err) |
316 | goto err_free; |
317 | usb_for_each_dev(data: usbport_data, fn: 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, trigger_data: usbport_data); |
323 | usb_register_notify(nb: &usbport_data->nb); |
324 | return 0; |
325 | |
326 | err_free: |
327 | kfree(objp: 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(kobj: &led_cdev->dev->kobj, grp: &ports_group); |
341 | |
342 | usb_unregister_notify(nb: &usbport_data->nb); |
343 | |
344 | kfree(objp: 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 | module_led_trigger(usbport_led_trigger); |
354 | |
355 | MODULE_AUTHOR("Rafał Miłecki <rafal@milecki.pl>" ); |
356 | MODULE_DESCRIPTION("USB port trigger" ); |
357 | MODULE_LICENSE("GPL v2" ); |
358 | |