diff --git a/Tools/AP_Bootloader/bl_protocol.cpp b/Tools/AP_Bootloader/bl_protocol.cpp
index 593aa55dc4..63f467da95 100644
--- a/Tools/AP_Bootloader/bl_protocol.cpp
+++ b/Tools/AP_Bootloader/bl_protocol.cpp
@@ -130,7 +130,9 @@ volatile unsigned timer[NTIMERS];
  */
 static void sys_tick_handler(void *ctx)
 {
+    chSysLock();
     chVTSetI(&systick_vt, chTimeMS2I(1), sys_tick_handler, nullptr);
+    chSysUnlock();
     uint8_t i;
     for (i = 0; i < NTIMERS; i++)
         if (timer[i] > 0) {
@@ -338,28 +340,47 @@ static void test_flash()
         delay(300);
     }
     uint32_t loop = 1;
+    bool init_done = false;
     while (true) {
         uint32_t addr = 0;
-        for (uint8_t p=0; p<15; p++) {
+        uint32_t page = 0;
+        while (true) {
             uint32_t v[8];
             for (uint8_t i=0; i<8; i++) {
-                v[i] = (p<<16) + loop;
+                v[i] = (page<<16) + loop;
             }
-            if (flash_func_sector_size(p) == 0) {
+            if (flash_func_sector_size(page) == 0) {
                 continue;
             }
-            uprintf("page %u size %u addr=0x%08x v=0x%08x\n", p, flash_func_sector_size(p), addr, v[0]); delay(10);
-            uprintf("CR1=0x%08x CR2=0x%08x SR1=0x%08x SR2=0x%08x\n",
-                    FLASH->CR1, FLASH->CR2, FLASH->SR1, FLASH->SR2); delay(100);
-            uprintf("page %u 0x%08x\n", p, flash_func_read_word(addr)); delay(100);
-            flash_func_erase_sector(p);
-            uprintf("page %u 0x%08x\n", p, flash_func_read_word(addr)); delay(10);
-            flash_func_write_words(addr, v, ARRAY_SIZE(v));
-            uprintf("page %u 0x%08x\n", p, flash_func_read_word(addr)); delay(10);
-            uprintf("CR1=0x%08x CR2=0x%08x SR1=0x%08x SR2=0x%08x\n",
-                    FLASH->CR1, FLASH->CR2, FLASH->SR1, FLASH->SR2);
-            addr += flash_func_sector_size(p);
+            uint32_t num_writes = flash_func_sector_size(page) / sizeof(v);
+            uprintf("page %u size %u addr=0x%08x v=0x%08x\n",
+                    page, flash_func_sector_size(page), addr, v[0]); delay(10);
+            if (init_done) {
+                for (uint32_t j=0; j<flash_func_sector_size(page)/4; j++) {
+                    uint32_t v1 = (page<<16) + (loop-1);
+                    uint32_t v2 = flash_func_read_word(addr+j*4);
+                    if (v2 != v1) {
+                        uprintf("read error at 0x%08x v=0x%08x v2=0x%08x\n", addr+j*4, v1, v2);
+                        break;
+                    }
+                }
+            }
+            if (!flash_func_erase_sector(page)) {
+                uprintf("erase of %u failed\n", page);
+            }
+            for (uint32_t j=0; j<num_writes; j++) {
+                if (!flash_func_write_words(addr+j*sizeof(v), v, ARRAY_SIZE(v))) {
+                    uprintf("write failed at 0x%08x\n", addr+j*sizeof(v));
+                    break;
+                }
+            }
+            addr += flash_func_sector_size(page);
+            page++;
+            if (flash_func_sector_size(page) == 0) {
+                break;
+            }
         }
+        init_done = true;
         delay(1000);
         loop++;
     }
diff --git a/Tools/AP_Bootloader/support.cpp b/Tools/AP_Bootloader/support.cpp
index c53bdc3f91..4e7d1137e0 100644
--- a/Tools/AP_Bootloader/support.cpp
+++ b/Tools/AP_Bootloader/support.cpp
@@ -109,11 +109,12 @@ uint32_t flash_func_sector_size(uint32_t sector)
     return stm32_flash_getpagesize(flash_base_page+sector);
 }
 
-void flash_func_erase_sector(uint32_t sector)
+bool flash_func_erase_sector(uint32_t sector)
 {
     if (!stm32_flash_ispageerased(flash_base_page+sector)) {
-        stm32_flash_erasepage(flash_base_page+sector);
+        return stm32_flash_erasepage(flash_base_page+sector);
     }
+    return true;
 }
 
 // read one-time programmable memory
diff --git a/Tools/AP_Bootloader/support.h b/Tools/AP_Bootloader/support.h
index 8d06997b84..51df9ca629 100644
--- a/Tools/AP_Bootloader/support.h
+++ b/Tools/AP_Bootloader/support.h
@@ -24,7 +24,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);
-void flash_func_erase_sector(uint32_t sector);
+bool flash_func_erase_sector(uint32_t sector);
 uint32_t flash_func_read_otp(uint32_t idx);
 uint32_t flash_func_read_sn(uint32_t idx);
 void flash_set_keep_unlocked(bool);
@@ -42,7 +42,9 @@ void led_off(unsigned led);
 void led_toggle(unsigned led);
 
 // printf to USB
+extern "C" {
 void uprintf(const char *fmt, ...);
+}
 
 // generate a LED sequence forever
 void led_pulses(uint8_t npulses);