From 2f0900b0a86c448bc5680ef270249eb3af36308f Mon Sep 17 00:00:00 2001 From: Mikhail Avkhimenia Date: Thu, 18 Sep 2014 17:35:22 +0400 Subject: [PATCH] HAL_Linux: add GPIO driver for Raspberry Pi --- .../AP_HAL_Linux/AP_HAL_Linux_Namespace.h | 3 +- libraries/AP_HAL_Linux/GPIO.cpp | 123 +--------------- libraries/AP_HAL_Linux/GPIO.h | 133 +---------------- libraries/AP_HAL_Linux/GPIO_BBB.cpp | 131 +++++++++++++++++ libraries/AP_HAL_Linux/GPIO_BBB.h | 137 ++++++++++++++++++ libraries/AP_HAL_Linux/GPIO_RPI.cpp | 100 +++++++++++++ libraries/AP_HAL_Linux/GPIO_RPI.h | 73 ++++++++++ libraries/AP_HAL_Linux/HAL_Linux_Class.cpp | 15 +- 8 files changed, 464 insertions(+), 251 deletions(-) create mode 100644 libraries/AP_HAL_Linux/GPIO_BBB.cpp create mode 100644 libraries/AP_HAL_Linux/GPIO_BBB.h create mode 100644 libraries/AP_HAL_Linux/GPIO_RPI.cpp create mode 100644 libraries/AP_HAL_Linux/GPIO_RPI.h diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h index f8e07a4416..f8ad671feb 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h @@ -15,7 +15,8 @@ namespace Linux { class LinuxAnalogSource; class LinuxAnalogIn; class LinuxStorage; - class LinuxGPIO; + class LinuxGPIO_BBB; + class LinuxGPIO_RPI; class LinuxDigitalSource; class LinuxRCInput; class LinuxRCInput_PRU; diff --git a/libraries/AP_HAL_Linux/GPIO.cpp b/libraries/AP_HAL_Linux/GPIO.cpp index c2b5944f0f..dbfaf04dab 100644 --- a/libraries/AP_HAL_Linux/GPIO.cpp +++ b/libraries/AP_HAL_Linux/GPIO.cpp @@ -1,130 +1,11 @@ #include +#include "GPIO.h" #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX -#include "GPIO.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include - using namespace Linux; static const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; -LinuxGPIO::LinuxGPIO() -{} - -void LinuxGPIO::init() -{ -#if LINUX_GPIO_NUM_BANKS == 4 - int mem_fd; - // Enable all GPIO banks - // Without this, access to deactivated banks (i.e. those with no clock source set up) will (logically) fail with SIGBUS - // Idea taken from https://groups.google.com/forum/#!msg/beagleboard/OYFp4EXawiI/Mq6s3sg14HoJ - - uint8_t bank_enable[3] = { 5, 65, 105 }; - int export_fd = open("/sys/class/gpio/export", O_WRONLY); - if (export_fd == -1) { - hal.scheduler->panic("unable to open /sys/class/gpio/export"); - } - for (uint8_t i=0; i<3; i++) { - dprintf(export_fd, "%u\n", (unsigned)bank_enable[i]); - } - close(export_fd); - - - /* open /dev/mem */ - if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) { - printf("can't open /dev/mem \n"); - exit (-1); - } - - /* mmap GPIO */ - off_t offsets[LINUX_GPIO_NUM_BANKS] = { GPIO0_BASE, GPIO1_BASE, GPIO2_BASE, GPIO3_BASE }; - for (uint8_t i=0; ipanic("unable to map GPIO bank"); - } - gpio_bank[i].oe = gpio_bank[i].base + GPIO_OE; - gpio_bank[i].in = gpio_bank[i].base + GPIO_IN; - gpio_bank[i].out = gpio_bank[i].base + GPIO_OUT; - } - - close(mem_fd); -#endif // LINUX_GPIO_NUM_BANKS -} - -void LinuxGPIO::pinMode(uint8_t pin, uint8_t output) -{ - uint8_t bank = pin/32; - uint8_t bankpin = pin & 0x1F; - if (bank >= LINUX_GPIO_NUM_BANKS) { - return; - } - if (output == HAL_GPIO_INPUT) { - *gpio_bank[bank].oe |= (1U<= LINUX_GPIO_NUM_BANKS) { - return 0; - } - return *gpio_bank[bank].in & (1U<= LINUX_GPIO_NUM_BANKS) { - return; - } - if (value == LOW) { - *gpio_bank[bank].out &= ~(1U< -#define SYSFS_GPIO_DIR "/sys/class/gpio" - -#define GPIO0_BASE 0x44E07000 -#define GPIO1_BASE 0x4804C000 -#define GPIO2_BASE 0x481AC000 -#define GPIO3_BASE 0x481AE000 - -#define GPIO_SIZE 0x00000FFF - -// OE: 0 is output, 1 is input -#define GPIO_OE 0x14d -#define GPIO_IN 0x14e -#define GPIO_OUT 0x14f - -#define LED_AMBER 117 -#define LED_BLUE 48 -#define LED_SAFETY 61 -#define SAFETY_SWITCH 116 -#define LOW 0 -#define HIGH 1 - +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE -#define LINUX_GPIO_NUM_BANKS 4 -#else -// disable GPIO -#define LINUX_GPIO_NUM_BANKS 0 +#include "GPIO_BBB.h" +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO +#include "GPIO_RPI.h" #endif -// BeagleBone Black GPIO mappings -#define BBB_USR0 53 -#define BBB_USR1 54 -#define BBB_USR2 55 -#define BBB_USR3 56 -#define BBB_P8_3 38 -#define BBB_P8_4 39 -#define BBB_P8_5 34 -#define BBB_P8_6 35 -#define BBB_P8_7 66 -#define BBB_P8_8 67 -#define BBB_P8_9 69 -#define BBB_P8_10 68 -#define BBB_P8_11 45 -#define BBB_P8_12 44 -#define BBB_P8_13 23 -#define BBB_P8_14 26 -#define BBB_P8_15 47 -#define BBB_P8_16 46 -#define BBB_P8_17 27 -#define BBB_P8_18 65 -#define BBB_P8_19 22 -#define BBB_P8_20 63 -#define BBB_P8_21 62 -#define BBB_P8_22 37 -#define BBB_P8_23 36 -#define BBB_P8_24 33 -#define BBB_P8_25 32 -#define BBB_P8_26 61 -#define BBB_P8_27 86 -#define BBB_P8_28 88 -#define BBB_P8_29 87 -#define BBB_P8_30 89 -#define BBB_P8_31 10 -#define BBB_P8_32 11 -#define BBB_P8_33 9 -#define BBB_P8_34 81 -#define BBB_P8_35 8 -#define BBB_P8_36 80 -#define BBB_P8_37 78 -#define BBB_P8_38 79 -#define BBB_P8_39 76 -#define BBB_P8_40 77 -#define BBB_P8_41 74 -#define BBB_P8_42 75 -#define BBB_P8_43 72 -#define BBB_P8_44 73 -#define BBB_P8_45 70 -#define BBB_P8_46 71 -#define BBB_P9_11 30 -#define BBB_P9_12 60 -#define BBB_P9_13 31 -#define BBB_P9_14 50 -#define BBB_P9_15 48 -#define BBB_P9_16 51 -#define BBB_P9_17 5 -#define BBB_P9_18 4 -#define BBB_P9_19 13 -#define BBB_P9_20 12 -#define BBB_P9_21 3 -#define BBB_P9_22 2 -#define BBB_P9_23 49 -#define BBB_P9_24 15 -#define BBB_P9_25 117 -#define BBB_P9_26 14 -#define BBB_P9_27 115 -#define BBB_P9_28 113 -#define BBB_P9_29 111 -#define BBB_P9_30 112 -#define BBB_P9_31 110 -#define BBB_P9_41 20 -#define BBB_P9_42 7 - -class Linux::LinuxGPIO : public AP_HAL::GPIO { -private: - struct GPIO { - volatile uint32_t *base; - volatile uint32_t *oe; - volatile uint32_t *in; - volatile uint32_t *out; - } gpio_bank[LINUX_GPIO_NUM_BANKS]; - -public: - LinuxGPIO(); - void init(); - void pinMode(uint8_t pin, uint8_t output); - int8_t analogPinToDigitalPin(uint8_t pin); - uint8_t read(uint8_t pin); - void write(uint8_t pin, uint8_t value); - void toggle(uint8_t pin); - - /* Alternative interface: */ - AP_HAL::DigitalSource* channel(uint16_t n); - - /* Interrupt interface: */ - bool attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, - uint8_t mode); - - /* return true if USB cable is connected */ - bool usb_connected(void); -}; - class Linux::LinuxDigitalSource : public AP_HAL::DigitalSource { public: LinuxDigitalSource(uint8_t v); @@ -146,4 +22,5 @@ private: }; +#endif // CONFIG_HAL_BOARD == HAL_BOARD_LINUX #endif // __AP_HAL_LINUX_GPIO_H__ diff --git a/libraries/AP_HAL_Linux/GPIO_BBB.cpp b/libraries/AP_HAL_Linux/GPIO_BBB.cpp new file mode 100644 index 0000000000..9fe299f96d --- /dev/null +++ b/libraries/AP_HAL_Linux/GPIO_BBB.cpp @@ -0,0 +1,131 @@ +#include + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE + +#include "GPIO.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace Linux; + +static const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; +LinuxGPIO_BBB::LinuxGPIO_BBB() +{} + +void LinuxGPIO_BBB::init() +{ +#if LINUX_GPIO_NUM_BANKS == 4 + int mem_fd; + // Enable all GPIO banks + // Without this, access to deactivated banks (i.e. those with no clock source set up) will (logically) fail with SIGBUS + // Idea taken from https://groups.google.com/forum/#!msg/beagleboard/OYFp4EXawiI/Mq6s3sg14HoJ + + uint8_t bank_enable[3] = { 5, 65, 105 }; + int export_fd = open("/sys/class/gpio/export", O_WRONLY); + if (export_fd == -1) { + hal.scheduler->panic("unable to open /sys/class/gpio/export"); + } + for (uint8_t i=0; i<3; i++) { + dprintf(export_fd, "%u\n", (unsigned)bank_enable[i]); + } + close(export_fd); + + + /* open /dev/mem */ + if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) { + printf("can't open /dev/mem \n"); + exit (-1); + } + + /* mmap GPIO */ + off_t offsets[LINUX_GPIO_NUM_BANKS] = { GPIO0_BASE, GPIO1_BASE, GPIO2_BASE, GPIO3_BASE }; + for (uint8_t i=0; ipanic("unable to map GPIO bank"); + } + gpio_bank[i].oe = gpio_bank[i].base + GPIO_OE; + gpio_bank[i].in = gpio_bank[i].base + GPIO_IN; + gpio_bank[i].out = gpio_bank[i].base + GPIO_OUT; + } + + close(mem_fd); +#endif // LINUX_GPIO_NUM_BANKS +} + +void LinuxGPIO_BBB::pinMode(uint8_t pin, uint8_t output) +{ + uint8_t bank = pin/32; + uint8_t bankpin = pin & 0x1F; + if (bank >= LINUX_GPIO_NUM_BANKS) { + return; + } + if (output == HAL_GPIO_INPUT) { + *gpio_bank[bank].oe |= (1U<= LINUX_GPIO_NUM_BANKS) { + return 0; + } + return *gpio_bank[bank].in & (1U<= LINUX_GPIO_NUM_BANKS) { + return; + } + if (value == LOW) { + *gpio_bank[bank].out &= ~(1U< + +#define SYSFS_GPIO_DIR "/sys/class/gpio" + +#define GPIO0_BASE 0x44E07000 +#define GPIO1_BASE 0x4804C000 +#define GPIO2_BASE 0x481AC000 +#define GPIO3_BASE 0x481AE000 + +#define GPIO_SIZE 0x00000FFF + +// OE: 0 is output, 1 is input +#define GPIO_OE 0x14d +#define GPIO_IN 0x14e +#define GPIO_OUT 0x14f + +#define LED_AMBER 117 +#define LED_BLUE 48 +#define LED_SAFETY 61 +#define SAFETY_SWITCH 116 +#define LOW 0 +#define HIGH 1 + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE +#define LINUX_GPIO_NUM_BANKS 4 +#else +// disable GPIO +#define LINUX_GPIO_NUM_BANKS 0 +#endif + +// BeagleBone Black GPIO mappings +#define BBB_USR0 53 +#define BBB_USR1 54 +#define BBB_USR2 55 +#define BBB_USR3 56 +#define BBB_P8_3 38 +#define BBB_P8_4 39 +#define BBB_P8_5 34 +#define BBB_P8_6 35 +#define BBB_P8_7 66 +#define BBB_P8_8 67 +#define BBB_P8_9 69 +#define BBB_P8_10 68 +#define BBB_P8_11 45 +#define BBB_P8_12 44 +#define BBB_P8_13 23 +#define BBB_P8_14 26 +#define BBB_P8_15 47 +#define BBB_P8_16 46 +#define BBB_P8_17 27 +#define BBB_P8_18 65 +#define BBB_P8_19 22 +#define BBB_P8_20 63 +#define BBB_P8_21 62 +#define BBB_P8_22 37 +#define BBB_P8_23 36 +#define BBB_P8_24 33 +#define BBB_P8_25 32 +#define BBB_P8_26 61 +#define BBB_P8_27 86 +#define BBB_P8_28 88 +#define BBB_P8_29 87 +#define BBB_P8_30 89 +#define BBB_P8_31 10 +#define BBB_P8_32 11 +#define BBB_P8_33 9 +#define BBB_P8_34 81 +#define BBB_P8_35 8 +#define BBB_P8_36 80 +#define BBB_P8_37 78 +#define BBB_P8_38 79 +#define BBB_P8_39 76 +#define BBB_P8_40 77 +#define BBB_P8_41 74 +#define BBB_P8_42 75 +#define BBB_P8_43 72 +#define BBB_P8_44 73 +#define BBB_P8_45 70 +#define BBB_P8_46 71 +#define BBB_P9_11 30 +#define BBB_P9_12 60 +#define BBB_P9_13 31 +#define BBB_P9_14 50 +#define BBB_P9_15 48 +#define BBB_P9_16 51 +#define BBB_P9_17 5 +#define BBB_P9_18 4 +#define BBB_P9_19 13 +#define BBB_P9_20 12 +#define BBB_P9_21 3 +#define BBB_P9_22 2 +#define BBB_P9_23 49 +#define BBB_P9_24 15 +#define BBB_P9_25 117 +#define BBB_P9_26 14 +#define BBB_P9_27 115 +#define BBB_P9_28 113 +#define BBB_P9_29 111 +#define BBB_P9_30 112 +#define BBB_P9_31 110 +#define BBB_P9_41 20 +#define BBB_P9_42 7 + +class Linux::LinuxGPIO_BBB : public AP_HAL::GPIO { +private: + struct GPIO { + volatile uint32_t *base; + volatile uint32_t *oe; + volatile uint32_t *in; + volatile uint32_t *out; + } gpio_bank[LINUX_GPIO_NUM_BANKS]; + +public: + LinuxGPIO_BBB(); + void init(); + void pinMode(uint8_t pin, uint8_t output); + int8_t analogPinToDigitalPin(uint8_t pin); + uint8_t read(uint8_t pin); + void write(uint8_t pin, uint8_t value); + void toggle(uint8_t pin); + + /* Alternative interface: */ + AP_HAL::DigitalSource* channel(uint16_t n); + + /* Interrupt interface: */ + bool attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, + uint8_t mode); + + /* return true if USB cable is connected */ + bool usb_connected(void); +}; + +#endif // __AP_HAL_LINUX_GPIO_BBB_H__ diff --git a/libraries/AP_HAL_Linux/GPIO_RPI.cpp b/libraries/AP_HAL_Linux/GPIO_RPI.cpp new file mode 100644 index 0000000000..3b3ff4287f --- /dev/null +++ b/libraries/AP_HAL_Linux/GPIO_RPI.cpp @@ -0,0 +1,100 @@ +#include + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO + +#include "GPIO.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace Linux; + +static const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; +LinuxGPIO_RPI::LinuxGPIO_RPI() +{} + +void LinuxGPIO_RPI::init() +{ + // open /dev/mem + if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) { + printf("can't open /dev/mem \n"); + exit(-1); + } + + // mmap GPIO + gpio_map = mmap( + NULL, // Any adddress in our space will do + BLOCK_SIZE, // Map length + PROT_READ|PROT_WRITE, // Enable reading & writting to mapped memory + MAP_SHARED, // Shared with other processes + mem_fd, // File to map + GPIO_BASE // Offset to GPIO peripheral + ); + + close(mem_fd); // No need to keep mem_fd open after mmap + + if (gpio_map == MAP_FAILED) { + printf("mmap error %d\n", (int)gpio_map); // errno also set! + exit(-1); + } + + gpio = (volatile unsigned *)gpio_map; // Always use volatile pointer! +} + +void LinuxGPIO_RPI::pinMode(uint8_t pin, uint8_t output) +{ + if (output == HAL_GPIO_INPUT) { + GPIO_MODE_IN(pin); + } else { + GPIO_MODE_IN(pin); + GPIO_MODE_OUT(pin); + } +} + +int8_t LinuxGPIO_RPI::analogPinToDigitalPin(uint8_t pin) +{ + return -1; +} + +uint8_t LinuxGPIO_RPI::read(uint8_t pin) +{ + return GPIO_GET(pin); +} + +void LinuxGPIO_RPI::write(uint8_t pin, uint8_t value) +{ + if (value == LOW) { + GPIO_SET_LOW = 1 << pin; + } else { + GPIO_SET_HIGH = 1 << pin; + } +} + +void LinuxGPIO_RPI::toggle(uint8_t pin) +{ + write(pin, !read(pin)); +} + +/* Alternative interface: */ +AP_HAL::DigitalSource* LinuxGPIO_RPI::channel(uint16_t n) { + return new LinuxDigitalSource(n); +} + +/* Interrupt interface: */ +bool LinuxGPIO_RPI::attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode) +{ + return true; +} + +bool LinuxGPIO_RPI::usb_connected(void) +{ + return false; +} + +#endif // CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO diff --git a/libraries/AP_HAL_Linux/GPIO_RPI.h b/libraries/AP_HAL_Linux/GPIO_RPI.h new file mode 100644 index 0000000000..b8724400fc --- /dev/null +++ b/libraries/AP_HAL_Linux/GPIO_RPI.h @@ -0,0 +1,73 @@ + +#ifndef __AP_HAL_LINUX_GPIO_RPI_H__ +#define __AP_HAL_LINUX_GPIO_RPI_H__ + +#include + +#define LOW 0 +#define HIGH 1 + +// Raspberry Pi GPIO memory +#define BCM2708_PERI_BASE 0x20000000 +#define GPIO_BASE (BCM2708_PERI_BASE + 0x200000) +#define PAGE_SIZE (4*1024) +#define BLOCK_SIZE (4*1024) + +// GPIO setup. Always use INP_GPIO(x) before OUT_GPIO(x) or SET_GPIO_ALT(x,y) +#define GPIO_MODE_IN(g) *(gpio+((g)/10)) &= ~(7<<(((g)%10)*3)) +#define GPIO_MODE_OUT(g) *(gpio+((g)/10)) |= (1<<(((g)%10)*3)) +#define GPIO_MODE_ALT(g,a) *(gpio+(((g)/10))) |= (((a)<=3?(a)+4:(a)==4?3:2)<<(((g)%10)*3)) +#define GPIO_SET_HIGH *(gpio+7) // sets bits which are 1 +#define GPIO_SET_LOW *(gpio+10) // clears bits which are 1 +#define GPIO_GET(g) (*(gpio+13)&(1<