From e75a8b045a59f4367a2fef2a63e9a09645b99443 Mon Sep 17 00:00:00 2001 From: Adrian Szyndela Date: Fri, 25 Jun 2021 15:14:35 +0200 Subject: [PATCH] refactoring: extract common udev code Change-Id: I40e73a8b266e634a1d51b4d1e010d4ecbf43e2a0 --- src/common.c | 101 +++++++++++++++++++++++++++++++++++ src/common.h | 8 +++ src/peripheral_gpio.c | 134 ++++++++-------------------------------------- src/peripheral_pwm.c | 143 ++++++++++---------------------------------------- 4 files changed, 159 insertions(+), 227 deletions(-) diff --git a/src/common.c b/src/common.c index 6c891d9..d1a36b1 100644 --- a/src/common.c +++ b/src/common.c @@ -1,5 +1,8 @@ #include "common.h" #include +#include +#include +#include #include #include @@ -39,3 +42,101 @@ bool peripheral_is_feature_supported(const char *feature_name, int *feature_stat return *feature_state == PERIPHERAL_FEATURE_TRUE; } +int peripheral_create_udev_monitor(struct udev **udev, struct udev_monitor **monitor, const char *filter_name) +{ + struct udev *_udev = NULL; + struct udev_monitor *_monitor = NULL; + int ret = -EIO; + + _udev = udev_new(); + if (!_udev) { + _E("Cannot create udev"); + goto error; + } + + _monitor = udev_monitor_new_from_netlink(_udev, "udev"); + if (!_monitor) { + _E("Cannot create udev monitor"); + goto error; + } + + ret = udev_monitor_filter_add_match_subsystem_devtype(_monitor, filter_name, NULL); + if (ret < 0) { + _E("Failed to add monitor filter"); + goto error; + } + + ret = udev_monitor_enable_receiving(_monitor); + if (ret < 0) { + _E("Failed to enable udev receiving"); + goto error; + } + + *udev = _udev; + *monitor = _monitor; + + return 0; + +error: + udev_monitor_unref(_monitor); + udev_unref(_udev); + return ret; +} + + +int peripheral_wait_for_udev(struct udev_monitor *monitor, UdevCompareFunc func, void *func_data) +{ + assert(monitor); + assert(func); + + struct udev_device *dev = NULL; + struct pollfd pfd; + + pfd.fd = udev_monitor_get_fd(monitor); + pfd.events = POLLIN; + + for (int cnt = 0; cnt < 10; cnt++) { + _D("poll iteration"); + if (poll(&pfd, 1, 100) < 0) { + _E("Failed to watch udev monitor"); + return -EIO; + } + + dev = udev_monitor_receive_device(monitor); + if (dev) { + if (func(dev, func_data)) { + udev_device_unref(dev); + return 0; + } + udev_device_unref(dev); + } + } + _E("Time out"); + + return 0; +} +int peripheral_write_pin_to_file(const char *file_name, int pin) +{ + assert(pin >= 0); + + char buf[sizeof MAX_d_FMT] = {0, }; /* space for pin %d */ + + int fd = open(file_name, O_WRONLY | O_CLOEXEC); + if (fd < 0) { + _E("open(%s) failed: %m", file_name); + return -1; + } + + int ret = 0; + int length = snprintf(buf, sizeof buf, "%d", pin); + if (write(fd, buf, length) != length) + ret = errno; + + if (close(fd) != 0) { + _E("close(%s) failed: %m", file_name); + if (ret == 0) + ret = -1; + } + + return ret; +} diff --git a/src/common.h b/src/common.h index 560242b..34bd3f2 100644 --- a/src/common.h +++ b/src/common.h @@ -16,6 +16,7 @@ #pragma once #include +#include #include #include #include @@ -92,3 +93,10 @@ bool peripheral_is_feature_supported(const char *feature_name, int *feature_stat return peripheral_is_feature_supported(feature_name, &feature); \ return feature == PERIPHERAL_FEATURE_TRUE; \ } + +int peripheral_create_udev_monitor(struct udev **udev, struct udev_monitor **monitor, const char *filter_name); + +typedef bool (*UdevCompareFunc)(struct udev_device *, void *); +int peripheral_wait_for_udev(struct udev_monitor *monitor, UdevCompareFunc func, void *func_data); + +int peripheral_write_pin_to_file(const char *file_name, int pin); diff --git a/src/peripheral_gpio.c b/src/peripheral_gpio.c index b6666bb..af3b5ce 100644 --- a/src/peripheral_gpio.c +++ b/src/peripheral_gpio.c @@ -66,79 +66,21 @@ struct _peripheral_gpio_s { peripheral_gpio_edge_e edge; }; -static int __gpio_create_udev_monitor(struct udev **udev, struct udev_monitor **monitor) -{ - struct udev *_udev = NULL; - struct udev_monitor *_monitor = NULL; - int ret = -EIO; - - _udev = udev_new(); - if (!_udev) { - _E("Cannot create udev"); - goto error; - } - - _monitor = udev_monitor_new_from_netlink(_udev, "udev"); - if (!_monitor) { - _E("Cannot create udev monitor"); - goto error; - } - - ret = udev_monitor_filter_add_match_subsystem_devtype(_monitor, "gpio", NULL); - if (ret < 0) { - _E("Failed to add monitor filter"); - goto error; - } - - ret = udev_monitor_enable_receiving(_monitor); - if (ret < 0) { - _E("Failed to enable udev receiving"); - goto error; - } - - *udev = _udev; - *monitor = _monitor; - - return 0; - -error: - udev_monitor_unref(_monitor); - udev_unref(_udev); - return ret; -} - -static int __gpio_wait_for_udev(struct udev_monitor *monitor, int pin) -{ - struct udev_device *dev; - struct pollfd pfd; #define GPIO_BASE "gpio" - char gpio_name[sizeof(GPIO_BASE MAX_d_FMT)]; - - pfd.fd = udev_monitor_get_fd(monitor); - pfd.events = POLLIN; - snprintf(gpio_name, sizeof gpio_name, "gpio%d", pin); +typedef struct FilterData { + char gpio_name[sizeof(GPIO_BASE MAX_d_FMT)]; +} FilterData; - for (int cnt = 0; cnt < 10; cnt++) { - _D("poll iteration"); - if (poll(&pfd, 1, 100) < 0) { - _E("Failed to watch udev monitor"); - return -EIO; - } +static bool __filter_device(struct udev_device *device, void *data) +{ + FilterData *filter = data; - dev = udev_monitor_receive_device(monitor); - if (dev) { - if (strcmp(udev_device_get_sysname(dev), gpio_name) == 0) { - _D("udev for %s is initialized", gpio_name); - udev_device_unref(dev); - return 0; - } - udev_device_unref(dev); - } + if (strcmp(udev_device_get_sysname(device), filter->gpio_name) == 0) { + _D("udev for %s is initialized", filter->gpio_name); + return true; } - _E("Time out"); - - return 0; + return false; } static int peripheral_gpio_export(int pin) @@ -146,49 +88,32 @@ static int peripheral_gpio_export(int pin) RETVM_IF(pin < 0, PERIPHERAL_ERROR_INVALID_PARAMETER, "Invalid gpio pin"); int ret; - int fd = -1; - int length; - char buf[sizeof MAX_d_FMT] = {0, }; /* space for pin %d */ struct udev *udev = NULL; struct udev_monitor *monitor = NULL; - ret = __gpio_create_udev_monitor(&udev, &monitor); + ret = peripheral_create_udev_monitor(&udev, &monitor, "gpio"); if (ret != 0) { _E("Failed to create udev monitor"); - goto out; - }; - - fd = open("/sys/class/gpio/export", O_WRONLY | O_CLOEXEC); - if (fd < 0) { - _E("open() failed: %m"); ret = PERIPHERAL_ERROR_IO_ERROR; goto out; - } + }; - length = snprintf(buf, sizeof buf, "%d", pin); - ret = write(fd, buf, length); - if (ret != length) { - /* If write() fails with EBUSY, then it means + ret = peripheral_write_pin_to_file("/sys/class/gpio/export", pin); + if (ret != 0) { + /* If export fails with EBUSY, then it means this pin has been already exported. That's fine for us. */ - if (errno != EBUSY) { - _E("write() failed: %m"); - ret = PERIPHERAL_ERROR_IO_ERROR; + if (ret != EBUSY) { + _E("gpio: export pin %d: failed with error %d", pin, ret); goto out; } } - ret = close(fd); - if (ret != 0) { - _E("close() failed: %m"); - ret = PERIPHERAL_ERROR_IO_ERROR; - goto out; - } else { - fd = -1; - } + FilterData filter; + snprintf(filter.gpio_name, sizeof filter.gpio_name, GPIO_BASE "%d", pin); - ret = __gpio_wait_for_udev(monitor, pin); + ret = peripheral_wait_for_udev(monitor, __filter_device, &filter); if (ret < 0) { _E("device nodes are not writable"); ret = PERIPHERAL_ERROR_IO_ERROR; @@ -196,8 +121,6 @@ static int peripheral_gpio_export(int pin) } out: - close_fd(fd); - udev_monitor_unref(monitor); udev_unref(udev); @@ -224,20 +147,9 @@ static inline int cleanup_handle(peripheral_gpio_h handle) { if (pin < 0) return PERIPHERAL_ERROR_NONE; - char buf[sizeof MAX_d_FMT] = {0, }; /* space for %d */ - int fd = open("/sys/class/gpio/unexport", O_WRONLY | O_CLOEXEC); - if (fd < 0) { - _E("gpio: unexport pin %d: open() failed: %m", pin); - return PERIPHERAL_ERROR_IO_ERROR; - } - - int length = snprintf(buf, sizeof(buf), "%d", pin); - int ret = write(fd, buf, length); - - close(fd); - - if (ret != length) { - _E("gpio: unexport pin %d: write() failed: %m", pin); + int ret = peripheral_write_pin_to_file("/sys/class/gpio/unexport", pin); + if (ret != 0) { + _E("gpio: unexport pin %d: failed with error %d", pin, ret); return PERIPHERAL_ERROR_IO_ERROR; } diff --git a/src/peripheral_pwm.c b/src/peripheral_pwm.c index d9b2c37..82eff5f 100644 --- a/src/peripheral_pwm.c +++ b/src/peripheral_pwm.c @@ -15,7 +15,6 @@ */ #include -#include #include #include #include @@ -41,85 +40,27 @@ struct _peripheral_pwm_s { int fd_enable; }; -static int __pwm_create_udev_monitor(struct udev **udev, struct udev_monitor **monitor) -{ - struct udev *_udev = NULL; - struct udev_monitor *_monitor = NULL; - int ret = -EIO; - - _udev = udev_new(); - if (!_udev) { - _E("Cannot create udev"); - goto error; - } - - _monitor = udev_monitor_new_from_netlink(_udev, "udev"); - if (!_monitor) { - _E("Cannot create udev monitor"); - goto error; - } - - ret = udev_monitor_filter_add_match_subsystem_devtype(_monitor, "pwm", NULL); - if (ret < 0) { - _E("Failed to add monitor filter"); - goto error; - } - - ret = udev_monitor_enable_receiving(_monitor); - if (ret < 0) { - _E("Failed to enable udev receiving"); - goto error; - } - - *udev = _udev; - *monitor = _monitor; - - return 0; - -error: - udev_monitor_unref(_monitor); - udev_unref(_udev); - return ret; -} +#define PWMCHIP_BASE "pwmchip" +#define PWM_BASE "pwm" -static int __pwm_wait_for_udev(struct udev_monitor *monitor, int chip, int pin) +typedef struct FilterData { - struct udev_device *dev = NULL; - struct pollfd pfd; -#define PWMCHIP_BASE "pwmchip" char pwmchip_name[sizeof(PWMCHIP_BASE MAX_d_FMT)]; /* space for pwmchip%d */ -#define PWM_BASE "pwm" char pwm_name[sizeof(PWM_BASE MAX_d_FMT)]; /* space for pwm%d */ +} FilterData; - pfd.fd = udev_monitor_get_fd(monitor); - pfd.events = POLLIN; - - snprintf(pwmchip_name, sizeof pwmchip_name, PWMCHIP_BASE "%d", chip); - snprintf(pwm_name, sizeof pwm_name, PWM_BASE "%d", pin); - - for (int cnt = 0; cnt < 10; cnt++) { - _D("poll iteration"); - if (poll(&pfd, 1, 100) < 0) { - _E("Failed to watch udev monitor"); - return -EIO; - } +static bool __filter_device(struct udev_device *device, void *data) +{ + FilterData *filterData = data; - dev = udev_monitor_receive_device(monitor); - if (dev) { - if (strcmp(udev_device_get_sysname(dev), pwmchip_name) == 0) { - const char *prop = udev_device_get_property_value(dev, "EXPORT"); - if (prop && strcmp(prop, pwm_name) == 0) { - _D("udev for %s is initialized", pwm_name); - udev_device_unref(dev); - return 0; - } - } - udev_device_unref(dev); + if (strcmp(udev_device_get_sysname(device), filterData->pwmchip_name) == 0) { + const char *prop = udev_device_get_property_value(device, "EXPORT"); + if (prop && strcmp(prop, filterData->pwm_name) == 0) { + _D("udev for %s is initialized", filterData->pwm_name); + return true; } } - _E("Time out"); - - return 0; + return false; } static int peripheral_pwm_export(int chip, int pin) @@ -127,57 +68,38 @@ static int peripheral_pwm_export(int chip, int pin) RETVM_IF(chip < 0, PERIPHERAL_ERROR_INVALID_PARAMETER, "Invalid pwm chip"); RETVM_IF(pin < 0, PERIPHERAL_ERROR_INVALID_PARAMETER, "Invalid pwm pin"); - int ret; - int fd = -1; - int length; #define EXPORT_PATH(chip) ("/sys/class/pwm/pwmchip" chip "/export") char path[sizeof EXPORT_PATH(MAX_d_FMT)] = {0, }; /* space for /sys/class/pwm/pwmchip%d/export */ - char buf[sizeof MAX_d_FMT] = {0, }; /* space for pin %d */ struct udev *udev = NULL; struct udev_monitor *monitor = NULL; + int ret; - ret = __pwm_create_udev_monitor(&udev, &monitor); + ret = peripheral_create_udev_monitor(&udev, &monitor, "pwm"); if (ret != 0) { _E("Failed to create udev monitor"); + ret = PERIPHERAL_ERROR_IO_ERROR; goto out; }; snprintf(path, sizeof path, EXPORT_PATH("%d"), chip); - fd = open(path, O_WRONLY | O_CLOEXEC); - if (fd < 0) { - _E("open() failed: %m"); - ret = PERIPHERAL_ERROR_IO_ERROR; + ret = peripheral_write_pin_to_file(path, pin); + if (ret != 0) { + _E("pwm: export pin %d failed with error %d", pin, ret); goto out; } - length = snprintf(buf, sizeof buf, "%d", pin); - ret = write(fd, buf, length); - if (ret != length) { - _E("write() failed: %m"); - ret = PERIPHERAL_ERROR_IO_ERROR; - goto out; - } + FilterData filter; + snprintf(filter.pwmchip_name, sizeof filter.pwmchip_name, PWMCHIP_BASE "%d", chip); + snprintf(filter.pwm_name, sizeof filter.pwm_name, PWM_BASE "%d", pin); - ret = close(fd); + ret = peripheral_wait_for_udev(monitor, __filter_device, &filter); if (ret != 0) { - _E("close() failed: %m"); - ret = PERIPHERAL_ERROR_IO_ERROR; - goto out; - } else { - fd = -1; - } - - ret = __pwm_wait_for_udev(monitor, chip, pin); - if (ret < 0) { _E("device nodes are not writable"); ret = PERIPHERAL_ERROR_IO_ERROR; goto out; } out: - if (fd != -1) - close(fd); - udev_monitor_unref(monitor); udev_unref(udev); @@ -207,27 +129,16 @@ static inline int cleanup_handle(peripheral_pwm_h handle) close_fd(handle->fd_period); /* this releases chip+pin flock() */ free(handle); - if (pin <0) + if (pin < 0) return PERIPHERAL_ERROR_NONE; - char buf[sizeof MAX_d_FMT] = {0, }; /* space for pin %d */ #define UNEXPORT_PATH(chip) ("/sys/class/pwm/pwmchip" chip "/unexport") char path[sizeof UNEXPORT_PATH(MAX_d_FMT)] = {0, }; /* space for /sys/class/pwm/pwmchip%d/unexport */ snprintf(path, sizeof path, UNEXPORT_PATH("%d"), chip); - int fd = open(path, O_WRONLY | O_CLOEXEC); - if (fd < 0) { - _E("pwm: unexport pin %d: open() failed: %m", pin); - return PERIPHERAL_ERROR_IO_ERROR; - } - - int length = snprintf(buf, sizeof(buf), "%d", pin); - int ret = write(fd, buf, length); - - close(fd); - - if (ret != length) { - _E("pwm: unexport pin %d: write() failed: %m", pin); + int ret = peripheral_write_pin_to_file(path, pin); + if (ret != 0) { + _E("pwm: unexport pin %d failed with error %d", pin, ret); return PERIPHERAL_ERROR_IO_ERROR; } -- 2.7.4