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"); 359