diff --git a/libraries/AP_HAL_ChibiOS/Flash.h b/libraries/AP_HAL_ChibiOS/Flash.h
index 85b9807ee0..8188003bc3 100644
--- a/libraries/AP_HAL_ChibiOS/Flash.h
+++ b/libraries/AP_HAL_ChibiOS/Flash.h
@@ -11,20 +11,20 @@ public:
     uint32_t getpagesize(uint32_t page) override { return stm32_flash_getpagesize(page); }
     uint32_t getnumpages(void) override { return stm32_flash_getnumpages(); }
     bool erasepage(uint32_t page) override {
-        chMtxLock(&sem);
+        sem.take(HAL_SEMAPHORE_BLOCK_FOREVER);
         bool ret = stm32_flash_erasepage(page);
-        chMtxUnlock(&sem);
+        sem.give();
         return ret;
     }
     bool write(uint32_t addr, const void *buf, uint32_t count) override {
-        chMtxLock(&sem);
+        sem.take(HAL_SEMAPHORE_BLOCK_FOREVER);
         bool ret = (stm32_flash_write(addr, buf, count) == count);
-        chMtxUnlock(&sem);
+        sem.give();
         return ret;
     }
     void keep_unlocked(bool set) override { stm32_flash_keep_unlocked(set); }
     bool ispageerased(uint32_t page) override { return stm32_flash_ispageerased(page); }
 
 private:
-    mutex_t sem;
+    Semaphore sem;
 };
diff --git a/libraries/AP_HAL_ChibiOS/Storage.cpp b/libraries/AP_HAL_ChibiOS/Storage.cpp
index 25dc31d026..cbe987c480 100644
--- a/libraries/AP_HAL_ChibiOS/Storage.cpp
+++ b/libraries/AP_HAL_ChibiOS/Storage.cpp
@@ -269,7 +269,7 @@ bool Storage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *
                                 (unsigned)sector, (unsigned)offset, (unsigned)length, (unsigned)ok);
         }
     }
-    return ret;
+    return false;
 #else
     return false;
 #endif
diff --git a/libraries/AP_HAL_ChibiOS/Storage.h b/libraries/AP_HAL_ChibiOS/Storage.h
index 075c7ad98a..eb78a29325 100644
--- a/libraries/AP_HAL_ChibiOS/Storage.h
+++ b/libraries/AP_HAL_ChibiOS/Storage.h
@@ -63,7 +63,7 @@ private:
 
 #ifdef STORAGE_FLASH_PAGE
     AP_FlashStorage _flash{_buffer,
-            hal.flash->getpagesize(STORAGE_FLASH_PAGE),
+            stm32_flash_getpagesize(STORAGE_FLASH_PAGE),
             FUNCTOR_BIND_MEMBER(&Storage::_flash_write_data, bool, uint8_t, uint32_t, const uint8_t *, uint16_t),
             FUNCTOR_BIND_MEMBER(&Storage::_flash_read_data, bool, uint8_t, uint32_t, uint8_t *, uint16_t),
             FUNCTOR_BIND_MEMBER(&Storage::_flash_erase_sector, bool, uint8_t),
diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp
index 99225831bb..f64b5a5c4b 100644
--- a/libraries/AP_HAL_ChibiOS/Util.cpp
+++ b/libraries/AP_HAL_ChibiOS/Util.cpp
@@ -216,10 +216,8 @@ bool Util::flash_bootloader()
     hal.console->printf("Flashing %s @%08x\n", fw_name, (unsigned int)addr);
     const uint8_t max_attempts = 10;
     for (uint8_t i=0; i<max_attempts; i++) {
-        void *context = hal.scheduler->disable_interrupts_save();
         bool ok = hal.flash->write(addr, fw, fw_size);
-        hal.scheduler->restore_interrupts(context);
-        if (written == -1 || written < fw_size) {
+        if (!ok) {
             hal.console->printf("Flash failed! (attempt=%u/%u)\n",
                                 i+1,
                                 max_attempts);