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