From 87c8847c13d7f759548c2384e400d2bc387b159a Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Tue, 20 Apr 2021 19:45:56 +0100 Subject: [PATCH] New SPI API supporting DMA Call targetConfiguration() once before config is loaded and again afterwards in case the config needs to be changed to load from SD card etc Drop SPI clock during binding Remove debug Add per device SPI DMA enable Fix sdioPinConfigure() declaration warning Reduce clock speed during SPI RX initialisation --- make/mcu/STM32H7.mk | 4 +- src/link/stm32_flash_f7_split.ld | 4 +- src/link/stm32_flash_h723_1m.ld | 35 +- src/link/stm32_flash_h743_2m.ld | 31 +- src/link/stm32_flash_h750_128k.ld | 34 +- src/link/stm32_flash_h750_1m.ld | 34 +- src/link/stm32_flash_h7a3_2m.ld | 35 +- src/link/stm32_h750_common.ld | 4 +- src/link/stm32_ram_h743.ld | 31 +- src/link/stm32_ram_h750_exst.ld | 34 +- src/main/cli/settings.c | 7 +- src/main/drivers/accgyro/accgyro.h | 6 +- src/main/drivers/accgyro/accgyro_mpu.c | 81 +-- src/main/drivers/accgyro/accgyro_mpu.h | 3 +- src/main/drivers/accgyro/accgyro_mpu6050.c | 16 +- src/main/drivers/accgyro/accgyro_mpu6500.c | 22 +- src/main/drivers/accgyro/accgyro_spi_bmi160.c | 76 ++- src/main/drivers/accgyro/accgyro_spi_bmi270.c | 91 ++-- src/main/drivers/accgyro/accgyro_spi_bmi270.h | 2 +- src/main/drivers/accgyro/accgyro_spi_icm20649.c | 44 +- src/main/drivers/accgyro/accgyro_spi_icm20689.c | 41 +- src/main/drivers/accgyro/accgyro_spi_icm20689.h | 2 +- src/main/drivers/accgyro/accgyro_spi_icm42605.c | 46 +- src/main/drivers/accgyro/accgyro_spi_icm42605.h | 2 +- src/main/drivers/accgyro/accgyro_spi_l3gd20.c | 15 +- src/main/drivers/accgyro/accgyro_spi_lsm6dso.c | 8 +- src/main/drivers/accgyro/accgyro_spi_lsm6dso.h | 2 +- .../drivers/accgyro/accgyro_spi_lsm6dso_init.c | 38 +- src/main/drivers/accgyro/accgyro_spi_mpu6000.c | 43 +- src/main/drivers/accgyro/accgyro_spi_mpu6000.h | 2 +- src/main/drivers/accgyro/accgyro_spi_mpu6500.c | 18 +- src/main/drivers/accgyro/accgyro_spi_mpu6500.h | 2 +- src/main/drivers/accgyro/accgyro_spi_mpu9250.c | 61 +-- src/main/drivers/accgyro/accgyro_spi_mpu9250.h | 8 +- src/main/drivers/barometer/barometer.h | 2 +- src/main/drivers/barometer/barometer_bmp085.c | 38 +- src/main/drivers/barometer/barometer_bmp280.c | 56 +-- src/main/drivers/barometer/barometer_bmp388.c | 64 +-- src/main/drivers/barometer/barometer_dps310.c | 89 ++-- src/main/drivers/barometer/barometer_lps.c | 56 +-- src/main/drivers/barometer/barometer_ms5611.c | 76 ++- src/main/drivers/barometer/barometer_qmp6988.c | 62 ++- src/main/drivers/bus.c | 179 +++---- src/main/drivers/bus.h | 132 +++-- src/main/drivers/bus_i2c_busdev.c | 44 +- .../accgyro_spi_icm42605.h => bus_i2c_busdev.h} | 21 +- src/main/drivers/bus_spi.c | 502 +++++++++++++++---- src/main/drivers/bus_spi.h | 86 ++-- src/main/drivers/bus_spi_hal.c | 213 ++++---- src/main/drivers/bus_spi_impl.h | 21 +- src/main/drivers/bus_spi_ll.c | 548 ++++++++++++++++----- src/main/drivers/bus_spi_stdperiph.c | 408 +++++++++------ src/main/drivers/compass/compass.h | 3 +- src/main/drivers/compass/compass_ak8963.c | 171 ++++--- src/main/drivers/compass/compass_ak8975.c | 36 +- src/main/drivers/compass/compass_hmc5883l.c | 41 +- src/main/drivers/compass/compass_lis3mdl.c | 26 +- src/main/drivers/compass/compass_mpu925x_ak8963.c | 19 +- src/main/drivers/compass/compass_qmc5883l.c | 28 +- src/main/drivers/display_ug2864hsweg01.c | 52 +- src/main/drivers/display_ug2864hsweg01.h | 16 +- src/main/drivers/dma.h | 3 + src/main/drivers/dshot_bitbang.c | 4 +- src/main/drivers/flash.c | 59 +-- src/main/drivers/flash_impl.h | 4 +- src/main/drivers/flash_m25p16.c | 295 ++++++----- src/main/drivers/flash_w25m.c | 43 +- src/main/drivers/flash_w25n01g.c | 200 +++++--- src/main/drivers/light_ws2811strip.c | 2 +- src/main/drivers/max7456.c | 344 ++++--------- src/main/drivers/nvic.h | 1 + src/main/drivers/rx/rx_cc2500.c | 24 +- src/main/drivers/rx/rx_spi.c | 56 ++- src/main/drivers/rx/rx_spi.h | 3 + src/main/drivers/sdcard_impl.h | 5 +- src/main/drivers/sdcard_spi.c | 443 ++++++++--------- src/main/drivers/system.c | 14 + src/main/drivers/system.h | 3 + src/main/drivers/vtx_rtc6705.c | 42 +- src/main/fc/init.c | 64 +-- src/main/io/asyncfatfs/asyncfatfs.c | 2 +- src/main/io/dashboard.c | 179 +++---- src/main/io/flashfs.c | 4 +- src/main/msc/emfat_file.c | 2 +- src/main/msp/msp.c | 3 +- src/main/osd/osd.c | 2 - src/main/pg/gyrodev.c | 6 +- src/main/pg/gyrodev.h | 2 +- src/main/pg/rcdevice.c | 2 +- src/main/pg/rcdevice.h | 2 +- src/main/pg/sdcard.c | 11 - src/main/pg/sdcard.h | 5 - src/main/rx/cc2500_frsky_shared.c | 4 + src/main/rx/rx.h | 1 + src/main/rx/rx_spi_common.c | 4 +- src/main/sensors/acceleration_init.c | 2 +- src/main/sensors/barometer.c | 47 +- src/main/sensors/barometer.h | 2 +- src/main/sensors/compass.c | 85 ++-- src/main/sensors/gyro_init.c | 13 +- src/main/sensors/gyro_init.h | 2 +- src/main/startup/stm32g4xx_hal_conf.h | 0 src/main/startup/stm32h7xx_hal_conf.h | 2 +- src/main/startup/system_stm32g4xx.h | 0 src/main/startup/system_stm32h7xx.c | 12 +- src/main/target/BLUEJAYF4/hardware_revision.h | 2 +- src/main/target/COLIBRI/target.c | 2 +- src/main/target/HAKRCF411/target.c | 2 +- src/main/target/HAKRCF411/target.h | 2 +- src/main/target/MAMBAF411/target.c | 2 +- src/main/target/MAMBAF722/target.c | 2 +- src/main/target/MATEKH743/config.c | 2 - .../target/{MERAKRCF722 => MERAKRCF405}/target.c | 16 +- src/main/target/MERAKRCF722/target.c | 16 +- src/main/target/MOTOLABF4/config.c | 1 - src/main/target/NUCLEOH723ZG/config.c | 1 - src/main/target/NUCLEOH723ZG/target.h | 4 +- src/main/target/NUCLEOH725ZG/config.c | 1 - src/main/target/NUCLEOH725ZG/target.h | 4 +- src/main/target/NUCLEOH743/config.c | 1 - src/main/target/NUCLEOH7A3ZI/config.c | 1 - src/main/target/NUCLEOH7A3ZI/target.h | 4 +- src/main/target/REVONANO/target.h | 8 +- src/main/target/REVONANO/target.mk | 5 +- src/main/target/SITL/target.h | 4 + src/main/target/SPRACINGF3EVO/config.c | 1 - src/main/target/SPRACINGH7EXTREME/config.c | 4 + src/main/target/SPRACINGH7ZERO/config.c | 4 + src/main/target/WORMFC/config.c | 1 - src/main/target/common_pre.h | 14 +- src/main/vcp_hal/usbd_conf_stm32g4xx.c | 0 src/test/unit/baro_bmp085_unittest.cc | 12 +- src/test/unit/baro_bmp280_unittest.cc | 17 +- src/test/unit/baro_bmp388_unittest.cc | 20 +- src/test/unit/baro_ms5611_unittest.cc | 14 +- src/test/unit/target.h | 2 + 136 files changed, 3451 insertions(+), 2592 deletions(-) copy src/main/drivers/{accgyro/accgyro_spi_icm42605.h => bus_i2c_busdev.h} (55%) mode change 100755 => 100644 src/main/startup/stm32g4xx_hal_conf.h mode change 100755 => 100644 src/main/startup/system_stm32g4xx.h mode change 100755 => 100644 src/main/target/HAKRCF411/target.c mode change 100755 => 100644 src/main/target/HAKRCF411/target.h copy src/main/target/{MERAKRCF722 => MERAKRCF405}/target.c (72%) mode change 100755 => 100644 src/main/vcp_hal/usbd_conf_stm32g4xx.c 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) { -#if !defined(USE_SPI) && !defined(USE_I2C) - UNUSED(reg); - UNUSED(data); -#endif - switch (busdev->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 -#endif -#ifdef USE_I2C - case BUSTYPE_I2C: - return i2cBusWriteRegister(busdev, reg, data); + if (dev->bus->busType == BUS_TYPE_SPI) { + return spiWriteRegRB(dev, reg, data); + } else #endif - default: - return false; + { + return busWriteRegisterStart(dev, reg, data); } } -bool busRawWriteRegisterStart(const busDevice_t *busdev, uint8_t reg, uint8_t data) +bool busRawReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length) { #ifdef USE_SPI - if (busdev->bustype == BUSTYPE_SPI) { -#ifdef USE_SPI_TRANSACTION - spiBusTransactionSetup(busdev); + if (dev->bus->busType == BUS_TYPE_SPI) { + return spiReadRegBufRB(dev, reg, data, length); + } else #endif - return spiBusWriteRegister(busdev, reg, data); + { + 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 busWriteRegisterStart(busdev, reg, data); + return busReadRegisterBufferStart(dev, reg, data, length); } } -bool busWriteRegisterStart(const busDevice_t *busdev, uint8_t reg, uint8_t data) + +// 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 i2cBusWriteRegisterStart(busdev, reg, data); + case BUS_TYPE_I2C: + return i2cBusWriteRegister(dev, reg, data); #endif default: return false; } } -bool busRawReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length) +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 (dev->bus->busType) { #ifdef USE_SPI - if (busdev->bustype == BUSTYPE_SPI) { -#ifdef USE_SPI_TRANSACTION - spiBusTransactionSetup(busdev); + case BUS_TYPE_SPI: + return spiWriteRegRB(dev, reg & 0x7f, data); #endif - return spiBusRawReadRegisterBuffer(busdev, reg, data, length); - } else +#ifdef USE_I2C + case BUS_TYPE_I2C: + return i2cBusWriteRegisterStart(dev, reg, data); #endif - { - return busReadRegisterBuffer(busdev, reg, data, length); + default: + return false; } } -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 + uint16_t speed; + bool leadingEdge; + } spi; + 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 deviceI2C_s { - I2CDevice device; + 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/accgyro/accgyro_spi_icm42605.h b/src/main/drivers/bus_i2c_busdev.h similarity index 55% copy from src/main/drivers/accgyro/accgyro_spi_icm42605.h copy to src/main/drivers/bus_i2c_busdev.h index ec302a4b4..1f2149ff7 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm42605.h +++ b/src/main/drivers/bus_i2c_busdev.h @@ -20,15 +20,12 @@ #pragma once -#include "drivers/bus.h" - -bool icm42605AccDetect(accDev_t *acc); -bool icm42605GyroDetect(gyroDev_t *gyro); - -void icm42605AccInit(accDev_t *acc); -void icm42605GyroInit(gyroDev_t *gyro); - -uint8_t icm42605SpiDetect(const busDevice_t *bus); - -bool icm42605SpiAccDetect(accDev_t *acc); -bool icm42605SpiGyroDetect(gyroDev_t *gyro); +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; + // 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 + + 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); } - return spiDevice[device].errorCount; } -void spiResetErrorCounter(SPI_TypeDef *instance) +// Wait for DMA completion +void spiWait(const extDevice_t *dev) { - SPIDevice device = spiDeviceByInstance(instance); - if (device != SPIINVALID) { - spiDevice[device].errorCount = 0; + // Wait for completion + while (dev->bus->curSegment != (busSegment_t *)NULL); +} + +// 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) +{ + // 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); +} + +// 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) +{ + // Ensure any prior DMA has completed before continuing + if (spiIsBusy(dev)) { + return false; } + + spiReadWriteBuf(dev, txData, rxData, length); + + return true; } -bool spiBusIsBusBusy(const busDevice_t *bus) +// Wait for bus to become free, then read/write a single byte +uint8_t spiReadWrite(const extDevice_t *dev, uint8_t data) { - return spiIsBusBusy(bus->busdev_u.spi.instance); + 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; } -uint8_t spiBusTransferByte(const busDevice_t *bus, uint8_t data) +// 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) { - return spiTransferByte(bus->busdev_u.spi.instance, 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; } -void spiBusWriteByte(const busDevice_t *bus, uint8_t data) +// Wait for bus to become free, then write a single byte +void spiWrite(const extDevice_t *dev, uint8_t data) { - IOLo(bus->busdev_u.spi.csnPin); - spiBusTransferByte(bus, data); - IOHi(bus->busdev_u.spi.csnPin); + // 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); } -bool spiBusRawTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int len) +// Write data to a register +void spiWriteReg(const extDevice_t *dev, uint8_t reg, uint8_t data) { - return spiTransfer(bus->busdev_u.spi.instance, txData, rxData, len); + // 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); } -bool spiBusWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) +// Write data to a register, returning false if the bus is busy +bool spiWriteRegRB(const extDevice_t *dev, 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); + // Ensure any prior DMA has completed before continuing + if (spiIsBusy(dev)) { + return false; + } + + spiWriteReg(dev, reg, data); return true; } -bool spiBusRawReadRegisterBuffer(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) { - 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); + // 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}, + }; - return true; + // Ensure any prior DMA has completed before continuing + spiWaitClaim(dev); + + spiSequence(dev, &segments[0]); + + spiWait(dev); } -bool spiBusReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, 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) { - return spiBusRawReadRegisterBuffer(bus, reg | 0x80, data, length); + // Ensure any prior DMA has completed before continuing + if (spiIsBusy(dev)) { + return false; + } + + spiReadRegBuf(dev, reg, data, length); + + return true; } -void spiBusWriteRegisterBuffer(const busDevice_t *bus, uint8_t reg, const uint8_t *data, uint8_t length) +// 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) { - 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); + return spiReadRegBufRB(dev, reg | 0x80, data, length); } -uint8_t spiBusRawReadRegister(const busDevice_t *bus, uint8_t reg) +// 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) { - 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}, + {data, NULL, length, true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; - return data; + // Ensure any prior DMA has completed before continuing + spiWaitClaim(dev); + + spiSequence(dev, &segments[0]); + + spiWait(dev); } -uint8_t spiBusReadRegister(const busDevice_t *bus, uint8_t reg) +// Wait for bus to become free, then read a byte from a register +uint8_t spiReadReg(const extDevice_t *dev, uint8_t reg) { - return spiBusRawReadRegister(bus, reg | 0x80); + uint8_t data; + // 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; } -void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance) +// 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) { - 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; + } -#ifdef USE_SPI_TRANSACTION -// Separate set of spiBusTransactionXXX to keep fast path for acc/gyros. + // Launch the next transfer + spiInternalStartDMA(dev); -void spiBusTransactionBegin(const busDevice_t *bus) + // Prepare the init structures ready for the next segment to reduce inter-segment time + spiInternalInitStream(dev, true); + } +} + +// 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); +// 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); +// 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); + +// 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); -bool spiBusTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int length); +/* + * 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); -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); +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); -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); -uint16_t spiCalculateDivider(uint32_t freq); -void spiBusSetDivisor(busDevice_t *bus, uint16_t divider); +uint8_t spiReadWrite(const extDevice_t *dev, uint8_t data); -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); +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,15 +40,50 @@ #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]); - if (!spi->dev) { + int prescalerIndex = ffs(divisor) - 2; // prescaler begins at "/2" + + if (prescalerIndex < 0 || prescalerIndex >= (int)ARRAYLEN(baudRatePrescaler)) { return; } - spi->leadingEdge = leadingEdge; + 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]); + + if (!spi->dev) { + return; + } // Enable SPI clock RCC_ClockCmd(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 true; + return (status == HAL_OK); } -// 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, -}; - -void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) +void spiInternalInitStream(const extDevice_t *dev, bool preInit) { - SPIDevice device = spiDeviceByInstance(instance); - - HAL_SPI_DeInit(&spiDevice[device].hspi); - - spiDevice_t *spi = &(spiDevice[device]); - - int prescalerIndex = ffs(divisor) - 2; // prescaler begins at "/2" - - if (prescalerIndex < 0 || prescalerIndex >= (int)ARRAYLEN(baudRatePrescaler)) { - return; - } - - spi->hspi.Init.BaudRatePrescaler = baudRatePrescaler[prescalerIndex]; - - HAL_SPI_Init(&spi->hspi); + UNUSED(dev); + UNUSED(preInit); } -#ifdef USE_DMA -DMA_HandleTypeDef* dmaHandleByInstance(SPI_TypeDef *instance) +void spiInternalStartDMA(const extDevice_t *dev) { - return &spiDevice[spiDeviceByInstance(instance)].hdma; + UNUSED(dev); } -void SPI1_IRQHandler(void) +void spiInternalStopDMA (const extDevice_t *dev) { - HAL_SPI_IRQHandler(&spiDevice[SPIDEV_1].hspi); + UNUSED(dev); } -void SPI2_IRQHandler(void) +void spiInternalResetDescriptors(busDevice_t *bus) { - HAL_SPI_IRQHandler(&spiDevice[SPIDEV_2].hspi); + UNUSED(bus); } -void SPI3_IRQHandler(void) +void spiInternalResetStream(dmaChannelDescriptor_t *descriptor) { - HAL_SPI_IRQHandler(&spiDevice[SPIDEV_3].hspi); + UNUSED(descriptor); } -void SPI4_IRQHandler(void) +// Transfer setup and start +void spiSequence(const extDevice_t *dev, busSegment_t *segments) { - HAL_SPI_IRQHandler(&spiDevice[SPIDEV_4].hspi); -} + 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); + } -void dmaSPIIRQHandler(dmaChannelDescriptor_t* descriptor) -{ - SPIDevice device = descriptor->userParam; - if (device != SPIINVALID) { - HAL_DMA_IRQHandler(&spiDevice[device].hdma); + // 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 // 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; @@ -68,22 +68,23 @@ typedef struct SPIDevice_s { #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,31 +71,66 @@ #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 !defined(STM32H7) + // SPI2 and SPI3 are on APB1/AHB1 which PCLK is half that of APB2/AHB2. - if (!spi->dev) { - return; + if (instance == SPI2 || instance == SPI3) { + divisor /= 2; // Safe for divisor == 0 or 1 } +#else + UNUSED(instance); +#endif + + divisor = constrain(divisor, 2, 256); -#ifndef USE_SPI_TRANSACTION - spi->leadingEdge = leadingEdge; +#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 - UNUSED(leadingEdge); + return (ffs(divisor) - 2) << SPI_CR1_BR_Pos; #endif +} + +void spiInitDevice(SPIDevice device) +{ + spiDevice_t *spi = &spiDevice[device]; + + if (!spi->dev) { + return; + } // Enable SPI clock RCC_ClockCmd(spi->rcc, ENABLE); @@ -106,78 +140,111 @@ void spiInitDevice(SPIDevice device, bool leadingEdge) 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); + IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->sckAF); 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; - } +#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 } -uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte) +void spiInternalResetDescriptors(busDevice_t *bus) { - timeUs_t timeoutStartUs = microsISR(); + LL_DMA_InitTypeDef *initTx = bus->initTx; + LL_DMA_InitTypeDef *initRx = bus->initRx; - 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); + 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; } -/** - * Return true if the bus is currently in the middle of a transmission. - */ -bool spiIsBusBusy(SPI_TypeDef *instance) +void spiInternalResetStream(dmaChannelDescriptor_t *descriptor) { - return LL_SPI_GetTxFIFOLevel(instance) != LL_SPI_TX_FIFO_EMPTY - || LL_SPI_IsActiveFlag_BSY(instance); + // 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); } -bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len) + +static bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len) { - timeUs_t timeoutStartUs; - +#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) { - timeoutStartUs = microsISR(); - while (!LL_SPI_IsActiveFlag_TXE(instance)) { - if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) { - return spiTimeoutUserCallback(instance); - } - } + while (!LL_SPI_IsActiveFlag_TXE(instance)); uint16_t w; if (txData) { w = *((uint16_t *)txData); @@ -187,12 +254,7 @@ bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, } LL_SPI_TransmitData16(instance, w); - timeoutStartUs = microsISR(); - while (!CHECK_SPI_RX_DATA_AVAILABLE(instance)) { - if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) { - return spiTimeoutUserCallback(instance); - } - } + while (!LL_SPI_IsActiveFlag_RXNE(instance)); w = LL_SPI_ReceiveData16(instance); if (rxData) { *((uint16_t *)rxData) = w; @@ -203,104 +265,324 @@ bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, // 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); - } - } + while (!LL_SPI_IsActiveFlag_TXE(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); - } - } + while (!LL_SPI_IsActiveFlag_RXNE(instance)); b = LL_SPI_ReceiveData8(instance); if (rxData) { *(rxData++) = b; } --len; } +#endif return true; } -static uint16_t spiDivisorToBRbits(SPI_TypeDef *instance, uint16_t divisor) +void spiInternalInitStream(const extDevice_t *dev, bool preInit) { -#if !(defined(STM32F1) || defined(STM32F3)) - // SPI2 and SPI3 are on APB1/AHB1 which PCLK is half that of APB2/AHB2. + 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; + } + } - if (instance == SPI2 || instance == SPI3) { - divisor /= 2; // Safe for divisor == 0 or 1 + 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 - UNUSED(instance); + // 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; +} - divisor = constrain(divisor, 2, 256); +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); - return (ffs(divisor) - 2) << SPI_CR1_BR_Pos; + SET_BIT(dev->bus->busType_u.spi.instance->CR2, SPI_CR2_TXDMAEN | SPI_CR2_RXDMAEN); +#endif } -void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) +void spiInternalStopDMA (const extDevice_t *dev) { - LL_SPI_Disable(instance); - LL_SPI_SetBaudRatePrescaler(instance, spiDivisorToBRbits(instance, divisor)); - LL_SPI_Enable(instance); + 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 } -#ifdef USE_SPI_TRANSACTION -void spiBusTransactionInit(busDevice_t *bus, SPIMode_e mode, uint16_t divisor) +// DMA transfer setup and start +void spiSequence(const extDevice_t *dev, busSegment_t *segments) { - 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; + 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; } - LL_SPI_Disable(bus->busdev_u.spi.instance); - LL_SPI_DeInit(bus->busdev_u.spi.instance); + // 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); + } - LL_SPI_Init(bus->busdev_u.spi.instance, &defaultInit); - LL_SPI_SetBaudRatePrescaler(bus->busdev_u.spi.instance, spiDivisorToBRbits(bus->busdev_u.spi.instance, divisor)); + bus->busType_u.spi.leadingEdge = dev->busType_u.spi.leadingEdge; + } - // Configure for 8-bit reads. XXX Is this STM32F303xC specific? - LL_SPI_SetRxFIFOThreshold(bus->busdev_u.spi.instance, SPI_RXFIFO_THRESHOLD_QF); +#if !defined(STM32H7) + LL_SPI_Enable(instance); +#endif - LL_SPI_Enable(bus->busdev_u.spi.instance); + /* 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; + } - bus->busdev_u.spi.device = &spiDevice[spiDeviceByInstance(bus->busdev_u.spi.instance)]; - bus->busdev_u.spi.modeCache = bus->busdev_u.spi.instance->CR1; -} + // Use DMA if possible + if (bus->useDMA && dmaSafe && ((segmentCount > 1) || (xferLen > 8))) { + // Intialise the init structures for the first transfer + spiInternalInitStream(dev, false); -void spiBusTransactionSetup(const busDevice_t *bus) -{ - // We rely on MSTR bit to detect valid modeCache + // 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->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; + 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++; + } - // 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"); + bus->curSegment = (busSegment_t *)NULL; } } -#endif // USE_SPI_TRANSACTION #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,9 +45,36 @@ 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) +static uint16_t spiDivisorToBRbits(SPI_TypeDef *instance, uint16_t divisor) +{ + // 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 + } +#endif + + divisor = constrain(divisor, 2, 256); + + return (ffs(divisor) - 2) << 3; // SPI_CR1_BR_Pos +} + +static void spiSetDivisorBRreg(SPI_TypeDef *instance, uint16_t divisor) +{ +#define BR_BITS ((BIT(5) | BIT(4) | BIT(3))) + const uint16_t tempRegister = (instance->CR1 & ~BR_BITS); + instance->CR1 = tempRegister | spiDivisorToBRbits(instance, divisor); +#undef BR_BITS +} + + +void spiInitDevice(SPIDevice device) { spiDevice_t *spi = &(spiDevice[device]); @@ -53,12 +82,6 @@ void spiInitDevice(SPIDevice device, bool leadingEdge) 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); @@ -67,118 +90,65 @@ void spiInitDevice(SPIDevice device, bool leadingEdge) 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->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); -#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_I2S_DMACmd(spi->dev, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, DISABLE); 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) +void spiInternalResetDescriptors(busDevice_t *bus) { - 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 + 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; } -/** - * Return true if the bus is currently in the middle of a transmission. - */ -bool spiIsBusBusy(SPI_TypeDef *instance) +void spiInternalResetStream(dmaChannelDescriptor_t *descriptor) { -#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 + 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); } -bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len) +static bool spiInternalReadWriteBufPolled(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 + while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET); 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 + while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET); b = SPI_I2S_ReceiveData(instance); -#endif if (rxData) { *(rxData++) = b; } @@ -187,86 +157,210 @@ bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, return true; } -static uint16_t spiDivisorToBRbits(SPI_TypeDef *instance, uint16_t divisor) +void spiInternalInitStream(const extDevice_t *dev, bool preInit) { -#if !(defined(STM32F1) || defined(STM32F3)) - // SPI2 and SPI3 are on APB1/AHB1 which PCLK is half that of APB2/AHB2. - - if (instance == SPI2 || instance == SPI3) { - divisor /= 2; // Safe for divisor == 0 or 1 + 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; + } } -#else - UNUSED(instance); -#endif - divisor = constrain(divisor, 2, 256); + uint8_t *txData = segment->txData; + uint8_t *rxData = segment->rxData; + int len = segment->len; - return (ffs(divisor) - 2) << 3; // SPI_CR1_BR_Pos + 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; } -static void spiSetDivisorBRreg(SPI_TypeDef *instance, uint16_t divisor) +void spiInternalStartDMA(const extDevice_t *dev) { -#define BR_BITS ((BIT(5) | BIT(4) | BIT(3))) - const uint16_t tempRegister = (instance->CR1 & ~BR_BITS); - instance->CR1 = tempRegister | spiDivisorToBRbits(instance, divisor); -#undef BR_BITS + // 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 spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) + +void spiInternalStopDMA (const extDevice_t *dev) { - SPI_Cmd(instance, DISABLE); - spiSetDivisorBRreg(instance, divisor); - SPI_Cmd(instance, ENABLE); -} + 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; -#ifdef USE_SPI_TRANSACTION + SPI_I2S_DMACmd(instance, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, DISABLE); +} -void spiBusTransactionInit(busDevice_t *bus, SPIMode_e mode, uint16_t divider) +// DMA transfer setup and start +void spiSequence(const extDevice_t *dev, busSegment_t *segments) { - 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; + 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); + + // 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; } - // Initialize the SPI instance to setup CR1 + if (dev->busType_u.spi.leadingEdge != bus->busType_u.spi.leadingEdge) { + // Switch SPI clock polarity/phase + instance->CR1 &= ~(SPI_CPOL_High | SPI_CPHA_2Edge); - 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 + // 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; + } - bus->busdev_u.spi.modeCache = bus->busdev_u.spi.instance->CR1; - bus->busdev_u.spi.device = &spiDevice[spiDeviceByInstance(bus->busdev_u.spi.instance)]; -} + SPI_Cmd(instance, ENABLE); -void spiBusTransactionSetup(const busDevice_t *bus) -{ - // We rely on MSTR bit to detect valid modeCache + // 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); + + // 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->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; + 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++; + } - // 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"); + 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); - -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); +bool ug2864hsweg01InitI2C(const extDevice_t *dev); + +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; - - // 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 + uint8_t readIdResponse[4] = { 0 }; + + 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) -{ - IOHi(bus->busdev_u.spi.csnPin); - __NOP(); -} - -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) +static uint8_t m25p16_readStatus(flashDevice_t *fdevice) { -#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 -} + STATIC_DMA_DATA_AUTO uint8_t readStatus[2] = { M25P16_INSTRUCTION_READ_STATUS_REG, 0 }; + STATIC_DMA_DATA_AUTO uint8_t readyStatus[2]; -/** - * 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); + spiReadWriteBuf(fdevice->io.handle.dev, readStatus, readyStatus, sizeof (readStatus)); - // Assume that we're about to do some writing, so the device is just about to become busy - fdevice->couldBeBusy = true; + return readyStatus[1]; } -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; + } - return !fdevice->couldBeBusy; -} + // Poll the FLASH device to see if it's busy + fdevice->couldBeBusy = ((m25p16_readStatus(fdevice) & M25P16_STATUS_FLAG_WRITE_IN_PROGRESS) != 0); -static void m25p16_setTimeout(flashDevice_t *fdevice, uint32_t timeoutMillis) -{ - uint32_t now = millis(); - fdevice->timeoutAt = now + timeoutMillis; + return !fdevice->couldBeBusy; } 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; } -/** - * 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 }; - - m25p16_setCommandAddress(&out[1], address, fdevice->isLargeFlash); - - m25p16_waitForReady(fdevice); - m25p16_writeEnable(fdevice); +// Called in ISR context +// A write enable has just been issued +busStatus_e m25p16_callbackWriteEnable(uint32_t arg) +{ + flashDevice_t *fdevice = (flashDevice_t *)arg; - m25p16_transfer(fdevice->io.handle.busdev, out, NULL, fdevice->isLargeFlash ? 5 : 4); + // As a write has just occurred, the device could be busy + fdevice->couldBeBusy = true; - m25p16_setTimeout(fdevice, SECTOR_ERASE_TIMEOUT_MILLIS); + return BUS_READY; } -static void m25p16_eraseCompletely(flashDevice_t *fdevice) +// Called in ISR context +// Check if the status was busy and if so repeat the poll +busStatus_e m25p16_callbackReady(uint32_t arg) { - m25p16_waitForReady(fdevice); + flashDevice_t *fdevice = (flashDevice_t *)arg; + extDevice_t *dev = fdevice->io.handle.dev; - m25p16_writeEnable(fdevice); + uint8_t readyPoll = dev->bus->curSegment->rxData[1]; + + if (readyPoll & M25P16_STATUS_FLAG_WRITE_IN_PROGRESS) { + return BUS_BUSY; + } - m25p16_performOneByteCommand(fdevice->io.handle.busdev, M25P16_INSTRUCTION_BULK_ERASE); + // Bus is now known not to be busy + fdevice->couldBeBusy = false; - m25p16_setTimeout(fdevice, BULK_ERASE_TIMEOUT_MILLIS); + return BUS_READY; } -static void m25p16_pageProgramBegin(flashDevice_t *fdevice, uint32_t address) -{ - UNUSED(fdevice); - fdevice->currentWriteAddress = address; +/** + * 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) +{ + 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}, + }; + + // Ensure any prior DMA has completed before continuing + spiWaitClaim(fdevice->io.handle.dev); + + m25p16_setCommandAddress(§orErase[1], address, fdevice->isLargeFlash); + + spiSequence(fdevice->io.handle.dev, &segments[0]); + + // Block pending completion of SPI access, but the erase will be ongoing + spiWait(fdevice->io.handle.dev); } -static void m25p16_pageProgramContinue(flashDevice_t *fdevice, const uint8_t *data, int length) +static void m25p16_eraseCompletely(flashDevice_t *fdevice) { - uint8_t command[5] = { M25P16_INSTRUCTION_PAGE_PROGRAM }; - - m25p16_setCommandAddress(&command[1], fdevice->currentWriteAddress, fdevice->isLargeFlash); - - 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}, + }; + + // Ensure any prior DMA has completed before continuing + spiWaitClaim(fdevice->io.handle.dev); + + spiSequence(fdevice->io.handle.dev, &segments[0]); + + // Block pending completion of SPI access, but the erase will be ongoing + spiWait(fdevice->io.handle.dev); +} - m25p16_writeEnable(fdevice); -#ifdef USE_SPI_TRANSACTION - spiBusTransactionBegin(fdevice->io.handle.busdev); -#else - m25p16_enable(fdevice->io.handle.busdev); -#endif +static void m25p16_pageProgramBegin(flashDevice_t *fdevice, uint32_t address) +{ + fdevice->currentWriteAddress = address; +} - 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); -#ifdef USE_SPI_TRANSACTION - spiBusTransactionEnd(fdevice->io.handle.busdev); -#else - m25p16_disable(fdevice->io.handle.busdev); -#endif +static void m25p16_pageProgramContinue(flashDevice_t *fdevice, const uint8_t *data, int length) +{ + // 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 }; + + 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}, + }; + + // Ensure any prior DMA has completed before continuing + spiWaitClaim(fdevice->io.handle.dev); + + // Patch the pageProgram segment + segments[2].len = fdevice->isLargeFlash ? 5 : 4; + m25p16_setCommandAddress(&pageProgram[1], fdevice->currentWriteAddress, fdevice->isLargeFlash); + + // Patch the data segment + segments[3].txData = (uint8_t *)data; + segments[3].len = length; + + spiSequence(fdevice->io.handle.dev, fdevice->couldBeBusy ? &segments[0] : &segments[1]); + + 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 }; - - m25p16_setCommandAddress(&command[1], address, fdevice->isLargeFlash); + 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 }; - if (!m25p16_waitForReady(fdevice)) { - return 0; - } + // Ensure any prior DMA has completed before continuing + spiWaitClaim(fdevice->io.handle.dev); -#ifndef FLASH_SPI_SHARED - spiSetDivisor(fdevice->io.handle.busdev->busdev_u.spi.instance, spiCalculateDivider(maxReadClkSPIHz)); -#endif + 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}, + }; -#ifdef USE_SPI_TRANSACTION - spiBusTransactionBegin(fdevice->io.handle.busdev); -#else - m25p16_enable(fdevice->io.handle.busdev); -#endif + // Patch the readBytes command + m25p16_setCommandAddress(&readBytes[1], address, fdevice->isLargeFlash); - 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); + spiSetClkDivisor(fdevice->io.handle.dev, spiCalculateDivider(maxReadClkSPIHz)); -#ifdef USE_SPI_TRANSACTION - spiBusTransactionEnd(fdevice->io.handle.busdev); -#else - m25p16_disable(fdevice->io.handle.busdev); -#endif + spiSequence(fdevice->io.handle.dev, fdevice->couldBeBusy ? &segments[0] : &segments[1]); -#ifndef FLASH_SPI_SHARED - spiSetDivisor(fdevice->io.handle.busdev->busdev_u.spi.instance, spiCalculateDivider(maxClkSPIHz)); -#endif + // Block until code is re-factored to exploit non-blocking + spiWait(fdevice->io.handle.dev); - 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; + + busSegment_t segments[] = { + {&command, NULL, sizeof (command), true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + + // Ensure any prior DMA has completed before continuing + spiWait(dev); - ENABLE(busdev); - spiTransferByte(busdev->busdev_u.spi.instance, command); - DISABLE(busdev); + 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 }; + + busSegment_t segments[] = { + {cmd, NULL, sizeof (cmd), true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + + // Ensure any prior DMA has completed before continuing + spiWait(dev); - ENABLE(busdev); - spiTransfer(busdev->busdev_u.spi.instance, cmd, NULL, sizeof(cmd)); - DISABLE(busdev); + 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 }; + + 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]); - ENABLE(busdev); - spiTransfer(busdev->busdev_u.spi.instance, cmd, NULL, sizeof(cmd)); - spiTransfer(busdev->busdev_u.spi.instance, data, NULL, length); - DISABLE(busdev); + // 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}, + }; + + // Ensure any prior DMA has completed before continuing + spiWait(dev); - spiTransfer(busdev->busdev_u.spi.instance, cmd, NULL, 2); + 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); - - __spiBusTransactionBegin(busdev); + spiSetClkDivisor(dev, spiCalculateDivider(MAX7456_INIT_MAX_SPI_CLK_HZ)); - uint8_t osdm = max7456Send(MAX7456ADD_OSDM|MAX7456ADD_READ, 0xff); + // Write 0xff to conclude any current SPI transaction the MAX7456 is expecting + spiWrite(dev, END_STRING); - __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) } } -bool rxSpiDeviceInit(const rxSpiConfig_t *rxSpiConfig) +void rxSpiNormalSpeed() { - SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(rxSpiConfig->spibus)); + spiSetClkDivisor(dev, spiCalculateDivider(RX_MAX_SPI_CLK_HZ)); +} - if (!instance) { +void rxSpiStartupSpeed() +{ + spiSetClkDivisor(dev, spiCalculateDivider(RX_STARTUP_MAX_SPI_CLK_HZ)); +} + +bool rxSpiDeviceInit(const rxSpiConfig_t *rxSpiConfig) +{ + 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; + + // 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}, + }; - return false; + 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}, + }; + + // Ensure any prior DMA has completed before continuing + spiWaitClaim(&sdcard.dev); + + spiSequence(&sdcard.dev, &segments[0]); - // Discard trailing CRC, we don't care - spiBusTransferByte(&sdcard.busdev, 0xFF); - spiBusTransferByte(&sdcard.busdev, 0xFF); + // 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}, + }; + + // Ensure any prior DMA has completed before continuing + spiWaitClaim(&sdcard.dev); - // Send a dummy CRC - spiBusTransferByte(&sdcard.busdev, 0x00); - spiBusTransferByte(&sdcard.busdev, 0x00); + spiSequence(&sdcard.dev, &segments[0]); - uint8_t dataResponseToken = spiBusTransferByte(&sdcard.busdev, 0xFF); + // 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; - - 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; - - 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; + // Ensure any prior DMA has completed before continuing + spiWaitClaim(&sdcard.dev); - xDMA_DeInit(sdcard.dma->ref); - xDMA_Init(sdcard.dma->ref, &init); + spiSequence(&sdcard.dev, &segments[0]); - 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); - - // 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; - } - } + IOHi(sdcard.dev.busType_u.spi.csnPin); + + // 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}, + }; + + // Ensure any prior DMA has completed before continuing + spiWaitClaim(&sdcard.dev); - spiBusTransferByte(&sdcard.busdev, SDCARD_MULTIPLE_BLOCK_WRITE_STOP_TOKEN); + 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; - -#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); - } + sendComplete = !spiIsBusy(&sdcard.dev); - // 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; @@ -657,6 +624,10 @@ void init(void) 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(); } else { @@ -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/MERAKRCF722/target.c b/src/main/target/MERAKRCF405/target.c similarity index 72% copy from src/main/target/MERAKRCF722/target.c copy to src/main/target/MERAKRCF405/target.c index a68368fe5..54f4a5837 100644 --- a/src/main/target/MERAKRCF722/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 -- 2.11.4.GIT