AP_FlashStorage: support H7 flash storage

this generalises AP_FlashStorage to support the 32 byte write
restrictions of the STM32H7

libraries/AP_FlashStorage/AP_FlashStorage.cpp

@@ -47,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);
 
@@ -59,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) {
@@ -74,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]) {
@@ -183,9 +183,11 @@ 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
 
         const uint32_t space_available = flash_sector_size - write_offset;
         const uint32_t space_required = sizeof(struct block_header) + max_write + reserved_space;
@@ -199,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);
@@ -229,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
@@ -288,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;
@@ -305,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));
 }
 
@@ -329,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));    
 }
 
@@ -375,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);
@@ -384,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("new sector unavailable; state=0x%02x\n", (unsigned)header.state);
+        debug("new sector unavailable; state=0x%02x\n", (unsigned)header.get_state());
         return false;
     }
 
@@ -398,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;
     }
@@ -433,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 &&
+        signature  == signature &&
+        signature2 == 0xFFFFFFFF &&
+        signature3 == 0xFFFFFFFF) {
+        return SECTOR_STATE_AVAILABLE;
+    }
+    if (state1 == 0xFFFFFFF1 &&
+        state2 == 0xFFFFFFF2 &&
+        state3 == 0xFFFFFFFF &&
+        signature  == signature &&
+        signature2 == signature &&
+        signature3 == 0xFFFFFFFF) {
+        return SECTOR_STATE_IN_USE;
+    }
+    if (state1 == 0xFFFFFFF1 &&
+        state2 == 0xFFFFFFF2 &&
+        state3 == 0xFFFFFFF3 &&
+        signature  == 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

@@ -39,14 +39,31 @@
 
 #include <AP_HAL/AP_HAL.h>
 
+/*
+  we support 3 different types of flash which have different restrictions
+ */
+#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_MULTI_WRITE 0
+#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
@@ -98,7 +121,7 @@ public:
     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;
@@ -114,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);
     };