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_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_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<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()
|
||||
{
|
||||
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<uint8_t>(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: */
|
||||
|
|
|
|||
|
|
@ -2,6 +2,7 @@
|
|||
|
||||
#include <stdint.h>
|
||||
#include "AP_HAL_Linux.h"
|
||||
#include "GPIO_RPI_HAL.h"
|
||||
|
||||
/**
|
||||
* @brief Check for valid Raspberry Pi pin range
|
||||
|
|
@ -15,13 +16,12 @@ template <uint8_t pin> 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;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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