diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h index fabdacd6c4..c2d9f5767a 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h @@ -14,9 +14,11 @@ namespace Linux { class Storage; class GPIO_BBB; class GPIO_RPI; + class GPIO_Sysfs; class Storage; class Storage_FRAM; class DigitalSource; + class DigitalSource_Sysfs; class RCInput; class RCInput_PRU; class RCInput_AioPRU; diff --git a/libraries/AP_HAL_Linux/GPIO_Sysfs.cpp b/libraries/AP_HAL_Linux/GPIO_Sysfs.cpp new file mode 100644 index 0000000000..44ef43561a --- /dev/null +++ b/libraries/AP_HAL_Linux/GPIO_Sysfs.cpp @@ -0,0 +1,253 @@ +#include "GPIO_Sysfs.h" + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX + +#include +#include +#include +#include +#include + +#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",\ + __PRETTY_FUNCTION__, v_, max_); \ + return __VA_ARGS__; \ + } \ + } while (0) + +using namespace Linux; + +static const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + +DigitalSource_Sysfs::DigitalSource_Sysfs(unsigned pin, int value_fd) + : _pin(pin) + , _value_fd(value_fd) +{ +} + +DigitalSource_Sysfs::~DigitalSource_Sysfs() +{ + if (_value_fd >= 0) { + ::close(_value_fd); + } +} + +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", + _pin); + return LOW; + } + return char_value == '1' ? HIGH : LOW; +} + +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", + _pin); + } +} + +void DigitalSource_Sysfs::mode(uint8_t output) +{ + auto gpio_sysfs = GPIO_Sysfs::from(hal.gpio); + gpio_sysfs->_pinMode(_pin, output); +} + +void DigitalSource_Sysfs::toggle() +{ + write(!read()); +} + +void GPIO_Sysfs::init() +{ +} + +void GPIO_Sysfs::pinMode(uint8_t vpin, uint8_t output) +{ + assert_vpin(vpin, n_pins); + + unsigned pin = pin_table[vpin]; + _pinMode(pin, 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); + + 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 = 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]; + 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); + return -1; + } + + return fd; +} + +uint8_t GPIO_Sysfs::read(uint8_t vpin) +{ + assert_vpin(vpin, n_pins, LOW); + + unsigned pin = pin_table[vpin]; + int fd = _open_pin_value(pin, O_RDONLY); + + if (fd < 0) + return LOW; + + 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; + } + + close(fd); + + return value; +} + +void GPIO_Sysfs::write(uint8_t vpin, uint8_t value) +{ + assert_vpin(vpin, n_pins); + + unsigned pin = pin_table[vpin]; + int fd = _open_pin_value(pin, O_WRONLY); + + if (fd < 0) + return; + + int r = ::pwrite(fd, value == HIGH ? "1" : "0", 1, 0); + if (r < 0) { + hal.console->printf("warning: unable to write pin %u\n", pin); + } + + close(fd); +} + +void GPIO_Sysfs::toggle(uint8_t vpin) +{ + write(vpin, !read(vpin)); +} + +int8_t GPIO_Sysfs::analogPinToDigitalPin(uint8_t vpin) +{ + return -1; +} + +AP_HAL::DigitalSource* GPIO_Sysfs::channel(uint16_t vpin) +{ + assert_vpin(vpin, n_pins, nullptr); + + 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); + } + } + + /* Even if we couldn't open the fd, return a new DigitalSource and let + * reads and writes fail later due to invalid. Otherwise we + * could crash in undesired places */ + return new DigitalSource_Sysfs(pin, value_fd); +} + +bool GPIO_Sysfs::attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, + uint8_t mode) +{ + return false; +} + +bool GPIO_Sysfs::usb_connected(void) +{ + return false; +} + +bool GPIO_Sysfs::export_pin(uint8_t vpin) +{ + uint8_t vpins[] = { vpin }; + return export_pins(vpins, 1); +} + +bool GPIO_Sysfs::export_pins(uint8_t vpins[], size_t len) +{ + int fd = open("/sys/class/gpio/export", O_WRONLY | O_CLOEXEC); + if (fd < 0) { + hal.console->printf("unable to open /sys/class/gpio/export"); + return false; + } + + bool success = true; + + 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]); + success = false; + break; + } + unsigned int pin = pin_table[vpins[i]]; + char gpio_path[PATH_MAX]; + + snprintf(gpio_path, PATH_MAX, GPIO_PATH_FORMAT, pin); + + // Verify if the pin is not already exported + if (access(gpio_path, F_OK) == 0) + continue; + + int r = dprintf(fd, "%u\n", pin); + if (r < 0) { + hal.console->printf("error on exporting gpio pin number %u\n", pin); + success = false; + break; + } + } + + close(fd); + + return success; +} + +#endif diff --git a/libraries/AP_HAL_Linux/GPIO_Sysfs.h b/libraries/AP_HAL_Linux/GPIO_Sysfs.h new file mode 100644 index 0000000000..a7b8a6b539 --- /dev/null +++ b/libraries/AP_HAL_Linux/GPIO_Sysfs.h @@ -0,0 +1,93 @@ +#pragma once + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX + +#include "AP_HAL_Linux.h" +#include "GPIO.h" + +class Linux::DigitalSource_Sysfs : public AP_HAL::DigitalSource { + friend class Linux::GPIO_Sysfs; +public: + ~DigitalSource_Sysfs(); + uint8_t read(); + void write(uint8_t value); + void mode(uint8_t output); + void toggle(); +private: + /* Only GPIO_Sysfs will be able to instantiate */ + DigitalSource_Sysfs(unsigned pin, int value_fd); + int _value_fd; + unsigned _pin; +}; + +/** + * Generic implementation of AP_HAL::GPIO for Linux based boards. + */ +class Linux::GPIO_Sysfs : public AP_HAL::GPIO { + friend class Linux::DigitalSource_Sysfs; +public: + /* Fill this table with the real pin numbers. */ + static const unsigned pin_table[]; + static const uint8_t n_pins; + + static GPIO_Sysfs *from(AP_HAL::GPIO *gpio) { + return static_cast(gpio); + } + + void init(); + + void pinMode(uint8_t vpin, uint8_t output) override; + uint8_t read(uint8_t vpin) override; + void write(uint8_t vpin, uint8_t value) override; + void toggle(uint8_t vpin) override; + + /* + * Export pin, instantiate a new DigitalSource_Sysfs and return its + * pointer. + */ + AP_HAL::DigitalSource *channel(uint16_t vpin) override; + + /* + * Currently this function always returns -1. + */ + int8_t analogPinToDigitalPin(uint8_t vpin) override; + + /* + * Currently this function always returns false. + */ + bool attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode) override; + + /* + * Currently this function always returns false. + */ + bool usb_connected() override; + + /* + * Make pin available for use. This function should be called before + * calling functions that use the pin number as parameter. + * + * Returns true if pin is exported successfully and false otherwise. + * + * Note: the pin is ignored if already exported. + */ + static bool export_pin(uint8_t vpin); + + /* + * Make pins available for use. This function should be called before + * calling functions that use pin number as parameter. + * + * If all pins are exported successfully, true is returned. If there is an + * error for one of them, false is returned. + * + * Note: pins already exported are ignored. + */ + static bool export_pins(uint8_t vpins[], size_t num_vpins); + +protected: + void _pinMode(unsigned int pin, uint8_t output); + int _open_pin_value(unsigned int pin, int flags); +}; + +#endif