[mirror_ubuntu-bionic-kernel.git] / drivers / usb / core / ledtrig-usbport.c

// SPDX-License-Identifier: GPL-2.0
/*
 * USB port LED trigger
 *
 * Copyright (C) 2016 Rafał Miłecki <rafal@milecki.pl>
 */

#include <linux/device.h>
#include <linux/leds.h>
#include <linux/module.h>
#include <linux/of.h>
#include <linux/slab.h>
#include <linux/usb.h>
#include <linux/usb/of.h>

struct usbport_trig_data {
	struct led_classdev *led_cdev;
	struct list_head ports;
	struct notifier_block nb;
	int count; /* Amount of connected matching devices */
};

struct usbport_trig_port {
	struct usbport_trig_data *data;
	struct usb_device *hub;
	int portnum;
	char *port_name;
	bool observed;
	struct device_attribute attr;
	struct list_head list;
};

/***************************************
 * Helpers
 ***************************************/

/**
 * usbport_trig_usb_dev_observed - Check if dev is connected to observed port
 */
static bool usbport_trig_usb_dev_observed(struct usbport_trig_data *usbport_data,
					  struct usb_device *usb_dev)
{
	struct usbport_trig_port *port;

	if (!usb_dev->parent)
		return false;

	list_for_each_entry(port, &usbport_data->ports, list) {
		if (usb_dev->parent == port->hub &&
		    usb_dev->portnum == port->portnum)
			return port->observed;
	}

	return false;
}

static int usbport_trig_usb_dev_check(struct usb_device *usb_dev, void *data)
{
	struct usbport_trig_data *usbport_data = data;

	if (usbport_trig_usb_dev_observed(usbport_data, usb_dev))
		usbport_data->count++;

	return 0;
}

/**
 * usbport_trig_update_count - Recalculate amount of connected matching devices
 */
static void usbport_trig_update_count(struct usbport_trig_data *usbport_data)
{
	struct led_classdev *led_cdev = usbport_data->led_cdev;

	usbport_data->count = 0;
	usb_for_each_dev(usbport_data, usbport_trig_usb_dev_check);
	led_set_brightness(led_cdev, usbport_data->count ? LED_FULL : LED_OFF);
}

/***************************************
 * Device attr
 ***************************************/

static ssize_t usbport_trig_port_show(struct device *dev,
				      struct device_attribute *attr, char *buf)
{
	struct usbport_trig_port *port = container_of(attr,
						      struct usbport_trig_port,
						      attr);

	return sprintf(buf, "%d\n", port->observed) + 1;
}

static ssize_t usbport_trig_port_store(struct device *dev,
				       struct device_attribute *attr,
				       const char *buf, size_t size)
{
	struct usbport_trig_port *port = container_of(attr,
						      struct usbport_trig_port,
						      attr);

	if (!strcmp(buf, "0") || !strcmp(buf, "0\n"))
		port->observed = 0;
	else if (!strcmp(buf, "1") || !strcmp(buf, "1\n"))
		port->observed = 1;
	else
		return -EINVAL;

	usbport_trig_update_count(port->data);

	return size;
}

static struct attribute *ports_attrs[] = {
	NULL,
};
static const struct attribute_group ports_group = {
	.name = "ports",
	.attrs = ports_attrs,
};

/***************************************
 * Adding & removing ports
 ***************************************/

/**
 * usbport_trig_port_observed - Check if port should be observed
 */
static bool usbport_trig_port_observed(struct usbport_trig_data *usbport_data,
				       struct usb_device *usb_dev, int port1)
{
	struct device *dev = usbport_data->led_cdev->dev;
	struct device_node *led_np = dev->of_node;
	struct of_phandle_args args;
	struct device_node *port_np;
	int count, i;

	if (!led_np)
		return false;

	/* Get node of port being added */
	port_np = usb_of_get_child_node(usb_dev->dev.of_node, port1);
	if (!port_np)
		return false;

	/* Amount of trigger sources for this LED */
	count = of_count_phandle_with_args(led_np, "trigger-sources",
					   "#trigger-source-cells");
	if (count < 0) {
		dev_warn(dev, "Failed to get trigger sources for %pOF\n",
			 led_np);
		return false;
	}

	/* Check list of sources for this specific port */
	for (i = 0; i < count; i++) {
		int err;

		err = of_parse_phandle_with_args(led_np, "trigger-sources",
						 "#trigger-source-cells", i,
						 &args);
		if (err) {
			dev_err(dev, "Failed to get trigger source phandle at index %d: %d\n",
				i, err);
			continue;
		}

		of_node_put(args.np);

		if (args.np == port_np)
			return true;
	}

	return false;
}

static int usbport_trig_add_port(struct usbport_trig_data *usbport_data,
				 struct usb_device *usb_dev,
				 const char *hub_name, int portnum)
{
	struct led_classdev *led_cdev = usbport_data->led_cdev;
	struct usbport_trig_port *port;
	size_t len;
	int err;

	port = kzalloc(sizeof(*port), GFP_KERNEL);
	if (!port) {
		err = -ENOMEM;
		goto err_out;
	}

	port->data = usbport_data;
	port->hub = usb_dev;
	port->portnum = portnum;
	port->observed = usbport_trig_port_observed(usbport_data, usb_dev,
						    portnum);

	len = strlen(hub_name) + 8;
	port->port_name = kzalloc(len, GFP_KERNEL);
	if (!port->port_name) {
		err = -ENOMEM;
		goto err_free_port;
	}
	snprintf(port->port_name, len, "%s-port%d", hub_name, portnum);

	sysfs_attr_init(&port->attr.attr);
	port->attr.attr.name = port->port_name;
	port->attr.attr.mode = S_IRUSR | S_IWUSR;
	port->attr.show = usbport_trig_port_show;
	port->attr.store = usbport_trig_port_store;

	err = sysfs_add_file_to_group(&led_cdev->dev->kobj, &port->attr.attr,
				      ports_group.name);
	if (err)
		goto err_free_port_name;

	list_add_tail(&port->list, &usbport_data->ports);

	return 0;

err_free_port_name:
	kfree(port->port_name);
err_free_port:
	kfree(port);
err_out:
	return err;
}

static int usbport_trig_add_usb_dev_ports(struct usb_device *usb_dev,
					  void *data)
{
	struct usbport_trig_data *usbport_data = data;
	int i;

	for (i = 1; i <= usb_dev->maxchild; i++)
		usbport_trig_add_port(usbport_data, usb_dev,
				      dev_name(&usb_dev->dev), i);

	return 0;
}

static void usbport_trig_remove_port(struct usbport_trig_data *usbport_data,
				     struct usbport_trig_port *port)
{
	struct led_classdev *led_cdev = usbport_data->led_cdev;

	list_del(&port->list);
	sysfs_remove_file_from_group(&led_cdev->dev->kobj, &port->attr.attr,
				     ports_group.name);
	kfree(port->port_name);
	kfree(port);
}

static void usbport_trig_remove_usb_dev_ports(struct usbport_trig_data *usbport_data,
					      struct usb_device *usb_dev)
{
	struct usbport_trig_port *port, *tmp;

	list_for_each_entry_safe(port, tmp, &usbport_data->ports, list) {
		if (port->hub == usb_dev)
			usbport_trig_remove_port(usbport_data, port);
	}
}

/***************************************
 * Init, exit, etc.
 ***************************************/

static int usbport_trig_notify(struct notifier_block *nb, unsigned long action,
			       void *data)
{
	struct usbport_trig_data *usbport_data =
		container_of(nb, struct usbport_trig_data, nb);
	struct led_classdev *led_cdev = usbport_data->led_cdev;
	struct usb_device *usb_dev = data;
	bool observed;

	observed = usbport_trig_usb_dev_observed(usbport_data, usb_dev);

	switch (action) {
	case USB_DEVICE_ADD:
		usbport_trig_add_usb_dev_ports(usb_dev, usbport_data);
		if (observed && usbport_data->count++ == 0)
			led_set_brightness(led_cdev, LED_FULL);
		return NOTIFY_OK;
	case USB_DEVICE_REMOVE:
		usbport_trig_remove_usb_dev_ports(usbport_data, usb_dev);
		if (observed && --usbport_data->count == 0)
			led_set_brightness(led_cdev, LED_OFF);
		return NOTIFY_OK;
	}

	return NOTIFY_DONE;
}

static void usbport_trig_activate(struct led_classdev *led_cdev)
{
	struct usbport_trig_data *usbport_data;
	int err;

	usbport_data = kzalloc(sizeof(*usbport_data), GFP_KERNEL);
	if (!usbport_data)
		return;
	usbport_data->led_cdev = led_cdev;

	/* List of ports */
	INIT_LIST_HEAD(&usbport_data->ports);
	err = sysfs_create_group(&led_cdev->dev->kobj, &ports_group);
	if (err)
		goto err_free;
	usb_for_each_dev(usbport_data, usbport_trig_add_usb_dev_ports);
	usbport_trig_update_count(usbport_data);

	/* Notifications */
	usbport_data->nb.notifier_call = usbport_trig_notify,
	led_cdev->trigger_data = usbport_data;
	usb_register_notify(&usbport_data->nb);

	led_cdev->activated = true;
	return;

err_free:
	kfree(usbport_data);
}

static void usbport_trig_deactivate(struct led_classdev *led_cdev)
{
	struct usbport_trig_data *usbport_data = led_cdev->trigger_data;
	struct usbport_trig_port *port, *tmp;

	if (!led_cdev->activated)
		return;

	list_for_each_entry_safe(port, tmp, &usbport_data->ports, list) {
		usbport_trig_remove_port(usbport_data, port);
	}

	usb_unregister_notify(&usbport_data->nb);

	sysfs_remove_group(&led_cdev->dev->kobj, &ports_group);

	kfree(usbport_data);

	led_cdev->activated = false;
}

static struct led_trigger usbport_led_trigger = {
	.name     = "usbport",
	.activate = usbport_trig_activate,
	.deactivate = usbport_trig_deactivate,
};

static int __init usbport_trig_init(void)
{
	return led_trigger_register(&usbport_led_trigger);
}

static void __exit usbport_trig_exit(void)
{
	led_trigger_unregister(&usbport_led_trigger);
}

module_init(usbport_trig_init);
module_exit(usbport_trig_exit);

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