diff --git a/make/mcu/STM32H7.mk b/make/mcu/STM32H7.mk index 00eca8fe7..b97156fae 100644 --- a/make/mcu/STM32H7.mk +++ b/make/mcu/STM32H7.mk @@ -60,6 +60,7 @@ EXCLUDES = \ stm32h7xx_hal_smartcard_ex.c \ stm32h7xx_hal_smbus.c \ stm32h7xx_hal_spdifrx.c \ + stm32h7xx_hal_spi.c \ stm32h7xx_hal_sram.c \ stm32h7xx_hal_swpmi.c \ stm32h7xx_hal_usart.c \ @@ -85,7 +86,6 @@ EXCLUDES = \ stm32h7xx_ll_rcc.c \ stm32h7xx_ll_rng.c \ stm32h7xx_ll_rtc.c \ - stm32h7xx_ll_spi.c \ stm32h7xx_ll_swpmi.c \ stm32h7xx_ll_usart.c \ stm32h7xx_ll_utils.c @@ -282,7 +282,7 @@ MCU_COMMON_SRC = \ drivers/serial_uart_hal.c \ drivers/serial_uart_stm32h7xx.c \ drivers/bus_quadspi_hal.c \ - drivers/bus_spi_hal.c \ + drivers/bus_spi_ll.c \ drivers/dma_stm32h7xx.c \ drivers/dshot_bitbang.c \ drivers/dshot_bitbang_decode.c \ diff --git a/src/link/stm32_flash_f7_split.ld b/src/link/stm32_flash_f7_split.ld index 404a0cb5e..de56d239b 100644 --- a/src/link/stm32_flash_f7_split.ld +++ b/src/link/stm32_flash_f7_split.ld @@ -120,7 +120,7 @@ SECTIONS . = ALIGN(4); _edata = .; /* define a global symbol at data end */ - } >RAM AT >AXIM_FLASH + } >RAM AT >AXIM_FLASH1 /* Uninitialized data section */ . = ALIGN(4); @@ -166,7 +166,7 @@ SECTIONS . = ALIGN(4); _efastram_data = .; /* define a global symbol at data end */ - } >FASTRAM AT >AXIM_FLASH + } >FASTRAM AT >AXIM_FLASH1 . = ALIGN(4); .fastram_bss (NOLOAD) : diff --git a/src/link/stm32_flash_h723_1m.ld b/src/link/stm32_flash_h723_1m.ld index 518e60d45..a0ff46629 100644 --- a/src/link/stm32_flash_h723_1m.ld +++ b/src/link/stm32_flash_h723_1m.ld @@ -162,7 +162,7 @@ SECTIONS . = ALIGN(4); _edata = .; /* define a global symbol at data end */ - } >RAM AT >FLASH + } >FASTRAM AT >FLASH /* Uninitialized data section */ . = ALIGN(4); @@ -178,7 +178,7 @@ SECTIONS . = ALIGN(4); _ebss = .; /* define a global symbol at bss end */ __bss_end__ = _ebss; - } >RAM + } >FASTRAM /* Uninitialized data section */ . = ALIGN(4); @@ -223,17 +223,42 @@ SECTIONS __fastram_bss_end__ = _efastram_bss; } >FASTRAM - .DMA_RAM (NOLOAD) : + /* used during startup to initialized dmaram_data */ + _sdmaram_idata = LOADADDR(.dmaram_data); + + . = ALIGN(32); + .dmaram_data : { - . = ALIGN(32); PROVIDE(dmaram_start = .); _sdmaram = .; _dmaram_start__ = _sdmaram; + _sdmaram_data = .; /* create a global symbol at data start */ + *(.dmaram_data) /* .data sections */ + *(.dmaram_data*) /* .data* sections */ + . = ALIGN(32); + _edmaram_data = .; /* define a global symbol at data end */ + } >RAM AT >FLASH + + . = ALIGN(32); + .dmaram_bss (NOLOAD) : + { + _sdmaram_bss = .; + __dmaram_bss_start__ = _sdmaram_bss; + *(.dmaram_bss) + *(SORT_BY_ALIGNMENT(.dmaram_bss*)) + . = ALIGN(32); + _edmaram_bss = .; + __dmaram_bss_end__ = _edmaram_bss; + } >RAM + + . = ALIGN(32); + .DMA_RAM (NOLOAD) : + { KEEP(*(.DMA_RAM)) PROVIDE(dmaram_end = .); _edmaram = .; _dmaram_end__ = _edmaram; - } >D2_RAM + } >RAM .DMA_RW_D2 (NOLOAD) : { diff --git a/src/link/stm32_flash_h743_2m.ld b/src/link/stm32_flash_h743_2m.ld index ac434a579..47f8132b8 100644 --- a/src/link/stm32_flash_h743_2m.ld +++ b/src/link/stm32_flash_h743_2m.ld @@ -239,17 +239,42 @@ SECTIONS __fastram_bss_end__ = _efastram_bss; } >FASTRAM - .DMA_RAM (NOLOAD) : + /* used during startup to initialized dmaram_data */ + _sdmaram_idata = LOADADDR(.dmaram_data); + + . = ALIGN(32); + .dmaram_data : { - . = ALIGN(32); PROVIDE(dmaram_start = .); _sdmaram = .; _dmaram_start__ = _sdmaram; + _sdmaram_data = .; /* create a global symbol at data start */ + *(.dmaram_data) /* .data sections */ + *(.dmaram_data*) /* .data* sections */ + . = ALIGN(32); + _edmaram_data = .; /* define a global symbol at data end */ + } >RAM AT >FLASH + + . = ALIGN(32); + .dmaram_bss (NOLOAD) : + { + _sdmaram_bss = .; + __dmaram_bss_start__ = _sdmaram_bss; + *(.dmaram_bss) + *(SORT_BY_ALIGNMENT(.dmaram_bss*)) + . = ALIGN(32); + _edmaram_bss = .; + __dmaram_bss_end__ = _edmaram_bss; + } >RAM + + . = ALIGN(32); + .DMA_RAM (NOLOAD) : + { KEEP(*(.DMA_RAM)) PROVIDE(dmaram_end = .); _edmaram = .; _dmaram_end__ = _edmaram; - } >D2_RAM + } >RAM .DMA_RW_D2 (NOLOAD) : { diff --git a/src/link/stm32_flash_h750_128k.ld b/src/link/stm32_flash_h750_128k.ld index 111c39fc2..727da9f00 100644 --- a/src/link/stm32_flash_h750_128k.ld +++ b/src/link/stm32_flash_h750_128k.ld @@ -50,19 +50,46 @@ REGION_ALIAS("STACKRAM", DTCM_RAM) REGION_ALIAS("FASTRAM", DTCM_RAM) REGION_ALIAS("MAIN", FLASH) +INCLUDE "stm32_h750_common.ld" + SECTIONS { - .DMA_RAM (NOLOAD) : + /* used during startup to initialized dmaram_data */ + _sdmaram_idata = LOADADDR(.dmaram_data); + + . = ALIGN(32); + .dmaram_data : { - . = ALIGN(32); PROVIDE(dmaram_start = .); _sdmaram = .; _dmaram_start__ = _sdmaram; + _sdmaram_data = .; /* create a global symbol at data start */ + *(.dmaram_data) /* .data sections */ + *(.dmaram_data*) /* .data* sections */ + . = ALIGN(32); + _edmaram_data = .; /* define a global symbol at data end */ + } >RAM AT >FLASH + + . = ALIGN(32); + .dmaram_bss (NOLOAD) : + { + _sdmaram_bss = .; + __dmaram_bss_start__ = _sdmaram_bss; + *(.dmaram_bss) + *(SORT_BY_ALIGNMENT(.dmaram_bss*)) + . = ALIGN(32); + _edmaram_bss = .; + __dmaram_bss_end__ = _edmaram_bss; + } >RAM + + . = ALIGN(32); + .DMA_RAM (NOLOAD) : + { KEEP(*(.DMA_RAM)) PROVIDE(dmaram_end = .); _edmaram = .; _dmaram_end__ = _edmaram; - } >D2_RAM + } >RAM .DMA_RW_D2 (NOLOAD) : { @@ -89,5 +116,4 @@ SECTIONS } >RAM } -INCLUDE "stm32_h750_common.ld" INCLUDE "stm32_h750_common_post.ld" diff --git a/src/link/stm32_flash_h750_1m.ld b/src/link/stm32_flash_h750_1m.ld index 46c558434..48afdbd29 100644 --- a/src/link/stm32_flash_h750_1m.ld +++ b/src/link/stm32_flash_h750_1m.ld @@ -50,19 +50,46 @@ REGION_ALIAS("STACKRAM", DTCM_RAM) REGION_ALIAS("FASTRAM", DTCM_RAM) REGION_ALIAS("MAIN", FLASH) +INCLUDE "stm32_h750_common.ld" + SECTIONS { - .DMA_RAM (NOLOAD) : + /* used during startup to initialized dmaram_data */ + _sdmaram_idata = LOADADDR(.dmaram_data); + + . = ALIGN(32); + .dmaram_data : { - . = ALIGN(32); PROVIDE(dmaram_start = .); _sdmaram = .; _dmaram_start__ = _sdmaram; + _sdmaram_data = .; /* create a global symbol at data start */ + *(.dmaram_data) /* .data sections */ + *(.dmaram_data*) /* .data* sections */ + . = ALIGN(32); + _edmaram_data = .; /* define a global symbol at data end */ + } >RAM AT >FLASH + + . = ALIGN(32); + .dmaram_bss (NOLOAD) : + { + _sdmaram_bss = .; + __dmaram_bss_start__ = _sdmaram_bss; + *(.dmaram_bss) + *(SORT_BY_ALIGNMENT(.dmaram_bss*)) + . = ALIGN(32); + _edmaram_bss = .; + __dmaram_bss_end__ = _edmaram_bss; + } >RAM + + . = ALIGN(32); + .DMA_RAM (NOLOAD) : + { KEEP(*(.DMA_RAM)) PROVIDE(dmaram_end = .); _edmaram = .; _dmaram_end__ = _edmaram; - } >D2_RAM + } >RAM .DMA_RW_D2 (NOLOAD) : { @@ -89,5 +116,4 @@ SECTIONS } >RAM } -INCLUDE "stm32_h750_common.ld" INCLUDE "stm32_h750_common_post.ld" diff --git a/src/link/stm32_flash_h7a3_2m.ld b/src/link/stm32_flash_h7a3_2m.ld index 2952263f6..3f4acb935 100644 --- a/src/link/stm32_flash_h7a3_2m.ld +++ b/src/link/stm32_flash_h7a3_2m.ld @@ -147,7 +147,7 @@ SECTIONS . = ALIGN(4); _edata = .; /* define a global symbol at data end */ - } >RAM AT >FLASH + } >FASTRAM AT >FLASH /* Uninitialized data section */ . = ALIGN(4); @@ -163,7 +163,7 @@ SECTIONS . = ALIGN(4); _ebss = .; /* define a global symbol at bss end */ __bss_end__ = _ebss; - } >RAM + } >FASTRAM /* Uninitialized data section */ . = ALIGN(4); @@ -208,17 +208,42 @@ SECTIONS __fastram_bss_end__ = _efastram_bss; } >FASTRAM - .DMA_RAM (NOLOAD) : + /* used during startup to initialized dmaram_data */ + _sdmaram_idata = LOADADDR(.dmaram_data); + + . = ALIGN(32); + .dmaram_data : { - . = ALIGN(32); PROVIDE(dmaram_start = .); _sdmaram = .; _dmaram_start__ = _sdmaram; + _sdmaram_data = .; /* create a global symbol at data start */ + *(.dmaram_data) /* .data sections */ + *(.dmaram_data*) /* .data* sections */ + . = ALIGN(32); + _edmaram_data = .; /* define a global symbol at data end */ + } >RAM AT >FLASH + + . = ALIGN(32); + .dmaram_bss (NOLOAD) : + { + _sdmaram_bss = .; + __dmaram_bss_start__ = _sdmaram_bss; + *(.dmaram_bss) + *(SORT_BY_ALIGNMENT(.dmaram_bss*)) + . = ALIGN(32); + _edmaram_bss = .; + __dmaram_bss_end__ = _edmaram_bss; + } >RAM + + . = ALIGN(32); + .DMA_RAM (NOLOAD) : + { KEEP(*(.DMA_RAM)) PROVIDE(dmaram_end = .); _edmaram = .; _dmaram_end__ = _edmaram; - } >D2_RAM + } >RAM .DMA_RW_D2 (NOLOAD) : { diff --git a/src/link/stm32_h750_common.ld b/src/link/stm32_h750_common.ld index 667fed120..b69eb91b3 100644 --- a/src/link/stm32_h750_common.ld +++ b/src/link/stm32_h750_common.ld @@ -15,7 +15,7 @@ _Min_Stack_Size = 0x800; /* required amount of stack */ /* Define output sections */ SECTIONS { - /* The startup code goes first into MAIN */ + /* The vector table goes first into MAIN */ .isr_vector : { . = ALIGN(512); @@ -111,7 +111,7 @@ SECTIONS . = ALIGN(4); _ebss = .; /* define a global symbol at bss end */ __bss_end__ = _ebss; - } >D2_RAM + } >DTCM_RAM /* Uninitialized data section */ . = ALIGN(4); diff --git a/src/link/stm32_ram_h743.ld b/src/link/stm32_ram_h743.ld index b9c8cd463..ebdabe7c6 100644 --- a/src/link/stm32_ram_h743.ld +++ b/src/link/stm32_ram_h743.ld @@ -214,17 +214,42 @@ SECTIONS __fastram_bss_end__ = _efastram_bss; } >FASTRAM - .DMA_RAM (NOLOAD) : + /* used during startup to initialized dmaram_data */ + _sdmaram_idata = LOADADDR(.dmaram_data); + + . = ALIGN(32); + .dmaram_data : { - . = ALIGN(32); PROVIDE(dmaram_start = .); _sdmaram = .; _dmaram_start__ = _sdmaram; + _sdmaram_data = .; /* create a global symbol at data start */ + *(.dmaram_data) /* .data sections */ + *(.dmaram_data*) /* .data* sections */ + . = ALIGN(32); + _edmaram_data = .; /* define a global symbol at data end */ + } >RAM + + . = ALIGN(32); + .dmaram_bss (NOLOAD) : + { + _sdmaram_bss = .; + __dmaram_bss_start__ = _sdmaram_bss; + *(.dmaram_bss) + *(SORT_BY_ALIGNMENT(.dmaram_bss*)) + . = ALIGN(32); + _edmaram_bss = .; + __dmaram_bss_end__ = _edmaram_bss; + } >RAM + + . = ALIGN(32); + .DMA_RAM (NOLOAD) : + { KEEP(*(.DMA_RAM)) PROVIDE(dmaram_end = .); _edmaram = .; _dmaram_end__ = _edmaram; - } >D2_RAM + } >RAM .DMA_RW_D2 (NOLOAD) : { diff --git a/src/link/stm32_ram_h750_exst.ld b/src/link/stm32_ram_h750_exst.ld index c50d48371..f322122e8 100644 --- a/src/link/stm32_ram_h750_exst.ld +++ b/src/link/stm32_ram_h750_exst.ld @@ -73,19 +73,46 @@ REGION_ALIAS("STACKRAM", DTCM_RAM) REGION_ALIAS("FASTRAM", DTCM_RAM) REGION_ALIAS("MAIN", CODE_RAM) +INCLUDE "stm32_h750_common.ld" + SECTIONS { - .DMA_RAM (NOLOAD) : + /* used during startup to initialized dmaram_data */ + _sdmaram_idata = LOADADDR(.dmaram_data); + + . = ALIGN(32); + .dmaram_data : { - . = ALIGN(32); PROVIDE(dmaram_start = .); _sdmaram = .; _dmaram_start__ = _sdmaram; + _sdmaram_data = .; /* create a global symbol at data start */ + *(.dmaram_data) /* .data sections */ + *(.dmaram_data*) /* .data* sections */ + . = ALIGN(32); + _edmaram_data = .; /* define a global symbol at data end */ + } >RAM AT >MAIN + + . = ALIGN(32); + .dmaram_bss (NOLOAD) : + { + _sdmaram_bss = .; + __dmaram_bss_start__ = _sdmaram_bss; + *(.dmaram_bss) + *(SORT_BY_ALIGNMENT(.dmaram_bss*)) + . = ALIGN(32); + _edmaram_bss = .; + __dmaram_bss_end__ = _edmaram_bss; + } >RAM + + . = ALIGN(32); + .DMA_RAM (NOLOAD) : + { KEEP(*(.DMA_RAM)) PROVIDE(dmaram_end = .); _edmaram = .; _dmaram_end__ = _edmaram; - } >D2_RAM + } >RAM .DMA_RW_D2 (NOLOAD) : { @@ -112,7 +139,6 @@ SECTIONS } >RAM } -INCLUDE "stm32_h750_common.ld" INCLUDE "stm32_h750_common_post.ld" INCLUDE "stm32_ram_h750_exst_post.ld" diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index b5d311a05..9c143a0b4 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -703,7 +703,7 @@ const clivalue_t valueTable[] = { // PG_BAROMETER_CONFIG #ifdef USE_BARO - { "baro_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_bustype) }, + { "baro_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_busType) }, { "baro_spi_device", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, 5 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_spi_device) }, { "baro_i2c_device", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, 5 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_i2c_device) }, { "baro_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_i2c_address) }, @@ -1254,7 +1254,6 @@ const clivalue_t valueTable[] = { { "sdcard_mode", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_SDCARD_MODE }, PG_SDCARD_CONFIG, offsetof(sdcardConfig_t, mode) }, #endif #ifdef USE_SDCARD_SPI - { "sdcard_dma", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SDCARD_CONFIG, offsetof(sdcardConfig_t, useDma) }, { "sdcard_spi_bus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_SDCARD_CONFIG, offsetof(sdcardConfig_t, device) }, #endif #ifdef USE_SDCARD_SDIO @@ -1599,7 +1598,7 @@ const clivalue_t valueTable[] = { #endif // PG_GYRO_DEVICE_CONFIG - { "gyro_1_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, bustype) }, + { "gyro_1_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, busType) }, { "gyro_1_spibus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, spiBus) }, { "gyro_1_i2cBus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2CDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, i2cBus) }, { "gyro_1_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, i2cAddress) }, @@ -1608,7 +1607,7 @@ const clivalue_t valueTable[] = { { "gyro_1_align_pitch", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, customAlignment.pitch) }, { "gyro_1_align_yaw", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, customAlignment.yaw) }, #ifdef USE_MULTI_GYRO - { "gyro_2_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, bustype) }, + { "gyro_2_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, busType) }, { "gyro_2_spibus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, spiBus) }, { "gyro_2_i2cBus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2CDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, i2cBus) }, { "gyro_2_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, i2cAddress) }, diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index fc21404bb..919365052 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -33,8 +33,6 @@ #pragma GCC diagnostic push #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD) #include -#elif !defined(UNIT_TEST) -#pragma GCC diagnostic warning "-Wpadded" #endif #define GYRO_SCALE_2000DPS (2000.0f / (1 << 15)) // 16.384 dps/lsb scalefactor for 2000dps sensors @@ -88,7 +86,7 @@ typedef struct gyroDev_s { sensorGyroReadFuncPtr readFn; // read 3 axis data function sensorGyroReadDataFuncPtr temperatureFn; // read temperature if available extiCallbackRec_t exti; - busDevice_t bus; + extDevice_t dev; float scale; // scalefactor float gyroZero[XYZ_AXIS_COUNT]; float gyroADC[XYZ_AXIS_COUNT]; // gyro data after calibration and alignment @@ -118,12 +116,12 @@ typedef struct accDev_s { float acc_1G_rec; sensorAccInitFuncPtr initFn; // initialize function sensorAccReadFuncPtr readFn; // read 3 axis data function - busDevice_t bus; uint16_t acc_1G; int16_t ADCRaw[XYZ_AXIS_COUNT]; mpuDetectionResult_t mpuDetectionResult; sensor_align_e accAlign; bool dataReady; + gyroDev_t *gyro; bool acc_high_fsr; char revisionCode; // a revision code for the sensor, if known uint8_t filler[2]; diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index b629f749b..d41d907b3 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -75,7 +75,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro) // determine product ID and revision uint8_t readBuffer[6]; - bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_XA_OFFS_H, readBuffer, 6); + bool ack = busReadRegisterBuffer(&gyro->dev, MPU_RA_XA_OFFS_H, readBuffer, 6); uint8_t revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01); if (ack && revision) { // Congrats, these parts are better @@ -90,7 +90,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro) } } else { uint8_t productId; - ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_PRODUCT_ID, &productId, 1); + ack = busReadRegisterBuffer(&gyro->dev, MPU_RA_PRODUCT_ID, &productId, 1); revision = productId & 0x0F; if (!ack || revision == 0) { failureMode(FAILURE_ACC_INCOMPATIBLE); @@ -149,7 +149,7 @@ bool mpuAccRead(accDev_t *acc) { uint8_t data[6]; - const bool ack = busReadRegisterBuffer(&acc->bus, MPU_RA_ACCEL_XOUT_H, data, 6); + const bool ack = busReadRegisterBuffer(&acc->gyro->dev, MPU_RA_ACCEL_XOUT_H, data, 6); if (!ack) { return false; } @@ -165,7 +165,7 @@ bool mpuGyroRead(gyroDev_t *gyro) { uint8_t data[6]; - const bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_GYRO_XOUT_H, data, 6); + const bool ack = busReadRegisterBuffer(&gyro->dev, MPU_RA_GYRO_XOUT_H, data, 6); if (!ack) { return false; } @@ -178,12 +178,29 @@ bool mpuGyroRead(gyroDev_t *gyro) } #ifdef USE_SPI_GYRO +bool mpuAccReadSPI(accDev_t *acc) +{ + STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {MPU_RA_ACCEL_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; + STATIC_DMA_DATA_AUTO uint8_t data[7]; + + const bool ack = spiReadWriteBufRB(&acc->gyro->dev, dataToSend, data, 7); + if (!ack) { + return false; + } + + acc->ADCRaw[X] = (int16_t)((data[1] << 8) | data[2]); + acc->ADCRaw[Y] = (int16_t)((data[3] << 8) | data[4]); + acc->ADCRaw[Z] = (int16_t)((data[5] << 8) | data[6]); + + return true; +} + bool mpuGyroReadSPI(gyroDev_t *gyro) { - static const uint8_t dataToSend[7] = {MPU_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; - uint8_t data[7]; + STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {MPU_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; + STATIC_DMA_DATA_AUTO uint8_t data[7]; - const bool ack = spiBusTransfer(&gyro->bus, dataToSend, data, 7); + const bool ack = spiReadWriteBufRB(&gyro->dev, dataToSend, data, 7); if (!ack) { return false; } @@ -195,7 +212,7 @@ bool mpuGyroReadSPI(gyroDev_t *gyro) return true; } -typedef uint8_t (*gyroSpiDetectFn_t)(const busDevice_t *bus); +typedef uint8_t (*gyroSpiDetectFn_t)(const extDevice_t *dev); static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = { #ifdef USE_GYRO_SPI_MPU6000 @@ -233,17 +250,15 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = { static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro, const gyroDeviceConfig_t *config) { - SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(config->spiBus)); - - if (!instance || !config->csnTag) { + if (!config->csnTag || !spiSetBusInstance(&gyro->dev, config->spiBus, OWNER_GYRO_CS)) { return false; } - spiBusSetInstance(&gyro->bus, instance); - gyro->bus.busdev_u.spi.csnPin = IOGetByTag(config->csnTag); - IOInit(gyro->bus.busdev_u.spi.csnPin, OWNER_GYRO_CS, RESOURCE_INDEX(config->index)); - IOConfigGPIO(gyro->bus.busdev_u.spi.csnPin, SPI_IO_CS_CFG); - IOHi(gyro->bus.busdev_u.spi.csnPin); // Ensure device is disabled, important when two devices are on the same bus. + gyro->dev.busType_u.spi.csnPin = IOGetByTag(config->csnTag); + + IOInit(gyro->dev.busType_u.spi.csnPin, OWNER_GYRO_CS, RESOURCE_INDEX(config->index)); + IOConfigGPIO(gyro->dev.busType_u.spi.csnPin, SPI_IO_CS_CFG); + IOHi(gyro->dev.busType_u.spi.csnPin); // Ensure device is disabled, important when two devices are on the same bus. uint8_t sensor = MPU_NONE; @@ -252,11 +267,10 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro, const gyro // May need a bitmap of hardware to detection function to do it right? for (size_t index = 0 ; gyroSpiDetectFnTable[index] ; index++) { - sensor = (gyroSpiDetectFnTable[index])(&gyro->bus); + sensor = (gyroSpiDetectFnTable[index])(&gyro->dev); if (sensor != MPU_NONE) { gyro->mpuDetectionResult.sensor = sensor; - busDeviceRegister(&gyro->bus); - + busDeviceRegister(&gyro->dev); return true; } } @@ -280,32 +294,35 @@ void mpuPreInit(const struct gyroDeviceConfig_s *config) bool mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config) { + static busDevice_t bus; + gyro->dev.bus = &bus; + // MPU datasheet specifies 30ms. delay(35); - if (config->bustype == BUSTYPE_NONE) { + if (config->busType == BUS_TYPE_NONE) { return false; } - if (config->bustype == BUSTYPE_GYRO_AUTO) { - gyro->bus.bustype = BUSTYPE_I2C; + if (config->busType == BUS_TYPE_GYRO_AUTO) { + gyro->dev.bus->busType = BUS_TYPE_I2C; } else { - gyro->bus.bustype = config->bustype; + gyro->dev.bus->busType = config->busType; } #ifdef USE_I2C_GYRO - if (gyro->bus.bustype == BUSTYPE_I2C) { - gyro->bus.busdev_u.i2c.device = I2C_CFG_TO_DEV(config->i2cBus); - gyro->bus.busdev_u.i2c.address = config->i2cAddress ? config->i2cAddress : MPU_ADDRESS; + if (gyro->dev.bus->busType == BUS_TYPE_I2C) { + gyro->dev.bus->busType_u.i2c.device = I2C_CFG_TO_DEV(config->i2cBus); + gyro->dev.busType_u.i2c.address = config->i2cAddress ? config->i2cAddress : MPU_ADDRESS; uint8_t sig = 0; - bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_WHO_AM_I, &sig, 1); + bool ack = busReadRegisterBuffer(&gyro->dev, MPU_RA_WHO_AM_I, &sig, 1); if (ack) { - busDeviceRegister(&gyro->bus); + busDeviceRegister(&gyro->dev); // If an MPU3050 is connected sig will contain 0. uint8_t inquiryResult; - ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_WHO_AM_I_LEGACY, &inquiryResult, 1); + ack = busReadRegisterBuffer(&gyro->dev, MPU_RA_WHO_AM_I_LEGACY, &inquiryResult, 1); inquiryResult &= MPU_INQUIRY_MASK; if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) { gyro->mpuDetectionResult.sensor = MPU_3050; @@ -325,7 +342,7 @@ bool mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config) #endif #ifdef USE_SPI_GYRO - gyro->bus.bustype = BUSTYPE_SPI; + gyro->dev.bus->busType = BUS_TYPE_SPI; return detectSPISensorsAndUpdateDetectionResult(gyro, config); #else @@ -370,10 +387,10 @@ uint8_t mpuGyroDLPF(gyroDev_t *gyro) } #ifdef USE_GYRO_REGISTER_DUMP -uint8_t mpuGyroReadRegister(const busDevice_t *bus, uint8_t reg) +uint8_t mpuGyroReadRegister(const extDevice_t *dev, uint8_t reg) { uint8_t data; - const bool ack = busReadRegisterBuffer(bus, reg, &data, 1); + const bool ack = busReadRegisterBuffer(dev, reg, &data, 1); if (ack) { return data; } else { diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 347e8f724..e2c1ce812 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -224,7 +224,8 @@ bool mpuGyroReadSPI(struct gyroDev_s *gyro); void mpuPreInit(const struct gyroDeviceConfig_s *config); bool mpuDetect(struct gyroDev_s *gyro, const struct gyroDeviceConfig_s *config); uint8_t mpuGyroDLPF(struct gyroDev_s *gyro); -uint8_t mpuGyroReadRegister(const busDevice_t *bus, uint8_t reg); +uint8_t mpuGyroReadRegister(const extDevice_t *dev, uint8_t reg); struct accDev_s; bool mpuAccRead(struct accDev_s *acc); +bool mpuAccReadSPI(struct accDev_s *acc); diff --git a/src/main/drivers/accgyro/accgyro_mpu6050.c b/src/main/drivers/accgyro/accgyro_mpu6050.c index 2accb1837..78979e6bd 100644 --- a/src/main/drivers/accgyro/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro/accgyro_mpu6050.c @@ -81,23 +81,23 @@ static void mpu6050GyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); - busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 + busWriteRegister(&gyro->dev, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 delay(100); - busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) - busWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) + busWriteRegister(&gyro->dev, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) + busWriteRegister(&gyro->dev, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure - busWriteRegister(&gyro->bus, MPU_RA_CONFIG, mpuGyroDLPF(gyro)); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) - busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec + busWriteRegister(&gyro->dev, MPU_RA_CONFIG, mpuGyroDLPF(gyro)); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) + busWriteRegister(&gyro->dev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec // ACC Init stuff. // Accel scale 8g (4096 LSB/g) - busWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + busWriteRegister(&gyro->dev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); - busWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, + busWriteRegister(&gyro->dev, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS #ifdef USE_MPU_DATA_READY_SIGNAL - busWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); + busWriteRegister(&gyro->dev, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); #endif } diff --git a/src/main/drivers/accgyro/accgyro_mpu6500.c b/src/main/drivers/accgyro/accgyro_mpu6500.c index 1b2dcda17..72c3dfb80 100644 --- a/src/main/drivers/accgyro/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_mpu6500.c @@ -64,33 +64,33 @@ void mpu6500GyroInit(gyroDev_t *gyro) accel_range = ICM_HIGH_RANGE_FSR_16G; } - busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); + busWriteRegister(&gyro->dev, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); delay(100); - busWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x07); + busWriteRegister(&gyro->dev, MPU_RA_SIGNAL_PATH_RESET, 0x07); delay(100); - busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0); + busWriteRegister(&gyro->dev, MPU_RA_PWR_MGMT_1, 0); delay(100); - busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); + busWriteRegister(&gyro->dev, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); delay(15); - busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, gyro_range << 3); + busWriteRegister(&gyro->dev, MPU_RA_GYRO_CONFIG, gyro_range << 3); delay(15); - busWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, accel_range << 3); + busWriteRegister(&gyro->dev, MPU_RA_ACCEL_CONFIG, accel_range << 3); delay(15); - busWriteRegister(&gyro->bus, MPU_RA_CONFIG, mpuGyroDLPF(gyro)); + busWriteRegister(&gyro->dev, MPU_RA_CONFIG, mpuGyroDLPF(gyro)); delay(15); - busWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops + busWriteRegister(&gyro->dev, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops delay(100); // Data ready interrupt configuration #ifdef USE_MPU9250_MAG - busWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN + busWriteRegister(&gyro->dev, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN #else - busWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR + busWriteRegister(&gyro->dev, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR #endif delay(15); #ifdef USE_MPU_DATA_READY_SIGNAL - busWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable + busWriteRegister(&gyro->dev, MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable #endif delay(15); } diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi160.c b/src/main/drivers/accgyro/accgyro_spi_bmi160.c index a0d785cfe..305a51104 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi160.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi160.c @@ -89,23 +89,23 @@ static volatile bool BMI160InitDone = false; static volatile bool BMI160Detected = false; //! Private functions -static int32_t BMI160_Config(const busDevice_t *bus); -static int32_t BMI160_do_foc(const busDevice_t *bus); +static int32_t BMI160_Config(const extDevice_t *dev); +static int32_t BMI160_do_foc(const extDevice_t *dev); -uint8_t bmi160Detect(const busDevice_t *bus) +uint8_t bmi160Detect(const extDevice_t *dev) { if (BMI160Detected) { return BMI_160_SPI; } - spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(BMI160_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(dev, spiCalculateDivider(BMI160_MAX_SPI_CLK_HZ)); /* Read this address to activate SPI (see p. 84) */ - spiBusReadRegister(bus, 0x7F); + spiReadRegMsk(dev, 0x7F); delay(100); // Give SPI some time to start up /* Check the chip ID */ - if (spiBusReadRegister(bus, BMI160_REG_CHIPID) != 0xd1) { + if (spiReadRegMsk(dev, BMI160_REG_CHIPID) != 0xd1) { return MPU_NONE; } @@ -118,14 +118,14 @@ uint8_t bmi160Detect(const busDevice_t *bus) * @brief Initialize the BMI160 6-axis sensor. * @return 0 for success, -1 for failure to allocate, -10 for failure to get irq */ -static void BMI160_Init(const busDevice_t *bus) +static void BMI160_Init(const extDevice_t *dev) { if (BMI160InitDone || !BMI160Detected) { return; } /* Configure the BMI160 Sensor */ - if (BMI160_Config(bus) != 0) { + if (BMI160_Config(dev) != 0) { return; } @@ -133,7 +133,7 @@ static void BMI160_Init(const busDevice_t *bus) /* Perform fast offset compensation if requested */ if (do_foc) { - BMI160_do_foc(bus); + BMI160_do_foc(dev); } BMI160InitDone = true; @@ -143,68 +143,68 @@ static void BMI160_Init(const busDevice_t *bus) /** * @brief Configure the sensor */ -static int32_t BMI160_Config(const busDevice_t *bus) +static int32_t BMI160_Config(const extDevice_t *dev) { // Set normal power mode for gyro and accelerometer - spiBusWriteRegister(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL); + spiWriteReg(dev, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL); delay(100); // can take up to 80ms - spiBusWriteRegister(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL); + spiWriteReg(dev, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL); delay(5); // can take up to 3.8ms // Verify that normal power mode was entered - uint8_t pmu_status = spiBusReadRegister(bus, BMI160_REG_PMU_STAT); + uint8_t pmu_status = spiReadRegMsk(dev, BMI160_REG_PMU_STAT); if ((pmu_status & 0x3C) != 0x14) { return -3; } // Set odr and ranges // Set acc_us = 0 acc_bwp = 0b010 so only the first filter stage is used - spiBusWriteRegister(bus, BMI160_REG_ACC_CONF, 0x20 | BMI160_ODR_800_Hz); + spiWriteReg(dev, BMI160_REG_ACC_CONF, 0x20 | BMI160_ODR_800_Hz); delay(1); // Set gyr_bwp = 0b010 so only the first filter stage is used - spiBusWriteRegister(bus, BMI160_REG_GYR_CONF, 0x20 | BMI160_ODR_3200_Hz); + spiWriteReg(dev, BMI160_REG_GYR_CONF, 0x20 | BMI160_ODR_3200_Hz); delay(1); - spiBusWriteRegister(bus, BMI160_REG_ACC_RANGE, BMI160_RANGE_8G); + spiWriteReg(dev, BMI160_REG_ACC_RANGE, BMI160_RANGE_8G); delay(1); - spiBusWriteRegister(bus, BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS); + spiWriteReg(dev, BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS); delay(1); // Enable offset compensation - uint8_t val = spiBusReadRegister(bus, BMI160_REG_OFFSET_0); - spiBusWriteRegister(bus, BMI160_REG_OFFSET_0, val | 0xC0); + uint8_t val = spiReadRegMsk(dev, BMI160_REG_OFFSET_0); + spiWriteReg(dev, BMI160_REG_OFFSET_0, val | 0xC0); // Enable data ready interrupt - spiBusWriteRegister(bus, BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY); + spiWriteReg(dev, BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY); delay(1); // Enable INT1 pin - spiBusWriteRegister(bus, BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG); + spiWriteReg(dev, BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG); delay(1); // Map data ready interrupt to INT1 pin - spiBusWriteRegister(bus, BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY); + spiWriteReg(dev, BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY); delay(1); return 0; } -static int32_t BMI160_do_foc(const busDevice_t *bus) +static int32_t BMI160_do_foc(const extDevice_t *dev) { // assume sensor is mounted on top uint8_t val = 0x7D;; - spiBusWriteRegister(bus, BMI160_REG_FOC_CONF, val); + spiWriteReg(dev, BMI160_REG_FOC_CONF, val); // Start FOC - spiBusWriteRegister(bus, BMI160_REG_CMD, BMI160_CMD_START_FOC); + spiWriteReg(dev, BMI160_REG_CMD, BMI160_CMD_START_FOC); // Wait for FOC to complete for (int i=0; i<50; i++) { - val = spiBusReadRegister(bus, BMI160_REG_STATUS); + val = spiReadRegMsk(dev, BMI160_REG_STATUS); if (val & BMI160_REG_STATUS_FOC_RDY) { break; } @@ -215,14 +215,14 @@ static int32_t BMI160_do_foc(const busDevice_t *bus) } // Program NVM - val = spiBusReadRegister(bus, BMI160_REG_CONF); - spiBusWriteRegister(bus, BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN); + val = spiReadRegMsk(dev, BMI160_REG_CONF); + spiWriteReg(dev, BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN); - spiBusWriteRegister(bus, BMI160_REG_CMD, BMI160_CMD_PROG_NVM); + spiWriteReg(dev, BMI160_REG_CMD, BMI160_CMD_PROG_NVM); // Wait for NVM programming to complete for (int i=0; i<50; i++) { - val = spiBusReadRegister(bus, BMI160_REG_STATUS); + val = spiReadRegMsk(dev, BMI160_REG_STATUS); if (val & BMI160_REG_STATUS_NVM_RDY) { break; } @@ -275,9 +275,7 @@ bool bmi160AccRead(accDev_t *acc) uint8_t bmi160_rx_buf[BUFFER_SIZE]; static const uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0}; - IOLo(acc->bus.busdev_u.spi.csnPin); - spiTransfer(acc->bus.busdev_u.spi.instance, bmi160_tx_buf, bmi160_rx_buf, BUFFER_SIZE); // receive response - IOHi(acc->bus.busdev_u.spi.csnPin); + spiReadWriteBufRB(&acc->gyro->dev, bmi160_tx_buf, bmi160_rx_buf, BUFFER_SIZE); // receive response acc->ADCRaw[X] = (int16_t)((bmi160_rx_buf[IDX_ACCEL_XOUT_H] << 8) | bmi160_rx_buf[IDX_ACCEL_XOUT_L]); acc->ADCRaw[Y] = (int16_t)((bmi160_rx_buf[IDX_ACCEL_YOUT_H] << 8) | bmi160_rx_buf[IDX_ACCEL_YOUT_L]); @@ -303,9 +301,7 @@ bool bmi160GyroRead(gyroDev_t *gyro) uint8_t bmi160_rx_buf[BUFFER_SIZE]; static const uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0}; - IOLo(gyro->bus.busdev_u.spi.csnPin); - spiTransfer(gyro->bus.busdev_u.spi.instance, bmi160_tx_buf, bmi160_rx_buf, BUFFER_SIZE); // receive response - IOHi(gyro->bus.busdev_u.spi.csnPin); + spiReadWriteBufRB(&acc->gyro->dev, bmi160_tx_buf, bmi160_rx_buf, BUFFER_SIZE); // receive response gyro->gyroADCRaw[X] = (int16_t)((bmi160_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi160_rx_buf[IDX_GYRO_XOUT_L]); gyro->gyroADCRaw[Y] = (int16_t)((bmi160_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi160_rx_buf[IDX_GYRO_YOUT_L]); @@ -317,7 +313,7 @@ bool bmi160GyroRead(gyroDev_t *gyro) void bmi160SpiGyroInit(gyroDev_t *gyro) { - BMI160_Init(&gyro->bus); + BMI160_Init(&gyro->dev); #if defined(USE_MPU_DATA_READY_SIGNAL) bmi160IntExtiInit(gyro); #endif @@ -325,7 +321,7 @@ void bmi160SpiGyroInit(gyroDev_t *gyro) void bmi160SpiAccInit(accDev_t *acc) { - BMI160_Init(&acc->bus); + BMI160_Init(&acc->dev); acc->acc_1G = 512 * 8; } @@ -333,7 +329,7 @@ void bmi160SpiAccInit(accDev_t *acc) bool bmi160SpiAccDetect(accDev_t *acc) { - if (bmi160Detect(&acc->bus) == MPU_NONE) { + if (bmi160Detect(&acc->dev) == MPU_NONE) { return false; } @@ -346,7 +342,7 @@ bool bmi160SpiAccDetect(accDev_t *acc) bool bmi160SpiGyroDetect(gyroDev_t *gyro) { - if (bmi160Detect(&gyro->bus) == MPU_NONE) { + if (bmi160Detect(&gyro->dev) == MPU_NONE) { return false; } diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c index bd379a185..f77805c6f 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi270.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c @@ -125,20 +125,20 @@ typedef enum { // BMI270 register reads are 16bits with the first byte a "dummy" value 0 // that must be ignored. The result is in the second byte. -static uint8_t bmi270RegisterRead(const busDevice_t *bus, bmi270Register_e registerId) +static uint8_t bmi270RegisterRead(const extDevice_t *dev, bmi270Register_e registerId) { uint8_t data[2] = { 0, 0 }; - if (spiBusReadRegisterBuffer(bus, registerId, data, 2)) { + if (spiReadRegMskBufRB(dev, registerId, data, 2)) { return data[1]; } else { return 0; } } -static void bmi270RegisterWrite(const busDevice_t *bus, bmi270Register_e registerId, uint8_t value, unsigned delayMs) +static void bmi270RegisterWrite(const extDevice_t *dev, bmi270Register_e registerId, uint8_t value, unsigned delayMs) { - spiBusWriteRegister(bus, registerId, value); + spiWriteReg(dev, registerId, value); if (delayMs) { delay(delayMs); } @@ -146,44 +146,41 @@ static void bmi270RegisterWrite(const busDevice_t *bus, bmi270Register_e registe // Toggle the CS to switch the device into SPI mode. // Device switches initializes as I2C and switches to SPI on a low to high CS transition -static void bmi270EnableSPI(const busDevice_t *bus) +static void bmi270EnableSPI(const extDevice_t *dev) { - IOLo(bus->busdev_u.spi.csnPin); + IOLo(dev->busType_u.spi.csnPin); delay(1); - IOHi(bus->busdev_u.spi.csnPin); + IOHi(dev->busType_u.spi.csnPin); delay(10); } -uint8_t bmi270Detect(const busDevice_t *bus) +uint8_t bmi270Detect(const extDevice_t *dev) { - spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(BMI270_MAX_SPI_CLK_HZ)); - bmi270EnableSPI(bus); + spiSetClkDivisor(dev, spiCalculateDivider(BMI270_MAX_SPI_CLK_HZ)); + bmi270EnableSPI(dev); - if (bmi270RegisterRead(bus, BMI270_REG_CHIP_ID) == BMI270_CHIP_ID) { + if (bmi270RegisterRead(dev, BMI270_REG_CHIP_ID) == BMI270_CHIP_ID) { return BMI_270_SPI; } return MPU_NONE; } -static void bmi270UploadConfig(const busDevice_t *bus) +static void bmi270UploadConfig(const extDevice_t *dev) { - bmi270RegisterWrite(bus, BMI270_REG_PWR_CONF, 0, 1); - bmi270RegisterWrite(bus, BMI270_REG_INIT_CTRL, 0, 1); + bmi270RegisterWrite(dev, BMI270_REG_PWR_CONF, 0, 1); + bmi270RegisterWrite(dev, BMI270_REG_INIT_CTRL, 0, 1); // Transfer the config file - IOLo(bus->busdev_u.spi.csnPin); - spiTransferByte(bus->busdev_u.spi.instance, BMI270_REG_INIT_DATA); - spiTransfer(bus->busdev_u.spi.instance, bmi270_maximum_fifo_config_file, NULL, sizeof(bmi270_maximum_fifo_config_file)); - IOHi(bus->busdev_u.spi.csnPin); + spiWriteRegBuf(dev, BMI270_REG_INIT_DATA, (uint8_t *)bmi270_maximum_fifo_config_file, sizeof(bmi270_maximum_fifo_config_file)); delay(10); - bmi270RegisterWrite(bus, BMI270_REG_INIT_CTRL, 1, 1); + bmi270RegisterWrite(dev, BMI270_REG_INIT_CTRL, 1, 1); } -static void bmi270Config(const gyroDev_t *gyro) +static void bmi270Config(gyroDev_t *gyro) { - const busDevice_t *bus = &gyro->bus; + extDevice_t *dev = &gyro->dev; // If running in hardware_lpf experimental mode then switch to FIFO-based, // 6.4KHz sampling, unfiltered data vs. the default 3.2KHz with hardware filtering @@ -195,55 +192,55 @@ static void bmi270Config(const gyroDev_t *gyro) // Perform a soft reset to set all configuration to default // Delay 100ms before continuing configuration - bmi270RegisterWrite(bus, BMI270_REG_CMD, BMI270_VAL_CMD_SOFTRESET, 100); + bmi270RegisterWrite(dev, BMI270_REG_CMD, BMI270_VAL_CMD_SOFTRESET, 100); // Toggle the chip into SPI mode - bmi270EnableSPI(bus); + bmi270EnableSPI(dev); - bmi270UploadConfig(bus); + bmi270UploadConfig(dev); // Configure the FIFO if (fifoMode) { - bmi270RegisterWrite(bus, BMI270_REG_FIFO_CONFIG_0, BMI270_VAL_FIFO_CONFIG_0, 1); - bmi270RegisterWrite(bus, BMI270_REG_FIFO_CONFIG_1, BMI270_VAL_FIFO_CONFIG_1, 1); - bmi270RegisterWrite(bus, BMI270_REG_FIFO_DOWNS, BMI270_VAL_FIFO_DOWNS, 1); - bmi270RegisterWrite(bus, BMI270_REG_FIFO_WTM_0, BMI270_VAL_FIFO_WTM_0, 1); - bmi270RegisterWrite(bus, BMI270_REG_FIFO_WTM_1, BMI270_VAL_FIFO_WTM_1, 1); + bmi270RegisterWrite(dev, BMI270_REG_FIFO_CONFIG_0, BMI270_VAL_FIFO_CONFIG_0, 1); + bmi270RegisterWrite(dev, BMI270_REG_FIFO_CONFIG_1, BMI270_VAL_FIFO_CONFIG_1, 1); + bmi270RegisterWrite(dev, BMI270_REG_FIFO_DOWNS, BMI270_VAL_FIFO_DOWNS, 1); + bmi270RegisterWrite(dev, BMI270_REG_FIFO_WTM_0, BMI270_VAL_FIFO_WTM_0, 1); + bmi270RegisterWrite(dev, BMI270_REG_FIFO_WTM_1, BMI270_VAL_FIFO_WTM_1, 1); } // Configure the accelerometer - bmi270RegisterWrite(bus, BMI270_REG_ACC_CONF, (BMI270_VAL_ACC_CONF_HP << 7) | (BMI270_VAL_ACC_CONF_BWP << 4) | BMI270_VAL_ACC_CONF_ODR800, 1); + bmi270RegisterWrite(dev, BMI270_REG_ACC_CONF, (BMI270_VAL_ACC_CONF_HP << 7) | (BMI270_VAL_ACC_CONF_BWP << 4) | BMI270_VAL_ACC_CONF_ODR800, 1); // Configure the accelerometer full-scale range - bmi270RegisterWrite(bus, BMI270_REG_ACC_RANGE, BMI270_VAL_ACC_RANGE_16G, 1); + bmi270RegisterWrite(dev, BMI270_REG_ACC_RANGE, BMI270_VAL_ACC_RANGE_16G, 1); // Configure the gyro - bmi270RegisterWrite(bus, BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (BMI270_VAL_GYRO_CONF_BWP << 4) | BMI270_VAL_GYRO_CONF_ODR3200, 1); + bmi270RegisterWrite(dev, BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (BMI270_VAL_GYRO_CONF_BWP << 4) | BMI270_VAL_GYRO_CONF_ODR3200, 1); // Configure the gyro full-range scale - bmi270RegisterWrite(bus, BMI270_REG_GYRO_RANGE, BMI270_VAL_GYRO_RANGE_2000DPS, 1); + bmi270RegisterWrite(dev, BMI270_REG_GYRO_RANGE, BMI270_VAL_GYRO_RANGE_2000DPS, 1); // Configure the gyro data ready interrupt if (fifoMode) { // Interrupt driven by FIFO watermark level - bmi270RegisterWrite(bus, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_FIFO_WM_INT1, 1); + bmi270RegisterWrite(dev, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_FIFO_WM_INT1, 1); } else { // Interrupt driven by data ready - bmi270RegisterWrite(bus, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_DATA_DRDY_INT1, 1); + bmi270RegisterWrite(dev, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_DATA_DRDY_INT1, 1); } // Configure the behavior of the INT1 pin - bmi270RegisterWrite(bus, BMI270_REG_INT1_IO_CTRL, BMI270_VAL_INT1_IO_CTRL_PINMODE, 1); + bmi270RegisterWrite(dev, BMI270_REG_INT1_IO_CTRL, BMI270_VAL_INT1_IO_CTRL_PINMODE, 1); // Configure the device for performance mode - bmi270RegisterWrite(bus, BMI270_REG_PWR_CONF, BMI270_VAL_PWR_CONF, 1); + bmi270RegisterWrite(dev, BMI270_REG_PWR_CONF, BMI270_VAL_PWR_CONF, 1); // Enable the gyro, accelerometer and temperature sensor - disable aux interface - bmi270RegisterWrite(bus, BMI270_REG_PWR_CTRL, BMI270_VAL_PWR_CTRL, 1); + bmi270RegisterWrite(dev, BMI270_REG_PWR_CTRL, BMI270_VAL_PWR_CTRL, 1); // Flush the FIFO if (fifoMode) { - bmi270RegisterWrite(bus, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 1); + bmi270RegisterWrite(dev, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 1); } } @@ -288,9 +285,7 @@ static bool bmi270AccRead(accDev_t *acc) uint8_t bmi270_rx_buf[BUFFER_SIZE]; static const uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0}; - IOLo(acc->bus.busdev_u.spi.csnPin); - spiTransfer(acc->bus.busdev_u.spi.instance, bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response - IOHi(acc->bus.busdev_u.spi.csnPin); + spiReadWriteBuf(&acc->gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response acc->ADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_XOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_XOUT_L]); acc->ADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_YOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_YOUT_L]); @@ -316,9 +311,7 @@ static bool bmi270GyroReadRegister(gyroDev_t *gyro) uint8_t bmi270_rx_buf[BUFFER_SIZE]; static const uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0}; - IOLo(gyro->bus.busdev_u.spi.csnPin); - spiTransfer(gyro->bus.busdev_u.spi.instance, bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response - IOHi(gyro->bus.busdev_u.spi.csnPin); + spiReadWriteBuf(&gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response gyro->gyroADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_XOUT_L]); gyro->gyroADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_YOUT_L]); @@ -351,9 +344,7 @@ static bool bmi270GyroReadFifo(gyroDev_t *gyro) // Burst read the FIFO length followed by the next 6 bytes containing the gyro axis data for // the first sample in the queue. It's possible for the FIFO to be empty so we need to check the // length before using the sample. - IOLo(gyro->bus.busdev_u.spi.csnPin); - spiTransfer(gyro->bus.busdev_u.spi.instance, bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response - IOHi(gyro->bus.busdev_u.spi.csnPin); + spiReadWriteBuf(&gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response int fifoLength = (uint16_t)((bmi270_rx_buf[IDX_FIFO_LENGTH_H] << 8) | bmi270_rx_buf[IDX_FIFO_LENGTH_L]); @@ -385,7 +376,7 @@ static bool bmi270GyroReadFifo(gyroDev_t *gyro) // would end up in a lock state of always re-reading the same partial or invalid sample. if (fifoLength > 0) { // Partial or additional frames left - flush the FIFO - bmi270RegisterWrite(&gyro->bus, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 0); + bmi270RegisterWrite(&gyro->dev, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 0); } return dataRead; @@ -456,6 +447,6 @@ bool bmi270SpiGyroDetect(gyroDev_t *gyro) // watermark reason as an idication of gyro data ready. uint8_t bmi270InterruptStatus(gyroDev_t *gyro) { - return bmi270RegisterRead(&gyro->bus, BMI270_REG_INT_STATUS_1); + return bmi270RegisterRead(&gyro->dev, BMI270_REG_INT_STATUS_1); } #endif // USE_ACCGYRO_BMI270 diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.h b/src/main/drivers/accgyro/accgyro_spi_bmi270.h index c0ea0495f..6059f6c1a 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi270.h +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.h @@ -22,7 +22,7 @@ #include "drivers/bus.h" -uint8_t bmi270Detect(const busDevice_t *bus); +uint8_t bmi270Detect(const extDevice_t *dev); bool bmi270SpiAccDetect(accDev_t *acc); bool bmi270SpiGyroDetect(gyroDev_t *gyro); uint8_t bmi270InterruptStatus(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20649.c b/src/main/drivers/accgyro/accgyro_spi_icm20649.c index 4936b3bd0..25c882ae2 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20649.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20649.c @@ -50,7 +50,9 @@ static void icm20649SpiInit(const busDevice_t *bus) return; } - spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ)); + // all registers can be read/written at full speed (7MHz +-10%) + // TODO verify that this works at 9MHz and 10MHz on non F7 + spiSetClkDivisor(dev, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ)); hardwareInitialised = true; } @@ -59,18 +61,18 @@ uint8_t icm20649SpiDetect(const busDevice_t *bus) { icm20649SpiInit(bus); - spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(dev, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ)); - spiBusWriteRegister(bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe + spiWriteReg(bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe delay(15); - spiBusWriteRegister(bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET); + spiWriteReg(bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET); uint8_t icmDetected = MPU_NONE; uint8_t attemptsRemaining = 20; do { delay(150); - const uint8_t whoAmI = spiBusReadRegister(bus, ICM20649_RA_WHO_AM_I); + const uint8_t whoAmI = spiReadRegMsk(bus, ICM20649_RA_WHO_AM_I); if (whoAmI == ICM20649_WHO_AM_I_CONST) { icmDetected = ICM_20649_SPI; } else { @@ -94,14 +96,14 @@ void icm20649AccInit(accDev_t *acc) // 1,024 LSB/g 30g acc->acc_1G = acc->acc_high_fsr ? 1024 : 2048; - spiSetDivisor(acc->bus.busdev_u.spi.instance, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(dev, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ)); - spiBusWriteRegister(&acc->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2 + spiWriteReg(&acc->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2 delay(15); const uint8_t acc_fsr = acc->acc_high_fsr ? ICM20649_FSR_30G : ICM20649_FSR_16G; - spiBusWriteRegister(&acc->bus, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1); + spiWriteReg(&acc->bus, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1); delay(15); - spiBusWriteRegister(&acc->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0 + spiWriteReg(&acc->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0 delay(15); } @@ -122,15 +124,15 @@ void icm20649GyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); - spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ)); // ensure proper speed + spiSetClkDivisor(dev, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ)); // ensure proper speed - spiBusWriteRegister(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe + spiWriteReg(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe delay(15); - spiBusWriteRegister(&gyro->bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET); + spiWriteReg(&gyro->bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET); delay(100); - spiBusWriteRegister(&gyro->bus, ICM20649_RA_PWR_MGMT_1, INV_CLK_PLL); + spiWriteReg(&gyro->bus, ICM20649_RA_PWR_MGMT_1, INV_CLK_PLL); delay(15); - spiBusWriteRegister(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2 + spiWriteReg(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2 delay(15); const uint8_t gyro_fsr = gyro->gyro_high_fsr ? ICM20649_FSR_4000DPS : ICM20649_FSR_2000DPS; @@ -141,20 +143,20 @@ void icm20649GyroInit(gyroDev_t *gyro) // the ICM20649 only has a single 9KHz DLPF cutoff. uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_1100_Hz ? 0 : 1; // deactivate GYRO_FCHOICE for sample rates over 1kHz (opposite of other invensense chips) raGyroConfigData |= gyro_fsr << 1; - spiBusWriteRegister(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData); + spiWriteReg(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData); delay(15); - spiBusWriteRegister(&gyro->bus, ICM20649_RA_GYRO_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops + spiWriteReg(&gyro->bus, ICM20649_RA_GYRO_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops delay(100); // Data ready interrupt configuration // back to bank 0 - spiBusWriteRegister(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); + spiWriteReg(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); delay(15); - spiBusWriteRegister(&gyro->bus, ICM20649_RA_INT_PIN_CFG, 0x11); // INT_ANYRD_2CLEAR, BYPASS_EN + spiWriteReg(&gyro->bus, ICM20649_RA_INT_PIN_CFG, 0x11); // INT_ANYRD_2CLEAR, BYPASS_EN delay(15); #ifdef USE_MPU_DATA_READY_SIGNAL - spiBusWriteRegister(&gyro->bus, ICM20649_RA_INT_ENABLE_1, 0x01); + spiWriteReg(&gyro->bus, ICM20649_RA_INT_ENABLE_1, 0x01); #endif } @@ -178,7 +180,7 @@ bool icm20649GyroReadSPI(gyroDev_t *gyro) static const uint8_t dataToSend[7] = {ICM20649_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; uint8_t data[7]; - const bool ack = spiBusTransfer(&gyro->bus, dataToSend, data, 7); + const bool ack = spiReadWriteBufRB(&gyro->bus, dataToSend, data, 7); if (!ack) { return false; } @@ -194,7 +196,7 @@ bool icm20649AccRead(accDev_t *acc) { uint8_t data[6]; - const bool ack = spiBusReadRegisterBuffer(&acc->bus, ICM20649_RA_ACCEL_XOUT_H, data, 6); + const bool ack = spiReadRegMskBufRB(&acc->bus, ICM20649_RA_ACCEL_XOUT_H, data, 6); if (!ack) { return false; } diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.c b/src/main/drivers/accgyro/accgyro_spi_icm20689.c index 065a70245..3bcfc40f2 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.c @@ -37,13 +37,14 @@ #include "drivers/time.h" + // 10 MHz max SPI frequency #define ICM20689_MAX_SPI_CLK_HZ 10000000 // 10 MHz max SPI frequency for intialisation #define ICM20689_MAX_SPI_INIT_CLK_HZ 1000000 -static void icm20689SpiInit(const busDevice_t *bus) +static void icm20689SpiInit(const extDevice_t *dev) { static bool hardwareInitialised = false; @@ -52,28 +53,28 @@ static void icm20689SpiInit(const busDevice_t *bus) } - spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(ICM20689_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(dev, spiCalculateDivider(ICM20689_MAX_SPI_CLK_HZ)); hardwareInitialised = true; } -uint8_t icm20689SpiDetect(const busDevice_t *bus) +uint8_t icm20689SpiDetect(const extDevice_t *dev) { - icm20689SpiInit(bus); + icm20689SpiInit(dev); - spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(ICM20689_MAX_SPI_INIT_CLK_HZ)); + spiSetClkDivisor(dev, spiCalculateDivider(ICM20689_MAX_SPI_INIT_CLK_HZ)); // reset the device configuration - spiBusWriteRegister(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); + spiWriteReg(dev, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); delay(100); // reset the device signal paths - spiBusWriteRegister(bus, MPU_RA_SIGNAL_PATH_RESET, 0x03); + spiWriteReg(dev, MPU_RA_SIGNAL_PATH_RESET, 0x03); delay(100); uint8_t icmDetected; - const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I); + const uint8_t whoAmI = spiReadRegMsk(dev, MPU_RA_WHO_AM_I); switch (whoAmI) { case ICM20601_WHO_AM_I_CONST: @@ -93,7 +94,7 @@ uint8_t icm20689SpiDetect(const busDevice_t *bus) break; } - spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(ICM20689_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(dev, spiCalculateDivider(ICM20689_MAX_SPI_CLK_HZ)); return icmDetected; } @@ -114,7 +115,7 @@ bool icm20689SpiAccDetect(accDev_t *acc) } acc->initFn = icm20689AccInit; - acc->readFn = mpuAccRead; + acc->readFn = mpuAccReadSPI; return true; } @@ -123,32 +124,32 @@ void icm20689GyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); - spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(ICM20689_MAX_SPI_INIT_CLK_HZ)); + spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM20689_MAX_SPI_INIT_CLK_HZ)); // Device was already reset during detection so proceed with configuration - spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); + spiWriteReg(&gyro->dev, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); delay(15); - spiBusWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); + spiWriteReg(&gyro->dev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); delay(15); - spiBusWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + spiWriteReg(&gyro->dev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delay(15); - spiBusWriteRegister(&gyro->bus, MPU_RA_CONFIG, mpuGyroDLPF(gyro)); + spiWriteReg(&gyro->dev, MPU_RA_CONFIG, mpuGyroDLPF(gyro)); delay(15); - spiBusWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops + spiWriteReg(&gyro->dev, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops delay(100); // Data ready interrupt configuration -// spiBusWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN - spiBusWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN +// spiWriteReg(&gyro->dev, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN + spiWriteReg(&gyro->dev, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN delay(15); #ifdef USE_MPU_DATA_READY_SIGNAL - spiBusWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable + spiWriteReg(&gyro->dev, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable #endif - spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(ICM20689_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM20689_MAX_SPI_CLK_HZ)); } bool icm20689SpiGyroDetect(gyroDev_t *gyro) diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.h b/src/main/drivers/accgyro/accgyro_spi_icm20689.h index 53188a282..45ffcc23b 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.h +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.h @@ -30,7 +30,7 @@ bool icm20689GyroDetect(gyroDev_t *gyro); void icm20689AccInit(accDev_t *acc); void icm20689GyroInit(gyroDev_t *gyro); -uint8_t icm20689SpiDetect(const busDevice_t *bus); +uint8_t icm20689SpiDetect(const extDevice_t *dev); bool icm20689SpiAccDetect(accDev_t *acc); bool icm20689SpiGyroDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/accgyro_spi_icm42605.c b/src/main/drivers/accgyro/accgyro_spi_icm42605.c index f81699fcc..31a7a9a04 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm42605.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm42605.c @@ -95,7 +95,7 @@ #define ICM42605_UI_DRDY_INT1_EN_DISABLED (0 << 3) #define ICM42605_UI_DRDY_INT1_EN_ENABLED (1 << 3) -static void icm42605SpiInit(const busDevice_t *bus) +static void icm42605SpiInit(const extDevice_t *dev) { static bool hardwareInitialised = false; @@ -104,24 +104,24 @@ static void icm42605SpiInit(const busDevice_t *bus) } - spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(ICM42605_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(dev, spiCalculateDivider(ICM42605_MAX_SPI_CLK_HZ)); hardwareInitialised = true; } -uint8_t icm42605SpiDetect(const busDevice_t *bus) +uint8_t icm42605SpiDetect(const extDevice_t *dev) { - icm42605SpiInit(bus); + icm42605SpiInit(dev); - spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(ICM42605_MAX_SPI_INIT_CLK_HZ)); + spiSetClkDivisor(dev, spiCalculateDivider(ICM42605_MAX_SPI_INIT_CLK_HZ)); - spiBusWriteRegister(bus, ICM42605_RA_PWR_MGMT0, 0x00); + spiWriteReg(dev, ICM42605_RA_PWR_MGMT0, 0x00); uint8_t icmDetected = MPU_NONE; uint8_t attemptsRemaining = 20; do { delay(150); - const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I); + const uint8_t whoAmI = spiReadRegMsk(dev, MPU_RA_WHO_AM_I); switch (whoAmI) { case ICM42605_WHO_AM_I_CONST: icmDetected = ICM_42605_SPI; @@ -138,7 +138,7 @@ uint8_t icm42605SpiDetect(const busDevice_t *bus) } } while (attemptsRemaining--); - spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(ICM42605_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(dev, spiCalculateDivider(ICM42605_MAX_SPI_CLK_HZ)); return icmDetected; } @@ -152,7 +152,7 @@ bool icm42605AccRead(accDev_t *acc) { uint8_t data[6]; - const bool ack = busReadRegisterBuffer(&acc->bus, ICM42605_RA_ACCEL_DATA_X1, data, 6); + const bool ack = busReadRegisterBuffer(&acc->gyro->dev, ICM42605_RA_ACCEL_DATA_X1, data, 6); if (!ack) { return false; } @@ -194,9 +194,9 @@ void icm42605GyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); - spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(ICM42605_MAX_SPI_INIT_CLK_HZ)); + spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM42605_MAX_SPI_INIT_CLK_HZ)); - spiBusWriteRegister(&gyro->bus, ICM42605_RA_PWR_MGMT0, ICM42605_PWR_MGMT0_TEMP_DISABLE_OFF | ICM42605_PWR_MGMT0_ACCEL_MODE_LN | ICM42605_PWR_MGMT0_GYRO_MODE_LN); + spiWriteReg(&gyro->dev, ICM42605_RA_PWR_MGMT0, ICM42605_PWR_MGMT0_TEMP_DISABLE_OFF | ICM42605_PWR_MGMT0_ACCEL_MODE_LN | ICM42605_PWR_MGMT0_GYRO_MODE_LN); delay(15); uint8_t outputDataRate = 0; @@ -220,39 +220,39 @@ void icm42605GyroInit(gyroDev_t *gyro) } STATIC_ASSERT(INV_FSR_2000DPS == 3, "INV_FSR_2000DPS must be 3 to generate correct value"); - spiBusWriteRegister(&gyro->bus, ICM42605_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (outputDataRate & 0x0F)); + spiWriteReg(&gyro->dev, ICM42605_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (outputDataRate & 0x0F)); delay(15); STATIC_ASSERT(INV_FSR_16G == 3, "INV_FSR_16G must be 3 to generate correct value"); - spiBusWriteRegister(&gyro->bus, ICM42605_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (outputDataRate & 0x0F)); + spiWriteReg(&gyro->dev, ICM42605_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (outputDataRate & 0x0F)); delay(15); - spiBusWriteRegister(&gyro->bus, ICM42605_RA_GYRO_ACCEL_CONFIG0, ICM42605_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM42605_GYRO_UI_FILT_BW_LOW_LATENCY); + spiWriteReg(&gyro->dev, ICM42605_RA_GYRO_ACCEL_CONFIG0, ICM42605_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM42605_GYRO_UI_FILT_BW_LOW_LATENCY); - spiBusWriteRegister(&gyro->bus, ICM42605_RA_INT_CONFIG, ICM42605_INT1_MODE_PULSED | ICM42605_INT1_DRIVE_CIRCUIT_PP | ICM42605_INT1_POLARITY_ACTIVE_HIGH); - spiBusWriteRegister(&gyro->bus, ICM42605_RA_INT_CONFIG0, ICM42605_UI_DRDY_INT_CLEAR_ON_SBR); + spiWriteReg(&gyro->dev, ICM42605_RA_INT_CONFIG, ICM42605_INT1_MODE_PULSED | ICM42605_INT1_DRIVE_CIRCUIT_PP | ICM42605_INT1_POLARITY_ACTIVE_HIGH); + spiWriteReg(&gyro->dev, ICM42605_RA_INT_CONFIG0, ICM42605_UI_DRDY_INT_CLEAR_ON_SBR); #ifdef USE_MPU_DATA_READY_SIGNAL - spiBusWriteRegister(&gyro->bus, ICM42605_RA_INT_SOURCE0, ICM42605_UI_DRDY_INT1_EN_ENABLED); + spiWriteReg(&gyro->dev, ICM42605_RA_INT_SOURCE0, ICM42605_UI_DRDY_INT1_EN_ENABLED); - uint8_t intConfig1Value = spiBusReadRegister(&gyro->bus, ICM42605_RA_INT_CONFIG1); + uint8_t intConfig1Value = spiReadRegMsk(&gyro->dev, ICM42605_RA_INT_CONFIG1); // Datasheet says: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation" intConfig1Value &= ~(1 << ICM42605_INT_ASYNC_RESET_BIT); intConfig1Value |= (ICM42605_INT_TPULSE_DURATION_8 | ICM42605_INT_TDEASSERT_DISABLED); - spiBusWriteRegister(&gyro->bus, ICM42605_RA_INT_CONFIG1, intConfig1Value); + spiWriteReg(&gyro->dev, ICM42605_RA_INT_CONFIG1, intConfig1Value); #endif // - spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(ICM42605_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM42605_MAX_SPI_CLK_HZ)); } bool icm42605GyroReadSPI(gyroDev_t *gyro) { - static const uint8_t dataToSend[7] = {ICM42605_RA_GYRO_DATA_X1 | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; - uint8_t data[7]; + STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {ICM42605_RA_GYRO_DATA_X1 | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; + STATIC_DMA_DATA_AUTO uint8_t data[7]; - const bool ack = spiBusTransfer(&gyro->bus, dataToSend, data, 7); + const bool ack = spiReadWriteBufRB(&gyro->dev, dataToSend, data, 7); if (!ack) { return false; } diff --git a/src/main/drivers/accgyro/accgyro_spi_icm42605.h b/src/main/drivers/accgyro/accgyro_spi_icm42605.h index ec302a4b4..56a8da612 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm42605.h +++ b/src/main/drivers/accgyro/accgyro_spi_icm42605.h @@ -28,7 +28,7 @@ bool icm42605GyroDetect(gyroDev_t *gyro); void icm42605AccInit(accDev_t *acc); void icm42605GyroInit(gyroDev_t *gyro); -uint8_t icm42605SpiDetect(const busDevice_t *bus); +uint8_t icm42605SpiDetect(const extDevice_t *dev); bool icm42605SpiAccDetect(accDev_t *acc); bool icm42605SpiGyroDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/accgyro_spi_l3gd20.c b/src/main/drivers/accgyro/accgyro_spi_l3gd20.c index 24ed2bfcb..aa9c83837 100644 --- a/src/main/drivers/accgyro/accgyro_spi_l3gd20.c +++ b/src/main/drivers/accgyro/accgyro_spi_l3gd20.c @@ -101,18 +101,18 @@ static void l3gd20IntExtiInit(gyroDev_t *gyro) void l3gd20GyroInit(gyroDev_t *gyro) { - spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(L3GD20_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(dev, spiCalculateDivider(L3GD20_MAX_SPI_CLK_HZ)); - spiBusWriteRegister(&gyro->bus, CTRL_REG5_ADDR, BOOT); + spiWriteReg(&gyro->bus, CTRL_REG5_ADDR, BOOT); delayMicroseconds(100); - spiBusWriteRegister(&gyro->bus, CTRL_REG1_ADDR, MODE_ACTIVE | OUTPUT_DATARATE_3 | AXES_ENABLE | BANDWIDTH_3); - //spiBusWriteRegister(&gyro->bus, CTRL_REG1_ADDR. MODE_ACTIVE | OUTPUT_DATARATE_3 | AXES_ENABLE | BANDWIDTH_4); + spiWriteReg(&gyro->bus, CTRL_REG1_ADDR, MODE_ACTIVE | OUTPUT_DATARATE_3 | AXES_ENABLE | BANDWIDTH_3); + //spiWriteReg(&gyro->bus, CTRL_REG1_ADDR. MODE_ACTIVE | OUTPUT_DATARATE_3 | AXES_ENABLE | BANDWIDTH_4); delayMicroseconds(1); - spiBusWriteRegister(&gyro->bus, CTRL_REG4_ADDR, BLOCK_DATA_UPDATE_CONTINUOUS | BLE_MSB | FULLSCALE_2000); + spiWriteReg(&gyro->bus, CTRL_REG4_ADDR, BLOCK_DATA_UPDATE_CONTINUOUS | BLE_MSB | FULLSCALE_2000); delay(100); @@ -126,7 +126,10 @@ static bool l3gd20GyroRead(gyroDev_t *gyro) { uint8_t buf[6]; - spiBusReadRegisterBuffer(&gyro->bus, OUT_X_L_ADDR | READ_CMD | MULTIPLEBYTE_CMD,buf, sizeof(buf)); + const bool ack = spiReadRegMskBufRB(&gyro->bus, OUT_X_L_ADDR | READ_CMD | MULTIPLEBYTE_CMD,buf, sizeof(buf)); + if (!ack) { + return false; + } gyro->gyroADCRaw[0] = (int16_t)((buf[0] << 8) | buf[1]); gyro->gyroADCRaw[1] = (int16_t)((buf[2] << 8) | buf[3]); diff --git a/src/main/drivers/accgyro/accgyro_spi_lsm6dso.c b/src/main/drivers/accgyro/accgyro_spi_lsm6dso.c index 074401fae..e80ed1fb8 100644 --- a/src/main/drivers/accgyro/accgyro_spi_lsm6dso.c +++ b/src/main/drivers/accgyro/accgyro_spi_lsm6dso.c @@ -54,8 +54,8 @@ bool lsm6dsoAccRead(accDev_t *acc) uint8_t lsm6dso_rx_buf[BUFFER_SIZE]; - const busDevice_t *busdev = &acc->bus; - busReadRegisterBuffer(busdev, LSM6DSO_REG_OUTX_L_A, lsm6dso_rx_buf, BUFFER_SIZE); + extDevice_t *dev = &acc->gyro->dev; + busReadRegisterBuffer(dev, LSM6DSO_REG_OUTX_L_A, lsm6dso_rx_buf, BUFFER_SIZE); acc->ADCRaw[X] = (int16_t)((lsm6dso_rx_buf[IDX_ACCEL_XOUT_H] << 8) | lsm6dso_rx_buf[IDX_ACCEL_XOUT_L]); acc->ADCRaw[Y] = (int16_t)((lsm6dso_rx_buf[IDX_ACCEL_YOUT_H] << 8) | lsm6dso_rx_buf[IDX_ACCEL_YOUT_L]); @@ -78,8 +78,8 @@ bool lsm6dsoGyroRead(gyroDev_t *gyro) uint8_t lsm6dso_rx_buf[BUFFER_SIZE]; - const busDevice_t *busdev = &gyro->bus; - busReadRegisterBuffer(busdev, LSM6DSO_REG_OUTX_L_G, lsm6dso_rx_buf, BUFFER_SIZE); + extDevice_t *dev = &gyro->dev; + busReadRegisterBuffer(dev, LSM6DSO_REG_OUTX_L_G, lsm6dso_rx_buf, BUFFER_SIZE); gyro->gyroADCRaw[X] = (int16_t)((lsm6dso_rx_buf[IDX_GYRO_XOUT_H] << 8) | lsm6dso_rx_buf[IDX_GYRO_XOUT_L]); gyro->gyroADCRaw[Y] = (int16_t)((lsm6dso_rx_buf[IDX_GYRO_YOUT_H] << 8) | lsm6dso_rx_buf[IDX_GYRO_YOUT_L]); diff --git a/src/main/drivers/accgyro/accgyro_spi_lsm6dso.h b/src/main/drivers/accgyro/accgyro_spi_lsm6dso.h index 2484c800e..ebcac8e7d 100644 --- a/src/main/drivers/accgyro/accgyro_spi_lsm6dso.h +++ b/src/main/drivers/accgyro/accgyro_spi_lsm6dso.h @@ -56,7 +56,7 @@ typedef enum { } lsm6dsoRegister_e; // Contained in accgyro_spi_lsm6dso_init.c which is size-optimized -uint8_t lsm6dsoDetect(const busDevice_t *bus); +uint8_t lsm6dsoDetect(const extDevice_t *dev); bool lsm6dsoSpiAccDetect(accDev_t *acc); bool lsm6dsoSpiGyroDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/accgyro_spi_lsm6dso_init.c b/src/main/drivers/accgyro/accgyro_spi_lsm6dso_init.c index 032be8a48..160278dea 100644 --- a/src/main/drivers/accgyro/accgyro_spi_lsm6dso_init.c +++ b/src/main/drivers/accgyro/accgyro_spi_lsm6dso_init.c @@ -78,12 +78,12 @@ typedef enum { LSM6DSO_MASK_CTRL9_XL = 0x02, // 0b00000010 } lsm6dsoConfigMasks_e; -uint8_t lsm6dsoDetect(const busDevice_t *bus) +uint8_t lsm6dsoDetect(const extDevice_t *dev) { uint8_t chipID = 0; - spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(LSM6DSO_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(dev, spiCalculateDivider(LSM6DSO_MAX_SPI_CLK_HZ)); - if (busReadRegisterBuffer(bus, LSM6DSO_REG_WHO_AM_I, &chipID, 1)) { + if (busReadRegisterBuffer(dev, LSM6DSO_REG_WHO_AM_I, &chipID, 1)) { if (chipID == LSM6DSO_CHIP_ID) { return LSM6DSO_SPI; } @@ -92,60 +92,60 @@ uint8_t lsm6dsoDetect(const busDevice_t *bus) return MPU_NONE; } -static void lsm6dsoWriteRegister(const busDevice_t *bus, lsm6dsoRegister_e registerID, uint8_t value, unsigned delayMs) +static void lsm6dsoWriteRegister(const extDevice_t *dev, lsm6dsoRegister_e registerID, uint8_t value, unsigned delayMs) { - busWriteRegister(bus, registerID, value); + busWriteRegister(dev, registerID, value); if (delayMs) { delay(delayMs); } } -static void lsm6dsoWriteRegisterBits(const busDevice_t *bus, lsm6dsoRegister_e registerID, lsm6dsoConfigMasks_e mask, uint8_t value, unsigned delayMs) +static void lsm6dsoWriteRegisterBits(const extDevice_t *dev, lsm6dsoRegister_e registerID, lsm6dsoConfigMasks_e mask, uint8_t value, unsigned delayMs) { uint8_t newValue; - if (busReadRegisterBuffer(bus, registerID, &newValue, 1)) { + if (busReadRegisterBuffer(dev, registerID, &newValue, 1)) { delayMicroseconds(2); newValue = (newValue & ~mask) | value; - lsm6dsoWriteRegister(bus, registerID, newValue, delayMs); + lsm6dsoWriteRegister(dev, registerID, newValue, delayMs); } } -static void lsm6dsoConfig(const gyroDev_t *gyro) +static void lsm6dsoConfig(gyroDev_t *gyro) { - const busDevice_t *bus = &gyro->bus; + extDevice_t *dev = &gyro->dev; // Reset the device (wait 100ms before continuing config) - lsm6dsoWriteRegisterBits(bus, LSM6DSO_REG_CTRL3_C, LSM6DSO_MASK_CTRL3_C_RESET, BIT(0), 100); + lsm6dsoWriteRegisterBits(dev, LSM6DSO_REG_CTRL3_C, LSM6DSO_MASK_CTRL3_C_RESET, BIT(0), 100); // Configure interrupt pin 1 for gyro data ready only - lsm6dsoWriteRegister(bus, LSM6DSO_REG_INT1_CTRL, LSM6DSO_VAL_INT1_CTRL, 1); + lsm6dsoWriteRegister(dev, LSM6DSO_REG_INT1_CTRL, LSM6DSO_VAL_INT1_CTRL, 1); // Disable interrupt pin 2 - lsm6dsoWriteRegister(bus, LSM6DSO_REG_INT2_CTRL, LSM6DSO_VAL_INT2_CTRL, 1); + lsm6dsoWriteRegister(dev, LSM6DSO_REG_INT2_CTRL, LSM6DSO_VAL_INT2_CTRL, 1); // Configure the accelerometer // 833hz ODR, 16G scale, use LPF1 output - lsm6dsoWriteRegister(bus, LSM6DSO_REG_CTRL1_XL, (LSM6DSO_VAL_CTRL1_XL_ODR833 << 4) | (LSM6DSO_VAL_CTRL1_XL_16G << 2) | (LSM6DSO_VAL_CTRL1_XL_LPF1 << 1), 1); + lsm6dsoWriteRegister(dev, LSM6DSO_REG_CTRL1_XL, (LSM6DSO_VAL_CTRL1_XL_ODR833 << 4) | (LSM6DSO_VAL_CTRL1_XL_16G << 2) | (LSM6DSO_VAL_CTRL1_XL_LPF1 << 1), 1); // Configure the gyro // 6664hz ODR, 2000dps scale - lsm6dsoWriteRegister(bus, LSM6DSO_REG_CTRL2_G, (LSM6DSO_VAL_CTRL2_G_ODR6664 << 4) | (LSM6DSO_VAL_CTRL2_G_2000DPS << 2), 1); + lsm6dsoWriteRegister(dev, LSM6DSO_REG_CTRL2_G, (LSM6DSO_VAL_CTRL2_G_ODR6664 << 4) | (LSM6DSO_VAL_CTRL2_G_2000DPS << 2), 1); // Configure control register 3 // latch LSB/MSB during reads; set interrupt pins active high; set interrupt pins push/pull; set 4-wire SPI; enable auto-increment burst reads - lsm6dsoWriteRegisterBits(bus, LSM6DSO_REG_CTRL3_C, LSM6DSO_MASK_CTRL3_C, (LSM6DSO_VAL_CTRL3_C_BDU | LSM6DSO_VAL_CTRL3_C_H_LACTIVE | LSM6DSO_VAL_CTRL3_C_PP_OD | LSM6DSO_VAL_CTRL3_C_SIM | LSM6DSO_VAL_CTRL3_C_IF_INC), 1); + lsm6dsoWriteRegisterBits(dev, LSM6DSO_REG_CTRL3_C, LSM6DSO_MASK_CTRL3_C, (LSM6DSO_VAL_CTRL3_C_BDU | LSM6DSO_VAL_CTRL3_C_H_LACTIVE | LSM6DSO_VAL_CTRL3_C_PP_OD | LSM6DSO_VAL_CTRL3_C_SIM | LSM6DSO_VAL_CTRL3_C_IF_INC), 1); // Configure control register 4 // enable accelerometer high performane mode; set gyro LPF1 cutoff to 335.5hz - lsm6dsoWriteRegisterBits(bus, LSM6DSO_REG_CTRL4_C, LSM6DSO_MASK_CTRL4_C, (LSM6DSO_VAL_CTRL4_C_I2C_DISABLE | LSM6DSO_VAL_CTRL4_C_LPF1_SEL_G), 1); + lsm6dsoWriteRegisterBits(dev, LSM6DSO_REG_CTRL4_C, LSM6DSO_MASK_CTRL4_C, (LSM6DSO_VAL_CTRL4_C_I2C_DISABLE | LSM6DSO_VAL_CTRL4_C_LPF1_SEL_G), 1); // Configure control register 6 // disable I2C interface; enable gyro LPF1 - lsm6dsoWriteRegisterBits(bus, LSM6DSO_REG_CTRL6_C, LSM6DSO_MASK_CTRL6_C, (LSM6DSO_VAL_CTRL6_C_XL_HM_MODE | LSM6DSO_VAL_CTRL6_C_FTYPE_335HZ), 1); + lsm6dsoWriteRegisterBits(dev, LSM6DSO_REG_CTRL6_C, LSM6DSO_MASK_CTRL6_C, (LSM6DSO_VAL_CTRL6_C_XL_HM_MODE | LSM6DSO_VAL_CTRL6_C_FTYPE_335HZ), 1); // Configure control register 9 // disable I3C interface - lsm6dsoWriteRegisterBits(bus, LSM6DSO_REG_CTRL9_XL, LSM6DSO_MASK_CTRL9_XL, LSM6DSO_VAL_CTRL9_XL_I3C_DISABLE, 1); + lsm6dsoWriteRegisterBits(dev, LSM6DSO_REG_CTRL9_XL, LSM6DSO_MASK_CTRL9_XL, LSM6DSO_VAL_CTRL9_XL_I3C_DISABLE, 1); } #if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL) diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c index 848807b20..ae8d8d56e 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c @@ -107,13 +107,13 @@ void mpu6000SpiGyroInit(gyroDev_t *gyro) mpu6000AccAndGyroInit(gyro); - spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(MPU6000_MAX_SPI_INIT_CLK_HZ)); + spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU6000_MAX_SPI_INIT_CLK_HZ)); // Accel and Gyro DLPF Setting - spiBusWriteRegister(&gyro->bus, MPU6000_CONFIG, mpuGyroDLPF(gyro)); + spiWriteReg(&gyro->dev, MPU6000_CONFIG, mpuGyroDLPF(gyro)); delayMicroseconds(1); - spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(MPU6000_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU6000_MAX_SPI_CLK_HZ)); mpuGyroRead(gyro); @@ -127,25 +127,26 @@ void mpu6000SpiAccInit(accDev_t *acc) acc->acc_1G = 512 * 4; } -uint8_t mpu6000SpiDetect(const busDevice_t *bus) +uint8_t mpu6000SpiDetect(const extDevice_t *dev) { - spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(MPU6000_MAX_SPI_INIT_CLK_HZ)); + spiSetClkDivisor(dev, spiCalculateDivider(MPU6000_MAX_SPI_INIT_CLK_HZ)); // reset the device configuration - spiBusWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET); + spiWriteReg(dev, MPU_RA_PWR_MGMT_1, BIT_H_RESET); delay(100); // datasheet specifies a 100ms delay after reset // reset the device signal paths - spiBusWriteRegister(bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP); + spiWriteReg(dev, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP); delay(100); // datasheet specifies a 100ms delay after signal path reset - const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I); + const uint8_t whoAmI = spiReadRegMsk(dev, MPU_RA_WHO_AM_I); + delayMicroseconds(1); // Ensure CS high time is met which is violated on H7 without this delay uint8_t detectedSensor = MPU_NONE; if (whoAmI == MPU6000_WHO_AM_I_CONST) { - const uint8_t productID = spiBusReadRegister(bus, MPU_RA_PRODUCT_ID); + const uint8_t productID = spiReadRegMsk(dev, MPU_RA_PRODUCT_ID); /* look for a product ID we recognise */ @@ -167,49 +168,49 @@ uint8_t mpu6000SpiDetect(const busDevice_t *bus) } } - spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(MPU6000_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(dev, spiCalculateDivider(MPU6000_MAX_SPI_CLK_HZ)); return detectedSensor; } static void mpu6000AccAndGyroInit(gyroDev_t *gyro) { - spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(MPU6000_MAX_SPI_INIT_CLK_HZ)); + spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU6000_MAX_SPI_INIT_CLK_HZ)); // Device was already reset during detection so proceed with configuration // Clock Source PPL with Z axis gyro reference - spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); + spiWriteReg(&gyro->dev, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); delayMicroseconds(15); // Disable Primary I2C Interface - spiBusWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS); + spiWriteReg(&gyro->dev, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS); delayMicroseconds(15); - spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_2, 0x00); + spiWriteReg(&gyro->dev, MPU_RA_PWR_MGMT_2, 0x00); delayMicroseconds(15); // Accel Sample Rate 1kHz // Gyroscope Output Rate = 1kHz when the DLPF is enabled - spiBusWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); + spiWriteReg(&gyro->dev, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); delayMicroseconds(15); // Gyro +/- 2000 DPS Full Scale - spiBusWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); + spiWriteReg(&gyro->dev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); delayMicroseconds(15); // Accel +/- 16 G Full Scale - spiBusWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + spiWriteReg(&gyro->dev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delayMicroseconds(15); - spiBusWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR + spiWriteReg(&gyro->dev, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR delayMicroseconds(15); #ifdef USE_MPU_DATA_READY_SIGNAL - spiBusWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); + spiWriteReg(&gyro->dev, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); delayMicroseconds(15); #endif - spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(MPU6000_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU6000_MAX_SPI_CLK_HZ)); delayMicroseconds(1); } @@ -220,7 +221,7 @@ bool mpu6000SpiAccDetect(accDev_t *acc) } acc->initFn = mpu6000SpiAccInit; - acc->readFn = mpuAccRead; + acc->readFn = mpuAccReadSPI; return true; } diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.h b/src/main/drivers/accgyro/accgyro_spi_mpu6000.h index b6d31c3ba..79ca7f93e 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.h +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6000.h @@ -34,7 +34,7 @@ // RF = Register Flag #define MPU_RF_DATA_RDY_EN (1 << 0) -uint8_t mpu6000SpiDetect(const busDevice_t *bus); +uint8_t mpu6000SpiDetect(const extDevice_t *dev); bool mpu6000SpiAccDetect(accDev_t *acc); bool mpu6000SpiGyroDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c index 7801007fc..051cdbfa2 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c @@ -44,17 +44,17 @@ #define BIT_SLEEP 0x40 -static void mpu6500SpiInit(const busDevice_t *bus) +static void mpu6500SpiInit(const extDevice_t *dev) { - spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(MPU6500_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(dev, spiCalculateDivider(MPU6500_MAX_SPI_CLK_HZ)); } -uint8_t mpu6500SpiDetect(const busDevice_t *bus) +uint8_t mpu6500SpiDetect(const extDevice_t *dev) { - mpu6500SpiInit(bus); + mpu6500SpiInit(dev); - const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I); + const uint8_t whoAmI = spiReadRegMsk(dev, MPU_RA_WHO_AM_I); uint8_t mpuDetected = MPU_NONE; switch (whoAmI) { @@ -90,16 +90,16 @@ void mpu6500SpiAccInit(accDev_t *acc) void mpu6500SpiGyroInit(gyroDev_t *gyro) { - spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(MPU6500_MAX_SPI_INIT_CLK_HZ)); + spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU6500_MAX_SPI_INIT_CLK_HZ)); delayMicroseconds(1); mpu6500GyroInit(gyro); // Disable Primary I2C Interface - spiBusWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS); + spiWriteReg(&gyro->dev, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS); delay(100); - spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(MPU6500_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU6500_MAX_SPI_CLK_HZ)); delayMicroseconds(1); } @@ -118,7 +118,7 @@ bool mpu6500SpiAccDetect(accDev_t *acc) } acc->initFn = mpu6500SpiAccInit; - acc->readFn = mpuAccRead; + acc->readFn = mpuAccReadSPI; return true; } diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.h b/src/main/drivers/accgyro/accgyro_spi_mpu6500.h index 099bd3886..2bf5d5f28 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.h +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6500.h @@ -22,7 +22,7 @@ #include "drivers/bus.h" -uint8_t mpu6500SpiDetect(const busDevice_t *bus); +uint8_t mpu6500SpiDetect(const extDevice_t *dev); bool mpu6500SpiAccDetect(accDev_t *acc); bool mpu6500SpiGyroDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c index 4a74a74d9..e69b09496 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c @@ -55,25 +55,19 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro); -bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) +bool mpu9250SpiWriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data) { - IOLo(bus->busdev_u.spi.csnPin); delayMicroseconds(1); - spiTransferByte(bus->busdev_u.spi.instance, reg); - spiTransferByte(bus->busdev_u.spi.instance, data); - IOHi(bus->busdev_u.spi.csnPin); + spiWriteRegBuf(dev, reg, &data, sizeof (data)); delayMicroseconds(1); return true; } -static bool mpu9250SpiSlowReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length) +static bool mpu9250SpiSlowReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length) { - IOLo(bus->busdev_u.spi.csnPin); delayMicroseconds(1); - spiTransferByte(bus->busdev_u.spi.instance, reg | 0x80); // read transaction - spiTransfer(bus->busdev_u.spi.instance, NULL, data, length); - IOHi(bus->busdev_u.spi.csnPin); + spiReadRegBuf(dev, reg | 0x80, data, length); delayMicroseconds(1); return true; @@ -85,14 +79,11 @@ void mpu9250SpiGyroInit(gyroDev_t *gyro) mpu9250AccAndGyroInit(gyro); - spiResetErrorCounter(gyro->bus.busdev_u.spi.instance); - - spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(MPU9250_MAX_SPI_CLK_HZ)); //high speed now that we don't need to write to the slow registers + spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU9250_MAX_SPI_CLK_HZ)); //high speed now that we don't need to write to the slow registers mpuGyroRead(gyro); - if ((((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) || spiGetErrorCounter(gyro->bus.busdev_u.spi.instance) != 0) { - spiResetErrorCounter(gyro->bus.busdev_u.spi.instance); + if ((((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1)) { failureMode(FAILURE_GYRO_INIT_FAILED); } } @@ -102,20 +93,20 @@ void mpu9250SpiAccInit(accDev_t *acc) acc->acc_1G = 512 * 4; } -bool mpu9250SpiWriteRegisterVerify(const busDevice_t *bus, uint8_t reg, uint8_t data) +bool mpu9250SpiWriteRegisterVerify(const extDevice_t *dev, uint8_t reg, uint8_t data) { - mpu9250SpiWriteRegister(bus, reg, data); + mpu9250SpiWriteRegister(dev, reg, data); delayMicroseconds(100); uint8_t attemptsRemaining = 20; do { uint8_t in; - mpu9250SpiSlowReadRegisterBuffer(bus, reg, &in, 1); + mpu9250SpiSlowReadRegisterBuffer(dev, reg, &in, 1); if (in == data) { return true; } else { debug[3]++; - mpu9250SpiWriteRegister(bus, reg, data); + mpu9250SpiWriteRegister(dev, reg, data); delayMicroseconds(100); } } while (attemptsRemaining--); @@ -124,39 +115,39 @@ bool mpu9250SpiWriteRegisterVerify(const busDevice_t *bus, uint8_t reg, uint8_t static void mpu9250AccAndGyroInit(gyroDev_t *gyro) { - spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(MPU9250_MAX_SPI_INIT_CLK_HZ)); //low speed for writing to slow registers + spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU9250_MAX_SPI_INIT_CLK_HZ)); //low speed for writing to slow registers - mpu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); + mpu9250SpiWriteRegister(&gyro->dev, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); delay(50); - mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); + mpu9250SpiWriteRegisterVerify(&gyro->dev, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); - mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); + mpu9250SpiWriteRegisterVerify(&gyro->dev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); - mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_CONFIG, mpuGyroDLPF(gyro)); + mpu9250SpiWriteRegisterVerify(&gyro->dev, MPU_RA_CONFIG, mpuGyroDLPF(gyro)); - mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); + mpu9250SpiWriteRegisterVerify(&gyro->dev, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); - mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); - mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN + mpu9250SpiWriteRegisterVerify(&gyro->dev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + mpu9250SpiWriteRegisterVerify(&gyro->dev, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN #if defined(USE_MPU_DATA_READY_SIGNAL) - mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly. + mpu9250SpiWriteRegisterVerify(&gyro->dev, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly. #endif - spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(MPU9250_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU9250_MAX_SPI_CLK_HZ)); } -uint8_t mpu9250SpiDetect(const busDevice_t *bus) +uint8_t mpu9250SpiDetect(const extDevice_t *dev) { - spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(MPU9250_MAX_SPI_INIT_CLK_HZ)); //low speed - mpu9250SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); + spiSetClkDivisor(dev, spiCalculateDivider(MPU9250_MAX_SPI_INIT_CLK_HZ)); //low speed + mpu9250SpiWriteRegister(dev, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); uint8_t attemptsRemaining = 20; do { delay(150); - const uint8_t in = spiBusReadRegister(bus, MPU_RA_WHO_AM_I); + const uint8_t in = spiReadRegMsk(dev, MPU_RA_WHO_AM_I); if (in == MPU9250_WHO_AM_I_CONST || in == MPU9255_WHO_AM_I_CONST) { break; } @@ -165,7 +156,7 @@ uint8_t mpu9250SpiDetect(const busDevice_t *bus) } } while (attemptsRemaining--); - spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(MPU9250_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(dev, spiCalculateDivider(MPU9250_MAX_SPI_CLK_HZ)); return MPU_9250_SPI; } @@ -177,7 +168,7 @@ bool mpu9250SpiAccDetect(accDev_t *acc) } acc->initFn = mpu9250SpiAccInit; - acc->readFn = mpuAccRead; + acc->readFn = mpuAccReadSPI; return true; } diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.h b/src/main/drivers/accgyro/accgyro_spi_mpu9250.h index 9bae748a6..ab721d94a 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.h +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.h @@ -44,11 +44,11 @@ void mpu9250SpiResetGyro(void); -uint8_t mpu9250SpiDetect(const busDevice_t *bus); +uint8_t mpu9250SpiDetect(const extDevice_t *dev); bool mpu9250SpiAccDetect(accDev_t *acc); bool mpu9250SpiGyroDetect(gyroDev_t *gyro); -bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); -bool mpu9250SpiWriteRegisterVerify(const busDevice_t *bus, uint8_t reg, uint8_t data); -bool mpu9250SpiReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); +bool mpu9250SpiWriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data); +bool mpu9250SpiWriteRegisterVerify(const extDevice_t *dev, uint8_t reg, uint8_t data); +bool mpu9250SpiReadRegBuf(const extDevice_t *dev, uint8_t reg, uint8_t length, uint8_t *data); diff --git a/src/main/drivers/barometer/barometer.h b/src/main/drivers/barometer/barometer.h index ce85fd752..88afe8a98 100644 --- a/src/main/drivers/barometer/barometer.h +++ b/src/main/drivers/barometer/barometer.h @@ -31,7 +31,7 @@ typedef void (*baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); / // the 'u' in these variable names means 'uncompensated', 't' is temperature, 'p' pressure. typedef struct baroDev_s { - busDevice_t busdev; + extDevice_t dev; #ifdef USE_EXTI extiCallbackRec_t exti; #endif diff --git a/src/main/drivers/barometer/barometer_bmp085.c b/src/main/drivers/barometer/barometer_bmp085.c index 208be5b28..1be968361 100644 --- a/src/main/drivers/barometer/barometer_bmp085.c +++ b/src/main/drivers/barometer/barometer_bmp085.c @@ -124,7 +124,7 @@ static bool bmp085InitDone = false; STATIC_UNIT_TESTED uint16_t bmp085_ut; // static result of temperature measurement STATIC_UNIT_TESTED uint32_t bmp085_up; // static result of pressure measurement -static void bmp085ReadCalibrarionParameters(busDevice_t *busdev); +static void bmp085ReadCalibrarionParameters(const extDevice_t *dev); static void bmp085StartUT(baroDev_t *baro); static bool bmp085ReadUT(baroDev_t *baro); static bool bmp085GetUT(baroDev_t *baro); @@ -175,26 +175,26 @@ bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro) delay(20); // datasheet says 10ms, we'll be careful and do 20. - busDevice_t *busdev = &baro->busdev; + extDevice_t *dev = &baro->dev; - if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) { + if ((dev->bus->busType == BUS_TYPE_I2C) && (dev->busType_u.i2c.address == 0)) { // Default address for BMP085 - busdev->busdev_u.i2c.address = BMP085_I2C_ADDR; + dev->busType_u.i2c.address = BMP085_I2C_ADDR; defaultAddressApplied = true; } - ack = busReadRegisterBuffer(busdev, BMP085_CHIP_ID__REG, &data, 1); /* read Chip Id */ + ack = busReadRegisterBuffer(dev, BMP085_CHIP_ID__REG, &data, 1); /* read Chip Id */ if (ack) { bmp085.chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID); bmp085.oversampling_setting = 3; if (bmp085.chip_id == BMP085_CHIP_ID) { /* get bitslice */ - busDeviceRegister(busdev); + busDeviceRegister(dev); - busReadRegisterBuffer(busdev, BMP085_VERSION_REG, &data, 1); /* read Version reg */ + busReadRegisterBuffer(dev, BMP085_VERSION_REG, &data, 1); /* read Version reg */ bmp085.ml_version = BMP085_GET_BITSLICE(data, BMP085_ML_VERSION); /* get ML Version */ bmp085.al_version = BMP085_GET_BITSLICE(data, BMP085_AL_VERSION); /* get AL Version */ - bmp085ReadCalibrarionParameters(busdev); /* readout bmp085 calibparam structure */ + bmp085ReadCalibrarionParameters(dev); /* readout bmp085 calibparam structure */ baro->ut_delay = UT_DELAY; baro->up_delay = UP_DELAY; baro->start_ut = bmp085StartUT; @@ -222,7 +222,7 @@ bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro) BMP085_OFF; if (defaultAddressApplied) { - busdev->busdev_u.i2c.address = 0; + dev->busType_u.i2c.address = 0; } return false; @@ -285,12 +285,12 @@ static void bmp085StartUT(baroDev_t *baro) { isConversionComplete = false; - busWriteRegisterStart(&baro->busdev, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE); + busWriteRegisterStart(&baro->dev, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE); } static bool bmp085ReadUT(baroDev_t *baro) { - if (busBusy(&baro->busdev, NULL)) { + if (busBusy(&baro->dev, NULL)) { return false; } @@ -299,14 +299,14 @@ static bool bmp085ReadUT(baroDev_t *baro) return false; } - busReadRegisterBufferStart(&baro->busdev, BMP085_ADC_OUT_MSB_REG, sensor_data, BMP085_DATA_TEMP_SIZE); + busReadRegisterBufferStart(&baro->dev, BMP085_ADC_OUT_MSB_REG, sensor_data, BMP085_DATA_TEMP_SIZE); return true; } static bool bmp085GetUT(baroDev_t *baro) { - if (busBusy(&baro->busdev, NULL)) { + if (busBusy(&baro->dev, NULL)) { return false; } @@ -323,12 +323,12 @@ static void bmp085StartUP(baroDev_t *baro) isConversionComplete = false; - busWriteRegisterStart(&baro->busdev, BMP085_CTRL_MEAS_REG, ctrl_reg_data); + busWriteRegisterStart(&baro->dev, BMP085_CTRL_MEAS_REG, ctrl_reg_data); } static bool bmp085ReadUP(baroDev_t *baro) { - if (busBusy(&baro->busdev, NULL)) { + if (busBusy(&baro->dev, NULL)) { return false; } @@ -337,7 +337,7 @@ static bool bmp085ReadUP(baroDev_t *baro) return false; } - busReadRegisterBufferStart(&baro->busdev, BMP085_ADC_OUT_MSB_REG, sensor_data, BMP085_DATA_PRES_SIZE); + busReadRegisterBufferStart(&baro->dev, BMP085_ADC_OUT_MSB_REG, sensor_data, BMP085_DATA_PRES_SIZE); return true; } @@ -348,7 +348,7 @@ static bool bmp085ReadUP(baroDev_t *baro) */ static bool bmp085GetUP(baroDev_t *baro) { - if (busBusy(&baro->busdev, NULL)) { + if (busBusy(&baro->dev, NULL)) { return false; } @@ -370,10 +370,10 @@ STATIC_UNIT_TESTED void bmp085Calculate(int32_t *pressure, int32_t *temperature) *temperature = temp; } -static void bmp085ReadCalibrarionParameters(busDevice_t *busdev) +static void bmp085ReadCalibrarionParameters(const extDevice_t *dev) { uint8_t data[22]; - busReadRegisterBuffer(busdev, BMP085_PROM_START__ADDR, data, BMP085_PROM_DATA__LEN); + busReadRegisterBuffer(dev, BMP085_PROM_START__ADDR, data, BMP085_PROM_DATA__LEN); /*parameters AC1-AC6*/ bmp085.cal_param.ac1 = data[0] << 8 | data[1]; diff --git a/src/main/drivers/barometer/barometer_bmp280.c b/src/main/drivers/barometer/barometer_bmp280.c index ee878311a..d6e641620 100644 --- a/src/main/drivers/barometer/barometer_bmp280.c +++ b/src/main/drivers/barometer/barometer_bmp280.c @@ -107,7 +107,7 @@ STATIC_UNIT_TESTED bmp280_calib_param_t bmp280_cal; // uncompensated pressure and temperature int32_t bmp280_up = 0; int32_t bmp280_ut = 0; -static uint8_t sensor_data[BMP280_DATA_FRAME_SIZE]; +static DMA_DATA_ZERO_INIT uint8_t sensor_data[BMP280_DATA_FRAME_SIZE]; static void bmp280StartUT(baroDev_t *baro); static bool bmp280ReadUT(baroDev_t *baro); @@ -118,32 +118,28 @@ static bool bmp280GetUP(baroDev_t *baro); STATIC_UNIT_TESTED void bmp280Calculate(int32_t *pressure, int32_t *temperature); -void bmp280BusInit(busDevice_t *busdev) +void bmp280BusInit(const extDevice_t *dev) { #ifdef USE_BARO_SPI_BMP280 - if (busdev->bustype == BUSTYPE_SPI) { - IOHi(busdev->busdev_u.spi.csnPin); // Disable - IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0); - IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP); -#ifdef USE_SPI_TRANSACTION - spiBusTransactionInit(busdev, SPI_MODE0_POL_LOW_EDGE_1ST, spiCalculateDivider(BMP280_MAX_SPI_CLK_HZ)); // BMP280 supports Mode 0 or 3 -#else - spiBusSetDivisor(busdev, spiCalculateDivider(BMP280_MAX_SPI_CLK_HZ)); -#endif + if (dev->bus->busType == BUS_TYPE_SPI) { + IOHi(dev->busType_u.spi.csnPin); // Disable + IOInit(dev->busType_u.spi.csnPin, OWNER_BARO_CS, 0); + IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP); + spiSetClkDivisor(dev, spiCalculateDivider(BMP280_MAX_SPI_CLK_HZ)); } #else - UNUSED(busdev); + UNUSED(dev); #endif } -void bmp280BusDeinit(busDevice_t *busdev) +void bmp280BusDeinit(const extDevice_t *dev) { #ifdef USE_BARO_SPI_BMP280 - if (busdev->bustype == BUSTYPE_SPI) { - spiPreinitByIO(busdev->busdev_u.spi.csnPin); + if (dev->bus->busType == BUS_TYPE_SPI) { + spiPreinitByIO(dev->busType_u.spi.csnPin); } #else - UNUSED(busdev); + UNUSED(dev); #endif } @@ -153,34 +149,34 @@ bool bmp280Detect(baroDev_t *baro) { delay(20); - busDevice_t *busdev = &baro->busdev; + extDevice_t *dev = &baro->dev; bool defaultAddressApplied = false; - bmp280BusInit(busdev); + bmp280BusInit(dev); - if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) { + if ((dev->bus->busType == BUS_TYPE_I2C) && (dev->busType_u.i2c.address == 0)) { // Default address for BMP280 - busdev->busdev_u.i2c.address = BMP280_I2C_ADDR; + dev->busType_u.i2c.address = BMP280_I2C_ADDR; defaultAddressApplied = true; } - busReadRegisterBuffer(busdev, BMP280_CHIP_ID_REG, &bmp280_chip_id, 1); /* read Chip Id */ + busReadRegisterBuffer(dev, BMP280_CHIP_ID_REG, &bmp280_chip_id, 1); /* read Chip Id */ if ((bmp280_chip_id != BMP280_DEFAULT_CHIP_ID) && (bmp280_chip_id != BME280_DEFAULT_CHIP_ID)) { - bmp280BusDeinit(busdev); + bmp280BusDeinit(dev); if (defaultAddressApplied) { - busdev->busdev_u.i2c.address = 0; + dev->busType_u.i2c.address = 0; } return false; } - busDeviceRegister(busdev); + busDeviceRegister(dev); // read calibration - busReadRegisterBuffer(busdev, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, (uint8_t *)&bmp280_cal, sizeof(bmp280_calib_param_t)); + busReadRegisterBuffer(dev, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, (uint8_t *)&bmp280_cal, sizeof(bmp280_calib_param_t)); // set oversampling + power mode (forced), and start sampling - busWriteRegister(busdev, BMP280_CTRL_MEAS_REG, BMP280_MODE); + busWriteRegister(dev, BMP280_CTRL_MEAS_REG, BMP280_MODE); // these are dummy as temperature is measured as part of pressure baro->combined_read = true; @@ -222,24 +218,24 @@ static void bmp280StartUP(baroDev_t *baro) { // start measurement // set oversampling + power mode (forced), and start sampling - busWriteRegisterStart(&baro->busdev, BMP280_CTRL_MEAS_REG, BMP280_MODE); + busWriteRegisterStart(&baro->dev, BMP280_CTRL_MEAS_REG, BMP280_MODE); } static bool bmp280ReadUP(baroDev_t *baro) { - if (busBusy(&baro->busdev, NULL)) { + if (busBusy(&baro->dev, NULL)) { return false; } // read data from sensor - busReadRegisterBufferStart(&baro->busdev, BMP280_PRESSURE_MSB_REG, sensor_data, BMP280_DATA_FRAME_SIZE); + busReadRegisterBufferStart(&baro->dev, BMP280_PRESSURE_MSB_REG, sensor_data, BMP280_DATA_FRAME_SIZE); return true; } static bool bmp280GetUP(baroDev_t *baro) { - if (busBusy(&baro->busdev, NULL)) { + if (busBusy(&baro->dev, NULL)) { return false; } diff --git a/src/main/drivers/barometer/barometer_bmp388.c b/src/main/drivers/barometer/barometer_bmp388.c index 909adfbf4..6a647ba4c 100644 --- a/src/main/drivers/barometer/barometer_bmp388.c +++ b/src/main/drivers/barometer/barometer_bmp388.c @@ -165,7 +165,7 @@ STATIC_UNIT_TESTED bmp388_calib_param_t bmp388_cal; // uncompensated pressure and temperature uint32_t bmp388_up = 0; uint32_t bmp388_ut = 0; -static uint8_t sensor_data[BMP388_DATA_FRAME_SIZE]; +static DMA_DATA_ZERO_INIT uint8_t sensor_data[BMP388_DATA_FRAME_SIZE]; STATIC_UNIT_TESTED int64_t t_lin = 0; @@ -190,40 +190,40 @@ void bmp388_extiHandler(extiCallbackRec_t* cb) baroDev_t *baro = container_of(cb, baroDev_t, exti); uint8_t intStatus = 0; - busReadRegisterBuffer(&baro->busdev, BMP388_INT_STATUS_REG, &intStatus, 1); + busReadRegisterBuffer(&baro->dev, BMP388_INT_STATUS_REG, &intStatus, 1); } #endif -void bmp388BusInit(busDevice_t *busdev) +void bmp388BusInit(const extDevice_t *dev) { #ifdef USE_BARO_SPI_BMP388 - if (busdev->bustype == BUSTYPE_SPI) { - IOHi(busdev->busdev_u.spi.csnPin); // Disable - IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0); - IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP); - spiBusSetDivisor(busdev, spiCalculateDivider(BMP388_MAX_SPI_CLK_HZ)); + if (dev->bus->busType == BUS_TYPE_SPI) { + IOHi(dev->busType_u.spi.csnPin); // Disable + IOInit(dev->busType_u.spi.csnPin, OWNER_BARO_CS, 0); + IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP); + spiSetClkDivisor(dev, spiCalculateDivider(BMP388_MAX_SPI_CLK_HZ)); } #else - UNUSED(busdev); + UNUSED(dev); #endif } -void bmp388BusDeinit(busDevice_t *busdev) +void bmp388BusDeinit(const extDevice_t *dev) { #ifdef USE_BARO_SPI_BMP388 - if (busdev->bustype == BUSTYPE_SPI) { - spiPreinitByIO(busdev->busdev_u.spi.csnPin); + if (dev->bus->busType == BUS_TYPE_SPI) { + spiPreinitByIO(dev->busType_u.spi.csnPin); } #else - UNUSED(busdev); + UNUSED(dev); #endif } -void bmp388BeginForcedMeasurement(busDevice_t *busdev) +void bmp388BeginForcedMeasurement(const extDevice_t *dev) { // enable pressure measurement, temperature measurement, set power mode and start sampling uint8_t mode = BMP388_MODE_FORCED << 4 | 1 << 1 | 1 << 0; - busWriteRegisterStart(busdev, BMP388_PWR_CTRL_REG, mode); + busWriteRegisterStart(dev, BMP388_PWR_CTRL_REG, mode); } bool bmp388Detect(const bmp388Config_t *config, baroDev_t *baro) @@ -242,28 +242,28 @@ bool bmp388Detect(const bmp388Config_t *config, baroDev_t *baro) UNUSED(config); #endif - busDevice_t *busdev = &baro->busdev; + extDevice_t *dev = &baro->dev; bool defaultAddressApplied = false; - bmp388BusInit(busdev); + bmp388BusInit(dev); - if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) { + if ((dev->bus->busType == BUS_TYPE_I2C) && (dev->busType_u.i2c.address == 0)) { // Default address for BMP388 - busdev->busdev_u.i2c.address = BMP388_I2C_ADDR; + dev->busType_u.i2c.address = BMP388_I2C_ADDR; defaultAddressApplied = true; } - busReadRegisterBuffer(busdev, BMP388_CHIP_ID_REG, &bmp388_chip_id, 1); + busReadRegisterBuffer(dev, BMP388_CHIP_ID_REG, &bmp388_chip_id, 1); if (bmp388_chip_id != BMP388_DEFAULT_CHIP_ID) { - bmp388BusDeinit(busdev); + bmp388BusDeinit(dev); if (defaultAddressApplied) { - busdev->busdev_u.i2c.address = 0; + dev->busType_u.i2c.address = 0; } return false; } - busDeviceRegister(busdev); + busDeviceRegister(dev); #ifdef USE_EXTI if (baroIntIO) { @@ -273,21 +273,21 @@ bool bmp388Detect(const bmp388Config_t *config, baroDev_t *baro) 0 << BMP388_INT_LATCH_BIT | 1 << BMP388_INT_LEVEL_BIT | 0 << BMP388_INT_OD_BIT; - busWriteRegister(busdev, BMP388_INT_CTRL_REG, intCtrlValue); + busWriteRegister(dev, BMP388_INT_CTRL_REG, intCtrlValue); } #endif // read calibration - busReadRegisterBuffer(busdev, BMP388_TRIMMING_NVM_PAR_T1_LSB_REG, (uint8_t *)&bmp388_cal, sizeof(bmp388_calib_param_t)); + busReadRegisterBuffer(dev, BMP388_TRIMMING_NVM_PAR_T1_LSB_REG, (uint8_t *)&bmp388_cal, sizeof(bmp388_calib_param_t)); // set oversampling - busWriteRegister(busdev, BMP388_OSR_REG, + busWriteRegister(dev, BMP388_OSR_REG, ((BMP388_PRESSURE_OSR << BMP388_OSR_P_BIT) & BMP388_OSR_P_MASK) | ((BMP388_TEMPERATURE_OSR << BMP388_OSR4_T_BIT) & BMP388_OSR4_T_MASK) ); - bmp388BeginForcedMeasurement(busdev); + bmp388BeginForcedMeasurement(dev); // these are dummy as temperature is measured as part of pressure baro->ut_delay = 0; @@ -306,7 +306,7 @@ bool bmp388Detect(const bmp388Config_t *config, baroDev_t *baro) baro->calculate = bmp388Calculate; - while (busBusy(&baro->busdev, NULL)); + while (busBusy(&baro->dev, NULL)); return true; } @@ -334,24 +334,24 @@ static bool bmp388GetUT(baroDev_t *baro) static void bmp388StartUP(baroDev_t *baro) { // start measurement - bmp388BeginForcedMeasurement(&baro->busdev); + bmp388BeginForcedMeasurement(&baro->dev); } static bool bmp388ReadUP(baroDev_t *baro) { - if (busBusy(&baro->busdev, NULL)) { + if (busBusy(&baro->dev, NULL)) { return false; } // read data from sensor - busReadRegisterBufferStart(&baro->busdev, BMP388_DATA_0_REG, sensor_data, BMP388_DATA_FRAME_SIZE); + busReadRegisterBufferStart(&baro->dev, BMP388_DATA_0_REG, sensor_data, BMP388_DATA_FRAME_SIZE); return true; } static bool bmp388GetUP(baroDev_t *baro) { - if (busBusy(&baro->busdev, NULL)) { + if (busBusy(&baro->dev, NULL)) { return false; } diff --git a/src/main/drivers/barometer/barometer_dps310.c b/src/main/drivers/barometer/barometer_dps310.c index 77647ae20..df7958d2f 100644 --- a/src/main/drivers/barometer/barometer_dps310.c +++ b/src/main/drivers/barometer/barometer_dps310.c @@ -115,23 +115,23 @@ static baroState_t baroState; static uint8_t buf[6]; // Helper functions -static uint8_t registerRead(busDevice_t * busDev, uint8_t reg) +static uint8_t registerRead(const extDevice_t *dev, uint8_t reg) { - return busReadRegister(busDev, reg); + return busReadRegister(dev, reg); } -static void registerWrite(busDevice_t * busDev, uint8_t reg, uint8_t value) +static void registerWrite(const extDevice_t *dev, uint8_t reg, uint8_t value) { - busWrite(busDev, reg, value); + busWrite(dev, reg, value); } -static void registerSetBits(busDevice_t * busDev, uint8_t reg, uint8_t setbits) +static void registerSetBits(const extDevice_t *dev, uint8_t reg, uint8_t setbits) { - uint8_t val = registerRead(busDev, reg); + uint8_t val = registerRead(dev, reg); if ((val & setbits) != setbits) { val |= setbits; - registerWrite(busDev, reg, val); + registerWrite(dev, reg, val); } } @@ -145,15 +145,15 @@ static int32_t getTwosComplement(uint32_t raw, uint8_t length) } } -static bool deviceConfigure(busDevice_t * busDev) +static bool deviceConfigure(const extDevice_t *dev) { // Trigger a chip reset - registerSetBits(busDev, DPS310_REG_RESET, DPS310_RESET_BIT_SOFT_RST); + registerSetBits(dev, DPS310_REG_RESET, DPS310_RESET_BIT_SOFT_RST); // Sleep 40ms delay(40); - uint8_t status = registerRead(busDev, DPS310_REG_MEAS_CFG); + uint8_t status = registerRead(dev, DPS310_REG_MEAS_CFG); // Check if coefficients are available if ((status & DPS310_MEAS_CFG_COEF_RDY) == 0) { @@ -167,16 +167,15 @@ static bool deviceConfigure(busDevice_t * busDev) // 1. Read the pressure calibration coefficients (c00, c10, c20, c30, c01, c11, and c21) from the Calibration Coefficient register. // Note: The coefficients read from the coefficient register are 2's complement numbers. - // Do the read of the coefficients in multiple parts, as the chip will return a read failure when trying to read all at once over I2C. #define COEFFICIENT_LENGTH 18 #define READ_LENGTH (COEFFICIENT_LENGTH / 2) uint8_t coef[COEFFICIENT_LENGTH]; - if (!busReadBuf(busDev, DPS310_REG_COEF, coef, READ_LENGTH)) { + if (!busReadBuf(dev, DPS310_REG_COEF, coef, READ_LENGTH)) { return false; } - if (!busReadBuf(busDev, DPS310_REG_COEF + READ_LENGTH, coef + READ_LENGTH, COEFFICIENT_LENGTH - READ_LENGTH)) { + if (!busReadBuf(dev, DPS310_REG_COEF + READ_LENGTH, coef + READ_LENGTH, COEFFICIENT_LENGTH - READ_LENGTH)) { return false; } @@ -208,31 +207,31 @@ static bool deviceConfigure(busDevice_t * busDev) baroState.calib.c30 = getTwosComplement(((uint32_t)coef[16] << 8) | (uint32_t)coef[17], 16); // PRS_CFG: pressure measurement rate (32 Hz) and oversampling (16 time standard) - registerSetBits(busDev, DPS310_REG_PRS_CFG, DPS310_PRS_CFG_BIT_PM_RATE_32HZ | DPS310_PRS_CFG_BIT_PM_PRC_16); + registerSetBits(dev, DPS310_REG_PRS_CFG, DPS310_PRS_CFG_BIT_PM_RATE_32HZ | DPS310_PRS_CFG_BIT_PM_PRC_16); // TMP_CFG: temperature measurement rate (32 Hz) and oversampling (16 times) - const uint8_t TMP_COEF_SRCE = registerRead(busDev, DPS310_REG_COEF_SRCE) & DPS310_COEF_SRCE_BIT_TMP_COEF_SRCE; - registerSetBits(busDev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16 | TMP_COEF_SRCE); + const uint8_t TMP_COEF_SRCE = registerRead(dev, DPS310_REG_COEF_SRCE) & DPS310_COEF_SRCE_BIT_TMP_COEF_SRCE; + registerSetBits(dev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16 | TMP_COEF_SRCE); // CFG_REG: set pressure and temperature result bit-shift (required when the oversampling rate is >8 times) - registerSetBits(busDev, DPS310_REG_CFG_REG, DPS310_CFG_REG_BIT_T_SHIFT | DPS310_CFG_REG_BIT_P_SHIFT); + registerSetBits(dev, DPS310_REG_CFG_REG, DPS310_CFG_REG_BIT_T_SHIFT | DPS310_CFG_REG_BIT_P_SHIFT); // MEAS_CFG: Continuous pressure and temperature measurement - registerSetBits(busDev, DPS310_REG_MEAS_CFG, DPS310_MEAS_CFG_MEAS_CTRL_CONT); + registerSetBits(dev, DPS310_REG_MEAS_CFG, DPS310_MEAS_CFG_MEAS_CTRL_CONT); return true; } static bool dps310ReadUP(baroDev_t *baro) { - if (busBusy(&baro->busdev, NULL)) { + if (busBusy(&baro->dev, NULL)) { return false; } // 1. Kick off read // No need to poll for data ready as the conversion rate is 32Hz and this is sampling at 20Hz // Read PSR_B2, PSR_B1, PSR_B0, TMP_B2, TMP_B1, TMP_B0 - busReadRegisterBufferStart(&baro->busdev, DPS310_REG_PSR_B2, buf, 6); + busReadRegisterBufferStart(&baro->dev, DPS310_REG_PSR_B2, buf, 6); return true; } @@ -287,14 +286,14 @@ static void deviceCalculate(int32_t *pressure, int32_t *temperature) #define DETECTION_MAX_RETRY_COUNT 5 -static bool deviceDetect(busDevice_t * busDev) +static bool deviceDetect(const extDevice_t *dev) { for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) { uint8_t chipId[1]; delay(100); - bool ack = busReadBuf(busDev, DPS310_REG_ID, chipId, 1); + bool ack = busReadBuf(dev, DPS310_REG_ID, chipId, 1); if (ack && chipId[0] == DPS310_ID_REV_AND_PROD_ID) { return true; @@ -328,63 +327,59 @@ static void dps310StartUP(baroDev_t *baro) UNUSED(baro); } -static void busDeviceInit(busDevice_t *busdev, resourceOwner_e owner) +static void deviceInit(const extDevice_t *dev, resourceOwner_e owner) { #ifdef USE_BARO_SPI_DPS310 - if (busdev->bustype == BUSTYPE_SPI) { - IOHi(busdev->busdev_u.spi.csnPin); // Disable - IOInit(busdev->busdev_u.spi.csnPin, owner, 0); - IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP); -#ifdef USE_SPI_TRANSACTION - spiBusTransactionInit(busdev, SPI_MODE0_POL_LOW_EDGE_1ST, spiCalculateDivider(DPS310_MAX_SPI_CLK_HZ)); // DPS310 supports Mode 0 or 3 -#else - spiBusSetDivisor(busdev, spiCalculateDivider(DPS310_MAX_SPI_CLK_HZ)); -#endif + if (dev->bus->busType == BUS_TYPE_SPI) { + IOHi(dev->busType_u.spi.csnPin); // Disable + IOInit(dev->busType_u.spi.csnPin, owner, 0); + IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP); + spiSetClkDivisor(dev, spiCalculateDivider(DPS310_MAX_SPI_CLK_HZ)); } #else - UNUSED(busdev); + UNUSED(dev); UNUSED(owner); #endif } -static void busDeviceDeInit(busDevice_t *busdev) +static void deviceDeInit(const extDevice_t *dev) { #ifdef USE_BARO_SPI_DPS310 - if (busdev->bustype == BUSTYPE_SPI) { - spiPreinitByIO(busdev->busdev_u.spi.csnPin); + if (dev->bus->busType == BUS_TYPE_SPI) { + spiPreinitByIO(dev->busType_u.spi.csnPin); } #else - UNUSED(busdev); + UNUSED(dev); #endif } bool baroDPS310Detect(baroDev_t *baro) { - busDevice_t *busdev = &baro->busdev; + extDevice_t *dev = &baro->dev; bool defaultAddressApplied = false; - busDeviceInit(&baro->busdev, OWNER_BARO_CS); + deviceInit(&baro->dev, OWNER_BARO_CS); - if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) { + if ((dev->bus->busType == BUS_TYPE_I2C) && (dev->busType_u.i2c.address == 0)) { // Default address for BMP280 - busdev->busdev_u.i2c.address = DPS310_I2C_ADDR; + dev->busType_u.i2c.address = DPS310_I2C_ADDR; defaultAddressApplied = true; } - if (!deviceDetect(busdev)) { - busDeviceDeInit(busdev); + if (!deviceDetect(dev)) { + deviceDeInit(dev); if (defaultAddressApplied) { - busdev->busdev_u.i2c.address = 0; + dev->busType_u.i2c.address = 0; } return false; } - if (!deviceConfigure(busdev)) { - busDeviceDeInit(busdev); + if (!deviceConfigure(dev)) { + deviceDeInit(dev); return false; } - busDeviceRegister(busdev); + busDeviceRegister(dev); baro->ut_delay = 0; baro->start_ut = dps310StartUT; diff --git a/src/main/drivers/barometer/barometer_lps.c b/src/main/drivers/barometer/barometer_lps.c index 8f3dcb197..15ec7146e 100644 --- a/src/main/drivers/barometer/barometer_lps.c +++ b/src/main/drivers/barometer/barometer_lps.c @@ -191,34 +191,34 @@ static uint32_t rawP = 0; static uint16_t rawT = 0; -bool lpsWriteCommand(busDevice_t *busdev, uint8_t cmd, uint8_t byte) +bool lpsWriteCommand(const extDevice_t *dev, uint8_t cmd, uint8_t byte) { - return spiBusWriteRegister(busdev, cmd, byte); + return spiWriteRegRB(dev, cmd, byte); } -bool lpsReadCommand(busDevice_t *busdev, uint8_t cmd, uint8_t *data, uint8_t len) +bool lpsReadCommand(const extDevice_t *dev, uint8_t cmd, uint8_t *data, uint8_t len) { - return spiBusReadRegisterBuffer(busdev, cmd | 0x80 | 0x40, data, len); + return spiReadRegMskBufRB(dev, cmd | 0x80 | 0x40, data, len); } -bool lpsWriteVerify(busDevice_t *busdev, uint8_t cmd, uint8_t byte) +bool lpsWriteVerify(const extDevice_t *dev, uint8_t cmd, uint8_t byte) { uint8_t temp = 0xff; - spiBusWriteRegister(busdev, cmd, byte); - spiBusReadRegisterBuffer(busdev, cmd, &temp, 1); + spiWriteReg(dev, cmd, byte); + temp = spiReadRegMsk(dev, cmd); if (byte == temp) return true; return false; } -static void lpsOn(busDevice_t *busdev, uint8_t CTRL1_val) +static void lpsOn(const extDevice_t *dev, uint8_t CTRL1_val) { - lpsWriteCommand(busdev, LPS_CTRL1, CTRL1_val | 0x80); + lpsWriteCommand(dev, LPS_CTRL1, CTRL1_val | 0x80); //Instead of delay let's ready status reg } -static void lpsOff(busDevice_t *busdev) +static void lpsOff(const extDevice_t *dev) { - lpsWriteCommand(busdev, LPS_CTRL1, 0x00 | (0x01 << 2)); + lpsWriteCommand(dev, LPS_CTRL1, 0x00 | (0x01 << 2)); } static void lpsNothing(baroDev_t *baro) @@ -236,10 +236,10 @@ static bool lpsNothingBool(baroDev_t *baro) static bool lpsRead(baroDev_t *baro) { uint8_t status = 0x00; - lpsReadCommand(&baro->busdev, LPS_STATUS, &status, 1); + lpsReadCommand(&baro->dev, LPS_STATUS, &status, 1); if (status & 0x03) { uint8_t temp[5]; - lpsReadCommand(&baro->busdev, LPS_OUT_XL, temp, 5); + lpsReadCommand(&baro->dev, LPS_OUT_XL, temp, 5); /* Build the raw data */ rawP = temp[0] | (temp[1] << 8) | (temp[2] << 16) | ((temp[2] & 0x80) ? 0xff000000 : 0); @@ -261,36 +261,32 @@ static void lpsCalculate(int32_t *pressure, int32_t *temperature) bool lpsDetect(baroDev_t *baro) { //Detect - busDevice_t *busdev = &baro->busdev; + extDevice_t *dev = &baro->dev; - if (busdev->bustype != BUSTYPE_SPI) { + if (dev->bus->busType != BUS_TYPE_SPI) { return false; } - IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0); - IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP); - IOHi(busdev->busdev_u.spi.csnPin); // Disable -#ifdef USE_SPI_TRANSACTION - spiBusTransactionInit(busdev, SPI_MODE3_POL_HIGH_EDGE_2ND, spiCalculateDivider(LPS_MAX_SPI_CLK_HZ)); // Baro can work only on up to 10Mhz SPI bus -#else - spiBusSetDivisor(busdev, spiCalculateDivider(LPS_MAX_SPI_CLK_HZ)); // Baro can work only on up to 10Mhz SPI bus -#endif + IOInit(dev->busType_u.spi.csnPin, OWNER_BARO_CS, 0); + IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP); + IOHi(dev->busType_u.spi.csnPin); // Disable + spiSetClkDivisor(dev, spiCalculateDivider(LPS_MAX_SPI_CLK_HZ)); uint8_t temp = 0x00; - lpsReadCommand(&baro->busdev, LPS_WHO_AM_I, &temp, 1); + lpsReadCommand(&baro->dev, LPS_WHO_AM_I, &temp, 1); if (temp != LPS25_ID && temp != LPS22_ID && temp != LPS33_ID && temp != LPS35_ID) { return false; } //Init, if writeVerify is false fallback to false on detect bool ret = false; - lpsOff(busdev); - ret = lpsWriteVerify(busdev, LPS_CTRL2, (0x00 << 1)); if (ret != true) return false; - ret = lpsWriteVerify(busdev, LPS_RES_CONF, (LPS_AVT_64 | LPS_AVP_512)); if (ret != true) return false; - ret = lpsWriteVerify(busdev, LPS_CTRL4, 0x01); if (ret != true) return false; - lpsOn(busdev, (0x04 << 4) | (0x01 << 1) | (0x01 << 2) | (0x01 << 3)); + lpsOff(dev); + ret = lpsWriteVerify(dev, LPS_CTRL2, (0x00 << 1)); if (ret != true) return false; + ret = lpsWriteVerify(dev, LPS_RES_CONF, (LPS_AVT_64 | LPS_AVP_512)); if (ret != true) return false; + ret = lpsWriteVerify(dev, LPS_CTRL4, 0x01); if (ret != true) return false; + lpsOn(dev, (0x04 << 4) | (0x01 << 1) | (0x01 << 2) | (0x01 << 3)); - lpsReadCommand(busdev, LPS_CTRL1, &temp, 1); + lpsReadCommand(dev, LPS_CTRL1, &temp, 1); baro->combined_read = true; baro->ut_delay = 1; diff --git a/src/main/drivers/barometer/barometer_ms5611.c b/src/main/drivers/barometer/barometer_ms5611.c index 9303320c9..b74786886 100644 --- a/src/main/drivers/barometer/barometer_ms5611.c +++ b/src/main/drivers/barometer/barometer_ms5611.c @@ -60,49 +60,45 @@ STATIC_UNIT_TESTED uint32_t ms5611_up; // static result of pressure measurement STATIC_UNIT_TESTED uint16_t ms5611_c[PROM_NB]; // on-chip ROM static uint8_t ms5611_osr = CMD_ADC_4096; #define MS5611_DATA_FRAME_SIZE 3 -static uint8_t sensor_data[MS5611_DATA_FRAME_SIZE]; +static DMA_DATA_ZERO_INIT uint8_t sensor_data[MS5611_DATA_FRAME_SIZE]; -void ms5611BusInit(busDevice_t *busdev) +void ms5611BusInit(const extDevice_t *dev) { #ifdef USE_BARO_SPI_MS5611 - if (busdev->bustype == BUSTYPE_SPI) { - IOHi(busdev->busdev_u.spi.csnPin); // Disable - IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0); - IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP); -#ifdef USE_SPI_TRANSACTION - spiBusTransactionInit(busdev, SPI_MODE3_POL_HIGH_EDGE_2ND, spiCalculateDivider(MS5611_MAX_SPI_CLK_HZ)); -#else - spiBusSetDivisor(busdev, spiCalculateDivider(MS5611_MAX_SPI_CLK_HZ)); // XXX -#endif + if (dev->bus->busType == BUS_TYPE_SPI) { + IOHi(dev->busType_u.spi.csnPin); // Disable + IOInit(dev->busType_u.spi.csnPin, OWNER_BARO_CS, 0); + IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP); + spiSetClkDivisor(dev, spiCalculateDivider(MS5611_MAX_SPI_CLK_HZ)); } #else - UNUSED(busdev); + UNUSED(dev); #endif } -void ms5611BusDeinit(busDevice_t *busdev) +void ms5611BusDeinit(const extDevice_t *dev) { #ifdef USE_BARO_SPI_MS5611 - if (busdev->bustype == BUSTYPE_SPI) { - spiPreinitByIO(busdev->busdev_u.spi.csnPin); + if (dev->bus->busType == BUS_TYPE_SPI) { + spiPreinitByIO(dev->busType_u.spi.csnPin); } #else - UNUSED(busdev); + UNUSED(dev); #endif } -static void ms5611Reset(busDevice_t *busdev) +static void ms5611Reset(const extDevice_t *dev) { - busRawWriteRegister(busdev, CMD_RESET, 1); + busRawWriteRegister(dev, CMD_RESET, 1); delayMicroseconds(2800); } -static uint16_t ms5611Prom(busDevice_t *busdev, int8_t coef_num) +static uint16_t ms5611Prom(const extDevice_t *dev, int8_t coef_num) { uint8_t rxbuf[2] = { 0, 0 }; - busRawReadRegisterBuffer(busdev, CMD_PROM_RD + coef_num * 2, rxbuf, 2); // send PROM READ command + busRawReadRegisterBuffer(dev, CMD_PROM_RD + coef_num * 2, rxbuf, 2); // send PROM READ command return rxbuf[0] << 8 | rxbuf[1]; } @@ -137,30 +133,30 @@ STATIC_UNIT_TESTED int8_t ms5611CRC(uint16_t *prom) return -1; } -static void ms5611ReadAdc(busDevice_t *busdev) +static void ms5611ReadAdc(const extDevice_t *dev) { - busRawReadRegisterBufferStart(busdev, CMD_ADC_READ, sensor_data, MS5611_DATA_FRAME_SIZE); // read ADC + busRawReadRegisterBufferStart(dev, CMD_ADC_READ, sensor_data, MS5611_DATA_FRAME_SIZE); // read ADC } static void ms5611StartUT(baroDev_t *baro) { - busRawWriteRegisterStart(&baro->busdev, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start! + busRawWriteRegisterStart(&baro->dev, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start! } static bool ms5611ReadUT(baroDev_t *baro) { - if (busBusy(&baro->busdev, NULL)) { + if (busBusy(&baro->dev, NULL)) { return false; } - ms5611ReadAdc(&baro->busdev); + ms5611ReadAdc(&baro->dev); return true; } static bool ms5611GetUT(baroDev_t *baro) { - if (busBusy(&baro->busdev, NULL)) { + if (busBusy(&baro->dev, NULL)) { return false; } @@ -171,23 +167,23 @@ static bool ms5611GetUT(baroDev_t *baro) static void ms5611StartUP(baroDev_t *baro) { - busRawWriteRegisterStart(&baro->busdev, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start! + busRawWriteRegisterStart(&baro->dev, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start! } static bool ms5611ReadUP(baroDev_t *baro) { - if (busBusy(&baro->busdev, NULL)) { + if (busBusy(&baro->dev, NULL)) { return false; } - ms5611ReadAdc(&baro->busdev); + ms5611ReadAdc(&baro->dev); return true; } static bool ms5611GetUP(baroDev_t *baro) { - if (busBusy(&baro->busdev, NULL)) { + if (busBusy(&baro->dev, NULL)) { return false; } @@ -236,32 +232,32 @@ bool ms5611Detect(baroDev_t *baro) delay(10); // No idea how long the chip takes to power-up, but let's make it 10ms - busDevice_t *busdev = &baro->busdev; + extDevice_t *dev = &baro->dev; - ms5611BusInit(busdev); + ms5611BusInit(dev); - if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) { + if ((dev->bus->busType == BUS_TYPE_I2C) && (dev->busType_u.i2c.address == 0)) { // Default address for MS5611 - busdev->busdev_u.i2c.address = MS5611_I2C_ADDR; + dev->busType_u.i2c.address = MS5611_I2C_ADDR; defaultAddressApplied = true; } - if (!busRawReadRegisterBuffer(busdev, CMD_PROM_RD, &sig, 1) || sig == 0xFF) { + if (!busRawReadRegisterBuffer(dev, CMD_PROM_RD, &sig, 1) || sig == 0xFF) { goto fail; } - ms5611Reset(busdev); + ms5611Reset(dev); // read all coefficients for (i = 0; i < PROM_NB; i++) - ms5611_c[i] = ms5611Prom(busdev, i); + ms5611_c[i] = ms5611Prom(dev, i); // check crc, bail out if wrong - we are probably talking to BMP085 w/o XCLR line! if (ms5611CRC(ms5611_c) != 0) { goto fail; } - busDeviceRegister(busdev); + busDeviceRegister(dev); // TODO prom + CRC baro->ut_delay = 10000; @@ -277,10 +273,10 @@ bool ms5611Detect(baroDev_t *baro) return true; fail:; - ms5611BusDeinit(busdev); + ms5611BusDeinit(dev); if (defaultAddressApplied) { - busdev->busdev_u.i2c.address = 0; + dev->busType_u.i2c.address = 0; } return false; diff --git a/src/main/drivers/barometer/barometer_qmp6988.c b/src/main/drivers/barometer/barometer_qmp6988.c index 065ade780..d95864311 100644 --- a/src/main/drivers/barometer/barometer_qmp6988.c +++ b/src/main/drivers/barometer/barometer_qmp6988.c @@ -94,7 +94,7 @@ STATIC_UNIT_TESTED qmp6988_calib_param_t qmp6988_cal; // uncompensated pressure and temperature int32_t qmp6988_up = 0; int32_t qmp6988_ut = 0; -static uint8_t sensor_data[QMP6988_DATA_FRAME_SIZE]; +static DMA_DATA_ZERO_INIT uint8_t sensor_data[QMP6988_DATA_FRAME_SIZE]; static void qmp6988StartUT(baroDev_t *baro); static bool qmp6988ReadUT(baroDev_t *baro); @@ -105,34 +105,30 @@ static bool qmp6988GetUP(baroDev_t *baro); STATIC_UNIT_TESTED void qmp6988Calculate(int32_t *pressure, int32_t *temperature); -void qmp6988BusInit(busDevice_t *busdev) +void qmp6988BusInit(const extDevice_t *dev) { #ifdef USE_BARO_SPI_QMP6988 - if (busdev->bustype == BUSTYPE_SPI) { - IOHi(busdev->busdev_u.spi.csnPin); - IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0); - IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP); -#ifdef USE_SPI_TRANSACTION - spiBusTransactionInit(busdev, SPI_MODE3_POL_HIGH_EDGE_2ND, spiCalculateDivider(QMP6988_MAX_SPI_CLK_HZ)); -#else - spiBusSetDivisor(busdev, spiCalculateDivider(QMP6988_MAX_SPI_CLK_HZ)); -#endif + if (dev->bus->busType == BUS_TYPE_SPI) { + IOHi(dev->busType_u.spi.csnPin); + IOInit(dev->busType_u.spi.csnPin, OWNER_BARO_CS, 0); + IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP); + spiSetClkDivisor(dev, spiCalculateDivider(QMP6988_MAX_SPI_CLK_HZ)); } #else - UNUSED(busdev); + UNUSED(dev); #endif } -void qmp6988BusDeinit(busDevice_t *busdev) +void qmp6988BusDeinit(const extDevice_t *dev) { #ifdef USE_BARO_SPI_QMP6988 - if (busdev->bustype == BUSTYPE_SPI) { - IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_IPU); - IORelease(busdev->busdev_u.spi.csnPin); - IOInit(busdev->busdev_u.spi.csnPin, OWNER_PREINIT, 0); + if (dev->bus->busType == BUS_TYPE_SPI) { + IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_IPU); + IORelease(dev->busType_u.spi.csnPin); + IOInit(dev->busType_u.spi.csnPin, OWNER_PREINIT, 0); } #else - UNUSED(busdev); + UNUSED(dev); #endif } @@ -156,33 +152,33 @@ bool qmp6988Detect(baroDev_t *baro) delay(20); - busDevice_t *busdev = &baro->busdev; + extDevice_t *dev = &baro->dev; bool defaultAddressApplied = false; - qmp6988BusInit(busdev); + qmp6988BusInit(dev); - if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) { - busdev->busdev_u.i2c.address = QMP6988_I2C_ADDR; + if ((dev->bus->busType == BUS_TYPE_I2C) && (dev->busType_u.i2c.address == 0)) { + dev->busType_u.i2c.address = QMP6988_I2C_ADDR; defaultAddressApplied = true; } - busReadRegisterBuffer(busdev, QMP6988_CHIP_ID_REG, &qmp6988_chip_id, 1); /* read Chip Id */ + busReadRegisterBuffer(dev, QMP6988_CHIP_ID_REG, &qmp6988_chip_id, 1); /* read Chip Id */ if (qmp6988_chip_id != QMP6988_DEFAULT_CHIP_ID) { - qmp6988BusDeinit(busdev); + qmp6988BusDeinit(dev); if (defaultAddressApplied) { - busdev->busdev_u.i2c.address = 0; + dev->busType_u.i2c.address = 0; } return false; } - busDeviceRegister(busdev); + busDeviceRegister(dev); // SetIIR - busWriteRegister(busdev, QMP6988_SET_IIR_REG, 0x05); + busWriteRegister(dev, QMP6988_SET_IIR_REG, 0x05); //read OTP - busReadRegisterBuffer(busdev, QMP6988_COE_B00_1_REG, databuf, 25); + busReadRegisterBuffer(dev, QMP6988_COE_B00_1_REG, databuf, 25); //algo OTP hw = databuf[0]; @@ -265,7 +261,7 @@ bool qmp6988Detect(baroDev_t *baro) qmp6988_cal.Coe_bp3= (1.30E-16)+(7.90E-17)*(float)Coe_bp3_/32767.0; // Set power mode and sample times - busWriteRegister(busdev, QMP6988_CTRL_MEAS_REG, QMP6988_PWR_SAMPLE_MODE); + busWriteRegister(dev, QMP6988_CTRL_MEAS_REG, QMP6988_PWR_SAMPLE_MODE); // these are dummy as temperature is measured as part of pressure baro->combined_read = true; @@ -306,24 +302,24 @@ static bool qmp6988GetUT(baroDev_t *baro) static void qmp6988StartUP(baroDev_t *baro) { // start measurement - busWriteRegister(&baro->busdev, QMP6988_CTRL_MEAS_REG, QMP6988_PWR_SAMPLE_MODE); + busWriteRegister(&baro->dev, QMP6988_CTRL_MEAS_REG, QMP6988_PWR_SAMPLE_MODE); } static bool qmp6988ReadUP(baroDev_t *baro) { - if (busBusy(&baro->busdev, NULL)) { + if (busBusy(&baro->dev, NULL)) { return false; } // read data from sensor - busReadRegisterBufferStart(&baro->busdev, QMP6988_PRESSURE_MSB_REG, sensor_data, QMP6988_DATA_FRAME_SIZE); + busReadRegisterBufferStart(&baro->dev, QMP6988_PRESSURE_MSB_REG, sensor_data, QMP6988_DATA_FRAME_SIZE); return true; } static bool qmp6988GetUP(baroDev_t *baro) { - if (busBusy(&baro->busdev, NULL)) { + if (busBusy(&baro->dev, NULL)) { return false; } diff --git a/src/main/drivers/bus.c b/src/main/drivers/bus.c index 2a83cfa99..031de9411 100644 --- a/src/main/drivers/bus.c +++ b/src/main/drivers/bus.c @@ -27,165 +27,138 @@ #include "drivers/bus_i2c_busdev.h" #include "drivers/bus_spi.h" -bool busRawWriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data) + +// Access routines where the register is accessed directly +bool busRawWriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data) { #ifdef USE_SPI - if (busdev->bustype == BUSTYPE_SPI) { -#ifdef USE_SPI_TRANSACTION - spiBusTransactionSetup(busdev); -#endif - return spiBusWriteRegister(busdev, reg, data); + if (dev->bus->busType == BUS_TYPE_SPI) { + return spiWriteRegRB(dev, reg, data); } else #endif { - return busWriteRegister(busdev, reg, data); + return busWriteRegister(dev, reg, data); } } -bool busWriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data) +bool busRawWriteRegisterStart(const extDevice_t *dev, uint8_t reg, uint8_t data) +{ +#ifdef USE_SPI + if (dev->bus->busType == BUS_TYPE_SPI) { + return spiWriteRegRB(dev, reg, data); + } else +#endif + { + return busWriteRegisterStart(dev, reg, data); + } +} + +bool busRawReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length) +{ +#ifdef USE_SPI + if (dev->bus->busType == BUS_TYPE_SPI) { + return spiReadRegBufRB(dev, reg, data, length); + } else +#endif + { + return busReadRegisterBuffer(dev, reg, data, length); + } +} + +bool busRawReadRegisterBufferStart(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length) +{ +#ifdef USE_SPI + if (dev->bus->busType == BUS_TYPE_SPI) { + return spiReadRegBufRB(dev, reg, data, length); + } else +#endif + { + return busReadRegisterBufferStart(dev, reg, data, length); + } +} + + +// Write routines where the register is masked with 0x7f +bool busWriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data) { #if !defined(USE_SPI) && !defined(USE_I2C) UNUSED(reg); UNUSED(data); #endif - switch (busdev->bustype) { + switch (dev->bus->busType) { #ifdef USE_SPI - case BUSTYPE_SPI: -#ifdef USE_SPI_TRANSACTION - // XXX Watch out fastpath users, if any - return spiBusTransactionWriteRegister(busdev, reg & 0x7f, data); -#else - return spiBusWriteRegister(busdev, reg & 0x7f, data); -#endif + case BUS_TYPE_SPI: + return spiWriteRegRB(dev, reg & 0x7f, data); #endif #ifdef USE_I2C - case BUSTYPE_I2C: - return i2cBusWriteRegister(busdev, reg, data); + case BUS_TYPE_I2C: + return i2cBusWriteRegister(dev, reg, data); #endif default: return false; } } -bool busRawWriteRegisterStart(const busDevice_t *busdev, uint8_t reg, uint8_t data) -{ -#ifdef USE_SPI - if (busdev->bustype == BUSTYPE_SPI) { -#ifdef USE_SPI_TRANSACTION - spiBusTransactionSetup(busdev); -#endif - return spiBusWriteRegister(busdev, reg, data); - } else -#endif - { - return busWriteRegisterStart(busdev, reg, data); - } -} - -bool busWriteRegisterStart(const busDevice_t *busdev, uint8_t reg, uint8_t data) +bool busWriteRegisterStart(const extDevice_t *dev, uint8_t reg, uint8_t data) { #if !defined(USE_SPI) && !defined(USE_I2C) UNUSED(reg); UNUSED(data); #endif - switch (busdev->bustype) { + switch (dev->bus->busType) { #ifdef USE_SPI - case BUSTYPE_SPI: -#ifdef USE_SPI_TRANSACTION - // XXX Watch out fastpath users, if any - return spiBusTransactionWriteRegister(busdev, reg & 0x7f, data); -#else - return spiBusWriteRegister(busdev, reg & 0x7f, data); -#endif + case BUS_TYPE_SPI: + return spiWriteRegRB(dev, reg & 0x7f, data); #endif #ifdef USE_I2C - case BUSTYPE_I2C: - return i2cBusWriteRegisterStart(busdev, reg, data); + case BUS_TYPE_I2C: + return i2cBusWriteRegisterStart(dev, reg, data); #endif default: return false; } } -bool busRawReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length) -{ -#ifdef USE_SPI - if (busdev->bustype == BUSTYPE_SPI) { -#ifdef USE_SPI_TRANSACTION - spiBusTransactionSetup(busdev); -#endif - return spiBusRawReadRegisterBuffer(busdev, reg, data, length); - } else -#endif - { - return busReadRegisterBuffer(busdev, reg, data, length); - } -} - -bool busReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length) +// Read routines where the register is ORed with 0x80 +bool busReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length) { #if !defined(USE_SPI) && !defined(USE_I2C) UNUSED(reg); UNUSED(data); UNUSED(length); #endif - switch (busdev->bustype) { + switch (dev->bus->busType) { #ifdef USE_SPI - case BUSTYPE_SPI: -#ifdef USE_SPI_TRANSACTION - // XXX Watch out fastpath users, if any - return spiBusTransactionReadRegisterBuffer(busdev, reg | 0x80, data, length); -#else - return spiBusReadRegisterBuffer(busdev, reg | 0x80, data, length); -#endif + case BUS_TYPE_SPI: + return spiReadRegMskBufRB(dev, reg | 0x80, data, length); #endif #ifdef USE_I2C - case BUSTYPE_I2C: - return i2cBusReadRegisterBuffer(busdev, reg, data, length); + case BUS_TYPE_I2C: + return i2cBusReadRegisterBuffer(dev, reg, data, length); #endif default: return false; } } -bool busRawReadRegisterBufferStart(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length) -{ -#ifdef USE_SPI - if (busdev->bustype == BUSTYPE_SPI) { -#ifdef USE_SPI_TRANSACTION - spiBusTransactionSetup(busdev); -#endif - return spiBusRawReadRegisterBuffer(busdev, reg, data, length); - } else -#endif - { - return busReadRegisterBufferStart(busdev, reg, data, length); - } -} - // Start the I2C read, but do not wait for completion -bool busReadRegisterBufferStart(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length) +bool busReadRegisterBufferStart(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length) { #if !defined(USE_SPI) && !defined(USE_I2C) UNUSED(reg); UNUSED(data); UNUSED(length); #endif - switch (busdev->bustype) { + switch (dev->bus->busType) { #ifdef USE_SPI - case BUSTYPE_SPI: + case BUS_TYPE_SPI: // For SPI allow the transaction to complete -#ifdef USE_SPI_TRANSACTION - // XXX Watch out fastpath users, if any - return spiBusTransactionReadRegisterBuffer(busdev, reg | 0x80, data, length); -#else - return spiBusReadRegisterBuffer(busdev, reg | 0x80, data, length); -#endif + return spiReadRegMskBufRB(dev, reg | 0x80, data, length); #endif #ifdef USE_I2C - case BUSTYPE_I2C: + case BUS_TYPE_I2C: // Initiate the read access - return i2cBusReadRegisterBufferStart(busdev, reg, data, length); + return i2cBusReadRegisterBufferStart(dev, reg, data, length); #endif default: return false; @@ -193,21 +166,21 @@ bool busReadRegisterBufferStart(const busDevice_t *busdev, uint8_t reg, uint8_t } // Returns true if bus is still busy -bool busBusy(const busDevice_t *busdev, bool *error) +bool busBusy(const extDevice_t *dev, bool *error) { #if !defined(USE_I2C) UNUSED(error); #endif - switch (busdev->bustype) { + switch (dev->bus->busType) { #ifdef USE_SPI - case BUSTYPE_SPI: + case BUS_TYPE_SPI: // No waiting on SPI return false; #endif #ifdef USE_I2C - case BUSTYPE_I2C: - return i2cBusBusy(busdev, error); + case BUS_TYPE_I2C: + return i2cBusBusy(dev, error); #endif default: @@ -215,35 +188,35 @@ bool busBusy(const busDevice_t *busdev, bool *error) } } -uint8_t busReadRegister(const busDevice_t *busdev, uint8_t reg) +uint8_t busReadRegister(const extDevice_t *dev, uint8_t reg) { #if !defined(USE_SPI) && !defined(USE_I2C) - UNUSED(busdev); + UNUSED(dev); UNUSED(reg); return false; #else uint8_t data; - busReadRegisterBuffer(busdev, reg, &data, 1); + busReadRegisterBuffer(dev, reg, &data, 1); return data; #endif } -void busDeviceRegister(const busDevice_t *busdev) +void busDeviceRegister(const extDevice_t *dev) { #if !defined(USE_SPI) && !defined(USE_I2C) - UNUSED(busdev); + UNUSED(dev); #endif - switch (busdev->bustype) { + switch (dev->bus->busType) { #if defined(USE_SPI) - case BUSTYPE_SPI: - spiBusDeviceRegister(busdev); + case BUS_TYPE_SPI: + spiBusDeviceRegister(dev); break; #endif #if defined(USE_I2C) - case BUSTYPE_I2C: - i2cBusDeviceRegister(busdev); + case BUS_TYPE_I2C: + i2cBusDeviceRegister(dev); break; #endif diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 7749ad0e1..d626d8470 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -24,55 +24,125 @@ #include "drivers/bus_i2c.h" #include "drivers/io_types.h" +#include "drivers/dma.h" typedef enum { - BUSTYPE_NONE = 0, - BUSTYPE_I2C, - BUSTYPE_SPI, - BUSTYPE_MPU_SLAVE, // Slave I2C on SPI master - BUSTYPE_GYRO_AUTO, // Only used by acc/gyro bus auto detection code + BUS_TYPE_NONE = 0, + BUS_TYPE_I2C, + BUS_TYPE_SPI, + BUS_TYPE_MPU_SLAVE, // Slave I2C on SPI master + BUS_TYPE_GYRO_AUTO, // Only used by acc/gyro bus auto detection code } busType_e; struct spiDevice_s; +typedef enum { + BUS_READY, + BUS_BUSY, + BUS_ABORT +} busStatus_e; + + +// Bus interface, independent of connected device typedef struct busDevice_s { - busType_e bustype; + busType_e busType; union { - struct deviceSpi_s { + struct busSpi_s { SPI_TypeDef *instance; -#ifdef USE_SPI_TRANSACTION - struct SPIDevice_s *device; // Back ptr to controller for this device. - // Cached SPI_CR1 for spiBusTransactionXXX - uint16_t modeCache; // XXX cr1Value may be a better name? -#endif -#if defined(USE_HAL_DRIVER) - SPI_HandleTypeDef* handle; // cached here for efficiency -#endif - IO_t csnPin; + uint16_t speed; + bool leadingEdge; } spi; - struct deviceI2C_s { + struct busI2C_s { I2CDevice device; + } i2c; + struct busMpuSlave_s { + struct extDevice_s *master; + } mpuSlave; + } busType_u; + bool useDMA; + bool useAtomicWait; + uint8_t deviceCount; + resourceOwner_e owner; // owner of first device to use this bus + dmaChannelDescriptor_t *dmaTx; + dmaChannelDescriptor_t *dmaRx; + uint32_t dmaTxChannel; + uint32_t dmaRxChannel; +#ifndef UNIT_TEST + // Use a reference here as this saves RAM for unused descriptors +#if defined(USE_FULL_LL_DRIVER) + LL_DMA_InitTypeDef *initTx; + LL_DMA_InitTypeDef *initRx; +#else + DMA_InitTypeDef *initTx; + DMA_InitTypeDef *initRx; +#endif +#endif // UNIT_TEST + struct busSegment_s* volatile curSegment; + bool initSegment; +} busDevice_t; + +/* Each SPI access may comprise multiple parts, for example, wait/write enable/write/data each of which + * is defined by a segment, with optional callback after each is completed + */ +typedef struct busSegment_s { + uint8_t *txData; + uint8_t *rxData; + int len; + bool negateCS; // Should CS be negated at the end of this segment + busStatus_e (*callback)(uint32_t arg); +} busSegment_t; + +// External device has an associated bus and bus dependent address +typedef struct extDevice_s { + busDevice_t *bus; + union { + struct extSpi_s { + uint16_t speed; + IO_t csnPin; + bool leadingEdge; + } spi; + struct extI2C_s { uint8_t address; } i2c; - struct deviceMpuSlave_s { - const struct busDevice_s *master; + struct extMpuSlave_s { uint8_t address; } mpuSlave; - } busdev_u; -} busDevice_t; + } busType_u; +#ifndef UNIT_TEST + // Cache the init structure for the next DMA transfer to reduce inter-segment delay +#if defined(USE_HAL_DRIVER) + LL_DMA_InitTypeDef initTx; + LL_DMA_InitTypeDef initRx; +#else + DMA_InitTypeDef initTx; + DMA_InitTypeDef initRx; +#endif +#endif // UNIT_TEST + // Support disabling DMA on a per device basis + bool useDMA; + // Per device buffer reference if needed + uint8_t *txBuf, *rxBuf; + // Connected devices on the same bus may support different speeds + uint32_t callbackArg; +} extDevice_t; + #ifdef TARGET_BUS_INIT void targetBusInit(void); #endif -bool busRawWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); -bool busWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); -bool busRawWriteRegisterStart(const busDevice_t *bus, uint8_t reg, uint8_t data); -bool busWriteRegisterStart(const busDevice_t *bus, uint8_t reg, uint8_t data); -bool busRawReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length); -bool busReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length); -uint8_t busReadRegister(const busDevice_t *bus, uint8_t reg); -bool busRawReadRegisterBufferStart(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length); -bool busReadRegisterBufferStart(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length); -bool busBusy(const busDevice_t *busdev, bool *error); -void busDeviceRegister(const busDevice_t *busdev); +// Access routines where the register is accessed directly +bool busRawWriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data); +bool busRawWriteRegisterStart(const extDevice_t *dev, uint8_t reg, uint8_t data); +bool busRawReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length); +bool busRawReadRegisterBufferStart(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length); +// Write routines where the register is masked with 0x7f +bool busWriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data); +bool busWriteRegisterStart(const extDevice_t *dev, uint8_t reg, uint8_t data); +// Read routines where the register is ORed with 0x80 +bool busReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length); +bool busReadRegisterBufferStart(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length); +uint8_t busReadRegister(const extDevice_t *dev, uint8_t reg); + +bool busBusy(const extDevice_t *dev, bool *error); +void busDeviceRegister(const extDevice_t *dev); diff --git a/src/main/drivers/bus_i2c_busdev.c b/src/main/drivers/bus_i2c_busdev.c index fd1035f4f..fee1212d3 100644 --- a/src/main/drivers/bus_i2c_busdev.c +++ b/src/main/drivers/bus_i2c_busdev.c @@ -31,46 +31,62 @@ static uint8_t i2cRegisteredDeviceCount = 0; -bool i2cBusWriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data) +bool i2cBusWriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data) { - return i2cWrite(busdev->busdev_u.i2c.device, busdev->busdev_u.i2c.address, reg, data); + return i2cWrite(dev->bus->busType_u.i2c.device, dev->busType_u.i2c.address, reg, data); } -bool i2cBusWriteRegisterStart(const busDevice_t *busdev, uint8_t reg, uint8_t data) +bool i2cBusWriteRegisterStart(const extDevice_t *dev, uint8_t reg, uint8_t data) { // Need a static value, not on the stack static uint8_t byte; byte = data; - return i2cWriteBuffer(busdev->busdev_u.i2c.device, busdev->busdev_u.i2c.address, reg, sizeof (byte), &byte); + return i2cWriteBuffer(dev->bus->busType_u.i2c.device, dev->busType_u.i2c.address, reg, sizeof (byte), &byte); } -bool i2cBusReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length) +bool i2cBusReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length) { - return i2cRead(busdev->busdev_u.i2c.device, busdev->busdev_u.i2c.address, reg, length, data); + return i2cRead(dev->bus->busType_u.i2c.device, dev->busType_u.i2c.address, reg, length, data); } -uint8_t i2cBusReadRegister(const busDevice_t *busdev, uint8_t reg) +uint8_t i2cBusReadRegister(const extDevice_t *dev, uint8_t reg) { uint8_t data; - i2cRead(busdev->busdev_u.i2c.device, busdev->busdev_u.i2c.address, reg, 1, &data); + i2cRead(dev->bus->busType_u.i2c.device, dev->busType_u.i2c.address, reg, 1, &data); return data; } -bool i2cBusReadRegisterBufferStart(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length) +bool i2cBusReadRegisterBufferStart(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length) { - return i2cReadBuffer(busdev->busdev_u.i2c.device, busdev->busdev_u.i2c.address, reg, length, data); + return i2cReadBuffer(dev->bus->busType_u.i2c.device, dev->busType_u.i2c.address, reg, length, data); } -bool i2cBusBusy(const busDevice_t *busdev, bool *error) +bool i2cBusBusy(const extDevice_t *dev, bool *error) { - return i2cBusy(busdev->busdev_u.i2c.device, error); + return i2cBusy(dev->bus->busType_u.i2c.device, error); } -void i2cBusDeviceRegister(const busDevice_t *busdev) +bool i2cBusSetInstance(extDevice_t *dev, uint32_t device) { - UNUSED(busdev); + // I2C bus structures to associate with external devices + static busDevice_t i2cBus[I2CDEV_COUNT]; + + if ((device < 1) || (device > I2CDEV_COUNT)) { + return false; + } + + dev->bus = &i2cBus[I2C_CFG_TO_DEV(device)]; + dev->bus->busType = BUS_TYPE_I2C; + dev->bus->busType_u.i2c.device = I2C_CFG_TO_DEV(device); + + return true; +} + +void i2cBusDeviceRegister(const extDevice_t *dev) +{ + UNUSED(dev); i2cRegisteredDeviceCount++; } diff --git a/src/main/drivers/bus_i2c_busdev.h b/src/main/drivers/bus_i2c_busdev.h index 023e454b6..1f2149ff7 100644 --- a/src/main/drivers/bus_i2c_busdev.h +++ b/src/main/drivers/bus_i2c_busdev.h @@ -20,10 +20,12 @@ #pragma once -bool i2cBusWriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data); -bool i2cBusWriteRegisterStart(const busDevice_t *busdev, uint8_t reg, uint8_t data); -bool i2cBusReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length); -uint8_t i2cBusReadRegister(const busDevice_t *bus, uint8_t reg); -bool i2cBusReadRegisterBufferStart(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length); -bool i2cBusBusy(const busDevice_t *busdev, bool *error); -void i2cBusDeviceRegister(const busDevice_t *busdev); +bool i2cBusWriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data); +bool i2cBusWriteRegisterStart(const extDevice_t *dev, uint8_t reg, uint8_t data); +bool i2cBusReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length); +uint8_t i2cBusReadRegister(const extDevice_t *dev, uint8_t reg); +bool i2cBusReadRegisterBufferStart(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length); +bool i2cBusBusy(const extDevice_t *dev, bool *error); +// Associate a device with an I2C bus +bool i2cBusSetInstance(const extDevice_t *dev, uint32_t device); +void i2cBusDeviceRegister(const extDevice_t *dev); diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 32ed26999..e7835150c 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -24,18 +24,24 @@ #include "platform.h" +#include "build/atomic.h" + #ifdef USE_SPI #include "drivers/bus.h" #include "drivers/bus_spi.h" #include "drivers/bus_spi_impl.h" +#include "drivers/dma_reqmap.h" #include "drivers/exti.h" #include "drivers/io.h" +#include "drivers/motor.h" #include "drivers/rcc.h" +#include "nvic.h" static uint8_t spiRegisteredDeviceCount = 0; spiDevice_t spiDevice[SPIDEV_COUNT]; +busDevice_t spiBusDevice[SPIDEV_COUNT]; SPIDevice spiDeviceByInstance(SPI_TypeDef *instance) { @@ -71,7 +77,7 @@ SPI_TypeDef *spiInstanceByDevice(SPIDevice device) return spiDevice[device].dev; } -bool spiInit(SPIDevice device, bool leadingEdge) +bool spiInit(SPIDevice device) { switch (device) { case SPIINVALID: @@ -79,7 +85,7 @@ bool spiInit(SPIDevice device, bool leadingEdge) case SPIDEV_1: #ifdef USE_SPI_DEVICE_1 - spiInitDevice(device, leadingEdge); + spiInitDevice(device); return true; #else break; @@ -87,7 +93,7 @@ bool spiInit(SPIDevice device, bool leadingEdge) case SPIDEV_2: #ifdef USE_SPI_DEVICE_2 - spiInitDevice(device, leadingEdge); + spiInitDevice(device); return true; #else break; @@ -95,7 +101,7 @@ bool spiInit(SPIDevice device, bool leadingEdge) case SPIDEV_3: #if defined(USE_SPI_DEVICE_3) && !defined(STM32F1) - spiInitDevice(device, leadingEdge); + spiInitDevice(device); return true; #else break; @@ -103,7 +109,7 @@ bool spiInit(SPIDevice device, bool leadingEdge) case SPIDEV_4: #if defined(USE_SPI_DEVICE_4) - spiInitDevice(device, leadingEdge); + spiInitDevice(device); return true; #else break; @@ -111,7 +117,7 @@ bool spiInit(SPIDevice device, bool leadingEdge) case SPIDEV_5: #if defined(USE_SPI_DEVICE_5) - spiInitDevice(device, leadingEdge); + spiInitDevice(device); return true; #else break; @@ -119,7 +125,7 @@ bool spiInit(SPIDevice device, bool leadingEdge) case SPIDEV_6: #if defined(USE_SPI_DEVICE_6) - spiInitDevice(device, leadingEdge); + spiInitDevice(device); return true; #else break; @@ -128,116 +134,248 @@ bool spiInit(SPIDevice device, bool leadingEdge) return false; } -uint32_t spiTimeoutUserCallback(SPI_TypeDef *instance) +// Return true if DMA engine is busy +bool spiIsBusy(const extDevice_t *dev) { - SPIDevice device = spiDeviceByInstance(instance); - if (device == SPIINVALID) { - return -1; - } - spiDevice[device].errorCount++; - return spiDevice[device].errorCount; + return (dev->bus->curSegment != (busSegment_t *)NULL); } -bool spiBusTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int length) +// Indicate that the bus on which this device resides may initiate DMA transfers from interrupt context +void spiSetAtomicWait(const extDevice_t *dev) { - IOLo(bus->busdev_u.spi.csnPin); - spiTransfer(bus->busdev_u.spi.instance, txData, rxData, length); - IOHi(bus->busdev_u.spi.csnPin); - return true; + dev->bus->useAtomicWait = true; } -uint16_t spiGetErrorCounter(SPI_TypeDef *instance) +// Wait for DMA completion and claim the bus driver +void spiWaitClaim(const extDevice_t *dev) { - SPIDevice device = spiDeviceByInstance(instance); - if (device == SPIINVALID) { - return 0; - } - return spiDevice[device].errorCount; -} + // If there is a device on the bus whose driver might call spiSequence from an ISR then an + // atomic access is required to claim the bus, however if not, then interrupts need not be + // disabled as this can result in edge triggered interrupts being missed -void spiResetErrorCounter(SPI_TypeDef *instance) -{ - SPIDevice device = spiDeviceByInstance(instance); - if (device != SPIINVALID) { - spiDevice[device].errorCount = 0; + if (dev->bus->useAtomicWait) { + // Prevent race condition where the bus appears free, but a gyro interrupt starts a transfer + do { + ATOMIC_BLOCK(NVIC_PRIO_MPU_INT_EXTI) { + if (dev->bus->curSegment == (busSegment_t *)NULL) { + dev->bus->curSegment = (busSegment_t *)0x04; + } + } + } while (dev->bus->curSegment != (busSegment_t *)0x04); + } else { + // Wait for completion + while (dev->bus->curSegment != (busSegment_t *)NULL); } } -bool spiBusIsBusBusy(const busDevice_t *bus) +// Wait for DMA completion +void spiWait(const extDevice_t *dev) { - return spiIsBusBusy(bus->busdev_u.spi.instance); + // Wait for completion + while (dev->bus->curSegment != (busSegment_t *)NULL); } -uint8_t spiBusTransferByte(const busDevice_t *bus, uint8_t data) +// Wait for bus to become free, then read/write block of data +void spiReadWriteBuf(const extDevice_t *dev, uint8_t *txData, uint8_t *rxData, int len) { - return spiTransferByte(bus->busdev_u.spi.instance, data); + // This routine blocks so no need to use static data + busSegment_t segments[] = { + {txData, rxData, len, true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + + // Ensure any prior DMA has completed before continuing + spiWaitClaim(dev); + + spiSequence(dev, &segments[0]); + + spiWait(dev); } -void spiBusWriteByte(const busDevice_t *bus, uint8_t data) +// Read/Write a block of data, returning false if the bus is busy +bool spiReadWriteBufRB(const extDevice_t *dev, uint8_t *txData, uint8_t *rxData, int length) { - IOLo(bus->busdev_u.spi.csnPin); - spiBusTransferByte(bus, data); - IOHi(bus->busdev_u.spi.csnPin); -} + // Ensure any prior DMA has completed before continuing + if (spiIsBusy(dev)) { + return false; + } -bool spiBusRawTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int len) -{ - return spiTransfer(bus->busdev_u.spi.instance, txData, rxData, len); -} - -bool spiBusWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) -{ - IOLo(bus->busdev_u.spi.csnPin); - spiTransferByte(bus->busdev_u.spi.instance, reg); - spiTransferByte(bus->busdev_u.spi.instance, data); - IOHi(bus->busdev_u.spi.csnPin); + spiReadWriteBuf(dev, txData, rxData, length); return true; } -bool spiBusRawReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length) +// Wait for bus to become free, then read/write a single byte +uint8_t spiReadWrite(const extDevice_t *dev, uint8_t data) { - IOLo(bus->busdev_u.spi.csnPin); - spiTransferByte(bus->busdev_u.spi.instance, reg); - spiTransfer(bus->busdev_u.spi.instance, NULL, data, length); - IOHi(bus->busdev_u.spi.csnPin); + uint8_t retval; + + // This routine blocks so no need to use static data + busSegment_t segments[] = { + {&data, &retval, sizeof (data), true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + + // Ensure any prior DMA has completed before continuing + spiWaitClaim(dev); + + spiSequence(dev, &segments[0]); + + spiWait(dev); + + return retval; +} + +// Wait for bus to become free, then read/write a single byte from a register +uint8_t spiReadWriteReg(const extDevice_t *dev, uint8_t reg, uint8_t data) +{ + uint8_t retval; + + // This routine blocks so no need to use static data + busSegment_t segments[] = { + {®, NULL, sizeof (reg), false, NULL}, + {&data, &retval, sizeof (data), true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + + // Ensure any prior DMA has completed before continuing + spiWaitClaim(dev); + + spiSequence(dev, &segments[0]); + + spiWait(dev); + + return retval; +} + +// Wait for bus to become free, then write a single byte +void spiWrite(const extDevice_t *dev, uint8_t data) +{ + // This routine blocks so no need to use static data + busSegment_t segments[] = { + {&data, NULL, sizeof (data), true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + + // Ensure any prior DMA has completed before continuing + spiWaitClaim(dev); + + spiSequence(dev, &segments[0]); + + spiWait(dev); +} + +// Write data to a register +void spiWriteReg(const extDevice_t *dev, uint8_t reg, uint8_t data) +{ + // This routine blocks so no need to use static data + busSegment_t segments[] = { + {®, NULL, sizeof (reg), false, NULL}, + {&data, NULL, sizeof (data), true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + + // Ensure any prior DMA has completed before continuing + spiWaitClaim(dev); + + spiSequence(dev, &segments[0]); + + spiWait(dev); +} + +// Write data to a register, returning false if the bus is busy +bool spiWriteRegRB(const extDevice_t *dev, uint8_t reg, uint8_t data) +{ + // Ensure any prior DMA has completed before continuing + if (spiIsBusy(dev)) { + return false; + } + + spiWriteReg(dev, reg, data); return true; } -bool spiBusReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length) +// Read a block of data from a register +void spiReadRegBuf(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length) { - return spiBusRawReadRegisterBuffer(bus, reg | 0x80, data, length); + // This routine blocks so no need to use static data + busSegment_t segments[] = { + {®, NULL, sizeof (reg), false, NULL}, + {NULL, data, length, true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + + // Ensure any prior DMA has completed before continuing + spiWaitClaim(dev); + + spiSequence(dev, &segments[0]); + + spiWait(dev); } -void spiBusWriteRegisterBuffer(const busDevice_t *bus, uint8_t reg, const uint8_t *data, uint8_t length) +// Read a block of data from a register, returning false if the bus is busy +bool spiReadRegBufRB(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length) { - IOLo(bus->busdev_u.spi.csnPin); - spiTransferByte(bus->busdev_u.spi.instance, reg); - spiTransfer(bus->busdev_u.spi.instance, data, NULL, length); - IOHi(bus->busdev_u.spi.csnPin); + // Ensure any prior DMA has completed before continuing + if (spiIsBusy(dev)) { + return false; + } + + spiReadRegBuf(dev, reg, data, length); + + return true; } -uint8_t spiBusRawReadRegister(const busDevice_t *bus, uint8_t reg) +// Read a block of data where the register is ORed with 0x80, returning false if the bus is busy +bool spiReadRegMskBufRB(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length) +{ + return spiReadRegBufRB(dev, reg | 0x80, data, length); +} + +// Wait for bus to become free, then write a block of data to a register +void spiWriteRegBuf(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint32_t length) +{ + // This routine blocks so no need to use static data + busSegment_t segments[] = { + {®, NULL, sizeof (reg), false, NULL}, + {data, NULL, length, true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + + // Ensure any prior DMA has completed before continuing + spiWaitClaim(dev); + + spiSequence(dev, &segments[0]); + + spiWait(dev); +} + +// Wait for bus to become free, then read a byte from a register +uint8_t spiReadReg(const extDevice_t *dev, uint8_t reg) { uint8_t data; - IOLo(bus->busdev_u.spi.csnPin); - spiTransferByte(bus->busdev_u.spi.instance, reg); - spiTransfer(bus->busdev_u.spi.instance, NULL, &data, 1); - IOHi(bus->busdev_u.spi.csnPin); + // This routine blocks so no need to use static data + busSegment_t segments[] = { + {®, NULL, sizeof (reg), false, NULL}, + {NULL, &data, sizeof (data), true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + + // Ensure any prior DMA has completed before continuing + spiWaitClaim(dev); + + spiSequence(dev, &segments[0]); + + spiWait(dev); return data; } -uint8_t spiBusReadRegister(const busDevice_t *bus, uint8_t reg) +// Wait for bus to become free, then read a byte of data where the register is ORed with 0x80 +uint8_t spiReadRegMsk(const extDevice_t *dev, uint8_t reg) { - return spiBusRawReadRegister(bus, reg | 0x80); -} - -void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance) -{ - bus->bustype = BUSTYPE_SPI; - bus->busdev_u.spi.instance = instance; + return spiReadReg(dev, reg | 0x80); } uint16_t spiCalculateDivider(uint32_t freq) @@ -259,54 +397,225 @@ uint16_t spiCalculateDivider(uint32_t freq) return divisor; } -void spiBusSetDivisor(busDevice_t *bus, uint16_t divisor) +// Interrupt handler for SPI receive DMA completion +static void spiRxIrqHandler(dmaChannelDescriptor_t* descriptor) { - spiSetDivisor(bus->busdev_u.spi.instance, divisor); - // bus->busdev_u.spi.modeCache = bus->busdev_u.spi.instance->CR1; + const extDevice_t *dev = (const extDevice_t *)descriptor->userParam; + + if (!dev) { + return; + } + + busDevice_t *bus = dev->bus; + + if (bus->curSegment->negateCS) { + // Negate Chip Select + IOHi(dev->busType_u.spi.csnPin); + } + + spiInternalStopDMA(dev); + +#ifdef __DCACHE_PRESENT +#ifdef STM32H7 + if (bus->curSegment->rxData && + ((bus->curSegment->rxData < &_dmaram_start__) || (bus->curSegment->rxData >= &_dmaram_end__))) { +#else + if (bus->curSegment->rxData) { +#endif + // Invalidate the D cache covering the area into which data has been read + SCB_InvalidateDCache_by_Addr( + (uint32_t *)((uint32_t)bus->curSegment->rxData & ~CACHE_LINE_MASK), + (((uint32_t)bus->curSegment->rxData & CACHE_LINE_MASK) + + bus->curSegment->len - 1 + CACHE_LINE_SIZE) & ~CACHE_LINE_MASK); + } +#endif // __DCACHE_PRESENT + + if (bus->curSegment->callback) { + switch(bus->curSegment->callback(dev->callbackArg)) { + case BUS_BUSY: + // Repeat the last DMA segment + bus->curSegment--; + // Reinitialise the cached init values as segment is not progressing + spiInternalInitStream(dev, true); + break; + + case BUS_ABORT: + bus->curSegment = (busSegment_t *)NULL; + return; + + case BUS_READY: + default: + // Advance to the next DMA segment + break; + } + } + + // Advance through the segment list + bus->curSegment++; + + if (bus->curSegment->len == 0) { + // The end of the segment list has been reached, so mark transactions as complete + bus->curSegment = (busSegment_t *)NULL; + } else { + // After the completion of the first segment setup the init structure for the subsequent segment + if (bus->initSegment) { + spiInternalInitStream(dev, false); + bus->initSegment = false; + } + + // Launch the next transfer + spiInternalStartDMA(dev); + + // Prepare the init structures ready for the next segment to reduce inter-segment time + spiInternalInitStream(dev, true); + } } -#ifdef USE_SPI_TRANSACTION -// Separate set of spiBusTransactionXXX to keep fast path for acc/gyros. - -void spiBusTransactionBegin(const busDevice_t *bus) +// Mark this bus as being SPI and record the first owner to use it +bool spiSetBusInstance(extDevice_t *dev, uint32_t device, resourceOwner_e owner) { - spiBusTransactionSetup(bus); - IOLo(bus->busdev_u.spi.csnPin); + if (device > SPIDEV_COUNT) { + return false; + } + + dev->bus = &spiBusDevice[SPI_CFG_TO_DEV(device)]; + + if (dev->bus->busType == BUS_TYPE_SPI) { + // This bus has already been initialised + dev->bus->deviceCount++; + return true; + } + + busDevice_t *bus = dev->bus; + + bus->busType_u.spi.instance = spiInstanceByDevice(SPI_CFG_TO_DEV(device)); + + if (bus->busType_u.spi.instance == NULL) { + return false; + } + + bus->busType = BUS_TYPE_SPI; + dev->useDMA = true; + bus->useDMA = false; + bus->useAtomicWait = false; + bus->deviceCount = 1; + bus->owner = owner; + bus->initTx = &dev->initTx; + bus->initRx = &dev->initRx; + + return true; } -void spiBusTransactionEnd(const busDevice_t *bus) +void spiInitBusDMA() { - IOHi(bus->busdev_u.spi.csnPin); + uint32_t device; +#ifdef STM32F4 + /* Check https://www.st.com/resource/en/errata_sheet/dm00037591-stm32f405407xx-and-stm32f415417xx-device-limitations-stmicroelectronics.pdf + * section 2.1.10 which reports an errata that corruption may occurs on DMA2 if AHB peripherals (eg GPIO ports) are + * access concurrently with APB peripherals (eg SPI busses). Bitbang DSHOT uses DMA2 to write to GPIO ports. If this + * is enabled, then don't enable DMA on an SPI bus using DMA2 + */ + const bool dshotBitbangActive = isDshotBitbangActive(&motorConfig()->dev); +#endif + + for (device = 0; device < SPIDEV_COUNT; device++) { + busDevice_t *bus = &spiBusDevice[device]; + + if (bus->busType != BUS_TYPE_SPI) { + // This bus is not in use + continue; + } + + dmaIdentifier_e dmaTxIdentifier = DMA_NONE; + dmaIdentifier_e dmaRxIdentifier = DMA_NONE; + + for (uint8_t opt = 0; opt < MAX_PERIPHERAL_DMA_OPTIONS; opt++) { + const dmaChannelSpec_t *dmaTxChannelSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_SPI_TX, device, opt); + + if (dmaTxChannelSpec) { + dmaTxIdentifier = dmaGetIdentifier(dmaTxChannelSpec->ref); + if (dmaGetOwner(dmaTxIdentifier)->owner != OWNER_FREE) { + dmaTxIdentifier = DMA_NONE; + continue; + } +#ifdef STM32F4 + if (dshotBitbangActive && (DMA_DEVICE_NO(dmaTxIdentifier) == 2)) { + dmaTxIdentifier = DMA_NONE; + break; + } +#endif + bus->dmaTxChannel = dmaTxChannelSpec->channel; + dmaInit(dmaTxIdentifier, bus->owner, 0); + break; + } + } + + for (uint8_t opt = 0; opt < MAX_PERIPHERAL_DMA_OPTIONS; opt++) { + const dmaChannelSpec_t *dmaRxChannelSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_SPI_RX, device, opt); + + if (dmaRxChannelSpec) { + dmaRxIdentifier = dmaGetIdentifier(dmaRxChannelSpec->ref); + if (dmaGetOwner(dmaRxIdentifier)->owner != OWNER_FREE) { + dmaRxIdentifier = DMA_NONE; + continue; + } +#ifdef STM32F4 + if (dshotBitbangActive && (DMA_DEVICE_NO(dmaRxIdentifier) == 2)) { + dmaRxIdentifier = DMA_NONE; + break; + } +#endif + bus->dmaRxChannel = dmaRxChannelSpec->channel; + dmaInit(dmaRxIdentifier, bus->owner, 0); + break; + } + } + + if (dmaTxIdentifier && dmaRxIdentifier) { + bus->dmaTx = dmaGetDescriptorByIdentifier(dmaTxIdentifier); + bus->dmaRx = dmaGetDescriptorByIdentifier(dmaRxIdentifier); + + // Ensure streams are disabled + spiInternalResetStream(bus->dmaRx); + spiInternalResetStream(bus->dmaTx); + + spiInternalResetDescriptors(bus); + + /* Note that this driver may be called both from the normal thread of execution, or from USB interrupt + * handlers, so the DMA completion interrupt must be at a higher priority + */ + dmaSetHandler(dmaRxIdentifier, spiRxIrqHandler, NVIC_PRIO_SPI_DMA, 0); + + bus->useDMA = true; + } + } } -bool spiBusTransactionTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int length) +void spiSetClkDivisor(const extDevice_t *dev, uint16_t divisor) { - spiBusTransactionSetup(bus); - return spiBusTransfer(bus, txData, rxData, length); + ((extDevice_t *)dev)->busType_u.spi.speed = divisor; } -bool spiBusTransactionWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) +// Set the clock phase/polarity to be used for accesses by the given device +void spiSetClkPhasePolarity(const extDevice_t *dev, bool leadingEdge) { - spiBusTransactionSetup(bus); - return spiBusWriteRegister(bus, reg, data); + ((extDevice_t *)dev)->busType_u.spi.leadingEdge = leadingEdge; } -uint8_t spiBusTransactionReadRegister(const busDevice_t *bus, uint8_t reg) +// Enable/disable DMA on a specific device. Enabled by default. +void spiDmaEnable(const extDevice_t *dev, bool enable) { - spiBusTransactionSetup(bus); - return spiBusReadRegister(bus, reg); + ((extDevice_t *)dev)->useDMA = enable; } -bool spiBusTransactionReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length) +bool spiUseDMA(const extDevice_t *dev) { - spiBusTransactionSetup(bus); - return spiBusReadRegisterBuffer(bus, reg, data, length); + return dev->bus->useDMA && dev->useDMA; } -#endif // USE_SPI_TRANSACTION -void spiBusDeviceRegister(const busDevice_t *bus) +void spiBusDeviceRegister(const extDevice_t *dev) { - UNUSED(bus); + UNUSED(dev); spiRegisteredDeviceCount++; } @@ -315,4 +624,9 @@ uint8_t spiGetRegisteredDeviceCount(void) { return spiRegisteredDeviceCount; } + +uint8_t spiGetExtDeviceCount(const extDevice_t *dev) +{ + return dev->bus->deviceCount; +} #endif diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 5220d5439..a8fe243e9 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -81,7 +81,6 @@ typedef enum SPIDevice { #define SPIDEV_COUNT 6 #else #define SPIDEV_COUNT 4 - #endif // Macros to convert between CLI bus number and SPIDevice. @@ -102,49 +101,67 @@ void spiPreinitRegister(ioTag_t iotag, uint8_t iocfg, uint8_t init); void spiPreinitByIO(IO_t io); void spiPreinitByTag(ioTag_t tag); -bool spiInit(SPIDevice device, bool leadingEdge); -void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor); -uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t data); -bool spiIsBusBusy(SPI_TypeDef *instance); +bool spiInit(SPIDevice device); -bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len); +// Called after all devices are initialised to enable SPI DMA where streams are available. +void spiInitBusDMA(); -uint16_t spiGetErrorCounter(SPI_TypeDef *instance); -void spiResetErrorCounter(SPI_TypeDef *instance); SPIDevice spiDeviceByInstance(SPI_TypeDef *instance); SPI_TypeDef *spiInstanceByDevice(SPIDevice device); -// // BusDevice API -// -bool spiBusIsBusBusy(const busDevice_t *bus); - -bool spiBusTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int length); - -uint8_t spiBusTransferByte(const busDevice_t *bus, uint8_t data); -void spiBusWriteByte(const busDevice_t *bus, uint8_t data); -bool spiBusRawTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int len); - -bool spiBusWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); -bool spiBusRawReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length); -bool spiBusReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length); -void spiBusWriteRegisterBuffer(const busDevice_t *bus, uint8_t reg, const uint8_t *data, uint8_t length); -uint8_t spiBusRawReadRegister(const busDevice_t *bus, uint8_t reg); -uint8_t spiBusReadRegister(const busDevice_t *bus, uint8_t reg); -void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance); +// Mark a device's associated bus as being SPI and record the first owner to use it +bool spiSetBusInstance(extDevice_t *dev, uint32_t device, resourceOwner_e owner); +// Determine the divisor to use for a given bus frequency uint16_t spiCalculateDivider(uint32_t freq); -void spiBusSetDivisor(busDevice_t *bus, uint16_t divider); +// Set the clock divisor to be used for accesses by the given device +void spiSetClkDivisor(const extDevice_t *dev, uint16_t divider); +// Set the clock phase/polarity to be used for accesses by the given device +void spiSetClkPhasePolarity(const extDevice_t *dev, bool leadingEdge); +// Enable/disable DMA on a specific device. Enabled by default. +void spiDmaEnable(const extDevice_t *dev, bool enable); -void spiBusTransactionInit(busDevice_t *bus, SPIMode_e mode, uint16_t divider); -void spiBusTransactionSetup(const busDevice_t *bus); -void spiBusTransactionBegin(const busDevice_t *bus); -void spiBusTransactionEnd(const busDevice_t *bus); -bool spiBusTransactionWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); -uint8_t spiBusTransactionReadRegister(const busDevice_t *bus, uint8_t reg); -bool spiBusTransactionReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length); -bool spiBusTransactionTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int length); +// DMA transfer setup and start +void spiSequence(const extDevice_t *dev, busSegment_t *segments); +// Wait for DMA completion +void spiWait(const extDevice_t *dev); +// Indicate that the bus on which this device resides may initiate DMA transfers from interrupt context +void spiSetAtomicWait(const extDevice_t *dev); +// Wait for DMA completion and claim the bus driver - use this when waiting for a prior access to complete before starting a new one +void spiWaitClaim(const extDevice_t *dev); +// Return true if DMA engine is busy +bool spiIsBusy(const extDevice_t *dev); + +/* + * Routine naming convention is: + * spi[Read][Write][Reg][Msk][Buf][RB] + * + * Read: Perform a read, returning the value read unless 'Buf' is specified + * Write Perform a write + * ReadWrite: Perform both a read and write, returning the value read unless 'Buf' is specified + * Reg: Register number 'reg' is written prior to the read being performed + * Msk: Register number is logically ORed with 0x80 as some devices indicate a read by accessing a register with bit 7 set + * Buf: Pass data of given length by reference + * RB: Return false immediately if the bus is busy, otherwise complete the access and return true + */ +uint8_t spiReadReg(const extDevice_t *dev, uint8_t reg); +uint8_t spiReadRegMsk(const extDevice_t *dev, uint8_t reg); +void spiReadRegBuf(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length); +bool spiReadRegBufRB(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length); +bool spiReadRegMskBufRB(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length); + +void spiWrite(const extDevice_t *dev, uint8_t data); +void spiWriteReg(const extDevice_t *dev, uint8_t reg, uint8_t data); +bool spiWriteRegRB(const extDevice_t *dev, uint8_t reg, uint8_t data); + +uint8_t spiReadWrite(const extDevice_t *dev, uint8_t data); + +void spiWriteRegBuf(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint32_t length); +uint8_t spiReadWriteReg(const extDevice_t *dev, uint8_t reg, uint8_t data); +void spiReadWriteBuf(const extDevice_t *dev, uint8_t *txData, uint8_t *rxData, int len); +bool spiReadWriteBufRB(const extDevice_t *dev, uint8_t *txData, uint8_t *rxData, int length); // // Config @@ -152,5 +169,8 @@ bool spiBusTransactionTransfer(const busDevice_t *bus, const uint8_t *txData, ui struct spiPinConfig_s; void spiPinConfigure(const struct spiPinConfig_s *pConfig); -void spiBusDeviceRegister(const busDevice_t *bus); +bool spiUseDMA(const extDevice_t *dev); +void spiBusDeviceRegister(const extDevice_t *dev); uint8_t spiGetRegisteredDeviceCount(void); +uint8_t spiGetExtDeviceCount(const extDevice_t *dev); + diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c index fb5901af7..913badfa8 100644 --- a/src/main/drivers/bus_spi_hal.c +++ b/src/main/drivers/bus_spi_hal.c @@ -20,6 +20,8 @@ * HAL version resurrected from v3.1.7 (by jflyper) */ +// Note that the HAL driver is polled only + #include #include #include @@ -38,7 +40,44 @@ #define SPI_TIMEOUT_SYS_TICKS (SPI_TIMEOUT_US / 1000) -void spiInitDevice(SPIDevice device, bool leadingEdge) +// Position of Prescaler bits are different from MCU to MCU + +static uint32_t baudRatePrescaler[8] = { + SPI_BAUDRATEPRESCALER_2, + SPI_BAUDRATEPRESCALER_4, + SPI_BAUDRATEPRESCALER_8, + SPI_BAUDRATEPRESCALER_16, + SPI_BAUDRATEPRESCALER_32, + SPI_BAUDRATEPRESCALER_64, + SPI_BAUDRATEPRESCALER_128, + SPI_BAUDRATEPRESCALER_256, +}; + +static void spiDivisorToBRbits(SPI_TypeDef *instance, uint16_t divisor) +{ + SPIDevice device = spiDeviceByInstance(instance); + + spiDevice_t *spi = &(spiDevice[device]); + + int prescalerIndex = ffs(divisor) - 2; // prescaler begins at "/2" + + if (prescalerIndex < 0 || prescalerIndex >= (int)ARRAYLEN(baudRatePrescaler)) { + return; + } + + if (spi->hspi.Init.BaudRatePrescaler != baudRatePrescaler[prescalerIndex]) { + spi->hspi.Init.BaudRatePrescaler = baudRatePrescaler[prescalerIndex]; + + MODIFY_REG(spi->hspi.Instance->CR1, SPI_CR1_BR_Msk, spi->hspi.Init.BaudRatePrescaler); + } +} + +static void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) +{ + spiDivisorToBRbits(instance, divisor); +} + +void spiInitDevice(SPIDevice device) { spiDevice_t *spi = &(spiDevice[device]); @@ -46,8 +85,6 @@ void spiInitDevice(SPIDevice device, bool leadingEdge) return; } - spi->leadingEdge = leadingEdge; - // Enable SPI clock RCC_ClockCmd(spi->rcc, ENABLE); RCC_ResetCmd(spi->rcc, ENABLE); @@ -91,128 +128,122 @@ void spiInitDevice(SPIDevice device, bool leadingEdge) spi->hspi.Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA; spi->hspi.Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_ENABLE; /* Recommanded setting to avoid glitches */ #endif - - if (spi->leadingEdge) { - spi->hspi.Init.CLKPolarity = SPI_POLARITY_LOW; - spi->hspi.Init.CLKPhase = SPI_PHASE_1EDGE; - } else { - spi->hspi.Init.CLKPolarity = SPI_POLARITY_HIGH; - spi->hspi.Init.CLKPhase = SPI_PHASE_2EDGE; - } + spi->hspi.Init.CLKPolarity = SPI_POLARITY_LOW; + spi->hspi.Init.CLKPhase = SPI_PHASE_1EDGE; // Init SPI hardware HAL_SPI_Init(&spi->hspi); } -// return uint8_t value or -1 when failure -uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t out) -{ - uint8_t in; - - spiTransfer(instance, &out, &in, 1); - return in; -} - -/** - * Return true if the bus is currently in the middle of a transmission. - */ -bool spiIsBusBusy(SPI_TypeDef *instance) -{ - SPIDevice device = spiDeviceByInstance(instance); - if (spiDevice[device].hspi.State == HAL_SPI_STATE_BUSY) { - return true; - } else { - return false; - } -} - -bool spiTransfer(SPI_TypeDef *instance, const uint8_t *out, uint8_t *in, int len) +static bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len) { SPIDevice device = spiDeviceByInstance(instance); HAL_StatusTypeDef status; - if (!in) { + if (!rxData) { // Tx only - status = HAL_SPI_Transmit(&spiDevice[device].hspi, out, len, SPI_TIMEOUT_SYS_TICKS); - } else if (!out) { + status = HAL_SPI_Transmit(&spiDevice[device].hspi, txData, len, SPI_TIMEOUT_SYS_TICKS); + } else if(!txData) { // Rx only - status = HAL_SPI_Receive(&spiDevice[device].hspi, in, len, SPI_TIMEOUT_SYS_TICKS); + status = HAL_SPI_Receive(&spiDevice[device].hspi, rxData, len, SPI_TIMEOUT_SYS_TICKS); } else { // Tx and Rx - status = HAL_SPI_TransmitReceive(&spiDevice[device].hspi, out, in, len, SPI_TIMEOUT_SYS_TICKS); + status = HAL_SPI_TransmitReceive(&spiDevice[device].hspi, txData, rxData, len, SPI_TIMEOUT_SYS_TICKS); } - if (status != HAL_OK) { - spiTimeoutUserCallback(instance); + return (status == HAL_OK); +} + +void spiInternalInitStream(const extDevice_t *dev, bool preInit) +{ + UNUSED(dev); + UNUSED(preInit); +} + +void spiInternalStartDMA(const extDevice_t *dev) +{ + UNUSED(dev); +} + +void spiInternalStopDMA (const extDevice_t *dev) +{ + UNUSED(dev); +} + +void spiInternalResetDescriptors(busDevice_t *bus) +{ + UNUSED(bus); +} + +void spiInternalResetStream(dmaChannelDescriptor_t *descriptor) +{ + UNUSED(descriptor); +} + +// Transfer setup and start +void spiSequence(const extDevice_t *dev, busSegment_t *segments) +{ + busDevice_t *bus = dev->bus; + SPIDevice device = spiDeviceByInstance(bus->busType_u.spi.instance); + SPI_HandleTypeDef *hspi = &spiDevice[device].hspi; + + bus->initSegment = true; + bus->curSegment = segments; + + // Switch bus speed + spiSetDivisor(bus->busType_u.spi.instance, dev->busType_u.spi.speed); + + // Switch SPI clock polarity/phase if necessary + if (dev->busType_u.spi.leadingEdge != bus->busType_u.spi.leadingEdge) { + if (dev->busType_u.spi.leadingEdge){ + hspi->Init.CLKPolarity = SPI_POLARITY_LOW; + hspi->Init.CLKPhase = SPI_PHASE_1EDGE; + } else { + hspi->Init.CLKPolarity = SPI_POLARITY_HIGH; + hspi->Init.CLKPhase = SPI_PHASE_2EDGE; + } + bus->busType_u.spi.leadingEdge = dev->busType_u.spi.leadingEdge; + + // Init SPI hardware + HAL_SPI_Init(hspi); } - return true; -} + // Manually work through the segment list performing a transfer for each + while (bus->curSegment->len) { + // Assert Chip Select + IOLo(dev->busType_u.spi.csnPin); -// Position of Prescaler bits are different from MCU to MCU + spiInternalReadWriteBufPolled( + bus->busType_u.spi.instance, + bus->curSegment->txData, + bus->curSegment->rxData, + bus->curSegment->len); -static uint32_t baudRatePrescaler[8] = { - SPI_BAUDRATEPRESCALER_2, - SPI_BAUDRATEPRESCALER_4, - SPI_BAUDRATEPRESCALER_8, - SPI_BAUDRATEPRESCALER_16, - SPI_BAUDRATEPRESCALER_32, - SPI_BAUDRATEPRESCALER_64, - SPI_BAUDRATEPRESCALER_128, - SPI_BAUDRATEPRESCALER_256, -}; + if (bus->curSegment->negateCS) { + // Negate Chip Select + IOHi(dev->busType_u.spi.csnPin); + } -void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) -{ - SPIDevice device = spiDeviceByInstance(instance); + if (bus->curSegment->callback) { + switch(bus->curSegment->callback(dev->callbackArg)) { + case BUS_BUSY: + // Repeat the last DMA segment + bus->curSegment--; + break; - HAL_SPI_DeInit(&spiDevice[device].hspi); + case BUS_ABORT: + bus->curSegment = (busSegment_t *)NULL; + return; - spiDevice_t *spi = &(spiDevice[device]); - - int prescalerIndex = ffs(divisor) - 2; // prescaler begins at "/2" - - if (prescalerIndex < 0 || prescalerIndex >= (int)ARRAYLEN(baudRatePrescaler)) { - return; + case BUS_READY: + default: + // Advance to the next DMA segment + break; + } + } + bus->curSegment++; } - spi->hspi.Init.BaudRatePrescaler = baudRatePrescaler[prescalerIndex]; - - HAL_SPI_Init(&spi->hspi); + bus->curSegment = (busSegment_t *)NULL; } - -#ifdef USE_DMA -DMA_HandleTypeDef* dmaHandleByInstance(SPI_TypeDef *instance) -{ - return &spiDevice[spiDeviceByInstance(instance)].hdma; -} - -void SPI1_IRQHandler(void) -{ - HAL_SPI_IRQHandler(&spiDevice[SPIDEV_1].hspi); -} - -void SPI2_IRQHandler(void) -{ - HAL_SPI_IRQHandler(&spiDevice[SPIDEV_2].hspi); -} - -void SPI3_IRQHandler(void) -{ - HAL_SPI_IRQHandler(&spiDevice[SPIDEV_3].hspi); -} - -void SPI4_IRQHandler(void) -{ - HAL_SPI_IRQHandler(&spiDevice[SPIDEV_4].hspi); -} - -void dmaSPIIRQHandler(dmaChannelDescriptor_t* descriptor) -{ - SPIDevice device = descriptor->userParam; - if (device != SPIINVALID) { - HAL_DMA_IRQHandler(&spiDevice[device].hdma); - } -} -#endif // USE_DMA #endif diff --git a/src/main/drivers/bus_spi_impl.h b/src/main/drivers/bus_spi_impl.h index 7b000fc56..fe07f32e6 100644 --- a/src/main/drivers/bus_spi_impl.h +++ b/src/main/drivers/bus_spi_impl.h @@ -49,7 +49,7 @@ typedef struct spiHardware_s { uint8_t af; #endif rccPeriphTag_t rcc; -#if defined(USE_DMA) && defined(USE_HAL_DRIVER) +#ifdef USE_DMA uint8_t dmaIrqHandler; #endif } spiHardware_t; @@ -67,23 +67,24 @@ typedef struct SPIDevice_s { uint8_t mosiAF; #else uint8_t af; +#endif +#if defined(HAL_SPI_MODULE_ENABLED) + SPI_HandleTypeDef hspi; #endif rccPeriphTag_t rcc; volatile uint16_t errorCount; bool leadingEdge; -#if defined(USE_HAL_DRIVER) - SPI_HandleTypeDef hspi; #ifdef USE_DMA - DMA_HandleTypeDef hdma; uint8_t dmaIrqHandler; #endif -#endif -#ifdef USE_SPI_TRANSACTION - uint16_t cr1SoftCopy; // Copy of active CR1 value for this SPI instance -#endif } spiDevice_t; extern spiDevice_t spiDevice[SPIDEV_COUNT]; -void spiInitDevice(SPIDevice device, bool leadingEdge); -uint32_t spiTimeoutUserCallback(SPI_TypeDef *instance); +void spiInitDevice(SPIDevice device); +void spiInternalInitStream(const extDevice_t *dev, bool preInit); +void spiInternalStartDMA(const extDevice_t *dev); +void spiInternalStopDMA (const extDevice_t *dev); +void spiInternalResetStream(dmaChannelDescriptor_t *descriptor); +void spiInternalResetDescriptors(busDevice_t *bus); + diff --git a/src/main/drivers/bus_spi_ll.c b/src/main/drivers/bus_spi_ll.c index 15f065ef9..ffec81910 100644 --- a/src/main/drivers/bus_spi_ll.c +++ b/src/main/drivers/bus_spi_ll.c @@ -36,7 +36,6 @@ #include "drivers/io.h" #include "drivers/nvic.h" #include "drivers/rcc.h" -#include "drivers/time.h" #ifndef SPI2_SCK_PIN #define SPI2_NSS_PIN PB12 @@ -72,165 +71,29 @@ #define SPI4_NSS_PIN NONE #endif +#define SPI_DEFAULT_TIMEOUT 10 + +#ifdef STM32H7 +#define IS_DTCM(p) (((uint32_t)p & 0xfffe0000) == 0x20000000) +#elif defined(STM32F7) +#define IS_DTCM(p) (((uint32_t)p & 0xffff0000) == 0x20000000) +#endif static LL_SPI_InitTypeDef defaultInit = { - .TransferDirection = SPI_DIRECTION_2LINES, - .Mode = SPI_MODE_MASTER, - .DataWidth = SPI_DATASIZE_8BIT, - .NSS = SPI_NSS_SOFT, - .BaudRate = SPI_BAUDRATEPRESCALER_8, - .BitOrder = SPI_FIRSTBIT_MSB, - .CRCPoly = 7, - .CRCCalculation = SPI_CRCCALCULATION_DISABLE, + .TransferDirection = LL_SPI_FULL_DUPLEX, + .Mode = LL_SPI_MODE_MASTER, + .DataWidth = LL_SPI_DATAWIDTH_8BIT, + .NSS = LL_SPI_NSS_SOFT, + .BaudRate = LL_SPI_BAUDRATEPRESCALER_DIV8, + .BitOrder = LL_SPI_MSB_FIRST, + .CRCCalculation = LL_SPI_CRCCALCULATION_DISABLE, + .ClockPolarity = LL_SPI_POLARITY_HIGH, + .ClockPhase = LL_SPI_PHASE_2EDGE, }; -void spiInitDevice(SPIDevice device, bool leadingEdge) +static uint32_t spiDivisorToBRbits(SPI_TypeDef *instance, uint16_t divisor) { - spiDevice_t *spi = &(spiDevice[device]); - - if (!spi->dev) { - return; - } - -#ifndef USE_SPI_TRANSACTION - spi->leadingEdge = leadingEdge; -#else - UNUSED(leadingEdge); -#endif - - // Enable SPI clock - RCC_ClockCmd(spi->rcc, ENABLE); - RCC_ResetCmd(spi->rcc, ENABLE); - - IOInit(IOGetByTag(spi->sck), OWNER_SPI_SCK, RESOURCE_INDEX(device)); - IOInit(IOGetByTag(spi->miso), OWNER_SPI_MISO, RESOURCE_INDEX(device)); - IOInit(IOGetByTag(spi->mosi), OWNER_SPI_MOSI, RESOURCE_INDEX(device)); - - if (spi->leadingEdge == true) { - IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->sckAF); - } else { - IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->sckAF); - } - - IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->misoAF); - IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->mosiAF); - - LL_SPI_Disable(spi->dev); - LL_SPI_DeInit(spi->dev); - -#ifndef USE_SPI_TRANSACTION - if (spi->leadingEdge) { - defaultInit.ClockPolarity = SPI_POLARITY_LOW; - defaultInit.ClockPhase = SPI_PHASE_1EDGE; - } else -#endif - { - defaultInit.ClockPolarity = SPI_POLARITY_HIGH; - defaultInit.ClockPhase = SPI_PHASE_2EDGE; - } - - LL_SPI_SetRxFIFOThreshold(spi->dev, SPI_RXFIFO_THRESHOLD_QF); - - LL_SPI_Init(spi->dev, &defaultInit); - LL_SPI_Enable(spi->dev); -} - -uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte) -{ - timeUs_t timeoutStartUs = microsISR(); - - while (!LL_SPI_IsActiveFlag_TXE(instance)) { - if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) { - return spiTimeoutUserCallback(instance); - } - } - LL_SPI_TransmitData8(instance, txByte); - - timeoutStartUs = microsISR(); - while (!CHECK_SPI_RX_DATA_AVAILABLE(instance)) { - if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) { - return spiTimeoutUserCallback(instance); - } - } - - return (uint8_t)LL_SPI_ReceiveData8(instance); -} - -/** - * Return true if the bus is currently in the middle of a transmission. - */ -bool spiIsBusBusy(SPI_TypeDef *instance) -{ - return LL_SPI_GetTxFIFOLevel(instance) != LL_SPI_TX_FIFO_EMPTY - || LL_SPI_IsActiveFlag_BSY(instance); -} - -bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len) -{ - timeUs_t timeoutStartUs; - - // set 16-bit transfer - CLEAR_BIT(instance->CR2, SPI_RXFIFO_THRESHOLD); - while (len > 1) { - timeoutStartUs = microsISR(); - while (!LL_SPI_IsActiveFlag_TXE(instance)) { - if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) { - return spiTimeoutUserCallback(instance); - } - } - uint16_t w; - if (txData) { - w = *((uint16_t *)txData); - txData += 2; - } else { - w = 0xFFFF; - } - LL_SPI_TransmitData16(instance, w); - - timeoutStartUs = microsISR(); - while (!CHECK_SPI_RX_DATA_AVAILABLE(instance)) { - if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) { - return spiTimeoutUserCallback(instance); - } - } - w = LL_SPI_ReceiveData16(instance); - if (rxData) { - *((uint16_t *)rxData) = w; - rxData += 2; - } - len -= 2; - } - // set 8-bit transfer - SET_BIT(instance->CR2, SPI_RXFIFO_THRESHOLD); - if (len) { - timeoutStartUs = microsISR(); - while (!LL_SPI_IsActiveFlag_TXE(instance)) { - if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) { - return spiTimeoutUserCallback(instance); - } - } - uint8_t b = txData ? *(txData++) : 0xFF; - LL_SPI_TransmitData8(instance, b); - - timeoutStartUs = microsISR(); - while (!CHECK_SPI_RX_DATA_AVAILABLE(instance)) { - if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) { - return spiTimeoutUserCallback(instance); - } - } - b = LL_SPI_ReceiveData8(instance); - if (rxData) { - *(rxData++) = b; - } - --len; - } - - return true; -} - -static uint16_t spiDivisorToBRbits(SPI_TypeDef *instance, uint16_t divisor) -{ -#if !(defined(STM32F1) || defined(STM32F3)) +#if !defined(STM32H7) // SPI2 and SPI3 are on APB1/AHB1 which PCLK is half that of APB2/AHB2. if (instance == SPI2 || instance == SPI3) { @@ -242,65 +105,484 @@ static uint16_t spiDivisorToBRbits(SPI_TypeDef *instance, uint16_t divisor) divisor = constrain(divisor, 2, 256); +#if defined(STM32H7) + const uint32_t baudRatePrescaler[8] = { + LL_SPI_BAUDRATEPRESCALER_DIV2, + LL_SPI_BAUDRATEPRESCALER_DIV4, + LL_SPI_BAUDRATEPRESCALER_DIV8, + LL_SPI_BAUDRATEPRESCALER_DIV16, + LL_SPI_BAUDRATEPRESCALER_DIV32, + LL_SPI_BAUDRATEPRESCALER_DIV64, + LL_SPI_BAUDRATEPRESCALER_DIV128, + LL_SPI_BAUDRATEPRESCALER_DIV256, + }; + int prescalerIndex = ffs(divisor) - 2; // prescaler begins at "/2" + + return baudRatePrescaler[prescalerIndex]; +#else return (ffs(divisor) - 2) << SPI_CR1_BR_Pos; -} - -void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) -{ - LL_SPI_Disable(instance); - LL_SPI_SetBaudRatePrescaler(instance, spiDivisorToBRbits(instance, divisor)); - LL_SPI_Enable(instance); -} - -#ifdef USE_SPI_TRANSACTION -void spiBusTransactionInit(busDevice_t *bus, SPIMode_e mode, uint16_t divisor) -{ - switch (mode) { - case SPI_MODE0_POL_LOW_EDGE_1ST: - defaultInit.ClockPolarity = SPI_POLARITY_LOW; - defaultInit.ClockPhase = SPI_PHASE_1EDGE; - break; - case SPI_MODE1_POL_LOW_EDGE_2ND: - defaultInit.ClockPolarity = SPI_POLARITY_LOW; - defaultInit.ClockPhase = SPI_PHASE_2EDGE; - break; - case SPI_MODE2_POL_HIGH_EDGE_1ST: - defaultInit.ClockPolarity = SPI_POLARITY_HIGH; - defaultInit.ClockPhase = SPI_PHASE_1EDGE; - break; - case SPI_MODE3_POL_HIGH_EDGE_2ND: - defaultInit.ClockPolarity = SPI_POLARITY_HIGH; - defaultInit.ClockPhase = SPI_PHASE_2EDGE; - break; - } - - LL_SPI_Disable(bus->busdev_u.spi.instance); - LL_SPI_DeInit(bus->busdev_u.spi.instance); - - LL_SPI_Init(bus->busdev_u.spi.instance, &defaultInit); - LL_SPI_SetBaudRatePrescaler(bus->busdev_u.spi.instance, spiDivisorToBRbits(bus->busdev_u.spi.instance, divisor)); - - // Configure for 8-bit reads. XXX Is this STM32F303xC specific? - LL_SPI_SetRxFIFOThreshold(bus->busdev_u.spi.instance, SPI_RXFIFO_THRESHOLD_QF); - - LL_SPI_Enable(bus->busdev_u.spi.instance); - - bus->busdev_u.spi.device = &spiDevice[spiDeviceByInstance(bus->busdev_u.spi.instance)]; - bus->busdev_u.spi.modeCache = bus->busdev_u.spi.instance->CR1; -} - -void spiBusTransactionSetup(const busDevice_t *bus) -{ - // We rely on MSTR bit to detect valid modeCache - - if (bus->busdev_u.spi.modeCache && bus->busdev_u.spi.modeCache != bus->busdev_u.spi.device->cr1SoftCopy) { - bus->busdev_u.spi.instance->CR1 = bus->busdev_u.spi.modeCache; - bus->busdev_u.spi.device->cr1SoftCopy = bus->busdev_u.spi.modeCache; - - // SCK seems to require some time to switch to a new initial level after CR1 is written. - // Here we buy some time in addition to the software copy save above. - __asm__("nop"); - } -} -#endif // USE_SPI_TRANSACTION +#endif +} + +void spiInitDevice(SPIDevice device) +{ + spiDevice_t *spi = &spiDevice[device]; + + if (!spi->dev) { + return; + } + + // Enable SPI clock + RCC_ClockCmd(spi->rcc, ENABLE); + RCC_ResetCmd(spi->rcc, ENABLE); + + IOInit(IOGetByTag(spi->sck), OWNER_SPI_SCK, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(spi->miso), OWNER_SPI_MISO, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(spi->mosi), OWNER_SPI_MOSI, RESOURCE_INDEX(device)); + + IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->misoAF); + IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->mosiAF); + IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->sckAF); + + LL_SPI_Disable(spi->dev); + LL_SPI_DeInit(spi->dev); + +#if defined(STM32H7) + // Prevent glitching when SPI is disabled + LL_SPI_EnableGPIOControl(spi->dev); + + LL_SPI_SetFIFOThreshold(spi->dev, LL_SPI_FIFO_TH_01DATA); + LL_SPI_Init(spi->dev, &defaultInit); +#else + LL_SPI_SetRxFIFOThreshold(spi->dev, SPI_RXFIFO_THRESHOLD_QF); + + LL_SPI_Init(spi->dev, &defaultInit); + LL_SPI_Enable(spi->dev); +#endif +} + +void spiInternalResetDescriptors(busDevice_t *bus) +{ + LL_DMA_InitTypeDef *initTx = bus->initTx; + LL_DMA_InitTypeDef *initRx = bus->initRx; + + LL_DMA_StructInit(initTx); +#if defined(STM32H7) + initTx->PeriphRequest = bus->dmaTxChannel; +#else + initTx->Channel = bus->dmaTxChannel; +#endif + initTx->Mode = LL_DMA_MODE_NORMAL; + initTx->Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH; +#if defined(STM32H7) + initTx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->TXDR; +#else + initTx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->DR; +#endif + initTx->Priority = LL_DMA_PRIORITY_LOW; + initTx->PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; + initTx->PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE; + initTx->MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE; + + LL_DMA_StructInit(initRx); +#if defined(STM32H7) + initRx->PeriphRequest = bus->dmaRxChannel; +#else + initRx->Channel = bus->dmaRxChannel; +#endif + initRx->Mode = LL_DMA_MODE_NORMAL; + initRx->Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY; +#if defined(STM32H7) + initRx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->RXDR; +#else + initRx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->DR; +#endif + initRx->Priority = LL_DMA_PRIORITY_LOW; + initRx->PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; + initRx->PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE; +} + +void spiInternalResetStream(dmaChannelDescriptor_t *descriptor) +{ + // Disable the stream + LL_DMA_DisableStream(descriptor->dma, descriptor->stream); + while (LL_DMA_IsEnabledStream(descriptor->dma, descriptor->stream)); + + // Clear any pending interrupt flags + DMA_CLEAR_FLAG(descriptor, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF); +} + + +static bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len) +{ +#if defined(STM32H7) + int txLen = len; + int rxLen = len; + + LL_SPI_SetTransferSize(instance, txLen); + LL_SPI_Enable(instance); + LL_SPI_StartMasterTransfer(instance); + while (txLen || rxLen) { + if (txLen && LL_SPI_IsActiveFlag_TXP(instance)) { + uint8_t b = txData ? *(txData++) : 0xFF; + LL_SPI_TransmitData8(instance, b); + txLen--; + } + + if (rxLen && LL_SPI_IsActiveFlag_RXP(instance)) { + uint8_t b = LL_SPI_ReceiveData8(instance); + if (rxData) { + *(rxData++) = b; + } + rxLen--; + } + } + while (!LL_SPI_IsActiveFlag_EOT(instance)); + LL_SPI_ClearFlag_TXTF(instance); + LL_SPI_Disable(instance); +#else + // set 16-bit transfer + CLEAR_BIT(instance->CR2, SPI_RXFIFO_THRESHOLD); + while (len > 1) { + while (!LL_SPI_IsActiveFlag_TXE(instance)); + uint16_t w; + if (txData) { + w = *((uint16_t *)txData); + txData += 2; + } else { + w = 0xFFFF; + } + LL_SPI_TransmitData16(instance, w); + + while (!LL_SPI_IsActiveFlag_RXNE(instance)); + w = LL_SPI_ReceiveData16(instance); + if (rxData) { + *((uint16_t *)rxData) = w; + rxData += 2; + } + len -= 2; + } + // set 8-bit transfer + SET_BIT(instance->CR2, SPI_RXFIFO_THRESHOLD); + if (len) { + while (!LL_SPI_IsActiveFlag_TXE(instance)); + uint8_t b = txData ? *(txData++) : 0xFF; + LL_SPI_TransmitData8(instance, b); + + while (!LL_SPI_IsActiveFlag_RXNE(instance)); + b = LL_SPI_ReceiveData8(instance); + if (rxData) { + *(rxData++) = b; + } + --len; + } +#endif + + return true; +} + +void spiInternalInitStream(const extDevice_t *dev, bool preInit) +{ + static uint8_t dummyTxByte = 0xff; + static uint8_t dummyRxByte; + busDevice_t *bus = dev->bus; + + busSegment_t *segment = bus->curSegment; + + if (preInit) { + // Prepare the init structure for the next segment to reduce inter-segment interval + segment++; + if(segment->len == 0) { + // There's no following segment + return; + } + } + + uint8_t *txData = segment->txData; + uint8_t *rxData = segment->rxData; + int len = segment->len; + + LL_DMA_InitTypeDef *initTx = bus->initTx; + LL_DMA_InitTypeDef *initRx = bus->initRx; + + if (txData) { +#ifdef __DCACHE_PRESENT +#ifdef STM32H7 + if ((txData < &_dmaram_start__) || (txData >= &_dmaram_end__)) { +#else + // No need to flush DTCM memory + if (!IS_DTCM(txData)) { +#endif + // Flush the D cache to ensure the data to be written is in main memory + SCB_CleanDCache_by_Addr( + (uint32_t *)((uint32_t)txData & ~CACHE_LINE_MASK), + (((uint32_t)txData & CACHE_LINE_MASK) + len - 1 + CACHE_LINE_SIZE) & ~CACHE_LINE_MASK); + } +#endif // __DCACHE_PRESENT + initTx->MemoryOrM2MDstAddress = (uint32_t)txData; + initTx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; + } else { + dummyTxByte = 0xff; + initTx->MemoryOrM2MDstAddress = (uint32_t)&dummyTxByte; + initTx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_NOINCREMENT; + } + initTx->NbData = len; + + if (rxData) { + /* Flush the D cache for the start and end of the receive buffer as + * the cache will be invalidated after the transfer and any valid data + * just before/after must be in memory at that point + */ +#ifdef __DCACHE_PRESENT + // No need to flush/invalidate DTCM memory +#ifdef STM32H7 + if ((rxData < &_dmaram_start__) || (rxData >= &_dmaram_end__)) { +#else + // No need to flush DTCM memory + if (!IS_DTCM(rxData)) { +#endif + SCB_CleanInvalidateDCache_by_Addr( + (uint32_t *)((uint32_t)rxData & ~CACHE_LINE_MASK), + (((uint32_t)rxData & CACHE_LINE_MASK) + len - 1 + CACHE_LINE_SIZE) & ~CACHE_LINE_MASK); + } +#endif // __DCACHE_PRESENT + initRx->MemoryOrM2MDstAddress = (uint32_t)rxData; + initRx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; + } else { + initRx->MemoryOrM2MDstAddress = (uint32_t)&dummyRxByte; + initRx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_NOINCREMENT; + } + // If possible use 16 bit memory writes to prevent atomic access issues on gyro data + if ((initRx->MemoryOrM2MDstAddress & 0x1) || (len & 0x1)) { + initRx->MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE; + } else { + initRx->MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_HALFWORD; + } + initRx->NbData = len; +} + +void spiInternalStartDMA(const extDevice_t *dev) +{ + busDevice_t *bus = dev->bus; + + // Assert Chip Select + IOLo(dev->busType_u.spi.csnPin); + + dmaChannelDescriptor_t *dmaTx = bus->dmaTx; + dmaChannelDescriptor_t *dmaRx = bus->dmaRx; + + DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref; + DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref; + + // Use the correct callback argument + dmaRx->userParam = (uint32_t)dev; + + // Clear transfer flags + DMA_CLEAR_FLAG(dmaTx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF); + DMA_CLEAR_FLAG(dmaRx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF); + + // Disable streams to enable update + LL_DMA_WriteReg(streamRegsTx, CR, 0U); + LL_DMA_WriteReg(streamRegsRx, CR, 0U); + + /* Use the Rx interrupt as this occurs once the SPI operation is complete whereas the Tx interrupt + * occurs earlier when the Tx FIFO is empty, but the SPI operation is still in progress + */ + LL_EX_DMA_EnableIT_TC(streamRegsRx); + + // Update streams + LL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx); + LL_DMA_Init(dmaRx->dma, dmaRx->stream, bus->initRx); + + /* Note from AN4031 + * + * If the user enables the used peripheral before the corresponding DMA stream, a “FEIF” + * (FIFO Error Interrupt Flag) may be set due to the fact the DMA is not ready to provide + * the first required data to the peripheral (in case of memory-to-peripheral transfer). + */ + + // Enable the SPI DMA Tx & Rx requests +#if defined(STM32H7) + LL_SPI_SetTransferSize(dev->bus->busType_u.spi.instance, dev->bus->curSegment->len); + LL_DMA_EnableStream(dmaTx->dma, dmaTx->stream); + LL_DMA_EnableStream(dmaRx->dma, dmaRx->stream); + SET_BIT(dev->bus->busType_u.spi.instance->CFG1, SPI_CFG1_RXDMAEN | SPI_CFG1_TXDMAEN); + LL_SPI_Enable(dev->bus->busType_u.spi.instance); + LL_SPI_StartMasterTransfer(dev->bus->busType_u.spi.instance); +#else + // Enable streams + LL_DMA_EnableStream(dmaTx->dma, dmaTx->stream); + LL_DMA_EnableStream(dmaRx->dma, dmaRx->stream); + + SET_BIT(dev->bus->busType_u.spi.instance->CR2, SPI_CR2_TXDMAEN | SPI_CR2_RXDMAEN); +#endif +} + +void spiInternalStopDMA (const extDevice_t *dev) +{ + busDevice_t *bus = dev->bus; + + dmaChannelDescriptor_t *dmaTx = bus->dmaTx; + dmaChannelDescriptor_t *dmaRx = bus->dmaRx; + SPI_TypeDef *instance = bus->busType_u.spi.instance; + + // Disable the DMA engine and SPI interface + LL_DMA_DisableStream(dmaRx->dma, dmaRx->stream); + LL_DMA_DisableStream(dmaTx->dma, dmaTx->stream); + + DMA_CLEAR_FLAG(dmaRx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF); + + LL_SPI_DisableDMAReq_TX(instance); + LL_SPI_DisableDMAReq_RX(instance); +#if defined(STM32H7) + LL_SPI_ClearFlag_TXTF(dev->bus->busType_u.spi.instance); + LL_SPI_Disable(dev->bus->busType_u.spi.instance); +#endif +} + +// DMA transfer setup and start +void spiSequence(const extDevice_t *dev, busSegment_t *segments) +{ + busDevice_t *bus = dev->bus; + SPI_TypeDef *instance = bus->busType_u.spi.instance; + spiDevice_t *spi = &spiDevice[spiDeviceByInstance(instance)]; + bool dmaSafe = dev->useDMA; + uint32_t xferLen = 0; + uint32_t segmentCount = 0; + + bus->initSegment = true; + bus->curSegment = segments; + + // Switch bus speed +#if !defined(STM32H7) + LL_SPI_Disable(instance); +#endif + + if (dev->busType_u.spi.speed != bus->busType_u.spi.speed) { + LL_SPI_SetBaudRatePrescaler(instance, spiDivisorToBRbits(instance, dev->busType_u.spi.speed)); + bus->busType_u.spi.speed = dev->busType_u.spi.speed; + } + + // Switch SPI clock polarity/phase if necessary + if (dev->busType_u.spi.leadingEdge != bus->busType_u.spi.leadingEdge) { + if (dev->busType_u.spi.leadingEdge) { + IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->sckAF); + LL_SPI_SetClockPhase(instance, LL_SPI_PHASE_1EDGE); + LL_SPI_SetClockPolarity(instance, LL_SPI_POLARITY_LOW); + } + else { + IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->sckAF); + LL_SPI_SetClockPhase(instance, LL_SPI_PHASE_2EDGE); + LL_SPI_SetClockPolarity(instance, LL_SPI_POLARITY_HIGH); + } + + bus->busType_u.spi.leadingEdge = dev->busType_u.spi.leadingEdge; + } + +#if !defined(STM32H7) + LL_SPI_Enable(instance); +#endif + + /* Where data is being read into a buffer which is cached, where the start or end of that + * buffer is not cache aligned, there is a risk of corruption of other data in that cache line. + * After the read is complete, the cache lines covering the structure will be invalidated to ensure + * that the processor sees the read data, not what was in cache previously. Unfortunately if + * there is any other data in the area covered by those cache lines, at the start or end of the + * buffer, it too will be invalidated, so had the processor written to those locations during the DMA + * operation those written values will be lost. + */ + + // Check that any reads are cache aligned and of multiple cache lines in length + for (busSegment_t *checkSegment = bus->curSegment; checkSegment->len; checkSegment++) { +#ifdef STM32H7 + // Check if RX data can be DMAed + if ((checkSegment->rxData) && + // DTCM can't be accessed by DMA1/2 on the H7 + (IS_DTCM(checkSegment->rxData) || + // Memory declared as DMA_RAM will have an address between &_dmaram_start__ and &_dmaram_end__ + (((checkSegment->rxData < &_dmaram_start__) || (checkSegment->rxData >= &_dmaram_end__)) && + (((uint32_t)checkSegment->rxData & (CACHE_LINE_SIZE - 1)) || (checkSegment->len & (CACHE_LINE_SIZE - 1)))))) { + dmaSafe = false; + break; + } + // Check if TX data can be DMAed + else if ((checkSegment->txData) && IS_DTCM(checkSegment->txData)) { + dmaSafe = false; + break; + } +#elif defined(STM32F7) + if ((checkSegment->rxData) && + // DTCM is accessible and uncached on the F7 + (!IS_DTCM(checkSegment->rxData) && + (((uint32_t)checkSegment->rxData & (CACHE_LINE_SIZE - 1)) || (checkSegment->len & (CACHE_LINE_SIZE - 1))))) { + dmaSafe = false; + break; + } +#elif defined(STM32F4) + // Check if RX data can be DMAed + if ((checkSegment->rxData) && + // DTCM can't be accessed by DMA1/2 on the F4 + (IS_DTCM(checkSegment->rxData)) { + dmaSafe = false; + break; + } + if ((checkSegment->txData) && + // DTCM can't be accessed by DMA1/2 on the F4 + (IS_DTCM(checkSegment->txData)) { + dmaSafe = false; + break; + } +#endif + // Note that these counts are only valid if dmaSafe is true + segmentCount++; + xferLen += checkSegment->len; + } + + // Use DMA if possible + if (bus->useDMA && dmaSafe && ((segmentCount > 1) || (xferLen > 8))) { + // Intialise the init structures for the first transfer + spiInternalInitStream(dev, false); + + // Start the transfers + spiInternalStartDMA(dev); + } else { + // Manually work through the segment list performing a transfer for each + while (bus->curSegment->len) { + // Assert Chip Select + IOLo(dev->busType_u.spi.csnPin); + + spiInternalReadWriteBufPolled( + bus->busType_u.spi.instance, + bus->curSegment->txData, + bus->curSegment->rxData, + bus->curSegment->len); + + if (bus->curSegment->negateCS) { + // Negate Chip Select + IOHi(dev->busType_u.spi.csnPin); + } + + if (bus->curSegment->callback) { + switch(bus->curSegment->callback(dev->callbackArg)) { + case BUS_BUSY: + // Repeat the last DMA segment + bus->curSegment--; + break; + + case BUS_ABORT: + bus->curSegment = (busSegment_t *)NULL; + return; + + case BUS_READY: + default: + // Advance to the next DMA segment + break; + } + } + bus->curSegment++; + } + + bus->curSegment = (busSegment_t *)NULL; + } +} #endif diff --git a/src/main/drivers/bus_spi_stdperiph.c b/src/main/drivers/bus_spi_stdperiph.c index 0f9dec372..47d614f30 100644 --- a/src/main/drivers/bus_spi_stdperiph.c +++ b/src/main/drivers/bus_spi_stdperiph.c @@ -26,6 +26,9 @@ #ifdef USE_SPI +// STM32F405 can't DMA to/from FASTRAM (CCM SRAM) +#define IS_CCM(p) (((uint32_t)p & 0xffff0000) == 0x10000000) + #include "common/maths.h" #include "drivers/bus.h" #include "drivers/bus_spi.h" @@ -33,7 +36,6 @@ #include "drivers/exti.h" #include "drivers/io.h" #include "drivers/rcc.h" -#include "drivers/time.h" static SPI_InitTypeDef defaultInit = { .SPI_Mode = SPI_Mode_Master, @@ -43,160 +45,19 @@ static SPI_InitTypeDef defaultInit = { .SPI_FirstBit = SPI_FirstBit_MSB, .SPI_CRCPolynomial = 7, .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge }; -void spiInitDevice(SPIDevice device, bool leadingEdge) -{ - spiDevice_t *spi = &(spiDevice[device]); - - if (!spi->dev) { - return; - } - -#ifndef USE_SPI_TRANSACTION - spi->leadingEdge = leadingEdge; -#else - UNUSED(leadingEdge); -#endif - - // Enable SPI clock - RCC_ClockCmd(spi->rcc, ENABLE); - RCC_ResetCmd(spi->rcc, ENABLE); - - IOInit(IOGetByTag(spi->sck), OWNER_SPI_SCK, RESOURCE_INDEX(device)); - IOInit(IOGetByTag(spi->miso), OWNER_SPI_MISO, RESOURCE_INDEX(device)); - IOInit(IOGetByTag(spi->mosi), OWNER_SPI_MOSI, RESOURCE_INDEX(device)); - -#if defined(STM32F3) || defined(STM32F4) - IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_CFG, spi->af); - IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_CFG, spi->af); - IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af); -#elif defined(STM32F10X) - IOConfigGPIO(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG); - IOConfigGPIO(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG); - IOConfigGPIO(IOGetByTag(spi->mosi), SPI_IO_AF_MOSI_CFG); -#else -#error Undefined MCU architecture -#endif - - // Init SPI hardware - SPI_I2S_DeInit(spi->dev); - -#ifndef USE_SPI_TRANSACTION - if (spi->leadingEdge) { - defaultInit.SPI_CPOL = SPI_CPOL_Low; - defaultInit.SPI_CPHA = SPI_CPHA_1Edge; - } else -#endif - { - defaultInit.SPI_CPOL = SPI_CPOL_High; - defaultInit.SPI_CPHA = SPI_CPHA_2Edge; - } - -#ifdef STM32F303xC - // Configure for 8-bit reads. - SPI_RxFIFOThresholdConfig(spi->dev, SPI_RxFIFOThreshold_QF); -#endif - - SPI_Init(spi->dev, &defaultInit); - SPI_Cmd(spi->dev, ENABLE); -} - -// return uint8_t value or -1 when failure -uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte) -{ - timeUs_t timeoutStartUs = microsISR(); - - DISCARD(instance->DR); - - while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET) { - if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) { - return spiTimeoutUserCallback(instance); - } - } - -#ifdef STM32F303xC - SPI_SendData8(instance, txByte); -#else - SPI_I2S_SendData(instance, txByte); -#endif - timeoutStartUs = microsISR(); - while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET) { - if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) { - return spiTimeoutUserCallback(instance); - } - } - -#ifdef STM32F303xC - return ((uint8_t)SPI_ReceiveData8(instance)); -#else - return ((uint8_t)SPI_I2S_ReceiveData(instance)); -#endif -} - -/** - * Return true if the bus is currently in the middle of a transmission. - */ -bool spiIsBusBusy(SPI_TypeDef *instance) -{ -#ifdef STM32F303xC - return SPI_GetTransmissionFIFOStatus(instance) != SPI_TransmissionFIFOStatus_Empty || SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_BSY) == SET; -#else - return SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET || SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_BSY) == SET; -#endif - -} - -bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len) -{ - timeUs_t timeoutStartUs; - - uint8_t b; - DISCARD(instance->DR); - while (len--) { - b = txData ? *(txData++) : 0xFF; - timeoutStartUs = microsISR(); - while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET) { - if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) { - return spiTimeoutUserCallback(instance); - } - } -#ifdef STM32F303xC - SPI_SendData8(instance, b); -#else - SPI_I2S_SendData(instance, b); -#endif - - timeoutStartUs = microsISR(); - - while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET) { - if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) { - return spiTimeoutUserCallback(instance); - } - } -#ifdef STM32F303xC - b = SPI_ReceiveData8(instance); -#else - b = SPI_I2S_ReceiveData(instance); -#endif - if (rxData) { - *(rxData++) = b; - } - } - - return true; -} - static uint16_t spiDivisorToBRbits(SPI_TypeDef *instance, uint16_t divisor) { -#if !(defined(STM32F1) || defined(STM32F3)) // SPI2 and SPI3 are on APB1/AHB1 which PCLK is half that of APB2/AHB2. - +#if defined(STM32F410xx) || defined(STM32F411xE) + UNUSED(instance); +#else if (instance == SPI2 || instance == SPI3) { divisor /= 2; // Safe for divisor == 0 or 1 } -#else - UNUSED(instance); #endif divisor = constrain(divisor, 2, 256); @@ -212,61 +73,294 @@ static void spiSetDivisorBRreg(SPI_TypeDef *instance, uint16_t divisor) #undef BR_BITS } -void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) + +void spiInitDevice(SPIDevice device) { + spiDevice_t *spi = &(spiDevice[device]); + + if (!spi->dev) { + return; + } + + // Enable SPI clock + RCC_ClockCmd(spi->rcc, ENABLE); + RCC_ResetCmd(spi->rcc, ENABLE); + + IOInit(IOGetByTag(spi->sck), OWNER_SPI_SCK, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(spi->miso), OWNER_SPI_MISO, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(spi->mosi), OWNER_SPI_MOSI, RESOURCE_INDEX(device)); + + IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG, spi->af); + IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->af); + IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af); + + // Init SPI hardware + SPI_I2S_DeInit(spi->dev); + + SPI_I2S_DMACmd(spi->dev, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, DISABLE); + SPI_Init(spi->dev, &defaultInit); + SPI_Cmd(spi->dev, ENABLE); +} + +void spiInternalResetDescriptors(busDevice_t *bus) +{ + DMA_InitTypeDef *initTx = bus->initTx; + DMA_InitTypeDef *initRx = bus->initRx; + + DMA_StructInit(initTx); + initTx->DMA_Channel = bus->dmaTxChannel; + initTx->DMA_DIR = DMA_DIR_MemoryToPeripheral; + initTx->DMA_Mode = DMA_Mode_Normal; + initTx->DMA_PeripheralBaseAddr = (uint32_t)&bus->busType_u.spi.instance->DR; + initTx->DMA_Priority = DMA_Priority_Low; + initTx->DMA_PeripheralInc = DMA_PeripheralInc_Disable; + initTx->DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; + initTx->DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + + DMA_StructInit(initRx); + initRx->DMA_Channel = bus->dmaRxChannel; + initRx->DMA_DIR = DMA_DIR_PeripheralToMemory; + initRx->DMA_Mode = DMA_Mode_Normal; + initRx->DMA_PeripheralBaseAddr = (uint32_t)&bus->busType_u.spi.instance->DR; + initRx->DMA_Priority = DMA_Priority_Low; + initRx->DMA_PeripheralInc = DMA_PeripheralInc_Disable; + initRx->DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; +} + +void spiInternalResetStream(dmaChannelDescriptor_t *descriptor) +{ + DMA_Stream_TypeDef *streamRegs = (DMA_Stream_TypeDef *)descriptor->ref; + + // Disable the stream + streamRegs->CR = 0U; + + // Clear any pending interrupt flags + DMA_CLEAR_FLAG(descriptor, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF); +} + +static bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len) +{ + uint8_t b; + + while (len--) { + b = txData ? *(txData++) : 0xFF; + while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET); + SPI_I2S_SendData(instance, b); + + while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET); + b = SPI_I2S_ReceiveData(instance); + if (rxData) { + *(rxData++) = b; + } + } + + return true; +} + +void spiInternalInitStream(const extDevice_t *dev, bool preInit) +{ + static uint8_t dummyTxByte = 0xff; + static uint8_t dummyRxByte; + busDevice_t *bus = dev->bus; + + volatile busSegment_t *segment = bus->curSegment; + + if (preInit) { + // Prepare the init structure for the next segment to reduce inter-segment interval + segment++; + if(segment->len == 0) { + // There's no following segment + return; + } + } + + uint8_t *txData = segment->txData; + uint8_t *rxData = segment->rxData; + int len = segment->len; + + DMA_InitTypeDef *initTx = bus->initTx; + DMA_InitTypeDef *initRx = bus->initRx; + + if (txData) { + initTx->DMA_Memory0BaseAddr = (uint32_t)txData; + initTx->DMA_MemoryInc = DMA_MemoryInc_Enable; + } else { + dummyTxByte = 0xff; + initTx->DMA_Memory0BaseAddr = (uint32_t)&dummyTxByte; + initTx->DMA_MemoryInc = DMA_MemoryInc_Disable; + } + initTx->DMA_BufferSize = len; + + if (rxData) { + initRx->DMA_Memory0BaseAddr = (uint32_t)rxData; + initRx->DMA_MemoryInc = DMA_MemoryInc_Enable; + } else { + initRx->DMA_Memory0BaseAddr = (uint32_t)&dummyRxByte; + initRx->DMA_MemoryInc = DMA_MemoryInc_Disable; + } + // If possible use 16 bit memory writes to prevent atomic access issues on gyro data + if ((initRx->DMA_Memory0BaseAddr & 0x1) || (len & 0x1)) + { + initRx->DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + } else { + initRx->DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; + } + initRx->DMA_BufferSize = len; +} + +void spiInternalStartDMA(const extDevice_t *dev) +{ + // Assert Chip Select + IOLo(dev->busType_u.spi.csnPin); + + dmaChannelDescriptor_t *dmaTx = dev->bus->dmaTx; + dmaChannelDescriptor_t *dmaRx = dev->bus->dmaRx; + DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref; + DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref; + + // Use the correct callback argument + dmaRx->userParam = (uint32_t)dev; + + // Clear transfer flags + DMA_CLEAR_FLAG(dmaTx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF); + DMA_CLEAR_FLAG(dmaRx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF); + + // Disable streams to enable update + streamRegsTx->CR = 0U; + streamRegsRx->CR = 0U; + + /* Use the Rx interrupt as this occurs once the SPI operation is complete whereas the Tx interrupt + * occurs earlier when the Tx FIFO is empty, but the SPI operation is still in progress + */ + DMA_ITConfig(streamRegsRx, DMA_IT_TC, ENABLE); + + // Update streams + DMA_Init(streamRegsTx, dev->bus->initTx); + DMA_Init(streamRegsRx, dev->bus->initRx); + + /* Note from AN4031 + * + * If the user enables the used peripheral before the corresponding DMA stream, a “FEIF” + * (FIFO Error Interrupt Flag) may be set due to the fact the DMA is not ready to provide + * the first required data to the peripheral (in case of memory-to-peripheral transfer). + */ + + // Enable streams + DMA_Cmd(streamRegsTx, ENABLE); + DMA_Cmd(streamRegsRx, ENABLE); + + /* Enable the SPI DMA Tx & Rx requests */ + SPI_I2S_DMACmd(dev->bus->busType_u.spi.instance, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE); +} + + +void spiInternalStopDMA (const extDevice_t *dev) +{ + dmaChannelDescriptor_t *dmaTx = dev->bus->dmaTx; + dmaChannelDescriptor_t *dmaRx = dev->bus->dmaRx; + DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref; + DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref; + SPI_TypeDef *instance = dev->bus->busType_u.spi.instance; + + // Disable streams + streamRegsTx->CR = 0U; + streamRegsRx->CR = 0U; + + SPI_I2S_DMACmd(instance, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, DISABLE); +} + +// DMA transfer setup and start +void spiSequence(const extDevice_t *dev, busSegment_t *segments) +{ + busDevice_t *bus = dev->bus; + SPI_TypeDef *instance = bus->busType_u.spi.instance; + bool dmaSafe = dev->useDMA; + uint32_t xferLen = 0; + uint32_t segmentCount = 0; + + dev->bus->initSegment = true; + dev->bus->curSegment = segments; + SPI_Cmd(instance, DISABLE); - spiSetDivisorBRreg(instance, divisor); + + // Switch bus speed + if (dev->busType_u.spi.speed != bus->busType_u.spi.speed) { + spiSetDivisorBRreg(bus->busType_u.spi.instance, dev->busType_u.spi.speed); + bus->busType_u.spi.speed = dev->busType_u.spi.speed; + } + + if (dev->busType_u.spi.leadingEdge != bus->busType_u.spi.leadingEdge) { + // Switch SPI clock polarity/phase + instance->CR1 &= ~(SPI_CPOL_High | SPI_CPHA_2Edge); + + // Apply setting + if (dev->busType_u.spi.leadingEdge) { + instance->CR1 |= SPI_CPOL_Low | SPI_CPHA_1Edge; + } else + { + instance->CR1 |= SPI_CPOL_High | SPI_CPHA_2Edge; + } + bus->busType_u.spi.leadingEdge = dev->busType_u.spi.leadingEdge; + } + SPI_Cmd(instance, ENABLE); -} -#ifdef USE_SPI_TRANSACTION - -void spiBusTransactionInit(busDevice_t *bus, SPIMode_e mode, uint16_t divider) -{ - switch (mode) { - case SPI_MODE0_POL_LOW_EDGE_1ST: - defaultInit.SPI_CPOL = SPI_CPOL_Low; - defaultInit.SPI_CPHA = SPI_CPHA_1Edge; - break; - case SPI_MODE1_POL_LOW_EDGE_2ND: - defaultInit.SPI_CPOL = SPI_CPOL_Low; - defaultInit.SPI_CPHA = SPI_CPHA_2Edge; - break; - case SPI_MODE2_POL_HIGH_EDGE_1ST: - defaultInit.SPI_CPOL = SPI_CPOL_High; - defaultInit.SPI_CPHA = SPI_CPHA_1Edge; - break; - case SPI_MODE3_POL_HIGH_EDGE_2ND: - defaultInit.SPI_CPOL = SPI_CPOL_High; - defaultInit.SPI_CPHA = SPI_CPHA_2Edge; - break; + // Check that any there are no attempts to DMA to/from CCD SRAM + for (busSegment_t *checkSegment = bus->curSegment; checkSegment->len; checkSegment++) { + if (((checkSegment->rxData) && IS_CCM(checkSegment->rxData)) || + ((checkSegment->txData) && IS_CCM(checkSegment->txData))) { + dmaSafe = false; + break; + } + // Note that these counts are only valid if dmaSafe is true + segmentCount++; + xferLen += checkSegment->len; } + // Use DMA if possible + if (bus->useDMA && dmaSafe && ((segmentCount > 1) || (xferLen > 8))) { + // Intialise the init structures for the first transfer + spiInternalInitStream(dev, false); - // Initialize the SPI instance to setup CR1 + // Start the transfers + spiInternalStartDMA(dev); + } else { + // Manually work through the segment list performing a transfer for each + while (bus->curSegment->len) { + // Assert Chip Select + IOLo(dev->busType_u.spi.csnPin); - SPI_Init(bus->busdev_u.spi.instance, &defaultInit); - spiSetDivisorBRreg(bus->busdev_u.spi.instance, divider); -#ifdef STM32F303xC - // Configure for 8-bit reads. - SPI_RxFIFOThresholdConfig(bus->busdev_u.spi.instance, SPI_RxFIFOThreshold_QF); -#endif + spiInternalReadWriteBufPolled( + bus->busType_u.spi.instance, + bus->curSegment->txData, + bus->curSegment->rxData, + bus->curSegment->len); - bus->busdev_u.spi.modeCache = bus->busdev_u.spi.instance->CR1; - bus->busdev_u.spi.device = &spiDevice[spiDeviceByInstance(bus->busdev_u.spi.instance)]; -} + if (bus->curSegment->negateCS) { + // Negate Chip Select + IOHi(dev->busType_u.spi.csnPin); + } -void spiBusTransactionSetup(const busDevice_t *bus) -{ - // We rely on MSTR bit to detect valid modeCache + if (bus->curSegment->callback) { + switch(bus->curSegment->callback(dev->callbackArg)) { + case BUS_BUSY: + // Repeat the last DMA segment + bus->curSegment--; + break; - if (bus->busdev_u.spi.modeCache && bus->busdev_u.spi.modeCache != bus->busdev_u.spi.device->cr1SoftCopy) { - bus->busdev_u.spi.instance->CR1 = bus->busdev_u.spi.modeCache; - bus->busdev_u.spi.device->cr1SoftCopy = bus->busdev_u.spi.modeCache; + case BUS_ABORT: + bus->curSegment = (busSegment_t *)NULL; + return; - // SCK seems to require some time to switch to a new initial level after CR1 is written. - // Here we buy some time in addition to the software copy save above. - __asm__("nop"); + case BUS_READY: + default: + // Advance to the next DMA segment + break; + } + } + bus->curSegment++; + } + + bus->curSegment = (busSegment_t *)NULL; } } -#endif // USE_SPI_TRANSACTION #endif diff --git a/src/main/drivers/compass/compass.h b/src/main/drivers/compass/compass.h index 3f49092c8..ffab6dde4 100644 --- a/src/main/drivers/compass/compass.h +++ b/src/main/drivers/compass/compass.h @@ -30,7 +30,8 @@ typedef struct magDev_s { sensorMagInitFuncPtr init; // initialize function sensorMagReadFuncPtr read; // read 3 axis data function extiCallbackRec_t exti; - busDevice_t busdev; + extDevice_t dev; + busDevice_t bus; // For MPU slave bus instance sensor_align_e magAlignment; fp_rotationMatrix_t rotationMatrix; ioTag_t magIntExtiTag; diff --git a/src/main/drivers/compass/compass_ak8963.c b/src/main/drivers/compass/compass_ak8963.c index 6b1b15a1f..5f43e874c 100644 --- a/src/main/drivers/compass/compass_ak8963.c +++ b/src/main/drivers/compass/compass_ak8963.c @@ -103,35 +103,35 @@ #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) -static bool ak8963SpiWriteRegisterDelay(const busDevice_t *bus, uint8_t reg, uint8_t data) +static bool ak8963SpiWriteRegisterDelay(const extDevice_t *dev, uint8_t reg, uint8_t data) { - spiBusWriteRegister(bus, reg, data); + spiWriteReg(dev, reg, data); delayMicroseconds(10); return true; } -static bool ak8963SlaveReadRegisterBuffer(const busDevice_t *slavedev, uint8_t reg, uint8_t *buf, uint8_t len) +static bool ak8963SlaveReadRegisterBuffer(const extDevice_t *slaveDev, uint8_t reg, uint8_t *buf, uint8_t len) { - const busDevice_t *bus = slavedev->busdev_u.mpuSlave.master; + extDevice_t *dev = slaveDev->bus->busType_u.mpuSlave.master; - ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.mpuSlave.address | READ_FLAG); // set I2C slave address for read - ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register - ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, (len & 0x0F) | I2C_SLV0_EN); // read number of bytes + ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_ADDR, slaveDev->busType_u.mpuSlave.address | READ_FLAG); // set I2C slave address for read + ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register + ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_CTRL, (len & 0x0F) | I2C_SLV0_EN); // read number of bytes delay(4); __disable_irq(); - bool ack = spiBusReadRegisterBuffer(bus, MPU_RA_EXT_SENS_DATA_00, buf, len); // read I2C + bool ack = spiReadRegMskBufRB(dev, MPU_RA_EXT_SENS_DATA_00, buf, len); // read I2C __enable_irq(); return ack; } -static bool ak8963SlaveWriteRegister(const busDevice_t *slavedev, uint8_t reg, uint8_t data) +static bool ak8963SlaveWriteRegister(const extDevice_t *slaveDev, uint8_t reg, uint8_t data) { - const busDevice_t *bus = slavedev->busdev_u.mpuSlave.master; + extDevice_t *dev = slaveDev->bus->busType_u.mpuSlave.master; - ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.mpuSlave.address); // set I2C slave address for write - ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register - ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_DO, data); // set I2C sLave value - ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, (1 & 0x0F) | I2C_SLV0_EN); // write 1 byte + ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_ADDR, slaveDev->busType_u.mpuSlave.address); // set I2C slave address for write + ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register + ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_DO, data); // set I2C sLave value + ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_CTRL, (1 & 0x0F) | I2C_SLV0_EN); // write 1 byte return true; } @@ -143,19 +143,19 @@ typedef struct queuedReadState_s { static queuedReadState_t queuedRead = { false, 0, 0}; -static bool ak8963SlaveStartRead(const busDevice_t *slavedev, uint8_t reg, uint8_t len) +static bool ak8963SlaveStartRead(const extDevice_t *slaveDev, uint8_t reg, uint8_t len) { if (queuedRead.waiting) { return false; } - const busDevice_t *bus = slavedev->busdev_u.mpuSlave.master; + extDevice_t *dev = slaveDev->bus->busType_u.mpuSlave.master; queuedRead.len = len; - ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.mpuSlave.address | READ_FLAG); // set I2C slave address for read - ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register - ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, (len & 0x0F) | I2C_SLV0_EN); // read number of bytes + ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_ADDR, slaveDev->busType_u.mpuSlave.address | READ_FLAG); // set I2C slave address for read + ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register + ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_CTRL, (len & 0x0F) | I2C_SLV0_EN); // read number of bytes queuedRead.readStartedAt = micros(); queuedRead.waiting = true; @@ -180,11 +180,11 @@ static uint32_t ak8963SlaveQueuedReadTimeRemaining(void) return timeRemaining; } -static bool ak8963SlaveCompleteRead(const busDevice_t *slavedev, uint8_t *buf) +static bool ak8963SlaveCompleteRead(const extDevice_t *slaveDev, uint8_t *buf) { uint32_t timeRemaining = ak8963SlaveQueuedReadTimeRemaining(); - const busDevice_t *bus = slavedev->busdev_u.mpuSlave.master; + extDevice_t *dev = slaveDev->bus->busType_u.mpuSlave.master; if (timeRemaining > 0) { delayMicroseconds(timeRemaining); @@ -192,11 +192,10 @@ static bool ak8963SlaveCompleteRead(const busDevice_t *slavedev, uint8_t *buf) queuedRead.waiting = false; - spiBusReadRegisterBuffer(bus, MPU_RA_EXT_SENS_DATA_00, buf, queuedRead.len); // read I2C buffer - return true; + return spiReadRegMskBufRB(dev, MPU_RA_EXT_SENS_DATA_00, buf, queuedRead.len); // read I2C buffer } -static bool ak8963SlaveReadData(const busDevice_t *busdev, uint8_t *buf) +static bool ak8963SlaveReadData(const extDevice_t *dev, uint8_t *buf) { typedef enum { CHECK_STATUS = 0, @@ -216,7 +215,7 @@ static bool ak8963SlaveReadData(const busDevice_t *busdev, uint8_t *buf) restart: switch (state) { case CHECK_STATUS: { - ak8963SlaveStartRead(busdev, AK8963_MAG_REG_ST1, 1); + ak8963SlaveStartRead(dev, AK8963_MAG_REG_ST1, 1); state = WAITING_FOR_STATUS; return false; } @@ -227,7 +226,7 @@ restart: return false; } - ack = ak8963SlaveCompleteRead(busdev, &buf[0]); + ack = ak8963SlaveCompleteRead(dev, &buf[0]); uint8_t status = buf[0]; @@ -242,7 +241,7 @@ restart: } // read the 6 bytes of data and the status2 register - ak8963SlaveStartRead(busdev, AK8963_MAG_REG_HXL, 7); + ak8963SlaveStartRead(dev, AK8963_MAG_REG_HXL, 7); state = WAITING_FOR_DATA; return false; @@ -254,7 +253,7 @@ restart: return false; } - ack = ak8963SlaveCompleteRead(busdev, &buf[0]); + ack = ak8963SlaveCompleteRead(dev, &buf[0]); state = CHECK_STATUS; } } @@ -263,37 +262,37 @@ restart: } #endif -static bool ak8963ReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *buf, uint8_t len) +static bool ak8963ReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *buf, uint8_t len) { #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) - if (busdev->bustype == BUSTYPE_MPU_SLAVE) { - return ak8963SlaveReadRegisterBuffer(busdev, reg, buf, len); + if (dev->bus->busType == BUS_TYPE_MPU_SLAVE) { + return ak8963SlaveReadRegisterBuffer(dev, reg, buf, len); } #endif - return busReadRegisterBuffer(busdev, reg, buf, len); + return busReadRegisterBuffer(dev, reg, buf, len); } -static bool ak8963WriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data) +static bool ak8963WriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data) { #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) - if (busdev->bustype == BUSTYPE_MPU_SLAVE) { - return ak8963SlaveWriteRegister(busdev, reg, data); + if (dev->bus->busType == BUS_TYPE_MPU_SLAVE) { + return ak8963SlaveWriteRegister(dev, reg, data); } #endif - return busWriteRegister(busdev, reg, data); + return busWriteRegister(dev, reg, data); } -static bool ak8963DirectReadData(const busDevice_t *busdev, uint8_t *buf) +static bool ak8963DirectReadData(const extDevice_t *dev, uint8_t *buf) { uint8_t status; - bool ack = ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_ST1, &status, 1); + bool ack = ak8963ReadRegisterBuffer(dev, AK8963_MAG_REG_ST1, &status, 1); if (!ack || (status & ST1_DATA_READY) == 0) { return false; } - return ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_HXL, buf, 7); + return ak8963ReadRegisterBuffer(dev, AK8963_MAG_REG_HXL, buf, 7); } static int16_t parseMag(uint8_t *raw, int16_t gain) { @@ -306,19 +305,19 @@ static bool ak8963Read(magDev_t *mag, int16_t *magData) bool ack = false; uint8_t buf[7]; - const busDevice_t *busdev = &mag->busdev; + extDevice_t *dev = &mag->dev; - switch (busdev->bustype) { + switch (dev->bus->busType) { #if defined(USE_MAG_SPI_AK8963) || defined(USE_MAG_AK8963) - case BUSTYPE_I2C: - case BUSTYPE_SPI: - ack = ak8963DirectReadData(busdev, buf); + case BUS_TYPE_I2C: + case BUS_TYPE_SPI: + ack = ak8963DirectReadData(dev, buf); break; #endif #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) - case BUSTYPE_MPU_SLAVE: - ack = ak8963SlaveReadData(busdev, buf); + case BUS_TYPE_MPU_SLAVE: + ack = ak8963SlaveReadData(dev, buf); break; #endif default: @@ -330,7 +329,7 @@ static bool ak8963Read(magDev_t *mag, int16_t *magData) return false; } - ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_BIT_16_BIT | CNTL1_MODE_ONCE); // start reading again uint8_t status2 = buf[6]; + ak8963WriteRegister(dev, AK8963_MAG_REG_CNTL1, CNTL1_BIT_16_BIT | CNTL1_MODE_ONCE); // start reading again uint8_t status2 = buf[6]; if (status2 & ST2_MAG_SENSOR_OVERFLOW) { return false; @@ -348,59 +347,55 @@ static bool ak8963Init(magDev_t *mag) uint8_t asa[3]; uint8_t status; - const busDevice_t *busdev = &mag->busdev; + extDevice_t *dev = &mag->dev; - busDeviceRegister(busdev); + busDeviceRegister(dev); - ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down before entering fuse mode - ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_FUSE_ROM); // Enter Fuse ROM access mode - ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_ASAX, asa, sizeof(asa)); // Read the x-, y-, and z-axis calibration values + ak8963WriteRegister(dev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down before entering fuse mode + ak8963WriteRegister(dev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_FUSE_ROM); // Enter Fuse ROM access mode + ak8963ReadRegisterBuffer(dev, AK8963_MAG_REG_ASAX, asa, sizeof(asa)); // Read the x-, y-, and z-axis calibration values mag->magGain[X] = asa[X] + 128; mag->magGain[Y] = asa[Y] + 128; mag->magGain[Z] = asa[Z] + 128; - ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down after reading. + ak8963WriteRegister(dev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down after reading. // Clear status registers - ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_ST1, &status, 1); - ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_ST2, &status, 1); + ak8963ReadRegisterBuffer(dev, AK8963_MAG_REG_ST1, &status, 1); + ak8963ReadRegisterBuffer(dev, AK8963_MAG_REG_ST2, &status, 1); // Trigger first measurement - ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_BIT_16_BIT | CNTL1_MODE_ONCE); + ak8963WriteRegister(dev, AK8963_MAG_REG_CNTL1, CNTL1_BIT_16_BIT | CNTL1_MODE_ONCE); return true; } -void ak8963BusInit(busDevice_t *busdev) +void ak8963BusInit(const extDevice_t *dev) { - switch (busdev->bustype) { + switch (dev->bus->busType) { #ifdef USE_MAG_AK8963 - case BUSTYPE_I2C: - UNUSED(busdev); + case BUS_TYPE_I2C: + UNUSED(dev); break; #endif #ifdef USE_MAG_SPI_AK8963 - case BUSTYPE_SPI: - IOHi(busdev->busdev_u.spi.csnPin); // Disable - IOInit(busdev->busdev_u.spi.csnPin, OWNER_COMPASS_CS, 0); - IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP); -#ifdef USE_SPI_TRANSACTION - spiBusTransactionInit(busdev, SPI_MODE3_POL_HIGH_EDGE_2ND, spiCalculateDivider(AK8963_MAX_SPI_CLK_HZ)); -#else - spiBusSetDivisor(busdev, spiCalculateDivider(AK8963_MAX_SPI_CLK_HZ)); -#endif + case BUS_TYPE_SPI: + IOHi(dev->busType_u.spi.csnPin); // Disable + IOInit(dev->busType_u.spi.csnPin, OWNER_COMPASS_CS, 0); + IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP); + spiSetClkDivisor(dev, spiCalculateDivider(AK8963_MAX_SPI_CLK_HZ)); break; #endif #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) - case BUSTYPE_MPU_SLAVE: + case BUS_TYPE_MPU_SLAVE: rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40)); // initialze I2C master via SPI bus - ak8963SpiWriteRegisterDelay(busdev->busdev_u.mpuSlave.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); - ak8963SpiWriteRegisterDelay(busdev->busdev_u.mpuSlave.master, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz - ak8963SpiWriteRegisterDelay(busdev->busdev_u.mpuSlave.master, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only + ak8963SpiWriteRegisterDelay(dev->bus->busType_u.mpuSlave.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); + ak8963SpiWriteRegisterDelay(dev->bus->busType_u.mpuSlave.master, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz + ak8963SpiWriteRegisterDelay(dev->bus->busType_u.mpuSlave.master, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only break; #endif default: @@ -408,24 +403,24 @@ void ak8963BusInit(busDevice_t *busdev) } } -void ak8963BusDeInit(const busDevice_t *busdev) +void ak8963BusDeInit(const extDevice_t *dev) { - switch (busdev->bustype) { + switch (dev->bus->busType) { #ifdef USE_MAG_AK8963 - case BUSTYPE_I2C: - UNUSED(busdev); + case BUS_TYPE_I2C: + UNUSED(dev); break; #endif #ifdef USE_MAG_SPI_AK8963 - case BUSTYPE_SPI: - spiPreinitByIO(busdev->busdev_u.spi.csnPin); + case BUS_TYPE_SPI: + spiPreinitByIO(dev->busType_u.spi.csnPin); break; #endif #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) - case BUSTYPE_MPU_SLAVE: - ak8963SpiWriteRegisterDelay(busdev->busdev_u.mpuSlave.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); + case BUS_TYPE_MPU_SLAVE: + ak8963SpiWriteRegisterDelay(dev->bus->busType_u.mpuSlave.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); break; #endif default: @@ -437,18 +432,18 @@ bool ak8963Detect(magDev_t *mag) { uint8_t sig = 0; - busDevice_t *busdev = &mag->busdev; + extDevice_t *dev = &mag->dev; - if ((busdev->bustype == BUSTYPE_I2C || busdev->bustype == BUSTYPE_MPU_SLAVE) && busdev->busdev_u.mpuSlave.address == 0) { - busdev->busdev_u.mpuSlave.address = AK8963_MAG_I2C_ADDRESS; + if ((dev->bus->busType == BUS_TYPE_I2C || dev->bus->busType == BUS_TYPE_MPU_SLAVE) && dev->busType_u.mpuSlave.address == 0) { + dev->busType_u.mpuSlave.address = AK8963_MAG_I2C_ADDRESS; } - ak8963BusInit(busdev); + ak8963BusInit(dev); - ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL2, CNTL2_SOFT_RESET); // reset MAG + ak8963WriteRegister(dev, AK8963_MAG_REG_CNTL2, CNTL2_SOFT_RESET); // reset MAG delay(4); - bool ack = ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_WIA, &sig, 1); // check for AK8963 + bool ack = ak8963ReadRegisterBuffer(dev, AK8963_MAG_REG_WIA, &sig, 1); // check for AK8963 if (ack && sig == AK8963_DEVICE_ID) // 0x48 / 01001000 / 'H' { @@ -458,7 +453,7 @@ bool ak8963Detect(magDev_t *mag) return true; } - ak8963BusDeInit(busdev); + ak8963BusDeInit(dev); return false; } diff --git a/src/main/drivers/compass/compass_ak8975.c b/src/main/drivers/compass/compass_ak8975.c index 9172f7a16..137a63f10 100644 --- a/src/main/drivers/compass/compass_ak8975.c +++ b/src/main/drivers/compass/compass_ak8975.c @@ -82,32 +82,32 @@ static bool ak8975Init(magDev_t *mag) uint8_t asa[3]; uint8_t status; - const busDevice_t *busdev = &mag->busdev; + extDevice_t *dev = &mag->dev; - busDeviceRegister(busdev); + busDeviceRegister(dev); - busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down before entering fuse mode + busWriteRegister(dev, AK8975_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down before entering fuse mode delay(20); - busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_MODE_FUSE_ROM); // Enter Fuse ROM access mode + busWriteRegister(dev, AK8975_MAG_REG_CNTL, CNTL_MODE_FUSE_ROM); // Enter Fuse ROM access mode delay(10); - busReadRegisterBuffer(busdev, AK8975_MAG_REG_ASAX, asa, sizeof(asa)); // Read the x-, y-, and z-axis asa values + busReadRegisterBuffer(dev, AK8975_MAG_REG_ASAX, asa, sizeof(asa)); // Read the x-, y-, and z-axis asa values delay(10); mag->magGain[X] = asa[X] + 128; mag->magGain[Y] = asa[Y] + 128; mag->magGain[Z] = asa[Z] + 128; - busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading. + busWriteRegister(dev, AK8975_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading. delay(10); // Clear status registers - busReadRegisterBuffer(busdev, AK8975_MAG_REG_ST1, &status, 1); - busReadRegisterBuffer(busdev, AK8975_MAG_REG_ST2, &status, 1); + busReadRegisterBuffer(dev, AK8975_MAG_REG_ST1, &status, 1); + busReadRegisterBuffer(dev, AK8975_MAG_REG_ST2, &status, 1); // Trigger first measurement - busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE); + busWriteRegister(dev, AK8975_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE); return true; } @@ -122,21 +122,21 @@ static bool ak8975Read(magDev_t *mag, int16_t *magData) uint8_t status; uint8_t buf[6]; - const busDevice_t *busdev = &mag->busdev; + extDevice_t *dev = &mag->dev; - ack = busReadRegisterBuffer(busdev, AK8975_MAG_REG_ST1, &status, 1); + ack = busReadRegisterBuffer(dev, AK8975_MAG_REG_ST1, &status, 1); if (!ack || (status & ST1_REG_DATA_READY) == 0) { return false; } - busReadRegisterBuffer(busdev, AK8975_MAG_REG_HXL, buf, 6); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH + busReadRegisterBuffer(dev, AK8975_MAG_REG_HXL, buf, 6); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH - ack = busReadRegisterBuffer(busdev, AK8975_MAG_REG_ST2, &status, 1); + ack = busReadRegisterBuffer(dev, AK8975_MAG_REG_ST2, &status, 1); if (!ack) { return false; } - busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE); // start reading again uint8_t status2 = buf[6]; + busWriteRegister(dev, AK8975_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE); // start reading again uint8_t status2 = buf[6]; if (status & ST2_REG_DATA_ERROR) { return false; @@ -157,13 +157,13 @@ bool ak8975Detect(magDev_t *mag) { uint8_t sig = 0; - busDevice_t *busdev = &mag->busdev; + extDevice_t *dev = &mag->dev; - if (busdev->bustype == BUSTYPE_I2C && busdev->busdev_u.i2c.address == 0) { - busdev->busdev_u.i2c.address = AK8975_MAG_I2C_ADDRESS; + if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) { + dev->busType_u.i2c.address = AK8975_MAG_I2C_ADDRESS; } - bool ack = busReadRegisterBuffer(busdev, AK8975_MAG_REG_WIA, &sig, 1); + bool ack = busReadRegisterBuffer(dev, AK8975_MAG_REG_WIA, &sig, 1); if (!ack || sig != AK8975_DEVICE_ID) { // 0x48 / 01001000 / 'H' return false; diff --git a/src/main/drivers/compass/compass_hmc5883l.c b/src/main/drivers/compass/compass_hmc5883l.c index b76222258..ed475de7b 100644 --- a/src/main/drivers/compass/compass_hmc5883l.c +++ b/src/main/drivers/compass/compass_hmc5883l.c @@ -186,20 +186,15 @@ static void hmc5883lConfigureDataReadyInterruptHandling(magDev_t* mag) } #ifdef USE_MAG_SPI_HMC5883 -static void hmc5883SpiInit(busDevice_t *busdev) +static void hmc5883SpiInit(const extDevice_t *dev) { - busDeviceRegister(busdev); + busDeviceRegister(dev); - IOHi(busdev->busdev_u.spi.csnPin); // Disable + IOHi(dev->busType_u.spi.csnPin); // Disable - IOInit(busdev->busdev_u.spi.csnPin, OWNER_COMPASS_CS, 0); - IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP); - -#ifdef USE_SPI_TRANSACTION - spiBusTransactionInit(busdev, SPI_MODE3_POL_HIGH_EDGE_2ND, spiCalculateDivider(HMC5883_MAX_SPI_CLK_HZ)); -#else - spiBusSetDivisor(busdev, spiCalculateDivider(HMC5883_MAX_SPI_CLK_HZ)); -#endif + IOInit(dev->busType_u.spi.csnPin, OWNER_COMPASS_CS, 0); + IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP); + spiSetClkDivisor(dev, spiCalculateDivider(HMC5883_MAX_SPI_CLK_HZ)); } #endif @@ -207,9 +202,9 @@ static bool hmc5883lRead(magDev_t *mag, int16_t *magData) { uint8_t buf[6]; - busDevice_t *busdev = &mag->busdev; + extDevice_t *dev = &mag->dev; - bool ack = busReadRegisterBuffer(busdev, HMC58X3_REG_DATA, buf, 6); + bool ack = busReadRegisterBuffer(dev, HMC58X3_REG_DATA, buf, 6); if (!ack) { return false; @@ -225,12 +220,12 @@ static bool hmc5883lRead(magDev_t *mag, int16_t *magData) static bool hmc5883lInit(magDev_t *mag) { - busDevice_t *busdev = &mag->busdev; + extDevice_t *dev = &mag->dev; // leave test mode - busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_8_SAMLES | HMC_CONFA_DOR_15HZ | HMC_CONFA_NORMAL); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode - busWriteRegister(busdev, HMC58X3_REG_CONFB, HMC_CONFB_GAIN_1_3GA); // Configuration Register B -- 001 00000 configuration gain 1.3Ga - busWriteRegister(busdev, HMC58X3_REG_MODE, HMC_MODE_CONTINOUS); // Mode register -- 000000 00 continuous Conversion Mode + busWriteRegister(dev, HMC58X3_REG_CONFA, HMC_CONFA_8_SAMLES | HMC_CONFA_DOR_15HZ | HMC_CONFA_NORMAL); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode + busWriteRegister(dev, HMC58X3_REG_CONFB, HMC_CONFB_GAIN_1_3GA); // Configuration Register B -- 001 00000 configuration gain 1.3Ga + busWriteRegister(dev, HMC58X3_REG_MODE, HMC_MODE_CONTINOUS); // Mode register -- 000000 00 continuous Conversion Mode delay(100); @@ -240,23 +235,23 @@ static bool hmc5883lInit(magDev_t *mag) bool hmc5883lDetect(magDev_t* mag) { - busDevice_t *busdev = &mag->busdev; + extDevice_t *dev = &mag->dev; uint8_t sig = 0; #ifdef USE_MAG_SPI_HMC5883 - if (busdev->bustype == BUSTYPE_SPI) { - hmc5883SpiInit(&mag->busdev); + if (dev->bus->busType == BUS_TYPE_SPI) { + hmc5883SpiInit(dev); } #endif #ifdef USE_MAG_HMC5883 - if (busdev->bustype == BUSTYPE_I2C && busdev->busdev_u.i2c.address == 0) { - busdev->busdev_u.i2c.address = HMC5883_MAG_I2C_ADDRESS; + if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) { + dev->busType_u.i2c.address = HMC5883_MAG_I2C_ADDRESS; } #endif - bool ack = busReadRegisterBuffer(&mag->busdev, HMC58X3_REG_IDA, &sig, 1); + bool ack = busReadRegisterBuffer(&mag->dev, HMC58X3_REG_IDA, &sig, 1); if (!ack || sig != HMC5883_DEVICE_ID) { return false; diff --git a/src/main/drivers/compass/compass_lis3mdl.c b/src/main/drivers/compass/compass_lis3mdl.c index 2ab961f15..5be0a9457 100644 --- a/src/main/drivers/compass/compass_lis3mdl.c +++ b/src/main/drivers/compass/compass_lis3mdl.c @@ -105,9 +105,9 @@ static bool lis3mdlRead(magDev_t * mag, int16_t *magData) { uint8_t buf[6]; - busDevice_t *busdev = &mag->busdev; + extDevice_t *dev = &mag->dev; - bool ack = busReadRegisterBuffer(busdev, LIS3MDL_REG_OUT_X_L, buf, 6); + bool ack = busReadRegisterBuffer(dev, LIS3MDL_REG_OUT_X_L, buf, 6); if (!ack) { return false; @@ -122,15 +122,15 @@ static bool lis3mdlRead(magDev_t * mag, int16_t *magData) static bool lis3mdlInit(magDev_t *mag) { - busDevice_t *busdev = &mag->busdev; + extDevice_t *dev = &mag->dev; - busDeviceRegister(busdev); + busDeviceRegister(dev); - busWriteRegister(busdev, LIS3MDL_REG_CTRL_REG2, LIS3MDL_FS_4GAUSS); - busWriteRegister(busdev, LIS3MDL_REG_CTRL_REG1, LIS3MDL_TEMP_EN | LIS3MDL_OM_ULTRA_HI_PROF | LIS3MDL_DO_80); - busWriteRegister(busdev, LIS3MDL_REG_CTRL_REG5, LIS3MDL_BDU); - busWriteRegister(busdev, LIS3MDL_REG_CTRL_REG4, LIS3MDL_ZOM_UHP); - busWriteRegister(busdev, LIS3MDL_REG_CTRL_REG3, 0x00); + busWriteRegister(dev, LIS3MDL_REG_CTRL_REG2, LIS3MDL_FS_4GAUSS); + busWriteRegister(dev, LIS3MDL_REG_CTRL_REG1, LIS3MDL_TEMP_EN | LIS3MDL_OM_ULTRA_HI_PROF | LIS3MDL_DO_80); + busWriteRegister(dev, LIS3MDL_REG_CTRL_REG5, LIS3MDL_BDU); + busWriteRegister(dev, LIS3MDL_REG_CTRL_REG4, LIS3MDL_ZOM_UHP); + busWriteRegister(dev, LIS3MDL_REG_CTRL_REG3, 0x00); delay(100); @@ -139,15 +139,15 @@ static bool lis3mdlInit(magDev_t *mag) bool lis3mdlDetect(magDev_t * mag) { - busDevice_t *busdev = &mag->busdev; + extDevice_t *dev = &mag->dev; uint8_t sig = 0; - if (busdev->bustype == BUSTYPE_I2C && busdev->busdev_u.i2c.address == 0) { - busdev->busdev_u.i2c.address = LIS3MDL_MAG_I2C_ADDRESS; + if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) { + dev->busType_u.i2c.address = LIS3MDL_MAG_I2C_ADDRESS; } - bool ack = busReadRegisterBuffer(&mag->busdev, LIS3MDL_REG_WHO_AM_I, &sig, 1); + bool ack = busReadRegisterBuffer(&mag->dev, LIS3MDL_REG_WHO_AM_I, &sig, 1); if (!ack || sig != LIS3MDL_DEVICE_ID) { return false; diff --git a/src/main/drivers/compass/compass_mpu925x_ak8963.c b/src/main/drivers/compass/compass_mpu925x_ak8963.c index 5ef5efdee..d6f2fc22a 100644 --- a/src/main/drivers/compass/compass_mpu925x_ak8963.c +++ b/src/main/drivers/compass/compass_mpu925x_ak8963.c @@ -37,7 +37,7 @@ #define AK8963_MAG_I2C_ADDRESS 0x0C #define MPU9250_BIT_RESET 0x80 -static bool mpu925xDeviceDetect(busDevice_t * dev) +static bool mpu925xDeviceDetect(const extDevice_t * dev) { busWriteRegister(dev, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); delay(150); @@ -52,22 +52,21 @@ static bool mpu925xDeviceDetect(busDevice_t * dev) bool mpu925Xak8963CompassDetect(magDev_t * mag) { - busDevice_t *busdev = &mag->busdev; - busdev->busdev_u.i2c.address = MPU925X_I2C_ADDRESS; - busDeviceRegister(busdev); - if (busdev == NULL || !mpu925xDeviceDetect(busdev)) { + extDevice_t *dev = &mag->dev; + busDeviceRegister(dev); + if (!mpu925xDeviceDetect(dev->bus->busType_u.mpuSlave.master)) { return false; } // set bypass mode on mpu9250 - busWriteRegister(busdev, MPU_RA_INT_PIN_CFG, 0x02); + busWriteRegister(dev->bus->busType_u.mpuSlave.master, MPU_RA_INT_PIN_CFG, 0x02); delay(150); // now we have ak8963 alike on the bus - busdev->busdev_u.i2c.address = AK8963_MAG_I2C_ADDRESS; - busDeviceRegister(busdev); + dev->busType_u.i2c.address = AK8963_MAG_I2C_ADDRESS; + busDeviceRegister(dev); if(!ak8963Detect(mag)) { // if ak8963 is not detected, reset the MPU to disable bypass mode - busdev->busdev_u.i2c.address = MPU925X_I2C_ADDRESS; - busWriteRegister(busdev, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); + dev->busType_u.i2c.address = MPU925X_I2C_ADDRESS; + busWriteRegister(dev, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); return false; } else { return true; diff --git a/src/main/drivers/compass/compass_qmc5883l.c b/src/main/drivers/compass/compass_qmc5883l.c index 3966a993c..73df4bae6 100644 --- a/src/main/drivers/compass/compass_qmc5883l.c +++ b/src/main/drivers/compass/compass_qmc5883l.c @@ -74,15 +74,13 @@ static bool qmc5883lInit(magDev_t *magDev) { - UNUSED(magDev); - bool ack = true; - busDevice_t *busdev = &magDev->busdev; + extDevice_t *dev = &magDev->dev; - busDeviceRegister(busdev); + busDeviceRegister(dev); - ack = ack && busWriteRegister(busdev, 0x0B, 0x01); - ack = ack && busWriteRegister(busdev, QMC5883L_REG_CONF1, QMC5883L_MODE_CONTINUOUS | QMC5883L_ODR_200HZ | QMC5883L_OSR_512 | QMC5883L_RNG_8G); + ack = ack && busWriteRegister(dev, 0x0B, 0x01); + ack = ack && busWriteRegister(dev, QMC5883L_REG_CONF1, QMC5883L_MODE_CONTINUOUS | QMC5883L_ODR_200HZ | QMC5883L_OSR_512 | QMC5883L_RNG_8G); if (!ack) { return false; @@ -101,15 +99,15 @@ static bool qmc5883lRead(magDev_t *magDev, int16_t *magData) magData[Y] = 0; magData[Z] = 0; - busDevice_t *busdev = &magDev->busdev; + extDevice_t *dev = &magDev->dev; - bool ack = busReadRegisterBuffer(busdev, QMC5883L_REG_STATUS, &status, 1); + bool ack = busReadRegisterBuffer(dev, QMC5883L_REG_STATUS, &status, 1); if (!ack || (status & 0x04) == 0) { return false; } - ack = busReadRegisterBuffer(busdev, QMC5883L_REG_DATA_OUTPUT_X, buf, 6); + ack = busReadRegisterBuffer(dev, QMC5883L_REG_DATA_OUTPUT_X, buf, 6); if (!ack) { return false; } @@ -124,22 +122,22 @@ static bool qmc5883lRead(magDev_t *magDev, int16_t *magData) bool qmc5883lDetect(magDev_t *magDev) { - busDevice_t *busdev = &magDev->busdev; + extDevice_t *dev = &magDev->dev; - if (busdev->bustype == BUSTYPE_I2C && busdev->busdev_u.i2c.address == 0) { - busdev->busdev_u.i2c.address = QMC5883L_MAG_I2C_ADDRESS; + if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) { + dev->busType_u.i2c.address = QMC5883L_MAG_I2C_ADDRESS; } // Must write reset first - don't care about the result - busWriteRegister(busdev, QMC5883L_REG_CONF2, QMC5883L_RST); + busWriteRegister(dev, QMC5883L_REG_CONF2, QMC5883L_RST); delay(20); uint8_t sig = 0; - bool ack = busReadRegisterBuffer(busdev, QMC5883L_REG_ID, &sig, 1); + bool ack = busReadRegisterBuffer(dev, QMC5883L_REG_ID, &sig, 1); if (ack && sig == QMC5883_ID_VAL) { // Should be in standby mode after soft reset and sensor is really present // Reading ChipID of 0xFF alone is not sufficient to be sure the QMC is present - ack = busReadRegisterBuffer(busdev, QMC5883L_REG_CONF1, &sig, 1); + ack = busReadRegisterBuffer(dev, QMC5883L_REG_CONF1, &sig, 1); if (ack && sig != QMC5883L_MODE_STANDBY) { return false; } diff --git a/src/main/drivers/display_ug2864hsweg01.c b/src/main/drivers/display_ug2864hsweg01.c index 08729065f..81fc0dbe2 100644 --- a/src/main/drivers/display_ug2864hsweg01.c +++ b/src/main/drivers/display_ug2864hsweg01.c @@ -173,15 +173,15 @@ static const uint8_t multiWiiFont[][5] = { // Refer to "Times New Roman" Font Da { 0x7A, 0x7E, 0x7E, 0x7E, 0x7A }, // (131) - 0x00C8 Vertical Bargraph - 6 (full) }; -static bool i2c_OLED_send_cmd(busDevice_t *bus, uint8_t command) +static bool i2c_OLED_send_cmd(const extDevice_t *dev, uint8_t command) { - return i2cWrite(bus->busdev_u.i2c.device, bus->busdev_u.i2c.address, 0x80, command); + return i2cWrite(dev->bus->busType_u.i2c.device, dev->busType_u.i2c.address, 0x80, command); } -static bool i2c_OLED_send_cmdarray(busDevice_t *bus, const uint8_t *commands, size_t len) +static bool i2c_OLED_send_cmdarray(const extDevice_t *dev, const uint8_t *commands, size_t len) { for (size_t i = 0 ; i < len ; i++) { - if (!i2c_OLED_send_cmd(bus, commands[i])) { + if (!i2c_OLED_send_cmd(dev, commands[i])) { return false; } } @@ -189,12 +189,12 @@ static bool i2c_OLED_send_cmdarray(busDevice_t *bus, const uint8_t *commands, si return true; } -static bool i2c_OLED_send_byte(busDevice_t *bus, uint8_t val) +static bool i2c_OLED_send_byte(const extDevice_t *dev, uint8_t val) { - return i2cWrite(bus->busdev_u.i2c.device, bus->busdev_u.i2c.address, 0x40, val); + return i2cWrite(dev->bus->busType_u.i2c.device, dev->busType_u.i2c.address, 0x40, val); } -void i2c_OLED_clear_display_quick(busDevice_t *bus) +void i2c_OLED_clear_display_quick(const extDevice_t *dev) { static const uint8_t i2c_OLED_cmd_clear_display_quick[] = { 0xb0, // set page address to 0 @@ -203,14 +203,14 @@ void i2c_OLED_clear_display_quick(busDevice_t *bus) 0x10, // Set high col address to 0 }; - i2c_OLED_send_cmdarray(bus, i2c_OLED_cmd_clear_display_quick, ARRAYLEN(i2c_OLED_cmd_clear_display_quick)); + i2c_OLED_send_cmdarray(dev, i2c_OLED_cmd_clear_display_quick, ARRAYLEN(i2c_OLED_cmd_clear_display_quick)); for (uint16_t i = 0; i < 1024; i++) { // fill the display's RAM with graphic... 128*64 pixel picture - i2c_OLED_send_byte(bus, 0x00); // clear + i2c_OLED_send_byte(dev, 0x00); // clear } } -void i2c_OLED_clear_display(busDevice_t *bus) +void i2c_OLED_clear_display(const extDevice_t *dev) { static const uint8_t i2c_OLED_cmd_clear_display_pre[] = { 0xa6, // Set Normal Display @@ -219,9 +219,9 @@ void i2c_OLED_clear_display(busDevice_t *bus) 0x00, // Set Memory Addressing Mode to Horizontal addressing mode }; - i2c_OLED_send_cmdarray(bus, i2c_OLED_cmd_clear_display_pre, ARRAYLEN(i2c_OLED_cmd_clear_display_pre)); + i2c_OLED_send_cmdarray(dev, i2c_OLED_cmd_clear_display_pre, ARRAYLEN(i2c_OLED_cmd_clear_display_pre)); - i2c_OLED_clear_display_quick(bus); + i2c_OLED_clear_display_quick(dev); static const uint8_t i2c_OLED_cmd_clear_display_post[] = { 0x81, // Setup CONTRAST CONTROL, following byte is the contrast Value... always a 2 byte instruction @@ -229,10 +229,10 @@ void i2c_OLED_clear_display(busDevice_t *bus) 0xaf, // display on }; - i2c_OLED_send_cmdarray(bus, i2c_OLED_cmd_clear_display_post, ARRAYLEN(i2c_OLED_cmd_clear_display_post)); + i2c_OLED_send_cmdarray(dev, i2c_OLED_cmd_clear_display_post, ARRAYLEN(i2c_OLED_cmd_clear_display_post)); } -void i2c_OLED_set_xy(busDevice_t *bus, uint8_t col, uint8_t row) +void i2c_OLED_set_xy(const extDevice_t *dev, uint8_t col, uint8_t row) { uint8_t i2c_OLED_cmd_set_xy[] = { 0xb0 + row, //set page address @@ -240,31 +240,31 @@ void i2c_OLED_set_xy(busDevice_t *bus, uint8_t col, uint8_t row) 0x10 + (((CHARACTER_WIDTH_TOTAL * col) >> 4) & 0x0f) //set high col address }; - i2c_OLED_send_cmdarray(bus, i2c_OLED_cmd_set_xy, ARRAYLEN(i2c_OLED_cmd_set_xy)); + i2c_OLED_send_cmdarray(dev, i2c_OLED_cmd_set_xy, ARRAYLEN(i2c_OLED_cmd_set_xy)); } -void i2c_OLED_set_line(busDevice_t *bus, uint8_t row) +void i2c_OLED_set_line(const extDevice_t *dev, uint8_t row) { - i2c_OLED_set_xy(bus, 0, row); + i2c_OLED_set_xy(dev, 0, row); } -void i2c_OLED_send_char(busDevice_t *bus, unsigned char ascii) +void i2c_OLED_send_char(const extDevice_t *dev, unsigned char ascii) { unsigned char i; uint8_t buffer; for (i = 0; i < 5; i++) { buffer = multiWiiFont[ascii - 32][i]; buffer ^= CHAR_FORMAT; // apply - i2c_OLED_send_byte(bus, buffer); + i2c_OLED_send_byte(dev, buffer); } - i2c_OLED_send_byte(bus, CHAR_FORMAT); // the gap + i2c_OLED_send_byte(dev, CHAR_FORMAT); // the gap } -void i2c_OLED_send_string(busDevice_t *bus, const char *string) +void i2c_OLED_send_string(const extDevice_t *dev, const char *string) { // Sends a string of chars until null terminator while (*string) { - i2c_OLED_send_char(bus, *string); + i2c_OLED_send_char(dev, *string); string++; } } @@ -273,11 +273,11 @@ void i2c_OLED_send_string(busDevice_t *bus, const char *string) * according to http://www.adafruit.com/datasheets/UG-2864HSWEG01.pdf Chapter 4.4 Page 15 */ -bool ug2864hsweg01InitI2C(busDevice_t *bus) +bool ug2864hsweg01InitI2C(const extDevice_t *dev) { // Set display OFF - if (!i2c_OLED_send_cmd(bus, 0xAE)) { + if (!i2c_OLED_send_cmd(dev, 0xAE)) { return false; } @@ -306,9 +306,9 @@ bool ug2864hsweg01InitI2C(busDevice_t *bus) 0xAF, // Set display On }; - i2c_OLED_send_cmdarray(bus, i2c_OLED_cmd_init, ARRAYLEN(i2c_OLED_cmd_init)); + i2c_OLED_send_cmdarray(dev, i2c_OLED_cmd_init, ARRAYLEN(i2c_OLED_cmd_init)); - i2c_OLED_clear_display(bus); + i2c_OLED_clear_display(dev); return true; } diff --git a/src/main/drivers/display_ug2864hsweg01.h b/src/main/drivers/display_ug2864hsweg01.h index 81aba7274..c146c44e3 100644 --- a/src/main/drivers/display_ug2864hsweg01.h +++ b/src/main/drivers/display_ug2864hsweg01.h @@ -39,11 +39,11 @@ #define VERTICAL_BARGRAPH_ZERO_CHARACTER (128 + 32) #define VERTICAL_BARGRAPH_CHARACTER_COUNT 7 -bool ug2864hsweg01InitI2C(busDevice_t *bus); +bool ug2864hsweg01InitI2C(const extDevice_t *dev); -void i2c_OLED_set_xy(busDevice_t *bus, uint8_t col, uint8_t row); -void i2c_OLED_set_line(busDevice_t *bus, uint8_t row); -void i2c_OLED_send_char(busDevice_t *bus, unsigned char ascii); -void i2c_OLED_send_string(busDevice_t *bus, const char *string); -void i2c_OLED_clear_display(busDevice_t *bus); -void i2c_OLED_clear_display_quick(busDevice_t *bus); +void i2c_OLED_set_xy(const extDevice_t *dev, uint8_t col, uint8_t row); +void i2c_OLED_set_line(const extDevice_t *dev, uint8_t row); +void i2c_OLED_send_char(const extDevice_t *dev, unsigned char ascii); +void i2c_OLED_send_string(const extDevice_t *dev, const char *string); +void i2c_OLED_clear_display(const extDevice_t *dev); +void i2c_OLED_clear_display_quick(const extDevice_t *dev); diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h index 1cf31dc2b..97ff5bc9c 100644 --- a/src/main/drivers/dma.h +++ b/src/main/drivers/dma.h @@ -22,6 +22,9 @@ #include "drivers/resource.h" +#define CACHE_LINE_SIZE 32 +#define CACHE_LINE_MASK (CACHE_LINE_SIZE - 1) + // dmaResource_t is a opaque data type which represents a single DMA engine, // called and implemented differently in different families of STM32s. // The opaque data type provides uniform handling of the engine in source code. diff --git a/src/main/drivers/dshot_bitbang.c b/src/main/drivers/dshot_bitbang.c index a7fd38bf6..24009a36d 100644 --- a/src/main/drivers/dshot_bitbang.c +++ b/src/main/drivers/dshot_bitbang.c @@ -70,8 +70,8 @@ dshotBitbangStatus_e bbStatus; // as the buffer region is attributed as not cachable. // If this is not desirable, we should use manual cache invalidation. #ifdef USE_DSHOT_CACHE_MGMT -#define BB_OUTPUT_BUFFER_ATTRIBUTE __attribute__((aligned(32))) -#define BB_INPUT_BUFFER_ATTRIBUTE __attribute__((aligned(32))) +#define BB_OUTPUT_BUFFER_ATTRIBUTE DMA_RW_AXI __attribute__((aligned(32))) +#define BB_INPUT_BUFFER_ATTRIBUTE DMA_RW_AXI __attribute__((aligned(32))) #else #if defined(STM32F4) #define BB_OUTPUT_BUFFER_ATTRIBUTE diff --git a/src/main/drivers/flash.c b/src/main/drivers/flash.c index 5be09aa87..a23bf1872 100644 --- a/src/main/drivers/flash.c +++ b/src/main/drivers/flash.c @@ -44,8 +44,8 @@ // 5 MHz max SPI init frequency #define FLASH_MAX_SPI_INIT_CLK 5000000 -static busDevice_t busInstance; -static busDevice_t *busdev; +static extDevice_t devInstance; +static extDevice_t *dev; static flashDevice_t flashDevice; static flashPartitionTable_t flashPartitionTable; @@ -135,44 +135,34 @@ void flashPreInit(const flashConfig_t *flashConfig) static bool flashSpiInit(const flashConfig_t *flashConfig) { // Read chip identification and send it to device detect - - busdev = &busInstance; + dev = &devInstance; if (flashConfig->csTag) { - busdev->busdev_u.spi.csnPin = IOGetByTag(flashConfig->csTag); + dev->busType_u.spi.csnPin = IOGetByTag(flashConfig->csTag); } else { return false; } - if (!IOIsFreeOrPreinit(busdev->busdev_u.spi.csnPin)) { + if (!IOIsFreeOrPreinit(dev->busType_u.spi.csnPin)) { return false; } - busdev->bustype = BUSTYPE_SPI; - - SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(flashConfig->spiDevice)); - if (!instance) { + if (!spiSetBusInstance(dev, flashConfig->spiDevice, OWNER_FLASH_CS)) { return false; } - spiBusSetInstance(busdev, instance); + // Set the callback argument when calling back to this driver for DMA completion + dev->callbackArg = (uint32_t)&flashDevice; - IOInit(busdev->busdev_u.spi.csnPin, OWNER_FLASH_CS, 0); - IOConfigGPIO(busdev->busdev_u.spi.csnPin, SPI_IO_CS_CFG); - IOHi(busdev->busdev_u.spi.csnPin); + IOInit(dev->busType_u.spi.csnPin, OWNER_FLASH_CS, 0); + IOConfigGPIO(dev->busType_u.spi.csnPin, SPI_IO_CS_CFG); + IOHi(dev->busType_u.spi.csnPin); -#ifdef USE_SPI_TRANSACTION - spiBusTransactionInit(busdev, SPI_MODE3_POL_HIGH_EDGE_2ND, spiCalculateDivider(FLASH_MAX_SPI_INIT_CLK)); -#else -#ifndef FLASH_SPI_SHARED - spiSetDivisor(busdev->busdev_u.spi.instance, spiCalculateDivider(FLASH_MAX_SPI_INIT_CLK)); -#endif -#endif + //Maximum speed for standard READ command is 20mHz, other commands tolerate 25mHz + spiSetClkDivisor(dev, spiCalculateDivider(FLASH_MAX_SPI_INIT_CLK)); flashDevice.io.mode = FLASHIO_SPI; - flashDevice.io.handle.busdev = busdev; - - const uint8_t out[] = { FLASH_INSTRUCTION_RDID, 0, 0, 0, 0 }; + flashDevice.io.handle.dev = dev; delay(50); // short delay required after initialisation of SPI device instance. @@ -180,18 +170,12 @@ static bool flashSpiInit(const flashConfig_t *flashConfig) * Some newer chips require one dummy byte to be read; we can read * 4 bytes for these chips while retaining backward compatibility. */ - uint8_t readIdResponse[5]; - readIdResponse[1] = readIdResponse[2] = 0; + uint8_t readIdResponse[4] = { 0 }; - // Clearing the CS bit terminates the command early so we don't have to read the chip UID: -#ifdef USE_SPI_TRANSACTION - spiBusTransactionTransfer(busdev, out, readIdResponse, sizeof(out)); -#else - spiBusTransfer(busdev, out, readIdResponse, sizeof(out)); -#endif + spiReadRegBuf(dev, FLASH_INSTRUCTION_RDID, readIdResponse, sizeof (readIdResponse)); // Manufacturer, memory type, and capacity - uint32_t chipID = (readIdResponse[1] << 16) | (readIdResponse[2] << 8) | (readIdResponse[3]); + uint32_t chipID = (readIdResponse[0] << 16) | (readIdResponse[1] << 8) | (readIdResponse[2]); #ifdef USE_FLASH_M25P16 if (m25p16_detect(&flashDevice, chipID)) { @@ -258,36 +242,43 @@ bool flashWaitForReady(void) void flashEraseSector(uint32_t address) { + flashDevice.callback = NULL; flashDevice.vTable->eraseSector(&flashDevice, address); } void flashEraseCompletely(void) { + flashDevice.callback = NULL; flashDevice.vTable->eraseCompletely(&flashDevice); } void flashPageProgramBegin(uint32_t address) { + flashDevice.callback = NULL; flashDevice.vTable->pageProgramBegin(&flashDevice, address); } void flashPageProgramContinue(const uint8_t *data, int length) { + flashDevice.callback = NULL; flashDevice.vTable->pageProgramContinue(&flashDevice, data, length); } void flashPageProgramFinish(void) { + flashDevice.callback = NULL; flashDevice.vTable->pageProgramFinish(&flashDevice); } void flashPageProgram(uint32_t address, const uint8_t *data, int length) { + flashDevice.callback = NULL; flashDevice.vTable->pageProgram(&flashDevice, address, data, length); } int flashReadBytes(uint32_t address, uint8_t *buffer, int length) { + flashDevice.callback = NULL; return flashDevice.vTable->readBytes(&flashDevice, address, buffer, length); } diff --git a/src/main/drivers/flash_impl.h b/src/main/drivers/flash_impl.h index 691765f69..4d0aa1e30 100644 --- a/src/main/drivers/flash_impl.h +++ b/src/main/drivers/flash_impl.h @@ -36,7 +36,7 @@ typedef enum { typedef struct flashDeviceIO_s { union { - busDevice_t *busdev; // Device interface dependent handle (spi/i2c) + extDevice_t *dev; // Device interface dependent handle (spi/i2c) #ifdef USE_QUADSPI QUADSPI_TypeDef *quadSpi; #endif @@ -55,6 +55,8 @@ typedef struct flashDevice_s { bool couldBeBusy; uint32_t timeoutAt; flashDeviceIO_t io; + void (*callback)(uint32_t arg); + uint32_t callbackArg; } flashDevice_t; typedef struct flashVTable_s { diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index cef1690cd..75341eaac 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -113,106 +113,46 @@ struct { { 0x000000, 0, 0, 0, 0 } }; -// IMPORTANT: Timeout values are currently required to be set to the highest value required by any of the supported flash chips by this driver. - -// The timeout we expect between being able to issue page program instructions -#define DEFAULT_TIMEOUT_MILLIS 6 -#define SECTOR_ERASE_TIMEOUT_MILLIS 5000 - -// etracer65 notes: For bulk erase The 25Q16 takes about 3 seconds and the 25Q128 takes about 49 -#define BULK_ERASE_TIMEOUT_MILLIS 50000 - #define M25P16_PAGESIZE 256 STATIC_ASSERT(M25P16_PAGESIZE < FLASH_MAX_PAGE_SIZE, M25P16_PAGESIZE_too_small); const flashVTable_t m25p16_vTable; -#ifndef USE_SPI_TRANSACTION -static void m25p16_disable(busDevice_t *bus) + +static uint8_t m25p16_readStatus(flashDevice_t *fdevice) { - IOHi(bus->busdev_u.spi.csnPin); - __NOP(); + STATIC_DMA_DATA_AUTO uint8_t readStatus[2] = { M25P16_INSTRUCTION_READ_STATUS_REG, 0 }; + STATIC_DMA_DATA_AUTO uint8_t readyStatus[2]; + + spiReadWriteBuf(fdevice->io.handle.dev, readStatus, readyStatus, sizeof (readStatus)); + + return readyStatus[1]; } -static void m25p16_enable(busDevice_t *bus) -{ - __NOP(); - IOLo(bus->busdev_u.spi.csnPin); -} -#endif - -static void m25p16_transfer(busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int len) -{ -#ifdef USE_SPI_TRANSACTION - spiBusTransactionTransfer(bus, txData, rxData, len); -#else - m25p16_enable(bus); - spiTransfer(bus->busdev_u.spi.instance, txData, rxData, len); - m25p16_disable(bus); -#endif -} - -/** - * Send the given command byte to the device. - */ -static void m25p16_performOneByteCommand(busDevice_t *bus, uint8_t command) -{ -#ifdef USE_SPI_TRANSACTION - m25p16_transfer(bus, &command, NULL, 1); -#else - m25p16_enable(bus); - spiTransferByte(bus->busdev_u.spi.instance, command); - m25p16_disable(bus); -#endif -} - -/** - * The flash requires this write enable command to be sent before commands that would cause - * a write like program and erase. - */ -static void m25p16_writeEnable(flashDevice_t *fdevice) -{ - m25p16_performOneByteCommand(fdevice->io.handle.busdev, M25P16_INSTRUCTION_WRITE_ENABLE); - - // Assume that we're about to do some writing, so the device is just about to become busy - fdevice->couldBeBusy = true; -} - -static uint8_t m25p16_readStatus(busDevice_t *bus) -{ - const uint8_t command[2] = { M25P16_INSTRUCTION_READ_STATUS_REG, 0 }; - uint8_t in[2]; - - m25p16_transfer(bus, command, in, sizeof(command)); - - return in[1]; -} static bool m25p16_isReady(flashDevice_t *fdevice) { + // If we're waiting on DMA completion, then SPI is busy + if (fdevice->io.handle.dev->bus->useDMA && spiIsBusy(fdevice->io.handle.dev)) { + return false; + } + // If couldBeBusy is false, don't bother to poll the flash chip for its status - fdevice->couldBeBusy = fdevice->couldBeBusy && ((m25p16_readStatus(fdevice->io.handle.busdev) & M25P16_STATUS_FLAG_WRITE_IN_PROGRESS) != 0); + if (!fdevice->couldBeBusy) { + return true; + } + + // Poll the FLASH device to see if it's busy + fdevice->couldBeBusy = ((m25p16_readStatus(fdevice) & M25P16_STATUS_FLAG_WRITE_IN_PROGRESS) != 0); return !fdevice->couldBeBusy; } -static void m25p16_setTimeout(flashDevice_t *fdevice, uint32_t timeoutMillis) -{ - uint32_t now = millis(); - fdevice->timeoutAt = now + timeoutMillis; -} - static bool m25p16_waitForReady(flashDevice_t *fdevice) { - while (!m25p16_isReady(fdevice)) { - uint32_t now = millis(); - if (cmp32(now, fdevice->timeoutAt) >= 0) { - return false; - } - } + while (!m25p16_isReady(fdevice)); - fdevice->timeoutAt = 0; return true; } @@ -252,13 +192,15 @@ bool m25p16_detect(flashDevice_t *fdevice, uint32_t chipID) geometry->totalSize = geometry->sectorSize * geometry->sectors; // Adjust the SPI bus clock frequency -#ifndef FLASH_SPI_SHARED - spiSetDivisor(fdevice->io.handle.busdev->busdev_u.spi.instance, spiCalculateDivider(maxReadClkSPIHz)); -#endif + spiSetClkDivisor(fdevice->io.handle.dev, spiCalculateDivider(maxReadClkSPIHz)); - if (fdevice->geometry.totalSize > 16 * 1024 * 1024) { + if (geometry->totalSize > 16 * 1024 * 1024) { fdevice->isLargeFlash = true; - m25p16_performOneByteCommand(fdevice->io.handle.busdev, W25Q256_INSTRUCTION_ENTER_4BYTE_ADDRESS_MODE); + + // This routine blocks so no need to use static data + uint8_t modeSet[] = { W25Q256_INSTRUCTION_ENTER_4BYTE_ADDRESS_MODE }; + + spiReadWriteBuf(fdevice->io.handle.dev, modeSet, NULL, sizeof (modeSet)); } fdevice->couldBeBusy = true; // Just for luck we'll assume the chip could be busy even though it isn't specced to be @@ -276,70 +218,130 @@ static void m25p16_setCommandAddress(uint8_t *buf, uint32_t address, bool useLon *buf = address & 0xff; } + +// Called in ISR context +// A write enable has just been issued +busStatus_e m25p16_callbackWriteEnable(uint32_t arg) +{ + flashDevice_t *fdevice = (flashDevice_t *)arg; + + // As a write has just occurred, the device could be busy + fdevice->couldBeBusy = true; + + return BUS_READY; +} + +// Called in ISR context +// Check if the status was busy and if so repeat the poll +busStatus_e m25p16_callbackReady(uint32_t arg) +{ + flashDevice_t *fdevice = (flashDevice_t *)arg; + extDevice_t *dev = fdevice->io.handle.dev; + + uint8_t readyPoll = dev->bus->curSegment->rxData[1]; + + if (readyPoll & M25P16_STATUS_FLAG_WRITE_IN_PROGRESS) { + return BUS_BUSY; + } + + // Bus is now known not to be busy + fdevice->couldBeBusy = false; + + return BUS_READY; +} + + /** * Erase a sector full of bytes to all 1's at the given byte offset in the flash chip. */ static void m25p16_eraseSector(flashDevice_t *fdevice, uint32_t address) { - uint8_t out[5] = { M25P16_INSTRUCTION_SECTOR_ERASE }; + STATIC_DMA_DATA_AUTO uint8_t sectorErase[5] = { M25P16_INSTRUCTION_SECTOR_ERASE }; + STATIC_DMA_DATA_AUTO uint8_t readStatus[2] = { M25P16_INSTRUCTION_READ_STATUS_REG, 0 }; + STATIC_DMA_DATA_AUTO uint8_t readyStatus[2]; + STATIC_DMA_DATA_AUTO uint8_t writeEnable[] = { M25P16_INSTRUCTION_WRITE_ENABLE }; + busSegment_t segments[] = { + {readStatus, readyStatus, sizeof (readStatus), true, m25p16_callbackReady}, + {writeEnable, NULL, sizeof (writeEnable), true, m25p16_callbackWriteEnable}, + {sectorErase, NULL, fdevice->isLargeFlash ? 5 : 4, true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; - m25p16_setCommandAddress(&out[1], address, fdevice->isLargeFlash); + // Ensure any prior DMA has completed before continuing + spiWaitClaim(fdevice->io.handle.dev); - m25p16_waitForReady(fdevice); + m25p16_setCommandAddress(§orErase[1], address, fdevice->isLargeFlash); - m25p16_writeEnable(fdevice); + spiSequence(fdevice->io.handle.dev, &segments[0]); - m25p16_transfer(fdevice->io.handle.busdev, out, NULL, fdevice->isLargeFlash ? 5 : 4); - - m25p16_setTimeout(fdevice, SECTOR_ERASE_TIMEOUT_MILLIS); + // Block pending completion of SPI access, but the erase will be ongoing + spiWait(fdevice->io.handle.dev); } static void m25p16_eraseCompletely(flashDevice_t *fdevice) { - m25p16_waitForReady(fdevice); + STATIC_DMA_DATA_AUTO uint8_t readStatus[2] = { M25P16_INSTRUCTION_READ_STATUS_REG, 0 }; + STATIC_DMA_DATA_AUTO uint8_t readyStatus[2]; + STATIC_DMA_DATA_AUTO uint8_t writeEnable[] = { M25P16_INSTRUCTION_WRITE_ENABLE }; + STATIC_DMA_DATA_AUTO uint8_t bulkErase[] = { M25P16_INSTRUCTION_BULK_ERASE }; + busSegment_t segments[] = { + {readStatus, readyStatus, sizeof (readStatus), true, m25p16_callbackReady}, + {writeEnable, NULL, sizeof (writeEnable), true, m25p16_callbackWriteEnable}, + {bulkErase, NULL, sizeof (bulkErase), true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; - m25p16_writeEnable(fdevice); + // Ensure any prior DMA has completed before continuing + spiWaitClaim(fdevice->io.handle.dev); - m25p16_performOneByteCommand(fdevice->io.handle.busdev, M25P16_INSTRUCTION_BULK_ERASE); + spiSequence(fdevice->io.handle.dev, &segments[0]); - m25p16_setTimeout(fdevice, BULK_ERASE_TIMEOUT_MILLIS); + // Block pending completion of SPI access, but the erase will be ongoing + spiWait(fdevice->io.handle.dev); } + static void m25p16_pageProgramBegin(flashDevice_t *fdevice, uint32_t address) { - UNUSED(fdevice); - fdevice->currentWriteAddress = address; } + static void m25p16_pageProgramContinue(flashDevice_t *fdevice, const uint8_t *data, int length) { - uint8_t command[5] = { M25P16_INSTRUCTION_PAGE_PROGRAM }; + // The segment list cannot be in automatic storage as this routine is non-blocking + STATIC_DMA_DATA_AUTO uint8_t readStatus[2] = { M25P16_INSTRUCTION_READ_STATUS_REG, 0 }; + STATIC_DMA_DATA_AUTO uint8_t readyStatus[2]; + STATIC_DMA_DATA_AUTO uint8_t writeEnable[] = { M25P16_INSTRUCTION_WRITE_ENABLE }; + STATIC_DMA_DATA_AUTO uint8_t pageProgram[5] = { M25P16_INSTRUCTION_PAGE_PROGRAM }; - m25p16_setCommandAddress(&command[1], fdevice->currentWriteAddress, fdevice->isLargeFlash); + static busSegment_t segments[] = { + {readStatus, readyStatus, sizeof (readStatus), true, m25p16_callbackReady}, + {writeEnable, NULL, sizeof (writeEnable), true, m25p16_callbackWriteEnable}, + {pageProgram, NULL, 0, false, NULL}, + {NULL, NULL, 0, true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; - m25p16_waitForReady(fdevice); + // Ensure any prior DMA has completed before continuing + spiWaitClaim(fdevice->io.handle.dev); - m25p16_writeEnable(fdevice); + // Patch the pageProgram segment + segments[2].len = fdevice->isLargeFlash ? 5 : 4; + m25p16_setCommandAddress(&pageProgram[1], fdevice->currentWriteAddress, fdevice->isLargeFlash); -#ifdef USE_SPI_TRANSACTION - spiBusTransactionBegin(fdevice->io.handle.busdev); -#else - m25p16_enable(fdevice->io.handle.busdev); -#endif + // Patch the data segment + segments[3].txData = (uint8_t *)data; + segments[3].len = length; - spiTransfer(fdevice->io.handle.busdev->busdev_u.spi.instance, command, NULL, fdevice->isLargeFlash ? 5 : 4); - spiTransfer(fdevice->io.handle.busdev->busdev_u.spi.instance, data, NULL, length); + spiSequence(fdevice->io.handle.dev, fdevice->couldBeBusy ? &segments[0] : &segments[1]); -#ifdef USE_SPI_TRANSACTION - spiBusTransactionEnd(fdevice->io.handle.busdev); -#else - m25p16_disable(fdevice->io.handle.busdev); -#endif + if (fdevice->callback == NULL) { + // No callback was provided so block + spiWait(fdevice->io.handle.dev); + } fdevice->currentWriteAddress += length; - - m25p16_setTimeout(fdevice, DEFAULT_TIMEOUT_MILLIS); } static void m25p16_pageProgramFinish(flashDevice_t *fdevice) @@ -379,38 +381,31 @@ static void m25p16_pageProgram(flashDevice_t *fdevice, uint32_t address, const u */ static int m25p16_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer, int length) { - uint8_t command[5] = { M25P16_INSTRUCTION_READ_BYTES }; + STATIC_DMA_DATA_AUTO uint8_t readStatus[2] = { M25P16_INSTRUCTION_READ_STATUS_REG, 0 }; + STATIC_DMA_DATA_AUTO uint8_t readyStatus[2]; + STATIC_DMA_DATA_AUTO uint8_t readBytes[5] = { M25P16_INSTRUCTION_READ_BYTES }; - m25p16_setCommandAddress(&command[1], address, fdevice->isLargeFlash); + // Ensure any prior DMA has completed before continuing + spiWaitClaim(fdevice->io.handle.dev); - if (!m25p16_waitForReady(fdevice)) { - return 0; - } + busSegment_t segments[] = { + {readStatus, readyStatus, sizeof (readStatus), true, m25p16_callbackReady}, + {readBytes, NULL, fdevice->isLargeFlash ? 5 : 4, false, NULL}, + {NULL, buffer, length, true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; -#ifndef FLASH_SPI_SHARED - spiSetDivisor(fdevice->io.handle.busdev->busdev_u.spi.instance, spiCalculateDivider(maxReadClkSPIHz)); -#endif + // Patch the readBytes command + m25p16_setCommandAddress(&readBytes[1], address, fdevice->isLargeFlash); -#ifdef USE_SPI_TRANSACTION - spiBusTransactionBegin(fdevice->io.handle.busdev); -#else - m25p16_enable(fdevice->io.handle.busdev); -#endif + spiSetClkDivisor(fdevice->io.handle.dev, spiCalculateDivider(maxReadClkSPIHz)); - spiTransfer(fdevice->io.handle.busdev->busdev_u.spi.instance, command, NULL, fdevice->isLargeFlash ? 5 : 4); - spiTransfer(fdevice->io.handle.busdev->busdev_u.spi.instance, NULL, buffer, length); + spiSequence(fdevice->io.handle.dev, fdevice->couldBeBusy ? &segments[0] : &segments[1]); -#ifdef USE_SPI_TRANSACTION - spiBusTransactionEnd(fdevice->io.handle.busdev); -#else - m25p16_disable(fdevice->io.handle.busdev); -#endif + // Block until code is re-factored to exploit non-blocking + spiWait(fdevice->io.handle.dev); -#ifndef FLASH_SPI_SHARED - spiSetDivisor(fdevice->io.handle.busdev->busdev_u.spi.instance, spiCalculateDivider(maxClkSPIHz)); -#endif - - m25p16_setTimeout(fdevice, DEFAULT_TIMEOUT_MILLIS); + spiSetClkDivisor(fdevice->io.handle.dev, spiCalculateDivider(maxClkSPIHz)); return length; } diff --git a/src/main/drivers/flash_w25m.c b/src/main/drivers/flash_w25m.c index 473413b46..0172d3d9d 100644 --- a/src/main/drivers/flash_w25m.c +++ b/src/main/drivers/flash_w25m.c @@ -62,7 +62,7 @@ static flashDevice_t dieDevice[MAX_DIE_COUNT]; static int dieCount; static uint32_t dieSize; -static void w25m_dieSelect(busDevice_t *busdev, int die) +static void w25m_dieSelect(const extDevice_t *dev, int die) { static int activeDie = -1; @@ -72,11 +72,18 @@ static void w25m_dieSelect(busDevice_t *busdev, int die) uint8_t command[2] = { W25M_INSTRUCTION_SOFTWARE_DIE_SELECT, die }; -#ifdef SPI_BUS_TRANSACTION - spiBusTransactionTransfer(busdev, command, NULL, 2); -#else - spiBusTransfer(busdev, command, NULL, 2); -#endif + busSegment_t segments[] = { + {command, NULL, sizeof (command), true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + + // Ensure any prior DMA has completed before continuing + spiWait(dev); + + spiSequence(dev, &segments[0]); + + // Block pending completion of SPI access, but the erase will be ongoing + spiWait(dev); activeDie = die; } @@ -85,7 +92,7 @@ static bool w25m_isReady(flashDevice_t *fdevice) { for (int die = 0 ; die < dieCount ; die++) { if (dieDevice[die].couldBeBusy) { - w25m_dieSelect(fdevice->io.handle.busdev, die); + w25m_dieSelect(fdevice->io.handle.dev, die); if (!dieDevice[die].vTable->isReady(&dieDevice[die])) { return false; } @@ -98,7 +105,7 @@ static bool w25m_isReady(flashDevice_t *fdevice) static bool w25m_waitForReady(flashDevice_t *fdevice) { for (int die = 0 ; die < dieCount ; die++) { - w25m_dieSelect(fdevice->io.handle.busdev, die); + w25m_dieSelect(fdevice->io.handle.dev, die); if (!dieDevice[die].vTable->waitForReady(&dieDevice[die])) { return false; } @@ -117,8 +124,8 @@ bool w25m_detect(flashDevice_t *fdevice, uint32_t chipID) dieCount = 2; for (int die = 0 ; die < dieCount ; die++) { - w25m_dieSelect(fdevice->io.handle.busdev, die); - dieDevice[die].io.handle.busdev = fdevice->io.handle.busdev; + w25m_dieSelect(fdevice->io.handle.dev, die); + dieDevice[die].io.handle.dev = fdevice->io.handle.dev; dieDevice[die].io.mode = fdevice->io.mode; m25p16_detect(&dieDevice[die], JEDEC_ID_WINBOND_W25Q256); } @@ -133,8 +140,8 @@ bool w25m_detect(flashDevice_t *fdevice, uint32_t chipID) dieCount = 2; for (int die = 0 ; die < dieCount ; die++) { - w25m_dieSelect(fdevice->io.handle.busdev, die); - dieDevice[die].io.handle.busdev = fdevice->io.handle.busdev; + w25m_dieSelect(fdevice->io.handle.dev, die); + dieDevice[die].io.handle.dev = fdevice->io.handle.dev; dieDevice[die].io.mode = fdevice->io.mode; w25n01g_detect(&dieDevice[die], JEDEC_ID_WINBOND_W25N01GV); } @@ -167,7 +174,7 @@ void w25m_eraseSector(flashDevice_t *fdevice, uint32_t address) { int dieNumber = address / dieSize; - w25m_dieSelect(fdevice->io.handle.busdev, dieNumber); + w25m_dieSelect(fdevice->io.handle.dev, dieNumber); dieDevice[dieNumber].vTable->eraseSector(&dieDevice[dieNumber], address % dieSize); } @@ -175,7 +182,7 @@ void w25m_eraseSector(flashDevice_t *fdevice, uint32_t address) void w25m_eraseCompletely(flashDevice_t *fdevice) { for (int dieNumber = 0 ; dieNumber < dieCount ; dieNumber++) { - w25m_dieSelect(fdevice->io.handle.busdev, dieNumber); + w25m_dieSelect(fdevice->io.handle.dev, dieNumber); dieDevice[dieNumber].vTable->eraseCompletely(&dieDevice[dieNumber]); } } @@ -185,12 +192,10 @@ static int currentWriteDie; void w25m_pageProgramBegin(flashDevice_t *fdevice, uint32_t address) { - UNUSED(fdevice); - currentWriteDie = address / dieSize; - w25m_dieSelect(fdevice->io.handle.busdev, currentWriteDie); + w25m_dieSelect(fdevice->io.handle.dev, currentWriteDie); currentWriteAddress = address % dieSize; - dieDevice[currentWriteDie].vTable->pageProgramBegin(&dieDevice[currentWriteDie], currentWriteAddress); + dieDevice[currentWriteDie].vTable->pageProgramBegin(&dieDevice[currentWriteDie], address); } void w25m_pageProgramContinue(flashDevice_t *fdevice, const uint8_t *data, int length) @@ -230,7 +235,7 @@ int w25m_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer, in uint32_t dieAddress = address % dieSize; tlen = MIN(dieAddress + rlen, dieSize) - dieAddress; - w25m_dieSelect(fdevice->io.handle.busdev, dieNumber); + w25m_dieSelect(fdevice->io.handle.dev, dieNumber); rbytes = dieDevice[dieNumber].vTable->readBytes(&dieDevice[dieNumber], dieAddress, buffer, tlen); diff --git a/src/main/drivers/flash_w25n01g.c b/src/main/drivers/flash_w25n01g.c index 5f9b21559..96b0a3deb 100644 --- a/src/main/drivers/flash_w25n01g.c +++ b/src/main/drivers/flash_w25n01g.c @@ -133,11 +133,6 @@ typedef struct bblut_s { uint16_t lba; } bblut_t; -// These will be gone - -#define DISABLE(busdev) IOHi((busdev)->busdev_u.spi.csnPin); __NOP() -#define ENABLE(busdev) __NOP(); IOLo((busdev)->busdev_u.spi.csnPin) - static bool w25n01g_waitForReady(flashDevice_t *fdevice); static void w25n01g_setTimeout(flashDevice_t *fdevice, uint32_t timeoutMillis) @@ -152,12 +147,20 @@ static void w25n01g_setTimeout(flashDevice_t *fdevice, uint32_t timeoutMillis) static void w25n01g_performOneByteCommand(flashDeviceIO_t *io, uint8_t command) { if (io->mode == FLASHIO_SPI) { - busDevice_t *busdev = io->handle.busdev; + extDevice_t *dev = io->handle.dev; - ENABLE(busdev); - spiTransferByte(busdev->busdev_u.spi.instance, command); - DISABLE(busdev); + busSegment_t segments[] = { + {&command, NULL, sizeof (command), true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + // Ensure any prior DMA has completed before continuing + spiWait(dev); + + spiSequence(dev, &segments[0]); + + // Block pending completion of SPI access + spiWait(dev); } #ifdef USE_QUADSPI else if (io->mode == FLASHIO_QUADSPI) { @@ -170,13 +173,22 @@ static void w25n01g_performOneByteCommand(flashDeviceIO_t *io, uint8_t command) static void w25n01g_performCommandWithPageAddress(flashDeviceIO_t *io, uint8_t command, uint32_t pageAddress) { if (io->mode == FLASHIO_SPI) { - busDevice_t *busdev = io->handle.busdev; + extDevice_t *dev = io->handle.dev; - const uint8_t cmd[] = { command, 0, (pageAddress >> 8) & 0xff, (pageAddress >> 0) & 0xff}; + uint8_t cmd[] = { command, 0, (pageAddress >> 8) & 0xff, (pageAddress >> 0) & 0xff}; - ENABLE(busdev); - spiTransfer(busdev->busdev_u.spi.instance, cmd, NULL, sizeof(cmd)); - DISABLE(busdev); + busSegment_t segments[] = { + {cmd, NULL, sizeof (cmd), true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + + // Ensure any prior DMA has completed before continuing + spiWait(dev); + + spiSequence(dev, &segments[0]); + + // Block pending completion of SPI access + spiWait(dev); } #ifdef USE_QUADSPI else if (io->mode == FLASHIO_QUADSPI) { @@ -190,14 +202,23 @@ static void w25n01g_performCommandWithPageAddress(flashDeviceIO_t *io, uint8_t c static uint8_t w25n01g_readRegister(flashDeviceIO_t *io, uint8_t reg) { if (io->mode == FLASHIO_SPI) { - busDevice_t *busdev = io->handle.busdev; + extDevice_t *dev = io->handle.dev; - ENABLE(busdev); - const uint8_t cmd[3] = { W25N01G_INSTRUCTION_READ_STATUS_REG, reg, 0 }; + uint8_t cmd[3] = { W25N01G_INSTRUCTION_READ_STATUS_REG, reg, 0 }; uint8_t in[3]; - spiTransfer(busdev->busdev_u.spi.instance, cmd, in, sizeof(cmd)); - DISABLE(busdev); + busSegment_t segments[] = { + {cmd, in, sizeof (cmd), true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + + // Ensure any prior DMA has completed before continuing + spiWait(dev); + + spiSequence(dev, &segments[0]); + + // Block pending completion of SPI access + spiWait(dev); return in[2]; } @@ -218,12 +239,21 @@ static uint8_t w25n01g_readRegister(flashDeviceIO_t *io, uint8_t reg) static void w25n01g_writeRegister(flashDeviceIO_t *io, uint8_t reg, uint8_t data) { if (io->mode == FLASHIO_SPI) { - busDevice_t *busdev = io->handle.busdev; - const uint8_t cmd[3] = { W25N01G_INSTRUCTION_WRITE_STATUS_REG, reg, data }; + extDevice_t *dev = io->handle.dev; + uint8_t cmd[3] = { W25N01G_INSTRUCTION_WRITE_STATUS_REG, reg, data }; - ENABLE(busdev); - spiTransfer(busdev->busdev_u.spi.instance, cmd, NULL, sizeof(cmd)); - DISABLE(busdev); + busSegment_t segments[] = { + {cmd, NULL, sizeof (cmd), true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + + // Ensure any prior DMA has completed before continuing + spiWait(dev); + + spiSequence(dev, &segments[0]); + + // Block pending completion of SPI access + spiWait(dev); } #ifdef USE_QUADSPI else if (io->mode == FLASHIO_QUADSPI) { @@ -246,7 +276,7 @@ static void w25n01g_deviceReset(flashDevice_t *fdevice) // Protection for upper 1/32 (BP[3:0] = 0101, TB=0), WP-E on; to protect bad block replacement area // DON'T DO THIS. This will prevent writes through the bblut as well. - // w25n01g_writeRegister(busdev, W25N01G_PROT_REG, W25N01G_PROT_PB0_ENABLE|W25N01G_PROT_PB2_ENABLE|W25N01G_PROT_WP_E_ENABLE); + // w25n01g_writeRegister(dev, W25N01G_PROT_REG, W25N01G_PROT_PB0_ENABLE|W25N01G_PROT_PB2_ENABLE|W25N01G_PROT_WP_E_ENABLE); // No protection, WP-E off, WP-E prevents use of IO2 w25n01g_writeRegister(io, W25N01G_PROT_REG, W25N01G_PROT_CLEAR); @@ -380,13 +410,22 @@ static void w25n01g_programDataLoad(flashDevice_t *fdevice, uint16_t columnAddre w25n01g_waitForReady(fdevice); if (fdevice->io.mode == FLASHIO_SPI) { - busDevice_t *busdev = fdevice->io.handle.busdev; - const uint8_t cmd[] = { W25N01G_INSTRUCTION_PROGRAM_DATA_LOAD, columnAddress >> 8, columnAddress & 0xff }; + extDevice_t *dev = fdevice->io.handle.dev; + uint8_t cmd[] = { W25N01G_INSTRUCTION_PROGRAM_DATA_LOAD, columnAddress >> 8, columnAddress & 0xff }; - ENABLE(busdev); - spiTransfer(busdev->busdev_u.spi.instance, cmd, NULL, sizeof(cmd)); - spiTransfer(busdev->busdev_u.spi.instance, data, NULL, length); - DISABLE(busdev); + busSegment_t segments[] = { + {cmd, NULL, sizeof (cmd), true, NULL}, + {(uint8_t *)data, NULL, length, true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + + // Ensure any prior DMA has completed before continuing + spiWait(dev); + + spiSequence(dev, &segments[0]); + + // Block pending completion of SPI access + spiWait(dev); } #ifdef USE_QUADSPI else if (fdevice->io.mode == FLASHIO_QUADSPI) { @@ -401,18 +440,26 @@ static void w25n01g_programDataLoad(flashDevice_t *fdevice, uint16_t columnAddre static void w25n01g_randomProgramDataLoad(flashDevice_t *fdevice, uint16_t columnAddress, const uint8_t *data, int length) { - const uint8_t cmd[] = { W25N01G_INSTRUCTION_RANDOM_PROGRAM_DATA_LOAD, columnAddress >> 8, columnAddress & 0xff }; + uint8_t cmd[] = { W25N01G_INSTRUCTION_RANDOM_PROGRAM_DATA_LOAD, columnAddress >> 8, columnAddress & 0xff }; w25n01g_waitForReady(fdevice); if (fdevice->io.mode == FLASHIO_SPI) { - busDevice_t *busdev = fdevice->io.handle.busdev; + extDevice_t *dev = fdevice->io.handle.dev; - ENABLE(busdev); - spiTransfer(busdev->busdev_u.spi.instance, cmd, NULL, sizeof(cmd)); - spiTransfer(busdev->busdev_u.spi.instance, data, NULL, length); - DISABLE(busdev); + busSegment_t segments[] = { + {cmd, NULL, sizeof (cmd), true, NULL}, + {(uint8_t *)data, NULL, length, true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + // Ensure any prior DMA has completed before continuing + spiWait(dev); + + spiSequence(dev, &segments[0]); + + // Block pending completion of SPI access + spiWait(dev); } #ifdef USE_QUADSPI else if (fdevice->io.mode == FLASHIO_QUADSPI) { @@ -496,8 +543,6 @@ void w25n01g_pageProgramBegin(flashDevice_t *fdevice, uint32_t address) void w25n01g_pageProgramContinue(flashDevice_t *fdevice, const uint8_t *data, int length) { - // Check for page boundary overrun - w25n01g_waitForReady(fdevice); w25n01g_writeEnable(fdevice); @@ -617,7 +662,7 @@ int w25n01g_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer, currentPage = targetPage; } - uint16_t column = W25N01G_LINEAR_TO_COLUMN(address); + int column = W25N01G_LINEAR_TO_COLUMN(address); uint16_t transferLength; if (length > W25N01G_PAGE_SIZE - column) { @@ -627,7 +672,7 @@ int w25n01g_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer, } if (fdevice->io.mode == FLASHIO_SPI) { - busDevice_t *busdev = fdevice->io.handle.busdev; + extDevice_t *dev = fdevice->io.handle.dev; uint8_t cmd[4]; cmd[0] = W25N01G_INSTRUCTION_READ_DATA; @@ -635,11 +680,19 @@ int w25n01g_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer, cmd[2] = (column >> 0) & 0xff; cmd[3] = 0; - ENABLE(busdev); - spiTransfer(busdev->busdev_u.spi.instance, cmd, NULL, sizeof(cmd)); - spiTransfer(busdev->busdev_u.spi.instance, NULL, buffer, length); - DISABLE(busdev); + busSegment_t segments[] = { + {cmd, NULL, sizeof (cmd), false, NULL}, + {NULL, buffer, length, true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + // Ensure any prior DMA has completed before continuing + spiWait(dev); + + spiSequence(dev, &segments[0]); + + // Block pending completion of SPI access + spiWait(dev); } #ifdef USE_QUADSPI else if (fdevice->io.mode == FLASHIO_QUADSPI) { @@ -687,7 +740,7 @@ int w25n01g_readExtensionBytes(flashDevice_t *fdevice, uint32_t address, uint8_t uint32_t column = 2048; if (fdevice->io.mode == FLASHIO_SPI) { - busDevice_t *busdev = fdevice->io.handle.busdev; + extDevice_t *dev = fdevice->io.handle.dev; uint8_t cmd[4]; cmd[0] = W25N01G_INSTRUCTION_READ_DATA; @@ -695,10 +748,19 @@ int w25n01g_readExtensionBytes(flashDevice_t *fdevice, uint32_t address, uint8_t cmd[2] = (column >> 0) & 0xff; cmd[3] = 0; - ENABLE(busdev); - spiTransfer(busdev->busdev_u.spi.instance, cmd, NULL, sizeof(cmd)); - spiTransfer(busdev->busdev_u.spi.instance, NULL, buffer, length); - DISABLE(busdev); + busSegment_t segments[] = { + {cmd, NULL, sizeof (cmd), false, NULL}, + {NULL, buffer, length, true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + + // Ensure any prior DMA has completed before continuing + spiWait(dev); + + spiSequence(dev, &segments[0]); + + // Block pending completion of SPI access + spiWait(dev); } #ifdef USE_QUADSPI @@ -740,28 +802,35 @@ const flashVTable_t w25n01g_vTable = { void w25n01g_readBBLUT(flashDevice_t *fdevice, bblut_t *bblut, int lutsize) { - uint8_t in[4]; if (fdevice->io.mode == FLASHIO_SPI) { - busDevice_t *busdev = fdevice->io.handle.busdev; + extDevice_t *dev = fdevice->io.handle.dev; uint8_t cmd[4]; cmd[0] = W25N01G_INSTRUCTION_READ_BBM_LUT; cmd[1] = 0; - ENABLE(busdev); + busSegment_t segments[] = { + {cmd, NULL, sizeof (cmd), false, NULL}, + {NULL, in, sizeof (in), true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; - spiTransfer(busdev->busdev_u.spi.instance, cmd, NULL, 2); + // Ensure any prior DMA has completed before continuing + spiWait(dev); + + spiSequence(dev, &segments[0]); + + // Block pending completion of SPI access + spiWait(dev); for (int i = 0 ; i < lutsize ; i++) { - spiTransfer(busdev->busdev_u.spi.instance, NULL, in, 4); + spiReadWriteBuf(dev, NULL, in, 4); bblut[i].pba = (in[0] << 16)|in[1]; bblut[i].lba = (in[2] << 16)|in[3]; } - - DISABLE(busdev); } #ifdef USE_QUADSPI else if (fdevice->io.mode == FLASHIO_QUADSPI) { @@ -788,13 +857,22 @@ void w25n01g_writeBBLUT(flashDevice_t *fdevice, uint16_t lba, uint16_t pba) w25n01g_waitForReady(fdevice); if (fdevice->io.mode == FLASHIO_SPI) { - busDevice_t *busdev = fdevice->io.handle.busdev; + extDevice_t *dev = fdevice->io.handle.dev; uint8_t cmd[5] = { W25N01G_INSTRUCTION_BB_MANAGEMENT, lba >> 8, lba, pba >> 8, pba }; - ENABLE(busdev); - spiTransfer(busdev->busdev_u.spi.instance, cmd, NULL, sizeof(cmd)); - DISABLE(busdev); + busSegment_t segments[] = { + {cmd, NULL, sizeof (cmd), true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + + // Ensure any prior DMA has completed before continuing + spiWait(dev); + + spiSequence(dev, &segments[0]); + + // Block pending completion of SPI access + spiWait(dev); } #ifdef USE_QUADSPI else if (fdevice->io.mode == FLASHIO_QUADSPI) { diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index c9b04bf47..0694be8f6 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -52,7 +52,7 @@ #define WS2811_DMA_BUF_CACHE_ALIGN_BYTES ((WS2811_DMA_BUF_BYTES + 0x20) & ~0x1f) // Size of array to create a cache aligned buffer #define WS2811_DMA_BUF_CACHE_ALIGN_LENGTH (WS2811_DMA_BUF_CACHE_ALIGN_BYTES / sizeof (uint32_t)) -__attribute__((aligned(32))) uint32_t ledStripDMABuffer[WS2811_DMA_BUF_CACHE_ALIGN_LENGTH]; +DMA_RW_AXI __attribute__((aligned(32))) uint32_t ledStripDMABuffer[WS2811_DMA_BUF_CACHE_ALIGN_LENGTH]; #else #if defined(STM32F1) || defined(STM32F3) uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index d79a4e096..7840f5be1 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -42,10 +42,9 @@ #include "drivers/time.h" -// 20 MHz max SPI frequency +// 10 MHz max SPI frequency #define MAX7456_MAX_SPI_CLK_HZ 10000000 -// 1 MHz restore SPI frequency for shared SPI bus -#define MAX7456_MAX_SPI_SHARED_CLK 1000000 +#define MAX7456_INIT_MAX_SPI_CLK_HZ 5000000 // DEBUG_MAX7456_SIGNAL #define DEBUG_MAX7456_SIGNAL_MODEREG 0 @@ -177,27 +176,17 @@ #define CHARS_PER_LINE 30 // XXX Should be related to VIDEO_BUFFER_CHARS_*? -// On shared SPI bus we want to change clock for OSD chip and restore for other devices. - -#ifdef USE_SPI_TRANSACTION - #define __spiBusTransactionBegin(busdev) spiBusTransactionBegin(busdev) - #define __spiBusTransactionEnd(busdev) spiBusTransactionEnd(busdev) -#else - #define __spiBusTransactionBegin(busdev) {spiBusSetDivisor(busdev, max7456SpiClock);IOLo((busdev)->busdev_u.spi.csnPin);} - #define __spiBusTransactionEnd(busdev) {IOHi((busdev)->busdev_u.spi.csnPin);spiSetDivisor((busdev)->busdev_u.spi.instance, spiCalculateDivider(MAX7456_MAX_SPI_SHARED_CLK));} -#endif - #define MAX7456_SUPPORTED_LAYER_COUNT (DISPLAYPORT_LAYER_BACKGROUND + 1) typedef struct max7456Layer_s { uint8_t buffer[VIDEO_BUFFER_CHARS_PAL]; } max7456Layer_t; -static max7456Layer_t displayLayers[MAX7456_SUPPORTED_LAYER_COUNT]; +static DMA_DATA_ZERO_INIT max7456Layer_t displayLayers[MAX7456_SUPPORTED_LAYER_COUNT]; static displayPortLayer_e activeLayer = DISPLAYPORT_LAYER_FOREGROUND; -busDevice_t max7456BusDevice; -busDevice_t *busdev = &max7456BusDevice; +extDevice_t max7456Device; +extDevice_t *dev = &max7456Device; static bool max7456DeviceDetected = false; static uint16_t max7456SpiClock; @@ -213,9 +202,6 @@ static uint8_t shadowBuffer[VIDEO_BUFFER_CHARS_PAL]; //Max chars to update in one idle #define MAX_CHARS2UPDATE 100 -#ifdef MAX7456_DMA_CHANNEL_TX -volatile bool dmaTransactionInProgress = false; -#endif static uint8_t spiBuff[MAX_CHARS2UPDATE*6]; @@ -249,128 +235,6 @@ static uint8_t *getActiveLayerBuffer(void) return getLayerBuffer(activeLayer); } -static uint8_t max7456Send(uint8_t add, uint8_t data) -{ - spiTransferByte(busdev->busdev_u.spi.instance, add); - return spiTransferByte(busdev->busdev_u.spi.instance, data); -} - -#ifdef MAX7456_DMA_CHANNEL_TX -static void max7456SendDma(void* tx_buffer, void* rx_buffer, uint16_t buffer_size) -{ - DMA_InitTypeDef DMA_InitStructure; -#ifdef MAX7456_DMA_CHANNEL_RX - static uint16_t dummy[] = {0xffff}; -#else - UNUSED(rx_buffer); -#endif - while (dmaTransactionInProgress); // Wait for prev DMA transaction - - DMA_DeInit(MAX7456_DMA_CHANNEL_TX); -#ifdef MAX7456_DMA_CHANNEL_RX - DMA_DeInit(MAX7456_DMA_CHANNEL_RX); -#endif - - // Common to both channels - DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)(&(busdev->busdev_u.spi.instance->DR)); - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; - DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMA_InitStructure.DMA_BufferSize = buffer_size; - DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; - DMA_InitStructure.DMA_Priority = DMA_Priority_Low; - -#ifdef MAX7456_DMA_CHANNEL_RX - // Rx Channel - -#ifdef STM32F4 - DMA_InitStructure.DMA_Memory0BaseAddr = rx_buffer ? (uint32_t)rx_buffer : (uint32_t)(dummy); - DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; -#else - DMA_InitStructure.DMA_MemoryBaseAddr = rx_buffer ? (uint32_t)rx_buffer : (uint32_t)(dummy); - DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; -#endif - DMA_InitStructure.DMA_MemoryInc = rx_buffer ? DMA_MemoryInc_Enable : DMA_MemoryInc_Disable; - - DMA_Init(MAX7456_DMA_CHANNEL_RX, &DMA_InitStructure); - DMA_Cmd(MAX7456_DMA_CHANNEL_RX, ENABLE); -#endif - - // Tx channel - -#ifdef STM32F4 - DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)tx_buffer; //max7456_screen; - DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; -#else - DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)tx_buffer; //max7456_screen; - DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; -#endif - DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - - DMA_Init(MAX7456_DMA_CHANNEL_TX, &DMA_InitStructure); - DMA_Cmd(MAX7456_DMA_CHANNEL_TX, ENABLE); - -#ifdef MAX7456_DMA_CHANNEL_RX - DMA_ITConfig(MAX7456_DMA_CHANNEL_RX, DMA_IT_TC, ENABLE); -#else - DMA_ITConfig(MAX7456_DMA_CHANNEL_TX, DMA_IT_TC, ENABLE); -#endif - - // Enable SPI TX/RX request - - __spiBusTransactionBegin(busdev); - dmaTransactionInProgress = true; - - SPI_I2S_DMACmd(busdev->busdev_u.spi.instance, -#ifdef MAX7456_DMA_CHANNEL_RX - SPI_I2S_DMAReq_Rx | -#endif - SPI_I2S_DMAReq_Tx, ENABLE); -} - -void max7456_dma_irq_handler(dmaChannelDescriptor_t* descriptor) -{ - if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { -#ifdef MAX7456_DMA_CHANNEL_RX - DMA_Cmd(MAX7456_DMA_CHANNEL_RX, DISABLE); -#endif - // Make sure SPI DMA transfer is complete - - while (SPI_I2S_GetFlagStatus (busdev->busdev_u.spi.instance, SPI_I2S_FLAG_TXE) == RESET) {}; - while (SPI_I2S_GetFlagStatus (busdev->busdev_u.spi.instance, SPI_I2S_FLAG_BSY) == SET) {}; - - // Empty RX buffer. RX DMA takes care of it if enabled. - // This should be done after transmission finish!!! - - while (SPI_I2S_GetFlagStatus(busdev->busdev_u.spi.instance, SPI_I2S_FLAG_RXNE) == SET) { - busdev->busdev_u.spi.instance->DR; - } - - DMA_Cmd(MAX7456_DMA_CHANNEL_TX, DISABLE); - - DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); - - SPI_I2S_DMACmd(busdev->busdev_u.spi.instance, -#ifdef MAX7456_DMA_CHANNEL_RX - SPI_I2S_DMAReq_Rx | -#endif - SPI_I2S_DMAReq_Tx, DISABLE); - - __spiBusTransactionEnd(busdev); - dmaTransactionInProgress = false; - } - - if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_HTIF)) { - DMA_CLEAR_FLAG(descriptor, DMA_IT_HTIF); - } - if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TEIF)) { - DMA_CLEAR_FLAG(descriptor, DMA_IT_TEIF); - } -} - -#endif - static void max7456SetRegisterVM1(void) { uint8_t backgroundGray = BACKGROUND_BRIGHTNESS_28; // this is the device default background gray level @@ -391,7 +255,7 @@ static void max7456SetRegisterVM1(void) } } vm1Register |= (backgroundGray << 4); - max7456Send(MAX7456ADD_VM1, vm1Register); + spiWriteReg(dev, MAX7456ADD_VM1, vm1Register); } uint8_t max7456GetRowsCount(void) @@ -412,15 +276,11 @@ static void max7456ClearLayer(displayPortLayer_e layer) memset(getLayerBuffer(layer), 0x20, VIDEO_BUFFER_CHARS_PAL); } - - void max7456ReInit(void) { uint8_t srdata = 0; static bool firstInit = true; - __spiBusTransactionBegin(busdev); - switch (videoSignalCfg) { case VIDEO_SYSTEM_PAL: videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE; @@ -431,7 +291,7 @@ void max7456ReInit(void) break; case VIDEO_SYSTEM_AUTO: - srdata = max7456Send(MAX7456ADD_STAT, 0x00); + srdata = spiReadRegMsk(dev, MAX7456ADD_STAT); if (VIN_IS_NTSC(srdata)) { videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE; @@ -454,19 +314,17 @@ void max7456ReInit(void) previousBlackWhiteRegister = INVALID_PREVIOUS_REGISTER_STATE; max7456Brightness(0, 2); // Re-enable MAX7456 (last function call disables it) - __spiBusTransactionBegin(busdev); // Make sure the Max7456 is enabled - max7456Send(MAX7456ADD_VM0, videoSignalReg); - max7456Send(MAX7456ADD_HOS, hosRegValue); - max7456Send(MAX7456ADD_VOS, vosRegValue); - max7456SetRegisterVM1(); + spiWriteReg(dev, MAX7456ADD_VM0, videoSignalReg); + spiWriteReg(dev, MAX7456ADD_HOS, hosRegValue); + spiWriteReg(dev, MAX7456ADD_VOS, vosRegValue); - max7456Send(MAX7456ADD_DMM, displayMemoryModeReg | CLEAR_DISPLAY); - __spiBusTransactionEnd(busdev); + max7456SetRegisterVM1(); // Clear shadow to force redraw all screen in non-dma mode. max7456ClearShadowBuffer(); + if (firstInit) { max7456DrawScreenSlow(); firstInit = false; @@ -483,7 +341,6 @@ void max7456PreInit(const max7456Config_t *max7456Config) max7456InitStatus_e max7456Init(const max7456Config_t *max7456Config, const vcdProfile_t *pVcdProfile, bool cpuOverclock) { - max7456SpiClock = spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ); max7456DeviceDetected = false; deviceBackgroundType = DISPLAY_BACKGROUND_TRANSPARENT; @@ -494,69 +351,62 @@ max7456InitStatus_e max7456Init(const max7456Config_t *max7456Config, const vcdP max7456HardwareReset(); - if (!max7456Config->csTag || !max7456Config->spiDevice) { + if (!max7456Config->csTag || !spiSetBusInstance(dev, max7456Config->spiDevice, OWNER_OSD_CS)) { return MAX7456_INIT_NOT_CONFIGURED; } - busdev->busdev_u.spi.csnPin = IOGetByTag(max7456Config->csTag); + dev->busType_u.spi.csnPin = IOGetByTag(max7456Config->csTag); - if (!IOIsFreeOrPreinit(busdev->busdev_u.spi.csnPin)) { + if (!IOIsFreeOrPreinit(dev->busType_u.spi.csnPin)) { return MAX7456_INIT_NOT_CONFIGURED; } - IOInit(busdev->busdev_u.spi.csnPin, OWNER_OSD_CS, 0); - IOConfigGPIO(busdev->busdev_u.spi.csnPin, SPI_IO_CS_CFG); - IOHi(busdev->busdev_u.spi.csnPin); - - spiBusSetInstance(busdev, spiInstanceByDevice(SPI_CFG_TO_DEV(max7456Config->spiDevice))); + IOInit(dev->busType_u.spi.csnPin, OWNER_OSD_CS, 0); + IOConfigGPIO(dev->busType_u.spi.csnPin, SPI_IO_CS_CFG); + IOHi(dev->busType_u.spi.csnPin); // Detect MAX7456 existence and device type. Do this at half the speed for safety. // Detect MAX7456 and compatible device by reading OSDM (OSD Insertion MUX) register. // This register is not modified in this driver, therefore ensured to remain at its default value (0x1B). - spiSetDivisor(busdev->busdev_u.spi.instance, spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ) * 2); + spiSetClkDivisor(dev, spiCalculateDivider(MAX7456_INIT_MAX_SPI_CLK_HZ)); - __spiBusTransactionBegin(busdev); + // Write 0xff to conclude any current SPI transaction the MAX7456 is expecting + spiWrite(dev, END_STRING); - uint8_t osdm = max7456Send(MAX7456ADD_OSDM|MAX7456ADD_READ, 0xff); - - __spiBusTransactionEnd(busdev); + uint8_t osdm = spiReadRegMsk(dev, MAX7456ADD_OSDM); if (osdm != 0x1B) { - IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_IPU); + IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_IPU); return MAX7456_INIT_NOT_FOUND; } // At this point, we can claim the ownership of the CS pin max7456DeviceDetected = true; - IOInit(busdev->busdev_u.spi.csnPin, OWNER_OSD_CS, 0); + IOInit(dev->busType_u.spi.csnPin, OWNER_OSD_CS, 0); // Detect device type by writing and reading CA[8] bit at CMAL[6]. // This is a bit for accessing second half of character glyph storage, supported only by AT variant. - __spiBusTransactionBegin(busdev); + spiWriteReg(dev, MAX7456ADD_CMAL, (1 << 6)); // CA[8] bit - max7456Send(MAX7456ADD_CMAL, (1 << 6)); // CA[8] bit - - if (max7456Send(MAX7456ADD_CMAL|MAX7456ADD_READ, 0xff) & (1 << 6)) { + if (spiReadRegMsk(dev, MAX7456ADD_CMAL) & (1 << 6)) { max7456DeviceType = MAX7456_DEVICE_TYPE_AT; } else { max7456DeviceType = MAX7456_DEVICE_TYPE_MAX; } - __spiBusTransactionEnd(busdev); - #if defined(USE_OVERCLOCK) // Determine SPI clock divisor based on config and the device type. switch (max7456Config->clockConfig) { case MAX7456_CLOCK_CONFIG_HALF: - max7456SpiClock = spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ) * 2; + max7456SpiClock = spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ / 2); break; case MAX7456_CLOCK_CONFIG_OC: - max7456SpiClock = (cpuOverclock && (max7456DeviceType == MAX7456_DEVICE_TYPE_MAX)) ? spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ) * 2 : spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ); + max7456SpiClock = (cpuOverclock && (max7456DeviceType == MAX7456_DEVICE_TYPE_MAX)) ? spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ / 2) : spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ); break; case MAX7456_CLOCK_CONFIG_FULL: @@ -572,26 +422,22 @@ max7456InitStatus_e max7456Init(const max7456Config_t *max7456Config, const vcdP UNUSED(cpuOverclock); #endif -#ifdef USE_SPI_TRANSACTION - spiBusTransactionInit(busdev, SPI_MODE3_POL_HIGH_EDGE_2ND, max7456SpiClock); -#else - spiBusSetDivisor(busdev, max7456SpiClock); -#endif + spiSetClkDivisor(dev, max7456SpiClock); // force soft reset on Max7456 - __spiBusTransactionBegin(busdev); - max7456Send(MAX7456ADD_VM0, MAX7456_RESET); - __spiBusTransactionEnd(busdev); + spiWriteReg(dev, MAX7456ADD_VM0, MAX7456_RESET); + + // Wait for 100us before polling for completion of reset + delayMicroseconds(100); + + // Wait for reset to complete + while ((spiReadRegMsk(dev, MAX7456ADD_VM0) & MAX7456_RESET) != 0x00); // Setup values to write to registers videoSignalCfg = pVcdProfile->video_system; hosRegValue = 32 - pVcdProfile->h_offset; vosRegValue = 16 - pVcdProfile->v_offset; -#ifdef MAX7456_DMA_CHANNEL_TX - dmaSetHandler(MAX7456_DMA_IRQ_HANDLER_ID, max7456_dma_irq_handler, NVIC_PRIO_MAX7456_DMA, 0); -#endif - // Real init will be made later when driver detect idle. return MAX7456_INIT_OK; } @@ -612,9 +458,7 @@ void max7456Invert(bool invert) // redrawn with the proper invert state max7456ClearShadowBuffer(); previousInvertRegister = displayMemoryModeReg; - __spiBusTransactionBegin(busdev); - max7456Send(MAX7456ADD_DMM, displayMemoryModeReg); - __spiBusTransactionEnd(busdev); + spiWriteReg(dev, MAX7456ADD_DMM, displayMemoryModeReg); } } @@ -630,11 +474,9 @@ void max7456Brightness(uint8_t black, uint8_t white) if (reg != previousBlackWhiteRegister) { previousBlackWhiteRegister = reg; - __spiBusTransactionBegin(busdev); for (int i = MAX7456ADD_RB0; i <= MAX7456ADD_RB15; i++) { - max7456Send(i, reg); + spiWriteReg(dev, i, reg); } - __spiBusTransactionEnd(busdev); } } @@ -692,11 +534,7 @@ bool max7456LayerCopy(displayPortLayer_e destLayer, displayPortLayer_e sourceLay bool max7456DmaInProgress(void) { -#ifdef MAX7456_DMA_CHANNEL_TX - return dmaTransactionInProgress; -#else - return false; -#endif + return spiIsBusy(dev); } bool max7456BuffersSynced(void) @@ -721,9 +559,11 @@ void max7456ReInitIfRequired(bool forceStallCheck) bool stalled = false; if (forceStallCheck || (lastStallCheckMs + MAX7456_STALL_CHECK_INTERVAL_MS < nowMs)) { lastStallCheckMs = nowMs; - __spiBusTransactionBegin(busdev); - stalled = (max7456Send(MAX7456ADD_VM0|MAX7456ADD_READ, 0x00) != videoSignalReg); - __spiBusTransactionEnd(busdev); + + // Write 0xff to conclude any current SPI transaction the MAX7456 is expecting + spiWrite(dev, END_STRING); + + stalled = (spiReadRegMsk(dev, MAX7456ADD_VM0) != videoSignalReg); } if (stalled) { @@ -731,11 +571,12 @@ void max7456ReInitIfRequired(bool forceStallCheck) } else if ((videoSignalCfg == VIDEO_SYSTEM_AUTO) && ((nowMs - lastSigCheckMs) > MAX7456_SIGNAL_CHECK_INTERVAL_MS)) { + // Write 0xff to conclude any current SPI transaction the MAX7456 is expecting + spiWrite(dev, END_STRING); + // Adjust output format based on the current input format. - __spiBusTransactionBegin(busdev); - const uint8_t videoSense = max7456Send(MAX7456ADD_STAT, 0x00); - __spiBusTransactionEnd(busdev); + const uint8_t videoSense = spiReadRegMsk(dev, MAX7456ADD_STAT); DEBUG_SET(DEBUG_MAX7456_SIGNAL, DEBUG_MAX7456_SIGNAL_MODEREG, videoSignalReg & VIDEO_MODE_MASK); DEBUG_SET(DEBUG_MAX7456_SIGNAL, DEBUG_MAX7456_SIGNAL_SENSE, videoSense & 0x7); @@ -767,6 +608,11 @@ void max7456ReInitIfRequired(bool forceStallCheck) void max7456DrawScreen(void) { static uint16_t pos = 0; + // This routine doesn't block so need to use static data + static busSegment_t segments[] = { + {NULL, NULL, 0, true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; if (!fontIsLoading) { @@ -795,13 +641,15 @@ void max7456DrawScreen(void) } if (buff_len) { -#ifdef MAX7456_DMA_CHANNEL_TX - max7456SendDma(spiBuff, NULL, buff_len); -#else - __spiBusTransactionBegin(busdev); - spiTransfer(busdev->busdev_u.spi.instance, spiBuff, NULL, buff_len); - __spiBusTransactionEnd(busdev); -#endif // MAX7456_DMA_CHANNEL_TX + segments[0].txData = spiBuff; + segments[0].len = buff_len; + + // Ensure any prior DMA has completed + spiWaitClaim(dev); + + spiSequence(dev, &segments[0]); + + // Non-blocking, so transfer still in progress } } } @@ -811,50 +659,49 @@ static void max7456DrawScreenSlow(void) bool escapeCharFound = false; uint8_t *buffer = getActiveLayerBuffer(); - __spiBusTransactionBegin(busdev); - // Enable auto-increment mode and update every character in the active buffer. - // The "escape" character 0xFF must be skipped as it causes the MAX7456 to exit auto-increment mode. - max7456Send(MAX7456ADD_DMAH, 0); - max7456Send(MAX7456ADD_DMAL, 0); - max7456Send(MAX7456ADD_DMM, displayMemoryModeReg | 1); + uint8_t dma_regs[3]; + dma_regs[0] = displayMemoryModeReg | 1; + dma_regs[1] = 0; + dma_regs[2] = 0; + + spiWriteRegBuf(dev, MAX7456ADD_DMM, dma_regs, sizeof (dma_regs)); + + // The "escape" character 0xFF must be skipped as it causes the MAX7456 to exit auto-increment mode. for (int xx = 0; xx < maxScreenSize; xx++) { if (buffer[xx] == END_STRING) { escapeCharFound = true; - max7456Send(MAX7456ADD_DMDI, ' '); // replace the 0xFF character with a blank in the first pass to avoid terminating auto-increment + spiWriteReg(dev, MAX7456ADD_DMDI, ' '); // replace the 0xFF character with a blank in the first pass to avoid terminating auto-increment } else { - max7456Send(MAX7456ADD_DMDI, buffer[xx]); + spiWriteReg(dev, MAX7456ADD_DMDI, buffer[xx]); } shadowBuffer[xx] = buffer[xx]; } - max7456Send(MAX7456ADD_DMDI, END_STRING); - max7456Send(MAX7456ADD_DMM, displayMemoryModeReg); + spiWriteReg(dev, MAX7456ADD_DMDI, END_STRING); + + // Turn off auto increment + spiWriteReg(dev, MAX7456ADD_DMM, displayMemoryModeReg); // If we found any of the "escape" character 0xFF, then make a second pass // to update them with direct addressing if (escapeCharFound) { + dma_regs[2] = END_STRING; for (int xx = 0; xx < maxScreenSize; xx++) { if (buffer[xx] == END_STRING) { - max7456Send(MAX7456ADD_DMAH, xx >> 8); - max7456Send(MAX7456ADD_DMAL, xx & 0xFF); - max7456Send(MAX7456ADD_DMDI, END_STRING); + dma_regs[0] = xx >> 8; + dma_regs[1] = xx & 0xFF; + spiWriteRegBuf(dev, MAX7456ADD_DMAH, dma_regs, sizeof (dma_regs)); } } } - - __spiBusTransactionEnd(busdev); } // should not be used when armed void max7456RefreshAll(void) { -#ifdef MAX7456_DMA_CHANNEL_TX - while (dmaTransactionInProgress); -#endif - max7456ReInitIfRequired(true); max7456DrawScreenSlow(); } @@ -864,20 +711,19 @@ bool max7456WriteNvm(uint8_t char_address, const uint8_t *font_data) if (!max7456DeviceDetected) { return false; } -#ifdef MAX7456_DMA_CHANNEL_TX - while (dmaTransactionInProgress); -#endif - __spiBusTransactionBegin(busdev); + // Block pending completion of any prior SPI access + spiWait(dev); + // disable display fontIsLoading = true; - max7456Send(MAX7456ADD_VM0, 0); + spiWriteReg(dev, MAX7456ADD_VM0, 0); - max7456Send(MAX7456ADD_CMAH, char_address); // set start address high + spiWriteReg(dev, MAX7456ADD_CMAH, char_address); // set start address high for (int x = 0; x < 54; x++) { - max7456Send(MAX7456ADD_CMAL, x); //set start address low - max7456Send(MAX7456ADD_CMDI, font_data[x]); + spiWriteReg(dev, MAX7456ADD_CMAL, x); //set start address low + spiWriteReg(dev, MAX7456ADD_CMDI, font_data[x]); #ifdef LED0_TOGGLE LED0_TOGGLE; #else @@ -887,13 +733,12 @@ bool max7456WriteNvm(uint8_t char_address, const uint8_t *font_data) // Transfer 54 bytes from shadow ram to NVM - max7456Send(MAX7456ADD_CMM, WRITE_NVR); + spiWriteReg(dev, MAX7456ADD_CMM, WRITE_NVR); // Wait until bit 5 in the status register returns to 0 (12ms) - while ((max7456Send(MAX7456ADD_STAT, 0x00) & STAT_NVR_BUSY) != 0x00); + while ((spiReadRegMsk(dev, MAX7456ADD_STAT) & STAT_NVR_BUSY) != 0x00); - __spiBusTransactionEnd(busdev); return true; } @@ -910,11 +755,14 @@ void max7456HardwareReset(void) IOInit(max7456ResetPin, OWNER_OSD, 0); IOConfigGPIO(max7456ResetPin, IO_RESET_CFG); - - // RESET + // RESET 50ms long pulse, followed by 100us pause IOLo(max7456ResetPin); - delay(100); + delay(50); IOHi(max7456ResetPin); + delayMicroseconds(100); +#else + // Allow device 50ms to powerup + delay(50); #endif } @@ -927,9 +775,7 @@ void max7456SetBackgroundType(displayPortBackground_e backgroundType) { deviceBackgroundType = backgroundType; - __spiBusTransactionBegin(busdev); max7456SetRegisterVM1(); - __spiBusTransactionEnd(busdev); } #endif // USE_MAX7456 diff --git a/src/main/drivers/nvic.h b/src/main/drivers/nvic.h index 025c9538c..f0aeac6bb 100644 --- a/src/main/drivers/nvic.h +++ b/src/main/drivers/nvic.h @@ -74,6 +74,7 @@ #define NVIC_PRIO_MAG_DATA_READY NVIC_BUILD_PRIORITY(0x0f, 0x0f) #define NVIC_PRIO_CALLBACK NVIC_BUILD_PRIORITY(0x0f, 0x0f) #define NVIC_PRIO_MAX7456_DMA NVIC_BUILD_PRIORITY(3, 0) +#define NVIC_PRIO_SPI_DMA NVIC_BUILD_PRIORITY(0, 0) #define NVIC_PRIO_SDIO_DMA NVIC_BUILD_PRIORITY(0, 0) #ifdef USE_HAL_DRIVER diff --git a/src/main/drivers/rx/rx_cc2500.c b/src/main/drivers/rx/rx_cc2500.c index ace926d14..5f8e6a20a 100644 --- a/src/main/drivers/rx/rx_cc2500.c +++ b/src/main/drivers/rx/rx_cc2500.c @@ -48,10 +48,28 @@ void cc2500ReadFifo(uint8_t *dpbuffer, uint8_t len) rxSpiReadCommandMulti(CC2500_3F_RXFIFO | CC2500_READ_BURST, NOP, dpbuffer, len); } +void cc2500WriteCommand(uint8_t command, uint8_t data) +{ + // Burst writes require an interbyte gap, see fig. 7, pg. 22 in https://www.ti.com/lit/ds/symlink/cc2500.pdf + // As such gaps can't be inserted if DMA is being used, inhibit DMA on this bus for the duration of this call + rxSpiDmaEnable(false); + rxSpiWriteCommand(command, data); + rxSpiDmaEnable(true); +} + +void cc2500WriteCommandMulti(uint8_t command, const uint8_t *data, uint8_t length) +{ + // Burst writes require an interbyte gap, see fig. 7, pg. 22 in https://www.ti.com/lit/ds/symlink/cc2500.pdf + // As such gaps can't be inserted if DMA is being used, inhibit DMA on this bus for the duration of this call + rxSpiDmaEnable(false); + rxSpiWriteCommandMulti(command, data, length); + rxSpiDmaEnable(true); +} + void cc2500WriteFifo(uint8_t *dpbuffer, uint8_t len) { cc2500Strobe(CC2500_SFTX); // 0x3B SFTX - rxSpiWriteCommandMulti(CC2500_3F_TXFIFO | CC2500_WRITE_BURST, + cc2500WriteCommandMulti(CC2500_3F_TXFIFO | CC2500_WRITE_BURST, dpbuffer, len); cc2500Strobe(CC2500_STX); // 0x35 } @@ -64,7 +82,7 @@ void cc2500ReadRegisterMulti(uint8_t address, uint8_t *data, uint8_t length) void cc2500WriteRegisterMulti(uint8_t address, uint8_t *data, uint8_t length) { - rxSpiWriteCommandMulti(address, data, length); + cc2500WriteCommandMulti(address, data, length); } uint8_t cc2500ReadReg(uint8_t reg) @@ -76,7 +94,7 @@ void cc2500Strobe(uint8_t address) { rxSpiWriteByte(address); } void cc2500WriteReg(uint8_t address, uint8_t data) { - rxSpiWriteCommand(address, data); + cc2500WriteCommand(address, data); } void cc2500SetPower(uint8_t power) diff --git a/src/main/drivers/rx/rx_spi.c b/src/main/drivers/rx/rx_spi.c index 6a2ab8c2d..e3d2e3843 100644 --- a/src/main/drivers/rx/rx_spi.c +++ b/src/main/drivers/rx/rx_spi.c @@ -43,14 +43,13 @@ #include "rx_spi.h" -// 10 MHz max SPI frequency -#define RX_MAX_SPI_CLK_HZ 10000000 +// 13.5 MHz max SPI frequency +#define RX_MAX_SPI_CLK_HZ 13500000 +// 6.5 MHz max SPI frequency during startup +#define RX_STARTUP_MAX_SPI_CLK_HZ 6500000 -#define ENABLE_RX() IOLo(busdev->busdev_u.spi.csnPin) -#define DISABLE_RX() IOHi(busdev->busdev_u.spi.csnPin) - -static busDevice_t rxSpiDevice; -static busDevice_t *busdev = &rxSpiDevice; +static extDevice_t rxSpiDevice; +static extDevice_t *dev = &rxSpiDevice; static IO_t extiPin = IO_NONE; static extiCallbackRec_t rxSpiExtiCallbackRec; @@ -76,27 +75,32 @@ void rxSpiExtiHandler(extiCallbackRec_t* callback) } } +void rxSpiNormalSpeed() +{ + spiSetClkDivisor(dev, spiCalculateDivider(RX_MAX_SPI_CLK_HZ)); +} + +void rxSpiStartupSpeed() +{ + spiSetClkDivisor(dev, spiCalculateDivider(RX_STARTUP_MAX_SPI_CLK_HZ)); +} + bool rxSpiDeviceInit(const rxSpiConfig_t *rxSpiConfig) { - SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(rxSpiConfig->spibus)); - - if (!instance) { + if (!spiSetBusInstance(dev, rxSpiConfig->spibus, OWNER_RX_SPI_CS)) { return false; } - spiBusSetInstance(busdev, instance); - const IO_t rxCsPin = IOGetByTag(rxSpiConfig->csnTag); IOInit(rxCsPin, OWNER_RX_SPI_CS, 0); IOConfigGPIO(rxCsPin, SPI_IO_CS_CFG); - busdev->busdev_u.spi.csnPin = rxCsPin; + dev->busType_u.spi.csnPin = rxCsPin; + + // Set the clock phase/polarity + spiSetClkPhasePolarity(dev, true); + rxSpiNormalSpeed(); IOHi(rxCsPin); -#ifdef USE_SPI_TRANSACTION - spiBusTransactionInit(busdev, SPI_MODE0_POL_LOW_EDGE_1ST, spiCalculateDivider(RX_MAX_SPI_CLK_HZ)); -#else - spiBusSetDivisor(busdev, spiCalculateDivider(RX_MAX_SPI_CLK_HZ)); -#endif extiPin = IOGetByTag(rxSpiConfig->extiIoTag); @@ -119,37 +123,41 @@ void rxSpiExtiInit(ioConfig_t rxSpiExtiPinConfig, extiTrigger_t rxSpiExtiPinTrig } } +void rxSpiDmaEnable(bool enable) +{ + spiDmaEnable(dev, enable); +} uint8_t rxSpiTransferByte(uint8_t data) { - return spiBusTransferByte(busdev, data); + return spiReadWrite(dev, data); } void rxSpiWriteByte(uint8_t data) { - spiBusWriteByte(busdev, data); + spiWrite(dev, data); } void rxSpiWriteCommand(uint8_t command, uint8_t data) { - spiBusWriteRegister(busdev, command, data); + spiWriteReg(dev, command, data); } void rxSpiWriteCommandMulti(uint8_t command, const uint8_t *data, uint8_t length) { - spiBusWriteRegisterBuffer(busdev, command, data, length); + spiWriteRegBuf(dev, command, (uint8_t *)data, length); } uint8_t rxSpiReadCommand(uint8_t command, uint8_t data) { UNUSED(data); - return spiBusRawReadRegister(busdev, command); + return spiReadReg(dev, command); } void rxSpiReadCommandMulti(uint8_t command, uint8_t commandData, uint8_t *retData, uint8_t length) { UNUSED(commandData); - spiBusRawReadRegisterBuffer(busdev, command, retData, length); + spiReadRegBuf(dev, command, retData, length); } bool rxSpiExtiConfigured(void) diff --git a/src/main/drivers/rx/rx_spi.h b/src/main/drivers/rx/rx_spi.h index 0312835d1..2633bcc93 100644 --- a/src/main/drivers/rx/rx_spi.h +++ b/src/main/drivers/rx/rx_spi.h @@ -32,6 +32,9 @@ struct rxSpiConfig_s; void rxSpiDevicePreInit(const struct rxSpiConfig_s *rxSpiConfig); bool rxSpiDeviceInit(const struct rxSpiConfig_s *rxSpiConfig); +void rxSpiNormalSpeed(); +void rxSpiStartupSpeed(); +void rxSpiDmaEnable(bool enable); uint8_t rxSpiTransferByte(uint8_t data); void rxSpiWriteByte(uint8_t data); void rxSpiWriteCommand(uint8_t command, uint8_t data); diff --git a/src/main/drivers/sdcard_impl.h b/src/main/drivers/sdcard_impl.h index 244ddae16..ddf23d9d8 100644 --- a/src/main/drivers/sdcard_impl.h +++ b/src/main/drivers/sdcard_impl.h @@ -90,9 +90,8 @@ typedef struct sdcard_t { IO_t cardDetectPin; #ifdef USE_SDCARD_SPI - busDevice_t busdev; - bool useDMAForTx; - dmaChannelDescriptor_t * dma; + extDevice_t dev; + uint8_t idleCount; #endif #ifdef USE_SDCARD_SDIO diff --git a/src/main/drivers/sdcard_spi.c b/src/main/drivers/sdcard_spi.c index 21f990469..97cf2ae86 100644 --- a/src/main/drivers/sdcard_spi.c +++ b/src/main/drivers/sdcard_spi.c @@ -73,27 +73,18 @@ static bool sdcardSpi_isFunctional(void) static void sdcard_select(void) { -#ifdef USE_SPI_TRANSACTION - spiBusTransactionBegin(&sdcard.busdev); -#else - IOLo(sdcard.busdev.busdev_u.spi.csnPin); -#endif + IOLo(sdcard.dev.busType_u.spi.csnPin); } static void sdcard_deselect(void) { // As per the SD-card spec, give the card 8 dummy clocks so it can finish its operation - //spiBusTransferByte(&sdcard.busdev, 0xFF); + //spiReadWrite(&sdcard.dev, 0xFF); - while (spiBusIsBusBusy(&sdcard.busdev)) { - } + spiWait(&sdcard.dev); delayMicroseconds(10); -#ifdef USE_SPI_TRANSACTION - spiBusTransactionEnd(&sdcard.busdev); -#else - IOHi(sdcard.busdev.busdev_u.spi.csnPin); -#endif + IOHi(sdcard.dev.busType_u.spi.csnPin); } /** @@ -110,11 +101,7 @@ static void sdcard_reset(void) } if (sdcard.state >= SDCARD_STATE_READY) { -#ifdef USE_SPI_TRANSACTION - spiBusTransactionInit(&sdcard.busdev, SDCARD_SPI_MODE, spiCalculateDivider(SDCARD_MAX_SPI_INIT_CLK_HZ)); -#else - spiSetDivisor(sdcard.busdev.busdev_u.spi.instance, spiCalculateDivider(SDCARD_MAX_SPI_INIT_CLK_HZ)); -#endif + spiSetClkDivisor(&sdcard.dev, spiCalculateDivider(SDCARD_MAX_SPI_INIT_CLK_HZ)); } sdcard.failureCount++; @@ -126,6 +113,50 @@ static void sdcard_reset(void) } } + +// Called in ISR context +// Wait until idle indicated by a read value of 0xff +busStatus_e sdcard_callbackIdle(uint32_t arg) +{ + sdcard_t *sdcard = (sdcard_t *)arg; + extDevice_t *dev = &sdcard->dev; + + uint8_t idleByte = dev->bus->curSegment->rxData[0]; + + if (idleByte == 0xff) { + return BUS_READY; + } + + if (--sdcard->idleCount == 0) { + dev->bus->curSegment->rxData[0] = 0x00; + return BUS_ABORT; + } + + return BUS_BUSY; +} + + +// Called in ISR context +// Wait until idle is no longer indicated by a read value of 0xff +busStatus_e sdcard_callbackNotIdle(uint32_t arg) +{ + sdcard_t *sdcard = (sdcard_t *)arg; + extDevice_t *dev = &sdcard->dev; + + uint8_t idleByte = dev->bus->curSegment->rxData[0]; + + if (idleByte != 0xff) { + return BUS_READY; + } + + if (sdcard->idleCount-- == 0) { + return BUS_ABORT; + } + + return BUS_BUSY; +} + + /** * The SD card spec requires 8 clock cycles to be sent by us on the bus after most commands so it can finish its * processing of that command. The easiest way for us to do this is to just wait for the bus to become idle before @@ -133,15 +164,25 @@ static void sdcard_reset(void) */ static bool sdcard_waitForIdle(int maxBytesToWait) { - while (maxBytesToWait > 0) { - uint8_t b = spiBusTransferByte(&sdcard.busdev, 0xFF); - if (b == 0xFF) { - return true; - } - maxBytesToWait--; - } + uint8_t idleByte; - return false; + // Note that this does not release the CS at the end of the transaction + busSegment_t segments[] = { + {NULL, &idleByte, sizeof (idleByte), false, sdcard_callbackIdle}, + {NULL, NULL, 0, false, NULL}, + }; + + sdcard.idleCount = maxBytesToWait; + + // Ensure any prior DMA has completed before continuing + spiWaitClaim(&sdcard.dev); + + spiSequence(&sdcard.dev, &segments[0]); + + // Block pending completion of SPI access + spiWait(&sdcard.dev); + + return (idleByte == 0xff); } /** @@ -151,15 +192,25 @@ static bool sdcard_waitForIdle(int maxBytesToWait) */ static uint8_t sdcard_waitForNonIdleByte(int maxDelay) { - for (int i = 0; i < maxDelay + 1; i++) { // + 1 so we can wait for maxDelay '0xFF' bytes before reading a response byte afterwards - uint8_t response = spiBusTransferByte(&sdcard.busdev, 0xFF); + uint8_t idleByte; - if (response != 0xFF) { - return response; - } - } + // Note that this does not release the CS at the end of the transaction + busSegment_t segments[] = { + {NULL, &idleByte, sizeof (idleByte), false, sdcard_callbackNotIdle}, + {NULL, NULL, 0, false, NULL}, + }; - return 0xFF; + sdcard.idleCount = maxDelay; + + // Ensure any prior DMA has completed before continuing + spiWaitClaim(&sdcard.dev); + + spiSequence(&sdcard.dev, &segments[0]); + + // Block pending completion of SPI access + spiWait(&sdcard.dev); + + return idleByte; } /** @@ -173,7 +224,7 @@ static uint8_t sdcard_waitForNonIdleByte(int maxDelay) */ static uint8_t sdcard_sendCommand(uint8_t commandCode, uint32_t commandArgument) { - const uint8_t command[6] = { + uint8_t command[6] = { 0x40 | commandCode, commandArgument >> 24, commandArgument >> 16, @@ -183,17 +234,29 @@ static uint8_t sdcard_sendCommand(uint8_t commandCode, uint32_t commandArgument) commands that require a CRC */ }; - // Go ahead and send the command even if the card isn't idle if this is the reset command + uint8_t idleByte; + + // Note that this does not release the CS at the end of the transaction + busSegment_t segments[] = { + {command, NULL, sizeof (command), false, NULL}, + {NULL, &idleByte, sizeof (idleByte), false, sdcard_callbackNotIdle}, + {NULL, NULL, 0, false, NULL}, + }; + if (!sdcard_waitForIdle(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY) && commandCode != SDCARD_COMMAND_GO_IDLE_STATE) return 0xFF; - spiBusRawTransfer(&sdcard.busdev, command, NULL, sizeof(command)); + sdcard.idleCount = SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY; - /* - * The card can take up to SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY bytes to send the response, in the meantime - * it'll transmit 0xFF filler bytes. - */ - return sdcard_waitForNonIdleByte(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY); + // Ensure any prior DMA has completed before continuing + spiWaitClaim(&sdcard.dev); + + spiSequence(&sdcard.dev, &segments[0]); + + // Block pending completion of SPI access + spiWait(&sdcard.dev); + + return idleByte; } static uint8_t sdcard_sendAppCommand(uint8_t commandCode, uint32_t commandArgument) @@ -223,7 +286,16 @@ static bool sdcard_validateInterfaceCondition(void) // V1 cards don't support this command sdcard.version = 1; } else if (status == SDCARD_R1_STATUS_BIT_IDLE) { - spiBusRawTransfer(&sdcard.busdev, NULL, ifCondReply, sizeof(ifCondReply)); + // Note that this does not release the CS at the end of the transaction + busSegment_t segments[] = { + {NULL, ifCondReply, sizeof (ifCondReply), false, NULL}, + {NULL, NULL, 0, false, NULL}, + }; + + spiSequence(&sdcard.dev, &segments[0]); + + // Block pending completion of SPI access + spiWait(&sdcard.dev); /* * We don't bother to validate the SDCard's operating voltage range since the spec requires it to accept our @@ -247,7 +319,16 @@ static bool sdcard_readOCRRegister(uint32_t *result) uint8_t response[4]; - spiBusRawTransfer(&sdcard.busdev, NULL, response, sizeof(response)); + // Note that this does not release the CS at the end of the transaction + busSegment_t segments[] = { + {NULL, response, sizeof (response), false, NULL}, + {NULL, NULL, 0, false, NULL}, + }; + + spiSequence(&sdcard.dev, &segments[0]); + + // Block pending completion of SPI access + spiWait(&sdcard.dev); if (status == 0) { sdcard_deselect(); @@ -285,30 +366,43 @@ static sdcardReceiveBlockStatus_e sdcard_receiveDataBlock(uint8_t *buffer, int c return SDCARD_RECEIVE_ERROR; } - spiBusRawTransfer(&sdcard.busdev, NULL, buffer, count); + // Note that this does not release the CS at the end of the transaction + busSegment_t segments[] = { + {NULL, buffer, count, false, NULL}, + // Discard trailing CRC, we don't care + {NULL, NULL, 2, false, NULL}, + {NULL, NULL, 0, false, NULL}, + }; - // Discard trailing CRC, we don't care - spiBusTransferByte(&sdcard.busdev, 0xFF); - spiBusTransferByte(&sdcard.busdev, 0xFF); + // Ensure any prior DMA has completed before continuing + spiWaitClaim(&sdcard.dev); + + spiSequence(&sdcard.dev, &segments[0]); + + // Block pending completion of SPI access + spiWait(&sdcard.dev); return SDCARD_RECEIVE_SUCCESS; } static bool sdcard_sendDataBlockFinish(void) { -#ifdef USE_HAL_DRIVER - // Drain anything left in the Rx FIFO (we didn't read it during the write) - // This is necessary here as when using msc there is timing issue - while (CHECK_SPI_RX_DATA_AVAILABLE(sdcard.busdev.busdev_u.spi.instance)) { - SPI_RX_DATA_REGISTER(sdcard.busdev.busdev_u.spi.instance); - } -#endif + uint16_t dummyCRC = 0; + uint8_t dataResponseToken; + // Note that this does not release the CS at the end of the transaction + busSegment_t segments[] = { + {(uint8_t *)&dummyCRC, NULL, sizeof (dummyCRC), false, NULL}, + {NULL, &dataResponseToken, sizeof (dataResponseToken), false, NULL}, + {NULL, NULL, 0, false, NULL}, + }; - // Send a dummy CRC - spiBusTransferByte(&sdcard.busdev, 0x00); - spiBusTransferByte(&sdcard.busdev, 0x00); + // Ensure any prior DMA has completed before continuing + spiWaitClaim(&sdcard.dev); - uint8_t dataResponseToken = spiBusTransferByte(&sdcard.busdev, 0xFF); + spiSequence(&sdcard.dev, &segments[0]); + + // Block pending completion of SPI access + spiWait(&sdcard.dev); /* * Check if the card accepted the write (no CRC error / no address error) @@ -328,85 +422,30 @@ static bool sdcard_sendDataBlockFinish(void) /** * Begin sending a buffer of SDCARD_BLOCK_SIZE bytes to the SD card. */ -static void sdcard_sendDataBlockBegin(const uint8_t *buffer, bool multiBlockWrite) +static void sdcard_sendDataBlockBegin(uint8_t *buffer, bool multiBlockWrite) { - // Card wants 8 dummy clock cycles between the write command's response and a data block beginning: - spiBusTransferByte(&sdcard.busdev, 0xFF); + static uint8_t token; - spiBusTransferByte(&sdcard.busdev, multiBlockWrite ? SDCARD_MULTIPLE_BLOCK_WRITE_START_TOKEN : SDCARD_SINGLE_BLOCK_WRITE_START_TOKEN); + token = multiBlockWrite ? SDCARD_MULTIPLE_BLOCK_WRITE_START_TOKEN : SDCARD_SINGLE_BLOCK_WRITE_START_TOKEN; - if (sdcard.useDMAForTx) { -#if defined(USE_HAL_DRIVER) - LL_DMA_InitTypeDef init; + // Note that this does not release the CS at the end of the transaction + static busSegment_t segments[] = { + // Write a single 0xff + {NULL, NULL, 1, false, NULL}, + {&token, NULL, sizeof (token), false, NULL}, + {NULL, NULL, 0, false, NULL}, + {NULL, NULL, 0, false, NULL}, + }; - LL_DMA_StructInit(&init); + segments[2].txData = buffer; + segments[2].len = spiUseDMA(&sdcard.dev) ? SDCARD_BLOCK_SIZE : SDCARD_NON_DMA_CHUNK_SIZE; -#if defined(STM32G4) || defined(STM32H7) - init.PeriphRequest = dmaGetChannel(sdcard.dmaChannel); -#else - init.Channel = dmaGetChannel(sdcard.dmaChannel); -#endif - init.Mode = LL_DMA_MODE_NORMAL; - init.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH; + // Ensure any prior DMA has completed before continuing + spiWaitClaim(&sdcard.dev); - init.PeriphOrM2MSrcAddress = (uint32_t)&SPI_RX_DATA_REGISTER(sdcard.busdev.busdev_u.spi.instance); - init.Priority = LL_DMA_PRIORITY_LOW; - init.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; - init.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE; + spiSequence(&sdcard.dev, &segments[0]); - init.MemoryOrM2MDstAddress = (uint32_t)buffer; - init.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; - init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE; - - init.NbData = SDCARD_BLOCK_SIZE; - - LL_DMA_DeInit(sdcard.dma->dma, sdcard.dma->stream); - LL_DMA_Init(sdcard.dma->dma, sdcard.dma->stream, &init); - -#if defined(STM32G4) - LL_DMA_EnableChannel(sdcard.dma->dma, sdcard.dma->stream); -#else - LL_DMA_EnableStream(sdcard.dma->dma, sdcard.dma->stream); -#endif - - LL_SPI_EnableDMAReq_TX(sdcard.busdev.busdev_u.spi.instance); - -#else - - DMA_InitTypeDef init; - - DMA_StructInit(&init); -#ifdef STM32F4 - init.DMA_Channel = dmaGetChannel(sdcard.dmaChannel); - init.DMA_Memory0BaseAddr = (uint32_t) buffer; - init.DMA_DIR = DMA_DIR_MemoryToPeripheral; -#else - init.DMA_M2M = DMA_M2M_Disable; - init.DMA_MemoryBaseAddr = (uint32_t) buffer; - init.DMA_DIR = DMA_DIR_PeripheralDST; -#endif - init.DMA_PeripheralBaseAddr = (uint32_t)&SPI_RX_DATA_REGISTER(sdcard.busdev.busdev_u.spi.instance); - init.DMA_Priority = DMA_Priority_Low; - init.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - init.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; - - init.DMA_MemoryInc = DMA_MemoryInc_Enable; - init.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; - - init.DMA_BufferSize = SDCARD_BLOCK_SIZE; - init.DMA_Mode = DMA_Mode_Normal; - - xDMA_DeInit(sdcard.dma->ref); - xDMA_Init(sdcard.dma->ref, &init); - - xDMA_Cmd(sdcard.dma->ref, ENABLE); - - SPI_I2S_DMACmd(sdcard.busdev.busdev_u.spi.instance, SPI_I2S_DMAReq_Tx, ENABLE); -#endif - } else { - // Send the first chunk now - spiBusRawTransfer(&sdcard.busdev, buffer, NULL, SDCARD_NON_DMA_CHUNK_SIZE); - } + // Don't block pending completion of SPI access } static bool sdcard_receiveCID(void) @@ -502,9 +541,7 @@ void sdcardSpi_preInit(const sdcardConfig_t *config) */ static void sdcardSpi_init(const sdcardConfig_t *config, const spiPinConfig_t *spiConfig) { -#ifndef USE_DMA_SPEC UNUSED(spiConfig); -#endif sdcard.enabled = config->mode; if (!sdcard.enabled) { @@ -512,32 +549,7 @@ static void sdcardSpi_init(const sdcardConfig_t *config, const spiPinConfig_t *s return; } - SPIDevice spiDevice = SPI_CFG_TO_DEV(config->device); - - spiBusSetInstance(&sdcard.busdev, spiInstanceByDevice(spiDevice)); - - if (config->useDma) { - dmaIdentifier_e dmaIdentifier = DMA_NONE; - -#ifdef USE_DMA_SPEC - const dmaChannelSpec_t *dmaChannelSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_SPI_TX, config->device, spiConfig[spiDevice].txDmaopt); - - if (dmaChannelSpec) { - dmaIdentifier = dmaGetIdentifier(dmaChannelSpec->ref); - sdcard.dmaChannel = dmaChannelSpec->channel; // XXX STM32F3 doesn't have this - } -#else - dmaIdentifier = config->dmaIdentifier; -#endif - - if (dmaIdentifier) { - sdcard.dma = dmaGetDescriptorByIdentifier(dmaIdentifier); - dmaInit(dmaIdentifier, OWNER_SDCARD, 0); - sdcard.useDMAForTx = true; - } else { - sdcard.useDMAForTx = false; - } - } + spiSetBusInstance(&sdcard.dev, config->device, OWNER_SDCARD_CS); IO_t chipSelectIO; if (config->chipSelectTag) { @@ -547,32 +559,38 @@ static void sdcardSpi_init(const sdcardConfig_t *config, const spiPinConfig_t *s } else { chipSelectIO = IO_NONE; } - sdcard.busdev.busdev_u.spi.csnPin = chipSelectIO; + sdcard.dev.busType_u.spi.csnPin = chipSelectIO; + + // Set the clock phase/polarity + spiSetClkPhasePolarity(&sdcard.dev, true); + + // Set the callback argument when calling back to this driver for DMA completion + sdcard.dev.callbackArg = (uint32_t)&sdcard; // Max frequency is initially 400kHz -#ifdef USE_SPI_TRANSACTION - spiBusTransactionInit(&sdcard.busdev, SDCARD_SPI_MODE, spiCalculateDivider(SDCARD_MAX_SPI_INIT_CLK_HZ)); -#else - spiSetDivisor(sdcard.busdev.busdev_u.spi.instance, spiCalculateDivider(SDCARD_MAX_SPI_INIT_CLK_HZ)); -#endif + spiSetClkDivisor(&sdcard.dev, spiCalculateDivider(SDCARD_MAX_SPI_INIT_CLK_HZ)); // SDCard wants 1ms minimum delay after power is applied to it delay(1000); // Transmit at least 74 dummy clock cycles with CS high so the SD card can start up - IOHi(sdcard.busdev.busdev_u.spi.csnPin); - spiBusRawTransfer(&sdcard.busdev, NULL, NULL, SDCARD_INIT_NUM_DUMMY_BYTES); + IOHi(sdcard.dev.busType_u.spi.csnPin); - // Wait for that transmission to finish before we enable the SDCard, so it receives the required number of cycles: - int time = 100000; - while (spiBusIsBusBusy(&sdcard.busdev)) { - if (time-- == 0) { - sdcard.state = SDCARD_STATE_NOT_PRESENT; - sdcard.failureCount++; - return; - } - } + // Note that this does not release the CS at the end of the transaction + busSegment_t segments[] = { + // Write a single 0xff + {NULL, NULL, SDCARD_INIT_NUM_DUMMY_BYTES, false, NULL}, + {NULL, NULL, 0, false, NULL}, + }; + + // Ensure any prior DMA has completed before continuing + spiWaitClaim(&sdcard.dev); + + spiSequence(&sdcard.dev, &segments[0]); + + // Block pending completion of SPI access + spiWait(&sdcard.dev); sdcard.operationStartTime = millis(); sdcard.state = SDCARD_STATE_RESET; @@ -610,12 +628,24 @@ static bool sdcard_isReady(void) */ static sdcardOperationStatus_e sdcard_endWriteBlocks(void) { + uint8_t token = SDCARD_MULTIPLE_BLOCK_WRITE_STOP_TOKEN; sdcard.multiWriteBlocksRemain = 0; - // 8 dummy clocks to guarantee N_WR clocks between the last card response and this token - spiBusTransferByte(&sdcard.busdev, 0xFF); + // Note that this does not release the CS at the end of the transaction + busSegment_t segments[] = { + // 8 dummy clocks to guarantee N_WR clocks between the last card response and this token + {NULL, NULL, 1, false, NULL}, + {&token, NULL, sizeof(token), false, NULL}, + {NULL, NULL, 0, false, NULL}, + }; - spiBusTransferByte(&sdcard.busdev, SDCARD_MULTIPLE_BLOCK_WRITE_STOP_TOKEN); + // Ensure any prior DMA has completed before continuing + spiWaitClaim(&sdcard.dev); + + spiSequence(&sdcard.dev, &segments[0]); + + // Block pending completion of SPI access + spiWait(&sdcard.dev); // Card may choose to raise a busy (non-0xFF) signal after at most N_BR (1 byte) delay if (sdcard_waitForNonIdleByte(1) == 0xFF) { @@ -720,11 +750,7 @@ static bool sdcardSpi_poll(void) // Now we're done with init and we can switch to the full speed clock (<25MHz) -#ifdef USE_SPI_TRANSACTION - spiBusTransactionInit(&sdcard.busdev, SDCARD_SPI_MODE, spiCalculateDivider(SDCARD_MAX_SPI_CLK_HZ)); -#else - spiSetDivisor(sdcard.busdev.busdev_u.spi.instance, spiCalculateDivider(SDCARD_MAX_SPI_CLK_HZ)); -#endif + spiSetClkDivisor(&sdcard.dev, spiCalculateDivider(SDCARD_MAX_SPI_CLK_HZ)); sdcard.multiWriteBlocksRemain = 0; @@ -734,54 +760,11 @@ static bool sdcardSpi_poll(void) break; case SDCARD_STATE_SENDING_WRITE: // Have we finished sending the write yet? - sendComplete = false; + sendComplete = !spiIsBusy(&sdcard.dev); -#if defined(USE_HAL_DRIVER) - if (sdcard.useDMAForTx && DMA_GET_FLAG_STATUS(sdcard.dma, DMA_IT_TCIF)) { - //Clear both flags after transfer - DMA_CLEAR_FLAG(sdcard.dma, DMA_IT_TCIF); - DMA_CLEAR_FLAG(sdcard.dma, DMA_IT_HTIF); - // Drain anything left in the Rx FIFO (we didn't read it during the write) - while (CHECK_SPI_RX_DATA_AVAILABLE(sdcard.busdev.busdev_u.spi.instance)) { - SPI_RX_DATA_REGISTER(sdcard.busdev.busdev_u.spi.instance); - } - - // Wait for the final bit to be transmitted - while (spiBusIsBusBusy(&sdcard.busdev)) { - } - - LL_SPI_DisableDMAReq_TX(sdcard.busdev.busdev_u.spi.instance); - - sendComplete = true; - } -#else -#ifdef STM32F4 - if (sdcard.useDMAForTx && xDMA_GetFlagStatus(sdcard.dma->ref, sdcard.dma->completeFlag) == SET) { - xDMA_ClearFlag(sdcard.dma->ref, sdcard.dma->completeFlag); -#else - if (sdcard.useDMAForTx && DMA_GetFlagStatus(sdcard.dma->completeFlag) == SET) { - DMA_ClearFlag(sdcard.dma->completeFlag); -#endif - - xDMA_Cmd(sdcard.dma->ref, DISABLE); - - // Drain anything left in the Rx FIFO (we didn't read it during the write) - while (SPI_I2S_GetFlagStatus(sdcard.busdev.busdev_u.spi.instance, SPI_I2S_FLAG_RXNE) == SET) { - SPI_RX_DATA_REGISTER(sdcard.busdev.busdev_u.spi.instance); - } - - // Wait for the final bit to be transmitted - while (spiBusIsBusBusy(&sdcard.busdev)) { - } - - SPI_I2S_DMACmd(sdcard.busdev.busdev_u.spi.instance, SPI_I2S_DMAReq_Tx, DISABLE); - - sendComplete = true; - } -#endif - if (!sdcard.useDMAForTx) { + if (!spiUseDMA(&sdcard.dev)) { // Send another chunk - spiBusRawTransfer(&sdcard.busdev, sdcard.pendingOperation.buffer + SDCARD_NON_DMA_CHUNK_SIZE * sdcard.pendingOperation.chunkIndex, NULL, SDCARD_NON_DMA_CHUNK_SIZE); + spiReadWriteBuf(&sdcard.dev, sdcard.pendingOperation.buffer + SDCARD_NON_DMA_CHUNK_SIZE * sdcard.pendingOperation.chunkIndex, NULL, SDCARD_NON_DMA_CHUNK_SIZE); sdcard.pendingOperation.chunkIndex++; diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index 92533f3a5..d6b340fc6 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -273,6 +273,20 @@ void initialiseMemorySections(void) #endif } +#ifdef STM32H7 +void initialiseD2MemorySections(void) +{ + /* Load DMA_DATA variable intializers into D2 RAM */ + extern uint8_t _sdmaram_bss; + extern uint8_t _edmaram_bss; + extern uint8_t _sdmaram_data; + extern uint8_t _edmaram_data; + extern uint8_t _sdmaram_idata; + bzero(&_sdmaram_bss, (size_t) (&_edmaram_bss - &_sdmaram_bss)); + memcpy(&_sdmaram_data, &_sdmaram_idata, (size_t) (&_edmaram_data - &_sdmaram_data)); +} +#endif + static void unusedPinInit(IO_t io) { if (IOGetOwner(io) == OWNER_FREE) { diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index a79428079..aeebeb95b 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -72,6 +72,9 @@ void systemCheckResetReason(void); #endif void initialiseMemorySections(void); +#ifdef STM32H7 +void initialiseD2MemorySections(void); +#endif void enableGPIOPowerUsageAndNoiseReductions(void); // current crystal frequency - 8 or 12MHz diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c index 3e6093052..cb463774b 100644 --- a/src/main/drivers/vtx_rtc6705.c +++ b/src/main/drivers/vtx_rtc6705.c @@ -56,10 +56,10 @@ static IO_t vtxPowerPin = IO_NONE; #endif -static busDevice_t *busdev = NULL; +static extDevice_t *dev = NULL; -#define DISABLE_RTC6705() IOHi(busdev->busdev_u.spi.csnPin) -#define ENABLE_RTC6705() IOLo(busdev->busdev_u.spi.csnPin) +#define DISABLE_RTC6705() IOHi(dev->busType_u.spi.csnPin) +#define ENABLE_RTC6705() IOLo(dev->busType_u.spi.csnPin) #define DP_5G_MASK 0x7000 // b111000000000000 #define PA5G_BS_MASK 0x0E00 // b000111000000000 @@ -94,7 +94,7 @@ static uint32_t reverse32(uint32_t in) */ bool rtc6705IOInit(const vtxIOConfig_t *vtxIOConfig) { - static busDevice_t busInstance; + static extDevice_t devInstance; IO_t csnPin = IOGetByTag(vtxIOConfig->csTag); if (!csnPin) { @@ -110,20 +110,14 @@ bool rtc6705IOInit(const vtxIOConfig_t *vtxIOConfig) IOConfigGPIO(vtxPowerPin, IOCFG_OUT_PP); } - SPI_TypeDef *vtxSpiInstance = spiInstanceByDevice(SPI_CFG_TO_DEV(vtxIOConfig->spiDevice)); - if (vtxSpiInstance) { - busdev = &busInstance; - - busdev->busdev_u.spi.csnPin = csnPin; - IOInit(busdev->busdev_u.spi.csnPin, OWNER_VTX_CS, 0); + if (vtxIOConfig->csTag && spiSetBusInstance(dev, vtxIOConfig->spiDevice, OWNER_VTX_CS)) { + devInstance.busType_u.spi.csnPin = csnPin; + IOInit(devInstance.busType_u.spi.csnPin, OWNER_VTX_CS, 0); DISABLE_RTC6705(); // GPIO bit is enabled so here so the output is not pulled low when the GPIO is set in output mode. // Note: It's critical to ensure that incorrect signals are not sent to the VTX. - IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP); - - busdev->bustype = BUSTYPE_SPI; - spiBusSetInstance(busdev, vtxSpiInstance); + IOConfigGPIO(devInstance.busType_u.spi.csnPin, IOCFG_OUT_PP); return true; #if defined(USE_VTX_RTC6705_SOFTSPI) @@ -142,18 +136,10 @@ bool rtc6705IOInit(const vtxIOConfig_t *vtxIOConfig) */ static void rtc6705Transfer(uint32_t command) { + // Perform bitwise reverse of the command. command = reverse32(command); - ENABLE_RTC6705(); - - spiTransferByte(busdev->busdev_u.spi.instance, (command >> 24) & 0xFF); - spiTransferByte(busdev->busdev_u.spi.instance, (command >> 16) & 0xFF); - spiTransferByte(busdev->busdev_u.spi.instance, (command >> 8) & 0xFF); - spiTransferByte(busdev->busdev_u.spi.instance, (command >> 0) & 0xFF); - - delayMicroseconds(2); - - DISABLE_RTC6705(); + spiReadWriteBuf(dev, (uint8_t *)&command, NULL, sizeof (command)); delayMicroseconds(2); } @@ -165,7 +151,7 @@ static void rtc6705Transfer(uint32_t command) void rtc6705SetFrequency(uint16_t frequency) { #if defined(USE_VTX_RTC6705_SOFTSPI) - if (!busdev) { + if (!dev) { rtc6705SoftSpiSetFrequency(frequency); return; @@ -181,7 +167,7 @@ void rtc6705SetFrequency(uint16_t frequency) val_hex |= (val_a << 5); val_hex |= (val_n << 12); - spiSetDivisor(busdev->busdev_u.spi.instance, spiCalculateDivider(RTC6705_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(dev, spiCalculateDivider(RTC6705_MAX_SPI_CLK_HZ)); rtc6705Transfer(RTC6705_SET_HEAD); delayMicroseconds(10); @@ -192,7 +178,7 @@ void rtc6705SetRFPower(uint8_t rf_power) { rf_power = constrain(rf_power, 1, 2); #if defined(USE_VTX_RTC6705_SOFTSPI) - if (!busdev) { + if (!dev) { rtc6705SoftSpiSetRFPower(rf_power); return; @@ -204,7 +190,7 @@ void rtc6705SetRFPower(uint8_t rf_power) const uint32_t data = rf_power > 1 ? PA_CONTROL_DEFAULT : (PA_CONTROL_DEFAULT | PD_Q5G_MASK) & (~(PA5G_PW_MASK | PA5G_BS_MASK)); val_hex |= data << 5; // 4 address bits and 1 rw bit. - spiSetDivisor(busdev->busdev_u.spi.instance, spiCalculateDivider(RTC6705_MAX_SPI_CLK_HZ)); + spiSetClkDivisor(dev, spiCalculateDivider(RTC6705_MAX_SPI_CLK_HZ)); rtc6705Transfer(val_hex); } diff --git a/src/main/fc/init.c b/src/main/fc/init.c index 900c96cde..16d48d1b8 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -208,40 +208,6 @@ static IO_t busSwitchResetPin = IO_NONE; } #endif -bool requiresSpiLeadingEdge(SPIDevice device) -{ -#if defined(CONFIG_IN_SDCARD) || defined(CONFIG_IN_EXTERNAL_FLASH) -#if !defined(SDCARD_SPI_INSTANCE) && !defined(RX_SPI_INSTANCE) - UNUSED(device); -#endif -#if defined(SDCARD_SPI_INSTANCE) - if (device == spiDeviceByInstance(SDCARD_SPI_INSTANCE)) { - return true; - } -#endif -#if defined(RX_SPI_INSTANCE) - if (device == spiDeviceByInstance(RX_SPI_INSTANCE)) { - return true; - } -#endif -#else -#if !defined(USE_SDCARD) && !defined(USE_RX_SPI) - UNUSED(device); -#endif -#if defined(USE_SDCARD) - if (device == SPI_CFG_TO_DEV(sdcardConfig()->device)) { - return true; - } -#endif -#if defined(USE_RX_SPI) - if (device == SPI_CFG_TO_DEV(rxSpiConfig()->spibus)) { - return true; - } -#endif -#endif // CONFIG_IN_SDCARD || CONFIG_IN_EXTERNAL_FLASH - - return false; -} static void configureSPIAndQuadSPI(void) { @@ -255,22 +221,22 @@ static void configureSPIAndQuadSPI(void) spiPreinit(); #ifdef USE_SPI_DEVICE_1 - spiInit(SPIDEV_1, requiresSpiLeadingEdge(SPIDEV_1)); + spiInit(SPIDEV_1); #endif #ifdef USE_SPI_DEVICE_2 - spiInit(SPIDEV_2, requiresSpiLeadingEdge(SPIDEV_2)); + spiInit(SPIDEV_2); #endif #ifdef USE_SPI_DEVICE_3 - spiInit(SPIDEV_3, requiresSpiLeadingEdge(SPIDEV_3)); + spiInit(SPIDEV_3); #endif #ifdef USE_SPI_DEVICE_4 - spiInit(SPIDEV_4, requiresSpiLeadingEdge(SPIDEV_4)); + spiInit(SPIDEV_4); #endif #ifdef USE_SPI_DEVICE_5 - spiInit(SPIDEV_5, requiresSpiLeadingEdge(SPIDEV_5)); + spiInit(SPIDEV_5); #endif #ifdef USE_SPI_DEVICE_6 - spiInit(SPIDEV_6, requiresSpiLeadingEdge(SPIDEV_6)); + spiInit(SPIDEV_6); #endif #endif // USE_SPI @@ -318,6 +284,11 @@ void init(void) detectHardwareRevision(); #endif +#if defined(USE_TARGET_CONFIG) + // Call once before the config is loaded for any target specific configuration required to support loading the config + targetConfiguration(); +#endif + #ifdef USE_BRUSHED_ESC_AUTODETECT // Opportunistically use the first motor pin of the default configuration for detection. // We are doing this as with some boards, timing seems to be important, and the later detection will fail. @@ -362,10 +333,6 @@ void init(void) pgResetAll(); -#if defined(STM32H7) && defined(USE_SDCARD_SDIO) // H7 only for now, likely should be applied to F4/F7 too - sdioPinConfigure(); - SDIO_GPIO_Init(); -#endif #ifdef USE_SDCARD_SPI configureSPIAndQuadSPI(); initFlags |= SPI_AND_QSPI_INIT_ATTEMPTED; @@ -656,6 +623,10 @@ void init(void) if (blackboxConfig()->device == BLACKBOX_DEVICE_FLASH) { emfat_init_files(); } +#endif + // There's no more initialisation to be done, so enable DMA where possible for SPI +#ifdef USE_SPI + spiInitBusDMA(); #endif if (mscStart() == 0) { mscWaitForButton(); @@ -1020,6 +991,11 @@ void init(void) motorEnable(); #endif +#ifdef USE_SPI + // Attempt to enable DMA on all SPI busses + spiInitBusDMA(); +#endif + swdPinsInit(); unusedPinsInit(); diff --git a/src/main/io/asyncfatfs/asyncfatfs.c b/src/main/io/asyncfatfs/asyncfatfs.c index bc7bcc8f1..759448849 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.c +++ b/src/main/io/asyncfatfs/asyncfatfs.c @@ -514,7 +514,7 @@ typedef struct afatfs_t { } afatfs_t; #ifdef STM32H7 -static DMA_RW_AXI uint8_t afatfs_cache[AFATFS_SECTOR_SIZE * AFATFS_NUM_CACHE_SECTORS] __attribute__((aligned(32))); +static DMA_DATA_ZERO_INIT uint8_t afatfs_cache[AFATFS_SECTOR_SIZE * AFATFS_NUM_CACHE_SECTORS] __attribute__((aligned(32))); #endif static afatfs_t afatfs; diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 59699b5ac..9765edb7b 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -81,7 +81,7 @@ #define DISPLAY_UPDATE_FREQUENCY (MICROSECONDS_IN_A_SECOND / 5) #define PAGE_CYCLE_FREQUENCY (MICROSECONDS_IN_A_SECOND * 5) -static busDevice_t *bus; +static extDevice_t *dev; static uint32_t nextDisplayUpdateAt = 0; static bool dashboardPresent = false; @@ -128,12 +128,12 @@ static pageState_t pageState; static void resetDisplay(void) { - dashboardPresent = ug2864hsweg01InitI2C(bus); + dashboardPresent = ug2864hsweg01InitI2C(dev); } void LCDprint(uint8_t i) { - i2c_OLED_send_char(bus, i); + i2c_OLED_send_char(dev, i); } static void padLineBuffer(void) @@ -182,8 +182,8 @@ static void fillScreenWithCharacters(void) { for (uint8_t row = 0; row < SCREEN_CHARACTER_ROW_COUNT; row++) { for (uint8_t column = 0; column < SCREEN_CHARACTER_COLUMN_COUNT; column++) { - i2c_OLED_set_xy(bus, column, row); - i2c_OLED_send_char(bus, 'A' + column); + i2c_OLED_set_xy(dev, column, row); + i2c_OLED_send_char(dev, 'A' + column); } } } @@ -193,22 +193,22 @@ static void fillScreenWithCharacters(void) static void updateTicker(void) { static uint8_t tickerIndex = 0; - i2c_OLED_set_xy(bus, SCREEN_CHARACTER_COLUMN_COUNT - 1, 0); - i2c_OLED_send_char(bus, tickerCharacters[tickerIndex]); + i2c_OLED_set_xy(dev, SCREEN_CHARACTER_COLUMN_COUNT - 1, 0); + i2c_OLED_send_char(dev, tickerCharacters[tickerIndex]); tickerIndex++; tickerIndex = tickerIndex % TICKER_CHARACTER_COUNT; } static void updateRxStatus(void) { - i2c_OLED_set_xy(bus, SCREEN_CHARACTER_COLUMN_COUNT - 2, 0); + i2c_OLED_set_xy(dev, SCREEN_CHARACTER_COLUMN_COUNT - 2, 0); char rxStatus = '!'; if (rxIsReceivingSignal()) { rxStatus = 'r'; } if (rxAreFlightChannelsValid()) { rxStatus = 'R'; } - i2c_OLED_send_char(bus, rxStatus); + i2c_OLED_send_char(dev, rxStatus); } static void updateFailsafeStatus(void) @@ -237,19 +237,19 @@ static void updateFailsafeStatus(void) failsafeIndicator = 'G'; break; } - i2c_OLED_set_xy(bus, SCREEN_CHARACTER_COLUMN_COUNT - 3, 0); - i2c_OLED_send_char(bus, failsafeIndicator); + i2c_OLED_set_xy(dev, SCREEN_CHARACTER_COLUMN_COUNT - 3, 0); + i2c_OLED_send_char(dev, failsafeIndicator); } static void showTitle(void) { - i2c_OLED_set_line(bus, 0); - i2c_OLED_send_string(bus, pageState.page->title); + i2c_OLED_set_line(dev, 0); + i2c_OLED_send_string(dev, pageState.page->title); } static void handlePageChange(void) { - i2c_OLED_clear_display_quick(bus); + i2c_OLED_clear_display_quick(dev); showTitle(); } @@ -265,7 +265,7 @@ static void drawRxChannel(uint8_t channelIndex, uint8_t width) static void showRxPage(void) { for (int channelIndex = 0; channelIndex < rxRuntimeState.channelCount && channelIndex < RX_CHANNELS_PER_PAGE_COUNT; channelIndex += 2) { - i2c_OLED_set_line(bus, (channelIndex / 2) + PAGE_TITLE_LINE_COUNT); + i2c_OLED_set_line(dev, (channelIndex / 2) + PAGE_TITLE_LINE_COUNT); drawRxChannel(channelIndex, HALF_SCREEN_CHARACTER_COLUMN_COUNT); @@ -286,11 +286,11 @@ static void showWelcomePage(void) uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; tfp_sprintf(lineBuffer, "v%s (%s)", FC_VERSION_STRING, shortGitRevision); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, targetName); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, targetName); } static void showArmedPage(void) @@ -302,8 +302,8 @@ static void showProfilePage(void) uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; tfp_sprintf(lineBuffer, "Profile: %d", getCurrentPidProfileIndex()); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); static const char* const axisTitles[3] = {"ROL", "PIT", "YAW"}; const pidProfile_t *pidProfile = currentPidProfile; @@ -315,8 +315,8 @@ static void showProfilePage(void) pidProfile->pid[axis].D ); padLineBuffer(); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); } } @@ -326,15 +326,15 @@ static void showRateProfilePage(void) const uint8_t currentRateProfileIndex = getCurrentControlRateProfileIndex(); tfp_sprintf(lineBuffer, "Rate profile: %d", currentRateProfileIndex); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); const controlRateConfig_t *controlRateConfig = controlRateProfiles(currentRateProfileIndex); tfp_sprintf(lineBuffer, " R P Y"); padLineBuffer(); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); tfp_sprintf(lineBuffer, "RcRate %3d %3d %3d", controlRateConfig->rcRates[FD_ROLL], @@ -342,8 +342,8 @@ static void showRateProfilePage(void) controlRateConfig->rcRates[FD_YAW] ); padLineBuffer(); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); tfp_sprintf(lineBuffer, "Super %3d %3d %3d", controlRateConfig->rates[FD_ROLL], @@ -351,8 +351,8 @@ static void showRateProfilePage(void) controlRateConfig->rates[FD_YAW] ); padLineBuffer(); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); tfp_sprintf(lineBuffer, "Expo %3d %3d %3d", controlRateConfig->rcExpo[FD_ROLL], @@ -360,8 +360,8 @@ static void showRateProfilePage(void) controlRateConfig->rcExpo[FD_YAW] ); padLineBuffer(); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); } #define SATELLITE_COUNT (sizeof(GPS_svinfo_cno) / sizeof(GPS_svinfo_cno[0])) @@ -385,64 +385,64 @@ static void showGpsPage(void) gpsTicker = gpsTicker % TICKER_CHARACTER_COUNT; } - i2c_OLED_set_xy(bus, 0, rowIndex); - i2c_OLED_send_char(bus, tickerCharacters[gpsTicker]); + i2c_OLED_set_xy(dev, 0, rowIndex); + i2c_OLED_send_char(dev, tickerCharacters[gpsTicker]); - i2c_OLED_set_xy(bus, MAX(0, (uint8_t)SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++); + i2c_OLED_set_xy(dev, MAX(0, (uint8_t)SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++); uint32_t index; for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; index++) { uint8_t bargraphOffset = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1); bargraphOffset = MIN(bargraphOffset, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1); - i2c_OLED_send_char(bus, VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset); + i2c_OLED_send_char(dev, VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset); } char fixChar = STATE(GPS_FIX) ? 'Y' : 'N'; tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", gpsSol.numSat, fixChar); padLineBuffer(); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); tfp_sprintf(lineBuffer, "La/Lo: %d/%d", gpsSol.llh.lat / GPS_DEGREES_DIVIDER, gpsSol.llh.lon / GPS_DEGREES_DIVIDER); padLineBuffer(); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); tfp_sprintf(lineBuffer, "Spd: %d", gpsSol.groundSpeed); padHalfLineBuffer(); - i2c_OLED_set_line(bus, rowIndex); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex); + i2c_OLED_send_string(dev, lineBuffer); tfp_sprintf(lineBuffer, "GC: %d", gpsSol.groundCourse); padHalfLineBuffer(); - i2c_OLED_set_xy(bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_xy(dev, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); tfp_sprintf(lineBuffer, "RX: %d", GPS_packetCount); padHalfLineBuffer(); - i2c_OLED_set_line(bus, rowIndex); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex); + i2c_OLED_send_string(dev, lineBuffer); tfp_sprintf(lineBuffer, "ERRs: %d", gpsData.errors); padHalfLineBuffer(); - i2c_OLED_set_xy(bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_xy(dev, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); tfp_sprintf(lineBuffer, "Dt: %d", gpsData.lastMessage - gpsData.lastLastMessage); padHalfLineBuffer(); - i2c_OLED_set_line(bus, rowIndex); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex); + i2c_OLED_send_string(dev, lineBuffer); tfp_sprintf(lineBuffer, "TOs: %d", gpsData.timeouts); padHalfLineBuffer(); - i2c_OLED_set_xy(bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_xy(dev, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); strncpy(lineBuffer, gpsPacketLog, GPS_PACKET_LOG_ENTRY_COUNT); padHalfLineBuffer(); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); } #endif @@ -453,11 +453,11 @@ static void showBatteryPage(void) if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) { tfp_sprintf(lineBuffer, "Volts: %d.%02d Cells: %d", getBatteryVoltage() / 100, getBatteryVoltage() % 100, getBatteryCellCount()); padLineBuffer(); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); uint8_t batteryPercentage = calculateBatteryPercentageRemaining(); - i2c_OLED_set_line(bus, rowIndex++); + i2c_OLED_set_line(dev, rowIndex++); drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, batteryPercentage); } @@ -468,11 +468,11 @@ static void showBatteryPage(void) // Amp: DDD.D mAh: DDDDD tfp_sprintf(lineBuffer, "Amp: %d.%d mAh: %d", amperage / 100, (amperage % 100) / 10, getMAhDrawn()); padLineBuffer(); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); uint8_t capacityPercentage = calculateBatteryPercentageRemaining(); - i2c_OLED_set_line(bus, rowIndex++); + i2c_OLED_set_line(dev, rowIndex++); drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, capacityPercentage); } } @@ -482,38 +482,38 @@ static void showSensorsPage(void) uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; static const char *format = "%s %5d %5d %5d"; - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, " X Y Z"); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, " X Y Z"); #if defined(USE_ACC) if (sensors(SENSOR_ACC)) { tfp_sprintf(lineBuffer, format, "ACC", lrintf(acc.accADC[X]), lrintf(acc.accADC[Y]), lrintf(acc.accADC[Z])); padLineBuffer(); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); } #endif if (sensors(SENSOR_GYRO)) { tfp_sprintf(lineBuffer, format, "GYR", lrintf(gyro.gyroADCf[X]), lrintf(gyro.gyroADCf[Y]), lrintf(gyro.gyroADCf[Z])); padLineBuffer(); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); } #ifdef USE_MAG if (sensors(SENSOR_MAG)) { tfp_sprintf(lineBuffer, format, "MAG", lrintf(mag.magADC[X]), lrintf(mag.magADC[Y]), lrintf(mag.magADC[Z])); padLineBuffer(); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); } #endif tfp_sprintf(lineBuffer, format, "I&H", attitude.values.roll, attitude.values.pitch, DECIDEGREES_TO_DEGREES(attitude.values.yaw)); padLineBuffer(); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); /* uint8_t length; @@ -526,8 +526,8 @@ static void showSensorsPage(void) } ftoa(EstG.A[Y], lineBuffer + length); padLineBuffer(); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); ftoa(EstG.A[Z], lineBuffer); length = strlen(lineBuffer); @@ -537,8 +537,8 @@ static void showSensorsPage(void) } ftoa(smallAngle, lineBuffer + length); padLineBuffer(); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); */ } @@ -549,8 +549,8 @@ static void showTasksPage(void) uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; static const char *format = "%2d%6d%5d%4d%4d"; - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, "Task max avg mx% av%"); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, "Task max avg mx% av%"); taskInfo_t taskInfo; for (taskId_e taskId = 0; taskId < TASK_COUNT; ++taskId) { getTaskInfo(taskId, &taskInfo); @@ -560,8 +560,8 @@ static void showTasksPage(void) const int averageLoad = (taskInfo.averageExecutionTimeUs * taskFrequency + 5000) / 10000; tfp_sprintf(lineBuffer, format, taskId, taskInfo.maxExecutionTimeUs, taskInfo.averageExecutionTimeUs, maxLoad, averageLoad); padLineBuffer(); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); if (rowIndex > SCREEN_CHARACTER_ROW_COUNT) { break; } @@ -594,8 +594,8 @@ static void showBBPage(void) } padLineBuffer(); - i2c_OLED_set_line(bus, rowIndex++); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex++); + i2c_OLED_send_string(dev, lineBuffer); } #endif @@ -606,8 +606,8 @@ static void showDebugPage(void) for (int rowIndex = 0; rowIndex < 4; rowIndex++) { tfp_sprintf(lineBuffer, "%d = %5d", rowIndex, debug[rowIndex]); padLineBuffer(); - i2c_OLED_set_line(bus, rowIndex + PAGE_TITLE_LINE_COUNT); - i2c_OLED_send_string(bus, lineBuffer); + i2c_OLED_set_line(dev, rowIndex + PAGE_TITLE_LINE_COUNT); + i2c_OLED_send_string(dev, lineBuffer); } } #endif @@ -722,16 +722,21 @@ void dashboardUpdate(timeUs_t currentTimeUs) void dashboardInit(void) { + static extDevice_t dashBoardDev; + // TODO Use bus singleton static busDevice_t dashBoardBus; - dashBoardBus.busdev_u.i2c.device = I2C_CFG_TO_DEV(dashboardConfig()->device); - dashBoardBus.busdev_u.i2c.address = dashboardConfig()->address; - bus = &dashBoardBus; + + dashBoardDev.bus = &dashBoardBus; + + dashBoardBus.busType_u.i2c.device = I2C_CFG_TO_DEV(dashboardConfig()->device); + dashBoardDev.busType_u.i2c.address = dashboardConfig()->address; + dev = &dashBoardDev; delay(200); resetDisplay(); delay(200); - displayPort = displayPortOledInit(bus); + displayPort = displayPortOledInit(dev); #if defined(USE_CMS) if (dashboardPresent) { cmsDisplayPortRegister(displayPort); diff --git a/src/main/io/flashfs.c b/src/main/io/flashfs.c index 6599d6051..235eba7c5 100644 --- a/src/main/io/flashfs.c +++ b/src/main/io/flashfs.c @@ -47,7 +47,7 @@ static const flashPartition_t *flashPartition = NULL; static const flashGeometry_t *flashGeometry = NULL; static uint32_t flashfsSize = 0; -static uint8_t flashWriteBuffer[FLASHFS_WRITE_BUFFER_SIZE]; +static DMA_DATA_ZERO_INIT uint8_t flashWriteBuffer[FLASHFS_WRITE_BUFFER_SIZE]; /* The position of our head and tail in the circular flash write buffer. * @@ -538,7 +538,7 @@ int flashfsIdentifyStartOfFreeSpace(void) STATIC_ASSERT(FREE_BLOCK_SIZE >= FLASH_MAX_PAGE_SIZE, FREE_BLOCK_SIZE_too_small); - union { + STATIC_DMA_DATA_AUTO union { uint8_t bytes[FREE_BLOCK_TEST_SIZE_BYTES]; uint32_t ints[FREE_BLOCK_TEST_SIZE_INTS]; } testBuffer; diff --git a/src/main/msc/emfat_file.c b/src/main/msc/emfat_file.c index c9dfc4bb1..80b569f28 100644 --- a/src/main/msc/emfat_file.c +++ b/src/main/msc/emfat_file.c @@ -311,12 +311,12 @@ static void emfat_add_log(emfat_entry_t *entry, int number, uint32_t offset, uin static int emfat_find_log(emfat_entry_t *entry, int maxCount, int flashfsUsedSpace) { + static uint8_t buffer[HDR_BUF_SIZE]; int lastOffset = 0; int currOffset = 0; int buffOffset; int hdrOffset; int fileNumber = 0; - uint8_t buffer[HDR_BUF_SIZE]; int logCount = 0; char *logHeader = "H Product:Blackbox"; int lenLogHeader = strlen(logHeader); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 232684dbe..883317ab0 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -521,7 +521,8 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uin #ifdef USE_HUFFMAN // compress in 256-byte chunks const uint16_t READ_BUFFER_SIZE = 256; - uint8_t readBuffer[READ_BUFFER_SIZE]; + // This may be DMAable, so make it cache aligned + __attribute__ ((aligned(32))) uint8_t readBuffer[READ_BUFFER_SIZE]; huffmanState_t state = { .bytesWritten = 0, diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c index 6909304ab..b4b010e35 100644 --- a/src/main/osd/osd.c +++ b/src/main/osd/osd.c @@ -1056,12 +1056,10 @@ void osdUpdate(timeUs_t currentTimeUs) showVisualBeeper = true; } -#ifdef MAX7456_DMA_CHANNEL_TX // don't touch buffers if DMA transaction is in progress if (displayIsTransferInProgress(osdDisplayPort)) { return; } -#endif // MAX7456_DMA_CHANNEL_TX #ifdef USE_SLOW_MSP_DISPLAYPORT_RATE_WHEN_UNARMED static uint32_t idlecounter = 0; diff --git a/src/main/pg/gyrodev.c b/src/main/pg/gyrodev.c index ca1894415..87510d8d6 100644 --- a/src/main/pg/gyrodev.c +++ b/src/main/pg/gyrodev.c @@ -49,7 +49,7 @@ static void gyroResetCommonDeviceConfig(gyroDeviceConfig_t *devconf, ioTag_t ext #ifdef USE_SPI_GYRO static void gyroResetSpiDeviceConfig(gyroDeviceConfig_t *devconf, SPI_TypeDef *instance, ioTag_t csnTag, ioTag_t extiTag, uint8_t alignment, sensorAlignment_t customAlignment) { - devconf->bustype = BUSTYPE_SPI; + devconf->busType = BUS_TYPE_SPI; devconf->spiBus = SPI_DEV_TO_CFG(spiDeviceByInstance(instance)); devconf->csnTag = csnTag; gyroResetCommonDeviceConfig(devconf, extiTag, alignment, customAlignment); @@ -59,7 +59,7 @@ static void gyroResetSpiDeviceConfig(gyroDeviceConfig_t *devconf, SPI_TypeDef *i #if defined(USE_I2C_GYRO) && !defined(USE_MULTI_GYRO) static void gyroResetI2cDeviceConfig(gyroDeviceConfig_t *devconf, I2CDevice i2cbus, ioTag_t extiTag, uint8_t alignment, sensorAlignment_t customAlignment) { - devconf->bustype = BUSTYPE_I2C; + devconf->busType = BUS_TYPE_I2C; devconf->i2cBus = I2C_DEV_TO_CFG(i2cbus); devconf->i2cAddress = GYRO_I2C_ADDRESS; gyroResetCommonDeviceConfig(devconf, extiTag, alignment, customAlignment); @@ -101,6 +101,6 @@ void pgResetFn_gyroDeviceConfig(gyroDeviceConfig_t *devconf) // Special treatment for very rare F3 targets with variants having either I2C or SPI acc/gyro chip; mark it for run time detection. #if defined(USE_SPI_GYRO) && defined(USE_I2C_GYRO) - devconf[0].bustype = BUSTYPE_GYRO_AUTO; + devconf[0].busType = BUS_TYPE_GYRO_AUTO; #endif } diff --git a/src/main/pg/gyrodev.h b/src/main/pg/gyrodev.h index d2937f81b..cba8dc192 100644 --- a/src/main/pg/gyrodev.h +++ b/src/main/pg/gyrodev.h @@ -29,7 +29,7 @@ typedef struct gyroDeviceConfig_s { int8_t index; - uint8_t bustype; + uint8_t busType; uint8_t spiBus; ioTag_t csnTag; uint8_t i2cBus; diff --git a/src/main/pg/rcdevice.c b/src/main/pg/rcdevice.c index 78fed2c8d..57177c091 100644 --- a/src/main/pg/rcdevice.c +++ b/src/main/pg/rcdevice.c @@ -31,4 +31,4 @@ void pgResetFn_rcdeviceConfig(rcdeviceConfig_t *rcdeviceConfig) rcdeviceConfig->initDeviceAttemptInterval = 1000; rcdeviceConfig->feature = 0; rcdeviceConfig->protocolVersion = 0; -} \ No newline at end of file +} diff --git a/src/main/pg/rcdevice.h b/src/main/pg/rcdevice.h index b9b78b942..65094be89 100644 --- a/src/main/pg/rcdevice.h +++ b/src/main/pg/rcdevice.h @@ -32,4 +32,4 @@ typedef struct rcdeviceConfig_s { uint8_t protocolVersion; } rcdeviceConfig_t; -PG_DECLARE(rcdeviceConfig_t, rcdeviceConfig); \ No newline at end of file +PG_DECLARE(rcdeviceConfig_t, rcdeviceConfig); diff --git a/src/main/pg/sdcard.c b/src/main/pg/sdcard.c index 6ecc8e826..8a0ec3218 100644 --- a/src/main/pg/sdcard.c +++ b/src/main/pg/sdcard.c @@ -43,7 +43,6 @@ void pgResetFn_sdcardConfig(sdcardConfig_t *config) // We can safely handle SPI and SDIO cases separately on custom targets, as these are exclusive per target. // On generic targets, SPI has precedence over SDIO; SDIO must be post-flash configured. - config->useDma = false; config->device = SPI_DEV_TO_CFG(SPIINVALID); #ifdef CONFIG_IN_SDCARD @@ -68,15 +67,5 @@ void pgResetFn_sdcardConfig(sdcardConfig_t *config) config->mode = SDCARD_MODE_SPI; } #endif - -#ifndef USE_DMA_SPEC -#ifdef USE_SDCARD_SPI -#if defined(SDCARD_DMA_STREAM_TX_FULL) - config->dmaIdentifier = (uint8_t)dmaGetIdentifier(SDCARD_DMA_STREAM_TX_FULL); -#elif defined(SDCARD_DMA_CHANNEL_TX) - config->dmaIdentifier = (uint8_t)dmaGetIdentifier(SDCARD_DMA_CHANNEL_TX); -#endif -#endif -#endif // !USE_DMA_SPEC } #endif diff --git a/src/main/pg/sdcard.h b/src/main/pg/sdcard.h index 99fd59f12..51e55f7b8 100644 --- a/src/main/pg/sdcard.h +++ b/src/main/pg/sdcard.h @@ -30,15 +30,10 @@ typedef enum { } sdcardMode_e; typedef struct sdcardConfig_s { - uint8_t useDma; int8_t device; ioTag_t cardDetectTag; ioTag_t chipSelectTag; uint8_t cardDetectInverted; -#ifndef USE_DMA_SPEC - uint8_t dmaIdentifier; - uint8_t dmaChannel; -#endif sdcardMode_e mode; } sdcardConfig_t; diff --git a/src/main/rx/cc2500_frsky_shared.c b/src/main/rx/cc2500_frsky_shared.c index adee361a4..1fa8e2f39 100644 --- a/src/main/rx/cc2500_frsky_shared.c +++ b/src/main/rx/cc2500_frsky_shared.c @@ -155,6 +155,8 @@ const cc2500RegisterConfigElement_t cc2500FrskyXLbtV2Config[] = }; static void initialise() { + rxSpiStartupSpeed(); + cc2500Reset(); cc2500ApplyRegisterConfig(cc2500FrskyBaseConfig, sizeof(cc2500FrskyBaseConfig)); @@ -197,6 +199,8 @@ static void initialise() { calData[c][1] = cc2500ReadReg(CC2500_24_FSCAL2); calData[c][2] = cc2500ReadReg(CC2500_25_FSCAL1); } + + rxSpiNormalSpeed(); } void initialiseData(bool inBindState) diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index becb5211d..86a636bb0 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -174,6 +174,7 @@ extern linkQualitySource_e linkQualitySource; extern rxRuntimeState_t rxRuntimeState; //!!TODO remove this extern, only needed once for channelCount void rxInit(void); +void rxProcessPending(bool state); bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs); bool rxIsReceivingSignal(void); bool rxAreFlightChannelsValid(void); diff --git a/src/main/rx/rx_spi_common.c b/src/main/rx/rx_spi_common.c index 7a89e63b9..808fa1699 100644 --- a/src/main/rx/rx_spi_common.c +++ b/src/main/rx/rx_spi_common.c @@ -27,9 +27,11 @@ #include "drivers/io.h" #include "drivers/time.h" -#include "rx/rx_spi_common.h" + #include "rx/rx_spi.h" +#include "rx_spi_common.h" + static IO_t ledPin; static bool ledInversion = false; diff --git a/src/main/sensors/acceleration_init.c b/src/main/sensors/acceleration_init.c index dceefdc54..81a8c6e91 100644 --- a/src/main/sensors/acceleration_init.c +++ b/src/main/sensors/acceleration_init.c @@ -352,7 +352,7 @@ bool accInit(uint16_t accSampleRateHz) { memset(&acc, 0, sizeof(acc)); // copy over the common gyro mpu settings - acc.dev.bus = *gyroSensorBus(); + acc.dev.gyro = gyroActiveDev(); acc.dev.mpuDetectionResult = *gyroMpuDetectionResult(); acc.dev.acc_high_fsr = accelerometerConfig()->acc_high_fsr; diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 0cfb82ae1..0dcc17513 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -43,6 +43,7 @@ #include "drivers/barometer/barometer_ms5611.h" #include "drivers/barometer/barometer_lps.h" #include "drivers/bus.h" +#include "drivers/bus_i2c_busdev.h" #include "drivers/bus_spi.h" #include "drivers/io.h" #include "drivers/time.h" @@ -113,21 +114,21 @@ void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig) #endif #if defined(DEFAULT_BARO_SPI_BMP388) || defined(DEFAULT_BARO_SPI_BMP280) || defined(DEFAULT_BARO_SPI_MS5611) || defined(DEFAULT_BARO_SPI_QMP6988) || defined(DEFAULT_BARO_SPI_LPS) || defined(DEFAULT_BARO_SPI_DPS310) - barometerConfig->baro_bustype = BUSTYPE_SPI; + barometerConfig->baro_busType = BUS_TYPE_SPI; barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(spiDeviceByInstance(BARO_SPI_INSTANCE)); barometerConfig->baro_spi_csn = IO_TAG(BARO_CS_PIN); barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(I2CINVALID); barometerConfig->baro_i2c_address = 0; #elif defined(DEFAULT_BARO_MS5611) || defined(DEFAULT_BARO_BMP388) || defined(DEFAULT_BARO_BMP280) || defined(DEFAULT_BARO_BMP085) ||defined(DEFAULT_BARO_QMP6988) || defined(DEFAULT_BARO_DPS310) // All I2C devices shares a default config with address = 0 (per device default) - barometerConfig->baro_bustype = BUSTYPE_I2C; + barometerConfig->baro_busType = BUS_TYPE_I2C; barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(BARO_I2C_INSTANCE); barometerConfig->baro_i2c_address = 0; barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(SPIINVALID); barometerConfig->baro_spi_csn = IO_TAG_NONE; #else barometerConfig->baro_hardware = BARO_NONE; - barometerConfig->baro_bustype = BUSTYPE_NONE; + barometerConfig->baro_busType = BUS_TYPE_NONE; barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(I2CINVALID); barometerConfig->baro_i2c_address = 0; barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(SPIINVALID); @@ -154,14 +155,16 @@ static bool baroReady = false; void baroPreInit(void) { #ifdef USE_SPI - if (barometerConfig()->baro_bustype == BUSTYPE_SPI) { + if (barometerConfig()->baro_busType == BUS_TYPE_SPI) { spiPreinitRegister(barometerConfig()->baro_spi_csn, IOCFG_IPU, 1); } #endif } -bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) +bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse) { + extDevice_t *dev = &baroDev->dev; + // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function baroSensor_e baroHardware = baroHardwareToUse; @@ -170,26 +173,22 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) UNUSED(dev); #endif - switch (barometerConfig()->baro_bustype) { + switch (barometerConfig()->baro_busType) { #ifdef USE_I2C - case BUSTYPE_I2C: - dev->busdev.bustype = BUSTYPE_I2C; - dev->busdev.busdev_u.i2c.device = I2C_CFG_TO_DEV(barometerConfig()->baro_i2c_device); - dev->busdev.busdev_u.i2c.address = barometerConfig()->baro_i2c_address; + case BUS_TYPE_I2C: + i2cBusSetInstance(dev, barometerConfig()->baro_i2c_device); + dev->busType_u.i2c.address = barometerConfig()->baro_i2c_address; break; #endif #ifdef USE_SPI - case BUSTYPE_SPI: + case BUS_TYPE_SPI: { - SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(barometerConfig()->baro_spi_device)); - if (!instance) { + if (!spiSetBusInstance(dev, barometerConfig()->baro_spi_device, OWNER_BARO_CS)) { return false; } - dev->busdev.bustype = BUSTYPE_SPI; - spiBusSetInstance(&dev->busdev, instance); - dev->busdev.busdev_u.spi.csnPin = IOGetByTag(barometerConfig()->baro_spi_csn); + dev->busType_u.spi.csnPin = IOGetByTag(barometerConfig()->baro_spi_csn); } break; #endif @@ -211,7 +210,7 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) static const bmp085Config_t *bmp085Config = &defaultBMP085Config; - if (bmp085Detect(bmp085Config, dev)) { + if (bmp085Detect(bmp085Config, baroDev)) { baroHardware = BARO_BMP085; break; } @@ -221,7 +220,7 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) case BARO_MS5611: #if defined(USE_BARO_MS5611) || defined(USE_BARO_SPI_MS5611) - if (ms5611Detect(dev)) { + if (ms5611Detect(baroDev)) { baroHardware = BARO_MS5611; break; } @@ -230,7 +229,7 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) case BARO_LPS: #if defined(USE_BARO_SPI_LPS) - if (lpsDetect(dev)) { + if (lpsDetect(baroDev)) { baroHardware = BARO_LPS; break; } @@ -240,7 +239,7 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) case BARO_DPS310: #if defined(USE_BARO_DPS310) || defined(USE_BARO_SPI_DPS310) { - if (baroDPS310Detect(dev)) { + if (baroDPS310Detect(baroDev)) { baroHardware = BARO_DPS310; break; } @@ -257,7 +256,7 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) static const bmp388Config_t *bmp388Config = &defaultBMP388Config; - if (bmp388Detect(bmp388Config, dev)) { + if (bmp388Detect(bmp388Config, baroDev)) { baroHardware = BARO_BMP388; break; } @@ -267,7 +266,7 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) case BARO_BMP280: #if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280) - if (bmp280Detect(dev)) { + if (bmp280Detect(baroDev)) { baroHardware = BARO_BMP280; break; } @@ -276,7 +275,7 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) case BARO_QMP6988: #if defined(USE_BARO_QMP6988) || defined(USE_BARO_SPI_QMP6988) - if (qmp6988Detect(dev)) { + if (qmp6988Detect(baroDev)) { baroHardware = BARO_QMP6988; break; } @@ -390,7 +389,7 @@ uint32_t baroUpdate(void) // Tell the scheduler to ignore how long this task takes unless the pressure is being read // as that takes the longest if (state != BAROMETER_NEEDS_PRESSURE_READ) { - ignoreTaskTime(); + ignoreTaskTime(); } switch (state) { diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 585e7d57c..e4b828178 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -38,7 +38,7 @@ typedef enum { #define BARO_SAMPLE_COUNT_MAX 48 typedef struct barometerConfig_s { - uint8_t baro_bustype; + uint8_t baro_busType; uint8_t baro_spi_device; ioTag_t baro_spi_csn; // Also used as XCLR (positive logic) for BMP085 uint8_t baro_i2c_device; diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 1b8f1c79d..f3a44e9ed 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -32,6 +32,7 @@ #include "drivers/bus.h" #include "drivers/bus_i2c.h" +#include "drivers/bus_i2c_busdev.h" #include "drivers/bus_spi.h" #include "drivers/compass/compass.h" #include "drivers/compass/compass_ak8975.h" @@ -63,7 +64,7 @@ static flightDynamicsTrims_t magZeroTempMin; static flightDynamicsTrims_t magZeroTempMax; magDev_t magDev; -mag_t mag; // mag access functions +mag_t mag; PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 3); @@ -80,26 +81,26 @@ void pgResetFn_compassConfig(compassConfig_t *compassConfig) // 3. Slave I2C device on SPI gyro #if defined(USE_SPI) && (defined(USE_MAG_SPI_HMC5883) || defined(USE_MAG_SPI_AK8963)) - compassConfig->mag_bustype = BUSTYPE_SPI; + compassConfig->mag_bustype = BUS_TYPE_SPI; compassConfig->mag_spi_device = SPI_DEV_TO_CFG(spiDeviceByInstance(MAG_SPI_INSTANCE)); compassConfig->mag_spi_csn = IO_TAG(MAG_CS_PIN); compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID); compassConfig->mag_i2c_address = 0; #elif defined(USE_MAG_HMC5883) || defined(USE_MAG_QMC5883) || defined(USE_MAG_AK8975) || (defined(USE_MAG_AK8963) && !(defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))) - compassConfig->mag_bustype = BUSTYPE_I2C; + compassConfig->mag_bustype = BUS_TYPE_I2C; compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(MAG_I2C_INSTANCE); compassConfig->mag_i2c_address = 0; compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID); compassConfig->mag_spi_csn = IO_TAG_NONE; #elif defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) - compassConfig->mag_bustype = BUSTYPE_MPU_SLAVE; + compassConfig->mag_bustype = BUS_TYPE_MPU_SLAVE; compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID); compassConfig->mag_i2c_address = 0; compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID); compassConfig->mag_spi_csn = IO_TAG_NONE; #else compassConfig->mag_hardware = MAG_NONE; - compassConfig->mag_bustype = BUSTYPE_NONE; + compassConfig->mag_bustype = BUS_TYPE_NONE; compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID); compassConfig->mag_i2c_address = 0; compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID); @@ -114,56 +115,56 @@ static uint8_t magInit = 0; void compassPreInit(void) { #ifdef USE_SPI - if (compassConfig()->mag_bustype == BUSTYPE_SPI) { + if (compassConfig()->mag_bustype == BUS_TYPE_SPI) { spiPreinitRegister(compassConfig()->mag_spi_csn, IOCFG_IPU, 1); } #endif } #if !defined(SIMULATOR_BUILD) -bool compassDetect(magDev_t *dev, uint8_t *alignment) +bool compassDetect(magDev_t *magDev, uint8_t *alignment) { *alignment = ALIGN_DEFAULT; // may be overridden if target specifies MAG_*_ALIGN magSensor_e magHardware = MAG_NONE; - busDevice_t *busdev = &dev->busdev; + extDevice_t *dev = &magDev->dev; + // Associate magnetometer bus with its device + dev->bus = &magDev->bus; #ifdef USE_MAG_DATA_READY_SIGNAL - dev->magIntExtiTag = compassConfig()->interruptTag; + magDev->magIntExtiTag = compassConfig()->interruptTag; #endif switch (compassConfig()->mag_bustype) { #ifdef USE_I2C - case BUSTYPE_I2C: - busdev->bustype = BUSTYPE_I2C; - busdev->busdev_u.i2c.device = I2C_CFG_TO_DEV(compassConfig()->mag_i2c_device); - busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; + case BUS_TYPE_I2C: + i2cBusSetInstance(dev, compassConfig()->mag_i2c_device); + dev->busType_u.i2c.address = compassConfig()->mag_i2c_address; break; #endif #ifdef USE_SPI - case BUSTYPE_SPI: + case BUS_TYPE_SPI: { - SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(compassConfig()->mag_spi_device)); - if (!instance) { + if (!spiSetBusInstance(dev, compassConfig()->mag_spi_device, OWNER_COMPASS_CS)) { return false; } - busdev->bustype = BUSTYPE_SPI; - spiBusSetInstance(busdev, instance); - busdev->busdev_u.spi.csnPin = IOGetByTag(compassConfig()->mag_spi_csn); + dev->busType_u.spi.csnPin = IOGetByTag(compassConfig()->mag_spi_csn); } break; #endif #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) - case BUSTYPE_MPU_SLAVE: + case BUS_TYPE_MPU_SLAVE: { if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) { - busdev->bustype = BUSTYPE_MPU_SLAVE; - busdev->busdev_u.mpuSlave.master = gyroSensorBus(); - busdev->busdev_u.mpuSlave.address = compassConfig()->mag_i2c_address; + extDevice_t *masterDev = &gyroActiveDev()->dev; + + dev->busType_u.i2c.address = compassConfig()->mag_i2c_address; + dev->bus->busType = BUS_TYPE_MPU_SLAVE; + dev->bus->busType_u.mpuSlave.master = masterDev; } else { return false; } @@ -181,11 +182,11 @@ bool compassDetect(magDev_t *dev, uint8_t *alignment) case MAG_HMC5883: #if defined(USE_MAG_HMC5883) || defined(USE_MAG_SPI_HMC5883) - if (busdev->bustype == BUSTYPE_I2C) { - busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; + if (dev->bus->busType == BUS_TYPE_I2C) { + dev->busType_u.i2c.address = compassConfig()->mag_i2c_address; } - if (hmc5883lDetect(dev)) { + if (hmc5883lDetect(magDev)) { #ifdef MAG_HMC5883_ALIGN *alignment = MAG_HMC5883_ALIGN; #endif @@ -197,11 +198,11 @@ bool compassDetect(magDev_t *dev, uint8_t *alignment) case MAG_LIS3MDL: #if defined(USE_MAG_LIS3MDL) - if (busdev->bustype == BUSTYPE_I2C) { - busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; + if (dev->bus->busType == BUS_TYPE_I2C) { + dev->busType_u.i2c.address = compassConfig()->mag_i2c_address; } - if (lis3mdlDetect(dev)) { + if (lis3mdlDetect(magDev)) { #ifdef MAG_LIS3MDL_ALIGN *alignment = MAG_LIS3MDL_ALIGN; #endif @@ -213,11 +214,11 @@ bool compassDetect(magDev_t *dev, uint8_t *alignment) case MAG_AK8975: #ifdef USE_MAG_AK8975 - if (busdev->bustype == BUSTYPE_I2C) { - busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; + if (dev->bus->busType == BUS_TYPE_I2C) { + dev->busType_u.i2c.address = compassConfig()->mag_i2c_address; } - if (ak8975Detect(dev)) { + if (ak8975Detect(magDev)) { #ifdef MAG_AK8975_ALIGN *alignment = MAG_AK8975_ALIGN; #endif @@ -229,16 +230,16 @@ bool compassDetect(magDev_t *dev, uint8_t *alignment) case MAG_AK8963: #if defined(USE_MAG_AK8963) || defined(USE_MAG_SPI_AK8963) - if (busdev->bustype == BUSTYPE_I2C) { - busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; + if (dev->bus->busType == BUS_TYPE_I2C) { + dev->busType_u.i2c.address = compassConfig()->mag_i2c_address; } if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) { - dev->busdev.bustype = BUSTYPE_MPU_SLAVE; - busdev->busdev_u.mpuSlave.address = compassConfig()->mag_i2c_address; - dev->busdev.busdev_u.mpuSlave.master = gyroSensorBus(); + dev->bus->busType = BUS_TYPE_MPU_SLAVE; + dev->busType_u.mpuSlave.address = compassConfig()->mag_i2c_address; + dev->bus->busType_u.mpuSlave.master = &gyroActiveDev()->dev; } - if (ak8963Detect(dev)) { + if (ak8963Detect(magDev)) { #ifdef MAG_AK8963_ALIGN *alignment = MAG_AK8963_ALIGN; #endif @@ -250,11 +251,11 @@ bool compassDetect(magDev_t *dev, uint8_t *alignment) case MAG_QMC5883: #ifdef USE_MAG_QMC5883 - if (busdev->bustype == BUSTYPE_I2C) { - busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; + if (dev->bus->busType == BUS_TYPE_I2C) { + dev->busType_u.i2c.address = compassConfig()->mag_i2c_address; } - if (qmc5883lDetect(dev)) { + if (qmc5883lDetect(magDev)) { #ifdef MAG_QMC5883L_ALIGN *alignment = MAG_QMC5883L_ALIGN; #endif @@ -273,7 +274,7 @@ bool compassDetect(magDev_t *dev, uint8_t *alignment) // Passthrough mode disables the gyro/acc part of the MPU, so we only want to detect this sensor if mag_hardware was explicitly set to MAG_MPU925X_AK8963 #ifdef USE_MAG_MPU925X_AK8963 if(compassConfig()->mag_hardware == MAG_MPU925X_AK8963){ - if (mpu925Xak8963CompassDetect(dev)) { + if (mpu925Xak8963CompassDetect(magDev)) { magHardware = MAG_MPU925X_AK8963; } else { return false; diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c index 7e9f0c4fc..f0baa5c70 100644 --- a/src/main/sensors/gyro_init.c +++ b/src/main/sensors/gyro_init.c @@ -708,9 +708,10 @@ void gyroSetTargetLooptime(uint8_t pidDenom) } } -const busDevice_t *gyroSensorBus(void) + +gyroDev_t *gyroActiveDev(void) { - return &ACTIVE_GYRO->gyroDev.bus; + return &ACTIVE_GYRO->gyroDev; } const mpuDetectionResult_t *gyroMpuDetectionResult(void) @@ -724,20 +725,20 @@ int16_t gyroRateDps(int axis) } #ifdef USE_GYRO_REGISTER_DUMP -static const busDevice_t *gyroSensorBusByDevice(uint8_t whichSensor) +static extDevice_t *gyroSensorDevByInstance(uint8_t whichSensor) { #ifdef USE_MULTI_GYRO if (whichSensor == GYRO_CONFIG_USE_GYRO_2) { - return &gyro.gyroSensor2.gyroDev.bus; + return &gyro.gyroSensor2.gyroDev.dev; } #else UNUSED(whichSensor); #endif - return &gyro.gyroSensor1.gyroDev.bus; + return &gyro.gyroSensor1.gyroDev.dev; } uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg) { - return mpuGyroReadRegister(gyroSensorBusByDevice(whichSensor), reg); + return mpuGyroReadRegister(gyroSensorDevByInstance(whichSensor), reg); } #endif // USE_GYRO_REGISTER_DUMP diff --git a/src/main/sensors/gyro_init.h b/src/main/sensors/gyro_init.h index 26d0f7e39..eb0628074 100644 --- a/src/main/sensors/gyro_init.h +++ b/src/main/sensors/gyro_init.h @@ -29,7 +29,7 @@ bool gyroInit(void); void gyroInitFilters(void); void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config); gyroDetectionFlags_t getGyroDetectionFlags(void); -const busDevice_t *gyroSensorBus(void); +gyroDev_t *gyroActiveDev(void); struct mpuDetectionResult_s; const struct mpuDetectionResult_s *gyroMpuDetectionResult(void); int16_t gyroRateDps(int axis); diff --git a/src/main/startup/stm32g4xx_hal_conf.h b/src/main/startup/stm32g4xx_hal_conf.h old mode 100755 new mode 100644 diff --git a/src/main/startup/stm32h7xx_hal_conf.h b/src/main/startup/stm32h7xx_hal_conf.h index 9e96a5924..a605ade24 100644 --- a/src/main/startup/stm32h7xx_hal_conf.h +++ b/src/main/startup/stm32h7xx_hal_conf.h @@ -95,7 +95,7 @@ //#define HAL_SMARTCARD_MODULE_ENABLED //#define HAL_SMBUS_MODULE_ENABLED //#define HAL_SPDIFRX_MODULE_ENABLED -#define HAL_SPI_MODULE_ENABLED +//#define HAL_SPI_MODULE_ENABLED //#define HAL_SRAM_MODULE_ENABLED //#define HAL_SWPMI_MODULE_ENABLED #define HAL_TIM_MODULE_ENABLED diff --git a/src/main/startup/system_stm32g4xx.h b/src/main/startup/system_stm32g4xx.h old mode 100755 new mode 100644 diff --git a/src/main/startup/system_stm32h7xx.c b/src/main/startup/system_stm32h7xx.c index b84874fa5..7fcdbc82b 100644 --- a/src/main/startup/system_stm32h7xx.c +++ b/src/main/startup/system_stm32h7xx.c @@ -173,9 +173,9 @@ void HandleStuckSysTick(void) uint32_t tickStart = HAL_GetTick(); uint32_t tickEnd = 0; - int attemptsRemaining = 80 * 1000; + // H7 at 480Mhz requires a loop count of 160000. Double this for the timeout to be safe. + int attemptsRemaining = 320000; while (((tickEnd = HAL_GetTick()) == tickStart) && --attemptsRemaining) { - // H7 at 400Mhz - attemptsRemaining was reduced by debug build: 5,550, release build: 33,245 } if (tickStart == tickEnd) { @@ -769,7 +769,9 @@ void SystemInit (void) #error Unknown MCU type #endif #elif defined(USE_EXST) - // Don't touch the vector table, the bootloader will have already set it. + extern void *isr_vector_table_base; + + SCB->VTOR = (uint32_t)&isr_vector_table_base; #else SCB->VTOR = FLASH_BANK1_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */ #endif @@ -781,6 +783,10 @@ void SystemInit (void) SystemClock_Config(); SystemCoreClockUpdate(); +#ifdef STM32H7 + initialiseD2MemorySections(); +#endif + // Configure MPU memProtConfigure(mpuRegions, mpuRegionCount); diff --git a/src/main/target/BLUEJAYF4/hardware_revision.h b/src/main/target/BLUEJAYF4/hardware_revision.h index 1ac8b75c6..923639f39 100644 --- a/src/main/target/BLUEJAYF4/hardware_revision.h +++ b/src/main/target/BLUEJAYF4/hardware_revision.h @@ -32,4 +32,4 @@ typedef enum bjf4HardwareRevision_t { extern uint8_t hardwareRevision; void updateHardwareRevision(void); -void detectHardwareRevision(void); \ No newline at end of file +void detectHardwareRevision(void); diff --git a/src/main/target/COLIBRI/target.c b/src/main/target/COLIBRI/target.c index b7748293c..533bb5a8c 100644 --- a/src/main/target/COLIBRI/target.c +++ b/src/main/target/COLIBRI/target.c @@ -47,4 +47,4 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM11, CH1, PB9, TIM_USE_MOTOR, 0, 0), // S8_OUT DEF_TIM(TIM4, CH2, PB7, TIM_USE_LED , 0, 0), // S8_OUT }; - \ No newline at end of file + diff --git a/src/main/target/HAKRCF411/target.c b/src/main/target/HAKRCF411/target.c old mode 100755 new mode 100644 index 27ae837d3..4b198a20a --- a/src/main/target/HAKRCF411/target.c +++ b/src/main/target/HAKRCF411/target.c @@ -36,5 +36,5 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM2, CH1, PA15, TIM_USE_MOTOR, 0, 0), DEF_TIM(TIM2, CH2, PB3, TIM_USE_MOTOR, 0, 0), DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), - + }; diff --git a/src/main/target/HAKRCF411/target.h b/src/main/target/HAKRCF411/target.h old mode 100755 new mode 100644 index 0ce5a6402..dbf4b1440 --- a/src/main/target/HAKRCF411/target.h +++ b/src/main/target/HAKRCF411/target.h @@ -42,7 +42,7 @@ // XXX CAMERA_CONTROL_PIN is deprecated. // XXX Target maintainer must create a valid timerHardware[] array entry for PB5 with TIM_USE_CAMERA_CONTROL -//#define CAMERA_CONTROL_PIN PB5 +//#define CAMERA_CONTROL_PIN PB5 #define USE_SPI #define USE_SPI_DEVICE_1 diff --git a/src/main/target/MAMBAF411/target.c b/src/main/target/MAMBAF411/target.c index 15eb659b3..c1b7b0ff4 100644 --- a/src/main/target/MAMBAF411/target.c +++ b/src/main/target/MAMBAF411/target.c @@ -39,4 +39,4 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0 ), // LED_STRIP - DMA2_ST3_CH6 -}; \ No newline at end of file +}; diff --git a/src/main/target/MAMBAF722/target.c b/src/main/target/MAMBAF722/target.c index 5514b96f5..b5e27da63 100644 --- a/src/main/target/MAMBAF722/target.c +++ b/src/main/target/MAMBAF722/target.c @@ -39,4 +39,4 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0 ), // LED_STRIP – DMA1_ST6 -}; \ No newline at end of file +}; diff --git a/src/main/target/MATEKH743/config.c b/src/main/target/MATEKH743/config.c index 98e19f36b..4c6be8d32 100644 --- a/src/main/target/MATEKH743/config.c +++ b/src/main/target/MATEKH743/config.c @@ -39,7 +39,5 @@ void targetConfiguration(void) pinioBoxConfigMutable()->permanentId[1] = 41; sdcardConfigMutable()->mode = SDCARD_MODE_SDIO; - sdcardConfigMutable()->useDma = true; - } diff --git a/src/main/target/MERAKRCF405/target.c b/src/main/target/MERAKRCF405/target.c index a68368fe5..54f4a5837 100644 --- a/src/main/target/MERAKRCF405/target.c +++ b/src/main/target/MERAKRCF405/target.c @@ -28,15 +28,15 @@ #include "drivers/timer_def.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, 0, 0), // PPM + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, 0, 0), // PPM // Motors - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MOTOR, 0, 0), // M1 - DEF_TIM(TIM3, CH2, PC7, TIM_USE_MOTOR, 0, 0), // M2 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 0, 0), // M3 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 0, 0), // M4 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // M5 - DEF_TIM(TIM1, CH3N, PB1, TIM_USE_MOTOR, 0, 0), // M6 + DEF_TIM(TIM3, CH1, PC6, TIM_USE_MOTOR, 0, 0), // M1 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_MOTOR, 0, 0), // M2 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 0, 0), // M3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 0, 0), // M4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // M5 + DEF_TIM(TIM1, CH3N, PB1, TIM_USE_MOTOR, 0, 0), // M6 // LED strip DEF_TIM(TIM4, CH3, PB8, TIM_USE_LED, 0, 0), // LED @@ -44,4 +44,4 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { //CAMERA_CONTROL DEF_TIM(TIM11, CH1, PB9, TIM_USE_CAMERA_CONTROL, 0, 0), // CAMERA_CONTROL -}; \ No newline at end of file +}; diff --git a/src/main/target/MERAKRCF722/target.c b/src/main/target/MERAKRCF722/target.c index a68368fe5..54f4a5837 100644 --- a/src/main/target/MERAKRCF722/target.c +++ b/src/main/target/MERAKRCF722/target.c @@ -28,15 +28,15 @@ #include "drivers/timer_def.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, 0, 0), // PPM + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, 0, 0), // PPM // Motors - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MOTOR, 0, 0), // M1 - DEF_TIM(TIM3, CH2, PC7, TIM_USE_MOTOR, 0, 0), // M2 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 0, 0), // M3 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 0, 0), // M4 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // M5 - DEF_TIM(TIM1, CH3N, PB1, TIM_USE_MOTOR, 0, 0), // M6 + DEF_TIM(TIM3, CH1, PC6, TIM_USE_MOTOR, 0, 0), // M1 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_MOTOR, 0, 0), // M2 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 0, 0), // M3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 0, 0), // M4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // M5 + DEF_TIM(TIM1, CH3N, PB1, TIM_USE_MOTOR, 0, 0), // M6 // LED strip DEF_TIM(TIM4, CH3, PB8, TIM_USE_LED, 0, 0), // LED @@ -44,4 +44,4 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { //CAMERA_CONTROL DEF_TIM(TIM11, CH1, PB9, TIM_USE_CAMERA_CONTROL, 0, 0), // CAMERA_CONTROL -}; \ No newline at end of file +}; diff --git a/src/main/target/MOTOLABF4/config.c b/src/main/target/MOTOLABF4/config.c index dd4d1dfd8..340743ea9 100644 --- a/src/main/target/MOTOLABF4/config.c +++ b/src/main/target/MOTOLABF4/config.c @@ -31,7 +31,6 @@ void targetConfiguration(void) { - sdcardConfigMutable()->useDma = true; telemetryConfigMutable()->halfDuplex = 0; } #endif diff --git a/src/main/target/NUCLEOH723ZG/config.c b/src/main/target/NUCLEOH723ZG/config.c index d9f91f24b..4fcef5467 100644 --- a/src/main/target/NUCLEOH723ZG/config.c +++ b/src/main/target/NUCLEOH723ZG/config.c @@ -39,6 +39,5 @@ void targetConfiguration(void) { targetSerialPortFunctionConfig(targetSerialPortFunction, ARRAYLEN(targetSerialPortFunction)); sdcardConfigMutable()->mode = SDCARD_MODE_SDIO; - sdcardConfigMutable()->useDma = true; } #endif diff --git a/src/main/target/NUCLEOH723ZG/target.h b/src/main/target/NUCLEOH723ZG/target.h index 5568cb4f3..1d340a8e0 100644 --- a/src/main/target/NUCLEOH723ZG/target.h +++ b/src/main/target/NUCLEOH723ZG/target.h @@ -32,9 +32,9 @@ // Nucleo-H7A3 has one button (The blue USER button). // Force two buttons to look at the single button so config reset on button works #define USE_BUTTONS -#define BUTTON_A_PIN PC13 +#define BUTTON_A_PIN PC13 #define BUTTON_A_PIN_INVERTED // Active high -#define BUTTON_B_PIN PC13 +#define BUTTON_B_PIN PC13 #define BUTTON_B_PIN_INVERTED // Active high #define USE_BEEPER diff --git a/src/main/target/NUCLEOH725ZG/config.c b/src/main/target/NUCLEOH725ZG/config.c index d9f91f24b..4fcef5467 100644 --- a/src/main/target/NUCLEOH725ZG/config.c +++ b/src/main/target/NUCLEOH725ZG/config.c @@ -39,6 +39,5 @@ void targetConfiguration(void) { targetSerialPortFunctionConfig(targetSerialPortFunction, ARRAYLEN(targetSerialPortFunction)); sdcardConfigMutable()->mode = SDCARD_MODE_SDIO; - sdcardConfigMutable()->useDma = true; } #endif diff --git a/src/main/target/NUCLEOH725ZG/target.h b/src/main/target/NUCLEOH725ZG/target.h index 0bbc47a32..ef6d6f958 100644 --- a/src/main/target/NUCLEOH725ZG/target.h +++ b/src/main/target/NUCLEOH725ZG/target.h @@ -41,9 +41,9 @@ // Nucleo-H7A3 has one button (The blue USER button). // Force two buttons to look at the single button so config reset on button works #define USE_BUTTONS -#define BUTTON_A_PIN PC13 +#define BUTTON_A_PIN PC13 #define BUTTON_A_PIN_INVERTED // Active high -#define BUTTON_B_PIN PC13 +#define BUTTON_B_PIN PC13 #define BUTTON_B_PIN_INVERTED // Active high #define USE_BEEPER diff --git a/src/main/target/NUCLEOH743/config.c b/src/main/target/NUCLEOH743/config.c index ffdb00411..8a435f64a 100644 --- a/src/main/target/NUCLEOH743/config.c +++ b/src/main/target/NUCLEOH743/config.c @@ -41,7 +41,6 @@ void targetConfiguration(void) targetSerialPortFunctionConfig(targetSerialPortFunction, ARRAYLEN(targetSerialPortFunction)); #if !defined(NUCLEOH743_RAMBASED) sdcardConfigMutable()->mode = SDCARD_MODE_SDIO; - sdcardConfigMutable()->useDma = true; #endif } #endif diff --git a/src/main/target/NUCLEOH7A3ZI/config.c b/src/main/target/NUCLEOH7A3ZI/config.c index feec67132..ade7f3cb7 100644 --- a/src/main/target/NUCLEOH7A3ZI/config.c +++ b/src/main/target/NUCLEOH7A3ZI/config.c @@ -39,6 +39,5 @@ void targetConfiguration(void) { targetSerialPortFunctionConfig(targetSerialPortFunction, ARRAYLEN(targetSerialPortFunction)); sdcardConfigMutable()->mode = SDCARD_MODE_SDIO; - sdcardConfigMutable()->useDma = true; } #endif diff --git a/src/main/target/NUCLEOH7A3ZI/target.h b/src/main/target/NUCLEOH7A3ZI/target.h index 8341fc892..259a9f785 100644 --- a/src/main/target/NUCLEOH7A3ZI/target.h +++ b/src/main/target/NUCLEOH7A3ZI/target.h @@ -32,9 +32,9 @@ // Nucleo-H7A3 has one button (The blue USER button). // Force two buttons to look at the single button so config reset on button works #define USE_BUTTONS -#define BUTTON_A_PIN PC13 +#define BUTTON_A_PIN PC13 #define BUTTON_A_PIN_INVERTED // Active high -#define BUTTON_B_PIN PC13 +#define BUTTON_B_PIN PC13 #define BUTTON_B_PIN_INVERTED // Active high #define USE_BEEPER diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index e511948d6..760747782 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -36,15 +36,19 @@ #define GYRO_1_SPI_INSTANCE SPI2 #define USE_ACC -#define USE_ACC_SPI_MPU6500 +#define USE_ACC_SPI_MPU9250 #define USE_GYRO -#define USE_GYRO_SPI_MPU6500 +#define USE_GYRO_SPI_MPU9250 #define GYRO_1_ALIGN CW270_DEG #define USE_BARO #define USE_BARO_MS5611 +#define USE_MAG +#define USE_MAG_AK8963 +#define USE_MAG_MPU925X_AK8963 + // MPU6500 interrupts #define USE_EXTI #define USE_GYRO_EXTI diff --git a/src/main/target/REVONANO/target.mk b/src/main/target/REVONANO/target.mk index ce4d9fd8c..a0b8a2f64 100644 --- a/src/main/target/REVONANO/target.mk +++ b/src/main/target/REVONANO/target.mk @@ -2,6 +2,7 @@ F411_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ - drivers/accgyro/accgyro_mpu6500.c \ - drivers/accgyro/accgyro_spi_mpu6500.c \ + drivers/compass/compass_ak8963.c \ + drivers/compass/compass_mpu925x_ak8963.c \ + drivers/accgyro/accgyro_spi_mpu9250.c \ drivers/barometer/barometer_ms5611.c diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 34978b636..e6f835b8d 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -189,6 +189,10 @@ typedef struct { void* test; } DMA_Channel_TypeDef; +typedef struct { + void* test; +} DMA_InitTypeDef; + uint8_t DMA_GetFlagStatus(void *); void DMA_Cmd(DMA_Channel_TypeDef*, FunctionalState ); void DMA_ClearFlag(uint32_t); diff --git a/src/main/target/SPRACINGF3EVO/config.c b/src/main/target/SPRACINGF3EVO/config.c index 618ee4308..3efdce7fb 100644 --- a/src/main/target/SPRACINGF3EVO/config.c +++ b/src/main/target/SPRACINGF3EVO/config.c @@ -41,7 +41,6 @@ void targetConfiguration(void) { // Temporary workaround: Disable SDCard DMA by default since it causes errors on this target - sdcardConfigMutable()->useDma = false; #if defined(SPRACINGF3MQ) diff --git a/src/main/target/SPRACINGH7EXTREME/config.c b/src/main/target/SPRACINGH7EXTREME/config.c index 8f876eaa0..ae3085941 100644 --- a/src/main/target/SPRACINGH7EXTREME/config.c +++ b/src/main/target/SPRACINGH7EXTREME/config.c @@ -27,6 +27,8 @@ #include "config_helper.h" +#include "drivers/sdio.h" + #include "io/serial.h" #include "osd/osd.h" @@ -43,5 +45,7 @@ void targetConfiguration(void) osdConfigMutable()->core_temp_alarm = 85; targetSerialPortFunctionConfig(targetSerialPortFunction, ARRAYLEN(targetSerialPortFunction)); sdcardConfigMutable()->mode = SDCARD_MODE_SDIO; + sdioPinConfigure(); + SDIO_GPIO_Init(); } #endif diff --git a/src/main/target/SPRACINGH7ZERO/config.c b/src/main/target/SPRACINGH7ZERO/config.c index 8f876eaa0..ae3085941 100644 --- a/src/main/target/SPRACINGH7ZERO/config.c +++ b/src/main/target/SPRACINGH7ZERO/config.c @@ -27,6 +27,8 @@ #include "config_helper.h" +#include "drivers/sdio.h" + #include "io/serial.h" #include "osd/osd.h" @@ -43,5 +45,7 @@ void targetConfiguration(void) osdConfigMutable()->core_temp_alarm = 85; targetSerialPortFunctionConfig(targetSerialPortFunction, ARRAYLEN(targetSerialPortFunction)); sdcardConfigMutable()->mode = SDCARD_MODE_SDIO; + sdioPinConfigure(); + SDIO_GPIO_Init(); } #endif diff --git a/src/main/target/WORMFC/config.c b/src/main/target/WORMFC/config.c index 199accd66..db7f8f1c9 100644 --- a/src/main/target/WORMFC/config.c +++ b/src/main/target/WORMFC/config.c @@ -30,6 +30,5 @@ void targetConfiguration(void) { sdcardConfigMutable()->mode = SDCARD_MODE_SDIO; - sdcardConfigMutable()->useDma = true; } #endif diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 16d83ad8c..da4a75496 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -65,7 +65,6 @@ #define USE_TIMER_MGMT #define USE_PERSISTENT_OBJECTS #define USE_CUSTOM_DEFAULTS_ADDRESS -#define USE_SPI_TRANSACTION #if defined(STM32F40_41xxx) || defined(STM32F411xE) #define USE_OVERCLOCK @@ -93,7 +92,6 @@ #define USE_TIMER_MGMT #define USE_PERSISTENT_OBJECTS #define USE_CUSTOM_DEFAULTS_ADDRESS -#define USE_SPI_TRANSACTION #endif // STM32F7 #ifdef STM32H7 @@ -169,14 +167,14 @@ #endif // USE_ITCM_RAM #ifdef USE_CCM_CODE -#define CCM_CODE __attribute__((section(".ccm_code"))) +#define CCM_CODE __attribute__((section(".ccm_code"))) #else #define CCM_CODE #endif #ifdef USE_FAST_DATA -#define FAST_DATA_ZERO_INIT __attribute__ ((section(".fastram_bss"), aligned(4))) -#define FAST_DATA __attribute__ ((section(".fastram_data"), aligned(4))) +#define FAST_DATA_ZERO_INIT __attribute__ ((section(".fastram_bss"), aligned(4))) +#define FAST_DATA __attribute__ ((section(".fastram_data"), aligned(4))) #else #define FAST_DATA_ZERO_INIT #define FAST_DATA @@ -186,17 +184,17 @@ // F4 can't DMA to/from CCM (core coupled memory) SRAM (where the stack lives) #define DMA_DATA_ZERO_INIT #define DMA_DATA -#define DMA_DATA_AUTO static +#define STATIC_DMA_DATA_AUTO static #elif defined (STM32F7) // F7 has no cache coherency issues DMAing to/from DTCM, otherwise buffers must be cache aligned #define DMA_DATA_ZERO_INIT FAST_DATA_ZERO_INIT #define DMA_DATA FAST_DATA -#define DMA_DATA_AUTO static DMA_DATA +#define STATIC_DMA_DATA_AUTO static DMA_DATA #else // DMA to/from any memory #define DMA_DATA_ZERO_INIT __attribute__ ((section(".dmaram_bss"), aligned(32))) #define DMA_DATA __attribute__ ((section(".dmaram_data"), aligned(32))) -#define DMA_DATA_AUTO static DMA_DATA +#define STATIC_DMA_DATA_AUTO static DMA_DATA #endif #if defined(STM32F4) || defined (STM32H7) diff --git a/src/main/vcp_hal/usbd_conf_stm32g4xx.c b/src/main/vcp_hal/usbd_conf_stm32g4xx.c old mode 100755 new mode 100644 diff --git a/src/test/unit/baro_bmp085_unittest.cc b/src/test/unit/baro_bmp085_unittest.cc index 4245944c1..6b7c46b1d 100644 --- a/src/test/unit/baro_bmp085_unittest.cc +++ b/src/test/unit/baro_bmp085_unittest.cc @@ -191,15 +191,15 @@ extern "C" { void RCC_APB2PeriphClockCmd() {} void delay(uint32_t) {} void delayMicroseconds(uint32_t) {} - bool busReadRegisterBuffer(const busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} - bool busWriteRegister(const busDevice_t*, uint8_t, uint8_t) {return true;} + bool busReadRegisterBuffer(const extDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} + bool busWriteRegister(const extDevice_t*, uint8_t, uint8_t) {return true;} void IOConfigGPIO() {} void IOHi() {} void IOLo() {} void IOInit() {} void IOGetByTag() {} - bool busBusy(const busDevice_t*, bool*) {return false;} - void busDeviceRegister(const busDevice_t*) {} - bool busReadRegisterBufferStart(const busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} - bool busWriteRegisterStart(const busDevice_t*, uint8_t, uint8_t) {return true;} + bool busBusy(const extDevice_t*, bool*) {return false;} + void busDeviceRegister(const extDevice_t*) {} + bool busReadRegisterBufferStart(const extDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} + bool busWriteRegisterStart(const extDevice_t*, uint8_t, uint8_t) {return true;} } diff --git a/src/test/unit/baro_bmp280_unittest.cc b/src/test/unit/baro_bmp280_unittest.cc index ca600eb95..2e64a3a18 100644 --- a/src/test/unit/baro_bmp280_unittest.cc +++ b/src/test/unit/baro_bmp280_unittest.cc @@ -147,21 +147,18 @@ TEST(baroBmp280Test, TestBmp280CalculateZeroP) extern "C" { void delay(uint32_t) {} -bool busBusy(const busDevice_t*, bool*) {return false;} -bool busReadRegisterBuffer(const busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} -bool busReadRegisterBufferStart(const busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} -bool busWriteRegister(const busDevice_t*, uint8_t, uint8_t) {return true;} -bool busWriteRegisterStart(const busDevice_t*, uint8_t, uint8_t) {return true;} -void busDeviceRegister(const busDevice_t*) {} +bool busBusy(const extDevice_t*, bool*) {return false;} +bool busReadRegisterBuffer(const extDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} +bool busReadRegisterBufferStart(const extDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} +bool busWriteRegister(const extDevice_t*, uint8_t, uint8_t) {return true;} +bool busWriteRegisterStart(const extDevice_t*, uint8_t, uint8_t) {return true;} +void busDeviceRegister(const extDevice_t*) {} uint16_t spiCalculateDivider() { return 2; } -void spiBusSetDivisor() { -} - -void spiBusTransactionInit() { +void spiSetClkDivisor() { } void spiPreinitByIO() { diff --git a/src/test/unit/baro_bmp388_unittest.cc b/src/test/unit/baro_bmp388_unittest.cc index 1f35a4d55..2784e7439 100644 --- a/src/test/unit/baro_bmp388_unittest.cc +++ b/src/test/unit/baro_bmp388_unittest.cc @@ -142,22 +142,20 @@ TEST(baroBmp388Test, TestBmp388CalculateWithSampleCalibration) extern "C" { -void delay(uint32_t) {} -bool busBusy(const busDevice_t*, bool*) {return false;} -bool busReadRegisterBuffer(const busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} -bool busReadRegisterBufferStart(const busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} -bool busWriteRegister(const busDevice_t*, uint8_t, uint8_t) {return true;} -bool busWriteRegisterStart(const busDevice_t*, uint8_t, uint8_t) {return true;} -void busDeviceRegister(const busDevice_t*) {} +void delay() {} + +bool busBusy(const extDevice_t*, bool*) {return false;} +bool busReadRegisterBuffer(const extDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} +bool busReadRegisterBufferStart(const extDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} +bool busWriteRegister(const extDevice_t*, uint8_t, uint8_t) {return true;} +bool busWriteRegisterStart(const extDevice_t*, uint8_t, uint8_t) {return true;} +void busDeviceRegister(const extDevice_t*) {} uint16_t spiCalculateDivider() { return 2; } -void spiBusSetDivisor() { -} - -void spiBusTransactionInit() { +void spiSetClkDivisor() { } void spiPreinitByIO(IO_t) { diff --git a/src/test/unit/baro_ms5611_unittest.cc b/src/test/unit/baro_ms5611_unittest.cc index b3918fb77..331bd6095 100644 --- a/src/test/unit/baro_ms5611_unittest.cc +++ b/src/test/unit/baro_ms5611_unittest.cc @@ -146,18 +146,18 @@ extern "C" { void delay(uint32_t) {} void delayMicroseconds(uint32_t) {} -bool busBusy(const busDevice_t*, bool*) {return false;} -bool busRawReadRegisterBuffer(const busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} -bool busRawReadRegisterBufferStart(const busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} -bool busRawWriteRegister(const busDevice_t*, uint8_t, uint8_t) {return true;} -bool busRawWriteRegisterStart(const busDevice_t*, uint8_t, uint8_t) {return true;} -void busDeviceRegister(const busDevice_t*) {} +bool busBusy(const extDevice_t*, bool*) {return false;} +bool busRawReadRegisterBuffer(const extDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} +bool busRawReadRegisterBufferStart(const extDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} +bool busRawWriteRegister(const extDevice_t*, uint8_t, uint8_t) {return true;} +bool busRawWriteRegisterStart(const extDevice_t*, uint8_t, uint8_t) {return true;} +void busDeviceRegister(const extDevice_t*) {} uint16_t spiCalculateDivider() { return 2; } -void spiBusSetDivisor() { +void spiSetClkDivisor() { } void spiPreinitByIO() { diff --git a/src/test/unit/target.h b/src/test/unit/target.h index 655100844..b2a365a06 100644 --- a/src/test/unit/target.h +++ b/src/test/unit/target.h @@ -20,6 +20,8 @@ #define SCHEDULER_DELAY_LIMIT 1 #define TASK_GYROPID_DESIRED_PERIOD 100 +#define DMA_DATA_ZERO_INIT + #define USE_ACC #define USE_CMS #define CMS_MAX_DEVICE 4