drivers/usb/core/ledtrig-usbport.c

   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
  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);
  76         led_set_brightness(led_cdev, usbport_data->count ? LED_FULL : LED_OFF);
  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
 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         /*
 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
 182 static 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
 226 err_free_port_name:
 227         kfree(port->port_name);
 228 err_free_port:
 229         kfree(port);
 230 err_out:
 231         return err;
 232 }
 233
 234 static 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
 247 static 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
 259 static 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
 274 static 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
 301 static 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
 327 err_free:
 328         kfree(usbport_data);
 329 }
 330
 331 static 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
 352 static struct led_trigger usbport_led_trigger = {
 353         .name     = "usbport",
 354         .activate = usbport_trig_activate,
 355         .deactivate = usbport_trig_deactivate,
 356 };
 357
 358 static int __init usbport_trig_init(void)
 359 {
 360         return led_trigger_register(&usbport_led_trigger);
 361 }
 362
 363 static void __exit usbport_trig_exit(void)
 364 {
 365         led_trigger_unregister(&usbport_led_trigger);
 366 }
 367
 368 module_init(usbport_trig_init);
 369 module_exit(usbport_trig_exit);
 370
 371 MODULE_AUTHOR("Rafał Miłecki <rafal@milecki.pl>");
 372 MODULE_DESCRIPTION("USB port trigger");
 373 MODULE_LICENSE("GPL v2");