From ba0ed840392ebde7a87e40b1846a267c86d23bbc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Roberto=20de=20Souza?= Date: Wed, 18 Nov 2015 14:30:17 -0200 Subject: [PATCH] AP_HAL_Linux: Refactor GPIO_Sysfs - Replaced PATH_MAX by the maximum stack memory it will use for GPIO paths - Added more information to error logs - Removed the '/n' when writing to GPIO sysfs files - Using Linux Util write_file() on _pinMode() --- libraries/AP_HAL_Linux/GPIO_Sysfs.cpp | 126 +++++++++++++------------- 1 file changed, 63 insertions(+), 63 deletions(-) diff --git a/libraries/AP_HAL_Linux/GPIO_Sysfs.cpp b/libraries/AP_HAL_Linux/GPIO_Sysfs.cpp index bfb5e78dba..efacee1751 100644 --- a/libraries/AP_HAL_Linux/GPIO_Sysfs.cpp +++ b/libraries/AP_HAL_Linux/GPIO_Sysfs.cpp @@ -5,14 +5,14 @@ #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_QFLIGHT #include -#include #include #include #include +#include "Util.h" + #define LOW 0 #define HIGH 1 -#define GPIO_PATH_FORMAT "/sys/class/gpio/gpio%u" #define assert_vpin(v_, max_, ...) do { \ if (v_ >= max_) { \ hal.console->printf("warning (%s): vpin %u out of range [0, %u)\n",\ @@ -25,6 +25,18 @@ using namespace Linux; static const AP_HAL::HAL& hal = AP_HAL::get_HAL(); +#define UINT32_MAX_STR "4294967295" + +/* Trick to use minimum stack space for each of the params */ +union gpio_params { + char export_gpio[sizeof("export")]; + char direction[sizeof("gpio" UINT32_MAX_STR "/direction")]; + char value[sizeof("gpio" UINT32_MAX_STR "/value")]; +}; + +#define GPIO_BASE_PATH "/sys/class/gpio/" +#define GPIO_PATH_MAX (sizeof(GPIO_BASE_PATH) + sizeof(gpio_params) - 1) + DigitalSource_Sysfs::DigitalSource_Sysfs(unsigned pin, int value_fd) : _pin(pin) , _value_fd(value_fd) @@ -41,20 +53,18 @@ DigitalSource_Sysfs::~DigitalSource_Sysfs() uint8_t DigitalSource_Sysfs::read() { char char_value; - int r = ::pread(_value_fd, &char_value, 1, 0); - if (r < 0) { - hal.console->printf("warning: unable to read GPIO value for pin %u\n", + if (::pread(_value_fd, &char_value, 1, 0) < 0) { + hal.console->printf("DigitalSource_Sysfs: Unable to read pin %u value.\n", _pin); return LOW; } - return char_value == '1' ? HIGH : LOW; + return char_value - '0'; } void DigitalSource_Sysfs::write(uint8_t value) { - int r = ::pwrite(_value_fd, value == HIGH ? "1" : "0", 1, 0); - if (r < 0) { - hal.console->printf("warning: unable to write GPIO value for pin %u\n", + if (::pwrite(_value_fd, value == HIGH ? "1" : "0", 1, 0) < 0) { + hal.console->printf("DigitalSource_Sysfs: Unable to write pin %u value.\n", _pin); } } @@ -84,34 +94,27 @@ void GPIO_Sysfs::pinMode(uint8_t vpin, uint8_t output) void GPIO_Sysfs::_pinMode(unsigned int pin, uint8_t output) { - char direction_path[PATH_MAX]; + const char *dir = output ? "out" : "in"; + char path[GPIO_PATH_MAX]; - snprintf(direction_path, PATH_MAX, GPIO_PATH_FORMAT "/direction", pin); - - int fd = open(direction_path, O_WRONLY | O_CLOEXEC); - if (fd < 0) { - hal.console->printf("unable to open %s\n", direction_path); - return; + int r = snprintf(path, GPIO_PATH_MAX, GPIO_BASE_PATH "gpio%u/direction", + pin); + if (r < 0 || r >= (int)GPIO_PATH_MAX + || Util::from(hal.util)->write_file(path, "%s", dir) < 0) { + hal.console->printf("GPIO_Sysfs: Unable to set pin %u mode.\n", pin); } - - int r = dprintf(fd, output == HAL_GPIO_INPUT ? "in\n" : "out\n"); - if (r < 0) { - hal.console->printf("unable to write to %s\n", direction_path); - } - - close(fd); } int GPIO_Sysfs::_open_pin_value(unsigned int pin, int flags) { - char path[PATH_MAX]; + char path[GPIO_PATH_MAX]; int fd; - snprintf(path, PATH_MAX, GPIO_PATH_FORMAT "/value", pin); - - fd = open(path, flags | O_CLOEXEC); - if (fd < 0) { - hal.console->printf("warning: unable to open %s\n", path); + int r = snprintf(path, GPIO_PATH_MAX, GPIO_BASE_PATH "gpio%u/value", pin); + if (r < 0 || r >= (int)GPIO_PATH_MAX + || (fd = open(path, flags | O_CLOEXEC)) < 0) { + hal.console->printf("GPIO_Sysfs: Unable to get value file descriptor for pin %u.\n", + pin); return -1; } @@ -122,43 +125,47 @@ uint8_t GPIO_Sysfs::read(uint8_t vpin) { assert_vpin(vpin, n_pins, LOW); - unsigned pin = pin_table[vpin]; + const unsigned pin = pin_table[vpin]; int fd = _open_pin_value(pin, O_RDONLY); - if (fd < 0) - return LOW; + if (fd < 0) { + goto error; + } char char_value; - uint8_t value; - int r = ::pread(fd, &char_value, 1, 0); - if (r < 0) { - hal.console->printf("warning: unable to read pin %u\n", pin); - value = LOW; - } else { - value = char_value == '1' ? HIGH : LOW; + if (::pread(fd, &char_value, 1, 0) < 0) { + goto error; } close(fd); - return value; + return char_value - '0'; + +error: + hal.console->printf("GPIO_Sysfs: Unable to read pin %u value.\n", vpin); + return LOW; } void GPIO_Sysfs::write(uint8_t vpin, uint8_t value) { assert_vpin(vpin, n_pins); - unsigned pin = pin_table[vpin]; + const unsigned pin = pin_table[vpin]; int fd = _open_pin_value(pin, O_WRONLY); - if (fd < 0) - return; + if (fd < 0) { + goto error; + } - int r = ::pwrite(fd, value == HIGH ? "1" : "0", 1, 0); - if (r < 0) { - hal.console->printf("warning: unable to write pin %u\n", pin); + if (::pwrite(fd, value == HIGH ? "1" : "0", 1, 0) < 0) { + goto error; } close(fd); + return; + +error: + hal.console->printf("GPIO_Sysfs: Unable to write pin %u value.\n", vpin); } void GPIO_Sysfs::toggle(uint8_t vpin) @@ -175,17 +182,11 @@ AP_HAL::DigitalSource* GPIO_Sysfs::channel(uint16_t vpin) { assert_vpin(vpin, n_pins, nullptr); - unsigned pin = pin_table[vpin]; + const unsigned pin = pin_table[vpin]; int value_fd = -1; if (_export_pin(vpin)) { - char value_path[PATH_MAX]; - snprintf(value_path, PATH_MAX, GPIO_PATH_FORMAT "/value", pin); - - value_fd = open(value_path, O_RDWR | O_CLOEXEC); - if (value_fd < 0) { - hal.console->printf("unable to open %s\n", value_path); - } + value_fd = _open_pin_value(pin, O_RDWR); } /* Even if we couldn't open the fd, return a new DigitalSource and let @@ -213,9 +214,9 @@ bool GPIO_Sysfs::_export_pin(uint8_t vpin) bool GPIO_Sysfs::_export_pins(uint8_t vpins[], size_t len) { - int fd = open("/sys/class/gpio/export", O_WRONLY | O_CLOEXEC); + int fd = open(GPIO_BASE_PATH "export", O_WRONLY | O_CLOEXEC); if (fd < 0) { - hal.console->printf("unable to open /sys/class/gpio/export"); + hal.console->printf("GPIO_Sysfs: Unable to open " GPIO_BASE_PATH "export."); return false; } @@ -223,23 +224,22 @@ bool GPIO_Sysfs::_export_pins(uint8_t vpins[], size_t len) for (unsigned int i = 0; i < len; i++) { if (vpins[i] >= n_pins) { - hal.console->printf("GPIO_Sysfs: can't open pin %u\n", - vpins[i]); + hal.console->printf("GPIO_Sysfs: Can't open pin %u\n", vpins[i]); success = false; break; } - unsigned int pin = pin_table[vpins[i]]; - char gpio_path[PATH_MAX]; + const unsigned int pin = pin_table[vpins[i]]; + char gpio_path[GPIO_PATH_MAX]; - snprintf(gpio_path, PATH_MAX, GPIO_PATH_FORMAT, pin); + snprintf(gpio_path, GPIO_PATH_MAX, GPIO_BASE_PATH "gpio%u", pin); // Verify if the pin is not already exported if (access(gpio_path, F_OK) == 0) continue; - int r = dprintf(fd, "%u\n", pin); + int r = dprintf(fd, "%u", pin); if (r < 0) { - hal.console->printf("error on exporting gpio pin number %u\n", pin); + hal.console->printf("GPIO_Sysfs: Unable to export pin %u.\n", pin); success = false; break; }