From 380e7f889018d5faaca4eebc2c615ef92c27ac2b Mon Sep 17 00:00:00 2001 From: illiliti Date: Fri, 14 Aug 2020 19:33:18 +0300 Subject: [PATCH] implement hotplugging support --- udev.h | 3 + udev_device.c | 99 +++++++++++++++++++----- udev_monitor.c | 204 ++++++++++++++++++++++++++++++++++++++++++++++--- 3 files changed, 278 insertions(+), 28 deletions(-) diff --git a/udev.h b/udev.h index b8b2454..08188ea 100644 --- a/udev.h +++ b/udev.h @@ -96,6 +96,9 @@ struct udev_hwdb *udev_hwdb_ref(struct udev_hwdb *hwdb); struct udev_hwdb *udev_hwdb_unref(struct udev_hwdb *hwdb); struct udev_list_entry *udev_hwdb_get_properties_list_entry(struct udev_hwdb *hwdb, const char *modalias, unsigned int flags); +// this is "libudev-zero" extension. do not use if portability is concern +struct udev_device *udev_device_new_from_file(struct udev *udev, const char *path); + #ifdef __cplusplus } #endif diff --git a/udev_device.c b/udev_device.c index e059ad3..a897cb3 100644 --- a/udev_device.c +++ b/udev_device.c @@ -9,7 +9,7 @@ #include "udev.h" #include "udev_list.h" -enum { BITS_SIZE = 96 }; +enum { BITS_MAX = 96 }; struct udev_device { struct udev_list_entry properties; @@ -148,7 +148,7 @@ int udev_device_get_is_initialized(struct udev_device *udev_device) const char *udev_device_get_action(struct udev_device *udev_device) { - return NULL; + return udev_device_get_property_value(udev_device, "ACTION"); } int udev_device_has_tag(struct udev_device *udev_device, const char *tag) @@ -328,7 +328,7 @@ static int populate_bit(unsigned long *arr, const char *val) bit = strtok_r(bits, " ", &save); - for (i = 0; bit && i < BITS_SIZE; i++) { + for (i = 0; bit && i < BITS_MAX; i++) { arr[i] = strtoul(bit, NULL, 16); bit = strtok_r(NULL, " ", &save); } @@ -357,12 +357,12 @@ static int find_bit(unsigned long *arr, int cnt, int bit) static void udev_device_set_properties_from_evdev(struct udev_device *udev_device) { int ev_cnt, rel_cnt, key_cnt, abs_cnt, prop_cnt; - unsigned long prop_bits[BITS_SIZE] = {0}; - unsigned long abs_bits[BITS_SIZE] = {0}; - unsigned long rel_bits[BITS_SIZE] = {0}; - unsigned long key_bits[BITS_SIZE] = {0}; - unsigned long ev_bits[BITS_SIZE] = {0}; - struct udev_device *parent; + unsigned long prop_bits[BITS_MAX] = {0}; + unsigned long abs_bits[BITS_MAX] = {0}; + unsigned long rel_bits[BITS_MAX] = {0}; + unsigned long key_bits[BITS_MAX] = {0}; + unsigned long ev_bits[BITS_MAX] = {0}; + struct udev_device *tmp; const char *subsystem; subsystem = udev_device_get_subsystem(udev_device); @@ -371,25 +371,25 @@ static void udev_device_set_properties_from_evdev(struct udev_device *udev_devic return; } - parent = udev_device_get_parent_with_subsystem_devtype(udev_device, "input", NULL); + tmp = udev_device; while (1) { - if (!parent) { + if (!tmp) { return; } - if (udev_device_get_property_value(parent, "EV")) { + if (udev_device_get_property_value(tmp, "EV")) { break; } - parent = udev_device_get_parent_with_subsystem_devtype(parent, "input", NULL); + tmp = udev_device_get_parent_with_subsystem_devtype(tmp, "input", NULL); } - ev_cnt = populate_bit(ev_bits, udev_device_get_property_value(parent, "EV")); - abs_cnt = populate_bit(abs_bits, udev_device_get_property_value(parent, "ABS")); - rel_cnt = populate_bit(rel_bits, udev_device_get_property_value(parent, "REL")); - key_cnt = populate_bit(key_bits, udev_device_get_property_value(parent, "KEY")); - prop_cnt = populate_bit(prop_bits, udev_device_get_property_value(parent, "PROP")); + ev_cnt = populate_bit(ev_bits, udev_device_get_property_value(tmp, "EV")); + abs_cnt = populate_bit(abs_bits, udev_device_get_property_value(tmp, "ABS")); + rel_cnt = populate_bit(rel_bits, udev_device_get_property_value(tmp, "REL")); + key_cnt = populate_bit(key_bits, udev_device_get_property_value(tmp, "KEY")); + prop_cnt = populate_bit(prop_bits, udev_device_get_property_value(tmp, "PROP")); udev_list_entry_add(&udev_device->properties, "ID_INPUT", "1", 0); @@ -548,6 +548,69 @@ struct udev_device *udev_device_new_from_subsystem_sysname(struct udev *udev, co return NULL; } +struct udev_device *udev_device_new_from_file(struct udev *udev, const char *path) +{ + char line[LINE_MAX], tmp[PATH_MAX]; + struct udev_device *udev_device; + char *key, *val, *sysname; + FILE *file; + int i; + + udev_device = calloc(1, sizeof(struct udev_device)); + + if (!udev_device) { + return NULL; + } + + udev_device->udev = udev; + udev_device->refcount = 1; + udev_device->parent = NULL; + + udev_list_entry_init(&udev_device->properties); + udev_list_entry_init(&udev_device->sysattrs); + + file = fopen(path, "r"); + + if (!file) { + return NULL; + } + + while (fgets(line, sizeof(line), file)) { + line[strcspn(line, "\n")] = '\0'; + + if (strncmp(line, "DEVPATH", 7) == 0) { + snprintf(tmp, sizeof(tmp), "/sys%s", line + 8); + udev_list_entry_add(&udev_device->properties, "SYSPATH", tmp, 0); + udev_list_entry_add(&udev_device->properties, "DEVPATH", line + 8, 0); + } + else if (strncmp(line, "DEVNAME", 7) == 0) { + snprintf(tmp, sizeof(tmp), "/dev/%s", line + 8); + udev_list_entry_add(&udev_device->properties, "DEVNAME", tmp, 0); + } + else if (strchr(line, '=')) { + val = strchr(line, '=') + 1; + key = line; + key[strcspn(key, "=")] = '\0'; + + udev_list_entry_add(&udev_device->properties, key, val, 1); + } + } + + fclose(file); + sysname = strrchr(udev_device_get_syspath(udev_device), '/'); + + for (i = 0; sysname[i] != '\0'; i++) { + if (sysname[i] >= '0' && sysname[i] <= '9') { + udev_list_entry_add(&udev_device->properties, "SYSNUM", sysname + i, 0); + break; + } + } + + udev_list_entry_add(&udev_device->properties, "SYSNAME", sysname, 0); + udev_device_set_properties_from_evdev(udev_device); + return udev_device; +} + struct udev_device *udev_device_new_from_device_id(struct udev *udev, const char *id) { // XXX NOT IMPLEMENTED diff --git a/udev_monitor.c b/udev_monitor.c index 1e88b70..3749817 100644 --- a/udev_monitor.c +++ b/udev_monitor.c @@ -1,33 +1,128 @@ -#include +#include +#include +#include #include #include +#include +#include +#include +#include +#include #include "udev.h" +#include "udev_list.h" + +#ifndef UDEV_MONITOR_DIR +#define UDEV_MONITOR_DIR "/tmp/.libudev-zero" +#endif + +enum { THREADS_MAX = 5 }; struct udev_monitor { + struct udev_list_entry subsystem_match; + struct udev_list_entry devtype_match; + pthread_t thread[THREADS_MAX]; struct udev *udev; int refcount; - int fd[2]; + int sfd[2]; + int ifd; + int efd; }; +static int udev_monitor_filter_devtype(struct udev_monitor *udev_monitor, struct udev_device *udev_device) +{ + struct udev_list_entry *list_entry; + const char *devtype; + + devtype = udev_device_get_devtype(udev_device); + list_entry = udev_list_entry_get_next(&udev_monitor->devtype_match); + + if (!list_entry) { + return 1; + } + + if (!devtype) { + return 0; + } + + while (list_entry) { + if (strcmp(devtype, udev_list_entry_get_name(list_entry)) == 0) { + return 1; + } + + list_entry = udev_list_entry_get_next(list_entry); + } + + return 0; +} + +static int udev_monitor_filter_subsystem(struct udev_monitor *udev_monitor, struct udev_device *udev_device) +{ + struct udev_list_entry *list_entry; + const char *subsystem; + + subsystem = udev_device_get_subsystem(udev_device); + list_entry = udev_list_entry_get_next(&udev_monitor->subsystem_match); + + if (!list_entry) { + return 1; + } + + if (!subsystem) { + return 0; + } + + while (list_entry) { + if (strcmp(subsystem, udev_list_entry_get_name(list_entry)) == 0) { + return 1; + } + + list_entry = udev_list_entry_get_next(list_entry); + } + + return 0; +} + struct udev_device *udev_monitor_receive_device(struct udev_monitor *udev_monitor) { - return (void *)1; + char file[PATH_MAX + sizeof(UDEV_MONITOR_DIR)], data[4096]; + struct udev_device *udev_device; + + if (recv(udev_monitor->sfd[0], data, sizeof(data), 0) == -1) { + return NULL; + } + + snprintf(file, sizeof(file), UDEV_MONITOR_DIR "/%s", data); + udev_device = udev_device_new_from_file(udev_monitor->udev, file); + + if (!udev_device) { + return NULL; + } + + if (!udev_monitor_filter_subsystem(udev_monitor, udev_device) || + !udev_monitor_filter_devtype(udev_monitor, udev_device)) { + udev_device_unref(udev_device); + return NULL; + } + + return NULL; } int udev_monitor_enable_receiving(struct udev_monitor *udev_monitor) { + // TODO pthread_create should be here return 0; } int udev_monitor_set_receive_buffer_size(struct udev_monitor *udev_monitor, int size) { + // XXX NOT IMPLEMENTED return 0; } int udev_monitor_get_fd(struct udev_monitor *udev_monitor) { - return udev_monitor ? udev_monitor->fd[0] : -1; + return udev_monitor ? udev_monitor->sfd[0] : -1; } struct udev *udev_monitor_get_udev(struct udev_monitor *udev_monitor) @@ -37,27 +132,71 @@ struct udev *udev_monitor_get_udev(struct udev_monitor *udev_monitor) int udev_monitor_filter_update(struct udev_monitor *udev_monitor) { + // XXX NOT IMPLEMENTED return 0; } int udev_monitor_filter_remove(struct udev_monitor *udev_monitor) { + // XXX NOT IMPLEMENTED return 0; } int udev_monitor_filter_add_match_subsystem_devtype(struct udev_monitor *udev_monitor, const char *subsystem, const char *devtype) { + if (!udev_monitor || !subsystem) { + return -1; + } + + udev_list_entry_add(&udev_monitor->subsystem_match, subsystem, NULL, 0); + + if (devtype) { + udev_list_entry_add(&udev_monitor->devtype_match, devtype, NULL, 0); + } + return 0; } int udev_monitor_filter_add_match_tag(struct udev_monitor *udev_monitor, const char *tag) { + // XXX NOT IMPLEMENTED return 0; } +static void *udev_monitor_handle_event(void *ptr) +{ + struct udev_monitor *udev_monitor = ptr; + struct inotify_event *event; + struct epoll_event epoll; + char data[4096]; + sigset_t mask; + ssize_t len; + int i; + + sigemptyset(&mask); + sigaddset(&mask, SIGUSR1); + + while (epoll_pwait(udev_monitor->efd, &epoll, 1, -1, &mask) != -1) { + len = read(udev_monitor->ifd, data, sizeof(data)); + + if (len == -1) { + continue; + } + + for (i = 0; i < len; i += sizeof(struct inotify_event) + event->len) { + event = (struct inotify_event *)&data[i]; + send(udev_monitor->sfd[1], event->name, event->len, 0); + } + } + + return NULL; +} + struct udev_monitor *udev_monitor_new_from_netlink(struct udev *udev, const char *name) { struct udev_monitor *udev_monitor; + struct epoll_event epoll; + pthread_attr_t attr; int i; if (!udev || !name) { @@ -70,16 +209,51 @@ struct udev_monitor *udev_monitor_new_from_netlink(struct udev *udev, const char return NULL; } - // TODO better no-op, HELP WANTED ! - if (pipe(udev_monitor->fd) == -1) { + udev_monitor->ifd = inotify_init1(IN_CLOEXEC | IN_NONBLOCK); + + if (udev_monitor->ifd == -1) { free(udev_monitor); return NULL; } - for (i = 0; i < 2; i++) { - fcntl(udev_monitor->fd[i], F_SETFD, FD_CLOEXEC | O_NONBLOCK); + if (inotify_add_watch(udev_monitor->ifd, UDEV_MONITOR_DIR, IN_CREATE) == -1) { + close(udev_monitor->ifd); + free(udev_monitor); + return NULL; } + udev_monitor->efd = epoll_create1(EPOLL_CLOEXEC); + + if (udev_monitor->efd == -1) { + close(udev_monitor->ifd); + free(udev_monitor); + return NULL; + } + + epoll.events = EPOLLIN | EPOLLET; + + if (epoll_ctl(udev_monitor->efd, EPOLL_CTL_ADD, udev_monitor->ifd, &epoll) == -1) { + close(udev_monitor->ifd); + close(udev_monitor->efd); + free(udev_monitor); + return NULL; + } + + if (socketpair(AF_UNIX, SOCK_SEQPACKET | SOCK_CLOEXEC | SOCK_NONBLOCK, 0, udev_monitor->sfd) == -1) { + close(udev_monitor->ifd); + close(udev_monitor->efd); + free(udev_monitor); + return NULL; + } + + pthread_attr_init(&attr); + pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED); + + for (i = 0; i < THREADS_MAX; i++) { + pthread_create(&udev_monitor->thread[i], &attr, udev_monitor_handle_event, udev_monitor); + } + + pthread_attr_destroy(&attr); udev_monitor->refcount = 1; udev_monitor->udev = udev; return udev_monitor; @@ -107,10 +281,20 @@ struct udev_monitor *udev_monitor_unref(struct udev_monitor *udev_monitor) return NULL; } - for (i = 0; i < 2; i++) { - close(udev_monitor->fd[i]); + udev_list_entry_free_all(&udev_monitor->devtype_match); + udev_list_entry_free_all(&udev_monitor->subsystem_match); + + // TODO fix possible race condition and UB + for (i = 0; i < THREADS_MAX; i++) { + pthread_kill(udev_monitor->thread[i], SIGUSR1); } + for (i = 0; i < 2; i++) { + close(udev_monitor->sfd[i]); + } + + close(udev_monitor->ifd); + close(udev_monitor->efd); free(udev_monitor); return NULL; }