drivers/usb/core/ledtrig-usbport.c

   1 /*
   2  * USB port LED trigger
   3  *
   4  * Copyright (C) 2016 Rafał Miłecki <[email protected]>
   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/slab.h>
  15 #include <linux/usb.h>
  16
  17 struct 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
  24 struct 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  */
  41 static 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
  58 static 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  */
  71 static 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_cdev->brightness_set(led_cdev,
  78                                  usbport_data->count ? LED_FULL : LED_OFF);
  79 }
  80
  81 /***************************************
  82  * Device attr
  83  ***************************************/
  84
  85 static ssize_t usbport_trig_port_show(struct device *dev,
  86                                       struct device_attribute *attr, char *buf)
  87 {
  88         struct usbport_trig_port *port = container_of(attr,
  89                                                       struct usbport_trig_port,
  90                                                       attr);
  91
  92         return sprintf(buf, "%d\n", port->observed) + 1;
  93 }
  94
  95 static ssize_t usbport_trig_port_store(struct device *dev,
  96                                        struct device_attribute *attr,
  97                                        const char *buf, size_t size)
  98 {
  99         struct usbport_trig_port *port = container_of(attr,
 100                                                       struct usbport_trig_port,
 101                                                       attr);
 102
 103         if (!strcmp(buf, "0") || !strcmp(buf, "0\n"))
 104                 port->observed = 0;
 105         else if (!strcmp(buf, "1") || !strcmp(buf, "1\n"))
 106                 port->observed = 1;
 107         else
 108                 return -EINVAL;
 109
 110         usbport_trig_update_count(port->data);
 111
 112         return size;
 113 }
 114
 115 static struct attribute *ports_attrs[] = {
 116         NULL,
 117 };
 118 static const struct attribute_group ports_group = {
 119         .name = "ports",
 120         .attrs = ports_attrs,
 121 };
 122
 123 /***************************************
 124  * Adding & removing ports
 125  ***************************************/
 126
 127 static int usbport_trig_add_port(struct usbport_trig_data *usbport_data,
 128                                  struct usb_device *usb_dev,
 129                                  const char *hub_name, int portnum)
 130 {
 131         struct led_classdev *led_cdev = usbport_data->led_cdev;
 132         struct usbport_trig_port *port;
 133         size_t len;
 134         int err;
 135
 136         port = kzalloc(sizeof(*port), GFP_KERNEL);
 137         if (!port) {
 138                 err = -ENOMEM;
 139                 goto err_out;
 140         }
 141
 142         port->data = usbport_data;
 143         port->hub = usb_dev;
 144         port->portnum = portnum;
 145
 146         len = strlen(hub_name) + 8;
 147         port->port_name = kzalloc(len, GFP_KERNEL);
 148         if (!port->port_name) {
 149                 err = -ENOMEM;
 150                 goto err_free_port;
 151         }
 152         snprintf(port->port_name, len, "%s-port%d", hub_name, portnum);
 153
 154         port->attr.attr.name = port->port_name;
 155         port->attr.attr.mode = S_IRUSR | S_IWUSR;
 156         port->attr.show = usbport_trig_port_show;
 157         port->attr.store = usbport_trig_port_store;
 158
 159         err = sysfs_add_file_to_group(&led_cdev->dev->kobj, &port->attr.attr,
 160                                       ports_group.name);
 161         if (err)
 162                 goto err_free_port_name;
 163
 164         list_add_tail(&port->list, &usbport_data->ports);
 165
 166         return 0;
 167
 168 err_free_port_name:
 169         kfree(port->port_name);
 170 err_free_port:
 171         kfree(port);
 172 err_out:
 173         return err;
 174 }
 175
 176 static int usbport_trig_add_usb_dev_ports(struct usb_device *usb_dev,
 177                                           void *data)
 178 {
 179         struct usbport_trig_data *usbport_data = data;
 180         int i;
 181
 182         for (i = 1; i <= usb_dev->maxchild; i++)
 183                 usbport_trig_add_port(usbport_data, usb_dev,
 184                                       dev_name(&usb_dev->dev), i);
 185
 186         return 0;
 187 }
 188
 189 static void usbport_trig_remove_port(struct usbport_trig_data *usbport_data,
 190                                      struct usbport_trig_port *port)
 191 {
 192         struct led_classdev *led_cdev = usbport_data->led_cdev;
 193
 194         list_del(&port->list);
 195         sysfs_remove_file_from_group(&led_cdev->dev->kobj, &port->attr.attr,
 196                                      ports_group.name);
 197         kfree(port->port_name);
 198         kfree(port);
 199 }
 200
 201 static void usbport_trig_remove_usb_dev_ports(struct usbport_trig_data *usbport_data,
 202                                               struct usb_device *usb_dev)
 203 {
 204         struct usbport_trig_port *port, *tmp;
 205
 206         list_for_each_entry_safe(port, tmp, &usbport_data->ports, list) {
 207                 if (port->hub == usb_dev)
 208                         usbport_trig_remove_port(usbport_data, port);
 209         }
 210 }
 211
 212 /***************************************
 213  * Init, exit, etc.
 214  ***************************************/
 215
 216 static int usbport_trig_notify(struct notifier_block *nb, unsigned long action,
 217                                void *data)
 218 {
 219         struct usbport_trig_data *usbport_data =
 220                 container_of(nb, struct usbport_trig_data, nb);
 221         struct led_classdev *led_cdev = usbport_data->led_cdev;
 222         struct usb_device *usb_dev = data;
 223         bool observed;
 224
 225         observed = usbport_trig_usb_dev_observed(usbport_data, usb_dev);
 226
 227         switch (action) {
 228         case USB_DEVICE_ADD:
 229                 usbport_trig_add_usb_dev_ports(usb_dev, usbport_data);
 230                 if (observed && usbport_data->count++ == 0)
 231                         led_cdev->brightness_set(led_cdev, LED_FULL);
 232                 return NOTIFY_OK;
 233         case USB_DEVICE_REMOVE:
 234                 usbport_trig_remove_usb_dev_ports(usbport_data, usb_dev);
 235                 if (observed && --usbport_data->count == 0)
 236                         led_cdev->brightness_set(led_cdev, LED_OFF);
 237                 return NOTIFY_OK;
 238         }
 239
 240         return NOTIFY_DONE;
 241 }
 242
 243 static void usbport_trig_activate(struct led_classdev *led_cdev)
 244 {
 245         struct usbport_trig_data *usbport_data;
 246         int err;
 247
 248         usbport_data = kzalloc(sizeof(*usbport_data), GFP_KERNEL);
 249         if (!usbport_data)
 250                 return;
 251         usbport_data->led_cdev = led_cdev;
 252
 253         /* List of ports */
 254         INIT_LIST_HEAD(&usbport_data->ports);
 255         err = sysfs_create_group(&led_cdev->dev->kobj, &ports_group);
 256         if (err)
 257                 goto err_free;
 258         usb_for_each_dev(usbport_data, usbport_trig_add_usb_dev_ports);
 259
 260         /* Notifications */
 261         usbport_data->nb.notifier_call = usbport_trig_notify,
 262         led_cdev->trigger_data = usbport_data;
 263         usb_register_notify(&usbport_data->nb);
 264
 265         led_cdev->activated = true;
 266         return;
 267
 268 err_free:
 269         kfree(usbport_data);
 270 }
 271
 272 static void usbport_trig_deactivate(struct led_classdev *led_cdev)
 273 {
 274         struct usbport_trig_data *usbport_data = led_cdev->trigger_data;
 275         struct usbport_trig_port *port, *tmp;
 276
 277         if (!led_cdev->activated)
 278                 return;
 279
 280         list_for_each_entry_safe(port, tmp, &usbport_data->ports, list) {
 281                 usbport_trig_remove_port(usbport_data, port);
 282         }
 283
 284         usb_unregister_notify(&usbport_data->nb);
 285
 286         sysfs_remove_group(&led_cdev->dev->kobj, &ports_group);
 287
 288         kfree(usbport_data);
 289
 290         led_cdev->activated = false;
 291 }
 292
 293 static struct led_trigger usbport_led_trigger = {
 294         .name     = "usbport",
 295         .activate = usbport_trig_activate,
 296         .deactivate = usbport_trig_deactivate,
 297 };
 298
 299 static int __init usbport_trig_init(void)
 300 {
 301         return led_trigger_register(&usbport_led_trigger);
 302 }
 303
 304 static void __exit usbport_trig_exit(void)
 305 {
 306         led_trigger_unregister(&usbport_led_trigger);
 307 }
 308
 309 module_init(usbport_trig_init);
 310 module_exit(usbport_trig_exit);
 311
 312 MODULE_AUTHOR("Rafał Miłecki <[email protected]>");
 313 MODULE_DESCRIPTION("USB port trigger");
 314 MODULE_LICENSE("GPL v2");