drivers/usb/core/ledtrig-usbport.c

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