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