diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index f4e545cd4e..913442520c 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -274,6 +274,11 @@ typedef enum dumpFlags_e { typedef bool printFn(dumpFlags_t dumpMask, bool equalsDefault, const char *format, ...); +typedef enum { + REBOOT_TARGET_FIRMWARE, + REBOOT_TARGET_BOOTLOADER_ROM, + REBOOT_TARGET_BOOTLOADER_FLASH, +} rebootTarget_e; static void backupPgConfig(const pgRegistry_t *pg) { @@ -3309,31 +3314,61 @@ static char *checkCommand(char *cmdline, const char *command) } } -static void cliRebootEx(bool bootLoader) +static void cliRebootEx(rebootTarget_e rebootTarget) { cliPrint("\r\nRebooting"); bufWriterFlush(cliWriter); waitForSerialPortToFinishTransmitting(cliPort); stopPwmAllMotors(); - if (bootLoader) { - systemResetToBootloader(); - return; + switch (rebootTarget) { + case REBOOT_TARGET_BOOTLOADER_ROM: + systemResetToBootloader(BOOTLOADER_REQUEST_ROM); + + break; +#if defined(USE_FLASH_BOOT_LOADER) + case REBOOT_TARGET_BOOTLOADER_FLASH: + systemResetToBootloader(BOATLOADER_REQUEST_FLASH); + + break; +#endif + case REBOOT_TARGET_FIRMWARE: + default: + systemReset(); + + break; } - systemReset(); } static void cliReboot(void) { - cliRebootEx(false); + cliRebootEx(REBOOT_TARGET_FIRMWARE); } static void cliBootloader(char *cmdline) { - UNUSED(cmdline); + rebootTarget_e rebootTarget; + if ( +#if !defined(USE_FLASH_BOOT_LOADER) + isEmpty(cmdline) || +#endif + strncasecmp(cmdline, "rom", 3) == 0) { + rebootTarget = REBOOT_TARGET_BOOTLOADER_ROM; - cliPrintHashLine("restarting in bootloader mode"); - cliRebootEx(true); + cliPrintHashLine("restarting in ROM bootloader mode"); +#if defined(USE_FLASH_BOOT_LOADER) + } else if (isEmpty(cmdline) || strncasecmp(cmdline, "flash", 5) == 0) { + rebootTarget = REBOOT_TARGET_BOOTLOADER_FLASH; + + cliPrintHashLine("restarting in flash bootloader mode"); +#endif + } else { + cliPrintErrorLinef("Invalid option"); + + return; + } + + cliRebootEx(rebootTarget); } static void cliExit(char *cmdline) @@ -5873,7 +5908,11 @@ const clicmd_t cmdTable[] = { #ifdef USE_RX_SPI CLI_COMMAND_DEF("bind_rx_spi", "initiate binding for RX SPI", NULL, cliRxSpiBind), #endif - CLI_COMMAND_DEF("bl", "reboot into bootloader", NULL, cliBootloader), +#if defined(USE_FLASH_BOOT_LOADER) + CLI_COMMAND_DEF("bl", "reboot into bootloader", "[flash|rom]", cliBootloader), +#else + CLI_COMMAND_DEF("bl", "reboot into bootloader", "[rom]", cliBootloader), +#endif #if defined(USE_BOARD_INFO) CLI_COMMAND_DEF("board_name", "get / set the name of the board model", "[board name]", cliBoardName), #endif diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index ac3bbc19db..c8cc02ee98 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -202,7 +202,7 @@ void failureMode(failureMode_e mode) #ifdef DEBUG systemReset(); #else - systemResetToBootloader(); + systemResetToBootloader(BOOTLOADER_REQUEST_ROM); #endif } diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 4aa17bec99..b98c6c647c 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -50,16 +50,23 @@ typedef enum { #define WARNING_CODE_DURATION_LONG_MS 250 #define WARNING_CODE_DURATION_SHORT_MS 50 +typedef enum { + BOOTLOADER_REQUEST_ROM, + BOATLOADER_REQUEST_FLASH, +} bootloaderRequestType_e; + // failure void indicateFailure(failureMode_e mode, int repeatCount); void failureMode(failureMode_e mode); // bootloader/IAP void systemReset(void); -void systemResetToBootloader(void); -void checkForBootLoaderRequest(void); +void systemResetToBootloader(bootloaderRequestType_e requestType); bool isMPUSoftReset(void); void cycleCounterInit(void); +#if defined(STM32H7) +void systemCheckResetReason(void); +#endif void initialiseMemorySections(void); diff --git a/src/main/drivers/system_stm32f10x.c b/src/main/drivers/system_stm32f10x.c index 2b52cd33ad..8bc4b506b5 100644 --- a/src/main/drivers/system_stm32f10x.c +++ b/src/main/drivers/system_stm32f10x.c @@ -114,7 +114,7 @@ void systemInit(void) SysTick_Config(SystemCoreClock / 1000); } -void checkForBootLoaderRequest(void) +static void checkForBootLoaderRequest(void) { void(*bootJump)(void); diff --git a/src/main/drivers/system_stm32f30x.c b/src/main/drivers/system_stm32f30x.c index e80b024d1a..759d81d024 100644 --- a/src/main/drivers/system_stm32f30x.c +++ b/src/main/drivers/system_stm32f30x.c @@ -35,8 +35,9 @@ void systemReset(void) SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04; } -void systemResetToBootloader(void) +void systemResetToBootloader(bootloaderRequestType_e requestType) { + UNUSED(requestType); // 1FFFF000 -> 20000200 -> SP // 1FFFF004 -> 1FFFF021 -> PC @@ -83,6 +84,23 @@ bool isMPUSoftReset(void) return false; } +static void checkForBootLoaderRequest(void) +{ + void(*bootJump)(void); + + if (*((uint32_t *)0x20009FFC) == 0xDEADBEEF) { + + *((uint32_t *)0x20009FFC) = 0x0; + + __enable_irq(); + __set_MSP(*((uint32_t *)0x1FFFD800)); + + bootJump = (void(*)(void))(*((uint32_t *) 0x1FFFD804)); + bootJump(); + while (1); + } +} + void systemInit(void) { checkForBootLoaderRequest(); @@ -106,20 +124,3 @@ void systemInit(void) // SysTick SysTick_Config(SystemCoreClock / 1000); } - -void checkForBootLoaderRequest(void) -{ - void(*bootJump)(void); - - if (*((uint32_t *)0x20009FFC) == 0xDEADBEEF) { - - *((uint32_t *)0x20009FFC) = 0x0; - - __enable_irq(); - __set_MSP(*((uint32_t *)0x1FFFD800)); - - bootJump = (void(*)(void))(*((uint32_t *) 0x1FFFD804)); - bootJump(); - while (1); - } -} diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index 0cbf60662e..1271898b92 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -39,9 +39,15 @@ void systemReset(void) NVIC_SystemReset(); } -void systemResetToBootloader(void) +void systemResetToBootloader(bootloaderRequestType_e requestType) { - persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_BOOTLOADER_REQUEST); + switch (requestType) { + case BOOTLOADER_REQUEST_ROM: + default: + persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_BOOTLOADER_REQUEST); + + break; + } __disable_irq(); NVIC_SystemReset(); @@ -54,6 +60,7 @@ typedef struct isrVector_s { resetHandler_t *resetHandler; } isrVector_t; +// Used in the startup files for F4 void checkForBootLoaderRequest(void) { uint32_t bootloaderRequest = persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON); diff --git a/src/main/drivers/system_stm32f7xx.c b/src/main/drivers/system_stm32f7xx.c index c519c63a96..47f8668a44 100644 --- a/src/main/drivers/system_stm32f7xx.c +++ b/src/main/drivers/system_stm32f7xx.c @@ -43,9 +43,16 @@ void systemReset(void) NVIC_SystemReset(); } -void systemResetToBootloader(void) +void systemResetToBootloader(bootloaderRequestType_e requestType) { - persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_BOOTLOADER_REQUEST); + switch (requestType) { + case BOOTLOADER_REQUEST_ROM: + default: + persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_BOOTLOADER_REQUEST); + + break; + } + __disable_irq(); NVIC_SystemReset(); } @@ -152,6 +159,30 @@ bool isMPUSoftReset(void) return false; } +static void checkForBootLoaderRequest(void) +{ + uint32_t bootloaderRequest = persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON); + + if (bootloaderRequest != RESET_BOOTLOADER_REQUEST) { + return; + } + persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE); + + void (*SysMemBootJump)(void); + + __SYSCFG_CLK_ENABLE(); + SYSCFG->MEMRMP |= SYSCFG_MEM_BOOT_ADD0 ; + + uint32_t p = (*((uint32_t *) 0x1ff00000)); + + __set_MSP(p); //Set the main stack pointer to its default values + + SysMemBootJump = (void (*)(void)) (*((uint32_t *) 0x1ff00004)); // Point the PC to the System Memory reset vector (+4) + SysMemBootJump(); + + while (1); +} + void systemInit(void) { checkForBootLoaderRequest(); @@ -186,29 +217,3 @@ void systemInit(void) HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK); } - -void(*bootJump)(void); - -void checkForBootLoaderRequest(void) -{ - uint32_t bootloaderRequest = persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON); - - if (bootloaderRequest != RESET_BOOTLOADER_REQUEST) { - return; - } - persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE); - - void (*SysMemBootJump)(void); - - __SYSCFG_CLK_ENABLE(); - SYSCFG->MEMRMP |= SYSCFG_MEM_BOOT_ADD0 ; - - uint32_t p = (*((uint32_t *) 0x1ff00000)); - - __set_MSP(p); //Set the main stack pointer to its default values - - SysMemBootJump = (void (*)(void)) (*((uint32_t *) 0x1ff00004)); // Point the PC to the System Memory reset vector (+4) - SysMemBootJump(); - - while (1); -} diff --git a/src/main/drivers/system_stm32h7xx.c b/src/main/drivers/system_stm32h7xx.c index 58ced013cd..9ded27d7c8 100644 --- a/src/main/drivers/system_stm32h7xx.c +++ b/src/main/drivers/system_stm32h7xx.c @@ -180,7 +180,7 @@ void systemInit(void) // Mark ITCM-RAM as read-only HAL_MPU_Disable(); - // "For Cortex®-M7, TCMs memories always behave as Non-cacheable, Non-shared normal memories, irrespectiveof the memory type attributes defined in the MPU for a memory region containing addresses held in the TCM" + // "For Cortex®-M7, TCMs memories always behave as Non-cacheable, Non-shared normal memories, irrespective of the memory type attributes defined in the MPU for a memory region containing addresses held in the TCM" // See AN4838 MPU_Region_InitTypeDef MPU_InitStruct; @@ -230,14 +230,6 @@ void systemInit(void) void systemReset(void) { -#if 0 -#ifdef USE_GYRO - if (mpuResetFn) { - mpuResetFn(); - } -#endif -#endif - SCB_DisableDCache(); SCB_DisableICache(); @@ -253,36 +245,36 @@ void forcedSystemResetWithoutDisablingCaches(void) NVIC_SystemReset(); } -void systemResetToBootloader(void) +void systemResetToBootloader(bootloaderRequestType_e requestType) { -#if 0 -#ifdef USE_GYRO - if (mpuResetFn) { - mpuResetFn(); + switch (requestType) { +#if defined(USE_FLASH_BOOT_LOADER) + case BOATLOADER_REQUEST_FLASH: + persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_FLASH_BOOTLOADER_REQUEST); + + break; +#endif + case BOOTLOADER_REQUEST_ROM: + default: + persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_BOOTLOADER_REQUEST); + + break; } -#endif -#endif -#ifdef USE_EXST - persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_FLASH_BOOTLOADER_REQUEST); -#else - persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_BOOTLOADER_REQUEST); -#endif + __disable_irq(); NVIC_SystemReset(); } static uint32_t bootloaderRequest; -bool systemIsFlashBootloaderRequested(void) -{ - return (bootloaderRequest == RESET_FLASH_BOOTLOADER_REQUEST); -} - void systemCheckResetReason(void) { bootloaderRequest = persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON); switch (bootloaderRequest) { +#if defined(USE_FLASH_BOOT_LOADER) + case BOATLOADER_REQUEST_FLASH: +#endif case RESET_BOOTLOADER_REQUEST: persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_BOOTLOADER_POST); break; diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 5678e37f5e..eb4c1d19c5 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -140,9 +140,10 @@ static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // enum { MSP_REBOOT_FIRMWARE = 0, - MSP_REBOOT_BOOTLOADER, + MSP_REBOOT_BOOTLOADER_ROM, MSP_REBOOT_MSC, MSP_REBOOT_MSC_UTC, + MSP_REBOOT_BOOTLOADER_FLASH, MSP_REBOOT_COUNT, }; @@ -263,8 +264,8 @@ static void mspRebootFn(serialPort_t *serialPort) systemReset(); break; - case MSP_REBOOT_BOOTLOADER: - systemResetToBootloader(); + case MSP_REBOOT_BOOTLOADER_ROM: + systemResetToBootloader(BOOTLOADER_REQUEST_ROM); break; #if defined(USE_USB_MSC) @@ -279,6 +280,12 @@ static void mspRebootFn(serialPort_t *serialPort) } break; #endif +#if defined(USE_FLASH_BOOT_LOADER) + case MSP_REBOOT_BOOTLOADER_FLASH: + systemResetToBootloader(BOOTLOADER_REQUEST_FLASH); + + break; +#endif default: return; diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index 4c5558c409..75ce5a5b40 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -425,16 +425,16 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr static void mspEvaluateNonMspData(mspPort_t * mspPort, uint8_t receivedChar) { + if (receivedChar == serialConfig()->reboot_character) { + mspPort->pendingRequest = MSP_PENDING_BOOTLOADER_ROM; #ifdef USE_CLI - if (receivedChar == '#') { + } else if (receivedChar == '#') { mspPort->pendingRequest = MSP_PENDING_CLI; - return; - } #endif - - if (receivedChar == serialConfig()->reboot_character) { - mspPort->pendingRequest = MSP_PENDING_BOOTLOADER; - return; +#if defined(USE_FLASH_BOOT_LOADER) + } else if (receivedChar == 'F') { + mspPort->pendingRequest = MSP_PENDING_BOOTLOADER_FLASH; +#endif } } @@ -446,18 +446,24 @@ static void mspProcessPendingRequest(mspPort_t * mspPort) } switch(mspPort->pendingRequest) { - case MSP_PENDING_BOOTLOADER: - systemResetToBootloader(); - break; + case MSP_PENDING_BOOTLOADER_ROM: + systemResetToBootloader(BOOTLOADER_REQUEST_ROM); + break; +#if defined(USE_FLASH_BOOT_LOADER) + case MSP_PENDING_BOOTLOADER_ROM: + systemResetToBootloader(BOATLOADER_REQUEST_FLASH); + + break; +#endif #ifdef USE_CLI - case MSP_PENDING_CLI: - cliEnter(mspPort->port); - break; + case MSP_PENDING_CLI: + cliEnter(mspPort->port); + break; #endif - default: - break; + default: + break; } } diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index 4aa9124ee4..d4adb04e52 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -59,8 +59,9 @@ typedef enum { typedef enum { MSP_PENDING_NONE, - MSP_PENDING_BOOTLOADER, - MSP_PENDING_CLI + MSP_PENDING_BOOTLOADER_ROM, + MSP_PENDING_CLI, + MSP_PENDING_BOOTLOADER_FLASH, } mspPendingSystemRequest_e; #define MSP_PORT_INBUF_SIZE 192 diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index b353321213..917d14736b 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -252,7 +252,9 @@ void systemReset(void){ pthread_join(udpWorker, NULL); exit(0); } -void systemResetToBootloader(void) { +void systemResetToBootloader(bootloaderRequestType_e requestType) { + UNUSED(requestType); + printf("[system]ResetToBootloader!\n"); workerRunning = false; pthread_join(tcpWorker, NULL); diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index dd4823f373..64be12f292 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -352,3 +352,7 @@ extern uint8_t eepromData[EEPROM_SIZE]; extern uint8_t __config_start; // configured via linker script when building binaries. extern uint8_t __config_end; #endif + +#if defined(USE_EXST) +#define USE_FLASH_BOOT_LOADER +#endif diff --git a/src/main/target/system_stm32h7xx.c b/src/main/target/system_stm32h7xx.c index 731ba2ce92..c07d7a241b 100644 --- a/src/main/target/system_stm32h7xx.c +++ b/src/main/target/system_stm32h7xx.c @@ -572,8 +572,6 @@ void MPU_Config() * @param None * @retval None */ -void systemCheckResetReason(void); - void resetMPU(void) { MPU_Region_InitTypeDef MPU_InitStruct;