ledtrig-usbport.c 8.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368
  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. #include <linux/device.h>
  8. #include <linux/leds.h>
  9. #include <linux/module.h>
  10. #include <linux/of.h>
  11. #include <linux/slab.h>
  12. #include <linux/usb.h>
  13. #include <linux/usb/of.h>
  14. struct usbport_trig_data {
  15. struct led_classdev *led_cdev;
  16. struct list_head ports;
  17. struct notifier_block nb;
  18. int count; /* Amount of connected matching devices */
  19. };
  20. struct usbport_trig_port {
  21. struct usbport_trig_data *data;
  22. struct usb_device *hub;
  23. int portnum;
  24. char *port_name;
  25. bool observed;
  26. struct device_attribute attr;
  27. struct list_head list;
  28. };
  29. /***************************************
  30. * Helpers
  31. ***************************************/
  32. /*
  33. * usbport_trig_usb_dev_observed - Check if dev is connected to observed port
  34. */
  35. static bool usbport_trig_usb_dev_observed(struct usbport_trig_data *usbport_data,
  36. struct usb_device *usb_dev)
  37. {
  38. struct usbport_trig_port *port;
  39. if (!usb_dev->parent)
  40. return false;
  41. list_for_each_entry(port, &usbport_data->ports, list) {
  42. if (usb_dev->parent == port->hub &&
  43. usb_dev->portnum == port->portnum)
  44. return port->observed;
  45. }
  46. return false;
  47. }
  48. static int usbport_trig_usb_dev_check(struct usb_device *usb_dev, void *data)
  49. {
  50. struct usbport_trig_data *usbport_data = data;
  51. if (usbport_trig_usb_dev_observed(usbport_data, usb_dev))
  52. usbport_data->count++;
  53. return 0;
  54. }
  55. /*
  56. * usbport_trig_update_count - Recalculate amount of connected matching devices
  57. */
  58. static void usbport_trig_update_count(struct usbport_trig_data *usbport_data)
  59. {
  60. struct led_classdev *led_cdev = usbport_data->led_cdev;
  61. usbport_data->count = 0;
  62. usb_for_each_dev(usbport_data, usbport_trig_usb_dev_check);
  63. led_set_brightness(led_cdev, usbport_data->count ? LED_FULL : LED_OFF);
  64. }
  65. /***************************************
  66. * Device attr
  67. ***************************************/
  68. static ssize_t usbport_trig_port_show(struct device *dev,
  69. struct device_attribute *attr, char *buf)
  70. {
  71. struct usbport_trig_port *port = container_of(attr,
  72. struct usbport_trig_port,
  73. attr);
  74. return sprintf(buf, "%d\n", port->observed) + 1;
  75. }
  76. static ssize_t usbport_trig_port_store(struct device *dev,
  77. struct device_attribute *attr,
  78. const char *buf, size_t size)
  79. {
  80. struct usbport_trig_port *port = container_of(attr,
  81. struct usbport_trig_port,
  82. attr);
  83. if (!strcmp(buf, "0") || !strcmp(buf, "0\n"))
  84. port->observed = 0;
  85. else if (!strcmp(buf, "1") || !strcmp(buf, "1\n"))
  86. port->observed = 1;
  87. else
  88. return -EINVAL;
  89. usbport_trig_update_count(port->data);
  90. return size;
  91. }
  92. static struct attribute *ports_attrs[] = {
  93. NULL,
  94. };
  95. static const struct attribute_group ports_group = {
  96. .name = "ports",
  97. .attrs = ports_attrs,
  98. };
  99. /***************************************
  100. * Adding & removing ports
  101. ***************************************/
  102. /*
  103. * usbport_trig_port_observed - Check if port should be observed
  104. */
  105. static bool usbport_trig_port_observed(struct usbport_trig_data *usbport_data,
  106. struct usb_device *usb_dev, int port1)
  107. {
  108. struct device *dev = usbport_data->led_cdev->dev;
  109. struct device_node *led_np = dev->of_node;
  110. struct of_phandle_args args;
  111. struct device_node *port_np;
  112. int count, i;
  113. if (!led_np)
  114. return false;
  115. /*
  116. * Get node of port being added
  117. *
  118. * FIXME: This is really the device node of the connected device
  119. */
  120. port_np = usb_of_get_device_node(usb_dev, port1);
  121. if (!port_np)
  122. return false;
  123. of_node_put(port_np);
  124. /* Amount of trigger sources for this LED */
  125. count = of_count_phandle_with_args(led_np, "trigger-sources",
  126. "#trigger-source-cells");
  127. if (count < 0) {
  128. dev_warn(dev, "Failed to get trigger sources for %pOF\n",
  129. led_np);
  130. return false;
  131. }
  132. /* Check list of sources for this specific port */
  133. for (i = 0; i < count; i++) {
  134. int err;
  135. err = of_parse_phandle_with_args(led_np, "trigger-sources",
  136. "#trigger-source-cells", i,
  137. &args);
  138. if (err) {
  139. dev_err(dev, "Failed to get trigger source phandle at index %d: %d\n",
  140. i, err);
  141. continue;
  142. }
  143. of_node_put(args.np);
  144. if (args.np == port_np)
  145. return true;
  146. }
  147. return false;
  148. }
  149. static int usbport_trig_add_port(struct usbport_trig_data *usbport_data,
  150. struct usb_device *usb_dev,
  151. const char *hub_name, int portnum)
  152. {
  153. struct led_classdev *led_cdev = usbport_data->led_cdev;
  154. struct usbport_trig_port *port;
  155. size_t len;
  156. int err;
  157. port = kzalloc(sizeof(*port), GFP_KERNEL);
  158. if (!port) {
  159. err = -ENOMEM;
  160. goto err_out;
  161. }
  162. port->data = usbport_data;
  163. port->hub = usb_dev;
  164. port->portnum = portnum;
  165. port->observed = usbport_trig_port_observed(usbport_data, usb_dev,
  166. portnum);
  167. len = strlen(hub_name) + 8;
  168. port->port_name = kzalloc(len, GFP_KERNEL);
  169. if (!port->port_name) {
  170. err = -ENOMEM;
  171. goto err_free_port;
  172. }
  173. snprintf(port->port_name, len, "%s-port%d", hub_name, portnum);
  174. sysfs_attr_init(&port->attr.attr);
  175. port->attr.attr.name = port->port_name;
  176. port->attr.attr.mode = S_IRUSR | S_IWUSR;
  177. port->attr.show = usbport_trig_port_show;
  178. port->attr.store = usbport_trig_port_store;
  179. err = sysfs_add_file_to_group(&led_cdev->dev->kobj, &port->attr.attr,
  180. ports_group.name);
  181. if (err)
  182. goto err_free_port_name;
  183. list_add_tail(&port->list, &usbport_data->ports);
  184. return 0;
  185. err_free_port_name:
  186. kfree(port->port_name);
  187. err_free_port:
  188. kfree(port);
  189. err_out:
  190. return err;
  191. }
  192. static int usbport_trig_add_usb_dev_ports(struct usb_device *usb_dev,
  193. void *data)
  194. {
  195. struct usbport_trig_data *usbport_data = data;
  196. int i;
  197. for (i = 1; i <= usb_dev->maxchild; i++)
  198. usbport_trig_add_port(usbport_data, usb_dev,
  199. dev_name(&usb_dev->dev), i);
  200. return 0;
  201. }
  202. static void usbport_trig_remove_port(struct usbport_trig_data *usbport_data,
  203. struct usbport_trig_port *port)
  204. {
  205. struct led_classdev *led_cdev = usbport_data->led_cdev;
  206. list_del(&port->list);
  207. sysfs_remove_file_from_group(&led_cdev->dev->kobj, &port->attr.attr,
  208. ports_group.name);
  209. kfree(port->port_name);
  210. kfree(port);
  211. }
  212. static void usbport_trig_remove_usb_dev_ports(struct usbport_trig_data *usbport_data,
  213. struct usb_device *usb_dev)
  214. {
  215. struct usbport_trig_port *port, *tmp;
  216. list_for_each_entry_safe(port, tmp, &usbport_data->ports, list) {
  217. if (port->hub == usb_dev)
  218. usbport_trig_remove_port(usbport_data, port);
  219. }
  220. }
  221. /***************************************
  222. * Init, exit, etc.
  223. ***************************************/
  224. static int usbport_trig_notify(struct notifier_block *nb, unsigned long action,
  225. void *data)
  226. {
  227. struct usbport_trig_data *usbport_data =
  228. container_of(nb, struct usbport_trig_data, nb);
  229. struct led_classdev *led_cdev = usbport_data->led_cdev;
  230. struct usb_device *usb_dev = data;
  231. bool observed;
  232. observed = usbport_trig_usb_dev_observed(usbport_data, usb_dev);
  233. switch (action) {
  234. case USB_DEVICE_ADD:
  235. usbport_trig_add_usb_dev_ports(usb_dev, usbport_data);
  236. if (observed && usbport_data->count++ == 0)
  237. led_set_brightness(led_cdev, LED_FULL);
  238. return NOTIFY_OK;
  239. case USB_DEVICE_REMOVE:
  240. usbport_trig_remove_usb_dev_ports(usbport_data, usb_dev);
  241. if (observed && --usbport_data->count == 0)
  242. led_set_brightness(led_cdev, LED_OFF);
  243. return NOTIFY_OK;
  244. }
  245. return NOTIFY_DONE;
  246. }
  247. static int usbport_trig_activate(struct led_classdev *led_cdev)
  248. {
  249. struct usbport_trig_data *usbport_data;
  250. int err;
  251. usbport_data = kzalloc(sizeof(*usbport_data), GFP_KERNEL);
  252. if (!usbport_data)
  253. return -ENOMEM;
  254. usbport_data->led_cdev = led_cdev;
  255. /* List of ports */
  256. INIT_LIST_HEAD(&usbport_data->ports);
  257. err = sysfs_create_group(&led_cdev->dev->kobj, &ports_group);
  258. if (err)
  259. goto err_free;
  260. usb_for_each_dev(usbport_data, usbport_trig_add_usb_dev_ports);
  261. usbport_trig_update_count(usbport_data);
  262. /* Notifications */
  263. usbport_data->nb.notifier_call = usbport_trig_notify;
  264. led_set_trigger_data(led_cdev, usbport_data);
  265. usb_register_notify(&usbport_data->nb);
  266. return 0;
  267. err_free:
  268. kfree(usbport_data);
  269. return err;
  270. }
  271. static void usbport_trig_deactivate(struct led_classdev *led_cdev)
  272. {
  273. struct usbport_trig_data *usbport_data = led_get_trigger_data(led_cdev);
  274. struct usbport_trig_port *port, *tmp;
  275. list_for_each_entry_safe(port, tmp, &usbport_data->ports, list) {
  276. usbport_trig_remove_port(usbport_data, port);
  277. }
  278. sysfs_remove_group(&led_cdev->dev->kobj, &ports_group);
  279. usb_unregister_notify(&usbport_data->nb);
  280. kfree(usbport_data);
  281. }
  282. static struct led_trigger usbport_led_trigger = {
  283. .name = "usbport",
  284. .activate = usbport_trig_activate,
  285. .deactivate = usbport_trig_deactivate,
  286. };
  287. static int __init usbport_trig_init(void)
  288. {
  289. return led_trigger_register(&usbport_led_trigger);
  290. }
  291. static void __exit usbport_trig_exit(void)
  292. {
  293. led_trigger_unregister(&usbport_led_trigger);
  294. }
  295. module_init(usbport_trig_init);
  296. module_exit(usbport_trig_exit);
  297. MODULE_AUTHOR("Rafał Miłecki <rafal@milecki.pl>");
  298. MODULE_DESCRIPTION("USB port trigger");
  299. MODULE_LICENSE("GPL v2");