diff --git a/Makefile b/Makefile index 1dde7cc6efb..56132fb16cc 100644 --- a/Makefile +++ b/Makefile @@ -367,11 +367,8 @@ $(TARGET_BIN): $(TARGET_UNPATCHED_BIN) @echo "Patching MD5 hash into HASH section" "$(STDOUT)" $(V1) cat $(TARGET_UNPATCHED_BIN).md5 | awk '{printf("%08x: %s",64-16,$$2);}' | xxd -r - $(TARGET_EXST_HASH_SECTION_FILE) -# For some currently unknown reason, OBJCOPY, with only input/output files, will generate a file around 2GB for the H730 unless we remove an unused-section -# As a workaround drop the ._user_heap_stack section, which is only used during build to show errors if there's not enough space for the heap/stack. -# The issue can be seen with `readelf -S $(TARGET_EXST_ELF)' vs `readelf -S $(TARGET_ELF)` $(V1) @echo "Patching updated HASH section into $(TARGET_EXST_ELF)" "$(STDOUT)" - $(OBJCOPY) $(TARGET_ELF) $(TARGET_EXST_ELF) --remove-section ._user_heap_stack --update-section .exst_hash=$(TARGET_EXST_HASH_SECTION_FILE) + $(OBJCOPY) $(TARGET_ELF) $(TARGET_EXST_ELF) --update-section .exst_hash=$(TARGET_EXST_HASH_SECTION_FILE) $(V1) $(READELF) -S $(TARGET_EXST_ELF) $(V1) $(READELF) -l $(TARGET_EXST_ELF) diff --git a/make/mcu/STM32H7.mk b/make/mcu/STM32H7.mk index 5649818ecb6..69f5fd2cc1b 100644 --- a/make/mcu/STM32H7.mk +++ b/make/mcu/STM32H7.mk @@ -207,9 +207,22 @@ else ifeq ($(TARGET),$(filter $(TARGET),$(H723xG_TARGETS))) DEVICE_FLAGS += -DSTM32H723xx DEFAULT_LD_SCRIPT = $(LINKER_DIR)/stm32_flash_h723_1m.ld STARTUP_SRC = startup_stm32h723xx.s -MCU_FLASH_SIZE := 1024 +DEFAULT_TARGET_FLASH := 1024 DEVICE_FLAGS += -DMAX_MPU_REGIONS=16 +ifeq ($(TARGET_FLASH),) +MCU_FLASH_SIZE := $(DEFAULT_TARGET_FLASH) +endif + +ifeq ($(EXST),yes) +FIRMWARE_SIZE := 1024 +# TARGET_FLASH now becomes the amount of MEMORY-MAPPED address space that is occupied by the firmware +# and the maximum size of the data stored on the external flash device. +MCU_FLASH_SIZE := FIRMWARE_SIZE +DEFAULT_LD_SCRIPT = $(LINKER_DIR)/stm32_ram_h723_exst.ld +LD_SCRIPTS = $(LINKER_DIR)/stm32_h723_common.ld $(LINKER_DIR)/stm32_h723_common_post.ld +endif + else ifeq ($(TARGET),$(filter $(TARGET),$(H725xG_TARGETS))) DEVICE_FLAGS += -DSTM32H725xx DEFAULT_LD_SCRIPT = $(LINKER_DIR)/stm32_flash_h723_1m.ld @@ -226,7 +239,7 @@ DEVICE_FLAGS += -DMAX_MPU_REGIONS=16 ifeq ($(TARGET_FLASH),) -MCU_FLASH_SIZE := $(DEFAULT_TARGET_FLASH) +MCU_FLASH_SIZE := $(DEFAULT_TARGET_FLASH) endif ifeq ($(EXST),yes) @@ -246,7 +259,7 @@ STARTUP_SRC = startup_stm32h743xx.s DEFAULT_TARGET_FLASH := 128 ifeq ($(TARGET_FLASH),) -MCU_FLASH_SIZE := $(DEFAULT_TARGET_FLASH) +MCU_FLASH_SIZE := $(DEFAULT_TARGET_FLASH) endif ifeq ($(EXST),yes) @@ -281,7 +294,7 @@ LD_SCRIPT = $(DEFAULT_LD_SCRIPT) endif ifneq ($(FIRMWARE_SIZE),) -DEVICE_FLAGS += -DFIRMWARE_SIZE=$(FIRMWARE_SIZE) +DEVICE_FLAGS += -DFIRMWARE_SIZE=$(FIRMWARE_SIZE) endif DEVICE_FLAGS += -DHSE_VALUE=$(HSE_VALUE) -DHSE_STARTUP_TIMEOUT=1000 @@ -304,6 +317,7 @@ MCU_COMMON_SRC = \ drivers/serial_uart_hal.c \ drivers/serial_uart_stm32h7xx.c \ drivers/bus_quadspi_hal.c \ + drivers/bus_octospi_stm32h7xx.c \ drivers/bus_spi_ll.c \ drivers/dma_stm32h7xx.c \ drivers/dshot_bitbang.c \ diff --git a/make/source.mk b/make/source.mk index 1590e1b4860..92ea4a2aaeb 100644 --- a/make/source.mk +++ b/make/source.mk @@ -20,6 +20,7 @@ COMMON_SRC = \ drivers/bus_i2c_config.c \ drivers/bus_i2c_busdev.c \ drivers/bus_i2c_soft.c \ + drivers/bus_octospi.c \ drivers/bus_quadspi.c \ drivers/bus_spi.c \ drivers/bus_spi_config.c \ diff --git a/src/link/stm32_h723_common.ld b/src/link/stm32_h723_common.ld new file mode 100644 index 00000000000..d10f3d957fa --- /dev/null +++ b/src/link/stm32_h723_common.ld @@ -0,0 +1,187 @@ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Highest address of the user mode stack */ +_estack = ORIGIN(STACKRAM) + LENGTH(STACKRAM); /* end of RAM */ + +/* Base address where the quad spi. */ +__octospi1_start = ORIGIN(OCTOSPI1); +__octospi2_start = ORIGIN(OCTOSPI2); + +/* Generate a link error if heap and stack don't fit into RAM */ +_Min_Heap_Size = 0; /* required amount of heap */ +_Min_Stack_Size = 0x800; /* required amount of stack */ + +/* Define output sections */ +SECTIONS +{ + _isr_vector_table_flash_base = LOADADDR(.isr_vector); + PROVIDE (isr_vector_table_flash_base = _isr_vector_table_flash_base); + + .isr_vector : + { + . = ALIGN(512); + PROVIDE (isr_vector_table_base = .); + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + PROVIDE (isr_vector_table_end = .); + } >VECTAB AT> MAIN + + _ram_isr_vector_table_base = LOADADDR(.ram_isr_vector); + PROVIDE (ram_isr_vector_table_base = _ram_isr_vector_table_base); + + .ram_isr_vector (NOLOAD) : + { + . = ALIGN(512); /* Vector table offset must be multiple of 0x200 */ + PROVIDE (ram_isr_vector_table_base = .); + . += (isr_vector_table_end - isr_vector_table_base); + . = ALIGN(4); + PROVIDE (ram_isr_vector_table_end = .); + } >DTCM_RAM + + /* The program code and other data goes into MAIN */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + } >MAIN + + /* Critical program code goes into ITCM RAM */ + /* Copy specific fast-executing code to ITCM RAM */ + tcm_code = LOADADDR(.tcm_code); + .tcm_code : + { + . = ALIGN(4); + tcm_code_start = .; + *(.tcm_code) + *(.tcm_code*) + . = ALIGN(4); + tcm_code_end = .; + } >ITCM_RAM AT >MAIN + + .ARM.extab : + { + *(.ARM.extab* .gnu.linkonce.armextab.*) + } >MAIN + + .ARM : + { + __exidx_start = .; + *(.ARM.exidx*) __exidx_end = .; + } >MAIN + + .pg_registry : + { + PROVIDE_HIDDEN (__pg_registry_start = .); + KEEP (*(.pg_registry)) + KEEP (*(SORT(.pg_registry.*))) + PROVIDE_HIDDEN (__pg_registry_end = .); + } >MAIN + + .pg_resetdata : + { + PROVIDE_HIDDEN (__pg_resetdata_start = .); + KEEP (*(.pg_resetdata)) + PROVIDE_HIDDEN (__pg_resetdata_end = .); + } >MAIN + + /* used by the startup to initialize data */ + _sidata = LOADADDR(.data); + + /* Initialized data sections goes into RAM, load LMA copy after code */ + .data : + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + } >DTCM_RAM AT >MAIN + + /* Non-critical program code goes into RAM */ + /* Copy specific slow-executing code to RAM */ + ram_code = LOADADDR(.ram_code); + .ram_code : + { + . = ALIGN(4); + ram_code_start = .; + *(.ram_code) + *(.ram_code*) + . = ALIGN(4); + ram_code_end = .; + } >RAM AT >MAIN + + /* Uninitialized data section */ + . = ALIGN(4); + .bss (NOLOAD) : + { + /* This is used by the startup in order to initialize the .bss secion */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.bss) + *(SORT_BY_ALIGNMENT(.bss*)) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + } >RAM + + /* Uninitialized data section */ + . = ALIGN(4); + .sram2 (NOLOAD) : + { + /* This is used by the startup in order to initialize the .sram2 secion */ + _ssram2 = .; /* define a global symbol at sram2 start */ + __sram2_start__ = _ssram2; + *(.sram2) + *(SORT_BY_ALIGNMENT(.sram2*)) + + . = ALIGN(4); + _esram2 = .; /* define a global symbol at sram2 end */ + __sram2_end__ = _esram2; + } >RAM + + /* used during startup to initialized fastram_data */ + _sfastram_idata = LOADADDR(.fastram_data); + + /* Initialized FAST_DATA section for unsuspecting developers */ + .fastram_data : + { + . = ALIGN(4); + _sfastram_data = .; /* create a global symbol at data start */ + *(.fastram_data) /* .data sections */ + *(.fastram_data*) /* .data* sections */ + + . = ALIGN(4); + _efastram_data = .; /* define a global symbol at data end */ + } >FASTRAM AT >MAIN + + . = ALIGN(4); + .fastram_bss (NOLOAD) : + { + _sfastram_bss = .; + __fastram_bss_start__ = _sfastram_bss; + *(.fastram_bss) + *(SORT_BY_ALIGNMENT(.fastram_bss*)) + + . = ALIGN(4); + _efastram_bss = .; + __fastram_bss_end__ = _efastram_bss; + } >FASTRAM +} diff --git a/src/link/stm32_h723_common_post.ld b/src/link/stm32_h723_common_post.ld new file mode 100644 index 00000000000..89b4224806f --- /dev/null +++ b/src/link/stm32_h723_common_post.ld @@ -0,0 +1,44 @@ +SECTIONS +{ + .persistent_data (NOLOAD) : + { + __persistent_data_start__ = .; + *(.persistent_data) + . = ALIGN(4); + __persistent_data_end__ = .; + } >RAM + + /* User_heap_stack section, used to check that there is enough RAM left */ + _heap_stack_end = ORIGIN(STACKRAM)+LENGTH(STACKRAM) - 8; /* 8 bytes to allow for alignment */ + _heap_stack_begin = _heap_stack_end - _Min_Stack_Size - _Min_Heap_Size; + . = _heap_stack_begin; + ._user_heap_stack : + { + . = ALIGN(4); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(4); + } >STACKRAM = 0xa5 + + /* MEMORY_bank1 section, code must be located here explicitly */ + /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ + .memory_b1_text : + { + *(.mb1text) /* .mb1text sections (code) */ + *(.mb1text*) /* .mb1text* sections (code) */ + *(.mb1rodata) /* read-only data (constants) */ + *(.mb1rodata*) + } >MEMORY_B1 + + /* Remove information from the standard libraries */ + /DISCARD/ : + { + libc.a ( * ) + libm.a ( * ) + libgcc.a ( * ) + } + + .ARM.attributes 0 : { *(.ARM.attributes) } +} diff --git a/src/link/stm32_h730_common.ld b/src/link/stm32_h730_common.ld index 0c99b43778a..db81e6c9e6d 100644 --- a/src/link/stm32_h730_common.ld +++ b/src/link/stm32_h730_common.ld @@ -128,6 +128,19 @@ SECTIONS _edata = .; /* define a global symbol at data end */ } >DTCM_RAM AT >MAIN + /* Non-critical program code goes into RAM */ + /* Copy specific slow-executing code to RAM */ + ram_code = LOADADDR(.ram_code); + .ram_code : + { + . = ALIGN(4); + ram_code_start = .; + *(.ram_code) + *(.ram_code*) + . = ALIGN(4); + ram_code_end = .; + } >RAM AT >MAIN + /* Uninitialized data section */ . = ALIGN(4); .bss (NOLOAD) : diff --git a/src/link/stm32_ram_h723_exst.ld b/src/link/stm32_ram_h723_exst.ld new file mode 100644 index 00000000000..c9f0bcd8741 --- /dev/null +++ b/src/link/stm32_ram_h723_exst.ld @@ -0,0 +1,143 @@ +/* +***************************************************************************** +** +** File : stm32_ram_h723_exst.ld +** +** Abstract : Linker script for STM32H723xG Device with +** 320K AXI-SRAM mapped onto AXI bus on D1 domain + (Shared AXI/I-TCM 192KB is all configured as AXI-SRAM) +** 16K SRAM1 mapped on D2 domain +** 16K SRAM2 mapped on D2 domain +** 16K SRAM mapped on D3 domain +** 64K ITCM +** 128K DTCM +** +***************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* +0x00000000 to 0x0000FFFF 64K ITCM +0x20000000 to 0x2001FFFF 128K DTCM, main RAM +0x24000000 to 0x2404FFFF 320K AXI SRAM, D1 domain +0x30000000 to 0x30003FFF 16K SRAM1, D2 domain +0x30004000 to 0x30007FFF 16K SRAM2, D2 domain +0x38000000 to 0x38003FFF 16K SRAM4, D3 domain, unused +0x38800000 to 0x38800FFF 4K BACKUP SRAM, Backup domain, unused + +0x08000000 to 0x0801FFFF 128K isr vector, startup code, firmware, no config! // FLASH_Sector_0 +*/ + +/* + +For H7 EXFL (External Flash) targets a binary is built that is placed on an external device. +The bootloader will enable the memory mapped mode on the CPU which allows code to run directly from +the external flash device. + +The bootloader then executes code at the CODE_RAM address. The address of CODE_RAM is fixed to 0x90010000 +and must not be changed. + +The initial CODE_RAM is sized at 1MB. + +*/ + +/* see .exst section below */ +_exst_hash_size = 64; + +/* Specify the memory areas */ +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + DTCM_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + RAM (rwx) : ORIGIN = 0x24000000, LENGTH = 128K + 192K /* 128K AXI SRAM + 192K ITCM & AXI = 320K */ + + D2_RAM (rwx) : ORIGIN = 0x30000000, LENGTH = 32K /* SRAM1 16K + SRAM2 16K */ + D3_RAM (rwx) : ORIGIN = 0x38000000, LENGTH = 16K /* SRAM4 16K */ + + BACKUP_SRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K + + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K + + OCTOSPI2 (rx) : ORIGIN = 0x70000000, LENGTH = 256M + OCTOSPI1 (rx) : ORIGIN = 0x90000000, LENGTH = 256M + OCTOSPI1_CODE (rx): ORIGIN = ORIGIN(OCTOSPI1) + 1M, LENGTH = 1M - _exst_hash_size /* hard coded start address, as required by SPRACINGH7 boot loader, don't change! */ + EXST_HASH (rx) : ORIGIN = ORIGIN(OCTOSPI1_CODE) + LENGTH(OCTOSPI1_CODE), LENGTH = _exst_hash_size +} + +REGION_ALIAS("STACKRAM", DTCM_RAM) +REGION_ALIAS("FASTRAM", DTCM_RAM) +REGION_ALIAS("MAIN", OCTOSPI1_CODE) + +REGION_ALIAS("VECTAB", MAIN) + +INCLUDE "stm32_h723_common.ld" + +SECTIONS +{ + /* used during startup to initialized dmaram_data */ + _sdmaram_idata = LOADADDR(.dmaram_data); + + . = ALIGN(32); + .dmaram_data : + { + 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; + } >RAM + + .DMA_RW_D2 (NOLOAD) : + { + . = ALIGN(32); + PROVIDE(dmarw_start = .); + _sdmarw = .; + _dmarw_start__ = _sdmarw; + KEEP(*(.DMA_RW)) + PROVIDE(dmarw_end = .); + _edmarw = .; + _dmarw_end__ = _edmarw; + } >D2_RAM + + .DMA_RW_AXI (NOLOAD) : + { + . = ALIGN(32); + PROVIDE(dmarwaxi_start = .); + _sdmarwaxi = .; + _dmarwaxi_start__ = _sdmarwaxi; + KEEP(*(.DMA_RW_AXI)) + PROVIDE(dmarwaxi_end = .); + _edmarwaxi = .; + _dmarwaxi_end__ = _edmarwaxi; + } >RAM +} + +INCLUDE "stm32_h723_common_post.ld" +INCLUDE "stm32_ram_h723_exst_post.ld" + diff --git a/src/link/stm32_ram_h723_exst_post.ld b/src/link/stm32_ram_h723_exst_post.ld new file mode 100644 index 00000000000..97ed6c28f3a --- /dev/null +++ b/src/link/stm32_ram_h723_exst_post.ld @@ -0,0 +1,29 @@ +SECTIONS +{ + /* Create space for a hash. Currently an MD5 has is used, which is 16 */ + /* bytes long. however the last 64 bytes are RESERVED for hash related */ + .exst_hash : + { + /* 64 bytes is the size of an MD5 hashing block size. */ + . = ORIGIN(EXST_HASH); + + BYTE(0x00); /* block format */ + BYTE(0x00); /* Checksum method, 0x00 = MD5 hash */ + BYTE(0x00); /* Reserved */ + BYTE(0x00); /* Reserved */ + + /* Fill the last 60 bytes with data, including an empty hash aligned */ + + /* to the last 16 bytes. */ + FILL(0x00000000); /* Reserved */ + + . = ORIGIN(EXST_HASH) + LENGTH(EXST_HASH) - 16; + __md5_hash_address__ = .; + LONG(0x00000000); + LONG(0x00000000); + LONG(0x00000000); + LONG(0x00000000); + . = ORIGIN(EXST_HASH) + LENGTH(EXST_HASH); + __firmware_end__ = .; + } >EXST_HASH +} diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 95208d7c59d..6031adb4370 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -2582,8 +2582,8 @@ static void cliFlashRead(const char *cmdName, char *cmdline) cliPrintLinefeed(); } } -#endif -#endif +#endif // USE_FLASH_TOOLS +#endif // USE_FLASHFS #ifdef USE_VTX_CONTROL static void printVtx(dumpFlags_t dumpMask, const vtxConfig_t *vtxConfig, const vtxConfig_t *vtxConfigDefault, const char *headingStr) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index f406524b72a..d46030f0251 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1592,7 +1592,7 @@ const clivalue_t valueTable[] = { { "usb_msc_pin_pullup", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_USB_CONFIG, offsetof(usbDev_t, mscButtonUsePullup) }, #endif // PG_FLASH_CONFIG -#ifdef USE_FLASH_CHIP +#ifdef USE_FLASH_SPI { "flash_spi_bus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_FLASH_CONFIG, offsetof(flashConfig_t, spiDevice) }, #endif // RCDEVICE diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c index d9adb982ea8..1e166cab7b7 100644 --- a/src/main/config/config_eeprom.c +++ b/src/main/config/config_eeprom.c @@ -83,8 +83,8 @@ typedef struct { uint32_t word; } PG_PACKED packingTest_t; -#if defined(CONFIG_IN_EXTERNAL_FLASH) -bool loadEEPROMFromExternalFlash(void) +#if defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH) +MMFLASH_CODE bool loadEEPROMFromExternalFlash(void) { const flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_CONFIG); const flashGeometry_t *flashGeometry = flashGetGeometry(); @@ -95,7 +95,9 @@ bool loadEEPROMFromExternalFlash(void) int bytesRead = 0; bool success = false; - +#ifdef CONFIG_IN_MEMORY_MAPPED_FLASH + flashMemoryMappedModeDisable(); +#endif do { bytesRead = flashReadBytes(flashStartAddress + totalBytesRead, &eepromData[totalBytesRead], EEPROM_SIZE - totalBytesRead); if (bytesRead > 0) { @@ -103,9 +105,55 @@ bool loadEEPROMFromExternalFlash(void) success = (totalBytesRead == EEPROM_SIZE); } } while (!success && bytesRead > 0); +#ifdef CONFIG_IN_MEMORY_MAPPED_FLASH + flashMemoryMappedModeEnable(); +#endif return success; } + +#ifdef CONFIG_IN_MEMORY_MAPPED_FLASH +MMFLASH_CODE_NOINLINE void saveEEPROMToMemoryMappedFlash(void) +{ + const flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_CONFIG); + const flashGeometry_t *flashGeometry = flashGetGeometry(); + + uint32_t flashSectorSize = flashGeometry->sectorSize; + uint32_t flashPageSize = flashGeometry->pageSize; + + uint32_t flashStartAddress = flashPartition->startSector * flashGeometry->sectorSize; + + uint32_t bytesRemaining = EEPROM_SIZE; + uint32_t offset = 0; + + flashMemoryMappedModeDisable(); + + do { + uint32_t flashAddress = flashStartAddress + offset; + + uint32_t bytesToWrite = bytesRemaining; + if (bytesToWrite > flashPageSize) { + bytesToWrite = flashPageSize; + } + + bool onSectorBoundary = flashAddress % flashSectorSize == 0; + if (onSectorBoundary) { + flashEraseSector(flashAddress); + } + + flashPageProgram(flashAddress, (uint8_t *)&eepromData[offset], bytesToWrite, NULL); + + bytesRemaining -= bytesToWrite; + offset += bytesToWrite; + } while (bytesRemaining > 0); + + flashWaitForReady(); + + flashMemoryMappedModeEnable(); +} +#endif + + #elif defined(CONFIG_IN_SDCARD) enum { @@ -255,7 +303,7 @@ void initEEPROM(void) #if defined(CONFIG_IN_FILE) loadEEPROMFromFile(); -#elif defined(CONFIG_IN_EXTERNAL_FLASH) +#elif defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH) bool eepromLoaded = loadEEPROMFromExternalFlash(); if (!eepromLoaded) { // Flash read failed - just die now @@ -336,7 +384,7 @@ uint16_t getEEPROMConfigSize(void) size_t getEEPROMStorageSize(void) { -#if defined(CONFIG_IN_EXTERNAL_FLASH) +#if defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH) const flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_CONFIG); return FLASH_PARTITION_SECTOR_COUNT(flashPartition) * flashGetGeometry()->sectorSize; @@ -464,7 +512,7 @@ void writeConfigToEEPROM(void) if (writeSettingsToEEPROM()) { success = true; -#ifdef CONFIG_IN_EXTERNAL_FLASH +#if defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH) // copy it back from flash to the in-memory buffer. success = loadEEPROMFromExternalFlash(); #endif diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c index b66c1ba793a..0d4ef55faf8 100644 --- a/src/main/config/config_streamer.c +++ b/src/main/config/config_streamer.c @@ -36,7 +36,7 @@ uint8_t eepromData[EEPROM_SIZE]; #endif -#if (defined(STM32H750xx) || defined(STM32H730xx)) && !(defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_RAM) || defined(CONFIG_IN_SDCARD)) +#if (defined(STM32H750xx) || defined(STM32H730xx)) && !(defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH) || defined(CONFIG_IN_RAM) || defined(CONFIG_IN_SDCARD)) #error "The configured MCU only has one flash page which contains the bootloader, no spare flash pages available, use external storage for persistent config or ram for target testing" #endif // @todo this is not strictly correct for F4/F7, where sector sizes are variable @@ -400,7 +400,7 @@ static int write_word(config_streamer_t *c, config_streamer_buffer_align_type_t flashPageProgramContinue(buffers, bufferSizes, 1); -#elif defined(CONFIG_IN_RAM) || defined(CONFIG_IN_SDCARD) +#elif defined(CONFIG_IN_RAM) || defined(CONFIG_IN_SDCARD) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH) if (c->address == (uintptr_t)&eepromData[0]) { memset(eepromData, 0, sizeof(eepromData)); } @@ -541,11 +541,13 @@ int config_streamer_finish(config_streamer_t *c) { if (c->unlocked) { #if defined(CONFIG_IN_SDCARD) - bool saveEEPROMToSDCard(void); // XXX forward declaration to avoid circular dependency between config_streamer / config_eeprom + bool saveEEPROMToSDCard(void); // forward declaration to avoid circular dependency between config_streamer / config_eeprom saveEEPROMToSDCard(); - // TODO overwrite the data in the file on the SD card. #elif defined(CONFIG_IN_EXTERNAL_FLASH) flashFlush(); +#elif defined(CONFIG_IN_MEMORY_MAPPED_FLASH) + void saveEEPROMToMemoryMappedFlash(void); // forward declaration to avoid circular dependency between config_streamer / config_eeprom + saveEEPROMToMemoryMappedFlash(); #elif defined(CONFIG_IN_RAM) // NOP #elif defined(CONFIG_IN_FILE) diff --git a/src/main/config/config_streamer.h b/src/main/config/config_streamer.h index 20b3861a2a9..98680089980 100644 --- a/src/main/config/config_streamer.h +++ b/src/main/config/config_streamer.h @@ -26,7 +26,7 @@ // Streams data out to the EEPROM, padding to the write size as // needed, and updating the checksum as it goes. -#ifdef CONFIG_IN_EXTERNAL_FLASH +#if defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH) #define CONFIG_STREAMER_BUFFER_SIZE 8 // Must not be greater than the smallest flash page size of all compiled-in flash devices. typedef uint32_t config_streamer_buffer_align_type_t; #elif defined(STM32H743xx) || defined(STM32H750xx) || defined(STM32H723xx) || defined(STM32H725xx) diff --git a/src/main/drivers/bus_octospi.c b/src/main/drivers/bus_octospi.c new file mode 100644 index 00000000000..d51c59397a0 --- /dev/null +++ b/src/main/drivers/bus_octospi.c @@ -0,0 +1,119 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + * + * Author: Dominic Clifton + */ + +/* + * OctoSPI support. + * + * Some STM32H7 MCUs support 2 OCTOSPI peripherals, each with up to 2 flash chips, using 2/4/8 IO lines each. Small MCU packages only have one instance. + * Additionally there is an OCTOSPIM peripheral which maps OCTOSPI1/2 peripherals to IO pins and can perform arbitration. + * + * Initial implementation is focused on supporting memory-mapped flash chips connected to an OCTOSPI peripheral that is already initialised by a bootloader. + * + * As such the following things are NOT supported: + * * Configuration of IO pins. + * * Clock configuration. + * * User-configuration. + * * OCTOSPIM configuration. + * * OCTOSPI2. + * + * Should the firmware need to know about the pins used by OCTOSPI then code can be written to determine this from the registers of the OCTOSPIM and OCTOSPI1/2 peripherals. + * + * Implementation notes: + * It's not possible to use the HAL libraries without modifying them and maintaining the internal state of the HAL structures. + * The HAL libraries were not designed to support the use-case of a bootloader configuring the flash in memory mapped mode and then + * having a second piece of software (this firmware) also use the flash. Furthermore many HAL methods were not designed to run with + * interrupts disabled which is necessary as other ISRs in this firmware will be running from external flash and must be disabled. + * See HAL_OSPI_Abort, OSPI_WaitFlagStateUntilTimeout, etc. + * All code that is executed when memory mapped mode is disabled needs to run from RAM, this would also involve modification of the HAL + * libraries. + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_OCTOSPI + +#include "bus_octospi.h" +#include "bus_octospi_impl.h" + +octoSpiDevice_t octoSpiDevice[OCTOSPIDEV_COUNT] = { 0 }; + +MMFLASH_CODE_NOINLINE OCTOSPIDevice octoSpiDeviceByInstance(OCTOSPI_TypeDef *instance) +{ +#ifdef USE_OCTOSPI_DEVICE_1 + if (instance == OCTOSPI1) { + return OCTOSPIDEV_1; + } +#endif + + return OCTOSPIINVALID; +} + +OCTOSPI_TypeDef *octoSpiInstanceByDevice(OCTOSPIDevice device) +{ + if (device == OCTOSPIINVALID || device >= OCTOSPIDEV_COUNT) { + return NULL; + } + + return octoSpiDevice[device].dev; +} + +const octoSpiHardware_t octoSpiHardware[] = { +#if defined(STM32H730xx) || defined(STM32H723xx) + { + .device = OCTOSPIDEV_1, + .reg = OCTOSPI1, + } +#else +#error MCU not supported. +#endif +}; + +bool octoSpiInit(OCTOSPIDevice device) +{ + for (size_t hwindex = 0; hwindex < ARRAYLEN(octoSpiHardware); hwindex++) { + const octoSpiHardware_t *hw = &octoSpiHardware[hwindex]; + + OCTOSPIDevice device = hw->device; + octoSpiDevice_t *pDev = &octoSpiDevice[device]; + + pDev->dev = hw->reg; + } + + switch (device) { + case OCTOSPIINVALID: + return false; + case OCTOSPIDEV_1: +#ifdef USE_OCTOSPI_DEVICE_1 + octoSpiInitDevice(OCTOSPIDEV_1); + return true; +#else + break; +#endif + } + return false; +} + +#endif diff --git a/src/main/drivers/bus_octospi.h b/src/main/drivers/bus_octospi.h new file mode 100644 index 00000000000..14178bd7934 --- /dev/null +++ b/src/main/drivers/bus_octospi.h @@ -0,0 +1,62 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + * + * Author: Dominic Clifton + */ + +#pragma once + +#ifdef USE_OCTOSPI + +typedef enum OCTOSPIDevice { + OCTOSPIINVALID = -1, + OCTOSPIDEV_1 = 0, +} OCTOSPIDevice; + +#define OCTOSPIDEV_COUNT 1 + +// Macros to convert between configuration ids and device ids. +#define OCTOSPI_CFG_TO_DEV(x) ((x) - 1) +#define OCTOSPI_DEV_TO_CFG(x) ((x) + 1) + +#if !defined(STM32H7) +#error OctoSPI unsupported on this MCU +#endif + +OCTOSPIDevice octoSpiDeviceByInstance(OCTOSPI_TypeDef *instance); +OCTOSPI_TypeDef *octoSpiInstanceByDevice(OCTOSPIDevice device); + + +bool octoSpiInit(OCTOSPIDevice device); +bool octoSpiReceive1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length); +bool octoSpiReceive4LINES(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length); +bool octoSpiTransmit1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length); + +bool octoSpiReceiveWithAddress1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length); +bool octoSpiReceiveWithAddress4LINES(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length); +bool octoSpiTransmitWithAddress1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length); +bool octoSpiTransmitWithAddress4LINES(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length); + +bool octoSpiInstructionWithAddress1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize); + + +void octoSpiDisableMemoryMappedMode(OCTOSPI_TypeDef *instance); +void octoSpiEnableMemoryMappedMode(OCTOSPI_TypeDef *instance); + +#endif diff --git a/src/main/drivers/bus_octospi_impl.h b/src/main/drivers/bus_octospi_impl.h new file mode 100644 index 00000000000..3a229892383 --- /dev/null +++ b/src/main/drivers/bus_octospi_impl.h @@ -0,0 +1,40 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + * + * Author: Dominic Clifton + */ + +#pragma once + +#ifdef USE_OCTOSPI + +typedef struct octoSpiHardware_s { + OCTOSPIDevice device; + OCTOSPI_TypeDef *reg; +} octoSpiHardware_t; + +typedef struct OCTOSPIDevice_s { + OCTOSPI_TypeDef *dev; +} octoSpiDevice_t; + +extern octoSpiDevice_t octoSpiDevice[OCTOSPIDEV_COUNT]; + +void octoSpiInitDevice(OCTOSPIDevice device); + +#endif diff --git a/src/main/drivers/bus_octospi_stm32h7xx.c b/src/main/drivers/bus_octospi_stm32h7xx.c new file mode 100644 index 00000000000..f45aaddb441 --- /dev/null +++ b/src/main/drivers/bus_octospi_stm32h7xx.c @@ -0,0 +1,853 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + * + * Author: Dominic Clifton + */ + +/* + * + */ +#include +#include +#include + +#include "platform.h" + +#ifdef USE_OCTOSPI + +#include "drivers/system.h" + +#include "drivers/bus_octospi.h" +#include "drivers/bus_octospi_impl.h" + +#if !(defined(STM32H730xx) || defined(STM32H723xx)) +#error MCU not supported. +#endif + +#define OCTOSPI_INTERFACE_COUNT 1 + +#define OSPI_FUNCTIONAL_MODE_INDIRECT_WRITE ((uint32_t)0x00000000) +#define OSPI_FUNCTIONAL_MODE_INDIRECT_READ ((uint32_t)OCTOSPI_CR_FMODE_0) +#define OSPI_FUNCTIONAL_MODE_AUTO_POLLING ((uint32_t)OCTOSPI_CR_FMODE_1) +#define OSPI_FUNCTIONAL_MODE_MEMORY_MAPPED ((uint32_t)OCTOSPI_CR_FMODE) + +#define OSPI_DHQC_DISABLE ((uint32_t)0x00000000U) +#define OSPI_DHQC_ENABLE ((uint32_t)OCTOSPI_TCR_DHQC) + +#define OSPI_OPTYPE_COMMON_CFG ((uint32_t)0x00000000U) + +#define OSPI_OPTYPE_READ_CFG ((uint32_t)0x00000001U) +#define OSPI_OPTYPE_WRITE_CFG ((uint32_t)0x00000002U) +#define OSPI_OPTYPE_WRAP_CFG ((uint32_t)0x00000003U) + +#define OSPI_INSTRUCTION_NONE ((uint32_t)0x00000000U) +#define OSPI_INSTRUCTION_1_LINE ((uint32_t)OCTOSPI_CCR_IMODE_0) +#define OSPI_INSTRUCTION_2_LINES ((uint32_t)OCTOSPI_CCR_IMODE_1) +#define OSPI_INSTRUCTION_4_LINES ((uint32_t)(OCTOSPI_CCR_IMODE_0 | OCTOSPI_CCR_IMODE_1)) +#define OSPI_INSTRUCTION_8_LINES ((uint32_t)OCTOSPI_CCR_IMODE_2) + +#define OSPI_INSTRUCTION_8_BITS ((uint32_t)0x00000000U) +#define OSPI_INSTRUCTION_16_BITS ((uint32_t)OCTOSPI_CCR_ISIZE_0) +#define OSPI_INSTRUCTION_24_BITS ((uint32_t)OCTOSPI_CCR_ISIZE_1) +#define OSPI_INSTRUCTION_32_BITS ((uint32_t)OCTOSPI_CCR_ISIZE) + +#define OSPI_INSTRUCTION_DTR_DISABLE ((uint32_t)0x00000000U) +#define OSPI_INSTRUCTION_DTR_ENABLE ((uint32_t)OCTOSPI_CCR_IDTR) + +#define OSPI_ADDRESS_NONE ((uint32_t)0x00000000U) /*!< No address */ +#define OSPI_ADDRESS_1_LINE ((uint32_t)OCTOSPI_CCR_ADMODE_0) /*!< Address on a single line */ +#define OSPI_ADDRESS_2_LINES ((uint32_t)OCTOSPI_CCR_ADMODE_1) /*!< Address on two lines */ +#define OSPI_ADDRESS_4_LINES ((uint32_t)(OCTOSPI_CCR_ADMODE_0 | OCTOSPI_CCR_ADMODE_1)) /*!< Address on four lines */ +#define OSPI_ADDRESS_8_LINES ((uint32_t)OCTOSPI_CCR_ADMODE_2) /*!< Address on eight lines */ + +#define OSPI_ADDRESS_8_BITS ((uint32_t)0x00000000U) /*!< 8-bit address */ +#define OSPI_ADDRESS_16_BITS ((uint32_t)OCTOSPI_CCR_ADSIZE_0) /*!< 16-bit address */ +#define OSPI_ADDRESS_24_BITS ((uint32_t)OCTOSPI_CCR_ADSIZE_1) /*!< 24-bit address */ +#define OSPI_ADDRESS_32_BITS ((uint32_t)OCTOSPI_CCR_ADSIZE) /*!< 32-bit address */ + +#define OSPI_ADDRESS_DTR_DISABLE ((uint32_t)0x00000000U) /*!< DTR mode disabled for address phase */ +#define OSPI_ADDRESS_DTR_ENABLE ((uint32_t)OCTOSPI_CCR_ADDTR) + +#define OSPI_DATA_NONE ((uint32_t)0x00000000U) +#define OSPI_DATA_1_LINE ((uint32_t)OCTOSPI_CCR_DMODE_0) +#define OSPI_DATA_2_LINES ((uint32_t)OCTOSPI_CCR_DMODE_1) +#define OSPI_DATA_4_LINES ((uint32_t)(OCTOSPI_CCR_DMODE_0 | OCTOSPI_CCR_DMODE_1)) +#define OSPI_DATA_8_LINES ((uint32_t)OCTOSPI_CCR_DMODE_2) + +#define OSPI_DATA_DTR_DISABLE ((uint32_t)0x00000000U) +#define OSPI_DATA_DTR_ENABLE ((uint32_t)OCTOSPI_CCR_DDTR) + +#define OSPI_ALTERNATE_BYTES_NONE ((uint32_t)0x00000000U) +#define OSPI_ALTERNATE_BYTES_1_LINE ((uint32_t)OCTOSPI_CCR_ABMODE_0) +#define OSPI_ALTERNATE_BYTES_2_LINES ((uint32_t)OCTOSPI_CCR_ABMODE_1) +#define OSPI_ALTERNATE_BYTES_4_LINES ((uint32_t)(OCTOSPI_CCR_ABMODE_0 | OCTOSPI_CCR_ABMODE_1)) +#define OSPI_ALTERNATE_BYTES_8_LINES ((uint32_t)OCTOSPI_CCR_ABMODE_2) + +#define OSPI_ALTERNATE_BYTES_8_BITS ((uint32_t)0x00000000U) +#define OSPI_ALTERNATE_BYTES_16_BITS ((uint32_t)OCTOSPI_CCR_ABSIZE_0) +#define OSPI_ALTERNATE_BYTES_24_BITS ((uint32_t)OCTOSPI_CCR_ABSIZE_1) +#define OSPI_ALTERNATE_BYTES_32_BITS ((uint32_t)OCTOSPI_CCR_ABSIZE) + +#define OSPI_ALTERNATE_BYTES_DTR_DISABLE ((uint32_t)0x00000000U) +#define OSPI_ALTERNATE_BYTES_DTR_ENABLE ((uint32_t)OCTOSPI_CCR_ABDTR) + +#define OSPI_DQS_DISABLE ((uint32_t)0x00000000U) +#define OSPI_DQS_ENABLE ((uint32_t)OCTOSPI_CCR_DQSE) + +#define OSPI_SIOO_INST_EVERY_CMD ((uint32_t)0x00000000U) +#define OSPI_SIOO_INST_ONLY_FIRST_CMD ((uint32_t)OCTOSPI_CCR_SIOO) + +MMFLASH_CODE_NOINLINE static void Error_Handler(void) { + while (1) { + NOOP; + } +} + + +#define __OSPI_GET_FLAG(__INSTANCE__, __FLAG__) ((READ_BIT((__INSTANCE__)->SR, (__FLAG__)) != 0U) ? SET : RESET) +#define __OSPI_CLEAR_FLAG(__INSTANCE__, __FLAG__) WRITE_REG((__INSTANCE__)->FCR, (__FLAG__)) +#define __OSPI_ENABLE(__INSTANCE__) SET_BIT((__INSTANCE__)->CR, OCTOSPI_CR_EN) +#define __OSPI_DISABLE(__INSTANCE__) CLEAR_BIT((__INSTANCE__)->CR, OCTOSPI_CR_EN) +#define __OSPI_IS_ENABLED(__INSTANCE__) (READ_BIT((__INSTANCE__)->CR, OCTOSPI_CR_EN) != 0U) + +MMFLASH_CODE_NOINLINE static void octoSpiAbort(OCTOSPI_TypeDef *instance) +{ + SET_BIT(instance->CR, OCTOSPI_CR_ABORT); +} + +MMFLASH_CODE_NOINLINE static void octoSpiWaitStatusFlags(OCTOSPI_TypeDef *instance, uint32_t mask, FlagStatus flagStatus) +{ + uint32_t regval; + + switch (flagStatus) { + case SET: + while (!((regval = READ_REG(instance->SR)) & mask)) + {} + break; + case RESET: + while (((regval = READ_REG(instance->SR)) & mask)) + {} + break; + } +} + + +typedef struct { + uint32_t OperationType; + + uint32_t Instruction; + uint32_t InstructionMode; + uint32_t InstructionSize; + uint32_t InstructionDtrMode; + + uint32_t Address; + uint32_t AddressMode; + uint32_t AddressSize; + uint32_t AddressDtrMode; + + uint32_t AlternateBytes; + uint32_t AlternateBytesMode; + uint32_t AlternateBytesSize; + uint32_t AlternateBytesDtrMode; + + uint32_t DummyCycles; // 0-31 + + uint32_t DataMode; + uint32_t DataDtrMode; + uint32_t NbData; + + uint32_t DQSMode; + uint32_t SIOOMode; +} OSPI_Command_t; + +// TODO rename cmd to command +MMFLASH_CODE_NOINLINE static ErrorStatus octoSpiConfigureCommand(OCTOSPI_TypeDef *instance, OSPI_Command_t *cmd) +{ + ErrorStatus status = SUCCESS; + + MODIFY_REG(instance->CR, OCTOSPI_CR_FMODE, 0U); + + instance->CCR = (cmd->DQSMode | cmd->SIOOMode); + + if (cmd->AlternateBytesMode != OSPI_ALTERNATE_BYTES_NONE) + { + instance->ABR = cmd->AlternateBytes; + + MODIFY_REG( + instance->ABR, + (OCTOSPI_CCR_ABMODE | OCTOSPI_CCR_ABDTR | OCTOSPI_CCR_ABSIZE), + (cmd->AlternateBytesMode | cmd->AlternateBytesDtrMode | cmd->AlternateBytesSize) + ); + } + + MODIFY_REG(instance->TCR, OCTOSPI_TCR_DCYC, cmd->DummyCycles); + + if (cmd->DataMode != OSPI_DATA_NONE) + { + if (cmd->OperationType == OSPI_OPTYPE_COMMON_CFG) + { + instance->DLR = (cmd->NbData - 1U); + } + } + + + if (cmd->InstructionMode != OSPI_INSTRUCTION_NONE) + { + if (cmd->AddressMode != OSPI_ADDRESS_NONE) + { + if (cmd->DataMode != OSPI_DATA_NONE) + { + // instruction, address and data + + MODIFY_REG(instance->CCR, (OCTOSPI_CCR_IMODE | OCTOSPI_CCR_IDTR | OCTOSPI_CCR_ISIZE | + OCTOSPI_CCR_ADMODE | OCTOSPI_CCR_ADDTR | OCTOSPI_CCR_ADSIZE | + OCTOSPI_CCR_DMODE | OCTOSPI_CCR_DDTR), + (cmd->InstructionMode | cmd->InstructionDtrMode | cmd->InstructionSize | + cmd->AddressMode | cmd->AddressDtrMode | cmd->AddressSize | + cmd->DataMode | cmd->DataDtrMode)); + } + else + { + // instruction and address + + MODIFY_REG(instance->CCR, (OCTOSPI_CCR_IMODE | OCTOSPI_CCR_IDTR | OCTOSPI_CCR_ISIZE | + OCTOSPI_CCR_ADMODE | OCTOSPI_CCR_ADDTR | OCTOSPI_CCR_ADSIZE), + (cmd->InstructionMode | cmd->InstructionDtrMode | cmd->InstructionSize | + cmd->AddressMode | cmd->AddressDtrMode | cmd->AddressSize)); + + // DHQC bit is linked with DDTR + if (((instance->TCR & OCTOSPI_TCR_DHQC_Msk) == OSPI_DHQC_ENABLE) && + (cmd->InstructionDtrMode == OSPI_INSTRUCTION_DTR_ENABLE)) + { + MODIFY_REG(instance->CCR, OCTOSPI_CCR_DDTR, OSPI_DATA_DTR_ENABLE); + } + } + + instance->IR = cmd->Instruction; + + instance->AR = cmd->Address; + } + else + { + if (cmd->DataMode != OSPI_DATA_NONE) + { + // instruction and data + + MODIFY_REG(instance->CCR, (OCTOSPI_CCR_IMODE | OCTOSPI_CCR_IDTR | OCTOSPI_CCR_ISIZE | + OCTOSPI_CCR_DMODE | OCTOSPI_CCR_DDTR), + (cmd->InstructionMode | cmd->InstructionDtrMode | cmd->InstructionSize | + cmd->DataMode | cmd->DataDtrMode)); + } + else + { + // instruction only + + MODIFY_REG(instance->CCR, (OCTOSPI_CCR_IMODE | OCTOSPI_CCR_IDTR | OCTOSPI_CCR_ISIZE), + (cmd->InstructionMode | cmd->InstructionDtrMode | cmd->InstructionSize)); + + // DHQC bit is linked with DDTR + if (((instance->TCR & OCTOSPI_TCR_DHQC_Msk) == OSPI_DHQC_ENABLE) && + (cmd->InstructionDtrMode == OSPI_INSTRUCTION_DTR_ENABLE)) + { + MODIFY_REG(instance->CCR, OCTOSPI_CCR_DDTR, OSPI_DATA_DTR_ENABLE); + } + } + + instance->IR = cmd->Instruction; + + } + } + else + { + if (cmd->AddressMode != OSPI_ADDRESS_NONE) + { + if (cmd->DataMode != OSPI_DATA_NONE) + { + // address and data + + MODIFY_REG(instance->CCR, (OCTOSPI_CCR_ADMODE | OCTOSPI_CCR_ADDTR | OCTOSPI_CCR_ADSIZE | + OCTOSPI_CCR_DMODE | OCTOSPI_CCR_DDTR), + (cmd->AddressMode | cmd->AddressDtrMode | cmd->AddressSize | + cmd->DataMode | cmd->DataDtrMode)); + } + else + { + // address only + + MODIFY_REG(instance->CCR, (OCTOSPI_CCR_ADMODE | OCTOSPI_CCR_ADDTR | OCTOSPI_CCR_ADSIZE), + (cmd->AddressMode | cmd->AddressDtrMode | cmd->AddressSize)); + } + + instance->AR = cmd->Address; + } + else + { + // no instruction, no address + status = ERROR; + } + } + + return status; +} + +MMFLASH_CODE_NOINLINE ErrorStatus octoSpiCommand(OCTOSPI_TypeDef *instance, OSPI_Command_t *cmd) +{ + octoSpiWaitStatusFlags(instance, OCTOSPI_SR_BUSY, RESET); + + ErrorStatus status = octoSpiConfigureCommand(instance, cmd); + if (status == SUCCESS) { + if (cmd->DataMode == OSPI_DATA_NONE) + { + // transfer is already started, wait now. + octoSpiWaitStatusFlags(instance, OCTOSPI_SR_TCF, SET); + __OSPI_CLEAR_FLAG(instance, OCTOSPI_SR_TCF); + } + } + + return status; +} + +/* + * Transmit + * + * Call optoSpiCommand first to configure the transaction stages. + */ +MMFLASH_CODE_NOINLINE ErrorStatus octoSpiTransmit(OCTOSPI_TypeDef *instance, uint8_t *data) +{ + if (data == NULL) { + return ERROR; + } + + + __IO uint32_t XferCount = READ_REG(instance->DLR) + 1U; + uint8_t *pBuffPtr = data; + + MODIFY_REG(instance->CR, OCTOSPI_CR_FMODE, OSPI_FUNCTIONAL_MODE_INDIRECT_WRITE); + + __IO uint32_t *data_reg = &instance->DR; + do + { + octoSpiWaitStatusFlags(instance, OCTOSPI_SR_FTF, SET); + + *((__IO uint8_t *)data_reg) = *pBuffPtr; + pBuffPtr++; + XferCount--; + } while (XferCount > 0U); + + octoSpiWaitStatusFlags(instance, OCTOSPI_SR_TCF, SET); + __OSPI_CLEAR_FLAG(instance, OCTOSPI_SR_TCF); + + return SUCCESS; +} + +/* + * Receive + * + * Call optoSpiCommand first to configure the transaction stages. + */ +MMFLASH_CODE_NOINLINE ErrorStatus octoSpiReceive(OCTOSPI_TypeDef *instance, uint8_t *data) +{ + if (data == NULL) { + return ERROR; + } + + __IO uint32_t XferCount = READ_REG(instance->DLR) + 1U; + uint8_t *pBuffPtr = data; + + MODIFY_REG(instance->CR, OCTOSPI_CR_FMODE, OSPI_FUNCTIONAL_MODE_INDIRECT_READ); + + uint32_t addr_reg = instance->AR; + uint32_t ir_reg = instance->IR; + + // Trigger the transfer by re-writing the address or instruction register + if (READ_BIT(instance->CCR, OCTOSPI_CCR_ADMODE) != OSPI_ADDRESS_NONE) + { + WRITE_REG(instance->AR, addr_reg); + } + else + { + WRITE_REG(instance->IR, ir_reg); + } + + __IO uint32_t *data_reg = &instance->DR; + + do + { + octoSpiWaitStatusFlags(instance, OCTOSPI_SR_FTF | OCTOSPI_SR_TCF, SET); + + *pBuffPtr = *((__IO uint8_t *)data_reg); + pBuffPtr++; + XferCount--; + } while(XferCount > 0U); + + octoSpiWaitStatusFlags(instance, OCTOSPI_SR_TCF, SET); + __OSPI_CLEAR_FLAG(instance, OCTOSPI_SR_TCF); + + return SUCCESS; +} + +typedef struct +{ + // CR register contains the all-important FMODE bits. + uint32_t CR; + + // flash chip specific configuration set by the bootloader + uint32_t CCR; + uint32_t TCR; + uint32_t IR; + uint32_t ABR; + // address register - no meaning. + // data length register no meaning. + +} octoSpiMemoryMappedModeConfigurationRegisterBackup_t; + +octoSpiMemoryMappedModeConfigurationRegisterBackup_t ospiMMMCRBackups[OCTOSPI_INTERFACE_COUNT]; + +void octoSpiBackupMemoryMappedModeConfiguration(OCTOSPI_TypeDef *instance) +{ + OCTOSPIDevice device = octoSpiDeviceByInstance(instance); + if (device == OCTOSPIINVALID) { + return; + } + + octoSpiMemoryMappedModeConfigurationRegisterBackup_t *ospiMMMCRBackup = &ospiMMMCRBackups[device]; + + // backup all the registers used by memory mapped mode that: + // a) the bootloader configured. + // b) that other code in this implementation may have modified when memory mapped mode is disabled. + + ospiMMMCRBackup->CR = instance->CR; + ospiMMMCRBackup->IR = instance->IR; + ospiMMMCRBackup->CCR = instance->CCR; + ospiMMMCRBackup->TCR = instance->TCR; + ospiMMMCRBackup->ABR = instance->ABR; +} + +MMFLASH_CODE_NOINLINE void octoSpiRestoreMemoryMappedModeConfiguration(OCTOSPI_TypeDef *instance) +{ + OCTOSPIDevice device = octoSpiDeviceByInstance(instance); + if (device == OCTOSPIINVALID) { + return; + } + + octoSpiMemoryMappedModeConfigurationRegisterBackup_t *ospiMMMCRBackup = &ospiMMMCRBackups[device]; + + octoSpiWaitStatusFlags(instance, OCTOSPI_SR_BUSY, RESET); + + instance->ABR = ospiMMMCRBackup->ABR; + instance->TCR = ospiMMMCRBackup->TCR; + + instance->DLR = 0; // "no meaning" in MM mode. + + instance->CCR = ospiMMMCRBackup->CCR; + + instance->IR = ospiMMMCRBackup->IR; + instance->AR = 0; // "no meaning" in MM mode. + + octoSpiAbort(instance); + octoSpiWaitStatusFlags(instance, OCTOSPI_SR_BUSY, RESET); + + instance->CR = ospiMMMCRBackup->CR; +} + +/* + * Disable memory mapped mode. + * + * @See octoSpiEnableMemoryMappedMode + * @See MMFLASH_CODE_NOINLINE + * + * Once this is called any code or data in the memory mapped region cannot be accessed. + * Thus, this function itself must be in RAM, and the caller's code and data should all be in RAM + * and this requirement continues until octoSpiEnableMemoryMappedMode is called. + * This applies to ISR code that runs from the memory mapped region, so likely the caller should + * also disable IRQs before calling this. + */ +MMFLASH_CODE_NOINLINE void octoSpiDisableMemoryMappedMode(OCTOSPI_TypeDef *instance) +{ + if (READ_BIT(OCTOSPI1->CR, OCTOSPI_CR_FMODE) != OCTOSPI_CR_FMODE) { + failureMode(FAILURE_DEVELOPER); // likely not booted with memory mapped mode enabled, or mismatched calls to enable/disable memory map mode. + } + + octoSpiAbort(instance); + if (__OSPI_GET_FLAG(instance, OCTOSPI_SR_BUSY) == SET) { + + __OSPI_DISABLE(instance); + octoSpiAbort(instance); + } + octoSpiWaitStatusFlags(instance, OCTOSPI_SR_BUSY, RESET); + + uint32_t fmode = 0x0; // b00 = indirect write, see OCTOSPI->CR->FMODE + MODIFY_REG(instance->CR, OCTOSPI_CR_FMODE, fmode); + + uint32_t regval = READ_REG(instance->CR); + if ((regval & OCTOSPI_CR_FMODE) != fmode) { + Error_Handler(); + } + + if (!__OSPI_IS_ENABLED(instance)) { + __OSPI_ENABLE(instance); + } +} + +/* + * Enable memory mapped mode. + * + * @See octoSpiDisableMemoryMappedMode + * @See MMFLASH_CODE_NOINLINE + */ + +MMFLASH_CODE_NOINLINE void octoSpiEnableMemoryMappedMode(OCTOSPI_TypeDef *instance) +{ + octoSpiAbort(instance); + octoSpiWaitStatusFlags(instance, OCTOSPI_SR_BUSY, RESET); + + octoSpiRestoreMemoryMappedModeConfiguration(instance); +} + +MMFLASH_CODE_NOINLINE void octoSpiTestEnableDisableMemoryMappedMode(octoSpiDevice_t *octoSpi) +{ + OCTOSPI_TypeDef *instance = octoSpi->dev; + + __disable_irq(); + octoSpiDisableMemoryMappedMode(instance); + octoSpiEnableMemoryMappedMode(instance); + __enable_irq(); +} + +MMFLASH_DATA static const uint32_t octoSpi_addressSizeMap[] = { + OSPI_ADDRESS_8_BITS, + OSPI_ADDRESS_16_BITS, + OSPI_ADDRESS_24_BITS, + OSPI_ADDRESS_32_BITS +}; + +MMFLASH_CODE static uint32_t octoSpi_addressSizeFromValue(uint8_t addressSize) +{ + return octoSpi_addressSizeMap[((addressSize + 1) / 8) - 1]; // rounds to nearest OSPI_ADDRESS_* value that will hold the address. +} + + +MMFLASH_CODE_NOINLINE bool octoSpiTransmit1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length) +{ + OSPI_Command_t cmd; // Can't initialise to zero as compiler optimization will use memset() which is not in RAM. + + cmd.OperationType = OSPI_OPTYPE_COMMON_CFG; + + cmd.Instruction = instruction; + cmd.InstructionDtrMode = OSPI_INSTRUCTION_DTR_DISABLE; + cmd.InstructionMode = OSPI_INSTRUCTION_1_LINE; + cmd.InstructionSize = OSPI_INSTRUCTION_8_BITS; + + cmd.AddressDtrMode = OSPI_ADDRESS_DTR_DISABLE; + cmd.AddressMode = OSPI_ADDRESS_NONE; + cmd.AddressSize = OSPI_ADDRESS_32_BITS; + + cmd.DummyCycles = dummyCycles; + + cmd.AlternateBytesMode = OSPI_ALTERNATE_BYTES_NONE; + + cmd.DataDtrMode = OSPI_DATA_DTR_DISABLE; + cmd.DataMode = OSPI_DATA_NONE; + cmd.NbData = length; + + cmd.DQSMode = OSPI_DQS_DISABLE; + cmd.SIOOMode = OSPI_SIOO_INST_EVERY_CMD; + + if (out) { + cmd.DataMode = OSPI_DATA_1_LINE; + } + + ErrorStatus status = octoSpiCommand(instance, &cmd); + + if (status == SUCCESS && out && length > 0) { + status = octoSpiTransmit(instance, (uint8_t *)out); + } + return status == SUCCESS; +} + +MMFLASH_CODE_NOINLINE bool octoSpiReceive1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length) +{ + OSPI_Command_t cmd; // Can't initialise to zero as compiler optimization will use memset() which is not in RAM. + + cmd.OperationType = OSPI_OPTYPE_COMMON_CFG; + + cmd.Instruction = instruction; + cmd.InstructionDtrMode = OSPI_INSTRUCTION_DTR_DISABLE; + cmd.InstructionMode = OSPI_INSTRUCTION_1_LINE; + cmd.InstructionSize = OSPI_INSTRUCTION_8_BITS; + + cmd.AddressDtrMode = OSPI_ADDRESS_DTR_DISABLE; + cmd.AddressMode = OSPI_ADDRESS_NONE; + cmd.AddressSize = OSPI_ADDRESS_32_BITS; + + cmd.AlternateBytesMode = OSPI_ALTERNATE_BYTES_NONE; + + cmd.DummyCycles = dummyCycles; + + cmd.DataDtrMode = OSPI_DATA_DTR_DISABLE; + cmd.DataMode = OSPI_DATA_1_LINE; + cmd.NbData = length; + + cmd.DQSMode = OSPI_DQS_DISABLE; + cmd.SIOOMode = OSPI_SIOO_INST_EVERY_CMD; + + ErrorStatus status = octoSpiCommand(instance, &cmd); + + if (status == SUCCESS) { + status = octoSpiReceive(instance, in); + } + + return status == SUCCESS; +} + +MMFLASH_CODE_NOINLINE bool octoSpiReceive4LINES(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length) +{ + OSPI_Command_t cmd; // Can't initialise to zero as compiler optimization will use memset() which is not in RAM. + + cmd.OperationType = OSPI_OPTYPE_COMMON_CFG; + + cmd.Instruction = instruction; + cmd.InstructionDtrMode = OSPI_INSTRUCTION_DTR_DISABLE; + cmd.InstructionMode = OSPI_INSTRUCTION_4_LINES; + cmd.InstructionSize = OSPI_INSTRUCTION_8_BITS; + + cmd.AddressDtrMode = OSPI_ADDRESS_DTR_DISABLE; + cmd.AddressMode = OSPI_ADDRESS_NONE; + cmd.AddressSize = OSPI_ADDRESS_32_BITS; + + cmd.AlternateBytesMode = OSPI_ALTERNATE_BYTES_NONE; + + cmd.DummyCycles = dummyCycles; + + cmd.DataDtrMode = OSPI_DATA_DTR_DISABLE; + cmd.DataMode = OSPI_DATA_4_LINES; + cmd.NbData = length; + + cmd.DQSMode = OSPI_DQS_DISABLE; + cmd.SIOOMode = OSPI_SIOO_INST_EVERY_CMD; + + ErrorStatus status = octoSpiCommand(instance, &cmd); + + if (status == SUCCESS) { + status = octoSpiReceive(instance, in); + } + + return status == SUCCESS; +} + +MMFLASH_CODE_NOINLINE bool octoSpiReceiveWithAddress1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length) +{ + OSPI_Command_t cmd; // Can't initialise to zero as compiler optimization will use memset() which is not in RAM. + + cmd.OperationType = OSPI_OPTYPE_COMMON_CFG; + + cmd.Instruction = instruction; + cmd.InstructionDtrMode = OSPI_INSTRUCTION_DTR_DISABLE; + cmd.InstructionMode = OSPI_INSTRUCTION_1_LINE; + cmd.InstructionSize = OSPI_INSTRUCTION_8_BITS; + + cmd.Address = address; + cmd.AddressDtrMode = OSPI_ADDRESS_DTR_DISABLE; + cmd.AddressMode = OSPI_ADDRESS_1_LINE; + cmd.AddressSize = octoSpi_addressSizeFromValue(addressSize); + + cmd.AlternateBytesMode = OSPI_ALTERNATE_BYTES_NONE; + + cmd.DummyCycles = dummyCycles; + + cmd.DataDtrMode = OSPI_DATA_DTR_DISABLE; + cmd.DataMode = OSPI_DATA_1_LINE; + cmd.NbData = length; + + cmd.SIOOMode = OSPI_SIOO_INST_EVERY_CMD; + cmd.DQSMode = OSPI_DQS_DISABLE; + + ErrorStatus status = octoSpiCommand(instance, &cmd); + + if (status == SUCCESS) { + status = octoSpiReceive(instance, in); + } + + return status == SUCCESS; +} + +MMFLASH_CODE_NOINLINE bool octoSpiReceiveWithAddress4LINES(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length) +{ + OSPI_Command_t cmd; // Can't initialise to zero as compiler optimization will use memset() which is not in RAM. + + cmd.OperationType = OSPI_OPTYPE_COMMON_CFG; + + cmd.Instruction = instruction; + cmd.InstructionDtrMode = OSPI_INSTRUCTION_DTR_DISABLE; + cmd.InstructionMode = OSPI_INSTRUCTION_1_LINE; + cmd.InstructionSize = OSPI_INSTRUCTION_8_BITS; + + cmd.Address = address; + cmd.AddressDtrMode = OSPI_ADDRESS_DTR_DISABLE; + cmd.AddressMode = OSPI_ADDRESS_1_LINE; + cmd.AddressSize = octoSpi_addressSizeFromValue(addressSize); + + cmd.AlternateBytesMode = OSPI_ALTERNATE_BYTES_NONE; + + cmd.DummyCycles = dummyCycles; + + cmd.DataDtrMode = OSPI_DATA_DTR_DISABLE; + cmd.DataMode = OSPI_DATA_4_LINES; + cmd.NbData = length; + + cmd.DQSMode = OSPI_DQS_DISABLE; + cmd.SIOOMode = OSPI_SIOO_INST_EVERY_CMD; + + ErrorStatus status = octoSpiCommand(instance, &cmd); + + if (status == SUCCESS) { + status = octoSpiReceive(instance, in); + } + + return status == SUCCESS; +} + +MMFLASH_CODE_NOINLINE bool octoSpiTransmitWithAddress1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length) +{ + OSPI_Command_t cmd; // Can't initialise to zero as compiler optimization will use memset() which is not in RAM. + + cmd.OperationType = OSPI_OPTYPE_COMMON_CFG; + + cmd.Instruction = instruction; + cmd.InstructionDtrMode = OSPI_INSTRUCTION_DTR_DISABLE; + cmd.InstructionMode = OSPI_INSTRUCTION_1_LINE; + cmd.InstructionSize = OSPI_INSTRUCTION_8_BITS; + + cmd.Address = address; + cmd.AddressDtrMode = OSPI_ADDRESS_DTR_DISABLE; + cmd.AddressMode = OSPI_ADDRESS_1_LINE; + cmd.AddressSize = octoSpi_addressSizeFromValue(addressSize); + + cmd.AlternateBytesMode = OSPI_ALTERNATE_BYTES_NONE; + + cmd.DummyCycles = dummyCycles; + + cmd.DataDtrMode = OSPI_DATA_DTR_DISABLE; + cmd.DataMode = OSPI_DATA_1_LINE; + cmd.NbData = length; + + cmd.DQSMode = OSPI_DQS_DISABLE; + cmd.SIOOMode = OSPI_SIOO_INST_EVERY_CMD; + + ErrorStatus status = octoSpiCommand(instance, &cmd); + + if (status == SUCCESS) { + status = octoSpiTransmit(instance, (uint8_t *)out); + } + + return status == SUCCESS; +} + +MMFLASH_CODE_NOINLINE bool octoSpiTransmitWithAddress4LINES(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length) +{ + OSPI_Command_t cmd; // Can't initialise to zero as compiler optimization will use memset() which is not in RAM. + + cmd.OperationType = OSPI_OPTYPE_COMMON_CFG; + + cmd.Instruction = instruction; + cmd.InstructionDtrMode = OSPI_INSTRUCTION_DTR_DISABLE; + cmd.InstructionMode = OSPI_INSTRUCTION_1_LINE; + cmd.InstructionSize = OSPI_INSTRUCTION_8_BITS; + + cmd.Address = address; + cmd.AddressDtrMode = OSPI_ADDRESS_DTR_DISABLE; + cmd.AddressMode = OSPI_ADDRESS_1_LINE; + cmd.AddressSize = octoSpi_addressSizeFromValue(addressSize); + + cmd.AlternateBytesMode = OSPI_ALTERNATE_BYTES_NONE; + + cmd.DummyCycles = dummyCycles; + + cmd.DataDtrMode = OSPI_DATA_DTR_DISABLE; + cmd.DataMode = OSPI_DATA_4_LINES; + cmd.NbData = length; + + cmd.DQSMode = OSPI_DQS_DISABLE; + cmd.SIOOMode = OSPI_SIOO_INST_EVERY_CMD; + + ErrorStatus status = octoSpiCommand(instance, &cmd); + + if (status == SUCCESS) { + status = octoSpiTransmit(instance, (uint8_t *)out); + } + + + return status == SUCCESS; +} + +MMFLASH_CODE_NOINLINE bool octoSpiInstructionWithAddress1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize) +{ + OSPI_Command_t cmd; // Can't initialise to zero as compiler optimization will use memset() which is not in RAM. + + cmd.OperationType = OSPI_OPTYPE_COMMON_CFG; + + cmd.Instruction = instruction; + cmd.InstructionDtrMode = OSPI_INSTRUCTION_DTR_DISABLE; + cmd.InstructionMode = OSPI_INSTRUCTION_1_LINE; + cmd.InstructionSize = OSPI_INSTRUCTION_8_BITS; + + cmd.Address = address; + cmd.AddressDtrMode = OSPI_ADDRESS_DTR_DISABLE; + cmd.AddressMode = OSPI_ADDRESS_1_LINE; + cmd.AddressSize = octoSpi_addressSizeFromValue(addressSize); + + cmd.AlternateBytesMode = OSPI_ALTERNATE_BYTES_NONE; + + cmd.DummyCycles = dummyCycles; + + cmd.DataDtrMode = OSPI_DATA_DTR_DISABLE; + cmd.DataMode = OSPI_DATA_NONE; + cmd.NbData = 0; + + cmd.DQSMode = OSPI_DQS_DISABLE; + cmd.SIOOMode = OSPI_SIOO_INST_EVERY_CMD; + + ErrorStatus status = octoSpiCommand(instance, &cmd); + + return status == SUCCESS; +} + + +void octoSpiInitDevice(OCTOSPIDevice device) +{ + octoSpiDevice_t *octoSpi = &(octoSpiDevice[device]); + +#if defined(STM32H730xx) || defined(STM32H723xx) + if (isMemoryMappedModeEnabledOnBoot()) { + // Bootloader has already configured the IO, clocks and peripherals. + octoSpiBackupMemoryMappedModeConfiguration(octoSpi->dev); + + octoSpiTestEnableDisableMemoryMappedMode(octoSpi); + } else { + failureMode(FAILURE_DEVELOPER); // trying to use this implementation when memory mapped mode is not already enabled by a bootloader + + // Here is where we would configure the OCTOSPI1/2 and OCTOSPIM peripherals for the non-memory-mapped use case. + } +#else +#error MCU not supported. +#endif + +} + +#endif diff --git a/src/main/drivers/flash.c b/src/main/drivers/flash.c index ee8b55a3d77..39be0e9d838 100644 --- a/src/main/drivers/flash.c +++ b/src/main/drivers/flash.c @@ -36,8 +36,12 @@ #include "flash_w25m.h" #include "drivers/bus_spi.h" #include "drivers/bus_quadspi.h" +#include "drivers/bus_octospi.h" #include "drivers/io.h" #include "drivers/time.h" +#include "drivers/system.h" + +#ifdef USE_FLASH_SPI // 20 MHz max SPI frequency #define FLASH_MAX_SPI_CLK_HZ 20000000 @@ -47,13 +51,143 @@ static extDevice_t devInstance; static extDevice_t *dev; +#endif + static flashDevice_t flashDevice; static flashPartitionTable_t flashPartitionTable; static int flashPartitions = 0; #define FLASH_INSTRUCTION_RDID 0x9F -#ifdef USE_QUADSPI +#ifdef USE_FLASH_MEMORY_MAPPED +MMFLASH_CODE_NOINLINE void flashMemoryMappedModeDisable(void) +{ + __disable_irq(); +#ifdef USE_FLASH_OCTOSPI + octoSpiDisableMemoryMappedMode(flashDevice.io.handle.octoSpi); +#else +#error Invalid configuration - Not implemented +#endif +} + +MMFLASH_CODE_NOINLINE void flashMemoryMappedModeEnable(void) +{ +#ifdef USE_FLASH_OCTOSPI + octoSpiEnableMemoryMappedMode(flashDevice.io.handle.octoSpi); + __enable_irq(); +#else +#error Invalid configuration - Not implemented +#endif +} +#endif + +#ifdef USE_FLASH_OCTOSPI +MMFLASH_CODE_NOINLINE static bool flashOctoSpiInit(const flashConfig_t *flashConfig) +{ + bool detected = false; + + enum { + TRY_1LINE = 0, TRY_4LINE, BAIL + } phase = TRY_1LINE; + +#ifdef USE_FLASH_MEMORY_MAPPED + bool memoryMappedModeEnabledOnBoot = isMemoryMappedModeEnabledOnBoot(); +#else + bool memoryMappedModeEnabledOnBoot = false; +#endif + +#ifndef USE_OCTOSPI_EXPERIMENTAL + if (!memoryMappedModeEnabledOnBoot) { + return false; // Not supported yet, enable USE_OCTOSPI_EXPERIMENTAL and test/update implementation as required. + } +#endif + + OCTOSPI_TypeDef *instance = octoSpiInstanceByDevice(OCTOSPI_CFG_TO_DEV(flashConfig->octoSpiDevice)); + + flashDevice.io.handle.octoSpi = instance; + flashDevice.io.mode = FLASHIO_OCTOSPI; + + if (memoryMappedModeEnabledOnBoot) { + flashMemoryMappedModeDisable(); + } + + do { +#ifdef USE_OCTOSPI_EXPERIMENTAL + if (!memoryMappedMode) { + octoSpiSetDivisor(instance, OCTOSPI_CLOCK_INITIALISATION); + } +#endif + // for the memory-mapped use-case, we rely on the bootloader to have already selected the correct speed for the flash chip. + + // 3 bytes for what we need, but some IC's need 8 dummy cycles after the instruction, so read 4 and make two attempts to + // assemble the chip id from the response. + uint8_t readIdResponse[4]; + + bool status = false; + switch (phase) { + case TRY_1LINE: + status = octoSpiReceive1LINE(instance, FLASH_INSTRUCTION_RDID, 0, readIdResponse, 4); + break; + case TRY_4LINE: + status = octoSpiReceive4LINES(instance, FLASH_INSTRUCTION_RDID, 2, readIdResponse, 3); + break; + default: + break; + } + + if (!status) { + phase++; + continue; + } + +#ifdef USE_OCTOSPI_EXPERIMENTAL + if (!memoryMappedModeEnabledOnBoot) { + octoSpiSetDivisor(instance, OCTOSPI_CLOCK_ULTRAFAST); + } +#endif + + for (uint8_t offset = 0; offset <= 1 && !detected; offset++) { + + uint32_t jedecID = (readIdResponse[offset + 0] << 16) | (readIdResponse[offset + 1] << 8) | (readIdResponse[offset + 2]); + + if (offset == 0) { +#if defined(USE_FLASH_W25Q128FV) + if (!detected && w25q128fv_identify(&flashDevice, jedecID)) { + detected = true; + } +#endif + } + + if (offset == 1) { +#ifdef USE_OCTOSPI_EXPERIMENTAL + if (!memoryMappedModeEnabledOnBoot) { + // These flash chips DO NOT support memory mapped mode; suitable flash read commands must be available. +#if defined(USE_FLASH_W25N01G) + if (!detected && w25n01g_identify(&flashDevice, jedecID)) { + detected = true; + } +#endif +#if defined(USE_FLASH_W25M02G) + if (!detected && w25m_identify(&flashDevice, jedecID)) { + detected = true; + } +#endif + } +#endif + } + } + phase++; + } while (phase != BAIL && !detected); + + if (memoryMappedModeEnabledOnBoot) { + flashMemoryMappedModeEnable(); + } + return detected; + +} +#endif // USE_FLASH_OCTOSPI + +#ifdef USE_FLASH_QUADSPI static bool flashQuadSpiInit(const flashConfig_t *flashConfig) { bool detected = false; @@ -63,6 +197,9 @@ static bool flashQuadSpiInit(const flashConfig_t *flashConfig) QUADSPI_TypeDef *hqspi = quadSpiInstanceByDevice(QUADSPI_CFG_TO_DEV(flashConfig->quadSpiDevice)); + flashDevice.io.handle.quadSpi = hqspi; + flashDevice.io.mode = FLASHIO_QUADSPI; + do { quadSpiSetDivisor(hqspi, QUADSPI_CLOCK_INITIALISATION); @@ -87,19 +224,16 @@ static bool flashQuadSpiInit(const flashConfig_t *flashConfig) continue; } - flashDevice.io.handle.quadSpi = hqspi; - flashDevice.io.mode = FLASHIO_QUADSPI; - quadSpiSetDivisor(hqspi, QUADSPI_CLOCK_ULTRAFAST); for (uint8_t offset = 0; offset <= 1 && !detected; offset++) { - uint32_t chipID = (readIdResponse[offset + 0] << 16) | (readIdResponse[offset + 1] << 8) | (readIdResponse[offset + 2]); + uint32_t jedecID = (readIdResponse[offset + 0] << 16) | (readIdResponse[offset + 1] << 8) | (readIdResponse[offset + 2]); if (offset == 0) { -#ifdef USE_FLASH_W25Q128FV - if (!detected && w25q128fv_detect(&flashDevice, chipID)) { +#if defined(USE_FLASH_W25Q128FV) + if (!detected && w25q128fv_identify(&flashDevice, jedecID)) { detected = true; } #endif @@ -112,20 +246,20 @@ static bool flashQuadSpiInit(const flashConfig_t *flashConfig) } if (offset == 1) { -#ifdef USE_FLASH_W25N01G - if (!detected && w25n01g_detect(&flashDevice, chipID)) { +#if defined(USE_FLASH_W25N01G) + if (!detected && w25n01g_identify(&flashDevice, jedecID)) { detected = true; } #endif #if defined(USE_FLASH_W25M02G) - if (!detected && w25m_detect(&flashDevice, chipID)) { + if (!detected && w25m_identify(&flashDevice, jedecID)) { detected = true; } #endif } if (detected) { - flashDevice.geometry.jedecId = chipID; + flashDevice.geometry.jedecId = jedecID; } } phase++; @@ -133,15 +267,9 @@ static bool flashQuadSpiInit(const flashConfig_t *flashConfig) return detected; } -#endif // USE_QUADSPI - -#ifdef USE_SPI - -void flashPreInit(const flashConfig_t *flashConfig) -{ - spiPreinitRegister(flashConfig->csTag, IOCFG_IPU, 1); -} +#endif // USE_FLASH_QUADSPI +#ifdef USE_FLASH_SPI static bool flashSpiInit(const flashConfig_t *flashConfig) { bool detected = false; @@ -186,39 +314,39 @@ static bool flashSpiInit(const flashConfig_t *flashConfig) spiReadRegBuf(dev, FLASH_INSTRUCTION_RDID, readIdResponse, sizeof(readIdResponse)); // Manufacturer, memory type, and capacity - uint32_t chipID = (readIdResponse[0] << 16) | (readIdResponse[1] << 8) | (readIdResponse[2]); + uint32_t jedecID = (readIdResponse[0] << 16) | (readIdResponse[1] << 8) | (readIdResponse[2]); #ifdef USE_FLASH_M25P16 - if (m25p16_detect(&flashDevice, chipID)) { + if (m25p16_identify(&flashDevice, jedecID)) { detected = true; } #endif #if defined(USE_FLASH_W25M512) || defined(USE_FLASH_W25M) - if (!detected && w25m_detect(&flashDevice, chipID)) { + if (!detected && w25m_identify(&flashDevice, jedecID)) { detected = true; } #endif if (!detected) { // Newer chips - chipID = (readIdResponse[1] << 16) | (readIdResponse[2] << 8) | (readIdResponse[3]); + jedecID = (readIdResponse[1] << 16) | (readIdResponse[2] << 8) | (readIdResponse[3]); } #ifdef USE_FLASH_W25N01G - if (!detected && w25n01g_detect(&flashDevice, chipID)) { + if (!detected && w25n01g_identify(&flashDevice, jedecID)) { detected = true; } #endif #ifdef USE_FLASH_W25M02G - if (!detected && w25m_detect(&flashDevice, chipID)) { + if (!detected && w25m_identify(&flashDevice, jedecID)) { detected = true; } #endif if (detected) { - flashDevice.geometry.jedecId = chipID; + flashDevice.geometry.jedecId = jedecID; return detected; } @@ -226,39 +354,65 @@ static bool flashSpiInit(const flashConfig_t *flashConfig) return false; } -#endif // USE_SPI +#endif // USE_FLASH_SPI + +void flashPreInit(const flashConfig_t *flashConfig) +{ + spiPreinitRegister(flashConfig->csTag, IOCFG_IPU, 1); +} bool flashDeviceInit(const flashConfig_t *flashConfig) { -#ifdef USE_SPI + bool haveFlash = false; + +#ifdef USE_FLASH_SPI bool useSpi = (SPI_CFG_TO_DEV(flashConfig->spiDevice) != SPIINVALID); if (useSpi) { - return flashSpiInit(flashConfig); + haveFlash = flashSpiInit(flashConfig); } #endif -#ifdef USE_QUADSPI +#ifdef USE_FLASH_QUADSPI bool useQuadSpi = (QUADSPI_CFG_TO_DEV(flashConfig->quadSpiDevice) != QUADSPIINVALID); if (useQuadSpi) { - return flashQuadSpiInit(flashConfig); + haveFlash = flashQuadSpiInit(flashConfig); } #endif - return false; +#ifdef USE_FLASH_OCTOSPI + bool useOctoSpi = (OCTOSPI_CFG_TO_DEV(flashConfig->octoSpiDevice) != OCTOSPIINVALID); + if (useOctoSpi) { + haveFlash = flashOctoSpiInit(flashConfig); + } +#endif + + if (haveFlash && flashDevice.vTable->configure) { + uint32_t configurationFlags = 0; + +#ifdef USE_FLASH_MEMORY_MAPPED + if (isMemoryMappedModeEnabledOnBoot()) { + configurationFlags |= FLASH_CF_SYSTEM_IS_MEMORY_MAPPED; + } +#endif + + flashDevice.vTable->configure(&flashDevice, configurationFlags); + } + + return haveFlash; } -bool flashIsReady(void) +MMFLASH_CODE bool flashIsReady(void) { return flashDevice.vTable->isReady(&flashDevice); } -bool flashWaitForReady(void) +MMFLASH_CODE bool flashWaitForReady(void) { return flashDevice.vTable->waitForReady(&flashDevice); } -void flashEraseSector(uint32_t address) +MMFLASH_CODE void flashEraseSector(uint32_t address) { flashDevice.callback = NULL; flashDevice.vTable->eraseSector(&flashDevice, address); @@ -273,12 +427,12 @@ void flashEraseCompletely(void) /* The callback, if provided, will receive the totoal number of bytes transfered * by each call to flashPageProgramContinue() once the transfer completes. */ -void flashPageProgramBegin(uint32_t address, void (*callback)(uint32_t length)) +MMFLASH_CODE void flashPageProgramBegin(uint32_t address, void (*callback)(uint32_t length)) { flashDevice.vTable->pageProgramBegin(&flashDevice, address, callback); } -uint32_t flashPageProgramContinue(const uint8_t **buffers, uint32_t *bufferSizes, uint32_t bufferCount) +MMFLASH_CODE uint32_t flashPageProgramContinue(const uint8_t **buffers, uint32_t *bufferSizes, uint32_t bufferCount) { uint32_t maxBytesToWrite = flashDevice.geometry.pageSize - (flashDevice.currentWriteAddress % flashDevice.geometry.pageSize); @@ -299,23 +453,23 @@ uint32_t flashPageProgramContinue(const uint8_t **buffers, uint32_t *bufferSizes return flashDevice.vTable->pageProgramContinue(&flashDevice, buffers, bufferSizes, bufferCount); } -void flashPageProgramFinish(void) +MMFLASH_CODE void flashPageProgramFinish(void) { flashDevice.vTable->pageProgramFinish(&flashDevice); } -void flashPageProgram(uint32_t address, const uint8_t *data, uint32_t length, void (*callback)(uint32_t length)) +MMFLASH_CODE void flashPageProgram(uint32_t address, const uint8_t *data, uint32_t length, void (*callback)(uint32_t length)) { flashDevice.vTable->pageProgram(&flashDevice, address, data, length, callback); } -int flashReadBytes(uint32_t address, uint8_t *buffer, uint32_t length) +MMFLASH_CODE int flashReadBytes(uint32_t address, uint8_t *buffer, uint32_t length) { flashDevice.callback = NULL; return flashDevice.vTable->readBytes(&flashDevice, address, buffer, length); } -void flashFlush(void) +MMFLASH_CODE void flashFlush(void) { if (flashDevice.vTable->flush) { flashDevice.vTable->flush(&flashDevice); @@ -381,7 +535,7 @@ static void flashConfigurePartitions(void) startSector = 0; #endif -#if defined(CONFIG_IN_EXTERNAL_FLASH) +#if defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH) const uint32_t configSize = EEPROM_SIZE; flashSector_t configSectors = (configSize / flashGeometry->sectorSize); diff --git a/src/main/drivers/flash.h b/src/main/drivers/flash.h index 426f8479fd6..da75304bb6e 100644 --- a/src/main/drivers/flash.h +++ b/src/main/drivers/flash.h @@ -48,6 +48,16 @@ typedef struct flashGeometry_s { uint32_t jedecId; } flashGeometry_t; + +typedef enum { + /* + * When set it indicates the system was booted in memory mapped mode, flash chip is already configured by + * the bootloader and does not need re-configuration. + * When un-set the system was booted normally and the flash chip needs configuration before use. + */ + FLASH_CF_SYSTEM_IS_MEMORY_MAPPED = (1 << 0), +} flashConfigurationFlags_e; + void flashPreInit(const flashConfig_t *flashConfig); bool flashInit(const flashConfig_t *flashConfig); @@ -63,6 +73,10 @@ int flashReadBytes(uint32_t address, uint8_t *buffer, uint32_t length); void flashFlush(void); const flashGeometry_t *flashGetGeometry(void); +void flashMemoryMappedModeDisable(void); +void flashMemoryMappedModeEnable(void); + + // // flash partitioning api // diff --git a/src/main/drivers/flash_impl.h b/src/main/drivers/flash_impl.h index afa0fdd1a6d..6bd88bd8dbb 100644 --- a/src/main/drivers/flash_impl.h +++ b/src/main/drivers/flash_impl.h @@ -22,6 +22,30 @@ * Author: jflyper */ +/* + * Each flash chip should: + * + * * expose a public `identify` method. + * - return true if the driver supports the passed JEDEC ID and false otherwise. + * - configure the `geometry` member of the flashDevice_t or set all `geometry` members to 0 if driver doesn't support the JEDEC ID. + * - configure the `vTable` member, with an appropriate API. + * - configure remaining flashDevice_t members, as appropriate. + * * not reconfigure the bus or flash chip when in memory mapped mode. + * - the firmware is free to do whatever it wants when memory mapped mode is disabled + * - when memory mapped mode is restored, e.g. after saving config to external flash, it should be in + * the same state that firmware found it in before the firmware disabled memory mapped mode. + * + * When memory mapped mode is disabled the following applies to all flash chip drivers uses in a memory mapped system: + * - do not call any methods or use data from the flash chip. i.e. memory mapped code/data is INACCESSIBLE. + * i.e. when saving the config, *ALL* the code to erase a block and write data should be in RAM, + * this includes any `delay` methods. + * - not require the use of use any ISRs - interrupts are disabled during flash access when memory mapped mode is disabled. + * + * When compiling a driver for use in a memory mapped flash system the following applies: + * - the vTable must live in RAM so it can be used when memory mapped mode is disabled. + * - other constant data structures that usually live in flash memory must be stored in RAM. + * - methods used to erase sectors, write data and read data much live in RAM. + */ #pragma once #include "drivers/bus.h" @@ -32,20 +56,30 @@ struct flashVTable_s; typedef enum { FLASHIO_NONE = 0, FLASHIO_SPI, - FLASHIO_QUADSPI + FLASHIO_QUADSPI, + FLASHIO_OCTOSPI, } flashDeviceIoMode_e; typedef struct flashDeviceIO_s { union { + #ifdef USE_FLASH_SPI extDevice_t *dev; // Device interface dependent handle (spi/i2c) - #ifdef USE_QUADSPI + #endif + #ifdef USE_FLASH_QUADSPI QUADSPI_TypeDef *quadSpi; #endif + #ifdef USE_FLASH_OCTOSPI + OCTOSPI_TypeDef *octoSpi; + #endif } handle; flashDeviceIoMode_e mode; } flashDeviceIO_t; typedef struct flashDevice_s { + // + // members to be configured by the flash chip implementation + // + const struct flashVTable_s *vTable; flashGeometry_t geometry; uint32_t currentWriteAddress; @@ -55,21 +89,33 @@ typedef struct flashDevice_s { // when it is definitely ready already. bool couldBeBusy; uint32_t timeoutAt; + + // + // members configured by the flash detection system, read-only from the flash chip implementation's perspective. + // + flashDeviceIO_t io; void (*callback)(uint32_t arg); uint32_t callbackArg; } flashDevice_t; typedef struct flashVTable_s { + void (*configure)(flashDevice_t *fdevice, uint32_t configurationFlags); + bool (*isReady)(flashDevice_t *fdevice); bool (*waitForReady)(flashDevice_t *fdevice); + void (*eraseSector)(flashDevice_t *fdevice, uint32_t address); void (*eraseCompletely)(flashDevice_t *fdevice); + void (*pageProgramBegin)(flashDevice_t *fdevice, uint32_t address, void (*callback)(uint32_t length)); uint32_t (*pageProgramContinue)(flashDevice_t *fdevice, uint8_t const **buffers, uint32_t *bufferSizes, uint32_t bufferCount); void (*pageProgramFinish)(flashDevice_t *fdevice); void (*pageProgram)(flashDevice_t *fdevice, uint32_t address, const uint8_t *data, uint32_t length, void (*callback)(uint32_t length)); + void (*flush)(flashDevice_t *fdevice); + int (*readBytes)(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer, uint32_t length); + const flashGeometry_t *(*getGeometry)(flashDevice_t *fdevice); } flashVTable_t; diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index dfa10252f1f..c4a24e1bda1 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -195,13 +195,13 @@ static bool m25p16_waitForReady(flashDevice_t *fdevice) * * Returns true if we get valid ident, false if something bad happened like there is no M25P16. */ -bool m25p16_detect(flashDevice_t *fdevice, uint32_t chipID) +bool m25p16_identify(flashDevice_t *fdevice, uint32_t jedecID) { flashGeometry_t *geometry = &fdevice->geometry; uint8_t index; for (index = 0; m25p16FlashConfig[index].jedecID; index++) { - if (m25p16FlashConfig[index].jedecID == chipID) { + if (m25p16FlashConfig[index].jedecID == jedecID) { maxClkSPIHz = m25p16FlashConfig[index].maxClkSPIMHz * 1000000; maxReadClkSPIHz = m25p16FlashConfig[index].maxReadClkSPIMHz * 1000000; geometry->sectors = m25p16FlashConfig[index].sectors; @@ -224,11 +224,32 @@ bool m25p16_detect(flashDevice_t *fdevice, uint32_t chipID) geometry->sectorSize = geometry->pagesPerSector * geometry->pageSize; geometry->totalSize = geometry->sectorSize * geometry->sectors; + fdevice->couldBeBusy = true; // Just for luck we'll assume the chip could be busy even though it isn't specced to be + fdevice->couldBeBusy = true; // Just for luck we'll assume the chip could be busy even though it isn't specced to be + + if (fdevice->io.mode == FLASHIO_SPI) { + fdevice->vTable = &m25p16_vTable; + } +#ifdef USE_QUADSPI + else if (fdevice->io.mode == FLASHIO_QUADSPI) { + fdevice->vTable = &m25p16Qspi_vTable; + } +#endif + return true; +} + +void m25p16_configure(flashDevice_t *fdevice, uint32_t configurationFlags) +{ + if (configurationFlags & FLASH_CF_SYSTEM_IS_MEMORY_MAPPED) { + return; + } + if (fdevice->io.mode == FLASHIO_SPI) { // Adjust the SPI bus clock frequency spiSetClkDivisor(fdevice->io.handle.dev, spiCalculateDivider(maxReadClkSPIHz)); } + flashGeometry_t *geometry = &fdevice->geometry; if (geometry->totalSize > 16 * 1024 * 1024) { fdevice->isLargeFlash = true; @@ -244,20 +265,9 @@ bool m25p16_detect(flashDevice_t *fdevice, uint32_t chipID) } #endif } - - fdevice->couldBeBusy = true; // Just for luck we'll assume the chip could be busy even though it isn't specced to be - - if (fdevice->io.mode == FLASHIO_SPI) { - fdevice->vTable = &m25p16_vTable; - } -#ifdef USE_QUADSPI - else if (fdevice->io.mode == FLASHIO_QUADSPI) { - fdevice->vTable = &m25p16Qspi_vTable; - } -#endif - return true; } + static void m25p16_setCommandAddress(uint8_t *buf, uint32_t address, bool useLongAddress) { if (useLongAddress) { @@ -612,6 +622,7 @@ static const flashGeometry_t* m25p16_getGeometry(flashDevice_t *fdevice) } const flashVTable_t m25p16_vTable = { + .configure = m25p16_configure, .isReady = m25p16_isReady, .waitForReady = m25p16_waitForReady, .eraseSector = m25p16_eraseSector, diff --git a/src/main/drivers/flash_m25p16.h b/src/main/drivers/flash_m25p16.h index fa606a49475..2e0d186d02f 100644 --- a/src/main/drivers/flash_m25p16.h +++ b/src/main/drivers/flash_m25p16.h @@ -22,4 +22,4 @@ #include "flash_impl.h" -bool m25p16_detect(flashDevice_t *fdevice, uint32_t chipID); +bool m25p16_identify(flashDevice_t *fdevice, uint32_t jedecID); diff --git a/src/main/drivers/flash_w25m.c b/src/main/drivers/flash_w25m.c index 9600e54175f..34d19ea21a3 100644 --- a/src/main/drivers/flash_w25m.c +++ b/src/main/drivers/flash_w25m.c @@ -114,10 +114,10 @@ static bool w25m_waitForReady(flashDevice_t *fdevice) return true; } -bool w25m_detect(flashDevice_t *fdevice, uint32_t chipID) +bool w25m_identify(flashDevice_t *fdevice, uint32_t jedecID) { - switch (chipID) { + switch (jedecID) { #ifdef USE_FLASH_W25M512 case JEDEC_ID_WINBOND_W25M512: // W25Q256 x 2 @@ -127,7 +127,7 @@ bool w25m_detect(flashDevice_t *fdevice, uint32_t chipID) 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); + m25p16_identify(&dieDevice[die], JEDEC_ID_WINBOND_W25Q256); } fdevice->geometry.flashType = FLASH_TYPE_NOR; @@ -143,7 +143,7 @@ bool w25m_detect(flashDevice_t *fdevice, uint32_t chipID) 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); + w25n01g_identify(&dieDevice[die], JEDEC_ID_WINBOND_W25N01GV); } fdevice->geometry.flashType = FLASH_TYPE_NAND; @@ -170,6 +170,14 @@ bool w25m_detect(flashDevice_t *fdevice, uint32_t chipID) return true; } +void w25m_configure(flashDevice_t *fdevice, uint32_t configurationFlags) +{ + for (int dieNumber = 0 ; dieNumber < dieCount ; dieNumber++) { + w25m_dieSelect(fdevice->io.handle.dev, dieNumber); + dieDevice[dieNumber].vTable->configure(&dieDevice[dieNumber], configurationFlags); + } +} + void w25m_eraseSector(flashDevice_t *fdevice, uint32_t address) { int dieNumber = address / dieSize; @@ -255,6 +263,7 @@ const flashGeometry_t* w25m_getGeometry(flashDevice_t *fdevice) } static const flashVTable_t w25m_vTable = { + .configure = w25m_configure, .isReady = w25m_isReady, .waitForReady = w25m_waitForReady, .eraseSector = w25m_eraseSector, diff --git a/src/main/drivers/flash_w25m.h b/src/main/drivers/flash_w25m.h index f778a07eb61..5d35df4a4bf 100644 --- a/src/main/drivers/flash_w25m.h +++ b/src/main/drivers/flash_w25m.h @@ -22,4 +22,4 @@ #include "flash_impl.h" -bool w25m_detect(flashDevice_t *fdevice, uint32_t chipID); +bool w25m_identify(flashDevice_t *fdevice, uint32_t jedecID); diff --git a/src/main/drivers/flash_w25n01g.c b/src/main/drivers/flash_w25n01g.c index b69f8f18451..87c3f604194 100644 --- a/src/main/drivers/flash_w25n01g.c +++ b/src/main/drivers/flash_w25n01g.c @@ -124,9 +124,9 @@ #define W25N01G_TIMEOUT_RESET_MS 500 // tRSTmax = 500ms // Sizes (in bits) -#define W28N01G_STATUS_REGISTER_SIZE 8 -#define W28N01G_STATUS_PAGE_ADDRESS_SIZE 16 -#define W28N01G_STATUS_COLUMN_ADDRESS_SIZE 16 +#define W25N01G_STATUS_REGISTER_SIZE 8 +#define W25N01G_STATUS_PAGE_ADDRESS_SIZE 16 +#define W25N01G_STATUS_COLUMN_ADDRESS_SIZE 16 typedef struct bblut_s { uint16_t pba; @@ -188,7 +188,7 @@ static void w25n01g_performCommandWithPageAddress(flashDeviceIO_t *io, uint8_t c else if (io->mode == FLASHIO_QUADSPI) { QUADSPI_TypeDef *quadSpi = io->handle.quadSpi; - quadSpiInstructionWithAddress1LINE(quadSpi, command, 0, pageAddress & 0xffff, W28N01G_STATUS_PAGE_ADDRESS_SIZE + 8); + quadSpiInstructionWithAddress1LINE(quadSpi, command, 0, pageAddress & 0xffff, W25N01G_STATUS_PAGE_ADDRESS_SIZE + 8); } #endif } @@ -221,8 +221,8 @@ static uint8_t w25n01g_readRegister(flashDeviceIO_t *io, uint8_t reg) QUADSPI_TypeDef *quadSpi = io->handle.quadSpi; - uint8_t in[1]; - quadSpiReceiveWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_READ_STATUS_REG, 0, reg, W28N01G_STATUS_REGISTER_SIZE, in, sizeof(in)); + uint8_t in[W25N01G_STATUS_REGISTER_SIZE / 8]; + quadSpiReceiveWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_READ_STATUS_REG, 0, reg, W25N01G_STATUS_REGISTER_SIZE, in, sizeof(in)); return in[0]; } @@ -253,7 +253,7 @@ static void w25n01g_writeRegister(flashDeviceIO_t *io, uint8_t reg, uint8_t data else if (io->mode == FLASHIO_QUADSPI) { QUADSPI_TypeDef *quadSpi = io->handle.quadSpi; - quadSpiTransmitWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_WRITE_STATUS_REG, 0, reg, W28N01G_STATUS_REGISTER_SIZE, &data, 1); + quadSpiTransmitWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_WRITE_STATUS_REG, 0, reg, W25N01G_STATUS_REGISTER_SIZE, &data, 1); } #endif } @@ -311,18 +311,11 @@ static void w25n01g_writeEnable(flashDevice_t *fdevice) fdevice->couldBeBusy = true; } -/** - * Read chip identification and geometry information (into global `geometry`). - * - * Returns true if we get valid ident, false if something bad happened like there is no M25P16. - */ const flashVTable_t w25n01g_vTable; -static void w25n01g_deviceInit(flashDevice_t *flashdev); - -bool w25n01g_detect(flashDevice_t *fdevice, uint32_t chipID) +bool w25n01g_identify(flashDevice_t *fdevice, uint32_t jedecID) { - switch (chipID) { + switch (jedecID) { case JEDEC_ID_WINBOND_W25N01GV: fdevice->geometry.sectors = 1024; // Blocks fdevice->geometry.pagesPerSector = 64; // Pages/Blocks @@ -348,6 +341,19 @@ bool w25n01g_detect(flashDevice_t *fdevice, uint32_t chipID) W25N01G_BB_MANAGEMENT_START_BLOCK + W25N01G_BB_MANAGEMENT_BLOCKS - 1); fdevice->couldBeBusy = true; // Just for luck we'll assume the chip could be busy even though it isn't specced to be + fdevice->vTable = &w25n01g_vTable; + + return true; +} + +static void w25n01g_deviceInit(flashDevice_t *flashdev); + + +void w25n01g_configure(flashDevice_t *fdevice, uint32_t configurationFlags) +{ + if (configurationFlags & FLASH_CF_SYSTEM_IS_MEMORY_MAPPED) { + return; + } w25n01g_deviceReset(fdevice); @@ -366,10 +372,6 @@ bool w25n01g_detect(flashDevice_t *fdevice, uint32_t chipID) // If it ever run out, the device becomes unusable. w25n01g_deviceInit(fdevice); - - fdevice->vTable = &w25n01g_vTable; - - return true; } /** @@ -422,7 +424,7 @@ static void w25n01g_programDataLoad(flashDevice_t *fdevice, uint16_t columnAddre else if (fdevice->io.mode == FLASHIO_QUADSPI) { QUADSPI_TypeDef *quadSpi = fdevice->io.handle.quadSpi; - quadSpiTransmitWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_PROGRAM_DATA_LOAD, 0, columnAddress, W28N01G_STATUS_COLUMN_ADDRESS_SIZE, data, length); + quadSpiTransmitWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_PROGRAM_DATA_LOAD, 0, columnAddress, W25N01G_STATUS_COLUMN_ADDRESS_SIZE, data, length); } #endif @@ -453,7 +455,7 @@ static void w25n01g_randomProgramDataLoad(flashDevice_t *fdevice, uint16_t colum else if (fdevice->io.mode == FLASHIO_QUADSPI) { QUADSPI_TypeDef *quadSpi = fdevice->io.handle.quadSpi; - quadSpiTransmitWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_RANDOM_PROGRAM_DATA_LOAD, 0, columnAddress, W28N01G_STATUS_COLUMN_ADDRESS_SIZE, data, length); + quadSpiTransmitWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_RANDOM_PROGRAM_DATA_LOAD, 0, columnAddress, W25N01G_STATUS_COLUMN_ADDRESS_SIZE, data, length); } #endif @@ -696,8 +698,8 @@ int w25n01g_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer, else if (fdevice->io.mode == FLASHIO_QUADSPI) { QUADSPI_TypeDef *quadSpi = fdevice->io.handle.quadSpi; - //quadSpiReceiveWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_READ_DATA, 8, column, W28N01G_STATUS_COLUMN_ADDRESS_SIZE, buffer, length); - quadSpiReceiveWithAddress4LINES(quadSpi, W25N01G_INSTRUCTION_FAST_READ_QUAD_OUTPUT, 8, column, W28N01G_STATUS_COLUMN_ADDRESS_SIZE, buffer, length); + //quadSpiReceiveWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_READ_DATA, 8, column, W25N01G_STATUS_COLUMN_ADDRESS_SIZE, buffer, length); + quadSpiReceiveWithAddress4LINES(quadSpi, W25N01G_INSTRUCTION_FAST_READ_QUAD_OUTPUT, 8, column, W25N01G_STATUS_COLUMN_ADDRESS_SIZE, buffer, length); } #endif @@ -765,7 +767,7 @@ int w25n01g_readExtensionBytes(flashDevice_t *fdevice, uint32_t address, uint8_t else if (fdevice->io.mode == FLASHIO_QUADSPI) { QUADSPI_TypeDef *quadSpi = fdevice->io.handle.quadSpi; - quadSpiReceiveWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_READ_DATA, 8, column, W28N01G_STATUS_COLUMN_ADDRESS_SIZE, buffer, length); + quadSpiReceiveWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_READ_DATA, 8, column, W25N01G_STATUS_COLUMN_ADDRESS_SIZE, buffer, length); } #endif @@ -785,6 +787,7 @@ const flashGeometry_t* w25n01g_getGeometry(flashDevice_t *fdevice) } const flashVTable_t w25n01g_vTable = { + .configure = w25n01g_configure, .isReady = w25n01g_isReady, .waitForReady = w25n01g_waitForReady, .eraseSector = w25n01g_eraseSector, diff --git a/src/main/drivers/flash_w25n01g.h b/src/main/drivers/flash_w25n01g.h index 219a3fa4660..ef80c7b6e32 100644 --- a/src/main/drivers/flash_w25n01g.h +++ b/src/main/drivers/flash_w25n01g.h @@ -25,4 +25,4 @@ // JEDEC ID #define JEDEC_ID_WINBOND_W25N01GV 0xEFAA21 -bool w25n01g_detect(flashDevice_t *fdevice, uint32_t chipID); +bool w25n01g_identify(flashDevice_t *fdevice, uint32_t jedecID); diff --git a/src/main/drivers/flash_w25q128fv.c b/src/main/drivers/flash_w25q128fv.c index d005f8e8eeb..e8fb03bf134 100644 --- a/src/main/drivers/flash_w25q128fv.c +++ b/src/main/drivers/flash_w25q128fv.c @@ -25,7 +25,7 @@ #include "platform.h" -#if defined(USE_FLASH_W25Q128FV) && defined(USE_QUADSPI) +#if defined(USE_FLASH_W25Q128FV) && (defined(USE_QUADSPI) || defined(USE_OCTOSPI)) #define USE_FLASH_WRITES_USING_4LINES #define USE_FLASH_READS_USING_4LINES @@ -33,15 +33,19 @@ #include "build/debug.h" #include "common/utils.h" +#include "drivers/time.h" #include "drivers/flash.h" #include "drivers/flash_impl.h" #include "drivers/flash_w25q128fv.h" #include "drivers/bus_quadspi.h" +#include "drivers/bus_octospi.h" // JEDEC ID -#define JEDEC_ID_WINBOND_W25Q128FV_SPI 0xEF4018 -#define JEDEC_ID_WINBOND_W25Q128FV_QUADSPI 0xEF6018 -#define JEDEC_ID_WINBOND_W25Q128JV_QUADSPI 0xEF7018 +#define JEDEC_ID_WINBOND_W25Q128FV_SPI 0xEF4018 +#define JEDEC_ID_WINBOND_W25Q128FV_QUADSPI 0xEF6018 +#define JEDEC_ID_WINBOND_W25Q128JV_QUADSPI 0xEF7018 +#define JEDEC_ID_WINBOND_W25Q16JV_SPI 0xEF4015 +#define JEDEC_ID_WINBOND_W25Q16JV_DTR_SPI 0xEF7015 // Device size parameters #define W25Q128FV_PAGE_SIZE 2048 @@ -90,14 +94,14 @@ //#define W25Q128FV_INSTRUCTION_WRITE_DISABLE 0x04 //#define W25Q128FV_INSTRUCTION_PAGE_PROGRAM 0x02 -// Timings (2ms minimum to avoid 1 tick advance in consecutive calls to HAL_GetTick). -#define W25Q128FV_TIMEOUT_PAGE_READ_MS 4 -#define W25Q128FV_TIMEOUT_RESET_MS 2 // tRST = 30us +// Values from W25Q128FV Datasheet Rev L. +#define W25Q128FV_TIMEOUT_PAGE_READ_MS 1 // No minimum specified in datasheet +#define W25Q128FV_TIMEOUT_RESET_MS 1 // tRST = 30us #define W25Q128FV_TIMEOUT_BLOCK_ERASE_64KB_MS 2000 // tBE2max = 2000ms, tBE2typ = 150ms #define W25Q128FV_TIMEOUT_CHIP_ERASE_MS (200 * 1000) // tCEmax 200s, tCEtyp = 40s -#define W25Q128FV_TIMEOUT_PAGE_PROGRAM_MS 2 // tPPmax = 700us, tPPtyp = 250us -#define W25Q128FV_TIMEOUT_WRITE_ENABLE_MS 2 +#define W25Q128FV_TIMEOUT_PAGE_PROGRAM_MS 3 // tPPmax = 3ms, tPPtyp = 0.7ms +#define W25Q128FV_TIMEOUT_WRITE_ENABLE_MS 1 typedef enum { @@ -115,45 +119,71 @@ w25q128fvState_t w25q128fvState = { 0 }; static bool w25q128fv_waitForReady(flashDevice_t *fdevice); static void w25q128fv_waitForTimeout(flashDevice_t *fdevice); -static void w25q128fv_setTimeout(flashDevice_t *fdevice, uint32_t timeoutMillis) +MMFLASH_CODE static void w25q128fv_setTimeout(flashDevice_t *fdevice, timeMs_t timeoutMillis) { - uint32_t now = HAL_GetTick(); - fdevice->timeoutAt = now + timeoutMillis; + timeMs_t nowMs = microsISR() / 1000; + fdevice->timeoutAt = nowMs + timeoutMillis; } -static void w25q128fv_performOneByteCommand(flashDeviceIO_t *io, uint8_t command) +MMFLASH_CODE static void w25q128fv_performOneByteCommand(flashDeviceIO_t *io, uint8_t command) { +#if defined(USE_QUADSPI) QUADSPI_TypeDef *quadSpi = io->handle.quadSpi; quadSpiTransmit1LINE(quadSpi, command, 0, NULL, 0); +#elif defined(USE_OCTOSPI) + OCTOSPI_TypeDef *octoSpi = io->handle.octoSpi; + octoSpiTransmit1LINE(octoSpi, command, 0, NULL, 0); +#endif + } -static void w25q128fv_performCommandWithAddress(flashDeviceIO_t *io, uint8_t command, uint32_t address) +MMFLASH_CODE static void w25q128fv_performCommandWithAddress(flashDeviceIO_t *io, uint8_t command, uint32_t address) { +#if defined(USE_QUADSPI) QUADSPI_TypeDef *quadSpi = io->handle.quadSpi; quadSpiInstructionWithAddress1LINE(quadSpi, command, 0, address & 0xffffff, W25Q128FV_ADDRESS_BITS); +#elif defined(USE_OCTOSPI) + OCTOSPI_TypeDef *octoSpi = io->handle.octoSpi; + + octoSpiInstructionWithAddress1LINE(octoSpi, command, 0, address & 0xffffff, W25Q128FV_ADDRESS_BITS); +#endif } -static void w25q128fv_writeEnable(flashDevice_t *fdevice) +MMFLASH_CODE static void w25q128fv_writeEnable(flashDevice_t *fdevice) { w25q128fv_performOneByteCommand(&fdevice->io, W25Q128FV_INSTRUCTION_WRITE_ENABLE); } -static uint8_t w25q128fv_readRegister(flashDeviceIO_t *io, uint8_t command) +MMFLASH_CODE static uint8_t w25q128fv_readRegister(flashDeviceIO_t *io, uint8_t command) { + uint8_t in[W25Q128FV_STATUS_REGISTER_BITS / 8] = { 0 }; +#if defined(USE_QUADSPI) QUADSPI_TypeDef *quadSpi = io->handle.quadSpi; - uint8_t in[1]; quadSpiReceive1LINE(quadSpi, command, 0, in, W25Q128FV_STATUS_REGISTER_BITS / 8); +#elif defined(USE_OCTOSPI) + OCTOSPI_TypeDef *octoSpi = io->handle.octoSpi; + + octoSpiReceive1LINE(octoSpi, command, 0, in, W25Q128FV_STATUS_REGISTER_BITS / 8); +#endif + return in[0]; } static void w25q128fv_writeRegister(flashDeviceIO_t *io, uint8_t command, uint8_t data) { +#if defined(USE_QUADSPI) QUADSPI_TypeDef *quadSpi = io->handle.quadSpi; - quadSpiTransmit1LINE(quadSpi, command, 0, &data, W25Q128FV_STATUS_REGISTER_BITS / 8); + quadSpiTransmit1LINE(quadSpi, command, 0, &data, W25Q128FV_STATUS_REGISTER_BITS / 8); +#elif defined(USE_OCTOSPI) + OCTOSPI_TypeDef *octoSpi = io->handle.octoSpi; + + octoSpiTransmit1LINE(octoSpi, command, 0, &data, W25Q128FV_STATUS_REGISTER_BITS / 8); +#endif + } static void w25q128fv_deviceReset(flashDevice_t *fdevice) @@ -203,7 +233,7 @@ static void w25q128fv_deviceReset(flashDevice_t *fdevice) #endif } -bool w25q128fv_isReady(flashDevice_t *fdevice) +MMFLASH_CODE bool w25q128fv_isReady(flashDevice_t *fdevice) { uint8_t status = w25q128fv_readRegister(&fdevice->io, W25Q128FV_INSTRUCTION_READ_STATUS1_REG); @@ -212,7 +242,7 @@ bool w25q128fv_isReady(flashDevice_t *fdevice) return !busy; } -static bool w25q128fv_isWritable(flashDevice_t *fdevice) +MMFLASH_CODE static bool w25q128fv_isWritable(flashDevice_t *fdevice) { uint8_t status = w25q128fv_readRegister(&fdevice->io, W25Q128FV_INSTRUCTION_READ_STATUS1_REG); @@ -221,23 +251,23 @@ static bool w25q128fv_isWritable(flashDevice_t *fdevice) return writable; } -bool w25q128fv_hasTimedOut(flashDevice_t *fdevice) +MMFLASH_CODE bool w25q128fv_hasTimedOut(flashDevice_t *fdevice) { - uint32_t now = HAL_GetTick(); - if (cmp32(now, fdevice->timeoutAt) >= 0) { + uint32_t nowMs = microsISR() / 1000; + if (cmp32(nowMs, fdevice->timeoutAt) >= 0) { return true; } return false; } -void w25q128fv_waitForTimeout(flashDevice_t *fdevice) +MMFLASH_CODE void w25q128fv_waitForTimeout(flashDevice_t *fdevice) { while (!w25q128fv_hasTimedOut(fdevice)) { } fdevice->timeoutAt = 0; } -bool w25q128fv_waitForReady(flashDevice_t *fdevice) +MMFLASH_CODE bool w25q128fv_waitForReady(flashDevice_t *fdevice) { bool ready = true; while (!w25q128fv_isReady(fdevice)) { @@ -255,15 +285,24 @@ const flashVTable_t w25q128fv_vTable; static void w25q128fv_deviceInit(flashDevice_t *flashdev); -bool w25q128fv_detect(flashDevice_t *fdevice, uint32_t chipID) +MMFLASH_CODE_NOINLINE bool w25q128fv_identify(flashDevice_t *fdevice, uint32_t jedecID) { - switch (chipID) { + switch (jedecID) { case JEDEC_ID_WINBOND_W25Q128FV_SPI: case JEDEC_ID_WINBOND_W25Q128FV_QUADSPI: case JEDEC_ID_WINBOND_W25Q128JV_QUADSPI: fdevice->geometry.sectors = 256; fdevice->geometry.pagesPerSector = 256; fdevice->geometry.pageSize = 256; + // = 16777216 128MBit 16MB + break; + + case JEDEC_ID_WINBOND_W25Q16JV_DTR_SPI: + case JEDEC_ID_WINBOND_W25Q16JV_SPI: + fdevice->geometry.sectors = 32; + fdevice->geometry.pagesPerSector = 256; + fdevice->geometry.pageSize = 256; + // = 2097152 16MBit 2MB break; default: @@ -276,7 +315,9 @@ bool w25q128fv_detect(flashDevice_t *fdevice, uint32_t chipID) } // use the chip id to determine the initial interface mode on cold-boot. - switch (chipID) { + switch (jedecID) { + case JEDEC_ID_WINBOND_W25Q16JV_SPI: + case JEDEC_ID_WINBOND_W25Q16JV_DTR_SPI: case JEDEC_ID_WINBOND_W25Q128FV_SPI: w25q128fvState.initialMode = INITIAL_MODE_SPI; break; @@ -294,18 +335,24 @@ bool w25q128fv_detect(flashDevice_t *fdevice, uint32_t chipID) fdevice->geometry.sectorSize = fdevice->geometry.pagesPerSector * fdevice->geometry.pageSize; fdevice->geometry.totalSize = fdevice->geometry.sectorSize * fdevice->geometry.sectors; - w25q128fv_deviceReset(fdevice); - - w25q128fv_deviceInit(fdevice); - fdevice->vTable = &w25q128fv_vTable; return true; } -static void w25q128fv_eraseSector(flashDevice_t *fdevice, uint32_t address) +void w25q128fv_configure(flashDevice_t *fdevice, uint32_t configurationFlags) { + if (configurationFlags & FLASH_CF_SYSTEM_IS_MEMORY_MAPPED) { + return; + } + w25q128fv_deviceReset(fdevice); + + w25q128fv_deviceInit(fdevice); +} + +MMFLASH_CODE static void w25q128fv_eraseSector(flashDevice_t *fdevice, uint32_t address) +{ w25q128fv_waitForReady(fdevice); w25q128fv_writeEnable(fdevice); @@ -326,17 +373,26 @@ static void w25q128fv_eraseCompletely(flashDevice_t *fdevice) w25q128fv_setTimeout(fdevice, W25Q128FV_TIMEOUT_CHIP_ERASE_MS); } - -static void w25q128fv_loadProgramData(flashDevice_t *fdevice, const uint8_t *data, int length) +MMFLASH_CODE static void w25q128fv_loadProgramData(flashDevice_t *fdevice, const uint8_t *data, int length) { w25q128fv_waitForReady(fdevice); +#if defined(USE_QUADSPI) QUADSPI_TypeDef *quadSpi = fdevice->io.handle.quadSpi; #ifdef USE_FLASH_WRITES_USING_4LINES quadSpiTransmitWithAddress4LINES(quadSpi, W25Q128FV_INSTRUCTION_QUAD_PAGE_PROGRAM, 0, w25q128fvState.currentWriteAddress, W25Q128FV_ADDRESS_BITS, data, length); #else quadSpiTransmitWithAddress1LINE(quadSpi, W25Q128FV_INSTRUCTION_PAGE_PROGRAM, 0, w25q128fvState.currentWriteAddress, W25Q128FV_ADDRESS_BITS, data, length); +#endif +#elif defined(USE_OCTOSPI) + OCTOSPI_TypeDef *octoSpi = fdevice->io.handle.octoSpi; + +#ifdef USE_FLASH_WRITES_USING_4LINES + octoSpiTransmitWithAddress4LINES(octoSpi, W25Q128FV_INSTRUCTION_QUAD_PAGE_PROGRAM, 0, w25q128fvState.currentWriteAddress, W25Q128FV_ADDRESS_BITS, data, length); +#else + octoSpiTransmitWithAddress1LINE(octoSpi, W25Q128FV_INSTRUCTION_PAGE_PROGRAM, 0, w25q128fvState.currentWriteAddress, W25Q128FV_ADDRESS_BITS, data, length); +#endif #endif w25q128fv_setTimeout(fdevice, W25Q128FV_TIMEOUT_PAGE_PROGRAM_MS); @@ -344,13 +400,13 @@ static void w25q128fv_loadProgramData(flashDevice_t *fdevice, const uint8_t *dat w25q128fvState.currentWriteAddress += length; } -static void w25q128fv_pageProgramBegin(flashDevice_t *fdevice, uint32_t address, void (*callback)(uint32_t length)) +MMFLASH_CODE static void w25q128fv_pageProgramBegin(flashDevice_t *fdevice, uint32_t address, void (*callback)(uint32_t length)) { fdevice->callback = callback; w25q128fvState.currentWriteAddress = address; } -static uint32_t w25q128fv_pageProgramContinue(flashDevice_t *fdevice, uint8_t const **buffers, uint32_t *bufferSizes, uint32_t bufferCount) +MMFLASH_CODE static uint32_t w25q128fv_pageProgramContinue(flashDevice_t *fdevice, uint8_t const **buffers, uint32_t *bufferSizes, uint32_t bufferCount) { for (uint32_t i = 0; i < bufferCount; i++) { w25q128fv_waitForReady(fdevice); @@ -374,35 +430,45 @@ static uint32_t w25q128fv_pageProgramContinue(flashDevice_t *fdevice, uint8_t co return fdevice->callbackArg; } -static void w25q128fv_pageProgramFinish(flashDevice_t *fdevice) +MMFLASH_CODE static void w25q128fv_pageProgramFinish(flashDevice_t *fdevice) { UNUSED(fdevice); } -static void w25q128fv_pageProgram(flashDevice_t *fdevice, uint32_t address, const uint8_t *data, uint32_t length, void (*callback)(uint32_t length)) +MMFLASH_CODE static void w25q128fv_pageProgram(flashDevice_t *fdevice, uint32_t address, const uint8_t *data, uint32_t length, void (*callback)(uint32_t length)) { w25q128fv_pageProgramBegin(fdevice, address, callback); w25q128fv_pageProgramContinue(fdevice, &data, &length, 1); w25q128fv_pageProgramFinish(fdevice); } -void w25q128fv_flush(flashDevice_t *fdevice) +MMFLASH_CODE void w25q128fv_flush(flashDevice_t *fdevice) { UNUSED(fdevice); } -static int w25q128fv_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer, uint32_t length) +MMFLASH_CODE static int w25q128fv_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer, uint32_t length) { if (!w25q128fv_waitForReady(fdevice)) { return 0; } +#if defined(USE_QUADSPI) QUADSPI_TypeDef *quadSpi = fdevice->io.handle.quadSpi; #ifdef USE_FLASH_READS_USING_4LINES bool status = quadSpiReceiveWithAddress4LINES(quadSpi, W25Q128FV_INSTRUCTION_FAST_READ_QUAD_OUTPUT, 8, address, W25Q128FV_ADDRESS_BITS, buffer, length); #else bool status = quadSpiReceiveWithAddress1LINE(quadSpi, W25Q128FV_INSTRUCTION_FAST_READ, 8, address, W25Q128FV_ADDRESS_BITS, buffer, length); #endif +#elif defined(USE_OCTOSPI) + OCTOSPI_TypeDef *octoSpi = fdevice->io.handle.octoSpi; +#ifdef USE_FLASH_READS_USING_4LINES + bool status = octoSpiReceiveWithAddress4LINES(octoSpi, W25Q128FV_INSTRUCTION_FAST_READ_QUAD_OUTPUT, 8, address, W25Q128FV_ADDRESS_BITS, buffer, length); +#else + bool status = octoSpiReceiveWithAddress1LINE(octoSpi, W25Q128FV_INSTRUCTION_FAST_READ, 8, address, W25Q128FV_ADDRESS_BITS, buffer, length); +#endif +#endif + w25q128fv_setTimeout(fdevice, W25Q128FV_TIMEOUT_PAGE_READ_MS); if (!status) { @@ -412,14 +478,13 @@ static int w25q128fv_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t return length; } - - const flashGeometry_t* w25q128fv_getGeometry(flashDevice_t *fdevice) { return &fdevice->geometry; } -const flashVTable_t w25q128fv_vTable = { +MMFLASH_DATA const flashVTable_t w25q128fv_vTable = { + .configure = w25q128fv_configure, .isReady = w25q128fv_isReady, .waitForReady = w25q128fv_waitForReady, .eraseSector = w25q128fv_eraseSector, diff --git a/src/main/drivers/flash_w25q128fv.h b/src/main/drivers/flash_w25q128fv.h index d28de244faf..4a82c0d9984 100644 --- a/src/main/drivers/flash_w25q128fv.h +++ b/src/main/drivers/flash_w25q128fv.h @@ -3,5 +3,5 @@ #ifdef USE_FLASH_W25Q128FV -bool w25q128fv_detect(flashDevice_t *fdevice, uint32_t chipID); +bool w25q128fv_identify(flashDevice_t *fdevice, uint32_t jedecID); #endif diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index b17522d848a..ec16015e1ee 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -103,7 +103,7 @@ void SysTick_Handler(void) // Return system uptime in microseconds (rollover in 70minutes) -uint32_t microsISR(void) +MMFLASH_CODE_NOINLINE uint32_t microsISR(void) { register uint32_t ms, pending, cycle_cnt; @@ -262,7 +262,7 @@ void failureMode(failureMode_e mode) void initialiseMemorySections(void) { #ifdef USE_ITCM_RAM - /* Load functions into ITCM RAM */ + /* Load fast-functions into ITCM RAM */ extern uint8_t tcm_code_start; extern uint8_t tcm_code_end; extern uint8_t tcm_code; @@ -284,6 +284,15 @@ void initialiseMemorySections(void) extern uint8_t _sfastram_idata; memcpy(&_sfastram_data, &_sfastram_idata, (size_t) (&_efastram_data - &_sfastram_data)); #endif + +#ifdef USE_RAM_CODE + /* Load slow-functions into ITCM RAM */ + extern uint8_t ram_code_start; + extern uint8_t ram_code_end; + extern uint8_t ram_code; + memcpy(&ram_code_start, &ram_code, (size_t) (&ram_code_end - &ram_code_start)); +#endif + } #ifdef STM32H7 diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 27609283840..21b1719c67b 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -73,6 +73,10 @@ uint32_t getCycleCounter(void); void systemProcessResetReason(void); #endif +// memory +void memoryMappedModeInit(void); +bool isMemoryMappedModeEnabledOnBoot(void); + void initialiseMemorySections(void); #ifdef STM32H7 void initialiseD2MemorySections(void); diff --git a/src/main/drivers/system_stm32h7xx.c b/src/main/drivers/system_stm32h7xx.c index 49f96f831be..8f4f8ce79b9 100644 --- a/src/main/drivers/system_stm32h7xx.c +++ b/src/main/drivers/system_stm32h7xx.c @@ -62,6 +62,41 @@ bool isMPUSoftReset(void) return false; } +#if defined(USE_FLASH_MEMORY_MAPPED) + +/* + * Memory mapped targets use a bootloader which enables memory mapped mode before running the firmware directly from external flash. + * Code running from external flash, i.e. most of the firmware, must not disable peripherals or reconfigure pins used by the CPU to access the flash chip. + * Refer to reference manuals and linker scripts for addresses of memory mapped regions. + * STM32H830 - RM0468 "Table 6. Memory map and default device memory area attributes" + * + * If the config is also stored on the same flash chip that code is running from then VERY special care must be taken when detecting the flash chip + * and when writing an updated config back to the flash. + */ + +static bool memoryMappedModeEnabledOnBoot = false; + +bool isMemoryMappedModeEnabledOnBoot(void) +{ + return memoryMappedModeEnabledOnBoot; +} + +void memoryMappedModeInit(void) +{ +#if defined(STM32H730xx) || defined(STM32H723xx) + // Smaller MCU packages have ONE OCTOSPI interface which supports memory mapped mode. + memoryMappedModeEnabledOnBoot = READ_BIT(OCTOSPI1->CR, OCTOSPI_CR_FMODE) == OCTOSPI_CR_FMODE; +#else +#error No Memory Mapped implementation on current MCU. +#endif +} +#else +bool isMemoryMappedModeEnabledOnBoot(void) +{ + return false; +} +#endif + void systemInit(void) { // Configure NVIC preempt/priority groups diff --git a/src/main/fc/init.c b/src/main/fc/init.c index 3ab56faf261..f6aa8f59b1d 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -47,6 +47,7 @@ #include "drivers/adc.h" #include "drivers/bus.h" #include "drivers/bus_i2c.h" +#include "drivers/bus_octospi.h" #include "drivers/bus_quadspi.h" #include "drivers/bus_spi.h" #include "drivers/buttons.h" @@ -195,8 +196,7 @@ void busSwitchInit(void) } #endif - -static void configureSPIAndQuadSPI(void) +static void configureSPIBusses(void) { #ifdef USE_SPI spiPinConfigure(spiPinConfig(0)); @@ -226,7 +226,10 @@ static void configureSPIAndQuadSPI(void) spiInit(SPIDEV_6); #endif #endif // USE_SPI +} +static void configureQuadSPIBusses(void) +{ #ifdef USE_QUADSPI quadSpiPinConfigure(quadSpiConfig(0)); @@ -236,6 +239,15 @@ static void configureSPIAndQuadSPI(void) #endif // USE_QUAD_SPI } +static void configureOctoSPIBusses(void) +{ +#ifdef USE_OCTOSPI +#ifdef USE_OCTOSPI_DEVICE_1 + octoSpiInit(OCTOSPIDEV_1); +#endif +#endif +} + #ifdef USE_SDCARD static void sdCardAndFSInit(void) { @@ -291,9 +303,10 @@ void init(void) #endif enum { - FLASH_INIT_ATTEMPTED = (1 << 0), - SD_INIT_ATTEMPTED = (1 << 1), - SPI_AND_QSPI_INIT_ATTEMPTED = (1 << 2), + FLASH_INIT_ATTEMPTED = (1 << 0), + SD_INIT_ATTEMPTED = (1 << 1), + SPI_BUSSES_INIT_ATTEMPTED = (1 << 2), + QUAD_OCTO_SPI_BUSSES_INIT_ATTEMPTED = (1 << 3), }; uint8_t initFlags = 0; @@ -303,19 +316,17 @@ void init(void) // Config in sdcard presents an issue with pin configuration since the pin and sdcard configs for the // sdcard are in the config which is on the sdcard which we can't read yet! // - // FIXME We need to add configuration somewhere, e.g. bootloader image or reserved flash area, that can be read by the firmware. - // it's currently possible for the firmware resource allocation to be wrong after the config is loaded if the user changes the settings. - // This would cause undefined behaviour once the config is loaded. so for now, users must NOT change sdio/spi configs needed for - // the system to boot and/or to save the config. + // FIXME For now, users must NOT change flash/pin configs needed for the system to boot and/or to save the config. + // One possible solution is to lock the pins for the flash chip so they cannot be modified post-boot. // - // note that target specific SDCARD/SDIO/SPI/QUADSPI configs are + // note that target specific SDCARD/SDIO/SPI/QUADSPI/OCTOSPI configs are // also not supported in USE_TARGET_CONFIG/targetConfigure() when using CONFIG_IN_SDCARD. // // // IMPORTANT: all default flash and pin configurations must be valid for the target after pgResetAll() is called. - // Target designers must ensure other devices connected the same SPI/QUADSPI interface as the flash chip do not - // cause communication issues with the flash chip. e.g. use external pullups on SPI/QUADSPI CS lines. + // Target designers must ensure other devices connected the same SPI/QUADSPI/OCTOSPI interface as the flash chip do not + // cause communication issues with the flash chip. e.g. use external pullups on SPI/QUADSPI/OCTOSPI CS lines. // #ifdef TARGET_BUS_INIT @@ -325,8 +336,8 @@ void init(void) pgResetAll(); #ifdef USE_SDCARD_SPI - configureSPIAndQuadSPI(); - initFlags |= SPI_AND_QSPI_INIT_ATTEMPTED; + configureSPIBusses(); + initFlags |= SPI_BUSSES_INIT_ATTEMPTED; #endif sdCardAndFSInit(); @@ -346,37 +357,42 @@ void init(void) #endif // CONFIG_IN_SDCARD -#ifdef CONFIG_IN_EXTERNAL_FLASH +#if defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH) // // Config on external flash presents an issue with pin configuration since the pin and flash configs for the // external flash are in the config which is on a chip which we can't read yet! // - // FIXME We need to add configuration somewhere, e.g. bootloader image or reserved flash area, that can be read by the firmware. - // it's currently possible for the firmware resource allocation to be wrong after the config is loaded if the user changes the settings. - // This would cause undefined behaviour once the config is loaded. so for now, users must NOT change flash/pin configs needed for - // the system to boot and/or to save the config. + // FIXME For now, users must NOT change flash/pin configs needed for the system to boot and/or to save the config. + // One possible solution is to lock the pins for the flash chip so they cannot be modified post-boot. // - // note that target specific FLASH/SPI/QUADSPI configs are - // also not supported in USE_TARGET_CONFIG/targetConfigure() when using CONFIG_IN_EXTERNAL_FLASH. + // note that target specific FLASH/SPI/QUADSPI/OCTOSPI configs are + // also not supported in USE_TARGET_CONFIG/targetConfigure() when using CONFIG_IN_EXTERNAL_FLASH/CONFIG_IN_MEMORY_MAPPED_FLASH. // // // IMPORTANT: all default flash and pin configurations must be valid for the target after pgResetAll() is called. - // Target designers must ensure other devices connected the same SPI/QUADSPI interface as the flash chip do not - // cause communication issues with the flash chip. e.g. use external pullups on SPI/QUADSPI CS lines. + // Target designers must ensure other devices connected the same SPI/QUADSPI/OCTOSPI interface as the flash chip do not + // cause communication issues with the flash chip. e.g. use external pullups on SPI/QUADSPI/OCTOSPI CS lines. // pgResetAll(); #ifdef TARGET_BUS_INIT -#error "CONFIG_IN_EXTERNAL_FLASH and TARGET_BUS_INIT are mutually exclusive" +#error "CONFIG_IN_EXTERNAL_FLASH/CONFIG_IN_MEMORY_MAPPED_FLASH and TARGET_BUS_INIT are mutually exclusive" #endif - configureSPIAndQuadSPI(); - initFlags |= SPI_AND_QSPI_INIT_ATTEMPTED; +#if defined(CONFIG_IN_EXTERNAL_FLASH) + configureSPIBusses(); + initFlags |= SPI_BUSSES_INIT_ATTEMPTED; +#endif +#if defined(CONFIG_IN_MEMORY_MAPPED_FLASH) || defined(CONFIG_IN_EXTERNAL_FLASH) + configureQuadSPIBusses(); + configureOctoSPIBusses(); + initFlags |= QUAD_OCTO_SPI_BUSSES_INIT_ATTEMPTED; +#endif #ifndef USE_FLASH_CHIP -#error "CONFIG_IN_EXTERNAL_FLASH requires USE_FLASH_CHIP to be defined." +#error "CONFIG_IN_EXTERNAL_FLASH/CONFIG_IN_MEMORY_MAPPED_FLASH requires USE_FLASH_CHIP to be defined." #endif bool haveFlash = flashInit(flashConfig()); @@ -386,7 +402,8 @@ void init(void) } initFlags |= FLASH_INIT_ATTEMPTED; -#endif // CONFIG_IN_EXTERNAL_FLASH +#endif // CONFIG_IN_EXTERNAL_FLASH || CONFIG_IN_MEMORY_MAPPED_FLASH + initEEPROM(); @@ -588,10 +605,16 @@ void init(void) #else - // Depending on compilation options SPI/QSPI initialisation may already be done. - if (!(initFlags & SPI_AND_QSPI_INIT_ATTEMPTED)) { - configureSPIAndQuadSPI(); - initFlags |= SPI_AND_QSPI_INIT_ATTEMPTED; + // Depending on compilation options SPI/QSPI/OSPI initialisation may already be done. + if (!(initFlags & SPI_BUSSES_INIT_ATTEMPTED)) { + configureSPIBusses(); + initFlags |= SPI_BUSSES_INIT_ATTEMPTED; + } + + if (!(initFlags & QUAD_OCTO_SPI_BUSSES_INIT_ATTEMPTED)) { + configureQuadSPIBusses(); + configureOctoSPIBusses(); + initFlags |= QUAD_OCTO_SPI_BUSSES_INIT_ATTEMPTED; } #if defined(USE_SDCARD_SDIO) && !defined(CONFIG_IN_SDCARD) && defined(STM32H7) diff --git a/src/main/pg/flash.c b/src/main/pg/flash.c index 1b3b8b4297b..e28b5707447 100644 --- a/src/main/pg/flash.c +++ b/src/main/pg/flash.c @@ -27,6 +27,7 @@ #include "drivers/bus_spi.h" #include "drivers/bus_quadspi.h" +#include "drivers/bus_octospi.h" #include "drivers/io.h" #include "pg/pg.h" @@ -42,12 +43,17 @@ PG_REGISTER_WITH_RESET_FN(flashConfig_t, flashConfig, PG_FLASH_CONFIG, 0); void pgResetFn_flashConfig(flashConfig_t *flashConfig) { + // CS pin can be used by all IO interfaces, not just SPI. flashConfig->csTag = IO_TAG(FLASH_CS_PIN); -#if defined(USE_SPI) && defined(FLASH_SPI_INSTANCE) + +#if defined(USE_FLASH_SPI) && defined(FLASH_SPI_INSTANCE) flashConfig->spiDevice = SPI_DEV_TO_CFG(spiDeviceByInstance(FLASH_SPI_INSTANCE)); #endif -#if defined(USE_QUADSPI) && defined(FLASH_QUADSPI_INSTANCE) +#if defined(USE_FLASH_QUADSPI) && defined(FLASH_QUADSPI_INSTANCE) flashConfig->quadSpiDevice = QUADSPI_DEV_TO_CFG(quadSpiDeviceByInstance(FLASH_QUADSPI_INSTANCE)); #endif +#if defined(USE_FLASH_OCTOSPI) && defined(FLASH_OCTOSPI_INSTANCE) + flashConfig->octoSpiDevice = OCTOSPI_DEV_TO_CFG(octoSpiDeviceByInstance(FLASH_OCTOSPI_INSTANCE)); +#endif } #endif diff --git a/src/main/pg/flash.h b/src/main/pg/flash.h index 20fc618b5a5..96b0bb76439 100644 --- a/src/main/pg/flash.h +++ b/src/main/pg/flash.h @@ -30,6 +30,7 @@ typedef struct flashConfig_s { ioTag_t csTag; uint8_t spiDevice; uint8_t quadSpiDevice; + uint8_t octoSpiDevice; } flashConfig_t; PG_DECLARE(flashConfig_t, flashConfig); diff --git a/src/main/startup/system_stm32h7xx.c b/src/main/startup/system_stm32h7xx.c index 890ed7ee578..c22fc2f19b2 100644 --- a/src/main/startup/system_stm32h7xx.c +++ b/src/main/startup/system_stm32h7xx.c @@ -735,6 +735,12 @@ void SystemInit (void) systemProcessResetReason(); #endif +#if defined(USE_FLASH_MEMORY_MAPPED) + memoryMappedModeInit(); + + // !IMPORTANT! Do NOT reset peripherals, clocks and GPIO pins used by the MCU to access the memory-mapped flash!!! +#endif + // FPU settings #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); // Set CP10 and CP11 Full Access diff --git a/src/main/target/STM32H723/target.h b/src/main/target/STM32H723/target.h index a60a97fd65d..3283a66969f 100644 --- a/src/main/target/STM32H723/target.h +++ b/src/main/target/STM32H723/target.h @@ -20,15 +20,35 @@ #pragma once +#ifndef TARGET_BOARD_IDENTIFIER #define TARGET_BOARD_IDENTIFIER "SH72" +#endif +#ifndef USBD_PRODUCT_STRING #define USBD_PRODUCT_STRING "Betaflight STM32H723" +#endif +#if !defined(USE_I2C) +#define USE_I2C #define USE_I2C_DEVICE_1 #define USE_I2C_DEVICE_2 #define USE_I2C_DEVICE_3 #define USE_I2C_DEVICE_4 #define USE_I2C_DEVICE_5 +#define I2C_FULL_RECONFIGURABILITY +#endif + +// Provide a default so that this target builds on the build server. +#if !defined(USE_SPI) +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 +#define USE_SPI_DEVICE_3 +#define USE_SPI_DEVICE_4 +#define USE_SPI_DEVICE_5 +#define USE_SPI_DEVICE_6 +#define SPI_FULL_RECONFIGURABILITY +#endif #define USE_UART1 #define USE_UART2 @@ -43,13 +63,6 @@ #define SERIAL_PORT_COUNT (UNIFIED_SERIAL_PORT_COUNT + 10) -#define USE_SPI_DEVICE_1 -#define USE_SPI_DEVICE_2 -#define USE_SPI_DEVICE_3 -#define USE_SPI_DEVICE_4 -#define USE_SPI_DEVICE_5 -#define USE_SPI_DEVICE_6 - #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff @@ -58,9 +71,6 @@ #define TARGET_IO_PORTF 0xffff #define TARGET_IO_PORTG 0xffff -#define USE_I2C -#define I2C_FULL_RECONFIGURABILITY - #define USE_BEEPER #ifdef USE_SDCARD @@ -68,8 +78,6 @@ #define USE_SDCARD_SDIO #endif -#define USE_SPI -#define SPI_FULL_RECONFIGURABILITY #define USE_VCP @@ -84,4 +92,6 @@ #define USE_ADC +#if !defined(USE_EXST) #define USE_CUSTOM_DEFAULTS +#endif diff --git a/src/main/target/STM32H723/target.mk b/src/main/target/STM32H723/target.mk index 0930806cbaf..5fb733e16d6 100644 --- a/src/main/target/STM32H723/target.mk +++ b/src/main/target/STM32H723/target.mk @@ -19,7 +19,9 @@ RX_SRC = \ H723xG_TARGETS += $(TARGET) +ifneq ($(EXST),yes) CUSTOM_DEFAULTS_EXTENDED = yes +endif FEATURES += VCP SDCARD_SPI SDCARD_SDIO ONBOARDFLASH diff --git a/src/main/target/STM32H730/target.h b/src/main/target/STM32H730/target.h index ca84647bb8f..23a1a74b0c6 100644 --- a/src/main/target/STM32H730/target.h +++ b/src/main/target/STM32H730/target.h @@ -74,7 +74,7 @@ #define USE_UART8 #define USE_UART9 #define USE_UART10 -#define USE_LP_UART1 +#define USE_LPUART1 #define SERIAL_PORT_COUNT (UNIFIED_SERIAL_PORT_COUNT + 11) diff --git a/src/main/target/STM32H750/target.h b/src/main/target/STM32H750/target.h index b8137f79cd3..2b40d8a37a9 100644 --- a/src/main/target/STM32H750/target.h +++ b/src/main/target/STM32H750/target.h @@ -84,9 +84,9 @@ #define USE_UART6 #define USE_UART7 #define USE_UART8 -#define USE_LP_UART1 +#define USE_LPUART1 -#define SERIAL_PORT_COUNT (UNIFIED_SERIAL_PORT_COUNT + 8) +#define SERIAL_PORT_COUNT (UNIFIED_SERIAL_PORT_COUNT + 9) #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 61049fccc2c..7ead61095c6 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -254,6 +254,18 @@ #define USE_FLASH_CHIP #endif +#if defined(USE_SPI) && (defined(USE_FLASH_M25P16) || defined(USE_FLASH_W25M512) || defined(USE_FLASH_W25N01G) || defined(USE_FLASH_W25M02G)) +#define USE_FLASH_SPI +#endif + +#if defined(USE_QUADSPI) && (defined(USE_FLASH_W25Q128FV) || defined(USE_FLASH_W25N01G)) +#define USE_FLASH_QUADSPI +#endif + +#if defined(USE_OCTOSPI) && (defined(USE_FLASH_W25Q128FV)) +#define USE_FLASH_OCTOSPI +#endif + #ifndef USE_FLASH_CHIP #undef USE_FLASHFS #endif @@ -426,7 +438,7 @@ #undef USE_ESCSERIAL #endif -#if defined(CONFIG_IN_RAM) || defined(CONFIG_IN_FILE) || defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_SDCARD) +#if defined(CONFIG_IN_RAM) || defined(CONFIG_IN_FILE) || defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_SDCARD) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH) #ifndef EEPROM_SIZE #define EEPROM_SIZE 4096 #endif @@ -445,6 +457,29 @@ extern uint8_t __config_end; #define USE_FLASH_BOOT_LOADER #endif +#if defined(USE_FLASH_MEMORY_MAPPED) +#if !defined(USE_RAM_CODE) +#define USE_RAM_CODE +#endif + +#define MMFLASH_CODE RAM_CODE +#define MMFLASH_CODE_NOINLINE RAM_CODE NOINLINE + +#define MMFLASH_DATA FAST_DATA +#define MMFLASH_DATA_ZERO_INIT FAST_DATA_ZERO_INIT +#else +#define MMFLASH_CODE +#define MMFLASH_CODE_NOINLINE +#define MMFLASH_DATA +#define MMFLASH_DATA_ZERO_INIT +#endif + +#ifdef USE_RAM_CODE +// RAM_CODE for methods that need to be in RAM, but don't need to be in the fastest type of memory. +// Note: if code is marked as RAM_CODE it *MUST* be in RAM, there is no alternative unlike functions marked with FAST_CODE/CCM_CODE +#define RAM_CODE __attribute__((section(".ram_code"))) +#endif + #if !defined(USE_RPM_FILTER) #undef USE_DYN_IDLE #endif diff --git a/support/scripts/build_nucleoh723zg_mmapped.sh b/support/scripts/build_nucleoh723zg_mmapped.sh new file mode 100644 index 00000000000..35cc3088b7b --- /dev/null +++ b/support/scripts/build_nucleoh723zg_mmapped.sh @@ -0,0 +1,108 @@ +#!/bin/sh +set -x + +make DEBUG=INFO EXST=yes EXST_ADJUST_VMA=0x90100000 TARGET=STM32H723 EXTRA_FLAGS="\ +-D'TARGET_BOARD_IDENTIFIER=\"H723\"' \ +-D'USBD_PRODUCT_STRING=\"Nucleo-H723-MMAPPED\"' \ +\ +-D'EEPROM_SIZE=8192' \ +\ +-DUSE_BUTTONS \ +-D'BUTTON_A_PIN=PC13' \ +-DBUTTON_A_PIN_INVERTED \ +-D'BUTTON_B_PIN=PC13' \ +-DBUTTON_B_PIN_INVERTED \ +\ +-DUSE_OCTOSPI \ +-DUSE_OCTOSPI_DEVICE_1 \ +-D'OCTOSPIM_P1_SCK_PIN=PB2' \ +-D'OCTOSPIM_P1_CS_PIN=PB6' \ +-D'OCTOSPIM_P1_IO0_PIN=PD11' \ +-D'OCTOSPIM_P1_IO1_PIN=PD12' \ +-D'OCTOSPIM_P1_IO2_PIN=PE2' \ +-D'OCTOSPIM_P1_IO3_PIN=PD13' \ +-D'OCTOSPIM_P1_IO4_PIN=NONE' \ +-D'OCTOSPIM_P1_IO5_PIN=NONE' \ +-D'OCTOSPIM_P1_IO6_PIN=NONE' \ +-D'OCTOSPIM_P1_IO7_PIN=NONE' \ +\ +-D'OCTOSPIM_P1_MODE=OCTOSPIM_P1_MODE_IO03_ONLY' \ +-D'OCTOSPIM_P1_CS_FLAGS=(OCTOSPIM_P1_CS_HARDWARE)' \ +\ +-DUSE_SPI \ +\ +-DUSE_SPI_DEVICE_1 \ +-D'SPI1_SCK_PIN=PB3' \ +-D'SPI1_MISO_PIN=PB4' \ +-D'SPI1_MOSI_PIN=PB5' \ +\ +-DUSE_SPI_DEVICE_2 \ +-D'SPI2_SCK_PIN=NONE' \ +-D'SPI2_MISO_PIN=NONE' \ +-D'SPI2_MOSI_PIN=NONE' \ +\ +-DUSE_SPI_DEVICE_3 \ +-D'SPI3_SCK_PIN=PC10' \ +-D'SPI3_MISO_PIN=PC11' \ +-D'SPI3_MOSI_PIN=PC12' \ +\ +-DUSE_SPI_DEVICE_4 \ +-D'SPI4_SCK_PIN=NONE' \ +-D'SPI4_MISO_PIN=NONE' \ +-D'SPI4_MOSI_PIN=NONE' \ +\ +-DUSE_SPI_DEVICE_5 \ +-D'SPI5_SCK_PIN=NONE' \ +-D'SPI5_MISO_PIN=NONE' \ +-D'SPI5_MOSI_PIN=NONE' \ +\ +-DUSE_SPI_DEVICE_6 \ +-D'SPI6_SCK_PIN=NONE' \ +-D'SPI6_MISO_PIN=NONE' \ +-D'SPI6_MOSI_PIN=NONE' \ +\ +-DUSE_FLASH_TOOLS \ +-DUSE_FLASH_W25Q128FV \ +-DUSE_FLASH_MEMORY_MAPPED \ +-DCONFIG_IN_MEMORY_MAPPED_FLASH \ +-D'FLASH_SPI_INSTANCE=NULL' \ +-D'FLASH_CS_PINS=NONE' \ +\ +-DUSE_SDCARD \ +-D'SDCARD_DETECT_PIN=NONE' \ +-D'SDIO_DEVICE=SDIODEV_2' \ +-D'SDIO_USE_4BIT=false' \ +-D'SDIO_CK_PIN=PD6' \ +-D'SDIO_CMD_PIN=PD7' \ +-D'SDIO_D0_PIN=PB14' \ +-D'SDIO_D1_PIN=NONE' \ +-D'SDIO_D2_PIN=NONE' \ +-D'SDIO_D3_PIN=NONE' \ +\ +-D'TARGET_IO_PORTA=0xffff' \ +-D'TARGET_IO_PORTB=0xffff' \ +-D'TARGET_IO_PORTC=0xffff' \ +-D'TARGET_IO_PORTD=0xffff' \ +-D'TARGET_IO_PORTE=0xffff' \ +-D'TARGET_IO_PORTF=0xffff' \ +-D'TARGET_IO_PORTG=0xffff' \ +\ +-DUSE_USB_ID \ +\ +-DUSE_I2C \ +-DUSE_I2C_DEVICE_1 \ +-D'I2C1_SCL=PB8' \ +-D'I2C1_SDA=PB9' \ +-D'I2C_DEVICE=(I2CDEV_1)' \ +\ +-DUSE_ACC \ +-DUSE_ACC_SPI_MPU6500 \ +-DUSE_GYRO \ +-DUSE_GYRO_SPI_MPU6500 \ +\ +-D'ADC1_DMA_OPT=8' \ +-D'ADC2_DMA_OPT=9' \ +-D'ADC3_DMA_OPT=10' \ +\ +-DENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT \ +" \ No newline at end of file diff --git a/support/scripts/build_spracingh7rf.sh b/support/scripts/build_spracingh7rf.sh new file mode 100644 index 00000000000..9f51863e542 --- /dev/null +++ b/support/scripts/build_spracingh7rf.sh @@ -0,0 +1,145 @@ +#!/bin/sh +set -x + +make DEBUG=INFO TARGET=STM32H730 EXTRA_FLAGS="\ +-D'TARGET_BOARD_IDENTIFIER=\"SP7R\"' \ +-D'USBD_PRODUCT_STRING=\"SPRacingH7RF\"' \ +\ +-D'EEPROM_SIZE=8192' \ +-DUSE_SPRACING_PERSISTENT_RTC_WORKAROUND \ +\ +-DUSE_BUTTONS \ +-D'BUTTON_A_PIN=PC14' \ +-DBUTTON_A_PIN_INVERTED \ +-D'BUTTON_B_PIN=PC14' \ +-DBUTTON_B_PIN_INVERTED \ +\ +-DUSE_OCTOSPI \ +-DUSE_OCTOSPI_DEVICE_1 \ +-D'OCTOSPIM_P1_SCK_PIN=PB2' \ +-D'OCTOSPIM_P1_CS_PIN=PB10' \ +-D'OCTOSPIM_P1_IO0_PIN=PD11' \ +-D'OCTOSPIM_P1_IO1_PIN=PD12' \ +-D'OCTOSPIM_P1_IO2_PIN=PE2' \ +-D'OCTOSPIM_P1_IO3_PIN=PD13' \ +-D'OCTOSPIM_P1_IO4_PIN=NONE' \ +-D'OCTOSPIM_P1_IO5_PIN=NONE' \ +-D'OCTOSPIM_P1_IO6_PIN=NONE' \ +-D'OCTOSPIM_P1_IO7_PIN=NONE' \ +-D'OCTOSPIM_P1_MODE=OCTOSPIM_P1_MODE_IO03_ONLY' \ +-D'OCTOSPIM_P1_CS_FLAGS=(OCTOSPIM_P1_CS_HARDWARE)' \ +\ +-DUSE_SPI \ +\ +-DUSE_SPI_DEVICE_2 \ +-D'SPI2_SCK_PIN=PD3' \ +-D'SPI2_MISO_PIN=PB14' \ +-D'SPI2_MOSI_PIN=PB15' \ +-D'SPI2_NSS_PIN=PB12' \ +\ +-DUSE_SPI_DEVICE_6 \ +-D'SPI6_SCK_PIN=PB3' \ +-D'SPI6_MISO_PIN=PB4' \ +-D'SPI6_MOSI_PIN=PB5' \ +-D'SPI6_NSS_PIN=PA15' \ +\ +-D'SX1280_BUSY_PIN=PC7' \ +-D'SX1280_DIO1_PIN=PC6' \ +-D'SX1280_DIO2_PIN=PD4' \ +-D'SX1280_DIO3_PIN=NONE' \ +-D'SX1280_NRESET_PIN=PD10' \ +-DUSE_RX_SPI \ +-DUSE_RX_EXPRESSLRS \ +-DUSE_RX_SX1280 \ +-D'RX_SPI_INSTANCE=SPI2' \ +-D'RX_NSS_PIN=SPI2_NSS_PIN' \ +-D'RX_SPI_EXTI_PIN=SX1280_DIO1_PIN' \ +-D'RX_EXPRESSLRS_SPI_RESET_PIN=SX1280_NRESET_PIN' \ +-D'RX_EXPRESSLRS_SPI_BUSY_PIN=SX1280_BUSY_PIN' \ +-D'RX_EXPRESSLRS_TIMER_INSTANCE=TIM6' \ +-D'DEFAULT_RX_FEATURE=FEATURE_RX_SPI' \ +-D'RX_SPI_DEFAULT_PROTOCOL=RX_SPI_EXPRESSLRS' \ +\ +-D'VTX_ENABLE_PIN=PC15' \ +-D'PINIO1_PIN=VTX_ENABLE_PIN' \ +\ +-DUSE_FLASH_MEMORY_MAPPED \ +-DUSE_FLASH_W25Q128FV \ +-D'FLASH_OCTOSPI_INSTANCE=OCTOSPI1' \ +-DCONFIG_IN_MEMORY_MAPPED_FLASH \ +-DUSE_FIRMWARE_PARTITION \ +\ +-D'SDCARD_DETECT_PIN=PC13' \ +-DSDCARD_DETECT_INVERTED \ +-D'SDIO_DEVICE=SDIODEV_1' \ +-D'SDIO_USE_4BIT=true' \ +-D'SDIO_CK_PIN=PC12' \ +-D'SDIO_CMD_PIN=PD2' \ +-D'SDIO_D0_PIN=PC8' \ +-D'SDIO_D1_PIN=PC9' \ +-D'SDIO_D2_PIN=PC10' \ +-D'SDIO_D3_PIN=PC11' \ +\ +-D'TARGET_IO_PORTA=0xffff' \ +-D'TARGET_IO_PORTB=(0xffff & ~(BIT(2)|BIT(6)))' \ +-D'TARGET_IO_PORTC=0xffff' \ +-D'TARGET_IO_PORTD=(0xffff & ~(BIT(11)|BIT(12)|BIT(13)))' \ +-D'TARGET_IO_PORTE=(0xffff & ~(BIT(2)|BIT(7)|BIT(8)|BIT(9)|BIT(10)))' \ +-D'TARGET_IO_PORTF=0xffff' \ +-D'TARGET_IO_PORTG=0xffff' \ +-D'TARGET_IO_PORTH=0xffff' \ +\ +-DUSE_I2C \ +-DUSE_I2C_DEVICE_1 \ +-D'I2C1_SCL=PB8' \ +-D'I2C1_SDA=PB9' \ +\ +-DUSE_I2C_DEVICE_2 \ +-D'I2C2_SCL=PB10' \ +-D'I2C2_SDA=PB11' \ +-D'MAG_I2C_INSTANCE=(I2CDEV_1)' \ +-D'BARO_I2C_INSTANCE=(I2CDEV_2)' \ +\ +-DUSE_ACC \ +-DUSE_GYRO \ +\ +-DUSE_MPU_DATA_READY_SIGNAL \ +-DENSURE_MPU_DATA_READY_IS_LOW \ +\ +-D'ADC3_DMA_OPT=10' \ +-D'ADC_INSTANCE=ADC3' \ +-D'CURRENT_METER_2_ADC_PIN=PC0' \ +-D'CURRENT_METER_2_ADC_INSTANCE=ADC3' \ +-D'CURRENT_METER_1_ADC_PIN=PC1' \ +-D'CURRENT_METER_1_ADC_INSTANCE=ADC3' \ +-D'EXTERNAL1_ADC_PIN=PC2' \ +-D'EXTERNAL1_ADC_INSTANCE=ADC3' \ +-D'VIDEO_IN_ADC_PIN=PC5' \ +-D'VIDEO_OUT_ADC_PIN=PC4' \ +-D'VBAT_ADC_PIN=PC3' \ +-D'VBAT_ADC_INSTANCE=ADC3' \ +-D'RSSI_ADC_PIN=CURRENT_METER_2_ADC_PIN' \ +-D'RSSI_ADC_INSTANCE=CURRENT_METER_2_ADC_INSTANCE' \ +-D'CURRENT_METER_ADC_PIN=CURRENT_METER_1_ADC_PIN' \ +-D'CURRENT_METER_ADC_INSTANCE=CURRENT_METER_1_ADC_INSTANCE' \ +-D'DEFAULT_VOLTAGE_METER_SOURCE=VOLTAGE_METER_ADC' \ +-D'DEFAULT_CURRENT_METER_SOURCE=CURRENT_METER_ADC' \ +\ +-DENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT \ +\ +-DUSE_SDCARD \ +-DUSE_ACC_SPI_ICM42605 \ +-DUSE_ACC_SPI_ICM42688P \ +-DUSE_GYRO_SPI_ICM42605 \ +-DUSE_GYRO_SPI_ICM42688P \ +-DUSE_FLASH_W25Q128FV \ +" + +# Settings that are currently defined in target/common_pre.h for non-cloud builds that probably shouldn't be. +# There are here to illustrate that they SHOULD be included in THIS target when they are removed by default. + +#-DUSE_MAG \ +#-DUSE_MAG_HMC5883 \ +#-DUSE_MAG_QMC5883 \ +#-DUSE_BARO \ +#-DUSE_BARO_BMP388 \