HAL_QURT: added persistent storage

libraries/AP_HAL_QURT/HAL_QURT_Class.cpp

@@ -51,7 +51,7 @@ static UARTDriver serial1Driver("/dev/gcs");
 
 static SPIDeviceManager spiDeviceManager;
 static Empty::AnalogIn analogIn;
-static Empty::Storage storageDriver;
+static Storage storageDriver;
 static Empty::GPIO gpioDriver;
 static Empty::RCInput rcinDriver;
 static RCOutput rcoutDriver;

libraries/AP_HAL_QURT/Scheduler.cpp

@@ -310,6 +310,9 @@ void *Scheduler::_io_thread(void *arg)
 
         // run registered IO processes
         sched->_run_io();
+
+        // update storage
+        hal.storage->_timer_tick();
     }
     return nullptr;
 }

libraries/AP_HAL_QURT/Storage.cpp

@@ -1,49 +1,198 @@
-#include <AP_HAL/AP_HAL.h>
-
-#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
+#include "Storage.h"
 
-#include <assert.h>
-#include <sys/types.h>
-#include <sys/stat.h>
 #include <fcntl.h>
-#include <unistd.h>
-#include <errno.h>
 #include <stdio.h>
-#include "Storage.h"
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include <AP_HAL/AP_HAL.h>
 
 using namespace QURT;
 
+#define QURT_STORAGE_MAX_WRITE 512
+#define QURT_STORAGE_LINE_SHIFT 9
+#define QURT_STORAGE_LINE_SIZE (1<<QURT_STORAGE_LINE_SHIFT)
+#define QURT_STORAGE_NUM_LINES (QURT_STORAGE_SIZE/QURT_STORAGE_LINE_SIZE)
+
 /*
-  This stores 'eeprom' data on the filesystem, with a 16k size
-  
-  Data is written on the ARM frontend via a RPC call
+  This stores 'eeprom' data on the SD card, with a 4k size, and a
+  in-memory buffer. This keeps the latency down.
  */
 
+// name the storage file after the sketch so you can use the same board
+// card for ArduCopter and ArduPlane
+#define STORAGE_FILE "APM/" AP_BUILD_TARGET_NAME ".stg"
+
 extern const AP_HAL::HAL& hal;
 
-volatile bool Storage::dirty;
-uint8_t Storage::buffer[QURT_STORAGE_SIZE];
-Semaphore Storage::lock;
+bool Storage::_storage_create(void)
+{
+    int fd = -1;
 
-void Storage::read_block(void *dst, uint16_t loc, size_t n) 
+    fd = open(STORAGE_FILE, O_WRONLY|O_CREAT|O_TRUNC, 0755);
+    if (fd == -1) {
+        AP_HAL::panic("Failed to open storage: %s", STORAGE_FILE);
+        return false;
+    }
+
+    // take up all needed space
+    if (write(fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) {
+        AP_HAL::panic("Failed to init %s", STORAGE_FILE);
+        goto fail;
+    }
+
+    // ensure the directory is updated with the new size
+    close(fd);
+
+    return true;
+
+fail:
+    if (fd != -1) {
+        close(fd);
+    }
+    return false;
+}
+
+void Storage::init()
 {
-    if (loc >= sizeof(buffer)-(n-1)) {
+    if (_initialised) {
         return;
     }
-    memcpy(dst, &buffer[loc], n);
+
+    _dirty_mask = 0;
+
+    int fd = open(STORAGE_FILE, O_RDWR);
+    if (fd == -1) {
+        _storage_create();
+        fd = open(STORAGE_FILE, O_RDWR);
+        if (fd == -1) {
+            AP_HAL::panic("Failed to create %s", STORAGE_FILE);
+        }
+    }
+
+    ssize_t ret = read(fd, _buffer, sizeof(_buffer));
+
+    if (ret != sizeof(_buffer)) {
+        close(fd);
+        _storage_create();
+        fd = open(STORAGE_FILE, O_RDWR);
+        if (fd == -1) {
+            AP_HAL::panic("Failed to open %s", STORAGE_FILE);
+        }
+        if (read(fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) {
+            AP_HAL::panic("Failed to read %s", STORAGE_FILE);
+        }
+    }
+
+    _fd = fd;
+    _initialised = true;
 }
 
-void Storage::write_block(uint16_t loc, const void *src, size_t n) 
+/*
+  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)
+{
+    if (length == 0) {
+        return;
+    }
+    uint16_t end = loc + length - 1;
+    for (uint8_t line=loc>>QURT_STORAGE_LINE_SHIFT;
+         line <= end>>QURT_STORAGE_LINE_SHIFT;
+         line++) {
+        _dirty_mask |= 1U << line;
+    }
+}
+
+void Storage::read_block(void *dst, uint16_t loc, size_t n)
+{
+    if (loc >= sizeof(_buffer)-(n-1)) {
+        return;
+    }
+    init();
+    memcpy(dst, &_buffer[loc], n);
+}
+
+void Storage::write_block(uint16_t loc, const void *src, size_t n)
+{
+    if (loc >= sizeof(_buffer)-(n-1)) {
+        return;
+    }
+    if (memcmp(src, &_buffer[loc], n) != 0) {
+        init();
+        memcpy(&_buffer[loc], src, n);
+        _mark_dirty(loc, n);
+    }
+}
+
+void Storage::_timer_tick(void)
 {
-    if (loc >= sizeof(buffer)-(n-1)) {
+    if (!_initialised || _dirty_mask == 0 || _fd == -1) {
+        return;
+    }
+
+    // write out the first dirty set of lines. We don't write more
+    // than one to keep the latency of this call to a minimum
+    uint8_t i, n;
+    for (i=0; i<QURT_STORAGE_NUM_LINES; i++) {
+        if (_dirty_mask & (1<<i)) {
+            break;
+        }
+    }
+    if (i == QURT_STORAGE_NUM_LINES) {
+        // this shouldn't be possible
         return;
     }
-    if (memcmp(src, &buffer[loc], n) != 0) {
-        lock.take(HAL_SEMAPHORE_BLOCK_FOREVER);
-        memcpy(&buffer[loc], src, n);
-        dirty = true;
-        lock.give();
+    uint32_t write_mask = (1U<<i);
+    // see how many lines to write
+    for (n=1; (i+n) < QURT_STORAGE_NUM_LINES &&
+             n < (QURT_STORAGE_MAX_WRITE>>QURT_STORAGE_LINE_SHIFT); n++) {
+        if (!(_dirty_mask & (1<<(n+i)))) {
+            break;
+        }
+        // mark that line clean
+        write_mask |= (1<<(n+i));
+    }
+
+    /*
+      write the lines. This also updates _dirty_mask. Note that
+      because this is a SCHED_FIFO thread it will not be preempted
+      by the main task except during blocking calls. This means we
+      don't need a semaphore around the _dirty_mask updates.
+     */
+    if (lseek(_fd, i<<QURT_STORAGE_LINE_SHIFT, SEEK_SET) == (i<<QURT_STORAGE_LINE_SHIFT)) {
+        _dirty_mask &= ~write_mask;
+        if (write(_fd, &_buffer[i<<QURT_STORAGE_LINE_SHIFT], n<<QURT_STORAGE_LINE_SHIFT) != n<<QURT_STORAGE_LINE_SHIFT) {
+            // write error - likely EINTR
+            _dirty_mask |= write_mask;
+            close(_fd);
+            _fd = -1;
+        }
+#if 0
+        if (_dirty_mask == 0) {
+            if (fsync(_fd) != 0) {
+                close(_fd);
+                _fd = -1;
+            }
+        }
+#endif
     }
 }
 
-#endif // CONFIG_HAL_BOARD
+/*
+  get storage size and ptr
+ */
+bool Storage::get_storage_ptr(void *&ptr, size_t &size)
+{
+    if (!_initialised) {
+        return false;
+    }
+    ptr = _buffer;
+    size = sizeof(_buffer);
+    return true;
+}

libraries/AP_HAL_QURT/Storage.h

@@ -1,27 +1,45 @@
 #pragma once
 
 #include <AP_HAL/AP_HAL.h>
-#include "AP_HAL_QURT_Namespace.h"
-#include "Semaphores.h"
-#include <stdio.h>
 
 #define QURT_STORAGE_SIZE HAL_STORAGE_SIZE
 
-class QURT::Storage : public AP_HAL::Storage
+namespace QURT {
+
+class Storage : public AP_HAL::Storage
 {
 public:
-    Storage() {}
+    Storage() : _fd(-1),_dirty_mask(0) { }
 
     static Storage *from(AP_HAL::Storage *storage) {
         return static_cast<Storage*>(storage);
     }
 
-    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);
 
-    static volatile bool dirty;
-    static uint8_t buffer[QURT_STORAGE_SIZE];
-    static Semaphore lock;
+    void init() override;
+
+    uint8_t  read_byte(uint16_t loc);
+    uint16_t read_word(uint16_t loc);
+    uint32_t read_dword(uint16_t loc);
+    void     read_block(void *dst, uint16_t src, size_t n) override;
+
+    void write_byte(uint16_t loc, uint8_t value);
+    void write_word(uint16_t loc, uint16_t value);
+    void write_dword(uint16_t loc, uint32_t value);
+    void write_block(uint16_t dst, const void* src, size_t n) override;
+
+    bool get_storage_ptr(void *&ptr, size_t &size) override;
+
+    virtual void _timer_tick(void) override;
+
+protected:
+    void _mark_dirty(uint16_t loc, uint16_t length);
+    bool _storage_create(void);
+
+    int _fd;
+    volatile bool _initialised;
+    volatile uint32_t _dirty_mask;
+    uint8_t _buffer[QURT_STORAGE_SIZE];
 };
 
+}