mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_Linux: Add support to Raspberry Pi 5
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 <patrickelectric@gmail.com>
This commit is contained in:
Patrick José Pereira
Willian Galvani
parent
3f451b9f4c
commit
6016e411e0
|
|
@ -8,237 +8,64 @@
|
||||||
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 || \
|
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 || \
|
||||||
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO
|
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO
|
||||||
|
|
||||||
#include <assert.h>
|
|
||||||
#include <errno.h>
|
|
||||||
#include <fcntl.h>
|
|
||||||
#include <poll.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <sys/mman.h>
|
|
||||||
#include <sys/stat.h>
|
|
||||||
#include <unistd.h>
|
|
||||||
|
|
||||||
#include "GPIO.h"
|
#include "GPIO.h"
|
||||||
|
#include "GPIO_RPI.h"
|
||||||
|
#include "GPIO_RPI_BCM.h"
|
||||||
|
#include "GPIO_RPI_RP1.h"
|
||||||
#include "Util_RPI.h"
|
#include "Util_RPI.h"
|
||||||
|
|
||||||
#define GPIO_RPI_MAX_NUMBER_PINS 32
|
|
||||||
|
|
||||||
using namespace Linux;
|
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
// Range based in the first memory address of the first register and the last memory addres
|
using namespace Linux;
|
||||||
// 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";
|
|
||||||
|
|
||||||
GPIO_RPI::GPIO_RPI()
|
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<uint32_t>(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<<tree_bits_position_in_register);
|
|
||||||
// Apply mask
|
|
||||||
_gpio[pin / pins_per_register] &= mask;
|
|
||||||
}
|
|
||||||
|
|
||||||
void GPIO_RPI::set_gpio_mode_out(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 to enable the bit that sets output functionality
|
|
||||||
// 0b00'000'000'000'000'000'000'001'000'000'000 enables output for the 4th pin
|
|
||||||
const uint32_t mask_with_bit = 0b001 << 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_bit;
|
|
||||||
}
|
|
||||||
|
|
||||||
void GPIO_RPI::set_gpio_high(int pin)
|
|
||||||
{
|
|
||||||
// Calculate index of the array for the register GPSET0 (0x7E20'001C)
|
|
||||||
constexpr uint32_t gpset0_memory_offset_value = 0x1c;
|
|
||||||
constexpr uint32_t gpset0_index_value = gpset0_memory_offset_value / sizeof(*_gpio);
|
|
||||||
_gpio[gpset0_index_value] = 1 << pin;
|
|
||||||
}
|
|
||||||
|
|
||||||
void GPIO_RPI::set_gpio_low(int pin)
|
|
||||||
{
|
|
||||||
// Calculate index of the array for the register GPCLR0 (0x7E20'0028)
|
|
||||||
constexpr uint32_t gpclr0_memory_offset_value = 0x28;
|
|
||||||
constexpr uint32_t gpclr0_index_value = gpclr0_memory_offset_value / sizeof(*_gpio);
|
|
||||||
_gpio[gpclr0_index_value] = 1 << pin;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool GPIO_RPI::get_gpio_logic_state(int pin)
|
|
||||||
{
|
|
||||||
// Calculate index of the array for the register GPLEV0 (0x7E20'0034)
|
|
||||||
constexpr uint32_t gplev0_memory_offset_value = 0x34;
|
|
||||||
constexpr uint32_t gplev0_index_value = gplev0_memory_offset_value / sizeof(*_gpio);
|
|
||||||
return _gpio[gplev0_index_value] & (1 << pin);
|
|
||||||
}
|
|
||||||
|
|
||||||
uint32_t GPIO_RPI::get_address(GPIO_RPI::Address address, GPIO_RPI::PeripheralOffset offset) const
|
|
||||||
{
|
|
||||||
return static_cast<uint32_t>(address) + static_cast<uint32_t>(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<volatile uint32_t*>(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()
|
void GPIO_RPI::init()
|
||||||
{
|
{
|
||||||
const LINUX_BOARD_TYPE rpi_version = UtilRPI::from(hal.util)->detect_linux_board_type();
|
const LINUX_BOARD_TYPE rpi_version = UtilRPI::from(hal.util)->detect_linux_board_type();
|
||||||
|
|
||||||
GPIO_RPI::Address peripheral_base;
|
switch (rpi_version) {
|
||||||
if(rpi_version == LINUX_BOARD_TYPE::RPI_ZERO_1) {
|
case LINUX_BOARD_TYPE::RPI_ZERO_1:
|
||||||
peripheral_base = Address::BCM2708_PERIPHERAL_BASE;
|
case LINUX_BOARD_TYPE::RPI_2_3_ZERO2:
|
||||||
} else if (rpi_version == LINUX_BOARD_TYPE::RPI_2_3_ZERO2) {
|
case LINUX_BOARD_TYPE::RPI_4:
|
||||||
peripheral_base = Address::BCM2709_PERIPHERAL_BASE;
|
gpioDriver = new GPIO_RPI_BCM();
|
||||||
} else if (rpi_version == LINUX_BOARD_TYPE::RPI_4) {
|
gpioDriver->init();
|
||||||
peripheral_base = Address::BCM2711_PERIPHERAL_BASE;
|
break;
|
||||||
} else {
|
case LINUX_BOARD_TYPE::RPI_5:
|
||||||
AP_HAL::panic("Unknown rpi_version, cannot locate peripheral base address");
|
gpioDriver = new GPIO_RPI_RP1();
|
||||||
return;
|
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)
|
void GPIO_RPI::pinMode(uint8_t pin, uint8_t output)
|
||||||
{
|
{
|
||||||
if (output == HAL_GPIO_INPUT) {
|
gpioDriver->pinMode(pin, output);
|
||||||
set_gpio_mode_in(pin);
|
|
||||||
} else {
|
|
||||||
set_gpio_mode_in(pin);
|
|
||||||
set_gpio_mode_out(pin);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void GPIO_RPI::pinMode(uint8_t pin, uint8_t output, uint8_t alt)
|
void GPIO_RPI::pinMode(uint8_t pin, uint8_t output, uint8_t alt)
|
||||||
{
|
{
|
||||||
if (output == HAL_GPIO_INPUT) {
|
gpioDriver->pinMode(pin, output, alt);
|
||||||
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::read(uint8_t pin)
|
uint8_t GPIO_RPI::read(uint8_t pin)
|
||||||
{
|
{
|
||||||
if (pin >= GPIO_RPI_MAX_NUMBER_PINS) {
|
return gpioDriver->read(pin);
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
return static_cast<uint8_t>(get_gpio_logic_state(pin));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void GPIO_RPI::write(uint8_t pin, uint8_t value)
|
void GPIO_RPI::write(uint8_t pin, uint8_t value)
|
||||||
{
|
{
|
||||||
if (value != 0) {
|
gpioDriver->write(pin, value);
|
||||||
set_gpio_high(pin);
|
|
||||||
} else {
|
|
||||||
set_gpio_low(pin);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void GPIO_RPI::toggle(uint8_t pin)
|
void GPIO_RPI::toggle(uint8_t pin)
|
||||||
{
|
{
|
||||||
if (pin >= GPIO_RPI_MAX_NUMBER_PINS) {
|
gpioDriver->toggle(pin);
|
||||||
return ;
|
|
||||||
}
|
|
||||||
uint32_t flag = (1 << pin);
|
|
||||||
_gpio_output_port_status ^= flag;
|
|
||||||
write(pin, (_gpio_output_port_status & flag) >> pin);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Alternative interface: */
|
/* Alternative interface: */
|
||||||
|
|
|
||||||
|
|
@ -2,6 +2,7 @@
|
||||||
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include "AP_HAL_Linux.h"
|
#include "AP_HAL_Linux.h"
|
||||||
|
#include "GPIO_RPI_HAL.h"
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Check for valid Raspberry Pi pin range
|
* @brief Check for valid Raspberry Pi pin range
|
||||||
|
|
@ -15,13 +16,12 @@ template <uint8_t pin> constexpr uint8_t RPI_GPIO_()
|
||||||
return pin;
|
return pin;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
namespace Linux {
|
namespace Linux {
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Class for Raspberry PI GPIO control
|
* @brief Class for Raspberry PI GPIO control
|
||||||
*
|
*
|
||||||
* For more information: https://elinux.org/RPi_BCM2835_GPIOs
|
|
||||||
*
|
|
||||||
*/
|
*/
|
||||||
class GPIO_RPI : public AP_HAL::GPIO {
|
class GPIO_RPI : public AP_HAL::GPIO {
|
||||||
public:
|
public:
|
||||||
|
|
@ -40,161 +40,7 @@ public:
|
||||||
bool usb_connected(void) override;
|
bool usb_connected(void) override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// Raspberry Pi BASE memory address
|
GPIO_RPI_HAL* gpioDriver;
|
||||||
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;
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,241 @@
|
||||||
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <poll.h>
|
||||||
|
#include <sys/mman.h>
|
||||||
|
#include <sys/stat.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
|
||||||
|
#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<uint32_t>(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<<tree_bits_position_in_register);
|
||||||
|
// Apply mask
|
||||||
|
_gpio[pin / pins_per_register] &= mask;
|
||||||
|
}
|
||||||
|
|
||||||
|
void GPIO_RPI_BCM::set_gpio_mode_out(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 to enable the bit that sets output functionality
|
||||||
|
// 0b00'000'000'000'000'000'000'001'000'000'000 enables output for the 4th pin
|
||||||
|
const uint32_t mask_with_bit = 0b001 << 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_bit;
|
||||||
|
}
|
||||||
|
|
||||||
|
void GPIO_RPI_BCM::set_gpio_high(int pin)
|
||||||
|
{
|
||||||
|
// Calculate index of the array for the register GPSET0 (0x7E20'001C)
|
||||||
|
constexpr uint32_t gpset0_memory_offset_value = 0x1c;
|
||||||
|
constexpr uint32_t gpset0_index_value = gpset0_memory_offset_value / sizeof(*_gpio);
|
||||||
|
_gpio[gpset0_index_value] = 1 << pin;
|
||||||
|
}
|
||||||
|
|
||||||
|
void GPIO_RPI_BCM::set_gpio_low(int pin)
|
||||||
|
{
|
||||||
|
// Calculate index of the array for the register GPCLR0 (0x7E20'0028)
|
||||||
|
constexpr uint32_t gpclr0_memory_offset_value = 0x28;
|
||||||
|
constexpr uint32_t gpclr0_index_value = gpclr0_memory_offset_value / sizeof(*_gpio);
|
||||||
|
_gpio[gpclr0_index_value] = 1 << pin;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool GPIO_RPI_BCM::get_gpio_logic_state(int pin)
|
||||||
|
{
|
||||||
|
// Calculate index of the array for the register GPLEV0 (0x7E20'0034)
|
||||||
|
constexpr uint32_t gplev0_memory_offset_value = 0x34;
|
||||||
|
constexpr uint32_t gplev0_index_value = gplev0_memory_offset_value / sizeof(*_gpio);
|
||||||
|
return _gpio[gplev0_index_value] & (1 << pin);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t GPIO_RPI_BCM::get_address(GPIO_RPI_BCM::Address address, GPIO_RPI_BCM::PeripheralOffset offset) const
|
||||||
|
{
|
||||||
|
return static_cast<uint32_t>(address) + static_cast<uint32_t>(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<volatile uint32_t*>(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<uint8_t>(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;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,188 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#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;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
|
|
@ -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;
|
||||||
|
};
|
||||||
|
|
@ -0,0 +1,178 @@
|
||||||
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
|
#include "GPIO_RPI_RP1.h"
|
||||||
|
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <poll.h>
|
||||||
|
#include <cstdlib>
|
||||||
|
#include <cstring>
|
||||||
|
#include <sys/mman.h>
|
||||||
|
#include <sys/stat.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
|
||||||
|
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<volatile uint32_t*>(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<Mode>(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<uint32_t>(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<uint8_t>(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);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,169 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <cstdint>
|
||||||
|
#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);
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue