AP_FlashStorage: a flash storage driver

this will allow for a storage backend using flash sectors in a log
structure

libraries/AP_FlashStorage/AP_FlashStorage.cpp

@@ -0,0 +1,345 @@
+/*
+   Please contribute your ideas! See http://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
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_FlashStorage/AP_FlashStorage.h>
+#include <stdio.h>
+
+#define FLASHSTORAGE_DEBUG 0
+
+#if FLASHSTORAGE_DEBUG
+#define debug(fmt, args...)  do { printf(fmt, ##args); } while(0)
+#else
+#define debug(fmt, args...)  do { } while(0)
+#endif
+
+// constructor.
+AP_FlashStorage::AP_FlashStorage(uint8_t *_mem_buffer,
+                                 uint32_t _flash_sector_size,
+                                 FlashWrite _flash_write,
+                                 FlashRead _flash_read,
+                                 FlashErase _flash_erase) :
+    mem_buffer(_mem_buffer),
+    flash_sector_size(_flash_sector_size),
+    flash_write(_flash_write),
+    flash_read(_flash_read),
+    flash_erase(_flash_erase) {}
+
+// initialise storage
+bool AP_FlashStorage::init(void)
+{
+    debug("running init()\n");
+    
+    // start with empty memory buffer
+    memset(mem_buffer, 0, storage_size);
+
+    // find state of sectors
+    struct sector_header header[2];
+
+    // read headers and possibly initialise if bad signature
+    for (uint8_t i=0; i<2; i++) {
+        if (!flash_read(i, 0, (uint8_t *)&header[i], sizeof(header[i]))) {
+            return false;
+        }
+        bool bad_header = (header[i].signature != signature);
+        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_FREE) {
+            bad_header = true;
+        }
+
+        // initialise if bad header
+        if (bad_header) {
+            return erase_all();
+        }
+    }
+
+    // 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};
+    uint8_t first_sector;
+
+    if (states[0] == states[1]) {
+        if (states[0] != SECTOR_STATE_AVAILABLE) {
+            return erase_all();
+        }
+        first_sector = 0;
+    } else if (states[0] == SECTOR_STATE_FULL) {
+        first_sector = 0;
+    } else if (states[1] == SECTOR_STATE_FULL) {
+        first_sector = 1;
+    } else if (states[0] == SECTOR_STATE_IN_USE) {
+        first_sector = 0;
+    } else if (states[1] == SECTOR_STATE_IN_USE) {
+        first_sector = 1;
+    } else {
+        // doesn't matter which is first
+        first_sector = 0;
+    }
+
+    // load data from any current sectors
+    for (uint8_t i=0; i<2; i++) {
+        uint8_t sector = (first_sector + i) & 1;
+        if (states[sector] == SECTOR_STATE_IN_USE ||
+            states[sector] == SECTOR_STATE_FULL) {
+            if (!load_sector(sector)) {
+                return erase_all();
+            }
+        }
+    }
+
+    // clear any write error
+    write_error = false;
+    reserved_space = 0;
+    
+    // if the first sector is full then write out all data so we can erase it
+    if (states[first_sector] == SECTOR_STATE_FULL) {
+        current_sector = first_sector ^ 1;
+        if (!write_all()) {
+            return erase_all();
+        }
+    }
+
+    // erase any sectors marked full or free
+    for (uint8_t i=0; i<2; i++) {
+        if (states[i] == SECTOR_STATE_FULL ||
+            states[i] == SECTOR_STATE_FREE) {
+            if (!erase_sector(i)) {
+                return false;
+            }
+        }
+    }
+
+    reserved_space = 0;
+    
+    // ready to use
+    return true;
+}
+
+// write some data to virtual EEPROM
+bool AP_FlashStorage::write(uint16_t offset, uint16_t length)
+{
+    if (write_error) {
+        return false;
+    }
+    //debug("write at %u for %u write_offset=%u\n", offset, length, write_offset);
+    
+    while (length > 0) {
+        uint8_t n = max_write;
+        if (length < n) {
+            n = length;
+        }
+
+        if (write_offset > flash_sector_size - (sizeof(struct block_header) + max_write + reserved_space)) {
+            if (!switch_sectors()) {
+                return false;
+            }
+        }
+        
+        struct block_header header;
+        header.state = BLOCK_STATE_WRITING;
+        header.block_num = offset / block_size;
+        header.num_blocks_minus_one = ((n + (block_size - 1)) / block_size)-1;
+        uint16_t block_ofs = header.block_num*block_size;
+        uint16_t block_nbytes = (header.num_blocks_minus_one+1)*block_size;
+        
+        if (!flash_write(current_sector, write_offset, (uint8_t*)&header, sizeof(header))) {
+            return false;
+        }
+        if (!flash_write(current_sector, write_offset+sizeof(header), &mem_buffer[block_ofs], block_nbytes)) {
+            return false;
+        }
+        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);
+        if (n2 > length) {
+            break;
+        }
+        offset += n2;
+        length -= n2;
+    }
+    
+    // handle wrap to next sector
+    // write data
+    // write header word
+    return true;
+}
+
+/*
+  load all data from a flash sector into mem_buffer
+ */
+bool AP_FlashStorage::load_sector(uint8_t sector)
+{
+    uint32_t ofs = sizeof(sector_header);
+    while (ofs < flash_sector_size - sizeof(struct block_header)) {
+        struct block_header header;
+        if (!flash_read(sector, ofs, (uint8_t *)&header, sizeof(header))) {
+            return false;
+        }
+        enum BlockState state = (enum BlockState)header.state;
+
+        switch (state) {
+        case BLOCK_STATE_AVAILABLE:
+            // we've reached the end
+            write_offset = ofs;
+            return true;
+
+        case BLOCK_STATE_VALID:
+        case BLOCK_STATE_WRITING: {
+            uint16_t block_ofs = header.block_num*block_size;
+            uint16_t block_nbytes = (header.num_blocks_minus_one+1)*block_size;
+            if (block_ofs + block_nbytes > storage_size) {
+                return false;
+            }
+            if (state == BLOCK_STATE_VALID &&
+                !flash_read(sector, ofs+sizeof(header), &mem_buffer[block_ofs], block_nbytes)) {
+                return false;
+            }
+            //debug("read at %u for %u\n", block_ofs, block_nbytes);
+            ofs += block_nbytes + sizeof(header);
+            break;
+        }
+        default:
+            // invalid state
+            return false;
+        }
+    }
+    write_offset = ofs;
+    return true;
+}
+
+/*
+  erase one sector
+ */
+bool AP_FlashStorage::erase_sector(uint8_t sector)
+{
+    if (!flash_erase(sector)) {
+        return false;
+    }
+
+    struct sector_header header;
+    header.signature = signature;
+    header.state = SECTOR_STATE_AVAILABLE;
+    return flash_write(sector, 0, (const uint8_t *)&header, sizeof(header));
+}
+
+/*
+  erase both sectors
+ */
+bool AP_FlashStorage::erase_all(void)
+{
+    write_error = false;
+    
+    // start with empty memory buffer
+    memset(mem_buffer, 0, storage_size);
+    current_sector = 0;
+    write_offset = sizeof(struct sector_header);
+    
+    if (!erase_sector(0) || !erase_sector(1)) {
+        return false;
+    }
+    
+    // mark current sector as in-use
+    struct sector_header header;
+    header.signature = signature;
+    header.state = SECTOR_STATE_IN_USE;
+    return flash_write(current_sector, 0, (const uint8_t *)&header, sizeof(header));    
+}
+
+/*
+  write all of mem_buffer to current sector
+ */
+bool AP_FlashStorage::write_all(void)
+{
+    debug("write_all to sector %u at %u with reserved_space=%u\n",
+           current_sector, write_offset, reserved_space);
+    for (uint16_t ofs=0; ofs<storage_size; ofs += max_write) {
+        if (!all_zero(ofs, max_write)) {
+            if (!write(ofs, max_write)) {
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
+// return true if all bytes are zero
+bool AP_FlashStorage::all_zero(uint16_t ofs, uint16_t size)
+{
+    while (size--) {
+        if (mem_buffer[ofs++] != 0) {
+            return false;
+        }
+    }
+    return true;
+}
+
+// switch to next sector for writing
+bool AP_FlashStorage::switch_sectors(void)
+{
+    if (reserved_space != 0) {
+        // other sector is already full
+        debug("both sectors are full\n");
+        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
+    current_sector ^= 1;
+
+    debug("switching to sector %u\n", current_sector);
+    
+    // check sector is available
+    if (!flash_read(current_sector, 0, (uint8_t *)&header, sizeof(header))) {
+        return false;
+    }
+    if (header.signature != signature) {
+        write_error = true;
+        return false;
+    }
+    if (SECTOR_STATE_AVAILABLE != (enum SectorState)header.state) {
+        write_error = true;
+        debug("both sectors full\n");
+        return false;
+    }
+
+    // mark it in-use
+    header.state = SECTOR_STATE_IN_USE;
+    if (!flash_write(current_sector, 0, (const uint8_t *)&header, sizeof(header))) {
+        return false;
+    }
+
+    // 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;
+    
+    write_offset = sizeof(header);
+    return true;    
+}

libraries/AP_FlashStorage/AP_FlashStorage.h

@@ -0,0 +1,140 @@
+/*
+   Please contribute your ideas! See http://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
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  a class to allow for FLASH to be used as a memory backed storage
+  backend for any HAL. The basic methodology is to use a log based
+  storage system over two flash sectors. Key design elements:
+
+  - erase of sectors only called on init, as erase will lock the flash
+    and prevent code execution
+
+  - write using log based system
+
+  - read requires scan of all log elements. This is expected to be called rarely
+
+  - assumes flash that erases to 0xFF and where writing can only clear
+    bits, not set them
+
+  - assumes flash sectors are much bigger than storage size. If they
+    aren't then caller can aggregate multiple sectors. Designed for
+    128k flash sectors with 16k storage size.
+
+  - assumes two flash sectors are available
+ */
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+
+/*
+  The StorageManager holds the layout of non-volatile storeage
+ */
+class AP_FlashStorage {
+private:
+    static const uint8_t block_size = 8;
+    static const uint16_t num_blocks = 2048;
+    static const uint8_t max_write = 64;
+
+public:
+    // caller provided function to write to a flash sector
+    FUNCTOR_TYPEDEF(FlashWrite, bool, uint8_t , uint32_t , const uint8_t *, uint16_t );
+
+    // caller provided function to read from a flash sector. Only called on init()
+    FUNCTOR_TYPEDEF(FlashRead, bool, uint8_t , uint32_t , uint8_t *, uint16_t );
+    
+    // caller provided function to erase a flash sector. Only called from init()
+    FUNCTOR_TYPEDEF(FlashErase, bool, uint8_t );
+
+    // 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
+
+    // initialise storage, filling mem_buffer with current contents
+    bool init(void);
+    
+    // write some data to storage from mem_buffer
+    bool write(uint16_t offset, uint16_t length);
+
+    // fixed storage size
+    static const uint16_t storage_size = block_size * num_blocks;
+    
+private:
+    uint8_t *mem_buffer;
+    const uint32_t flash_sector_size;
+    FlashWrite flash_write;
+    FlashRead flash_read;
+    FlashErase flash_erase;
+
+    uint8_t current_sector;
+    uint32_t write_offset;
+    uint32_t reserved_space;
+    bool write_error;
+
+    // 24 bit signature
+    static const uint32_t signature = 0x51685B;
+
+    // 8 bit sector states
+    enum SectorState {
+        SECTOR_STATE_AVAILABLE = 0xFF,
+        SECTOR_STATE_IN_USE    = 0xFE,
+        SECTOR_STATE_FULL      = 0xFC,
+        SECTOR_STATE_FREE      = 0xF8,
+    };
+
+    // header in first word of each sector
+    struct sector_header {
+        uint32_t state:8;
+        uint32_t signature:24;
+    };
+
+
+    enum BlockState {
+        BLOCK_STATE_AVAILABLE = 0x3,
+        BLOCK_STATE_WRITING   = 0x1,
+        BLOCK_STATE_VALID     = 0x0
+    };
+    
+    // header of each block of data
+    struct block_header {
+        uint16_t state:2;
+        uint16_t block_num:11;
+        uint16_t num_blocks_minus_one:3;
+    };
+
+    // amount of space needed to write full storage
+    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);
+
+    // erase a sector and write header
+    bool erase_sector(uint8_t sector);
+
+    // erase all sectors and reset
+    bool erase_all(void);
+
+    // write all of mem_buffer to current sector
+    bool write_all(void);
+
+    // return true if all bytes are zero
+    bool all_zero(uint16_t ofs, uint16_t size);
+
+    // switch to next sector for writing
+    bool switch_sectors(void);
+};

libraries/AP_FlashStorage/examples/FlashTest/FlashTest.cpp

@@ -0,0 +1,155 @@
+//
+// Unit tests for the AP_Math rotations code
+//
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_FlashStorage/AP_FlashStorage.h>
+#include <stdio.h>
+
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+class FlashTest : public AP_HAL::HAL::Callbacks {
+public:
+    // HAL::Callbacks implementation.
+    void setup() override;
+    void loop() override;
+
+private:
+    static const uint32_t flash_sector_size = 128U * 1024U;
+
+    uint8_t mem_buffer[AP_FlashStorage::storage_size];
+    uint8_t mem_mirror[AP_FlashStorage::storage_size];
+
+    // flash buffer
+    uint8_t flash[2][flash_sector_size];
+
+    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);
+    
+    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)};
+
+    // write to storage and mem_mirror
+    void write(uint16_t offset, const uint8_t *data, uint16_t length);
+};
+
+bool FlashTest::flash_write(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length)
+{
+    if (sector > 1) {
+        AP_HAL::panic("FATAL: write to sector %u\n", (unsigned)sector);
+    }
+    if (offset + length > flash_sector_size) {
+        AP_HAL::panic("FATAL: write to sector %u at offset %u length %u\n",
+                      (unsigned)sector,
+                      (unsigned)offset,
+                      (unsigned)length);
+    }
+    uint8_t *b = &flash[sector][offset];
+    for (uint16_t i=0; i<length; i++) {
+        b[i] &= data[i];
+    }
+    return true;
+}
+
+bool FlashTest::flash_read(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length)
+{
+    if (sector > 1) {
+        AP_HAL::panic("FATAL: read from sector %u\n", (unsigned)sector);
+    }
+    if (offset + length > flash_sector_size) {
+        AP_HAL::panic("FATAL: read from sector %u at offset %u length %u\n",
+                      (unsigned)sector,
+                      (unsigned)offset,
+                      (unsigned)length);
+    }
+    memcpy(data, &flash[sector][offset], length);
+    return true;
+}
+
+bool FlashTest::flash_erase(uint8_t sector)
+{
+    if (sector > 1) {
+        AP_HAL::panic("FATAL: erase sector %u\n", (unsigned)sector);
+    }
+    memset(&flash[sector][0], 0xFF, flash_sector_size);
+    return true;
+}
+
+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 (!storage.init()) {
+            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");
+    }
+}
+
+/*
+ * test flash storage
+ */
+void FlashTest::setup(void)
+{
+    flash_erase(0);
+    flash_erase(1);
+    hal.console->printf("AP_FlashStorage test\n");
+
+    if (!storage.init()) {
+        AP_HAL::panic("Failed first init()");
+    }
+
+    // fill with 10k random writes
+    for (uint32_t i=0; i<50000000; i++) {
+        uint16_t ofs = get_random16() % sizeof(mem_buffer);
+        uint16_t length = get_random16() & 0x1F;
+        length = MIN(length, sizeof(mem_buffer) - ofs);
+        uint8_t data[length];
+        for (uint8_t j=0; j<length; j++) {
+            data[j] = get_random16() & 0xFF;
+        }
+        write(ofs, data, length);
+
+        if (i % 1000 == 0) {
+            if (memcmp(mem_buffer, mem_mirror, sizeof(mem_buffer)) != 0) {
+                AP_HAL::panic("FATAL: data mis-match at i=%u", i);
+            }
+        }
+    }
+
+    if (memcmp(mem_buffer, mem_mirror, sizeof(mem_buffer)) != 0) {
+        AP_HAL::panic("FATAL: data mis-match before re-init");
+    }
+    
+    // re-init
+    printf("re-init\n");
+    memset(mem_buffer, 0, sizeof(mem_buffer));
+    if (!storage.init()) {
+        AP_HAL::panic("Failed second init()");
+    }
+
+    if (memcmp(mem_buffer, mem_mirror, sizeof(mem_buffer)) != 0) {
+        AP_HAL::panic("FATAL: data mis-match");
+    }
+    AP_HAL::panic("TEST PASSED");
+}
+
+void FlashTest::loop(void)
+{
+    hal.console->printf("loop\n");
+}
+
+FlashTest flashtest;
+
+AP_HAL_MAIN_CALLBACKS(&flashtest);

libraries/AP_FlashStorage/examples/FlashTest/wscript

@@ -0,0 +1,7 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+def build(bld):
+    bld.ap_example(
+        use='ap',
+    )