usb/core/ledtrig-usbport.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * USB port LED trigger
0004  *
0005  * Copyright (C) 2016 Rafał Miłecki <rafal@milecki.pl>
0006  */
0007
0008 #include <linux/device.h>
0009 #include <linux/leds.h>
0010 #include <linux/module.h>
0011 #include <linux/of.h>
0012 #include <linux/slab.h>
0013 #include <linux/usb.h>
0014 #include <linux/usb/of.h>
0015
0016 struct usbport_trig_data {
0017     struct led_classdev *led_cdev;
0018     struct list_head ports;
0019     struct notifier_block nb;
0020     int count; /* Amount of connected matching devices */
0021 };
0022
0023 struct usbport_trig_port {
0024     struct usbport_trig_data *data;
0025     struct usb_device *hub;
0026     int portnum;
0027     char *port_name;
0028     bool observed;
0029     struct device_attribute attr;
0030     struct list_head list;
0031 };
0032
0033 /***************************************
0034  * Helpers
0035  ***************************************/
0036
0037 /*
0038  * usbport_trig_usb_dev_observed - Check if dev is connected to observed port
0039  */
0040 static bool usbport_trig_usb_dev_observed(struct usbport_trig_data *usbport_data,
0041                       struct usb_device *usb_dev)
0042 {
0043     struct usbport_trig_port *port;
0044
0045     if (!usb_dev->parent)
0046         return false;
0047
0048     list_for_each_entry(port, &usbport_data->ports, list) {
0049         if (usb_dev->parent == port->hub &&
0050             usb_dev->portnum == port->portnum)
0051             return port->observed;
0052     }
0053
0054     return false;
0055 }
0056
0057 static int usbport_trig_usb_dev_check(struct usb_device *usb_dev, void *data)
0058 {
0059     struct usbport_trig_data *usbport_data = data;
0060
0061     if (usbport_trig_usb_dev_observed(usbport_data, usb_dev))
0062         usbport_data->count++;
0063
0064     return 0;
0065 }
0066
0067 /*
0068  * usbport_trig_update_count - Recalculate amount of connected matching devices
0069  */
0070 static void usbport_trig_update_count(struct usbport_trig_data *usbport_data)
0071 {
0072     struct led_classdev *led_cdev = usbport_data->led_cdev;
0073
0074     usbport_data->count = 0;
0075     usb_for_each_dev(usbport_data, usbport_trig_usb_dev_check);
0076     led_set_brightness(led_cdev, usbport_data->count ? LED_FULL : LED_OFF);
0077 }
0078
0079 /***************************************
0080  * Device attr
0081  ***************************************/
0082
0083 static ssize_t usbport_trig_port_show(struct device *dev,
0084                       struct device_attribute *attr, char *buf)
0085 {
0086     struct usbport_trig_port *port = container_of(attr,
0087                               struct usbport_trig_port,
0088                               attr);
0089
0090     return sprintf(buf, "%d\n", port->observed) + 1;
0091 }
0092
0093 static ssize_t usbport_trig_port_store(struct device *dev,
0094                        struct device_attribute *attr,
0095                        const char *buf, size_t size)
0096 {
0097     struct usbport_trig_port *port = container_of(attr,
0098                               struct usbport_trig_port,
0099                               attr);
0100
0101     if (!strcmp(buf, "0") || !strcmp(buf, "0\n"))
0102         port->observed = 0;
0103     else if (!strcmp(buf, "1") || !strcmp(buf, "1\n"))
0104         port->observed = 1;
0105     else
0106         return -EINVAL;
0107
0108     usbport_trig_update_count(port->data);
0109
0110     return size;
0111 }
0112
0113 static struct attribute *ports_attrs[] = {
0114     NULL,
0115 };
0116
0117 static const struct attribute_group ports_group = {
0118     .name = "ports",
0119     .attrs = ports_attrs,
0120 };
0121
0122 /***************************************
0123  * Adding & removing ports
0124  ***************************************/
0125
0126 /*
0127  * usbport_trig_port_observed - Check if port should be observed
0128  */
0129 static bool usbport_trig_port_observed(struct usbport_trig_data *usbport_data,
0130                        struct usb_device *usb_dev, int port1)
0131 {
0132     struct device *dev = usbport_data->led_cdev->dev;
0133     struct device_node *led_np = dev->of_node;
0134     struct of_phandle_args args;
0135     struct device_node *port_np;
0136     int count, i;
0137
0138     if (!led_np)
0139         return false;
0140
0141     /*
0142      * Get node of port being added
0143      *
0144      * FIXME: This is really the device node of the connected device
0145      */
0146     port_np = usb_of_get_device_node(usb_dev, port1);
0147     if (!port_np)
0148         return false;
0149
0150     of_node_put(port_np);
0151
0152     /* Amount of trigger sources for this LED */
0153     count = of_count_phandle_with_args(led_np, "trigger-sources",
0154                        "#trigger-source-cells");
0155     if (count < 0) {
0156         dev_warn(dev, "Failed to get trigger sources for %pOF\n",
0157              led_np);
0158         return false;
0159     }
0160
0161     /* Check list of sources for this specific port */
0162     for (i = 0; i < count; i++) {
0163         int err;
0164
0165         err = of_parse_phandle_with_args(led_np, "trigger-sources",
0166                          "#trigger-source-cells", i,
0167                          &args);
0168         if (err) {
0169             dev_err(dev, "Failed to get trigger source phandle at index %d: %d\n",
0170                 i, err);
0171             continue;
0172         }
0173
0174         of_node_put(args.np);
0175
0176         if (args.np == port_np)
0177             return true;
0178     }
0179
0180     return false;
0181 }
0182
0183 static int usbport_trig_add_port(struct usbport_trig_data *usbport_data,
0184                  struct usb_device *usb_dev,
0185                  const char *hub_name, int portnum)
0186 {
0187     struct led_classdev *led_cdev = usbport_data->led_cdev;
0188     struct usbport_trig_port *port;
0189     size_t len;
0190     int err;
0191
0192     port = kzalloc(sizeof(*port), GFP_KERNEL);
0193     if (!port) {
0194         err = -ENOMEM;
0195         goto err_out;
0196     }
0197
0198     port->data = usbport_data;
0199     port->hub = usb_dev;
0200     port->portnum = portnum;
0201     port->observed = usbport_trig_port_observed(usbport_data, usb_dev,
0202                             portnum);
0203
0204     len = strlen(hub_name) + 8;
0205     port->port_name = kzalloc(len, GFP_KERNEL);
0206     if (!port->port_name) {
0207         err = -ENOMEM;
0208         goto err_free_port;
0209     }
0210     snprintf(port->port_name, len, "%s-port%d", hub_name, portnum);
0211
0212     sysfs_attr_init(&port->attr.attr);
0213     port->attr.attr.name = port->port_name;
0214     port->attr.attr.mode = S_IRUSR | S_IWUSR;
0215     port->attr.show = usbport_trig_port_show;
0216     port->attr.store = usbport_trig_port_store;
0217
0218     err = sysfs_add_file_to_group(&led_cdev->dev->kobj, &port->attr.attr,
0219                       ports_group.name);
0220     if (err)
0221         goto err_free_port_name;
0222
0223     list_add_tail(&port->list, &usbport_data->ports);
0224
0225     return 0;
0226
0227 err_free_port_name:
0228     kfree(port->port_name);
0229 err_free_port:
0230     kfree(port);
0231 err_out:
0232     return err;
0233 }
0234
0235 static int usbport_trig_add_usb_dev_ports(struct usb_device *usb_dev,
0236                       void *data)
0237 {
0238     struct usbport_trig_data *usbport_data = data;
0239     int i;
0240
0241     for (i = 1; i <= usb_dev->maxchild; i++)
0242         usbport_trig_add_port(usbport_data, usb_dev,
0243                       dev_name(&usb_dev->dev), i);
0244
0245     return 0;
0246 }
0247
0248 static void usbport_trig_remove_port(struct usbport_trig_data *usbport_data,
0249                      struct usbport_trig_port *port)
0250 {
0251     struct led_classdev *led_cdev = usbport_data->led_cdev;
0252
0253     list_del(&port->list);
0254     sysfs_remove_file_from_group(&led_cdev->dev->kobj, &port->attr.attr,
0255                      ports_group.name);
0256     kfree(port->port_name);
0257     kfree(port);
0258 }
0259
0260 static void usbport_trig_remove_usb_dev_ports(struct usbport_trig_data *usbport_data,
0261                           struct usb_device *usb_dev)
0262 {
0263     struct usbport_trig_port *port, *tmp;
0264
0265     list_for_each_entry_safe(port, tmp, &usbport_data->ports, list) {
0266         if (port->hub == usb_dev)
0267             usbport_trig_remove_port(usbport_data, port);
0268     }
0269 }
0270
0271 /***************************************
0272  * Init, exit, etc.
0273  ***************************************/
0274
0275 static int usbport_trig_notify(struct notifier_block *nb, unsigned long action,
0276                    void *data)
0277 {
0278     struct usbport_trig_data *usbport_data =
0279         container_of(nb, struct usbport_trig_data, nb);
0280     struct led_classdev *led_cdev = usbport_data->led_cdev;
0281     struct usb_device *usb_dev = data;
0282     bool observed;
0283
0284     observed = usbport_trig_usb_dev_observed(usbport_data, usb_dev);
0285
0286     switch (action) {
0287     case USB_DEVICE_ADD:
0288         usbport_trig_add_usb_dev_ports(usb_dev, usbport_data);
0289         if (observed && usbport_data->count++ == 0)
0290             led_set_brightness(led_cdev, LED_FULL);
0291         return NOTIFY_OK;
0292     case USB_DEVICE_REMOVE:
0293         usbport_trig_remove_usb_dev_ports(usbport_data, usb_dev);
0294         if (observed && --usbport_data->count == 0)
0295             led_set_brightness(led_cdev, LED_OFF);
0296         return NOTIFY_OK;
0297     }
0298
0299     return NOTIFY_DONE;
0300 }
0301
0302 static int usbport_trig_activate(struct led_classdev *led_cdev)
0303 {
0304     struct usbport_trig_data *usbport_data;
0305     int err;
0306
0307     usbport_data = kzalloc(sizeof(*usbport_data), GFP_KERNEL);
0308     if (!usbport_data)
0309         return -ENOMEM;
0310     usbport_data->led_cdev = led_cdev;
0311
0312     /* List of ports */
0313     INIT_LIST_HEAD(&usbport_data->ports);
0314     err = sysfs_create_group(&led_cdev->dev->kobj, &ports_group);
0315     if (err)
0316         goto err_free;
0317     usb_for_each_dev(usbport_data, usbport_trig_add_usb_dev_ports);
0318     usbport_trig_update_count(usbport_data);
0319
0320     /* Notifications */
0321     usbport_data->nb.notifier_call = usbport_trig_notify;
0322     led_set_trigger_data(led_cdev, usbport_data);
0323     usb_register_notify(&usbport_data->nb);
0324     return 0;
0325
0326 err_free:
0327     kfree(usbport_data);
0328     return err;
0329 }
0330
0331 static void usbport_trig_deactivate(struct led_classdev *led_cdev)
0332 {
0333     struct usbport_trig_data *usbport_data = led_get_trigger_data(led_cdev);
0334     struct usbport_trig_port *port, *tmp;
0335
0336     list_for_each_entry_safe(port, tmp, &usbport_data->ports, list) {
0337         usbport_trig_remove_port(usbport_data, port);
0338     }
0339
0340     sysfs_remove_group(&led_cdev->dev->kobj, &ports_group);
0341
0342     usb_unregister_notify(&usbport_data->nb);
0343
0344     kfree(usbport_data);
0345 }
0346
0347 static struct led_trigger usbport_led_trigger = {
0348     .name     = "usbport",
0349     .activate = usbport_trig_activate,
0350     .deactivate = usbport_trig_deactivate,
0351 };
0352
0353 static int __init usbport_trig_init(void)
0354 {
0355     return led_trigger_register(&usbport_led_trigger);
0356 }
0357
0358 static void __exit usbport_trig_exit(void)
0359 {
0360     led_trigger_unregister(&usbport_led_trigger);
0361 }
0362
0363 module_init(usbport_trig_init);
0364 module_exit(usbport_trig_exit);
0365
0366 MODULE_AUTHOR("Rafał Miłecki <rafal@milecki.pl>");
0367 MODULE_DESCRIPTION("USB port trigger");
0368 MODULE_LICENSE("GPL v2");