diff --git a/Tools/AP_Bootloader/bl_protocol.cpp b/Tools/AP_Bootloader/bl_protocol.cpp index 12b8cb3ba3..c0b7171a20 100644 --- a/Tools/AP_Bootloader/bl_protocol.cpp +++ b/Tools/AP_Bootloader/bl_protocol.cpp @@ -116,6 +116,8 @@ #define PROTO_EXTF_READ_MULTI 0x36 // read bytes at address and increment #define PROTO_EXTF_GET_CRC 0x37 // compute & return a CRC of data in external flash +#define PROTO_CHIP_FULL_ERASE 0x40 // erase program area and reset program address, skip any flash wear optimization and force an erase + #define PROTO_PROG_MULTI_MAX 64 // maximum PROG_MULTI size #define PROTO_READ_MULTI_MAX 255 // size of the size field @@ -597,6 +599,9 @@ bootloader(unsigned timeout) // erase failure: INSYNC/FAILURE // case PROTO_CHIP_ERASE: +#if defined(STM32F7) || defined(STM32H7) + case PROTO_CHIP_FULL_ERASE: +#endif if (!done_sync || !CHECK_GET_DEVICE_FINISHED(done_get_device_flags)) { // lower chance of random data on a uart triggering erase @@ -621,7 +626,11 @@ bootloader(unsigned timeout) // erase all sectors for (uint8_t i = 0; flash_func_sector_size(i) != 0; i++) { +#if defined(STM32F7) || defined(STM32H7) + if (!flash_func_erase_sector(i, c == PROTO_CHIP_FULL_ERASE)) { +#else if (!flash_func_erase_sector(i)) { +#endif goto cmd_fail; } } diff --git a/Tools/AP_Bootloader/support.cpp b/Tools/AP_Bootloader/support.cpp index 1b92efac55..5048a4a643 100644 --- a/Tools/AP_Bootloader/support.cpp +++ b/Tools/AP_Bootloader/support.cpp @@ -139,9 +139,9 @@ uint32_t flash_func_sector_size(uint32_t sector) return stm32_flash_getpagesize(flash_base_page+sector); } -bool flash_func_erase_sector(uint32_t sector) +bool flash_func_erase_sector(uint32_t sector, bool force_erase) { - if (!stm32_flash_ispageerased(flash_base_page+sector)) { + if (force_erase || !stm32_flash_ispageerased(flash_base_page+sector)) { return stm32_flash_erasepage(flash_base_page+sector); } return true; diff --git a/Tools/AP_Bootloader/support.h b/Tools/AP_Bootloader/support.h index cc2dd99c53..13f3085ba3 100644 --- a/Tools/AP_Bootloader/support.h +++ b/Tools/AP_Bootloader/support.h @@ -25,7 +25,7 @@ uint32_t flash_func_read_word(uint32_t offset); bool flash_func_write_word(uint32_t offset, uint32_t v); bool flash_func_write_words(uint32_t offset, uint32_t *v, uint8_t n); uint32_t flash_func_sector_size(uint32_t sector); -bool flash_func_erase_sector(uint32_t sector); +bool flash_func_erase_sector(uint32_t sector, bool force_erase = false); uint32_t flash_func_read_otp(uint32_t idx); uint32_t flash_func_read_sn(uint32_t idx); void flash_set_keep_unlocked(bool);