HAL_ChibiOS: use hal.flash API

# Conflicts:
#	libraries/AP_HAL_ChibiOS/Storage.cpp
#	libraries/AP_HAL_ChibiOS/Util.cpp

libraries/AP_HAL_ChibiOS/Flash.h

@@ -0,0 +1,30 @@
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#include "AP_HAL_ChibiOS_Namespace.h"
+#include "Semaphores.h"
+#include "hwdef/common/flash.h"
+
+class ChibiOS::Flash : public AP_HAL::Flash {
+public:
+    uint32_t getpageaddr(uint32_t page) override { return stm32_flash_getpageaddr(page); }
+    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);
+        bool ret = stm32_flash_erasepage(page);
+        chMtxUnlock(&sem);
+        return ret;
+    }
+    bool write(uint32_t addr, const void *buf, uint32_t count) override {
+        chMtxLock(&sem);
+        bool ret = (stm32_flash_write(addr, buf, count) == count);
+        chMtxUnlock(&sem);
+        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;
+};

libraries/AP_HAL_ChibiOS/Storage.cpp

@@ -39,6 +39,8 @@ extern const AP_HAL::HAL& hal;
 #define HAL_STORAGE_BACKUP_FILE "/APM/" SKETCHNAME ".bak"
 #endif
 
+#define STORAGE_FLASH_RETRIES 5
+
 void Storage::_storage_open(void)
 {
     if (_initialised) {
@@ -249,9 +251,14 @@ void Storage::_flash_write(uint16_t line)
 bool Storage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length)
 {
 #ifdef STORAGE_FLASH_PAGE
-    size_t base_address = stm32_flash_getpageaddr(_flash_page+sector);
+    size_t base_address = hal.flash->getpageaddr(_flash_page+sector);
-    bool ret = stm32_flash_write(base_address+offset, data, length) == length;
+    for (uint8_t i=0; i<STORAGE_FLASH_RETRIES; i++) {
-    if (!ret && _flash_erase_ok()) {
+        if (hal.flash->write(base_address+offset, data, length)) {
+            return true;
+        }
+        hal.scheduler->delay(1);
+    }
+    if (_flash_erase_ok()) {
         // we are getting flash write errors while disarmed. Try
         // re-writing all of flash
         uint32_t now = AP_HAL::millis();
@@ -273,7 +280,7 @@ bool Storage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *
  */
 bool Storage::_flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length)
 {
-    size_t base_address = stm32_flash_getpageaddr(_flash_page+sector);
+    size_t base_address = hal.flash->getpageaddr(_flash_page+sector);
     const uint8_t *b = ((const uint8_t *)base_address)+offset;
     memcpy(data, b, length);
     return true;
@@ -284,7 +291,13 @@ bool Storage::_flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, u
  */
 bool Storage::_flash_erase_sector(uint8_t sector)
 {
-    return stm32_flash_erasepage(_flash_page+sector);
+    for (uint8_t i=0; i<STORAGE_FLASH_RETRIES; i++) {
+        if (hal.flash->erasepage(_flash_page+sector)) {
+            return true;
+        }
+        hal.scheduler->delay(1);
+    }
+    return false;
 }
 
 /*

libraries/AP_HAL_ChibiOS/Storage.h

@@ -63,7 +63,7 @@ private:
 
 #ifdef STORAGE_FLASH_PAGE
     AP_FlashStorage _flash{_buffer,
-            stm32_flash_getpagesize(STORAGE_FLASH_PAGE),
+            hal.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),

libraries/AP_HAL_ChibiOS/Util.cpp

@@ -198,7 +198,7 @@ bool Util::flash_bootloader()
         return false;
     }
 
-    const uint32_t addr = stm32_flash_getpageaddr(0);
+    const uint32_t addr = hal.flash->getpageaddr(0);
     if (!memcmp(fw, (const void*)addr, fw_size)) {
         hal.console->printf("Bootloader up-to-date\n");
         free(fw);
@@ -207,7 +207,7 @@ bool Util::flash_bootloader()
     }
 
     hal.console->printf("Erasing\n");
-    if (!stm32_flash_erasepage(0)) {
+    if (!hal.flash->erasepage(0)) {
         hal.console->printf("Erase failed\n");
         free(fw);
         hal.scheduler->expect_delay_ms(0);
@@ -217,7 +217,7 @@ bool Util::flash_bootloader()
     const uint8_t max_attempts = 10;
     for (uint8_t i=0; i<max_attempts; i++) {
         void *context = hal.scheduler->disable_interrupts_save();
-        const int32_t written = stm32_flash_write(addr, fw, fw_size);
+        bool ok = hal.flash->write(addr, fw, fw_size);
         hal.scheduler->restore_interrupts(context);
         if (written == -1 || written < fw_size) {
             hal.console->printf("Flash failed! (attempt=%u/%u)\n",