HAL_SITL: enable optional storage in flash

this brings storage in SITL very close to storage in ChibiOS

libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h

@@ -4,7 +4,7 @@ namespace HALSITL {
 class UARTDriver;
 class Scheduler;
 class SITL_State;
-class EEPROMStorage;
+class Storage;
 class AnalogIn;
 class RCInput;
 class RCOutput;

libraries/AP_HAL_SITL/HAL_SITL_Class.cpp

@@ -23,7 +23,7 @@
 
 using namespace HALSITL;
 
-static EEPROMStorage sitlEEPROMStorage;
+static Storage sitlStorage;
 static SITL_State sitlState;
 static Scheduler sitlScheduler(&sitlState);
 static RCInput  sitlRCInput(&sitlState);
@@ -58,7 +58,7 @@ HAL_SITL::HAL_SITL() :
         &i2c_mgr_instance,
         &emptySPI,          /* spi */
         &sitlAnalogIn,      /* analogin */
-        &sitlEEPROMStorage, /* storage */
+        &sitlStorage, /* storage */
         &sitlUart0Driver,   /* console */
         &sitlGPIO,          /* gpio */
         &sitlRCInput,       /* rcinput */

libraries/AP_HAL_SITL/Scheduler.cpp

@@ -199,6 +199,7 @@ void Scheduler::_run_io_procs()
     hal.uartE->_timer_tick();
     hal.uartF->_timer_tick();
     hal.uartG->_timer_tick();
+    hal.storage->_timer_tick();
 
     check_thread_stacks();
 }

libraries/AP_HAL_SITL/Storage.cpp

@@ -1,35 +1,290 @@
 #include <AP_HAL/AP_HAL.h>
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 
 #include <assert.h>
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <fcntl.h>
 #include <unistd.h>
-
 #include "Storage.h"
+
 using namespace HALSITL;
 
-void EEPROMStorage::_eeprom_open(void)
+extern const AP_HAL::HAL& hal;
+
+void Storage::_storage_open(void)
+{
+    if (_initialised) {
+        return;
+    }
+
+#if STORAGE_USE_POSIX
+    // if we have failed filesystem init don't try again
+    if (log_fd == -1) {
+        return;
+    }
+#endif
+        
+    _dirty_mask.clearall();
+
+#if STORAGE_USE_FLASH
+    // load from storage backend
+    _flash_load();
+#elif STORAGE_USE_POSIX
+    log_fd = open(HAL_STORAGE_FILE, O_RDWR|O_CREAT, 0644);
+    if (log_fd == -1) {
+        hal.console->printf("open failed of " HAL_STORAGE_FILE "\n");
+        return;
+    }
+    int ret = read(log_fd, _buffer, HAL_STORAGE_SIZE);
+    if (ret < 0) {
+        hal.console->printf("read failed for " HAL_STORAGE_FILE "\n");
+        close(log_fd);
+        log_fd = -1;
+        return;
+    }
+    // pre-fill to full size
+    if (lseek(log_fd, ret, SEEK_SET) != ret ||
+        write(log_fd, &_buffer[ret], HAL_STORAGE_SIZE-ret) != HAL_STORAGE_SIZE-ret) {
+        hal.console->printf("setup failed for " HAL_STORAGE_FILE "\n");
+        close(log_fd);
+        log_fd = -1;
+        return;        
+    }
+    using_filesystem = true;
+#else
+#error "No storage system enabled"
+#endif
+
+    _initialised = true;
+}
+
+/*
+  mark some lines as dirty. Note that there is no attempt to avoid
+  the race condition between this code and the _timer_tick() code
+  below, which both update _dirty_mask. If we lose the race then the
+  result is that a line is written more than once, but it won't result
+  in a line not being written.
+*/
+void Storage::_mark_dirty(uint16_t loc, uint16_t length)
+{
+    uint16_t end = loc + length;
+    for (uint16_t line=loc>>STORAGE_LINE_SHIFT;
+         line <= end>>STORAGE_LINE_SHIFT;
+         line++) {
+        _dirty_mask.set(line);
+    }
+}
+
+void Storage::read_block(void *dst, uint16_t loc, size_t n)
 {
-    if (_eeprom_fd == -1) {
-        _eeprom_fd = open("eeprom.bin", O_RDWR|O_CREAT|O_CLOEXEC, 0777);
-        assert(ftruncate(_eeprom_fd, HAL_STORAGE_SIZE) == 0);
+    if (loc >= sizeof(_buffer)-(n-1)) {
+        return;
     }
+    _storage_open();
+    memcpy(dst, &_buffer[loc], n);
 }
 
-void EEPROMStorage::read_block(void *dst, uint16_t src, size_t n)
+void Storage::write_block(uint16_t loc, const void *src, size_t n)
 {
-    assert(src < HAL_STORAGE_SIZE && src + n <= HAL_STORAGE_SIZE);
-    _eeprom_open();
-    assert(pread(_eeprom_fd, dst, n, src) == (ssize_t)n);
+    if (loc >= sizeof(_buffer)-(n-1)) {
+        return;
+    }
+    if (memcmp(src, &_buffer[loc], n) != 0) {
+        _storage_open();
+        memcpy(&_buffer[loc], src, n);
+        _mark_dirty(loc, n);
+    }
 }
 
-void EEPROMStorage::write_block(uint16_t dst, const void *src, size_t n)
+void Storage::_timer_tick(void)
 {
-    assert(dst < HAL_STORAGE_SIZE);
-    _eeprom_open();
-    assert(pwrite(_eeprom_fd, src, n, dst) == (ssize_t)n);
+    if (!_initialised) {
+        return;
+    }
+    if (_dirty_mask.empty()) {
+        _last_empty_ms = AP_HAL::millis();
+        return;
+    }
+
+    // write out the first dirty line. We don't write more
+    // than one to keep the latency of this call to a minimum
+    uint16_t i;
+    for (i=0; i<STORAGE_NUM_LINES; i++) {
+        if (_dirty_mask.get(i)) {
+            break;
+        }
+    }
+    if (i == STORAGE_NUM_LINES) {
+        // this shouldn't be possible
+        return;
+    }
+
+#if STORAGE_USE_POSIX
+    if (using_filesystem && log_fd != -1) {
+        uint32_t offset = STORAGE_LINE_SIZE*i;
+        if (lseek(log_fd, offset, SEEK_SET) != offset) {
+            return;
+        }
+        if (write(log_fd, &_buffer[offset], STORAGE_LINE_SIZE) != STORAGE_LINE_SIZE) {
+            return;
+        }
+        _dirty_mask.clear(i);
+        return;
+    } 
+#endif
+    
+#if STORAGE_USE_FLASH
+    // save to storage backend
+    _flash_write(i);
+#endif
 }
 
+/*
+  load all data from flash
+ */
+void Storage::_flash_load(void)
+{
+#if STORAGE_USE_FLASH
+    if (!_flash.init()) {
+        AP_HAL::panic("unable to init flash storage");
+    }
+#else
+    AP_HAL::panic("unable to init storage");
 #endif
+}
+
+/*
+  write one storage line. This also updates _dirty_mask. 
+*/
+void Storage::_flash_write(uint16_t line)
+{
+#if STORAGE_USE_FLASH
+    if (_flash.write(line*STORAGE_LINE_SIZE, STORAGE_LINE_SIZE)) {
+        // mark the line clean
+        _dirty_mask.clear(line);
+    }
+#endif
+}
+
+
+#if STORAGE_USE_FLASH
+/*
+  emulate writing to flash
+ */
+static int flash_fd = -1;
+
+static uint32_t sitl_flash_getpageaddr(uint32_t page)
+{
+    return page * HAL_STORAGE_SIZE;
+}
+
+static void sitl_flash_open(void)
+{
+    if (flash_fd == -1) {
+        flash_fd = open("flash.dat", O_RDWR, 0644);
+        if (flash_fd == -1) {
+            flash_fd = open("flash.dat", O_RDWR|O_CREAT, 0644);
+            if (flash_fd == -1) {
+                AP_HAL::panic("Failed to open flash.dat");
+            }
+            if (ftruncate(flash_fd, 2*HAL_STORAGE_SIZE) != 0) {
+                AP_HAL::panic("Failed to create flash.dat");
+            }
+            uint8_t fill[HAL_STORAGE_SIZE*2];
+            memset(fill, 0xff, sizeof(fill));
+            pwrite(flash_fd, fill, sizeof(fill), 0);
+        }
+    }
+}
+
+static bool sitl_flash_write(uint32_t addr, const uint8_t *data, uint32_t length)
+{
+    sitl_flash_open();
+    uint8_t old[length];
+    if (pread(flash_fd, old, length, addr) != length) {
+        AP_HAL::panic("Failed to read flash.dat");
+    }
+    // check that we are only ever clearing bits (real flash storage can only ever clear bits,
+    // except for an erase
+    for (uint32_t i=0; i<length; i++) {
+        if (data[i] & ~old[i]) {
+            AP_HAL::panic("Attempt to set flash byte 0x%02x from 0x%02x at %u\n", data[i], old[i], addr+i);
+        }
+    }
+    return pwrite(flash_fd, data, length, addr) == length;
+}
+
+static bool sitl_flash_read(uint32_t addr, uint8_t *data, uint32_t length)
+{
+    sitl_flash_open();
+    return pread(flash_fd, data, length, addr) == length;
+}
+
+static bool sitl_flash_erasepage(uint32_t page)
+{
+    uint8_t fill[HAL_STORAGE_SIZE];
+    memset(fill, 0xff, sizeof(fill));
+    sitl_flash_open();
+    bool ret = pwrite(flash_fd, fill, sizeof(fill), page * HAL_STORAGE_SIZE) == sizeof(fill);
+    printf("erase %u -> %u\n", page, ret);
+    return ret;
+}
+
+/*
+  callback to write data to flash
+ */
+bool Storage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length)
+{
+    size_t base_address = sitl_flash_getpageaddr(sector);
+    bool ret = sitl_flash_write(base_address+offset, data, length);
+    if (!ret && _flash_erase_ok()) {
+        // we are getting flash write errors while disarmed. Try
+        // re-writing all of flash
+        uint32_t now = AP_HAL::millis();
+        if (now - _last_re_init_ms > 5000) {
+            _last_re_init_ms = now;
+            bool ok = _flash.re_initialise();
+            hal.console->printf("Storage: failed at %u:%u for %u - re-init %u\n",
+                                (unsigned)sector, (unsigned)offset, (unsigned)length, (unsigned)ok);
+        }
+    }
+    return ret;
+}
+
+/*
+  callback to read data from flash
+ */
+bool Storage::_flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length)
+{
+    size_t base_address = sitl_flash_getpageaddr(sector);
+    return sitl_flash_read(base_address+offset, data, length);
+}
+
+/*
+  callback to erase flash sector
+ */
+bool Storage::_flash_erase_sector(uint8_t sector)
+{
+    return sitl_flash_erasepage(sector);
+}
+
+/*
+  callback to check if erase is allowed
+ */
+bool Storage::_flash_erase_ok(void)
+{
+    // only allow erase while disarmed
+    return !hal.util->get_soft_armed();
+}
+#endif // STORAGE_USE_FLASH
+
+/*
+  consider storage healthy if we have nothing to write sometime in the
+  last 2 seconds
+ */
+bool Storage::healthy(void)
+{
+    return _initialised && AP_HAL::millis() - _last_empty_ms < 2000;
+}
+
+

libraries/AP_HAL_SITL/Storage.h

@@ -1,18 +1,70 @@
 #pragma once
 
 #include <AP_HAL/AP_HAL.h>
+#include <AP_Common/Bitmask.h>
 #include "AP_HAL_SITL_Namespace.h"
+#include <AP_FlashStorage/AP_FlashStorage.h>
 
-class HALSITL::EEPROMStorage : public AP_HAL::Storage {
+#ifndef HAL_STORAGE_FILE
+#define HAL_STORAGE_FILE "eeprom.bin"
+#endif
+
+// define which storage system to use. This allows us to test flash storage with --sitl-flash-storage
+// configure option
+#ifndef STORAGE_USE_FLASH
+#define STORAGE_USE_FLASH 0
+#endif
+
+#define STORAGE_USE_POSIX 1
+
+#define STORAGE_LINE_SHIFT 3
+
+#define STORAGE_LINE_SIZE (1<<STORAGE_LINE_SHIFT)
+#define STORAGE_NUM_LINES (HAL_STORAGE_SIZE/STORAGE_LINE_SIZE)
+
+class HALSITL::Storage : public AP_HAL::Storage {
 public:
-    EEPROMStorage() {
-        _eeprom_fd = -1;
-    }
-    void init() {}
-    void read_block(void *dst, uint16_t src, size_t n);
-    void write_block(uint16_t dst, const void* src, size_t n);
+    void init() override {}
+    void read_block(void *dst, uint16_t src, size_t n) override;
+    void write_block(uint16_t dst, const void* src, size_t n) override;
+
+    void _timer_tick(void) override;
+    bool healthy(void) override;
 
 private:
-    int _eeprom_fd;
-    void _eeprom_open(void);
+    volatile bool _initialised;
+    void _storage_create(void);
+    void _storage_open(void);
+    void _save_backup(void);
+    void _mark_dirty(uint16_t loc, uint16_t length);
+    uint8_t _buffer[HAL_STORAGE_SIZE] __attribute__((aligned(4)));
+    Bitmask _dirty_mask{STORAGE_NUM_LINES};
+
+#if STORAGE_USE_FLASH
+    bool _flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length);
+    bool _flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length);
+    bool _flash_erase_sector(uint8_t sector);
+    bool _flash_erase_ok(void);
+#endif
+
+    bool _flash_failed;
+    uint32_t _last_re_init_ms;
+    uint32_t _last_empty_ms;
+
+#if STORAGE_USE_FLASH
+    AP_FlashStorage _flash{_buffer,
+            HAL_STORAGE_SIZE,
+            FUNCTOR_BIND_MEMBER(&Storage::_flash_write_data, bool, uint8_t, uint32_t, const uint8_t *, uint16_t),
+            FUNCTOR_BIND_MEMBER(&Storage::_flash_read_data, bool, uint8_t, uint32_t, uint8_t *, uint16_t),
+            FUNCTOR_BIND_MEMBER(&Storage::_flash_erase_sector, bool, uint8_t),
+            FUNCTOR_BIND_MEMBER(&Storage::_flash_erase_ok, bool)};
+#endif
+    
+    void _flash_load(void);
+    void _flash_write(uint16_t line);
+
+#if STORAGE_USE_POSIX
+    bool using_filesystem;
+    int log_fd;
+#endif
 };