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/sysfs.h>
  14 #include <linux/usb.h>
  15 #include <linux/usb/of.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_set_brightness(led_cdev, usbport_data->count ? LED_FULL : LED_OFF);
  78 }
  79
  80 /***************************************
  81  * Device attr
  82  ***************************************/
  83
  84 static ssize_t usbport_trig_port_show(struct device *dev,
  85                                       struct device_attribute *attr, char *buf)
  86 {
  87         struct usbport_trig_port *port = container_of(attr,
  88                                                       struct usbport_trig_port,
  89                                                       attr);
  90
  91         return sysfs_emit(buf, "%d\n", port->observed) + 1;
  92 }
  93
  94 static ssize_t usbport_trig_port_store(struct device *dev,
  95                                        struct device_attribute *attr,
  96                                        const char *buf, size_t size)
  97 {
  98         struct usbport_trig_port *port = container_of(attr,
  99                                                       struct usbport_trig_port,
 100                                                       attr);
 101
 102         if (!strcmp(buf, "0") || !strcmp(buf, "0\n"))
 103                 port->observed = 0;
 104         else if (!strcmp(buf, "1") || !strcmp(buf, "1\n"))
 105                 port->observed = 1;
 106         else
 107                 return -EINVAL;
 108
 109         usbport_trig_update_count(port->data);
 110
 111         return size;
 112 }
 113
 114 static struct attribute *ports_attrs[] = {
 115         NULL,
 116 };
 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 /*
 128  * usbport_trig_port_observed - Check if port should be observed
 129  */
 130 static bool usbport_trig_port_observed(struct usbport_trig_data *usbport_data,
 131                                        struct usb_device *usb_dev, int port1)
 132 {
 133         struct device *dev = usbport_data->led_cdev->dev;
 134         struct device_node *led_np = dev->of_node;
 135         struct of_phandle_args args;
 136         struct device_node *port_np;
 137         int count, i;
 138
 139         if (!led_np)
 140                 return false;
 141
 142         /*
 143          * Get node of port being added
 144          *
 145          * FIXME: This is really the device node of the connected device
 146          */
 147         port_np = usb_of_get_device_node(usb_dev, port1);
 148         if (!port_np)
 149                 return false;
 150
 151         of_node_put(port_np);
 152
 153         /* Amount of trigger sources for this LED */
 154         count = of_count_phandle_with_args(led_np, "trigger-sources",
 155                                            "#trigger-source-cells");
 156         if (count < 0) {
 157                 dev_warn(dev, "Failed to get trigger sources for %pOF\n",
 158                          led_np);
 159                 return false;
 160         }
 161
 162         /* Check list of sources for this specific port */
 163         for (i = 0; i < count; i++) {
 164                 int err;
 165
 166                 err = of_parse_phandle_with_args(led_np, "trigger-sources",
 167                                                  "#trigger-source-cells", i,
 168                                                  &args);
 169                 if (err) {
 170                         dev_err(dev, "Failed to get trigger source phandle at index %d: %d\n",
 171                                 i, err);
 172                         continue;
 173                 }
 174
 175                 of_node_put(args.np);
 176
 177                 if (args.np == port_np)
 178                         return true;
 179         }
 180
 181         return false;
 182 }
 183
 184 static int usbport_trig_add_port(struct usbport_trig_data *usbport_data,
 185                                  struct usb_device *usb_dev,
 186                                  const char *hub_name, int portnum)
 187 {
 188         struct led_classdev *led_cdev = usbport_data->led_cdev;
 189         struct usbport_trig_port *port;
 190         size_t len;
 191         int err;
 192
 193         port = kzalloc(sizeof(*port), GFP_KERNEL);
 194         if (!port) {
 195                 err = -ENOMEM;
 196                 goto err_out;
 197         }
 198
 199         port->data = usbport_data;
 200         port->hub = usb_dev;
 201         port->portnum = portnum;
 202         port->observed = usbport_trig_port_observed(usbport_data, usb_dev,
 203                                                     portnum);
 204
 205         len = strlen(hub_name) + 8;
 206         port->port_name = kzalloc(len, GFP_KERNEL);
 207         if (!port->port_name) {
 208                 err = -ENOMEM;
 209                 goto err_free_port;
 210         }
 211         snprintf(port->port_name, len, "%s-port%d", hub_name, portnum);
 212
 213         sysfs_attr_init(&port->attr.attr);
 214         port->attr.attr.name = port->port_name;
 215         port->attr.attr.mode = S_IRUSR | S_IWUSR;
 216         port->attr.show = usbport_trig_port_show;
 217         port->attr.store = usbport_trig_port_store;
 218
 219         err = sysfs_add_file_to_group(&led_cdev->dev->kobj, &port->attr.attr,
 220                                       ports_group.name);
 221         if (err)
 222                 goto err_free_port_name;
 223
 224         list_add_tail(&port->list, &usbport_data->ports);
 225
 226         return 0;
 227
 228 err_free_port_name:
 229         kfree(port->port_name);
 230 err_free_port:
 231         kfree(port);
 232 err_out:
 233         return err;
 234 }
 235
 236 static int usbport_trig_add_usb_dev_ports(struct usb_device *usb_dev,
 237                                           void *data)
 238 {
 239         struct usbport_trig_data *usbport_data = data;
 240         int i;
 241
 242         for (i = 1; i <= usb_dev->maxchild; i++)
 243                 usbport_trig_add_port(usbport_data, usb_dev,
 244                                       dev_name(&usb_dev->dev), i);
 245
 246         return 0;
 247 }
 248
 249 static void usbport_trig_remove_port(struct usbport_trig_data *usbport_data,
 250                                      struct usbport_trig_port *port)
 251 {
 252         struct led_classdev *led_cdev = usbport_data->led_cdev;
 253
 254         list_del(&port->list);
 255         sysfs_remove_file_from_group(&led_cdev->dev->kobj, &port->attr.attr,
 256                                      ports_group.name);
 257         kfree(port->port_name);
 258         kfree(port);
 259 }
 260
 261 static void usbport_trig_remove_usb_dev_ports(struct usbport_trig_data *usbport_data,
 262                                               struct usb_device *usb_dev)
 263 {
 264         struct usbport_trig_port *port, *tmp;
 265
 266         list_for_each_entry_safe(port, tmp, &usbport_data->ports, list) {
 267                 if (port->hub == usb_dev)
 268                         usbport_trig_remove_port(usbport_data, port);
 269         }
 270 }
 271
 272 /***************************************
 273  * Init, exit, etc.
 274  ***************************************/
 275
 276 static int usbport_trig_notify(struct notifier_block *nb, unsigned long action,
 277                                void *data)
 278 {
 279         struct usbport_trig_data *usbport_data =
 280                 container_of(nb, struct usbport_trig_data, nb);
 281         struct led_classdev *led_cdev = usbport_data->led_cdev;
 282         struct usb_device *usb_dev = data;
 283         bool observed;
 284
 285         observed = usbport_trig_usb_dev_observed(usbport_data, usb_dev);
 286
 287         switch (action) {
 288         case USB_DEVICE_ADD:
 289                 usbport_trig_add_usb_dev_ports(usb_dev, usbport_data);
 290                 if (observed && usbport_data->count++ == 0)
 291                         led_set_brightness(led_cdev, LED_FULL);
 292                 return NOTIFY_OK;
 293         case USB_DEVICE_REMOVE:
 294                 usbport_trig_remove_usb_dev_ports(usbport_data, usb_dev);
 295                 if (observed && --usbport_data->count == 0)
 296                         led_set_brightness(led_cdev, LED_OFF);
 297                 return NOTIFY_OK;
 298         }
 299
 300         return NOTIFY_DONE;
 301 }
 302
 303 static int usbport_trig_activate(struct led_classdev *led_cdev)
 304 {
 305         struct usbport_trig_data *usbport_data;
 306         int err;
 307
 308         usbport_data = kzalloc(sizeof(*usbport_data), GFP_KERNEL);
 309         if (!usbport_data)
 310                 return -ENOMEM;
 311         usbport_data->led_cdev = led_cdev;
 312
 313         /* List of ports */
 314         INIT_LIST_HEAD(&usbport_data->ports);
 315         err = sysfs_create_group(&led_cdev->dev->kobj, &ports_group);
 316         if (err)
 317                 goto err_free;
 318         usb_for_each_dev(usbport_data, usbport_trig_add_usb_dev_ports);
 319         usbport_trig_update_count(usbport_data);
 320
 321         /* Notifications */
 322         usbport_data->nb.notifier_call = usbport_trig_notify;
 323         led_set_trigger_data(led_cdev, usbport_data);
 324         usb_register_notify(&usbport_data->nb);
 325         return 0;
 326
 327 err_free:
 328         kfree(usbport_data);
 329         return err;
 330 }
 331
 332 static void usbport_trig_deactivate(struct led_classdev *led_cdev)
 333 {
 334         struct usbport_trig_data *usbport_data = led_get_trigger_data(led_cdev);
 335         struct usbport_trig_port *port, *tmp;
 336
 337         list_for_each_entry_safe(port, tmp, &usbport_data->ports, list) {
 338                 usbport_trig_remove_port(usbport_data, port);
 339         }
 340
 341         sysfs_remove_group(&led_cdev->dev->kobj, &ports_group);
 342
 343         usb_unregister_notify(&usbport_data->nb);
 344
 345         kfree(usbport_data);
 346 }
 347
 348 static struct led_trigger usbport_led_trigger = {
 349         .name     = "usbport",
 350         .activate = usbport_trig_activate,
 351         .deactivate = usbport_trig_deactivate,
 352 };
 353
 354 module_led_trigger(usbport_led_trigger);
 355
 356 MODULE_AUTHOR("Rafał Miłecki <rafal@milecki.pl>");
 357 MODULE_DESCRIPTION("USB port trigger");
 358 MODULE_LICENSE("GPL v2");