AP_HAL_Linux: Organize and document GPIO_RPI class

- Move macros to functions and improve explanation about logic behind it
- Break code in functions to make it more readable
- Add doxygen comments about functions and how they work in a comprehensive manner
- Improve general code maintainability

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>

libraries/AP_HAL_Linux/GPIO_RPI.cpp

@@ -6,6 +6,7 @@
     CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \
     CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR
 
+#include <assert.h>
 #include <errno.h>
 #include <fcntl.h>
 #include <poll.h>
@@ -19,85 +20,193 @@
 #include "GPIO.h"
 #include "Util_RPI.h"
 
-// Raspberry Pi GPIO memory
-#define BCM2708_PERI_BASE   0x20000000
-#define BCM2709_PERI_BASE   0x3F000000
-#define BCM2711_PERI_BASE   0xFE000000
-#define GPIO_BASE(address)  (address + 0x200000)
-
-// GPIO setup. Always use INP_GPIO(x) before OUT_GPIO(x) or SET_GPIO_ALT(x,y)
-#define GPIO_MODE_IN(g)     *(_gpio+((g)/10)) &= ~(7<<(((g)%10)*3))
-#define GPIO_MODE_OUT(g)    *(_gpio+((g)/10)) |=  (1<<(((g)%10)*3))
-#define GPIO_MODE_ALT(g,a)  *(_gpio+(((g)/10))) |= (((a)<=3?(a)+4:(a)==4?3:2)<<(((g)%10)*3))
-#define GPIO_SET_HIGH       *(_gpio+7)  // sets   bits which are 1
-#define GPIO_SET_LOW        *(_gpio+10) // clears bits which are 1
-#define GPIO_GET(g)         (*(_gpio+13)&(1<<g)) // 0 if LOW, (1<<g) if HIGH
 #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";
+
 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()
 {
-    int rpi_version = UtilRPI::from(hal.util)->get_rpi_version();
-    uint32_t gpio_address;
+    const int rpi_version = UtilRPI::from(hal.util)->get_rpi_version();
+
+    GPIO_RPI::Address peripheral_base;
     if(rpi_version == 1) {
-        gpio_address = GPIO_BASE(BCM2708_PERI_BASE);
+        peripheral_base = Address::BCM2708_PERIPHERAL_BASE;
     } else if (rpi_version == 2) {
-        gpio_address = GPIO_BASE(BCM2709_PERI_BASE);
+        peripheral_base = Address::BCM2709_PERIPHERAL_BASE;
     } else {
-        gpio_address = GPIO_BASE(BCM2711_PERI_BASE);
+        peripheral_base = Address::BCM2711_PERIPHERAL_BASE;
     }
 
-    int mem_fd = open("/dev/mem", O_RDWR|O_SYNC|O_CLOEXEC);
-    if (mem_fd < 0) {
-        AP_HAL::panic("Can't open /dev/mem");
+    if (!openMemoryDevice()) {
+        AP_HAL::panic("Failed to initialize memory device.");
+        return;
     }
 
-    // mmap GPIO
-    void *gpio_map = mmap(
-        nullptr,              // Any adddress in our space will do
-        BLOCK_SIZE,           // Map length
-        PROT_READ|PROT_WRITE, // Enable reading & writting to mapped memory
-        MAP_SHARED,           // Shared with other processes
-        mem_fd,               // File to map
-        gpio_address          // Offset to GPIO peripheral
-    );
-
-    close(mem_fd); // No need to keep mem_fd open after mmap
+    const uint32_t gpio_address = get_address(peripheral_base, PeripheralOffset::GPIO);
 
-    if (gpio_map == MAP_FAILED) {
-        AP_HAL::panic("Can't open /dev/mem");
+    _gpio = get_memory_pointer(gpio_address, _gpio_registers_memory_range);
+    if (!_gpio) {
+        AP_HAL::panic("Failed to get GPIO memory map.");
     }
 
-    _gpio = (volatile uint32_t *)gpio_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) {
-        GPIO_MODE_IN(pin);
+        set_gpio_mode_in(pin);
     } else {
-        GPIO_MODE_IN(pin);
-        GPIO_MODE_OUT(pin);
+        set_gpio_mode_in(pin);
+        set_gpio_mode_out(pin);
     }
 }
 
 void GPIO_RPI::pinMode(uint8_t pin, uint8_t output, uint8_t alt)
 {
+    assert(alt < 6);
     if (output == HAL_GPIO_INPUT) {
-        GPIO_MODE_IN(pin);
+        set_gpio_mode_in(pin);
     } else if (output == HAL_GPIO_ALT) {
-        GPIO_MODE_IN(pin);
-        GPIO_MODE_ALT(pin, alt);
+        set_gpio_mode_in(pin);
+        set_gpio_mode_alt(pin, alt);
     } else {
-        GPIO_MODE_IN(pin);
-        GPIO_MODE_OUT(pin);
+        set_gpio_mode_in(pin);
+        set_gpio_mode_out(pin);
     }
 }
 
@@ -106,16 +215,15 @@ uint8_t GPIO_RPI::read(uint8_t pin)
     if (pin >= GPIO_RPI_MAX_NUMBER_PINS) {
         return 0;
     }
-    uint32_t value = GPIO_GET(pin);
-    return value ? 1: 0;
+    return static_cast<uint8_t>(get_gpio_logic_state(pin));
 }
 
 void GPIO_RPI::write(uint8_t pin, uint8_t value)
 {
-    if (value == LOW) {
-        GPIO_SET_LOW = 1 << pin;
+    if (value != 0) {
+        set_gpio_high(pin);
     } else {
-        GPIO_SET_HIGH = 1 << pin;
+        set_gpio_low(pin);
     }
 }
 

libraries/AP_HAL_Linux/GPIO_RPI.h

@@ -3,12 +3,6 @@
 #include <stdint.h>
 #include "AP_HAL_Linux.h"
 
-#define LOW                 0
-#define HIGH                1
-
-#define PAGE_SIZE           (4*1024)
-#define BLOCK_SIZE          (4*1024)
-
 /**
  * @brief Check for valid Raspberry Pi pin range
  *
@@ -23,6 +17,12 @@ template <uint8_t pin> constexpr uint8_t RPI_GPIO_()
 
 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:
     GPIO_RPI();
@@ -40,7 +40,158 @@ public:
     bool    usb_connected(void) override;
 
 private:
-    volatile uint32_t *_gpio;
+    // 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;
 };
 
 }