From 749cf9a600eaebe441632fa9e8d61132db54bfb3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Thu, 9 May 2024 09:56:42 -0300 Subject: [PATCH] AP_HAL_Linux: Add support to Raspberry Pi 5 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The Raspberry Pi 5 has a coprocessor that takes care of the IO over the BCM. This adds support to the new RP1 processor. Signed-off-by: Patrick José Pereira --- libraries/AP_HAL_Linux/GPIO_RPI.cpp | 219 +++------------------ libraries/AP_HAL_Linux/GPIO_RPI.h | 160 +--------------- libraries/AP_HAL_Linux/GPIO_RPI_BCM.cpp | 241 ++++++++++++++++++++++++ libraries/AP_HAL_Linux/GPIO_RPI_BCM.h | 188 ++++++++++++++++++ libraries/AP_HAL_Linux/GPIO_RPI_HAL.h | 13 ++ libraries/AP_HAL_Linux/GPIO_RPI_RP1.cpp | 178 +++++++++++++++++ libraries/AP_HAL_Linux/GPIO_RPI_RP1.h | 169 +++++++++++++++++ 7 files changed, 815 insertions(+), 353 deletions(-) create mode 100644 libraries/AP_HAL_Linux/GPIO_RPI_BCM.cpp create mode 100644 libraries/AP_HAL_Linux/GPIO_RPI_BCM.h create mode 100644 libraries/AP_HAL_Linux/GPIO_RPI_HAL.h create mode 100644 libraries/AP_HAL_Linux/GPIO_RPI_RP1.cpp create mode 100644 libraries/AP_HAL_Linux/GPIO_RPI_RP1.h diff --git a/libraries/AP_HAL_Linux/GPIO_RPI.cpp b/libraries/AP_HAL_Linux/GPIO_RPI.cpp index 974e6523e4..74d300185e 100644 --- a/libraries/AP_HAL_Linux/GPIO_RPI.cpp +++ b/libraries/AP_HAL_Linux/GPIO_RPI.cpp @@ -8,237 +8,64 @@ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - #include "GPIO.h" +#include "GPIO_RPI.h" +#include "GPIO_RPI_BCM.h" +#include "GPIO_RPI_RP1.h" #include "Util_RPI.h" -#define GPIO_RPI_MAX_NUMBER_PINS 32 - -using namespace Linux; - extern const AP_HAL::HAL& hal; -// Range based in the first memory address of the first register and the last memory addres -// for the GPIO section (0x7E20'00B4 - 0x7E20'0000). -const uint8_t GPIO_RPI::_gpio_registers_memory_range = 0xB4; -const char* GPIO_RPI::_system_memory_device_path = "/dev/mem"; +using namespace Linux; GPIO_RPI::GPIO_RPI() { } -void GPIO_RPI::set_gpio_mode_alt(int pin, int alternative) -{ - // Each register can contain 10 pins - const uint8_t pins_per_register = 10; - // Calculates the position of the 3 bit mask in the 32 bits register - const uint8_t tree_bits_position_in_register = (pin%pins_per_register)*3; - /** Creates a mask to enable the alternative function based in the following logic: - * - * | Alternative Function | 3 bits value | - * |:--------------------:|:------------:| - * | Function 0 | 0b100 | - * | Function 1 | 0b101 | - * | Function 2 | 0b110 | - * | Function 3 | 0b111 | - * | Function 4 | 0b011 | - * | Function 5 | 0b010 | - */ - const uint8_t alternative_value = - (alternative < 4 ? (alternative + 4) : (alternative == 4 ? 3 : 2)); - // 0b00'000'000'000'000'000'000'ALT'000'000'000 enables alternative for the 4th pin - const uint32_t mask_with_alt = static_cast(alternative_value) << tree_bits_position_in_register; - const uint32_t mask = 0b111 << tree_bits_position_in_register; - // Clear all bits in our position and apply our mask with alt values - uint32_t register_value = _gpio[pin / pins_per_register]; - register_value &= ~mask; - _gpio[pin / pins_per_register] = register_value | mask_with_alt; -} - -void GPIO_RPI::set_gpio_mode_in(int pin) -{ - // Each register can contain 10 pins - const uint8_t pins_per_register = 10; - // Calculates the position of the 3 bit mask in the 32 bits register - const uint8_t tree_bits_position_in_register = (pin%pins_per_register)*3; - // Create a mask that only removes the bits in this specific GPIO pin, E.g: - // 0b11'111'111'111'111'111'111'000'111'111'111 for the 4th pin - const uint32_t mask = ~(0b111<(address) + static_cast(offset); -} - -volatile uint32_t* GPIO_RPI::get_memory_pointer(uint32_t address, uint32_t range) const -{ - auto pointer = mmap( - nullptr, // Any adddress in our space will do - range, // Map length - PROT_READ|PROT_WRITE|PROT_EXEC, // Enable reading & writing to mapped memory - MAP_SHARED|MAP_LOCKED, // Shared with other processes - _system_memory_device, // File to map - address // Offset to GPIO peripheral - ); - - if (pointer == MAP_FAILED) { - return nullptr; - } - - return static_cast(pointer); -} - -bool GPIO_RPI::openMemoryDevice() -{ - _system_memory_device = open(_system_memory_device_path, O_RDWR|O_SYNC|O_CLOEXEC); - if (_system_memory_device < 0) { - AP_HAL::panic("Can't open %s", GPIO_RPI::_system_memory_device_path); - return false; - } - - return true; -} - -void GPIO_RPI::closeMemoryDevice() -{ - close(_system_memory_device); - // Invalidate device variable - _system_memory_device = -1; -} - void GPIO_RPI::init() { const LINUX_BOARD_TYPE rpi_version = UtilRPI::from(hal.util)->detect_linux_board_type(); - GPIO_RPI::Address peripheral_base; - if(rpi_version == LINUX_BOARD_TYPE::RPI_ZERO_1) { - peripheral_base = Address::BCM2708_PERIPHERAL_BASE; - } else if (rpi_version == LINUX_BOARD_TYPE::RPI_2_3_ZERO2) { - peripheral_base = Address::BCM2709_PERIPHERAL_BASE; - } else if (rpi_version == LINUX_BOARD_TYPE::RPI_4) { - peripheral_base = Address::BCM2711_PERIPHERAL_BASE; - } else { - AP_HAL::panic("Unknown rpi_version, cannot locate peripheral base address"); - return; + switch (rpi_version) { + case LINUX_BOARD_TYPE::RPI_ZERO_1: + case LINUX_BOARD_TYPE::RPI_2_3_ZERO2: + case LINUX_BOARD_TYPE::RPI_4: + gpioDriver = new GPIO_RPI_BCM(); + gpioDriver->init(); + break; + case LINUX_BOARD_TYPE::RPI_5: + gpioDriver = new GPIO_RPI_RP1(); + gpioDriver->init(); + break; + default: + AP_HAL::panic("Unknown rpi_version, cannot locate peripheral base address"); + return; } - - if (!openMemoryDevice()) { - AP_HAL::panic("Failed to initialize memory device."); - return; - } - - const uint32_t gpio_address = get_address(peripheral_base, PeripheralOffset::GPIO); - - _gpio = get_memory_pointer(gpio_address, _gpio_registers_memory_range); - if (!_gpio) { - AP_HAL::panic("Failed to get GPIO memory map."); - } - - // No need to keep mem_fd open after mmap - closeMemoryDevice(); } void GPIO_RPI::pinMode(uint8_t pin, uint8_t output) { - if (output == HAL_GPIO_INPUT) { - set_gpio_mode_in(pin); - } else { - set_gpio_mode_in(pin); - set_gpio_mode_out(pin); - } + gpioDriver->pinMode(pin, output); } void GPIO_RPI::pinMode(uint8_t pin, uint8_t output, uint8_t alt) { - if (output == HAL_GPIO_INPUT) { - set_gpio_mode_in(pin); - } else if (output == HAL_GPIO_ALT) { - set_gpio_mode_in(pin); - set_gpio_mode_alt(pin, alt); - } else { - set_gpio_mode_in(pin); - set_gpio_mode_out(pin); - } + gpioDriver->pinMode(pin, output, alt); } uint8_t GPIO_RPI::read(uint8_t pin) { - if (pin >= GPIO_RPI_MAX_NUMBER_PINS) { - return 0; - } - return static_cast(get_gpio_logic_state(pin)); + return gpioDriver->read(pin); } void GPIO_RPI::write(uint8_t pin, uint8_t value) { - if (value != 0) { - set_gpio_high(pin); - } else { - set_gpio_low(pin); - } + gpioDriver->write(pin, value); } void GPIO_RPI::toggle(uint8_t pin) { - if (pin >= GPIO_RPI_MAX_NUMBER_PINS) { - return ; - } - uint32_t flag = (1 << pin); - _gpio_output_port_status ^= flag; - write(pin, (_gpio_output_port_status & flag) >> pin); + gpioDriver->toggle(pin); } /* Alternative interface: */ diff --git a/libraries/AP_HAL_Linux/GPIO_RPI.h b/libraries/AP_HAL_Linux/GPIO_RPI.h index f694dce679..ac934a42ef 100644 --- a/libraries/AP_HAL_Linux/GPIO_RPI.h +++ b/libraries/AP_HAL_Linux/GPIO_RPI.h @@ -2,6 +2,7 @@ #include #include "AP_HAL_Linux.h" +#include "GPIO_RPI_HAL.h" /** * @brief Check for valid Raspberry Pi pin range @@ -15,13 +16,12 @@ template constexpr uint8_t RPI_GPIO_() return pin; } + namespace Linux { /** * @brief Class for Raspberry PI GPIO control * - * For more information: https://elinux.org/RPi_BCM2835_GPIOs - * */ class GPIO_RPI : public AP_HAL::GPIO { public: @@ -40,161 +40,7 @@ public: bool usb_connected(void) override; private: - // Raspberry Pi BASE memory address - enum class Address : uint32_t { - BCM2708_PERIPHERAL_BASE = 0x20000000, // Raspberry Pi 0/1 - BCM2709_PERIPHERAL_BASE = 0x3F000000, // Raspberry Pi 2/3 - BCM2711_PERIPHERAL_BASE = 0xFE000000, // Raspberry Pi 4 - }; - - // Offset between peripheral base address - enum class PeripheralOffset : uint32_t { - GPIO = 0x200000, - }; - - /** - * @brief Open memory device to allow gpio address access - * Should be used before get_memory_pointer calls in the initialization - * - * @return true - * @return false - */ - bool openMemoryDevice(); - - /** - * @brief Close open memory device - * - */ - void closeMemoryDevice(); - - /** - * @brief Return pointer to memory location with specific range access - * - * @param address - * @param range - * @return volatile uint32_t* - */ - volatile uint32_t* get_memory_pointer(uint32_t address, uint32_t range) const; - - /** - * @brief Get memory address based in base address and peripheral offset - * - * @param address - * @param offset - * @return uint32_t - */ - uint32_t get_address(GPIO_RPI::Address address, GPIO_RPI::PeripheralOffset offset) const; - - /** - * @brief Change functionality of GPIO Function Select Registers (GPFSELn) to any alternative function. - * Each GPIO pin is mapped to 3 bits inside a 32 bits register, E.g: - * - * 0b00...'010'101 - * ││ │││ ││└── GPIO Pin N, 1st bit, LSBit - * ││ │││ │└─── GPIO Pin N, 2nd bit - * ││ │││ └──── GPIO Pin N, 3rd bit, MSBit - * ││ ││└────── GPIO Pin N+1, 1st bit, LSBit - * ││ │└─────── GPIO Pin N+1, 2nd bit, - * ││ └──────── GPIO Pin N+1, 3rd bit, MSBit - * ││ ... - * │└───────────── Reserved - * └────────────── Reserved - * - * And the value of this 3 bits selects the functionality of the GPIO pin, E.g: - * 000 = GPIO Pin N is an input - * 001 = GPIO Pin N is an output - * 100 = GPIO Pin N takes alternate function 0 - * 101 = GPIO Pin N takes alternate function 1 - * 110 = GPIO Pin N takes alternate function 2 - * 111 = GPIO Pin N takes alternate function 3 - * 011 = GPIO Pin N takes alternate function 4 - * 010 = GPIO Pin N takes alternate function 5 - * - * The alternative functions are defined in the BCM datasheet under "Alternative Function" - * section for each pin. - * - * This information is also valid for: - * - Linux::GPIO_RPI::set_gpio_mode_in - * - Linux::GPIO_RPI::set_gpio_mode_out - * - * @param pin - */ - void set_gpio_mode_alt(int pin, int alternative); - - /** - * @brief Set a specific GPIO as input - * Check Linux::GPIO_RPI::set_gpio_mode_alt for more information. - * - * @param pin - */ - void set_gpio_mode_in(int pin); - - /** - * @brief Set a specific GPIO as output - * Check Linux::GPIO_RPI::set_gpio_mode_alt for more information. - * - * @param pin - */ - void set_gpio_mode_out(int pin); - - /** - * @brief Modify GPSET0 (GPIO Pin Output Set 0) register to set pin as high - * Writing zero to this register has no effect, please use Linux::GPIO_RPI::set_gpio_low - * to set pin as low. - * - * GPSET0 is a 32bits register that each bit points to a respective GPIO pin: - * 0b...101 - * ││└── GPIO Pin 1, 1st bit, LSBit, defined as High - * │└─── GPIO Pin 2, 2nd bit, No effect - * └──── GPIO Pin 3, 3rd bit, defined as High - * ... - * - * @param pin - */ - void set_gpio_high(int pin); - - /** - * @brief Modify GPCLR0 (GPIO Pin Output Clear 0) register to set pin as low - * Writing zero to this register has no effect, please use Linux::GPIO_RPI::set_gpio_high - * to set pin as high. - * - * GPCLR0 is a 32bits register that each bit points to a respective GPIO pin: - * 0b...101 - * ││└── GPIO Pin 1, 1st bit, LSBit, defined as Low - * │└─── GPIO Pin 2, 2nd bit, No effect - * └──── GPIO Pin 3, 3rd bit, defined as Low - * - * @param pin - */ - void set_gpio_low(int pin); - - /** - * @brief Read GPLEV0 (GPIO Pin Level 0) register check the logic state of a specific pin - * - * GPLEV0 is a 32bits register that each bit points to a respective GPIO pin: - * 0b...101 - * ││└── GPIO Pin 1, 1st bit, LSBit, Pin is in High state - * │└─── GPIO Pin 2, 2nd bit, Pin is in Low state - * └──── GPIO Pin 3, 3rd bit, Pin is in High state - * - * @param pin - * @return true - * @return false - */ - bool get_gpio_logic_state(int pin); - - // Memory pointer to gpio registers - volatile uint32_t* _gpio; - // Memory range for the gpio registers - static const uint8_t _gpio_registers_memory_range; - // Path to memory device (E.g: /dev/mem) - static const char* _system_memory_device_path; - // File descriptor for the memory device file - // If it's negative, then there was an error opening the file. - int _system_memory_device; - // store GPIO output status. - uint32_t _gpio_output_port_status = 0x00; - + GPIO_RPI_HAL* gpioDriver; }; } diff --git a/libraries/AP_HAL_Linux/GPIO_RPI_BCM.cpp b/libraries/AP_HAL_Linux/GPIO_RPI_BCM.cpp new file mode 100644 index 0000000000..38f20237ff --- /dev/null +++ b/libraries/AP_HAL_Linux/GPIO_RPI_BCM.cpp @@ -0,0 +1,241 @@ +#include + +#include +#include +#include +#include +#include + +#include "GPIO.h" +#include "Util_RPI.h" +#include "GPIO_RPI_BCM.h" + +#define GPIO_RPI_MAX_NUMBER_PINS 32 + +using namespace Linux; + +extern const AP_HAL::HAL& hal; + +// Range based in the first memory address of the first register and the last memory addres +// for the GPIO section (0x7E20'00B4 - 0x7E20'0000). +const uint8_t GPIO_RPI_BCM::_gpio_registers_memory_range = 0xB4; +const char* GPIO_RPI_BCM::_system_memory_device_path = "/dev/mem"; + +GPIO_RPI_BCM::GPIO_RPI_BCM() +{ +} + +void GPIO_RPI_BCM::set_gpio_mode_alt(int pin, int alternative) +{ + // Each register can contain 10 pins + const uint8_t pins_per_register = 10; + // Calculates the position of the 3 bit mask in the 32 bits register + const uint8_t tree_bits_position_in_register = (pin%pins_per_register)*3; + /** Creates a mask to enable the alternative function based in the following logic: + * + * | Alternative Function | 3 bits value | + * |:--------------------:|:------------:| + * | Function 0 | 0b100 | + * | Function 1 | 0b101 | + * | Function 2 | 0b110 | + * | Function 3 | 0b111 | + * | Function 4 | 0b011 | + * | Function 5 | 0b010 | + */ + const uint8_t alternative_value = + (alternative < 4 ? (alternative + 4) : (alternative == 4 ? 3 : 2)); + // 0b00'000'000'000'000'000'000'ALT'000'000'000 enables alternative for the 4th pin + const uint32_t mask_with_alt = static_cast(alternative_value) << tree_bits_position_in_register; + const uint32_t mask = 0b111 << tree_bits_position_in_register; + // Clear all bits in our position and apply our mask with alt values + uint32_t register_value = _gpio[pin / pins_per_register]; + register_value &= ~mask; + _gpio[pin / pins_per_register] = register_value | mask_with_alt; +} + +void GPIO_RPI_BCM::set_gpio_mode_in(int pin) +{ + // Each register can contain 10 pins + const uint8_t pins_per_register = 10; + // Calculates the position of the 3 bit mask in the 32 bits register + const uint8_t tree_bits_position_in_register = (pin%pins_per_register)*3; + // Create a mask that only removes the bits in this specific GPIO pin, E.g: + // 0b11'111'111'111'111'111'111'000'111'111'111 for the 4th pin + const uint32_t mask = ~(0b111<(address) + static_cast(offset); +} + +volatile uint32_t* GPIO_RPI_BCM::get_memory_pointer(uint32_t address, uint32_t range) const +{ + auto pointer = mmap( + nullptr, // Any adddress in our space will do + range, // Map length + PROT_READ|PROT_WRITE|PROT_EXEC, // Enable reading & writing to mapped memory + MAP_SHARED|MAP_LOCKED, // Shared with other processes + _system_memory_device, // File to map + address // Offset to GPIO peripheral + ); + + if (pointer == MAP_FAILED) { + return nullptr; + } + + return static_cast(pointer); +} + +bool GPIO_RPI_BCM::openMemoryDevice() +{ + _system_memory_device = open(_system_memory_device_path, O_RDWR|O_SYNC|O_CLOEXEC); + if (_system_memory_device < 0) { + AP_HAL::panic("Can't open %s", GPIO_RPI_BCM::_system_memory_device_path); + return false; + } + + return true; +} + +void GPIO_RPI_BCM::closeMemoryDevice() +{ + close(_system_memory_device); + // Invalidate device variable + _system_memory_device = -1; +} + +void GPIO_RPI_BCM::init() +{ + const LINUX_BOARD_TYPE rpi_version = UtilRPI::from(hal.util)->detect_linux_board_type(); + + GPIO_RPI_BCM::Address peripheral_base; + if(rpi_version == LINUX_BOARD_TYPE::RPI_ZERO_1) { + peripheral_base = Address::BCM2708_PERIPHERAL_BASE; + } else if (rpi_version == LINUX_BOARD_TYPE::RPI_2_3_ZERO2) { + peripheral_base = Address::BCM2709_PERIPHERAL_BASE; + } else if (rpi_version == LINUX_BOARD_TYPE::RPI_4) { + peripheral_base = Address::BCM2711_PERIPHERAL_BASE; + } else { + AP_HAL::panic("Unknown rpi_version, cannot locate peripheral base address"); + return; + } + + if (!openMemoryDevice()) { + AP_HAL::panic("Failed to initialize memory device."); + return; + } + + const uint32_t gpio_address = get_address(peripheral_base, PeripheralOffset::GPIO); + + _gpio = get_memory_pointer(gpio_address, _gpio_registers_memory_range); + if (!_gpio) { + AP_HAL::panic("Failed to get GPIO memory map."); + } + + // No need to keep mem_fd open after mmap + closeMemoryDevice(); +} + +void GPIO_RPI_BCM::pinMode(uint8_t pin, uint8_t output) +{ + if (output == HAL_GPIO_INPUT) { + set_gpio_mode_in(pin); + } else { + set_gpio_mode_in(pin); + set_gpio_mode_out(pin); + } +} + +void GPIO_RPI_BCM::pinMode(uint8_t pin, uint8_t output, uint8_t alt) +{ + if (output == HAL_GPIO_INPUT) { + set_gpio_mode_in(pin); + } else if (output == HAL_GPIO_ALT) { + set_gpio_mode_in(pin); + set_gpio_mode_alt(pin, alt); + } else { + set_gpio_mode_in(pin); + set_gpio_mode_out(pin); + } +} + +uint8_t GPIO_RPI_BCM::read(uint8_t pin) +{ + if (pin >= GPIO_RPI_MAX_NUMBER_PINS) { + return 0; + } + return static_cast(get_gpio_logic_state(pin)); +} + +void GPIO_RPI_BCM::write(uint8_t pin, uint8_t value) +{ + if (value != 0) { + set_gpio_high(pin); + } else { + set_gpio_low(pin); + } +} + +void GPIO_RPI_BCM::toggle(uint8_t pin) +{ + if (pin >= GPIO_RPI_MAX_NUMBER_PINS) { + return ; + } + uint32_t flag = (1 << pin); + _gpio_output_port_status ^= flag; + write(pin, (_gpio_output_port_status & flag) >> pin); +} + +/* Alternative interface: */ +AP_HAL::DigitalSource* GPIO_RPI_BCM::channel(uint16_t n) +{ + return new DigitalSource(n); +} + +bool GPIO_RPI_BCM::usb_connected(void) +{ + return false; +} diff --git a/libraries/AP_HAL_Linux/GPIO_RPI_BCM.h b/libraries/AP_HAL_Linux/GPIO_RPI_BCM.h new file mode 100644 index 0000000000..f0bf6c293b --- /dev/null +++ b/libraries/AP_HAL_Linux/GPIO_RPI_BCM.h @@ -0,0 +1,188 @@ +#pragma once + +#include +#include "GPIO_RPI_HAL.h" + +namespace Linux { + +/** + * @brief Class for Raspberry PI GPIO control + * + * For more information: https://elinux.org/RPi_BCM2835_GPIOs + * + */ +class GPIO_RPI_BCM : public GPIO_RPI_HAL { +public: + GPIO_RPI_BCM(); + void init() override; + void pinMode(uint8_t pin, uint8_t output) override; + void pinMode(uint8_t pin, uint8_t output, uint8_t alt) override; + uint8_t read(uint8_t pin) override; + void write(uint8_t pin, uint8_t value) override; + void toggle(uint8_t pin) override; + + /* Alternative interface: */ + AP_HAL::DigitalSource* channel(uint16_t n); + + /* return true if USB cable is connected */ + bool usb_connected(void); + +private: + // Raspberry Pi BASE memory address + enum class Address : uint32_t { + BCM2708_PERIPHERAL_BASE = 0x20000000, // Raspberry Pi 0/1 + BCM2709_PERIPHERAL_BASE = 0x3F000000, // Raspberry Pi 2/3 + BCM2711_PERIPHERAL_BASE = 0xFE000000, // Raspberry Pi 4 + }; + + // Offset between peripheral base address + enum class PeripheralOffset : uint32_t { + GPIO = 0x200000, + }; + + /** + * @brief Open memory device to allow gpio address access + * Should be used before get_memory_pointer calls in the initialization + * + * @return true + * @return false + */ + bool openMemoryDevice(); + + /** + * @brief Close open memory device + * + */ + void closeMemoryDevice(); + + /** + * @brief Return pointer to memory location with specific range access + * + * @param address + * @param range + * @return volatile uint32_t* + */ + volatile uint32_t* get_memory_pointer(uint32_t address, uint32_t range) const; + + /** + * @brief Get memory address based in base address and peripheral offset + * + * @param address + * @param offset + * @return uint32_t + */ + uint32_t get_address(GPIO_RPI_BCM::Address address, GPIO_RPI_BCM::PeripheralOffset offset) const; + + /** + * @brief Change functionality of GPIO Function Select Registers (GPFSELn) to any alternative function. + * Each GPIO pin is mapped to 3 bits inside a 32 bits register, E.g: + * + * 0b00...'010'101 + * ││ │││ ││└── GPIO Pin N, 1st bit, LSBit + * ││ │││ │└─── GPIO Pin N, 2nd bit + * ││ │││ └──── GPIO Pin N, 3rd bit, MSBit + * ││ ││└────── GPIO Pin N+1, 1st bit, LSBit + * ││ │└─────── GPIO Pin N+1, 2nd bit, + * ││ └──────── GPIO Pin N+1, 3rd bit, MSBit + * ││ ... + * │└───────────── Reserved + * └────────────── Reserved + * + * And the value of this 3 bits selects the functionality of the GPIO pin, E.g: + * 000 = GPIO Pin N is an input + * 001 = GPIO Pin N is an output + * 100 = GPIO Pin N takes alternate function 0 + * 101 = GPIO Pin N takes alternate function 1 + * 110 = GPIO Pin N takes alternate function 2 + * 111 = GPIO Pin N takes alternate function 3 + * 011 = GPIO Pin N takes alternate function 4 + * 010 = GPIO Pin N takes alternate function 5 + * + * The alternative functions are defined in the BCM datasheet under "Alternative Function" + * section for each pin. + * + * This information is also valid for: + * - Linux::GPIO_RPI_BCM::set_gpio_mode_in + * - Linux::GPIO_RPI_BCM::set_gpio_mode_out + * + * @param pin + */ + void set_gpio_mode_alt(int pin, int alternative); + + /** + * @brief Set a specific GPIO as input + * Check Linux::GPIO_RPI_BCM::set_gpio_mode_alt for more information. + * + * @param pin + */ + void set_gpio_mode_in(int pin); + + /** + * @brief Set a specific GPIO as output + * Check Linux::GPIO_RPI_BCM::set_gpio_mode_alt for more information. + * + * @param pin + */ + void set_gpio_mode_out(int pin); + + /** + * @brief Modify GPSET0 (GPIO Pin Output Set 0) register to set pin as high + * Writing zero to this register has no effect, please use Linux::GPIO_RPI_BCM::set_gpio_low + * to set pin as low. + * + * GPSET0 is a 32bits register that each bit points to a respective GPIO pin: + * 0b...101 + * ││└── GPIO Pin 1, 1st bit, LSBit, defined as High + * │└─── GPIO Pin 2, 2nd bit, No effect + * └──── GPIO Pin 3, 3rd bit, defined as High + * ... + * + * @param pin + */ + void set_gpio_high(int pin); + + /** + * @brief Modify GPCLR0 (GPIO Pin Output Clear 0) register to set pin as low + * Writing zero to this register has no effect, please use Linux::GPIO_RPI_BCM::set_gpio_high + * to set pin as high. + * + * GPCLR0 is a 32bits register that each bit points to a respective GPIO pin: + * 0b...101 + * ││└── GPIO Pin 1, 1st bit, LSBit, defined as Low + * │└─── GPIO Pin 2, 2nd bit, No effect + * └──── GPIO Pin 3, 3rd bit, defined as Low + * + * @param pin + */ + void set_gpio_low(int pin); + + /** + * @brief Read GPLEV0 (GPIO Pin Level 0) register check the logic state of a specific pin + * + * GPLEV0 is a 32bits register that each bit points to a respective GPIO pin: + * 0b...101 + * ││└── GPIO Pin 1, 1st bit, LSBit, Pin is in High state + * │└─── GPIO Pin 2, 2nd bit, Pin is in Low state + * └──── GPIO Pin 3, 3rd bit, Pin is in High state + * + * @param pin + * @return true + * @return false + */ + bool get_gpio_logic_state(int pin); + + // Memory pointer to gpio registers + volatile uint32_t* _gpio; + // Memory range for the gpio registers + static const uint8_t _gpio_registers_memory_range; + // Path to memory device (E.g: /dev/mem) + static const char* _system_memory_device_path; + // File descriptor for the memory device file + // If it's negative, then there was an error opening the file. + int _system_memory_device; + // store GPIO output status. + uint32_t _gpio_output_port_status = 0x00; + +}; + +} diff --git a/libraries/AP_HAL_Linux/GPIO_RPI_HAL.h b/libraries/AP_HAL_Linux/GPIO_RPI_HAL.h new file mode 100644 index 0000000000..7cec066a94 --- /dev/null +++ b/libraries/AP_HAL_Linux/GPIO_RPI_HAL.h @@ -0,0 +1,13 @@ +#pragma once + +class GPIO_RPI_HAL { +public: + GPIO_RPI_HAL() {} + virtual void init() = 0; + virtual void pinMode(uint8_t pin, uint8_t output) = 0; + virtual void pinMode(uint8_t pin, uint8_t output, uint8_t alt) {}; + + virtual uint8_t read(uint8_t pin) = 0; + virtual void write(uint8_t pin, uint8_t value) = 0; + virtual void toggle(uint8_t pin) = 0; +}; diff --git a/libraries/AP_HAL_Linux/GPIO_RPI_RP1.cpp b/libraries/AP_HAL_Linux/GPIO_RPI_RP1.cpp new file mode 100644 index 0000000000..bef2897ba4 --- /dev/null +++ b/libraries/AP_HAL_Linux/GPIO_RPI_RP1.cpp @@ -0,0 +1,178 @@ +#include + +#include "GPIO_RPI_RP1.h" + +#include +#include +#include +#include +#include +#include +#include + +using namespace Linux; + +using namespace AP_HAL; + +GPIO_RPI_RP1::GPIO_RPI_RP1() { +} + +bool GPIO_RPI_RP1::openMemoryDevice() { + _system_memory_device = open(PATH_DEV_GPIOMEM, O_RDWR | O_SYNC | O_CLOEXEC); + if (_system_memory_device < 0) { + AP_HAL::panic("Can't open %s", PATH_DEV_GPIOMEM); + return false; + } + return true; +} + +void GPIO_RPI_RP1::closeMemoryDevice() { + close(_system_memory_device); + _system_memory_device = -1; +} + +volatile uint32_t* GPIO_RPI_RP1::get_memory_pointer(uint32_t address, uint32_t length) const { + auto pointer = mmap( + nullptr, + length, + PROT_READ | PROT_WRITE | PROT_EXEC, + MAP_SHARED | MAP_LOCKED, // Shared with other processes + _system_memory_device, // File to map + address // Offset to GPIO peripheral + ); + + if (pointer == MAP_FAILED) { + return nullptr; + } + + return static_cast(pointer); +} + +void GPIO_RPI_RP1::init() { + if (!openMemoryDevice()) { + AP_HAL::panic("Failed to initialize memory device."); + return; + } + + _gpio = get_memory_pointer(IO_BANK0_OFFSET, MEM_SIZE); + if (!_gpio) { + AP_HAL::panic("Failed to get GPIO memory map."); + } + + closeMemoryDevice(); +} + +uint32_t GPIO_RPI_RP1::read_register(uint32_t offset) const { + return _gpio[offset]; +} + +void GPIO_RPI_RP1::write_register(uint32_t offset, uint32_t value) { + _gpio[offset] = value; +} + +GPIO_RPI_RP1::Mode GPIO_RPI_RP1::direction(uint8_t pin) const { + uint32_t offset = (SYS_RIO0_OFFSET + RIO_OE) / REG_SIZE; + uint32_t value = (read_register(offset) >> pin) & 0b1; + return value > 0 ? Mode::Output : Mode::Input; +} + +void GPIO_RPI_RP1::set_direction(uint8_t pin, Mode mode) { + uint32_t offset = (SYS_RIO0_OFFSET + RIO_OE) / REG_SIZE; + offset += (mode == Mode::Output ? SET_OFFSET : CLR_OFFSET) / REG_SIZE; + write_register(offset, 1 << pin); +} + +void GPIO_RPI_RP1::input_enable(uint8_t pin) { + uint32_t offset = (PADS_BANK0_OFFSET + PADS_GPIO + pin * PADS_OFFSET + SET_OFFSET) / REG_SIZE; + write_register(offset, PADS_IN_ENABLE_MASK); +} + +void GPIO_RPI_RP1::input_disable(uint8_t pin) { + uint32_t offset = (PADS_BANK0_OFFSET + PADS_GPIO + pin * PADS_OFFSET + CLR_OFFSET) / REG_SIZE; + write_register(offset, PADS_IN_ENABLE_MASK); +} + +void GPIO_RPI_RP1::output_enable(uint8_t pin) { + uint32_t offset = (PADS_BANK0_OFFSET + PADS_GPIO + pin * PADS_OFFSET + CLR_OFFSET) / REG_SIZE; + write_register(offset, PADS_OUT_DISABLE_MASK); +} + +void GPIO_RPI_RP1::output_disable(uint8_t pin) { + uint32_t offset = (PADS_BANK0_OFFSET + PADS_GPIO + pin * PADS_OFFSET + SET_OFFSET) / REG_SIZE; + write_register(offset, PADS_OUT_DISABLE_MASK); +} + +void GPIO_RPI_RP1::pinMode(uint8_t pin, uint8_t mode) { + if (mode == HAL_GPIO_INPUT) { + input_enable(pin); + set_direction(pin, Mode::Input); + set_mode(pin, Mode::Input); + } else if (mode == HAL_GPIO_OUTPUT) { + output_enable(pin); + set_direction(pin, Mode::Output); + set_mode(pin, Mode::Input); + } +} + +void GPIO_RPI_RP1::pinMode(uint8_t pin, uint8_t mode, uint8_t alt) { + if (mode == HAL_GPIO_ALT) { + set_mode(pin, static_cast(alt)); + return; + } + pinMode(pin, mode); +} + +void GPIO_RPI_RP1::set_mode(uint8_t pin, Mode mode) { + FunctionSelect alt_value; + + switch (mode) { + // ALT5 is used for GPIO, check datasheet pinout alternative functions + case Mode::Alt5: + case Mode::Input: + case Mode::Output: + alt_value = FunctionSelect::Alt5; + break; + case Mode::Alt0: alt_value = FunctionSelect::Alt0; break; + case Mode::Alt1: alt_value = FunctionSelect::Alt1; break; + case Mode::Alt2: alt_value = FunctionSelect::Alt2; break; + case Mode::Alt3: alt_value = FunctionSelect::Alt3; break; + case Mode::Alt4: alt_value = FunctionSelect::Alt4; break; + case Mode::Alt6: alt_value = FunctionSelect::Alt6; break; + case Mode::Alt7: alt_value = FunctionSelect::Alt7; break; + case Mode::Alt8: alt_value = FunctionSelect::Alt8; break; + default: alt_value = FunctionSelect::Null; break; + } + + uint32_t ctrl_offset = (IO_BANK0_OFFSET + GPIO_CTRL + (pin * GPIO_OFFSET) + RW_OFFSET) / REG_SIZE; + uint32_t reg_val = read_register(ctrl_offset); + reg_val &= ~CTRL_FUNCSEL_MASK; // Clear existing function select bits + reg_val |= (static_cast(alt_value) << CTRL_FUNCSEL_LSB); + write_register(ctrl_offset, reg_val); + +} + +void GPIO_RPI_RP1::set_pull(uint8_t pin, PadsPull mode) { + uint32_t pads_offset = (PADS_BANK0_OFFSET + PADS_GPIO + (pin * PADS_OFFSET) + RW_OFFSET) / REG_SIZE; + uint32_t reg_val = read_register(pads_offset); + reg_val &= ~PADS_PULL_MASK; // Clear existing bias bits + reg_val |= (static_cast(mode) << PADS_PULL_LSB); + write_register(pads_offset, reg_val); +} + +uint8_t GPIO_RPI_RP1::read(uint8_t pin) { + uint32_t in_offset = (SYS_RIO0_OFFSET + RIO_IN) / REG_SIZE; + uint32_t reg_val = read_register(in_offset); + return (reg_val >> pin) & 0x1; +} + +void GPIO_RPI_RP1::write(uint8_t pin, uint8_t value) { + uint32_t register_offset = value ? SET_OFFSET : CLR_OFFSET; + uint32_t offset = (SYS_RIO0_OFFSET + RIO_OUT + register_offset) / REG_SIZE; + write_register(offset, 1 << pin); +} + +void GPIO_RPI_RP1::toggle(uint8_t pin) { + uint32_t flag = (1 << pin); + _gpio_output_port_status ^= flag; + write(pin, (_gpio_output_port_status & flag) >> pin); +} diff --git a/libraries/AP_HAL_Linux/GPIO_RPI_RP1.h b/libraries/AP_HAL_Linux/GPIO_RPI_RP1.h new file mode 100644 index 0000000000..8ea1f342fd --- /dev/null +++ b/libraries/AP_HAL_Linux/GPIO_RPI_RP1.h @@ -0,0 +1,169 @@ +#pragma once + +#include +#include "GPIO_RPI_HAL.h" + +namespace Linux { + +/** + * @brief Class for Raspberry PI 5 GPIO control + * + * For more information: + * - RP1 datasheet: https://datasheets.raspberrypi.com/rp1/rp1-peripherals.pdf + * - gpiomem0: https://github.com/raspberrypi/linux/blob/1e53604087930e7cf42eee3d42572d0d6f54c86a/arch/arm/boot/dts/broadcom/bcm2712-rpi.dtsi#L178 + * - Address: 0x400d'0000, Size: 0x3'0000 + * + */ +class GPIO_RPI_RP1 : public GPIO_RPI_HAL { +public: + GPIO_RPI_RP1(); + void init() override; + void pinMode(uint8_t pin, uint8_t mode) override; + void pinMode(uint8_t pin, uint8_t mode, uint8_t alt) override; + + uint8_t read(uint8_t pin) override; + void write(uint8_t pin, uint8_t value) override; + void toggle(uint8_t pin) override; + + enum class PadsPull : uint8_t { + Off = 0, + Down = 1, + Up = 2, + }; + + void set_pull(uint8_t pin, PadsPull mode); + +private: + // gpiomem0 already maps the 0x400d'0000 address for us + static constexpr const char* PATH_DEV_GPIOMEM = "/dev/gpiomem0"; + static constexpr uint32_t MEM_SIZE = 0x30000; + static constexpr uint32_t REG_SIZE = sizeof(uint32_t); + + // Register offsets from RP1 datasheet 'Table 2. Peripheral Address Map' + // E.g: + // - IO_BANK0_OFFSET: 0x0'0000 result in 0x400d'0000 + // - SYS_RIO0_OFFSET: 0x1'0000 result in 0x400e'0000 + // ... + static constexpr uint32_t IO_BANK0_OFFSET = 0x00000; + static constexpr uint32_t SYS_RIO0_OFFSET = 0x10000; + static constexpr uint32_t PADS_BANK0_OFFSET = 0x20000; + + // GPIO control from https://github.com/raspberrypi/linux/blob/21012295fe87a7ccc1c356d1e268fd289aafbad1/drivers/pinctrl/pinctrl-rp1.c + static constexpr uint32_t RIO_OUT = 0x00; + static constexpr uint32_t RIO_OE = 0x04; + static constexpr uint32_t RIO_IN = 0x08; + + // GPIO control from '2.4. Atomic register access' + static constexpr uint32_t RW_OFFSET = 0x0000; + static constexpr uint32_t XOR_OFFSET = 0x1000; + static constexpr uint32_t SET_OFFSET = 0x2000; + static constexpr uint32_t CLR_OFFSET = 0x3000; + + static constexpr uint32_t GPIO_CTRL = 0x0004; + static constexpr uint32_t GPIO_OFFSET = 8; + + static constexpr uint32_t PADS_GPIO = 0x04; + static constexpr uint32_t PADS_OFFSET = 4; + + /** + * GPIO control from 'Table 8. GPI0_CTRL, GPI1_CTRL, ...' + * + * 0b0000'0000'0000'0000'0000'0000'0001'1101 + * ├┘│├─┘├┘├─┘├┘├─┘├┘├─┘├┘│ ├──────┘└────┴─ Bits 4:0 FUNCSEL: Function select + * │ ││ │ │ │ │ │ │ │ │ └────────────── Bits 11:5 F_M: Filter/debounce time constant M + * │ ││ │ │ │ │ │ │ │ └──────────────── Bits 13:12 OUTOVER: Output control + * │ ││ │ │ │ │ │ │ └────────────────── Bits 15:14 OEOVER: Output enable control + * │ ││ │ │ │ │ │ └───────────────────── Bits 17:16 INOVER: Input control + * │ ││ │ │ │ │ └─────────────────────── Bits 19:18 Reserved + * │ ││ │ │ │ └────────────────────────── Bits 20:21 IRQMASK_EDGE_LOW/HIGH + * │ ││ │ │ └──────────────────────────── Bits 22:23 IRQMASK_LEVEL_LOW/HIGH + * │ ││ │ └─────────────────────────────── Bits 24:25 IRQMASK_F_EDGE_LOW/HIGH + * │ ││ └───────────────────────────────── Bits 26:27 IRQMASK_DB_LEVEL_LOW/HIGH + * │ │└──────────────────────────────────── Bit 28 IRQRESET: Interrupt edge detector reset + * │ └───────────────────────────────────── Bit 29 Reserved + * └─────────────────────────────────────── Bits 31:30 IRQOVER: Interrupt control + */ + static constexpr uint32_t CTRL_FUNCSEL_MASK = 0x001f; + static constexpr uint32_t CTRL_FUNCSEL_LSB = 0; + static constexpr uint32_t CTRL_OUTOVER_MASK = 0x3000; + static constexpr uint32_t CTRL_OUTOVER_LSB = 12; + static constexpr uint32_t CTRL_OEOVER_MASK = 0xc000; + static constexpr uint32_t CTRL_OEOVER_LSB = 14; + static constexpr uint32_t CTRL_INOVER_MASK = 0x30000; + static constexpr uint32_t CTRL_INOVER_LSB = 16; + static constexpr uint32_t CTRL_IRQOVER_MASK = 0xc0000000; + static constexpr uint32_t CTRL_IRQOVER_LSB = 30; + + /** + * Mask for PADS_BANK control from 'Table 21.' + * + * 0b0...001'1101 + * ├──┘││├─┘││└─ Bit 1 SLEWFAST: Slew rate control. 1 = Fast, 0 = Slow + * │ │││ │└── Bit 2 SCHMITT: Enable schmitt trigger + * │ │││ └─── Bit 3 PDE: Pull down enable + * │ ││└────── Bits 4:5 DRIVE: Drive strength + * │ │└─────── Bit 6 IE: Input enable + * │ └──────── Bit 7 OD: Output disable. Has priority over output enable from + * └──────────── Bits 31:8 Reserved + */ + static constexpr uint32_t PADS_IN_ENABLE_MASK = 0x40; + static constexpr uint32_t PADS_OUT_DISABLE_MASK = 0x80; + static constexpr uint32_t PADS_PULL_MASK = 0x0c; + static constexpr uint32_t PADS_PULL_LSB = 2; + + enum class FunctionSelect : uint8_t { + Alt0 = 0, + Alt1 = 1, + Alt2 = 2, + Alt3 = 3, + Alt4 = 4, + Alt5 = 5, + Alt6 = 6, + Alt7 = 7, + Alt8 = 8, + Null = 31 + }; + + enum Mode { + Input, + Output, + Alt0, + Alt1, + Alt2, + Alt3, + Alt4, + Alt5, + Alt6, + Alt7, + Alt8, + Null + }; + + enum Bias { + Off, + PullDown, + PullUp + }; + + volatile uint32_t* _gpio; + int _system_memory_device; + uint32_t _gpio_output_port_status = 0; + + bool openMemoryDevice(); + void closeMemoryDevice(); + volatile uint32_t* get_memory_pointer(uint32_t address, uint32_t range) const; + + uint32_t read_register(uint32_t offset) const; + void write_register(uint32_t offset, uint32_t value); + + Mode direction(uint8_t pin) const; + void set_direction(uint8_t pin, Mode mode); + void input_enable(uint8_t pin); + void input_disable(uint8_t pin); + void output_enable(uint8_t pin); + void output_disable(uint8_t pin); + + void set_mode(uint8_t pin, Mode mode); +}; + +}