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/sysfs.h>
 14#include <linux/usb.h>
 15#include <linux/usb/of.h>
 16
 17struct usbport_trig_data {
 18	struct led_classdev *led_cdev;
 19	struct list_head ports;
 20	struct notifier_block nb;
 21	int count; /* Amount of connected matching devices */
 22};
 23
 24struct usbport_trig_port {
 25	struct usbport_trig_data *data;
 26	struct usb_device *hub;
 27	int portnum;
 28	char *port_name;
 29	bool observed;
 30	struct device_attribute attr;
 31	struct list_head list;
 32};
 33
 34/***************************************
 35 * Helpers
 36 ***************************************/
 37
 38/*
 39 * usbport_trig_usb_dev_observed - Check if dev is connected to observed port
 40 */
 41static bool usbport_trig_usb_dev_observed(struct usbport_trig_data *usbport_data,
 42					  struct usb_device *usb_dev)
 43{
 44	struct usbport_trig_port *port;
 45
 46	if (!usb_dev->parent)
 47		return false;
 48
 49	list_for_each_entry(port, &usbport_data->ports, list) {
 50		if (usb_dev->parent == port->hub &&
 51		    usb_dev->portnum == port->portnum)
 52			return port->observed;
 53	}
 54
 55	return false;
 56}
 57
 58static int usbport_trig_usb_dev_check(struct usb_device *usb_dev, void *data)
 59{
 60	struct usbport_trig_data *usbport_data = data;
 61
 62	if (usbport_trig_usb_dev_observed(usbport_data, usb_dev))
 63		usbport_data->count++;
 64
 65	return 0;
 66}
 67
 68/*
 69 * usbport_trig_update_count - Recalculate amount of connected matching devices
 70 */
 71static void usbport_trig_update_count(struct usbport_trig_data *usbport_data)
 72{
 73	struct led_classdev *led_cdev = usbport_data->led_cdev;
 74
 75	usbport_data->count = 0;
 76	usb_for_each_dev(usbport_data, usbport_trig_usb_dev_check);
 77	led_set_brightness(led_cdev, usbport_data->count ? LED_FULL : LED_OFF);
 78}
 79
 80/***************************************
 81 * Device attr
 82 ***************************************/
 83
 84static ssize_t usbport_trig_port_show(struct device *dev,
 85				      struct device_attribute *attr, char *buf)
 86{
 87	struct usbport_trig_port *port = container_of(attr,
 88						      struct usbport_trig_port,
 89						      attr);
 90
 91	return sysfs_emit(buf, "%d\n", port->observed) + 1;
 92}
 93
 94static ssize_t usbport_trig_port_store(struct device *dev,
 95				       struct device_attribute *attr,
 96				       const char *buf, size_t size)
 97{
 98	struct usbport_trig_port *port = container_of(attr,
 99						      struct usbport_trig_port,
100						      attr);
101
102	if (!strcmp(buf, "0") || !strcmp(buf, "0\n"))
103		port->observed = 0;
104	else if (!strcmp(buf, "1") || !strcmp(buf, "1\n"))
105		port->observed = 1;
106	else
107		return -EINVAL;
108
109	usbport_trig_update_count(port->data);
110
111	return size;
112}
113
114static struct attribute *ports_attrs[] = {
115	NULL,
116};
117
118static const struct attribute_group ports_group = {
119	.name = "ports",
120	.attrs = ports_attrs,
121};
122
123/***************************************
124 * Adding & removing ports
125 ***************************************/
126
127/*
128 * usbport_trig_port_observed - Check if port should be observed
129 */
130static bool usbport_trig_port_observed(struct usbport_trig_data *usbport_data,
131				       struct usb_device *usb_dev, int port1)
132{
133	struct device *dev = usbport_data->led_cdev->dev;
134	struct device_node *led_np = dev->of_node;
135	struct of_phandle_args args;
136	struct device_node *port_np;
137	int count, i;
138
139	if (!led_np)
140		return false;
141
142	/*
143	 * Get node of port being added
144	 *
145	 * FIXME: This is really the device node of the connected device
146	 */
147	port_np = usb_of_get_device_node(usb_dev, port1);
148	if (!port_np)
149		return false;
150
151	of_node_put(port_np);
152
153	/* Amount of trigger sources for this LED */
154	count = of_count_phandle_with_args(led_np, "trigger-sources",
155					   "#trigger-source-cells");
156	if (count < 0) {
157		dev_warn(dev, "Failed to get trigger sources for %pOF\n",
158			 led_np);
159		return false;
160	}
161
162	/* Check list of sources for this specific port */
163	for (i = 0; i < count; i++) {
164		int err;
165
166		err = of_parse_phandle_with_args(led_np, "trigger-sources",
167						 "#trigger-source-cells", i,
168						 &args);
169		if (err) {
170			dev_err(dev, "Failed to get trigger source phandle at index %d: %d\n",
171				i, err);
172			continue;
173		}
174
175		of_node_put(args.np);
176
177		if (args.np == port_np)
178			return true;
179	}
180
181	return false;
182}
183
184static int usbport_trig_add_port(struct usbport_trig_data *usbport_data,
185				 struct usb_device *usb_dev,
186				 const char *hub_name, int portnum)
187{
188	struct led_classdev *led_cdev = usbport_data->led_cdev;
189	struct usbport_trig_port *port;
190	size_t len;
191	int err;
192
193	port = kzalloc(sizeof(*port), GFP_KERNEL);
194	if (!port) {
195		err = -ENOMEM;
196		goto err_out;
197	}
198
199	port->data = usbport_data;
200	port->hub = usb_dev;
201	port->portnum = portnum;
202	port->observed = usbport_trig_port_observed(usbport_data, usb_dev,
203						    portnum);
204
205	len = strlen(hub_name) + 8;
206	port->port_name = kzalloc(len, GFP_KERNEL);
207	if (!port->port_name) {
208		err = -ENOMEM;
209		goto err_free_port;
210	}
211	snprintf(port->port_name, len, "%s-port%d", hub_name, portnum);
212
213	sysfs_attr_init(&port->attr.attr);
214	port->attr.attr.name = port->port_name;
215	port->attr.attr.mode = S_IRUSR | S_IWUSR;
216	port->attr.show = usbport_trig_port_show;
217	port->attr.store = usbport_trig_port_store;
218
219	err = sysfs_add_file_to_group(&led_cdev->dev->kobj, &port->attr.attr,
220				      ports_group.name);
221	if (err)
222		goto err_free_port_name;
223
224	list_add_tail(&port->list, &usbport_data->ports);
225
226	return 0;
227
228err_free_port_name:
229	kfree(port->port_name);
230err_free_port:
231	kfree(port);
232err_out:
233	return err;
234}
235
236static int usbport_trig_add_usb_dev_ports(struct usb_device *usb_dev,
237					  void *data)
238{
239	struct usbport_trig_data *usbport_data = data;
240	int i;
241
242	for (i = 1; i <= usb_dev->maxchild; i++)
243		usbport_trig_add_port(usbport_data, usb_dev,
244				      dev_name(&usb_dev->dev), i);
245
246	return 0;
247}
248
249static void usbport_trig_remove_port(struct usbport_trig_data *usbport_data,
250				     struct usbport_trig_port *port)
251{
252	struct led_classdev *led_cdev = usbport_data->led_cdev;
253
254	list_del(&port->list);
255	sysfs_remove_file_from_group(&led_cdev->dev->kobj, &port->attr.attr,
256				     ports_group.name);
257	kfree(port->port_name);
258	kfree(port);
259}
260
261static void usbport_trig_remove_usb_dev_ports(struct usbport_trig_data *usbport_data,
262					      struct usb_device *usb_dev)
263{
264	struct usbport_trig_port *port, *tmp;
265
266	list_for_each_entry_safe(port, tmp, &usbport_data->ports, list) {
267		if (port->hub == usb_dev)
268			usbport_trig_remove_port(usbport_data, port);
269	}
270}
271
272/***************************************
273 * Init, exit, etc.
274 ***************************************/
275
276static int usbport_trig_notify(struct notifier_block *nb, unsigned long action,
277			       void *data)
278{
279	struct usbport_trig_data *usbport_data =
280		container_of(nb, struct usbport_trig_data, nb);
281	struct led_classdev *led_cdev = usbport_data->led_cdev;
282	struct usb_device *usb_dev = data;
283	bool observed;
284
285	observed = usbport_trig_usb_dev_observed(usbport_data, usb_dev);
286
287	switch (action) {
288	case USB_DEVICE_ADD:
289		usbport_trig_add_usb_dev_ports(usb_dev, usbport_data);
290		if (observed && usbport_data->count++ == 0)
291			led_set_brightness(led_cdev, LED_FULL);
292		return NOTIFY_OK;
293	case USB_DEVICE_REMOVE:
294		usbport_trig_remove_usb_dev_ports(usbport_data, usb_dev);
295		if (observed && --usbport_data->count == 0)
296			led_set_brightness(led_cdev, LED_OFF);
297		return NOTIFY_OK;
298	}
299
300	return NOTIFY_DONE;
301}
302
303static int usbport_trig_activate(struct led_classdev *led_cdev)
304{
305	struct usbport_trig_data *usbport_data;
306	int err;
307
308	usbport_data = kzalloc(sizeof(*usbport_data), GFP_KERNEL);
309	if (!usbport_data)
310		return -ENOMEM;
311	usbport_data->led_cdev = led_cdev;
312
313	/* List of ports */
314	INIT_LIST_HEAD(&usbport_data->ports);
315	err = sysfs_create_group(&led_cdev->dev->kobj, &ports_group);
316	if (err)
317		goto err_free;
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	return 0;
326
327err_free:
328	kfree(usbport_data);
329	return err;
330}
331
332static void usbport_trig_deactivate(struct led_classdev *led_cdev)
333{
334	struct usbport_trig_data *usbport_data = led_get_trigger_data(led_cdev);
335	struct usbport_trig_port *port, *tmp;
336
337	list_for_each_entry_safe(port, tmp, &usbport_data->ports, list) {
338		usbport_trig_remove_port(usbport_data, port);
339	}
340
341	sysfs_remove_group(&led_cdev->dev->kobj, &ports_group);
342
343	usb_unregister_notify(&usbport_data->nb);
344
345	kfree(usbport_data);
346}
347
348static struct led_trigger usbport_led_trigger = {
349	.name     = "usbport",
350	.activate = usbport_trig_activate,
351	.deactivate = usbport_trig_deactivate,
352};
353
354module_led_trigger(usbport_led_trigger);
355
356MODULE_AUTHOR("Rafał Miłecki <rafal@milecki.pl>");
357MODULE_DESCRIPTION("USB port trigger");
358MODULE_LICENSE("GPL v2");