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