mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
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()
This commit is contained in:
José Roberto de Souza
Lucas De Marchi
parent
0c0d595b55
commit
ba0ed84039
@ -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];
|
||||
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;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user