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 /***************************************
123  * Adding & removing ports
124  ***************************************/
125 
126 /*
127  * usbport_trig_port_observed - Check if port should be observed
128  */
usbport_trig_port_observed(struct usbport_trig_data * usbport_data,struct usb_device * usb_dev,int port1)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 
usbport_trig_add_port(struct usbport_trig_data * usbport_data,struct usb_device * usb_dev,const char * hub_name,int portnum)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 
usbport_trig_add_usb_dev_ports(struct usb_device * usb_dev,void * data)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 
usbport_trig_remove_port(struct usbport_trig_data * usbport_data,struct usbport_trig_port * port)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 
usbport_trig_remove_usb_dev_ports(struct usbport_trig_data * usbport_data,struct usb_device * usb_dev)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 
usbport_trig_notify(struct notifier_block * nb,unsigned long action,void * data)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 
usbport_trig_activate(struct led_classdev * led_cdev)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 
usbport_trig_deactivate(struct led_classdev * led_cdev)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 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