From 21e550fdba7a6af3990ef308851db0222fd25eb1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Feb 2024 11:52:08 +1300 Subject: [PATCH] tools/bootloader: add force-erase option If the STM32H7 fails to program or erase a full chunk of 256 bytes, the ECC check will trigger a busfault when trying to read from it. To speed up erasing and optimize wear, we read before erasing to check if it actually needs erasing. That's when a busfault happens and the erase time outs. The workaround is to add an option to do a full erase without check. Credit goes to: https://github.com/ArduPilot/ardupilot/pull/22090 And the protocol option added to the bootloader is the same as for ArduPilot, so compatible. Signed-off-by: Julian Oes --- Tools/px_uploader.py | 25 ++++++++--- platforms/nuttx/src/bootloader/common/bl.c | 12 +++++- platforms/nuttx/src/bootloader/common/bl.h | 4 +- .../src/bootloader/nxp/imxrt_common/main.c | 42 +++++++++---------- .../src/bootloader/stm/stm32_common/main.c | 9 +--- 5 files changed, 56 insertions(+), 36 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index d479be314837..6328b914990d 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -195,6 +195,7 @@ class uploader(object): GET_CHIP = b'\x2c' # rev5+ , get chip version SET_BOOT_DELAY = b'\x2d' # rev5+ , set boot delay GET_CHIP_DES = b'\x2e' # rev5+ , get chip description in ASCII + CHIP_FULL_ERASE = b'\x40' # full erase of flash, rev6+ MAX_DES_LENGTH = 20 REBOOT = b'\x30' @@ -235,6 +236,7 @@ def __init__(self, portname, baudrate_bootloader, baudrate_flightstack): self.baudrate_bootloader = baudrate_bootloader self.baudrate_flightstack = baudrate_flightstack self.baudrate_flightstack_idx = -1 + self.force_erase = False def close(self): if self.port is not None: @@ -423,8 +425,14 @@ def __drawProgressBar(self, label, progress, maxVal): def __erase(self, label): print("Windowed mode: %s" % self.ackWindowedMode) print("\n", end='') - self.__send(uploader.CHIP_ERASE + - uploader.EOC) + + if self.force_erase: + print("Trying force erase of full chip...\n") + self.__send(uploader.CHIP_FULL_ERASE + + uploader.EOC) + else: + self.__send(uploader.CHIP_ERASE + + uploader.EOC) # erase is very slow, give it 30s deadline = _time() + 30.0 @@ -441,9 +449,14 @@ def __erase(self, label): if self.__trySync(): self.__drawProgressBar(label, 10.0, 10.0) + if self.force_erase: + print("\nForce erase done.\n") return - raise RuntimeError("timed out waiting for erase") + if self.force_erase: + raise RuntimeError("timed out waiting for erase, force erase is likely not supported by bootloader!") + else: + raise RuntimeError("timed out waiting for erase") # send a PROG_MULTI command to write a collection of bytes def __program_multi(self, data, windowMode): @@ -582,7 +595,8 @@ def identify(self): self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE) # upload the firmware - def upload(self, fw_list, force=False, boot_delay=None, boot_check=False): + def upload(self, fw_list, force=False, boot_delay=None, boot_check=False, force_erase=False): + self.force_erase = force_erase # select correct binary found_suitable_firmware = False for file in fw_list: @@ -790,6 +804,7 @@ def main(): parser.add_argument('--baud-bootloader', action="store", type=int, default=115200, help="Baud rate of the serial port (default is 115200) when communicating with bootloader, only required for true serial ports.") parser.add_argument('--baud-flightstack', action="store", default="57600", help="Comma-separated list of baud rate of the serial port (default is 57600) when communicating with flight stack (Mavlink or NSH), only required for true serial ports.") parser.add_argument('--force', action='store_true', default=False, help='Override board type check, or silicon errata checks and continue loading') + parser.add_argument('--force-erase', action="store_true", help="Do not perform the blank check, always erase every sector of the application space") parser.add_argument('--boot-delay', type=int, default=None, help='minimum boot delay to store in flash') parser.add_argument('--use-protocol-splitter-format', action='store_true', help='use protocol splitter format for reboot') parser.add_argument('firmware', action="store", nargs='+', help="Firmware file(s)") @@ -907,7 +922,7 @@ def main(): try: # ok, we have a bootloader, try flashing it - up.upload(args.firmware, force=args.force, boot_delay=args.boot_delay) + up.upload(args.firmware, force=args.force, boot_delay=args.boot_delay, force_erase=args.force_erase) # if we made this far without raising exceptions, the upload was successful successful = True diff --git a/platforms/nuttx/src/bootloader/common/bl.c b/platforms/nuttx/src/bootloader/common/bl.c index 61f3881d6f9e..e1dcfe8c5b8f 100644 --- a/platforms/nuttx/src/bootloader/common/bl.c +++ b/platforms/nuttx/src/bootloader/common/bl.c @@ -114,6 +114,7 @@ #define PROTO_RESERVED_0X37 0x37 // Reserved #define PROTO_RESERVED_0X38 0x38 // Reserved #define PROTO_RESERVED_0X39 0x39 // Reserved +#define PROTO_CHIP_FULL_ERASE 0x40 // Full erase, without any flash wear optimization #define PROTO_PROG_MULTI_MAX 64 // maximum PROG_MULTI size #define PROTO_READ_MULTI_MAX 255 // size of the size field @@ -649,6 +650,8 @@ bootloader(unsigned timeout) led_on(LED_ACTIVITY); + bool full_erase = false; + // handle the command byte switch (c) { @@ -728,6 +731,10 @@ bootloader(unsigned timeout) // success reply: INSYNC/OK // erase failure: INSYNC/FAILURE // + case PROTO_CHIP_FULL_ERASE: + full_erase = true; + + // Fallthrough case PROTO_CHIP_ERASE: /* expect EOC */ @@ -755,17 +762,18 @@ bootloader(unsigned timeout) arch_flash_unlock(); for (int i = 0; flash_func_sector_size(i) != 0; i++) { - flash_func_erase_sector(i); + flash_func_erase_sector(i, full_erase); } // disable the LED while verifying the erase led_set(LED_OFF); // verify the erase - for (address = 0; address < board_info.fw_size; address += 4) + for (address = 0; address < board_info.fw_size; address += 4) { if (flash_func_read_word(address) != 0xffffffff) { goto cmd_fail; } + } address = 0; SET_BL_STATE(STATE_PROTO_CHIP_ERASE); diff --git a/platforms/nuttx/src/bootloader/common/bl.h b/platforms/nuttx/src/bootloader/common/bl.h index 4115200db3bb..a39e7ce9086f 100644 --- a/platforms/nuttx/src/bootloader/common/bl.h +++ b/platforms/nuttx/src/bootloader/common/bl.h @@ -39,6 +39,8 @@ #pragma once +#include + /***************************************************************************** * Generic bootloader functions. */ @@ -105,7 +107,7 @@ extern void board_deinit(void); extern uint32_t board_get_devices(void); extern void clock_deinit(void); extern uint32_t flash_func_sector_size(unsigned sector); -extern void flash_func_erase_sector(unsigned sector); +extern void flash_func_erase_sector(unsigned sector, bool force); extern void flash_func_write_word(uintptr_t address, uint32_t word); extern uint32_t flash_func_read_word(uintptr_t address); extern uint32_t flash_func_read_otp(uintptr_t address); diff --git a/platforms/nuttx/src/bootloader/nxp/imxrt_common/main.c b/platforms/nuttx/src/bootloader/nxp/imxrt_common/main.c index 1197427a942c..fd0578f885c0 100644 --- a/platforms/nuttx/src/bootloader/nxp/imxrt_common/main.c +++ b/platforms/nuttx/src/bootloader/nxp/imxrt_common/main.c @@ -395,6 +395,24 @@ flash_func_sector_size(unsigned sector) return 0; } +/* imxRT uses Flash lib, not up_progmem so let's stub it here */ +up_progmem_ispageerased(unsigned sector) +{ + const uint32_t bytes_per_sector = flash_func_sector_size(sector); + uint32_t *address = (uint32_t *)(IMXRT_FLEXSPI1_CIPHER_BASE + (sector * bytes_per_sector)); + const uint32_t uint32_per_sector = bytes_per_sector / sizeof(*address); + + int blank = 0; /* Assume it is Bank */ + + for (uint32_t i = 0; i < uint32_per_sector; i++) { + if (address[i] != 0xffffffff) { + blank = -1; /* It is not blank */ + break; + } + } + + return blank; +} /*! * @name Configuration Option @@ -407,31 +425,15 @@ flash_func_sector_size(unsigned sector) * */ locate_code(".ramfunc") void -flash_func_erase_sector(unsigned sector) +flash_func_erase_sector(unsigned sector, bool force) { - if (sector > BOARD_FLASH_SECTORS || (int)sector < BOARD_FIRST_FLASH_SECTOR_TO_ERASE) { return; } - /* blank-check the sector */ - const uint32_t bytes_per_sector = flash_func_sector_size(sector); - uint32_t *address = (uint32_t *)(IMXRT_FLEXSPI1_CIPHER_BASE + (sector * bytes_per_sector)); - const uint32_t uint32_per_sector = bytes_per_sector / sizeof(*address); - bool blank = true; - - for (uint32_t i = 0; i < uint32_per_sector; i++) { - if (address[i] != 0xffffffff) { - blank = false; - break; - } - } + if (force || flash_func_is_sector_blank(sector) != 0) { + struct flexspi_nor_config_s *pConfig = &g_bootConfig; - - struct flexspi_nor_config_s *pConfig = &g_bootConfig; - - /* erase the sector if it failed the blank check */ - if (!blank) { uintptr_t offset = ((uintptr_t) address) - IMXRT_FLEXSPI1_CIPHER_BASE; irqstate_t flags; flags = enter_critical_section(); @@ -439,8 +441,6 @@ flash_func_erase_sector(unsigned sector) leave_critical_section(flags); UNUSED(status); } - - } void diff --git a/platforms/nuttx/src/bootloader/stm/stm32_common/main.c b/platforms/nuttx/src/bootloader/stm/stm32_common/main.c index 859d861b2094..cccfd41bea4c 100644 --- a/platforms/nuttx/src/bootloader/stm/stm32_common/main.c +++ b/platforms/nuttx/src/bootloader/stm/stm32_common/main.c @@ -466,18 +466,13 @@ flash_func_sector_size(unsigned sector) } void -flash_func_erase_sector(unsigned sector) +flash_func_erase_sector(unsigned sector, bool force) { if (sector > BOARD_FLASH_SECTORS || (int)sector < BOARD_FIRST_FLASH_SECTOR_TO_ERASE) { return; } - /* blank-check the sector */ - - bool blank = up_progmem_ispageerased(sector) == 0; - - /* erase the sector if it failed the blank check */ - if (!blank) { + if (force || (up_progmem_ispageerased(sector) != 0)) { up_progmem_eraseblock(sector); } }