AP_FlashStorage: added erase_ok callback

this allows for simpler operation when application wants to allow
erase while disarmed.

libraries/AP_FlashStorage/AP_FlashStorage.cpp

@@ -32,12 +32,14 @@ AP_FlashStorage::AP_FlashStorage(uint8_t *_mem_buffer,
                                  uint32_t _flash_sector_size,
                                  FlashWrite _flash_write,
                                  FlashRead _flash_read,
-                                 FlashErase _flash_erase) :
+                                 FlashErase _flash_erase,
+                                 FlashEraseOK _flash_erase_ok) :
     mem_buffer(_mem_buffer),
     flash_sector_size(_flash_sector_size),
     flash_write(_flash_write),
     flash_read(_flash_read),
-    flash_erase(_flash_erase) {}
+    flash_erase(_flash_erase),
+    flash_erase_ok(_flash_erase_ok) {}
 
 // initialise storage
 bool AP_FlashStorage::init(void)
@@ -59,8 +61,7 @@ bool AP_FlashStorage::init(void)
         enum SectorState state = (enum SectorState)header[i].state;
         if (state != SECTOR_STATE_AVAILABLE &&
             state != SECTOR_STATE_IN_USE &&
-            state != SECTOR_STATE_FULL &&
+            state != SECTOR_STATE_FULL) {
-            state != SECTOR_STATE_FREE) {
             bad_header = true;
         }
 
@@ -115,10 +116,9 @@ bool AP_FlashStorage::init(void)
         }
     }
 
-    // erase any sectors marked full or free
+    // erase any sectors marked full
     for (uint8_t i=0; i<2; i++) {
-        if (states[i] == SECTOR_STATE_FULL ||
+        if (states[i] == SECTOR_STATE_FULL) {
-            states[i] == SECTOR_STATE_FREE) {
             if (!erase_sector(i)) {
                 return false;
             }
@@ -131,6 +131,29 @@ bool AP_FlashStorage::init(void)
     return true;
 }
 
+
+
+// switch full sector - should only be called when safe to have CPU
+// offline for considerable periods as an erase will be needed
+bool AP_FlashStorage::switch_full_sector(void)
+{
+    debug("running switch_full_sector()\n");
+    
+    // clear any write error
+    write_error = false;
+    reserved_space = 0;
+    
+    if (!write_all()) {
+        return false;
+    }
+
+    if (!erase_sector(current_sector ^ 1)) {
+        return false;
+    }
+
+    return switch_sectors();
+}
+
 // write some data to virtual EEPROM
 bool AP_FlashStorage::write(uint16_t offset, uint16_t length)
 {
@@ -147,7 +170,12 @@ bool AP_FlashStorage::write(uint16_t offset, uint16_t length)
 
         if (write_offset > flash_sector_size - (sizeof(struct block_header) + max_write + reserved_space)) {
             if (!switch_sectors()) {
-                return false;
+                if (!flash_erase_ok()) {
+                    return false;
+                }
+                if (!switch_full_sector()) {
+                    return false;                    
+                }
             }
         }
         
@@ -303,21 +331,14 @@ bool AP_FlashStorage::switch_sectors(void)
         return false;
     }
 
-    // mark current sector as full
     struct sector_header header;
     header.signature = signature;
-    header.state = SECTOR_STATE_FULL;
-    if (!flash_write(current_sector, 0, (const uint8_t *)&header, sizeof(header))) {
-        return false;
-    }
 
-    // switch sectors
+    uint8_t new_sector = current_sector ^ 1;
-    current_sector ^= 1;
+    debug("switching to sector %u\n", new_sector);
-
-    debug("switching to sector %u\n", current_sector);
     
     // check sector is available
-    if (!flash_read(current_sector, 0, (uint8_t *)&header, sizeof(header))) {
+    if (!flash_read(new_sector, 0, (uint8_t *)&header, sizeof(header))) {
         return false;
     }
     if (header.signature != signature) {
@@ -332,10 +353,20 @@ bool AP_FlashStorage::switch_sectors(void)
 
     // mark it in-use
     header.state = SECTOR_STATE_IN_USE;
+    if (!flash_write(new_sector, 0, (const uint8_t *)&header, sizeof(header))) {
+        return false;
+    }
+
+
+    // mark current sector as full
+    header.state = SECTOR_STATE_FULL;
     if (!flash_write(current_sector, 0, (const uint8_t *)&header, sizeof(header))) {
         return false;
     }
 
+    // switch sectors
+    current_sector = new_sector;
+        
     // we need to reserve some space in next sector to ensure we can successfully do a
     // full write out on init()
     reserved_space = reserve_size;

libraries/AP_FlashStorage/AP_FlashStorage.h

@@ -58,16 +58,24 @@ public:
     // caller provided function to erase a flash sector. Only called from init()
     FUNCTOR_TYPEDEF(FlashErase, bool, uint8_t );
 
+    // caller provided function to indicate if erasing is allowed
+    FUNCTOR_TYPEDEF(FlashEraseOK, bool);
+    
     // constructor. 
     AP_FlashStorage(uint8_t *mem_buffer,        // buffer of storage_size bytes
                     uint32_t flash_sector_size, // size of each flash sector in bytes
                     FlashWrite flash_write,     // function to write to flash
                     FlashRead flash_read,       // function to read from flash
-                    FlashErase flash_erase);    // function to erase flash
+                    FlashErase flash_erase,     // function to erase flash
+                    FlashEraseOK flash_erase_ok); // function to check if erasing allowed
 
     // initialise storage, filling mem_buffer with current contents
     bool init(void);
 
+    // switch full sector - should only be called when safe to have CPU
+    // offline for considerable periods as an erase will be needed
+    bool switch_full_sector(void);
+    
     // write some data to storage from mem_buffer
     bool write(uint16_t offset, uint16_t length);
 
@@ -80,6 +88,7 @@ private:
     FlashWrite flash_write;
     FlashRead flash_read;
     FlashErase flash_erase;
+    FlashEraseOK flash_erase_ok;
 
     uint8_t current_sector;
     uint32_t write_offset;
@@ -93,8 +102,7 @@ private:
     enum SectorState {
         SECTOR_STATE_AVAILABLE = 0xFF,
         SECTOR_STATE_IN_USE    = 0xFE,
-        SECTOR_STATE_FULL      = 0xFC,
+        SECTOR_STATE_FULL      = 0xFC
-        SECTOR_STATE_FREE      = 0xF8,
     };
 
     // header in first word of each sector

libraries/AP_FlashStorage/examples/FlashTest/FlashTest.cpp

@@ -27,15 +27,19 @@ private:
     bool flash_write(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length);
     bool flash_read(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length);
     bool flash_erase(uint8_t sector);
+    bool flash_erase_ok(void);
     
     AP_FlashStorage storage{mem_buffer,
             flash_sector_size,
             FUNCTOR_BIND_MEMBER(&FlashTest::flash_write, bool, uint8_t, uint32_t, const uint8_t *, uint16_t),
             FUNCTOR_BIND_MEMBER(&FlashTest::flash_read, bool, uint8_t, uint32_t, uint8_t *, uint16_t),
-            FUNCTOR_BIND_MEMBER(&FlashTest::flash_erase, bool, uint8_t)};
+            FUNCTOR_BIND_MEMBER(&FlashTest::flash_erase, bool, uint8_t),
+            FUNCTOR_BIND_MEMBER(&FlashTest::flash_erase_ok, bool)};
 
     // write to storage and mem_mirror
     void write(uint16_t offset, const uint8_t *data, uint16_t length);
+
+    bool erase_ok;
 };
 
 bool FlashTest::flash_write(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length)
@@ -80,20 +84,19 @@ bool FlashTest::flash_erase(uint8_t sector)
     return true;
 }
 
+bool FlashTest::flash_erase_ok(void)
+{
+    return erase_ok;
+}
+
 void FlashTest::write(uint16_t offset, const uint8_t *data, uint16_t length)
 {
     memcpy(&mem_mirror[offset], data, length);
     memcpy(&mem_buffer[offset], data, length);
     if (!storage.write(offset, length)) {
-        printf("Failed to write at %u for %u\n", offset, length);
+        if (erase_ok) {
-        if (!storage.init()) {
+            printf("Failed to write at %u for %u\n", offset, length);
-            AP_HAL::panic("Failed to re-init");
         }
-        memcpy(&mem_buffer[offset], data, length);
-        if (!storage.write(offset, length)) {
-            AP_HAL::panic("Failed 2nd write at %u for %u", offset, length);
-        }
-        printf("re-init OK\n");
     }
 }
 
@@ -111,7 +114,7 @@ void FlashTest::setup(void)
     }
 
     // fill with 10k random writes
-    for (uint32_t i=0; i<50000000; i++) {
+    for (uint32_t i=0; i<5000000; i++) {
         uint16_t ofs = get_random16() % sizeof(mem_buffer);
         uint16_t length = get_random16() & 0x1F;
         length = MIN(length, sizeof(mem_buffer) - ofs);
@@ -119,15 +122,22 @@ void FlashTest::setup(void)
         for (uint8_t j=0; j<length; j++) {
             data[j] = get_random16() & 0xFF;
         }
+
+        erase_ok = (i % 1000 == 0);
         write(ofs, data, length);
 
-        if (i % 1000 == 0) {
+        if (erase_ok) {
             if (memcmp(mem_buffer, mem_mirror, sizeof(mem_buffer)) != 0) {
                 AP_HAL::panic("FATAL: data mis-match at i=%u", i);
             }
         }
     }
 
+    // force final write to allow for flush with erase_ok
+    erase_ok = true;
+    uint8_t b = 42;
+    write(37, &b, 1);
+    
     if (memcmp(mem_buffer, mem_mirror, sizeof(mem_buffer)) != 0) {
         AP_HAL::panic("FATAL: data mis-match before re-init");
     }