From ac070c92f5e72285493b48d0f924b591d130ad3e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 8 Feb 2019 16:37:03 +1100 Subject: [PATCH] AP_Bootloader: fully working on H7 --- Tools/AP_Bootloader/bl_protocol.cpp | 134 ++++++++++++++++++++++------ Tools/AP_Bootloader/support.cpp | 14 ++- Tools/AP_Bootloader/support.h | 2 +- 3 files changed, 117 insertions(+), 33 deletions(-) diff --git a/Tools/AP_Bootloader/bl_protocol.cpp b/Tools/AP_Bootloader/bl_protocol.cpp index d9017f63c4..e58993c017 100644 --- a/Tools/AP_Bootloader/bl_protocol.cpp +++ b/Tools/AP_Bootloader/bl_protocol.cpp @@ -40,6 +40,7 @@ ****************************************************************************/ #include +#include #include "ch.h" #include "hal.h" #include "hwdef.h" @@ -47,6 +48,8 @@ #include "bl_protocol.h" #include "support.h" +// #pragma GCC optimize("O0") + // bootloader flash update protocol. // @@ -125,6 +128,10 @@ static enum led_state {LED_BLINK, LED_ON, LED_OFF} led_state; volatile unsigned timer[NTIMERS]; +// keep back 32 bytes at the front of flash. This is long enough to allow for aligned +// access on STM32H7 +#define RESERVE_LEAD_WORDS 8 + /* 1ms timer tick callback */ @@ -174,7 +181,7 @@ led_set(enum led_state state) static void do_jump(uint32_t stacktop, uint32_t entrypoint) { -#if defined(STM32F7) +#if defined(STM32F7) || defined(STM32H7) // disable caches on F7 before starting program __DSB(); __ISB(); @@ -200,11 +207,14 @@ jump_to_app() const uint32_t *app_base = (const uint32_t *)(APP_START_ADDRESS); /* - * We refuse to program the first word of the app until the upload is marked - * complete by the host. So if it's not 0xffffffff, we should try booting it. + * We hold back the programming of the lead words until the upload + * is marked complete by the host. So if they are not 0xffffffff, + * we should try booting it. */ - if (app_base[0] == 0xffffffff) { - return; + for (uint8_t i=0; i 0 && address != fbuf.address + fbuf.n*4) { + if (!flash_write_flush()) { + return false; + } + } + while (nwords > 0) { + if (fbuf.n == 0) { + fbuf.address = address; + memset(fbuf.buffer, 0xff, sizeof(fbuf.buffer)); + } + uint8_t n = MIN(ARRAY_SIZE(fbuf.buffer)-fbuf.n, nwords); + memcpy(&fbuf.buffer[fbuf.n], v, n*4); + address += n*4; + v += n; + nwords -= n; + fbuf.n += n; + if (fbuf.n == ARRAY_SIZE(fbuf.buffer)) { + if (!flash_write_flush()) { + return false; + } + } + } + return true; +} + #define TEST_FLASH 0 #if TEST_FLASH static void test_flash() { - for (uint8_t i=0; i<10; i++) { - uprintf("waiting %u...\n", i); - delay(300); - } uint32_t loop = 1; bool init_done = false; while (true) { @@ -395,12 +458,18 @@ bootloader(unsigned timeout) #endif uint32_t address = board_info.fw_size; /* force erase before upload will work */ - uint32_t first_word = 0xffffffff; + uint32_t first_words[RESERVE_LEAD_WORDS]; bool done_sync = false; bool done_get_device = false; + static bool done_timer_init; + + memset(first_words, 0xFF, sizeof(first_words)); - chVTObjectInit(&systick_vt); - chVTSet(&systick_vt, chTimeMS2I(1), sys_tick_handler, nullptr); + if (!done_timer_init) { + done_timer_init = true; + chVTObjectInit(&systick_vt); + chVTSet(&systick_vt, chTimeMS2I(1), sys_tick_handler, nullptr); + } /* if we are working with a timeout, start it running */ if (timeout) { @@ -533,7 +602,9 @@ bootloader(unsigned timeout) // erase all sectors for (uint8_t i = 0; flash_func_sector_size(i) != 0; i++) { - flash_func_erase_sector(i); + if (!flash_func_erase_sector(i)) { + goto cmd_fail; + } } // enable the LED while verifying the erase @@ -601,18 +672,20 @@ bootloader(unsigned timeout) goto cmd_bad; } - if (address == 0) { - // save the first word and don't program it until everything else is done - first_word = flash_buffer.w[0]; - // replace first word with bits we can overwrite later - flash_buffer.w[0] = 0xffffffff; + // save the first words and don't program it until everything else is done + if (address < sizeof(first_words)) { + uint8_t n = MIN(sizeof(first_words)-address, arg); + memcpy(&first_words[address/4], &flash_buffer.w[0], n); + // replace first words with 1 bits we can overwrite later + memset(&flash_buffer.w[0], 0xFF, n); } arg /= 4; // program the words - if (!flash_func_write_words(address, flash_buffer.w, arg)) { + if (!flash_write_buffer(address, flash_buffer.w, arg)) { goto cmd_fail; } + address += arg * 4; break; // fetch CRC of the entire flash area @@ -626,19 +699,21 @@ bootloader(unsigned timeout) goto cmd_bad; } + if (!flash_write_flush()) { + goto cmd_bad; + } + // compute CRC of the programmed area uint32_t sum = 0; for (unsigned p = 0; p < board_info.fw_size; p += 4) { uint32_t bytes; - if ((p == 0) && (first_word != 0xffffffff)) { - bytes = first_word; - + if (p < sizeof(first_words) && first_words[0] != 0xFFFFFFFF) { + bytes = first_words[p/4]; } else { bytes = flash_func_read_word(p); } - sum = crc32((uint8_t *)&bytes, sizeof(bytes), sum); } @@ -779,16 +854,17 @@ bootloader(unsigned timeout) goto cmd_bad; } - // program the deferred first word - if (first_word != 0xffffffff) { - flash_func_write_word(0, first_word); + if (!flash_write_flush()) { + goto cmd_fail; + } - if (flash_func_read_word(0) != first_word) { + // program the deferred first word + if (first_words[0] != 0xffffffff) { + if (!flash_write_buffer(0, first_words, RESERVE_LEAD_WORDS)) { goto cmd_fail; } - // revert in case the flash was bad... - first_word = 0xffffffff; + memset(first_words, 0xff, sizeof(first_words)); } // send a sync and wait for it to be collected diff --git a/Tools/AP_Bootloader/support.cpp b/Tools/AP_Bootloader/support.cpp index 4e7d1137e0..02603d1f5e 100644 --- a/Tools/AP_Bootloader/support.cpp +++ b/Tools/AP_Bootloader/support.cpp @@ -25,6 +25,8 @@ static uint8_t last_uart; #define BOOTLOADER_BAUDRATE 115200 #endif +// #pragma GCC optimize("O0") + int16_t cin(unsigned timeout_ms) { uint8_t b = 0; @@ -254,12 +256,18 @@ extern "C" { void uprintf(const char *fmt, ...) { #if HAL_USE_SERIAL_USB == TRUE - char msg[200]; va_list ap; + static bool initialised; + char umsg[200]; + if (!initialised) { + initialised = true; + sercfg.speed = 57600; + sdStart(&SD7, &sercfg); + } va_start(ap, fmt); - uint32_t n = vsnprintf(msg, sizeof(msg), fmt, ap); + uint32_t n = vsnprintf(umsg, sizeof(umsg), fmt, ap); va_end(ap); - chnWriteTimeout(&SDU1, (const uint8_t *)msg, n, chTimeMS2I(100)); + chnWriteTimeout(&SD7, (const uint8_t *)umsg, n, chTimeMS2I(100)); #endif } diff --git a/Tools/AP_Bootloader/support.h b/Tools/AP_Bootloader/support.h index 51df9ca629..8096db348a 100644 --- a/Tools/AP_Bootloader/support.h +++ b/Tools/AP_Bootloader/support.h @@ -41,7 +41,7 @@ void led_on(unsigned led); void led_off(unsigned led); void led_toggle(unsigned led); -// printf to USB +// printf to debug uart (or USB) extern "C" { void uprintf(const char *fmt, ...); }