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