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
16struct 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
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 * Helpers
35 ***************************************/
36
37/*
38 * usbport_trig_usb_dev_observed - Check if dev is connected to observed port
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 * usbport_trig_update_count - Recalculate amount of connected matching devices
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 * Device attr
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 * Adding & removing ports
124 ***************************************/
125
126/*
127 * usbport_trig_port_observed - Check if port should be observed
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	 * 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
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 * Init, exit, etc.
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	/* 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
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
353module_led_trigger(usbport_led_trigger);
354
355MODULE_AUTHOR("Rafa�� Mi��ecki <rafal@milecki.pl>");
356MODULE_DESCRIPTION("USB port trigger");
357MODULE_LICENSE("GPL v2");
358