diff --git a/src/platform/PICO/bus_i2c_pico.c b/src/platform/PICO/bus_i2c_pico.c new file mode 100644 index 0000000000..d96d9eac5c --- /dev/null +++ b/src/platform/PICO/bus_i2c_pico.c @@ -0,0 +1,210 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#if defined(USE_I2C) && !defined(SOFT_I2C) + +#include "hardware/i2c.h" +#include "hardware/dma.h" +#include "hardware/irq.h" + +#include "drivers/bus_i2c.h" +#include "drivers/bus_i2c_impl.h" +#include "drivers/io.h" +#include "drivers/io_impl.h" + +#define I2C_TX_BUFFER_LENGTH 32 + +static volatile uint16_t i2cErrorCount = 0; + +i2cDevice_t i2cDevice[I2CDEV_COUNT]; + +const i2cHardware_t i2cHardware[I2CDEV_COUNT] = { +#ifdef USE_I2C_DEVICE_0 + { + .device = I2CDEV_0, + .reg = I2C0, + .sclPins = { + { DEFIO_TAG_E(PA1) }, + { DEFIO_TAG_E(PA5) }, + { DEFIO_TAG_E(PA9) }, + { DEFIO_TAG_E(PA13) }, + }, + .sdaPins = { + { DEFIO_TAG_E(PA0) }, + { DEFIO_TAG_E(PA4) }, + { DEFIO_TAG_E(PA8) }, + { DEFIO_TAG_E(PA12) }, + }, + }, +#endif +#ifdef USE_I2C_DEVICE_1 + { + .device = I2CDEV_1, + .reg = I2C1, + .sclPins = { + { DEFIO_TAG_E(PA3) }, + { DEFIO_TAG_E(PA7) }, + { DEFIO_TAG_E(PA11) }, + { DEFIO_TAG_E(PA15) }, + }, + .sdaPins = { + { DEFIO_TAG_E(PA2) }, + { DEFIO_TAG_E(PA6) }, + { DEFIO_TAG_E(PA10) }, + { DEFIO_TAG_E(PA14) }, + } + }, +#endif +}; + +static bool i2cHandleHardwareFailure(I2CDevice device) +{ + UNUSED(device); + i2cErrorCount++; + return false; +} + +uint16_t i2cGetErrorCounter(void) +{ + return i2cErrorCount; +} + +bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data) +{ + return i2cWriteBuffer(device, addr_, reg_, 1, &data); +} + +bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) +{ + // TODO: Implement non-blocking write using DMA or similar mechanism + if (device == I2CINVALID || device >= I2CDEV_COUNT) { + return false; + } + + i2c_inst_t *port = I2C_INST(&i2cHardware[device].reg); + + if (!port) { + return false; + } + + if (len_ > I2C_TX_BUFFER_LENGTH - 1) { + return false; // Buffer too long + } + + uint8_t buf[I2C_TX_BUFFER_LENGTH] = { reg_, 0 }; + memcpy(&buf[1], data, len_); + int status = i2c_write_timeout_us(port, addr_ << 1, buf, len_ + 1, true, I2C_TIMEOUT_US); + + if (status < 0) { + return i2cHandleHardwareFailure(device); + } + + return true; +} + +bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) +{ + if (device == I2CINVALID || device >= I2CDEV_COUNT) { + return false; + } + + i2c_inst_t *port = I2C_INST(&i2cHardware[device].reg); + + if (!port) { + return false; + } + + int status = i2c_write_timeout_us(port, addr_ << 1, ®_, 1, true, I2C_TIMEOUT_US); + if (status < 0) { + return i2cHandleHardwareFailure(device); + } + + status = i2c_read_timeout_us(port, addr_ << 1, buf, len, true, I2C_TIMEOUT_US); + if (status < 0) { + return i2cHandleHardwareFailure(device); + } + + return true; +} + +bool i2cReadBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) +{ + // TODO: Implement genuine non-blocking read using DMA or similar mechanism + return i2cRead(device, addr_, reg_, len, buf); +} + +bool i2cBusy(I2CDevice device, bool *error) +{ + if (device == I2CINVALID || device >= I2CDEV_COUNT) { + return false; + } + + i2c_inst_t *port = I2C_INST(&i2cHardware[device].reg); + + if (!port) { + return false; + } + + if (error) { + *error = 0; + } + + // Read the IC_STATUS register + uint32_t status_reg = port->hw->status; + + // The bit for MST_ACTIVITY is (1 << 5). + return (status_reg & (1 << 5)) != 0; +} + +void i2cInit(I2CDevice device) +{ + if (device == I2CINVALID) { + return; + } + + i2cDevice_t *pDev = &i2cDevice[device]; + + const i2cHardware_t *hardware = pDev->hardware; + const IO_t scl = pDev->scl; + const IO_t sda = pDev->sda; + + if (!hardware || IOGetOwner(scl) || IOGetOwner(sda)) { + return; + } + + i2c_init(I2C_INST(hardware->reg), pDev->clockSpeed); + + // Set up GPIO pins for I2C + gpio_set_function(IO_Pin(sda), GPIO_FUNC_I2C); + gpio_set_function(IO_Pin(scl), GPIO_FUNC_I2C); + + // Enable internal pull-up resistors + gpio_pull_up(IO_Pin(sda)); + gpio_pull_up(IO_Pin(scl)); +} + +#endif diff --git a/src/platform/PICO/dma_pico.c b/src/platform/PICO/dma_pico.c index 1faf3f50b9..c75713cb78 100644 --- a/src/platform/PICO/dma_pico.c +++ b/src/platform/PICO/dma_pico.c @@ -24,13 +24,106 @@ #include #include "platform.h" -#include "drivers/dma.h" -#include "dma_pico.h" -dmaChannelDescriptor_t dmaDescriptors[DMA_HANDLER_END-1] = { - +#ifdef USE_DMA + +#include "drivers/dma.h" + +#include "platform/dma.h" +#include "pico/multicore.h" +#include "hardware/dma.h" +#include "hardware/irq.h" + +volatile bool dma_irq0_handler_registered = false; +volatile bool dma_irq1_handler_registered = false; + +dmaChannelDescriptor_t dmaDescriptors[DMA_LAST_HANDLER] = { + DEFINE_DMA_CHANNEL(DMA_CH0_HANDLER), + DEFINE_DMA_CHANNEL(DMA_CH1_HANDLER), + DEFINE_DMA_CHANNEL(DMA_CH2_HANDLER), + DEFINE_DMA_CHANNEL(DMA_CH3_HANDLER), + DEFINE_DMA_CHANNEL(DMA_CH4_HANDLER), + DEFINE_DMA_CHANNEL(DMA_CH5_HANDLER), + DEFINE_DMA_CHANNEL(DMA_CH6_HANDLER), + DEFINE_DMA_CHANNEL(DMA_CH7_HANDLER), + DEFINE_DMA_CHANNEL(DMA_CH8_HANDLER), + DEFINE_DMA_CHANNEL(DMA_CH9_HANDLER), + DEFINE_DMA_CHANNEL(DMA_CH10_HANDLER), + DEFINE_DMA_CHANNEL(DMA_CH11_HANDLER), +#ifdef RP2350 + DEFINE_DMA_CHANNEL(DMA_CH12_HANDLER), + DEFINE_DMA_CHANNEL(DMA_CH13_HANDLER), + DEFINE_DMA_CHANNEL(DMA_CH14_HANDLER), + DEFINE_DMA_CHANNEL(DMA_CH15_HANDLER), +#endif }; +#define DMA_CHANNEL_TO_INDEX(channel) ((channel) + 1) + +void dma_irq_handler(bool isIrq1) +{ + uint32_t status = isIrq1 ? dma_hw->ints1 : dma_hw->ints0; // Read the status register once + + // Iterate through all possible DMA channels that have triggered an interrupt + for (uint8_t channel = 0; channel < DMA_LAST_HANDLER; channel++) { + if (status & (1u << channel)) { + uint8_t index = DMA_CHANNEL_TO_INDEX(channel); + // Call the handler if it is set + if (dmaDescriptors[index].irqHandlerCallback) { + dmaDescriptors[index].irqHandlerCallback(&dmaDescriptors[index]); + } + + // Acknowledge the interrupt for this channel + if (isIrq1) { + dma_channel_acknowledge_irq1(channel); + } else { + dma_channel_acknowledge_irq0(channel); + } + } + } +} +void dma_irq0_handler(void) +{ + dma_irq_handler(false); +} + +void dma_irq1_handler(void) +{ + dma_irq_handler(true); +} + void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) { + UNUSED(priority); + /* + Assign the interrupt handler for the DMA channel based on the core + this is to minimise contention on the DMA IRQs. + + Each core has its own DMA IRQ: + - CORE 0 uses DMA_IRQ_0 + - CORE 1 uses DMA_IRQ_1 + + */ + uint8_t core = get_core_num(); + + if (core) { + // Core 1 uses DMA IRQ1 + if (!dma_irq1_handler_registered) { + irq_set_exclusive_handler(DMA_IRQ_1, dma_irq1_handler); + irq_set_enabled(DMA_IRQ_1, true); + dma_irq1_handler_registered = true; + } + } else { + // Core 0 uses DMA IRQ0 + if (!dma_irq0_handler_registered) { + irq_set_exclusive_handler(DMA_IRQ_0, dma_irq0_handler); + irq_set_enabled(DMA_IRQ_0, true); + dma_irq0_handler_registered = true; + } + } + + dmaDescriptors[identifier].irqHandlerCallback = callback; + dmaDescriptors[identifier].userParam = userParam; } + +#endif diff --git a/src/platform/PICO/include/platform/dma.h b/src/platform/PICO/include/platform/dma.h index dbe6eafd9d..225998b335 100644 --- a/src/platform/PICO/include/platform/dma.h +++ b/src/platform/PICO/include/platform/dma.h @@ -19,13 +19,14 @@ * If not, see . */ - #pragma once +#pragma once - #include "platform.h" +#include "platform.h" typedef enum { DMA_NONE = 0, - DMA_CH1_HANDLER = 1, + DMA_CH0_HANDLER = 1, + DMA_CH1_HANDLER, DMA_CH2_HANDLER, DMA_CH3_HANDLER, DMA_CH4_HANDLER, @@ -36,14 +37,31 @@ typedef enum { DMA_CH9_HANDLER, DMA_CH10_HANDLER, DMA_CH11_HANDLER, - DMA_CH12_HANDLER, #ifdef RP2350 + DMA_CH12_HANDLER, DMA_CH13_HANDLER, DMA_CH14_HANDLER, DMA_CH15_HANDLER, - DMA_CH16_HANDLER, - DMA_LAST_HANDLER = DMA_CH16_HANDLER + DMA_LAST_HANDLER = DMA_CH15_HANDLER #else - DMA_LAST_HANDLER = DMA_CH12_HANDLER + DMA_LAST_HANDLER = DMA_CH11_HANDLER #endif } dmaIdentifier_e; + +#define DMA_DEVICE_NO(x) (0) +#define DMA_DEVICE_INDEX(x) ((x)-1) +#define DMA_OUTPUT_INDEX 0 +#define DMA_OUTPUT_STRING "DMA%d Channel %d:" +#define DMA_INPUT_STRING "DMA%d_CH%d" + +#define DEFINE_DMA_CHANNEL(c) { \ + .dma = NULL, \ + .ref = NULL, \ + .channel = c-1, \ + .irqHandlerCallback = NULL, \ + .flagsShift = 0, \ + .irqN = 0, \ + .userParam = 0, \ + .owner.owner = 0, \ + .owner.resourceIndex = 0 \ + } diff --git a/src/platform/PICO/include/platform/platform.h b/src/platform/PICO/include/platform/platform.h index 0ac7dd9e1d..7b37c5a3ac 100644 --- a/src/platform/PICO/include/platform/platform.h +++ b/src/platform/PICO/include/platform/platform.h @@ -38,9 +38,8 @@ typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState; #define I2C_TypeDef I2C0_Type -#define I2C_INST(i2c) ((i2c_inst_t *)(i2c)) +#define I2C_INST(i2c) ((i2c_inst_t *)(i2c)) -//#define I2C_HandleTypeDef #define GPIO_TypeDef io_bank0_hw_t //#define GPIO_InitTypeDef #define TIM_TypeDef void* @@ -67,7 +66,7 @@ typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState; // which are defined in pico-sdk as SPI0_Type*. // SPI_INST converts to the correct type for use in pico-sdk functions. #define SPI_TypeDef SPI0_Type -#define SPI_INST(spi) ((spi_inst_t *)(spi)) +#define SPI_INST(spi) ((spi_inst_t *)(spi)) #endif @@ -80,12 +79,13 @@ typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState; #ifdef TEST_SLOW_SCHEDULE // (testing) allow time for more / all tasks -#define TASK_GYROPID_DESIRED_PERIOD 30000 // 1000 // 50000 // 125 // 125us = 8kHz +// 1000 // 50000 // 125 // 125us = 8kHz +#define TASK_GYROPID_DESIRED_PERIOD 30000 #else -#define TASK_GYROPID_DESIRED_PERIOD 125 // 125us = 8kHz +// 125us = 8kHz +#define TASK_GYROPID_DESIRED_PERIOD 125 #endif - #define SCHEDULER_DELAY_LIMIT 10 #define IO_CONFIG(mode, speed, pupd) ((mode) | ((speed) << 2) | ((pupd) << 5)) diff --git a/src/platform/PICO/mk/PICO.mk b/src/platform/PICO/mk/PICO.mk index 1ad08c7625..23b524cbca 100644 --- a/src/platform/PICO/mk/PICO.mk +++ b/src/platform/PICO/mk/PICO.mk @@ -376,10 +376,13 @@ MCU_COMMON_SRC = \ drivers/inverter.c \ drivers/bus_spi.c \ drivers/bus_spi_config.c \ + drivers/bus_i2c_config.c \ + drivers/bus_i2c_utils.c \ drivers/serial_pinconfig.c \ drivers/serial_uart_pinconfig.c \ drivers/usb_io.c \ drivers/dshot.c \ + PICO/dma_pico.c \ PICO/dshot_pico.c \ PICO/pwm_pico.c \ PICO/stdio_pico_stub.c \ @@ -387,6 +390,7 @@ MCU_COMMON_SRC = \ PICO/system.c \ PICO/io_pico.c \ PICO/bus_spi_pico.c \ + PICO/bus_i2c_pico.c \ PICO/serial_uart_pico.c \ PICO/exti_pico.c \ PICO/config_flash.c \ diff --git a/src/platform/PICO/target/RP2350A/target.h b/src/platform/PICO/target/RP2350A/target.h index ae7b31dcc4..a178a84e2c 100644 --- a/src/platform/PICO/target/RP2350A/target.h +++ b/src/platform/PICO/target/RP2350A/target.h @@ -56,6 +56,10 @@ #define USE_SPI_DEVICE_0 #define USE_SPI_DEVICE_1 +#define USE_I2C +#define USE_I2C_DEVICE_0 +#define USE_I2C_DEVICE_1 + // one of these ... // #define USE_SPI_DMA_ENABLE_EARLY #define USE_SPI_DMA_ENABLE_LATE @@ -71,7 +75,6 @@ #undef USE_SDCARD #undef USE_TIMER -#undef USE_I2C #undef USE_RCC #undef USE_CLI #undef USE_RX_PWM @@ -256,3 +259,5 @@ SPARE3 PA47 #define UARTHARDWARE_MAX_PINS 8 #define UART_TRAIT_AF_PORT 1 + +#define I2CDEV_COUNT 2 diff --git a/src/platform/PICO/target/RP2350B/target.h b/src/platform/PICO/target/RP2350B/target.h index fcce7d5c3b..697291a346 100644 --- a/src/platform/PICO/target/RP2350B/target.h +++ b/src/platform/PICO/target/RP2350B/target.h @@ -53,6 +53,10 @@ #define USE_SPI_DEVICE_0 #define USE_SPI_DEVICE_1 +#define USE_I2C +#define USE_I2C_DEVICE_0 +#define USE_I2C_DEVICE_1 + // one of these ... // #define USE_SPI_DMA_ENABLE_EARLY #define USE_SPI_DMA_ENABLE_LATE @@ -68,7 +72,6 @@ #undef USE_SDCARD #undef USE_TIMER -#undef USE_I2C #undef USE_RCC #undef USE_RX_PWM @@ -105,7 +108,7 @@ #undef USE_MSP_UART #undef USE_MSP_DISPLAYPORT -#undef USE_DMA +//#undef USE_DMA #undef USE_DMA_SPEC #undef USE_DSHOT_TELEMETRY @@ -210,7 +213,7 @@ #undef USE_ACCGYRO_BMI160 #undef USE_ACCGYRO_BMI270 #endif - + //#define USE_FLASH //#define USE_FLASH_W25Q128FV //#define USE_MAX7456 @@ -283,3 +286,5 @@ SPARE3 PA47 #define UARTHARDWARE_MAX_PINS 12 #define UART_TRAIT_AF_PORT 1 + +#define I2CDEV_COUNT 2