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

@@ -5,14 +5,14 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
 
 #include <fcntl.h>
-#include <linux/limits.h>
 #include <stdio.h>
 #include <sys/stat.h>
 #include <unistd.h>
 
+#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];
-
-    snprintf(direction_path, PATH_MAX, GPIO_PATH_FORMAT "/direction", pin);
+    const char *dir = output ? "out" : "in";
+    char path[GPIO_PATH_MAX];
 
-    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;
         }