AP_FlashStorage: resync for 4.0 update

libraries/AP_FlashStorage/AP_FlashStorage.cpp

@@ -1,5 +1,5 @@
 /*
-   Please contribute your ideas! See http://dev.ardupilot.org for details
+   Please contribute your ideas! See https://dev.ardupilot.org for details
 
    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
@@ -18,6 +18,7 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_FlashStorage/AP_FlashStorage.h>
 #include <AP_Math/AP_Math.h>
+#include <AP_InternalError/AP_InternalError.h>
 #include <stdio.h>
 
 #define FLASHSTORAGE_DEBUG 0
@@ -46,7 +47,7 @@ AP_FlashStorage::AP_FlashStorage(uint8_t *_mem_buffer,
 bool AP_FlashStorage::init(void)
 {
     debug("running init()\n");
-    
+
     // start with empty memory buffer
     memset(mem_buffer, 0, storage_size);
 
@@ -58,8 +59,8 @@ bool AP_FlashStorage::init(void)
         if (!flash_read(i, 0, (uint8_t *)&header[i], sizeof(header[i]))) {
             return false;
         }
-        bool bad_header = (header[i].signature != signature);
+        bool bad_header = !header[i].signature_ok();
-        enum SectorState state = (enum SectorState)header[i].state;
+        enum SectorState state = header[i].get_state();
         if (state != SECTOR_STATE_AVAILABLE &&
             state != SECTOR_STATE_IN_USE &&
             state != SECTOR_STATE_FULL) {
@@ -73,7 +74,7 @@ bool AP_FlashStorage::init(void)
     }
 
     // work out the first sector to read from using sector states
-    enum SectorState states[2] {(enum SectorState)header[0].state, (enum SectorState)header[1].state};
+    enum SectorState states[2] {header[0].get_state(), header[1].get_state()};
     uint8_t first_sector;
 
     if (states[0] == states[1]) {
@@ -139,7 +140,24 @@ bool AP_FlashStorage::init(void)
 bool AP_FlashStorage::switch_full_sector(void)
 {
     debug("running switch_full_sector()\n");
-    
+
+    if (in_switch_full_sector) {
+        INTERNAL_ERROR(AP_InternalError::error_t::switch_full_sector_recursion);
+        return false;
+    }
+    in_switch_full_sector = true;
+    bool ret = protected_switch_full_sector();
+    in_switch_full_sector = false;
+    return ret;
+}
+
+// protected_switch_full_sector is protected by switch_full_sector to
+// avoid an infinite recursion problem; switch_full_sectory calls
+// write() which can call switch_full_sector.  This has been seen in
+// practice, and while it might be caused by corruption... corruption
+// happens.
+bool AP_FlashStorage::protected_switch_full_sector(void)
+{
     // clear any write error
     write_error = false;
     reserved_space = 0;
@@ -165,11 +183,15 @@ bool AP_FlashStorage::write(uint16_t offset, uint16_t length)
     
     while (length > 0) {
         uint8_t n = max_write;
+#if AP_FLASHSTORAGE_TYPE != AP_FLASHSTORAGE_TYPE_H7
         if (length < n) {
             n = length;
         }
+#endif
 
-        if (write_offset > flash_sector_size - (sizeof(struct block_header) + max_write + reserved_space)) {
+        const uint32_t space_available = flash_sector_size - write_offset;
+        const uint32_t space_required = sizeof(struct block_header) + max_write + reserved_space;
+        if (space_available < space_required) {
             if (!switch_sectors()) {
                 if (!flash_erase_ok()) {
                     return false;
@@ -179,27 +201,45 @@ bool AP_FlashStorage::write(uint16_t offset, uint16_t length)
                 }
             }
         }
-        
+
-        struct block_header header;
+        struct PACKED {
-        header.state = BLOCK_STATE_WRITING;
+            struct block_header header;
-        header.block_num = offset / block_size;
+            uint8_t data[max_write];
-        header.num_blocks_minus_one = ((n + (block_size - 1)) / block_size)-1;
+        } blk;
-        uint16_t block_ofs = header.block_num*block_size;
+
-        uint16_t block_nbytes = (header.num_blocks_minus_one+1)*block_size;
+        blk.header.state = BLOCK_STATE_WRITING;
-        
+        blk.header.block_num = offset / block_size;
-#if AP_FLASHSTORAGE_MULTI_WRITE
+        blk.header.num_blocks_minus_one = ((n + (block_size - 1)) / block_size)-1;
-        if (!flash_write(current_sector, write_offset, (uint8_t*)&header, sizeof(header))) {
+
+        uint16_t block_ofs = blk.header.block_num*block_size;
+        uint16_t block_nbytes = (blk.header.num_blocks_minus_one+1)*block_size;
+
+        memcpy(blk.data, &mem_buffer[block_ofs], block_nbytes);
+
+#if AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_F4
+        if (!flash_write(current_sector, write_offset, (uint8_t*)&blk.header, sizeof(blk.header))) {
+            return false;
+        }
+        if (!flash_write(current_sector, write_offset+sizeof(blk.header), blk.data, block_nbytes)) {
+            return false;
+        }
+        blk.header.state = BLOCK_STATE_VALID;
+        if (!flash_write(current_sector, write_offset, (uint8_t*)&blk.header, sizeof(blk.header))) {
+            return false;
+        }
+#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_F1
+        blk.header.state = BLOCK_STATE_VALID;
+        if (!flash_write(current_sector, write_offset, (uint8_t*)&blk, sizeof(blk.header) + block_nbytes)) {
+            return false;
+        }
+#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_H7
+        blk.header.state = BLOCK_STATE_VALID;
+        if (!flash_write(current_sector, write_offset, (uint8_t*)&blk, sizeof(blk.header) + max_write)) {
             return false;
         }
 #endif
-        if (!flash_write(current_sector, write_offset+sizeof(header), &mem_buffer[block_ofs], block_nbytes)) {
+
-            return false;
+        write_offset += sizeof(blk.header) + block_nbytes;
-        }
-        header.state = BLOCK_STATE_VALID;
-        if (!flash_write(current_sector, write_offset, (uint8_t*)&header, sizeof(header))) {
-            return false;
-        }
-        write_offset += sizeof(header) + block_nbytes;
 
         uint8_t n2 = block_nbytes - (offset % block_size);
         //debug("write_block at %u for %u n2=%u\n", block_ofs, block_nbytes, n2);
@@ -209,7 +249,9 @@ bool AP_FlashStorage::write(uint16_t offset, uint16_t length)
         offset += n2;
         length -= n2;
     }
-    
+
+    //debug("write_offset %u\n", write_offset);
+
     // handle wrap to next sector
     // write data
     // write header word
@@ -268,6 +310,10 @@ bool AP_FlashStorage::load_sector(uint8_t sector)
             // invalid state
             return false;
         }
+#if AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_H7
+        // offsets must be advanced to a multiple of 32 on H7
+        ofs = (ofs + 31U) & ~31U;
+#endif
     }
     write_offset = ofs;
     return true;
@@ -285,8 +331,7 @@ bool AP_FlashStorage::erase_sector(uint8_t sector, bool mark_available)
         return true;
     }
     struct sector_header header;
-    header.signature = signature;
+    header.set_state(SECTOR_STATE_AVAILABLE);
-    header.state = SECTOR_STATE_AVAILABLE;
     return flash_write(sector, 0, (const uint8_t *)&header, sizeof(header));
 }
 
@@ -309,8 +354,7 @@ bool AP_FlashStorage::erase_all(void)
     
     // mark current sector as in-use
     struct sector_header header;
-    header.signature = signature;
+    header.set_state(SECTOR_STATE_IN_USE);
-    header.state = SECTOR_STATE_IN_USE;
     return flash_write(current_sector, 0, (const uint8_t *)&header, sizeof(header));    
 }
 
@@ -355,7 +399,6 @@ bool AP_FlashStorage::switch_sectors(void)
     }
 
     struct sector_header header;
-    header.signature = signature;
 
     uint8_t new_sector = current_sector ^ 1;
     debug("switching to sector %u\n", new_sector);
@@ -364,13 +407,13 @@ bool AP_FlashStorage::switch_sectors(void)
     if (!flash_read(new_sector, 0, (uint8_t *)&header, sizeof(header))) {
         return false;
     }
-    if (header.signature != signature) {
+    if (!header.signature_ok()) {
         write_error = true;
         return false;
     }
-    if (SECTOR_STATE_AVAILABLE != (enum SectorState)header.state) {
+    if (SECTOR_STATE_AVAILABLE != header.get_state()) {
         write_error = true;
-        debug("both sectors full\n");
+        debug("new sector unavailable; state=0x%02x\n", (unsigned)header.get_state());
         return false;
     }
 
@@ -378,13 +421,13 @@ bool AP_FlashStorage::switch_sectors(void)
     // mark the new sector as in-use so that a power failure between
     // the two steps doesn't leave us with an erase on the
     // reboot. Thanks to night-ghost for spotting this.
-    header.state = SECTOR_STATE_FULL;
+    header.set_state(SECTOR_STATE_FULL);
     if (!flash_write(current_sector, 0, (const uint8_t *)&header, sizeof(header))) {
         return false;
     }
 
     // mark new sector as in-use
-    header.state = SECTOR_STATE_IN_USE;
+    header.set_state(SECTOR_STATE_IN_USE);
     if (!flash_write(new_sector, 0, (const uint8_t *)&header, sizeof(header))) {
         return false;
     }
@@ -413,3 +456,163 @@ bool AP_FlashStorage::re_initialise(void)
     }
     return write_all();
 }
+
+#if AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_H7
+/*
+  H7 specific sector header functions
+ */
+bool AP_FlashStorage::sector_header::signature_ok(void) const
+{
+    for (uint8_t i=0; i<ARRAY_SIZE(pad1); i++) {
+        if (pad1[i] != 0xFFFFFFFFU || pad2[i] != 0xFFFFFFFFU || pad3[i] != 0xFFFFFFFFU) {
+            return false;
+        }
+    }
+    return signature1 == signature;
+}
+
+AP_FlashStorage::SectorState AP_FlashStorage::sector_header::get_state(void) const
+{
+    if (state1 == 0xFFFFFFF1 &&
+        state2 == 0xFFFFFFFF &&
+        state3 == 0xFFFFFFFF &&
+        signature1 == signature &&
+        signature2 == 0xFFFFFFFF &&
+        signature3 == 0xFFFFFFFF) {
+        return SECTOR_STATE_AVAILABLE;
+    }
+    if (state1 == 0xFFFFFFF1 &&
+        state2 == 0xFFFFFFF2 &&
+        state3 == 0xFFFFFFFF &&
+        signature1 == signature &&
+        signature2 == signature &&
+        signature3 == 0xFFFFFFFF) {
+        return SECTOR_STATE_IN_USE;
+    }
+    if (state1 == 0xFFFFFFF1 &&
+        state2 == 0xFFFFFFF2 &&
+        state3 == 0xFFFFFFF3 &&
+        signature1 == signature &&
+        signature2 == signature &&
+        signature3 == signature) {
+        return SECTOR_STATE_FULL;
+    }
+    return SECTOR_STATE_INVALID;
+}
+
+void AP_FlashStorage::sector_header::set_state(SectorState state)
+{
+    memset(pad1, 0xff, sizeof(pad1));
+    memset(pad2, 0xff, sizeof(pad2));
+    memset(pad3, 0xff, sizeof(pad3));
+    switch (state) {
+    case SECTOR_STATE_AVAILABLE:
+        signature1 = signature;
+        signature2 = 0xFFFFFFFF;
+        signature3 = 0xFFFFFFFF;
+        state1 = 0xFFFFFFF1;
+        state2 = 0xFFFFFFFF;
+        state3 = 0xFFFFFFFF;
+        break;
+    case SECTOR_STATE_IN_USE:
+        signature1 = signature;
+        signature2 = signature;
+        signature3 = 0xFFFFFFFF;
+        state1 = 0xFFFFFFF1;
+        state2 = 0xFFFFFFF2;
+        state3 = 0xFFFFFFFF;
+        break;
+    case SECTOR_STATE_FULL:
+        signature1 = signature;
+        signature2 = signature;
+        signature3 = signature;
+        state1 = 0xFFFFFFF1;
+        state2 = 0xFFFFFFF2;
+        state3 = 0xFFFFFFF3;
+        break;
+    default:
+        break;
+    }
+}
+
+#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_F1
+/*
+  F1/F3 specific sector header functions
+ */
+bool AP_FlashStorage::sector_header::signature_ok(void) const
+{
+    return signature1 == signature;
+}
+
+AP_FlashStorage::SectorState AP_FlashStorage::sector_header::get_state(void) const
+{
+    if (state1 == 0xFFFFFFFF) {
+        return SECTOR_STATE_AVAILABLE;
+    }
+    if (state1 == 0xFFFFFFF1) {
+        return SECTOR_STATE_IN_USE;
+    }
+    if (state1 == 0xFFF2FFF1) {
+        return SECTOR_STATE_FULL;
+    }
+    return SECTOR_STATE_INVALID;
+}
+
+void AP_FlashStorage::sector_header::set_state(SectorState state)
+{
+    signature1 = signature;
+    switch (state) {
+    case SECTOR_STATE_AVAILABLE:
+        state1 = 0xFFFFFFFF;
+        break;
+    case SECTOR_STATE_IN_USE:
+        state1 = 0xFFFFFFF1;
+        break;
+    case SECTOR_STATE_FULL:
+        state1 = 0xFFF2FFF1;
+        break;
+    default:
+        break;
+    }
+}
+#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_F4
+/*
+  F4 specific sector header functions
+ */
+bool AP_FlashStorage::sector_header::signature_ok(void) const
+{
+    return signature1 == signature;
+}
+
+AP_FlashStorage::SectorState AP_FlashStorage::sector_header::get_state(void) const
+{
+    if (state1 == 0xFF) {
+        return SECTOR_STATE_AVAILABLE;
+    }
+    if (state1 == 0xFE) {
+        return SECTOR_STATE_IN_USE;
+    }
+    if (state1 == 0xFC) {
+        return SECTOR_STATE_FULL;
+    }
+    return SECTOR_STATE_INVALID;
+}
+
+void AP_FlashStorage::sector_header::set_state(SectorState state)
+{
+    signature1 = signature;
+    switch (state) {
+    case SECTOR_STATE_AVAILABLE:
+        state1 = 0xFF;
+        break;
+    case SECTOR_STATE_IN_USE:
+        state1 = 0xFE;
+        break;
+    case SECTOR_STATE_FULL:
+        state1 = 0xFC;
+        break;
+    default:
+        break;
+    }
+}
+#endif

libraries/AP_FlashStorage/AP_FlashStorage.h

@@ -1,5 +1,5 @@
 /*
-   Please contribute your ideas! See http://dev.ardupilot.org for details
+   Please contribute your ideas! See https://dev.ardupilot.org for details
 
    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
@@ -39,14 +39,31 @@
 
 #include <AP_HAL/AP_HAL.h>
 
-#if defined(STM32F1)
 /*
-  the STM32F1 can't change individual bits from 1 to 0 unless all bits in
+  we support 3 different types of flash which have different restrictions
-  the 16 bit word are 1
  */
-#define AP_FLASHSTORAGE_MULTI_WRITE 0
+#define AP_FLASHSTORAGE_TYPE_F1  1 // F1 and F3
+#define AP_FLASHSTORAGE_TYPE_F4  2 // F4 and F7
+#define AP_FLASHSTORAGE_TYPE_H7  3 // H7
+
+#ifndef AP_FLASHSTORAGE_TYPE
+#if defined(STM32F1) || defined(STM32F3)
+/*
+  the STM32F1 and STM32F3 can't change individual bits from 1 to 0
+  unless all bits in the 16 bit word are 1
+ */
+#define AP_FLASHSTORAGE_TYPE AP_FLASHSTORAGE_TYPE_F1
+#elif defined(STM32H7)
+/*
+  STM32H7 can only write in 32 byte chunks, and must only write when all bits are 1
+ */
+#define AP_FLASHSTORAGE_TYPE AP_FLASHSTORAGE_TYPE_H7
 #else
-#define AP_FLASHSTORAGE_MULTI_WRITE 1
+/*
+  STM32HF4 and STM32H7 can update bits from 1 to 0
+ */
+#define AP_FLASHSTORAGE_TYPE AP_FLASHSTORAGE_TYPE_F4
+#endif
 #endif
 
 /*
@@ -54,9 +71,15 @@
  */
 class AP_FlashStorage {
 private:
+#if AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_H7
+    // need to write in 32 byte chunks, with 2 byte header
+    static const uint8_t block_size = 30;
+    static const uint8_t max_write = block_size;
+#else
     static const uint8_t block_size = 8;
-    static const uint16_t num_blocks = HAL_STORAGE_SIZE / block_size;
     static const uint8_t max_write = 64;
+#endif
+    static const uint16_t num_blocks = (HAL_STORAGE_SIZE+(block_size-1)) / block_size;
 
 public:
     // caller provided function to write to a flash sector
@@ -82,18 +105,23 @@ public:
     // initialise storage, filling mem_buffer with current contents
     bool init(void);
 
+    // erase sectors and re-initialise
+    bool erase(void) WARN_IF_UNUSED {
+        return erase_all();
+    }
+    
     // re-initialise storage, using current mem_buffer
-    bool re_initialise(void);
+    bool re_initialise(void) WARN_IF_UNUSED;
     
     // 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);
+    bool switch_full_sector(void) WARN_IF_UNUSED;
-    
+
     // write some data to storage from mem_buffer
-    bool write(uint16_t offset, uint16_t length);
+    bool write(uint16_t offset, uint16_t length) WARN_IF_UNUSED;
 
     // fixed storage size
-    static const uint16_t storage_size = block_size * num_blocks;
+    static const uint16_t storage_size = HAL_STORAGE_SIZE;
     
 private:
     uint8_t *mem_buffer;
@@ -109,34 +137,47 @@ private:
     bool write_error;
 
     // 24 bit signature
-#if AP_FLASHSTORAGE_MULTI_WRITE
+#if AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_F4
     static const uint32_t signature = 0x51685B;
-#else
+#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_F1
     static const uint32_t signature = 0x51;
+#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_H7
+    static const uint32_t signature = 0x51685B62;
+#else
+#error "Unknown AP_FLASHSTORAGE_TYPE"
 #endif
 
-    // 8 bit sector states
+    // sector states, representation depends on storage type
     enum SectorState {
-#if AP_FLASHSTORAGE_MULTI_WRITE
+        SECTOR_STATE_AVAILABLE = 1,
-        SECTOR_STATE_AVAILABLE = 0xFF,
+        SECTOR_STATE_IN_USE    = 2,
-        SECTOR_STATE_IN_USE    = 0xFE,
+        SECTOR_STATE_FULL      = 3,
-        SECTOR_STATE_FULL      = 0xFC
+        SECTOR_STATE_INVALID   = 4
-#else
-        SECTOR_STATE_AVAILABLE = 0xFFFFFFFF,
-        SECTOR_STATE_IN_USE    = 0xFFFFFFF1,
-        SECTOR_STATE_FULL      = 0xFFF2FFF1,
-#endif
     };
 
     // header in first word of each sector
     struct sector_header {
-#if AP_FLASHSTORAGE_MULTI_WRITE
+#if AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_F4
-        uint32_t state:8;
+        uint32_t state1:8;
-        uint32_t signature:24;
+        uint32_t signature1:24;
-#else
+#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_F1
-        uint32_t state:32;
+        uint32_t state1:32;
-        uint32_t signature:16;
+        uint32_t signature1:16;
+#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_H7
+        // needs to be 96 bytes on H7 to support 3 states
+        uint32_t state1;
+        uint32_t signature1;
+        uint32_t pad1[6];
+        uint32_t state2;
+        uint32_t signature2;
+        uint32_t pad2[6];
+        uint32_t state3;
+        uint32_t signature3;
+        uint32_t pad3[6];
 #endif
+        bool signature_ok(void) const;
+        SectorState get_state() const;
+        void set_state(SectorState state);
     };
 
 
@@ -157,20 +198,27 @@ private:
     static const uint32_t reserve_size = (storage_size / max_write) * (sizeof(block_header) + max_write) + max_write;
         
     // load data from a sector
-    bool load_sector(uint8_t sector);
+    bool load_sector(uint8_t sector) WARN_IF_UNUSED;
 
     // erase a sector and write header
-    bool erase_sector(uint8_t sector, bool mark_available);
+    bool erase_sector(uint8_t sector, bool mark_available) WARN_IF_UNUSED;
 
     // erase all sectors and reset
-    bool erase_all();
+    bool erase_all() WARN_IF_UNUSED;
 
     // write all of mem_buffer to current sector
-    bool write_all();
+    bool write_all() WARN_IF_UNUSED;
 
     // return true if all bytes are zero
-    bool all_zero(uint16_t ofs, uint16_t size);
+    bool all_zero(uint16_t ofs, uint16_t size) WARN_IF_UNUSED;
 
     // switch to next sector for writing
-    bool switch_sectors(void);
+    bool switch_sectors(void) WARN_IF_UNUSED;
+
+    // _switch_full_sector is protected by switch_full_sector to avoid
+    // an infinite recursion problem; switch_full_sectory calls
+    // write() which can call switch_full_sector.  This has been seen
+    // in practice.
+    bool protected_switch_full_sector(void) WARN_IF_UNUSED;
+    bool in_switch_full_sector;
 };