diff --git a/make/mcu/STM32H7.mk b/make/mcu/STM32H7.mk index 382720906f..9b146b8d1c 100644 --- a/make/mcu/STM32H7.mk +++ b/make/mcu/STM32H7.mk @@ -109,7 +109,7 @@ USBHID_SRC = $(notdir $(wildcard $(USBHID_DIR)/Src/*.c)) USBMSC_DIR = $(ROOT)/lib/main/STM32H7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC USBMSC_SRC = $(notdir $(wildcard $(USBMSC_DIR)/Src/*.c)) -EXCLUDES = usbd_msc_storage_template.c usbd_msc_scsi.c +EXCLUDES = usbd_msc_storage_template.c USBMSC_SRC := $(filter-out ${EXCLUDES}, $(USBMSC_SRC)) VPATH := $(VPATH):$(USBCDC_DIR)/Src:$(USBCORE_DIR)/Src:$(USBHID_DIR)/Src:$(USBMSC_DIR)/Src @@ -259,10 +259,10 @@ MCU_EXCLUDES = \ drivers/bus_i2c.c \ drivers/timer.c -#MSC_SRC = \ -# drivers/usb_msc_common.c \ -# drivers/usb_msc_h7xx.c \ -# msc/usbd_storage.c +MSC_SRC = \ + drivers/usb_msc_common.c \ + drivers/usb_msc_h7xx.c \ + msc/usbd_storage.c ifneq ($(filter SDCARD_SDIO,$(FEATURES)),) MCU_COMMON_SRC += \ @@ -271,17 +271,17 @@ MSC_SRC += \ msc/usbd_storage_sdio.c endif -#ifneq ($(filter SDCARD_SPI,$(FEATURES)),) -#MSC_SRC += \ -# msc/usbd_storage_sd_spi.c -#endif +ifneq ($(filter SDCARD_SPI,$(FEATURES)),) +MSC_SRC += \ + msc/usbd_storage_sd_spi.c +endif -#ifneq ($(filter ONBOARDFLASH,$(FEATURES)),) -#MSC_SRC += \ -# msc/usbd_storage_emfat.c \ -# msc/emfat.c \ -# msc/emfat_file.c -#endif +ifneq ($(filter ONBOARDFLASH,$(FEATURES)),) +MSC_SRC += \ + msc/usbd_storage_emfat.c \ + msc/emfat.c \ + msc/emfat_file.c +endif DSP_LIB := $(ROOT)/lib/main/CMSIS/DSP DEVICE_FLAGS += -DARM_MATH_MATRIX_CHECK -DARM_MATH_ROUNDING -D__FPU_PRESENT=1 -DUNALIGNED_SUPPORT_DISABLE -DARM_MATH_CM7 diff --git a/src/link/stm32_h750_common.ld b/src/link/stm32_h750_common.ld index 5c34f3e25f..667fed120a 100644 --- a/src/link/stm32_h750_common.ld +++ b/src/link/stm32_h750_common.ld @@ -111,7 +111,7 @@ SECTIONS . = ALIGN(4); _ebss = .; /* define a global symbol at bss end */ __bss_end__ = _ebss; - } >RAM + } >D2_RAM /* Uninitialized data section */ . = ALIGN(4); diff --git a/src/main/drivers/dshot_bitbang.c b/src/main/drivers/dshot_bitbang.c index c594c215eb..7986783d27 100644 --- a/src/main/drivers/dshot_bitbang.c +++ b/src/main/drivers/dshot_bitbang.c @@ -77,8 +77,8 @@ dshotBitbangStatus_e bbStatus; #define BB_OUTPUT_BUFFER_ATTRIBUTE FAST_DATA_ZERO_INIT #define BB_INPUT_BUFFER_ATTRIBUTE FAST_DATA_ZERO_INIT #elif defined(STM32H7) -#define BB_OUTPUT_BUFFER_ATTRIBUTE DMA_RAM -#define BB_INPUT_BUFFER_ATTRIBUTE DMA_RAM +#define BB_OUTPUT_BUFFER_ATTRIBUTE __attribute__((aligned(32))) +#define BB_INPUT_BUFFER_ATTRIBUTE __attribute__((aligned(32))) #elif defined(STM32G4) #define BB_OUTPUT_BUFFER_ATTRIBUTE DMA_RAM_W #define BB_INPUT_BUFFER_ATTRIBUTE DMA_RAM_R diff --git a/src/main/drivers/flash.c b/src/main/drivers/flash.c index 935b851ee2..135dfeb38b 100644 --- a/src/main/drivers/flash.c +++ b/src/main/drivers/flash.c @@ -402,6 +402,7 @@ const char *flashPartitionGetTypeName(flashPartitionType_e type) bool flashInit(const flashConfig_t *flashConfig) { memset(&flashPartitionTable, 0x00, sizeof(flashPartitionTable)); + flashPartitions = 0; bool haveFlash = flashDeviceInit(flashConfig); diff --git a/src/main/drivers/nvic.h b/src/main/drivers/nvic.h index f36a8faec5..410e10bdcf 100644 --- a/src/main/drivers/nvic.h +++ b/src/main/drivers/nvic.h @@ -65,6 +65,7 @@ #define NVIC_PRIO_MAG_DATA_READY NVIC_BUILD_PRIORITY(0x0f, 0x0f) #define NVIC_PRIO_CALLBACK NVIC_BUILD_PRIORITY(0x0f, 0x0f) #define NVIC_PRIO_MAX7456_DMA NVIC_BUILD_PRIORITY(3, 0) +#define NVIC_PRIO_SDIO_DMA NVIC_BUILD_PRIORITY(0, 0) #ifdef USE_HAL_DRIVER // utility macros to join/split priority diff --git a/src/main/drivers/persistent.c b/src/main/drivers/persistent.c index eee8fed590..b7998adcfc 100644 --- a/src/main/drivers/persistent.c +++ b/src/main/drivers/persistent.c @@ -47,6 +47,17 @@ void persistentObjectWrite(persistentObjectId_e id, uint32_t value) RTC_HandleTypeDef rtcHandle = { .Instance = RTC }; HAL_RTCEx_BKUPWrite(&rtcHandle, id, value); + +#ifdef USE_SPRACING_PERSISTENT_RTC_WORKAROUND + // Also write the persistent location used by the bootloader to support DFU etc. + if (id == PERSISTENT_OBJECT_RESET_REASON) { + // SPRACING firmware sometimes enters DFU mode when MSC mode is requested + if (value == RESET_MSC_REQUEST) { + value = RESET_NONE; + } + HAL_RTCEx_BKUPWrite(&rtcHandle, PERSISTENT_OBJECT_RESET_REASON_FWONLY, value); + } +#endif } void persistentObjectRTCEnable(void) diff --git a/src/main/drivers/persistent.h b/src/main/drivers/persistent.h index 656d8f7819..2b7ba80b18 100644 --- a/src/main/drivers/persistent.h +++ b/src/main/drivers/persistent.h @@ -29,10 +29,19 @@ typedef enum { PERSISTENT_OBJECT_MAGIC = 0, PERSISTENT_OBJECT_HSE_VALUE, PERSISTENT_OBJECT_OVERCLOCK_LEVEL, +#ifdef USE_SPRACING_PERSISTENT_RTC_WORKAROUND + // SPRACING H7 firmware always leaves this value reset so only use this location to invoke DFU + PERSISTENT_OBJECT_RESET_REASON_FWONLY, +#else PERSISTENT_OBJECT_RESET_REASON, +#endif PERSISTENT_OBJECT_RTC_HIGH, // high 32 bits of rtcTime_t PERSISTENT_OBJECT_RTC_LOW, // low 32 bits of rtcTime_t PERSISTENT_OBJECT_COUNT, +#ifdef USE_SPRACING_PERSISTENT_RTC_WORKAROUND + // On SPRACING H7 firmware use this alternate location for all reset reasons interpreted by this firmware + PERSISTENT_OBJECT_RESET_REASON, +#endif } persistentObjectId_e; // Values for PERSISTENT_OBJECT_RESET_REASON diff --git a/src/main/drivers/sdio_h7xx.c b/src/main/drivers/sdio_h7xx.c index 080e977ab1..026d505582 100644 --- a/src/main/drivers/sdio_h7xx.c +++ b/src/main/drivers/sdio_h7xx.c @@ -38,6 +38,7 @@ #include "drivers/io.h" #include "drivers/io_impl.h" +#include "drivers/nvic.h" #include "drivers/sdio.h" typedef struct SD_Handle_s @@ -50,7 +51,6 @@ typedef struct SD_Handle_s SD_HandleTypeDef hsd1; - SD_CardInfo_t SD_CardInfo; SD_CardType_t SD_CardType; @@ -192,7 +192,7 @@ void HAL_SD_MspInit(SD_HandleTypeDef* hsd) IOConfigGPIOAF(d3, IOCFG_SDMMC, sdioPin[SDIO_PIN_D3].af); } - HAL_NVIC_SetPriority(sdioHardware->irqn, 0, 0); + HAL_NVIC_SetPriority(sdioHardware->irqn, NVIC_PRIORITY_BASE(NVIC_PRIO_SDIO_DMA), NVIC_PRIORITY_SUB(NVIC_PRIO_SDIO_DMA)); HAL_NVIC_EnableIRQ(sdioHardware->irqn); } @@ -537,12 +537,12 @@ SD_Error_t SD_WriteBlocks_DMA(uint64_t WriteAddress, uint32_t *buffer, uint32_t return SD_ERROR; // unsupported. } - /* - the SCB_CleanDCache_by_Addr() requires a 32-Byte aligned address - adjust the address and the D-Cache size to clean accordingly. - */ - uint32_t alignedAddr = (uint32_t)buffer & ~0x1F; - SCB_CleanDCache_by_Addr((uint32_t*)alignedAddr, NumberOfBlocks * BlockSize + ((uint32_t)buffer - alignedAddr)); + if ((uint32_t)buffer & 0x1f) { + return SD_ADDR_MISALIGNED; + } + + // Ensure the data is flushed to main memory + SCB_CleanDCache_by_Addr(buffer, NumberOfBlocks * BlockSize); HAL_StatusTypeDef status; if ((status = HAL_SD_WriteBlocks_DMA(&hsd1, (uint8_t *)buffer, WriteAddress, NumberOfBlocks)) != HAL_OK) { @@ -568,13 +568,16 @@ SD_Error_t SD_ReadBlocks_DMA(uint64_t ReadAddress, uint32_t *buffer, uint32_t Bl return SD_ERROR; // unsupported. } + if ((uint32_t)buffer & 0x1f) { + return SD_ADDR_MISALIGNED; + } + SD_Handle.RXCplt = 1; sdReadParameters.buffer = buffer; sdReadParameters.BlockSize = BlockSize; sdReadParameters.NumberOfBlocks = NumberOfBlocks; - HAL_StatusTypeDef status; if ((status = HAL_SD_ReadBlocks_DMA(&hsd1, (uint8_t *)buffer, ReadAddress, NumberOfBlocks)) != HAL_OK) { return SD_ERROR; diff --git a/src/main/drivers/system_stm32h7xx.c b/src/main/drivers/system_stm32h7xx.c index 11e2f070f6..ec893a9be9 100644 --- a/src/main/drivers/system_stm32h7xx.c +++ b/src/main/drivers/system_stm32h7xx.c @@ -102,7 +102,7 @@ void systemReset(void) void forcedSystemResetWithoutDisablingCaches(void) { - persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_FORCED); + // Don't overwrite the PERSISTENT_OBJECT_RESET_REASON; just make another attempt __disable_irq(); NVIC_SystemReset(); diff --git a/src/main/drivers/usb_msc.h b/src/main/drivers/usb_msc.h index 4b5303dc20..b838aef5be 100644 --- a/src/main/drivers/usb_msc.h +++ b/src/main/drivers/usb_msc.h @@ -25,7 +25,7 @@ #pragma once void mscInit(void); -bool mscCheckBoot(void); +bool mscCheckBootAndReset(void); uint8_t mscStart(void); bool mscCheckButton(void); void mscWaitForButton(void); diff --git a/src/main/drivers/usb_msc_common.c b/src/main/drivers/usb_msc_common.c index ada2bdfb66..347ec159f7 100644 --- a/src/main/drivers/usb_msc_common.c +++ b/src/main/drivers/usb_msc_common.c @@ -66,13 +66,24 @@ void mscInit(void) } } -bool mscCheckBoot(void) +bool mscCheckBootAndReset(void) { - const uint32_t bootModeRequest = persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON); - return bootModeRequest == RESET_MSC_REQUEST; - // Note that we can't clear the persisent object after checking here. This is because - // this function is called multiple times during initialization. So we clear on a reset - // out of MSC mode. + static bool firstCheck = true; + static bool mscMode; + + if (firstCheck) { + // Cache the bootup value of RESET_MSC_REQUEST + const uint32_t bootModeRequest = persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON); + if (bootModeRequest == RESET_MSC_REQUEST) { + mscMode = true; + // Ensure the next reset is to the configurator as the H7 processor retains the RTC value so + // a brief interruption of power is not enough to switch out of MSC mode + persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE); + firstCheck = false; + } + } + + return mscMode; } void mscSetActive(void) diff --git a/src/main/drivers/usb_msc_h7xx.c b/src/main/drivers/usb_msc_h7xx.c new file mode 100644 index 0000000000..9fbf62c8ee --- /dev/null +++ b/src/main/drivers/usb_msc_h7xx.c @@ -0,0 +1,110 @@ +/* + * 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: Chris Hockuba (https://github.com/conkerkh) + * + */ + +#include +#include +#include + +#include "platform.h" + +#if defined(USE_USB_MSC) + +#include "build/build_config.h" + +#include "common/utils.h" + +#include "blackbox/blackbox.h" +#include "drivers/io.h" +#include "drivers/nvic.h" +#include "drivers/serial_usb_vcp.h" +#include "drivers/system.h" +#include "drivers/time.h" +#include "drivers/usb_msc.h" + +#include "msc/usbd_storage.h" + +#include "pg/sdcard.h" +#include "pg/usb.h" + +#include "vcp_hal/usbd_cdc_interface.h" + +#include "usb_io.h" +#include "usbd_msc.h" + +uint8_t mscStart(void) +{ + //Start USB + usbGenerateDisconnectPulse(); + + IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0); + IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0); + + USBD_Init(&USBD_Device, &VCP_Desc, 0); + + /** Regsiter class */ + USBD_RegisterClass(&USBD_Device, USBD_MSC_CLASS); + + /** Register interface callbacks */ + switch (blackboxConfig()->device) { +#ifdef USE_SDCARD + case BLACKBOX_DEVICE_SDCARD: + switch (sdcardConfig()->mode) { +#ifdef USE_SDCARD_SDIO + case SDCARD_MODE_SDIO: + USBD_MSC_RegisterStorage(&USBD_Device, &USBD_MSC_MICRO_SDIO_fops); + break; +#endif +#ifdef USE_SDCARD_SPI + case SDCARD_MODE_SPI: + USBD_MSC_RegisterStorage(&USBD_Device, &USBD_MSC_MICRO_SD_SPI_fops); + break; +#endif + default: + return 1; + } + break; +#endif + +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: + USBD_MSC_RegisterStorage(&USBD_Device, &USBD_MSC_EMFAT_fops); + break; +#endif + + default: + return 1; + } + + USBD_Start(&USBD_Device); + + // NVIC configuration for SYSTick + NVIC_DisableIRQ(SysTick_IRQn); + NVIC_SetPriority(SysTick_IRQn, NVIC_BUILD_PRIORITY(0, 0)); + NVIC_EnableIRQ(SysTick_IRQn); + + return 0; +} + +#endif diff --git a/src/main/fc/init.c b/src/main/fc/init.c index 54969febd9..6bc923ec83 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -337,7 +337,6 @@ void init(void) }; uint8_t initFlags = 0; - #ifdef CONFIG_IN_SDCARD // @@ -445,6 +444,16 @@ void init(void) systemState |= SYSTEM_STATE_CONFIG_LOADED; +#ifdef USE_SDCARD + // Ensure the SD card is initialised before the USB MSC starts to avoid a race condition +#if !defined(CONFIG_IN_SDCARD) && defined(STM32H7) && defined(USE_SDCARD_SDIO) // H7 only for now, likely should be applied to F4/F7 too + sdioPinConfigure(); + SDIO_GPIO_Init(); + initFlags |= SD_INIT_ATTEMPTED; + sdCardAndFSInit(); +#endif +#endif + #ifdef USE_BRUSHED_ESC_AUTODETECT // Now detect again with the actually configured pin for motor 1, if it is not the default pin. ioTag_t configuredMotorIoTag = motorConfig()->dev.ioTags[0]; @@ -635,7 +644,7 @@ void init(void) /* MSC mode will start after init, but will not allow scheduler to run, * so there is no bottleneck in reading and writing data */ mscInit(); - if (mscCheckBoot() || mscCheckButton()) { + if (mscCheckBootAndReset() || mscCheckButton()) { ledInit(statusLedConfig()); #if defined(USE_FLASHFS) @@ -686,14 +695,6 @@ void init(void) updateHardwareRevision(); #endif -#if defined(STM32H7) && defined(USE_SDCARD_SDIO) // H7 only for now, likely should be applied to F4/F7 too - if (!(initFlags & SD_INIT_ATTEMPTED)) { - sdioPinConfigure(); - SDIO_GPIO_Init(); - } -#endif - - #ifdef USE_VTX_RTC6705 bool useRTC6705 = rtc6705IOInit(vtxIOConfig()); #endif diff --git a/src/main/target/SPRACINGH7EXTREME/config.c b/src/main/target/SPRACINGH7EXTREME/config.c index 7d29627fb5..8f876eaa00 100644 --- a/src/main/target/SPRACINGH7EXTREME/config.c +++ b/src/main/target/SPRACINGH7EXTREME/config.c @@ -43,6 +43,5 @@ void targetConfiguration(void) osdConfigMutable()->core_temp_alarm = 85; targetSerialPortFunctionConfig(targetSerialPortFunction, ARRAYLEN(targetSerialPortFunction)); sdcardConfigMutable()->mode = SDCARD_MODE_SDIO; - sdcardConfigMutable()->useDma = true; } #endif diff --git a/src/main/target/SPRACINGH7EXTREME/target.h b/src/main/target/SPRACINGH7EXTREME/target.h index ae85c9c637..2e58e46da8 100644 --- a/src/main/target/SPRACINGH7EXTREME/target.h +++ b/src/main/target/SPRACINGH7EXTREME/target.h @@ -25,6 +25,8 @@ #define USE_TARGET_CONFIG +#define USE_SPRACING_PERSISTENT_RTC_WORKAROUND + #define LED0_PIN PE3 #define USE_BEEPER diff --git a/src/main/target/SPRACINGH7NANO/target.h b/src/main/target/SPRACINGH7NANO/target.h index 44a043f762..f8609350bd 100644 --- a/src/main/target/SPRACINGH7NANO/target.h +++ b/src/main/target/SPRACINGH7NANO/target.h @@ -25,6 +25,8 @@ #define USE_TARGET_CONFIG +#define USE_SPRACING_PERSISTENT_RTC_WORKAROUND + #define LED0_PIN PE3 #define USE_BEEPER diff --git a/src/main/target/SPRACINGH7ZERO/config.c b/src/main/target/SPRACINGH7ZERO/config.c index 7d29627fb5..8f876eaa00 100644 --- a/src/main/target/SPRACINGH7ZERO/config.c +++ b/src/main/target/SPRACINGH7ZERO/config.c @@ -43,6 +43,5 @@ void targetConfiguration(void) osdConfigMutable()->core_temp_alarm = 85; targetSerialPortFunctionConfig(targetSerialPortFunction, ARRAYLEN(targetSerialPortFunction)); sdcardConfigMutable()->mode = SDCARD_MODE_SDIO; - sdcardConfigMutable()->useDma = true; } #endif diff --git a/src/main/target/SPRACINGH7ZERO/target.h b/src/main/target/SPRACINGH7ZERO/target.h index 82fb49535d..e3a62d779a 100644 --- a/src/main/target/SPRACINGH7ZERO/target.h +++ b/src/main/target/SPRACINGH7ZERO/target.h @@ -25,6 +25,8 @@ #define USE_TARGET_CONFIG +#define USE_SPRACING_PERSISTENT_RTC_WORKAROUND + #define LED0_PIN PE3 #define USE_BEEPER diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index f91d6b6fb2..837e26eed1 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -48,7 +48,6 @@ #endif #ifdef STM32F4 -#define USE_SRAM2 #if defined(STM32F40_41xxx) #define USE_FAST_DATA #endif @@ -80,7 +79,6 @@ #endif // STM32F4 #ifdef STM32F7 -#define USE_SRAM2 #define USE_ITCM_RAM #define USE_FAST_DATA #define USE_DSHOT @@ -124,6 +122,9 @@ #define USE_TIMER_MGMT #define USE_PERSISTENT_OBJECTS #define USE_DMA_RAM +#define USE_USB_MSC +#define USE_RTC_TIME +#define USE_PERSISTENT_MSC_RTC #endif #ifdef STM32G4 diff --git a/src/main/vcp_hal/usbd_conf_stm32f7xx.c b/src/main/vcp_hal/usbd_conf_stm32f7xx.c index 77b079ee04..2a730c8e36 100644 --- a/src/main/vcp_hal/usbd_conf_stm32f7xx.c +++ b/src/main/vcp_hal/usbd_conf_stm32f7xx.c @@ -415,7 +415,7 @@ USBD_StatusTypeDef USBD_LL_Init(USBD_HandleTypeDef *pdev) #ifdef USE_USB_CDC_HID #ifdef USE_USB_MSC - if (usbDevConfig()->type == COMPOSITE && !mscCheckBoot()) { + if (usbDevConfig()->type == COMPOSITE && !mscCheckBootAndReset()) { #else if (usbDevConfig()->type == COMPOSITE) { #endif diff --git a/src/main/vcp_hal/usbd_conf_stm32h7xx.c b/src/main/vcp_hal/usbd_conf_stm32h7xx.c index 30d92d2a64..75cbec2dfb 100644 --- a/src/main/vcp_hal/usbd_conf_stm32h7xx.c +++ b/src/main/vcp_hal/usbd_conf_stm32h7xx.c @@ -398,7 +398,7 @@ USBD_StatusTypeDef USBD_LL_Init(USBD_HandleTypeDef * pdev) #ifdef USE_USB_CDC_HID #ifdef USE_USB_MSC - if (usbDevConfig()->type == COMPOSITE && !mscCheckBoot()) { + if (usbDevConfig()->type == COMPOSITE && !mscCheckBootAndReset()) { #else if (usbDevConfig()->type == COMPOSITE) { #endif diff --git a/src/main/vcp_hal/usbd_desc.c b/src/main/vcp_hal/usbd_desc.c index bd0a94ba3a..b38b8f8a5b 100644 --- a/src/main/vcp_hal/usbd_desc.c +++ b/src/main/vcp_hal/usbd_desc.c @@ -187,7 +187,7 @@ uint8_t *USBD_VCP_DeviceDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { (void)speed; #ifdef USE_USB_MSC - if (mscCheckBoot()) { + if (mscCheckBootAndReset()) { *length = sizeof(USBD_MSC_DeviceDesc); return USBD_MSC_DeviceDesc; }