Fix FC Storage data verification.

Former-commit-id: aefdf7f

Author: h2zero

cores/nRF5/FC_Store/FC_Store.cpp

@@ -105,13 +105,8 @@ bool FCStore::write (uint32_t id, uint8_t* buf, uint16_t len) {
     uint8_t numWords = len / WORD_BYTES;
     uint8_t numBytes = len & (uint16_t)0x03;
 
-    // store the word length in the lower 16 bits
-    if (numBytes > 0) {
-        len += WORD_BYTES - numBytes;
-    }
-    storeDesc.setStoreLength(len);
-
-    if ((nextWriteAddr + sizeof(FCStoreDesc) + len) > lastPage) {
+    // Make sure we check with +1 to the word count if there are stray bytes.
+    if ((nextWriteAddr + sizeof(FCStoreDesc) + (numBytes ? len + 1 : len)) > lastPage) {
         STORE_PRINTF("Error not enough storage\n");
         return false;
     }
@@ -121,12 +116,11 @@ bool FCStore::write (uint32_t id, uint8_t* buf, uint16_t len) {
     nextWriteAddr += sizeof(storeDesc);
     cbf->flashWriteWords(nextWriteAddr, (uint32_t*)buf, numWords);
     nextWriteAddr += numWords * WORD_BYTES;
-    buf += numWords * WORD_BYTES;
 
     if (numBytes) {
         // convert the leftover bytes to a word to maintain alignment
         uint32_t val32 = 0;
-        memcpy(&val32, buf, numBytes);
+        memcpy(&val32, buf + (numWords * WORD_BYTES), numBytes);
         cbf->flashWriteWord(nextWriteAddr, val32);
         nextWriteAddr += WORD_BYTES;
     }
@@ -143,9 +137,10 @@ bool FCStore::write (uint32_t id, uint8_t* buf, uint16_t len) {
         return false;
     }
 
-    uint16_t datalen = desc->getByteLength();
+    size_t datalen = desc->getByteLength();
     uint32_t dataAddr = (uint32_t)desc + sizeof(FCStoreDesc);
     if (datalen == len && memcmp(buf, (uint8_t*)dataAddr, datalen) == 0) {
+        STORE_PRINTF("Data stored successfully\n");
         return true;
     }
 
@@ -183,6 +178,7 @@ void FCStore::format() {
     eraseAll();
     cbf->flashWriteWord(firstPage, FC_STORE_MAGIC_VAL);
     nextWriteAddr = firstPage + WORD_BYTES;
+    initialized = true;
 }
 
 bool FCStore::defrag() {

cores/nRF5/FC_Store/FC_Store.h

@@ -24,11 +24,13 @@
 #define FC_STORE_MAGIC_VAL 0xf1a5c0f5UL
 #define FC_STORE_EMPTY_VAL 0xFFFFFFFFUL
 
+#ifndef FC_WORD_BYTES
 #define WORD_BYTES 4
+#else
+#define WORD_BYTES FC_WORD_BYTES
+#endif
 
-//#define STORE_DEBUG
-
-#ifdef STORE_DEBUG
+#ifdef FC_STORE_DEBUG
 #define STORE_PRINTF(msg, ...) printf((msg), ##__VA_ARGS__);
 #else
 #define STORE_PRINTF(msg, ...) void(msg)
@@ -51,10 +53,12 @@ class FCStoreDesc {
 
 public:
     FCStoreDesc(uint32_t val) { id = val; newAddress = FC_STORE_EMPTY_VAL; length = 0; }
-    uint16_t getByteLength(){ return (uint16_t)((length >> 16) & (uint32_t)0x0000FFFF); }
-    uint16_t getStoreLength(){ return (uint16_t)(length & (uint32_t)0x0000FFFF); }
-    void     setByteLength(uint16_t len) { length = (uint32_t)((len << 16) & (uint32_t)0xFFFF0000); }
-    void     setStoreLength(uint16_t len) { length |= (uint32_t)(len & (uint32_t)0x0000FFFF); }
+    uint16_t getByteLength() { return (uint16_t)((length >> 16) & (uint32_t)0x0000FFFF); }
+    uint16_t getStoreLength() {
+        uint16_t Blen = getByteLength();
+        return (uint16_t)(Blen + WORD_BYTES - (Blen & (uint16_t)(WORD_BYTES  - 1)));
+    }
+    void     setByteLength(uint16_t len) { length = (uint32_t)((len << 16) | (uint32_t)0x0000FFFF); }
     void     setId(uint32_t val) { id = val;}
     uint32_t getId() { return id; }
     uint32_t getNewAddress() { return newAddress; }

cores/nRF5/FC_Store/README.md

@@ -1,9 +1,32 @@
-# Flash Config Store
-
+# Flash Config Storage
 This library was originally designed to support storing bond info from NimBLE and has been redesigned to allow use as an EEPROM emulation for storing config data.
 
 ## Design
+A **minimum** of 2 flash pages are required. Pages used for FC storage are identified with the value 0xf1a5c0f5UL as the first word in the page.
+
+Wear leveling is accomplished by sequentially writing data across a number of flash pages. When data is updated the old data is marked with the location of the new data. When the end of the page is reached the original page is defragmented and valid data copied to the swap page which becomes the new data page and the old page is erased to be used for as the swap for the next defragmentation.
+
+### Data format
+The data is stored with a header in the following format:
+
+|  Data ID  | New Address | Length  | Reserved | Data       |
+|-----------|-------------|---------|----------|------------|
+|  32 bits  | 32 bits     | 16 bits | 16 bits  | data bytes |
+
+- The data ID can be any value from 0x00 to 0xFFFFFFFE (0xFFFFFFFFF is reserved as empty).
+- The New Address specifies the location of any updated data (0xFFFFFFFF if unchanged).
+- The length is the length of the data in Bytes.
+- Data is the stored data.
+
+When a data value is written FC storage will write the header and data in the next available space. If there is not enough space for the new data a defragmentation will be triggered in an attempt to provide space.
+
+When a data value is updated the New Address field of the previous data is updated to reflect the address of the new data location to read the updated data from. This continues for each time the value is updated.
+
+When reading a value FC Storage will find the first instance of the ID and check the New Address field, if a new address has been set it will check the New Address field of that location, move to the next if set, and so on until the latest data is found.
+
+### Defragmentation
+The defragmentation process consolidates all the current valid data to the swap page(s) and updates the page pointer to the beginning of the data. This sets all New Address header fields to empty and frees up space needed for new data. The old data page(s) will then be erased and become the swap page for the next defragmentation.
+
+
 
-A **minimum** of 2 flash pages are required. 
 
-Wear leveling is accomplished by sequentially writing data accross a page of flash. When data is updated the old data is marked with the location of the new data. When the end of the page is reached the original page is defragmented and valid data copied to the swap page which becomes the new main page and the old page is erased.