HAL_ChibiOS: fixed race condition in storage write

we could mark a line as clean when it should be dirty if we lose a
race condition between storage thread and writer

libraries/AP_HAL_ChibiOS/Storage.cpp

@@ -18,9 +18,12 @@
 #include <AP_BoardConfig/AP_BoardConfig.h>
 
 #include "Storage.h"
+#include "HAL_ChibiOS_Class.h"
+#include "Scheduler.h"
 #include "hwdef/common/flash.h"
 #include <AP_Filesystem/AP_Filesystem.h>
 #include "sdcard.h"
+#include <stdio.h>
 
 using namespace ChibiOS;
 
@@ -41,66 +44,77 @@ extern const AP_HAL::HAL& hal;
 
 #define STORAGE_FLASH_RETRIES 5
 
+// by default don't allow fallback to sdcard for storage
+#ifndef HAL_RAMTRON_ALLOW_FALLBACK
+#define HAL_RAMTRON_ALLOW_FALLBACK 0
+#endif
+
 void Storage::_storage_open(void)
 {
-    if (_initialised) {
+    if (_initialisedType != StorageBackend::None) {
         return;
     }
 
-#ifdef USE_POSIX
-    // if we have failed filesystem init don't try again
-    if (log_fd == -1) {
-        return;
-    }
-#endif
-
     _dirty_mask.clearall();
 
 #if HAL_WITH_RAMTRON
-    using_fram = fram.init();
+    if (fram.init() && fram.read(0, _buffer, CH_STORAGE_SIZE)) {
-    if (using_fram) {
+        _save_backup();
-        if (!fram.read(0, _buffer, CH_STORAGE_SIZE)) {
+        _initialisedType = StorageBackend::FRAM;
+        ::printf("Initialised Storage type=%d\n", _initialisedType);
+        return;
+    }
+
+#if !HAL_RAMTRON_ALLOW_FALLBACK
+    AP_HAL::panic("Unable to init RAMTRON storage");
+#endif
+
+#endif // HAL_WITH_RAMTRON
+
+// allow for devices with no FRAM chip to fall through to other storage
+#ifdef STORAGE_FLASH_PAGE
+        // load from storage backend
+        _flash_load();
+        _save_backup();
+        _initialisedType = StorageBackend::Flash;
+#elif defined(USE_POSIX)
+        // if we have failed filesystem init don't try again
+        if (log_fd == -1) {
             return;
         }
-        _save_backup();
+
-        _initialised = true;
+        // use microSD based storage
-        return;
+        if (sdcard_retry()) {
-    }
+            log_fd = AP::FS().open(HAL_STORAGE_FILE, O_RDWR|O_CREAT);
-    // allow for FMUv3 with no FRAM chip, fall through to flash storage
+            if (log_fd == -1) {
+                ::printf("open failed of " HAL_STORAGE_FILE "\n");
+                return;
+            }
+            int ret = AP::FS().read(log_fd, _buffer, CH_STORAGE_SIZE);
+            if (ret < 0) {
+                ::printf("read failed for " HAL_STORAGE_FILE "\n");
+                AP::FS().close(log_fd);
+                log_fd = -1;
+                return;
+            }
+            // pre-fill to full size
+            if (AP::FS().lseek(log_fd, ret, SEEK_SET) != ret ||
+                AP::FS().write(log_fd, &_buffer[ret], CH_STORAGE_SIZE-ret) != CH_STORAGE_SIZE-ret) {
+                ::printf("setup failed for " HAL_STORAGE_FILE "\n");
+                AP::FS().close(log_fd);
+                log_fd = -1;
+                return;
+            }
+            _save_backup();
+            _initialisedType = StorageBackend::SDCard;
+        }
 #endif
 
-#ifdef STORAGE_FLASH_PAGE
+    if (_initialisedType != StorageBackend::None) {
-    // load from storage backend
+        ::printf("Initialised Storage type=%d\n", _initialisedType);
-    _flash_load();
+    } else {
-#elif defined(USE_POSIX)
+        AP_HAL::panic("Unable to init Storage backend");
-    // allow for fallback to microSD based storage
-    sdcard_retry();
-
-    log_fd = AP::FS().open(HAL_STORAGE_FILE, O_RDWR|O_CREAT);
-    if (log_fd == -1) {
-        hal.console->printf("open failed of " HAL_STORAGE_FILE "\n");
-        return;
     }
-    int ret = AP::FS().read(log_fd, _buffer, CH_STORAGE_SIZE);
-    if (ret < 0) {
-        hal.console->printf("read failed for " HAL_STORAGE_FILE "\n");
-        AP::FS().close(log_fd);
-        log_fd = -1;
-        return;
-    }
-    // pre-fill to full size
-    if (AP::FS().lseek(log_fd, ret, SEEK_SET) != ret ||
-        AP::FS().write(log_fd, &_buffer[ret], CH_STORAGE_SIZE-ret) != CH_STORAGE_SIZE-ret) {
-        hal.console->printf("setup failed for " HAL_STORAGE_FILE "\n");
-        AP::FS().close(log_fd);
-        log_fd = -1;
-        return;
-    }
-    using_filesystem = true;
-#endif
-
-    _save_backup();
-    _initialised = true;
 }
 
 /*
@@ -112,11 +126,12 @@ void Storage::_save_backup(void)
 {
 #ifdef USE_POSIX
     // allow for fallback to microSD based storage
-    sdcard_retry();
+    if (sdcard_retry()) {
-    int fd = AP::FS().open(HAL_STORAGE_BACKUP_FILE, O_WRONLY|O_CREAT|O_TRUNC);
+        int fd = AP::FS().open(HAL_STORAGE_BACKUP_FILE, O_WRONLY|O_CREAT|O_TRUNC);
-    if (fd != -1) {
+        if (fd != -1) {
-        AP::FS().write(fd, _buffer, CH_STORAGE_SIZE);
+            AP::FS().write(fd, _buffer, CH_STORAGE_SIZE);
-        AP::FS().close(fd);
+            AP::FS().close(fd);
+        }
     }
 #endif
 }
@@ -143,7 +158,7 @@ void Storage::_mark_dirty(uint16_t loc, uint16_t length)
 
 void Storage::read_block(void *dst, uint16_t loc, size_t n)
 {
-    if (loc >= sizeof(_buffer)-(n-1)) {
+    if ((n > sizeof(_buffer)) || (loc > (sizeof(_buffer) - n))) {
         return;
     }
     _storage_open();
@@ -152,11 +167,12 @@ void Storage::read_block(void *dst, uint16_t loc, size_t n)
 
 void Storage::write_block(uint16_t loc, const void *src, size_t n)
 {
-    if (loc >= sizeof(_buffer)-(n-1)) {
+    if ((n > sizeof(_buffer)) || (loc > (sizeof(_buffer) - n))) {
         return;
     }
     if (memcmp(src, &_buffer[loc], n) != 0) {
         _storage_open();
+        WITH_SEMAPHORE(sem);
         memcpy(&_buffer[loc], src, n);
         _mark_dirty(loc, n);
     }
@@ -164,7 +180,7 @@ void Storage::write_block(uint16_t loc, const void *src, size_t n)
 
 void Storage::_timer_tick(void)
 {
-    if (!_initialised) {
+    if (_initialisedType == StorageBackend::None) {
         return;
     }
     if (_dirty_mask.empty()) {
@@ -185,17 +201,24 @@ void Storage::_timer_tick(void)
         return;
     }
 
+    {
+        // take a copy of the line we are writing with a semaphore held
+        WITH_SEMAPHORE(sem);
+        memcpy(tmpline, &_buffer[CH_STORAGE_LINE_SIZE*i], CH_STORAGE_LINE_SIZE);
+    }
+
+    bool write_ok = false;
+
 #if HAL_WITH_RAMTRON
-    if (using_fram) {
+    if (_initialisedType == StorageBackend::FRAM) {
-        if (fram.write(CH_STORAGE_LINE_SIZE*i, &_buffer[CH_STORAGE_LINE_SIZE*i], CH_STORAGE_LINE_SIZE)) {
+        if (fram.write(CH_STORAGE_LINE_SIZE*i, tmpline, CH_STORAGE_LINE_SIZE)) {
-            _dirty_mask.clear(i);
+            write_ok = true;
         }
-        return;
     }
 #endif
 
 #ifdef USE_POSIX
-    if (using_filesystem && log_fd != -1) {
+    if ((_initialisedType == StorageBackend::SDCard) && log_fd != -1) {
         uint32_t offset = CH_STORAGE_LINE_SIZE*i;
         if (AP::FS().lseek(log_fd, offset, SEEK_SET) != offset) {
             return;
@@ -206,15 +229,31 @@ void Storage::_timer_tick(void)
         if (AP::FS().fsync(log_fd) != 0) {
             return;
         }
-        _dirty_mask.clear(i);
+        write_ok = true;
-        return;
     }
 #endif
 
 #ifdef STORAGE_FLASH_PAGE
-    // save to storage backend
+    if (_initialisedType == StorageBackend::Flash) {
-    _flash_write(i);
+        // save to storage backend
+        if (_flash_write(i)) {
+            write_ok = true;
+        }
+    }
 #endif
+
+    if (write_ok) {
+        WITH_SEMAPHORE(sem);
+        // while holding the semaphore we check if the copy of the
+        // line is different from the original line. If it is
+        // different then someone has re-dirtied the line while we
+        // were writing it, in which case we should not mark it
+        // clean. If it matches then we know we can mark the line as
+        // clean
+        if (memcmp(tmpline, &_buffer[CH_STORAGE_LINE_SIZE*i], CH_STORAGE_LINE_SIZE) == 0) {
+            _dirty_mask.clear(i);
+        }
+    }
 }
 
 /*
@@ -225,26 +264,25 @@ void Storage::_flash_load(void)
 #ifdef STORAGE_FLASH_PAGE
     _flash_page = STORAGE_FLASH_PAGE;
 
-    hal.console->printf("Storage: Using flash pages %u and %u\n", _flash_page, _flash_page+1);
+    ::printf("Storage: Using flash pages %u and %u\n", _flash_page, _flash_page+1);
 
     if (!_flash.init()) {
-        AP_HAL::panic("unable to init flash storage");
+        AP_HAL::panic("Unable to init flash storage");
     }
 #else
-    AP_HAL::panic("unable to init storage");
+    AP_HAL::panic("Unable to init storage");
 #endif
 }
 
 /*
   write one storage line. This also updates _dirty_mask.
 */
-void Storage::_flash_write(uint16_t line)
+bool Storage::_flash_write(uint16_t line)
 {
 #ifdef STORAGE_FLASH_PAGE
-    if (_flash.write(line*CH_STORAGE_LINE_SIZE, CH_STORAGE_LINE_SIZE)) {
+    return _flash.write(line*CH_STORAGE_LINE_SIZE, CH_STORAGE_LINE_SIZE);
-        // mark the line clean
+#else
-        _dirty_mask.clear(line);
+    return false;
-    }
 #endif
 }
 
@@ -268,8 +306,8 @@ bool Storage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *
         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",
+            ::printf("Storage: failed at %u:%u for %u - re-init %u\n",
-                                (unsigned)sector, (unsigned)offset, (unsigned)length, (unsigned)ok);
+                     (unsigned)sector, (unsigned)offset, (unsigned)length, (unsigned)ok);
         }
     }
     return false;
@@ -283,10 +321,14 @@ bool Storage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *
  */
 bool Storage::_flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length)
 {
+#ifdef STORAGE_FLASH_PAGE
     size_t base_address = hal.flash->getpageaddr(_flash_page+sector);
     const uint8_t *b = ((const uint8_t *)base_address)+offset;
     memcpy(data, b, length);
     return true;
+#else
+    return false;
+#endif
 }
 
 /*
@@ -294,6 +336,9 @@ bool Storage::_flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, u
  */
 bool Storage::_flash_erase_sector(uint8_t sector)
 {
+#ifdef STORAGE_FLASH_PAGE
+    // erasing a page can take long enough that USB may not initialise properly if it happens
+    // while the host is connecting. Only do a flash erase if we have been up for more than 4s
     for (uint8_t i=0; i<STORAGE_FLASH_RETRIES; i++) {
         if (hal.flash->erasepage(_flash_page+sector)) {
             return true;
@@ -301,6 +346,9 @@ bool Storage::_flash_erase_sector(uint8_t sector)
         hal.scheduler->delay(1);
     }
     return false;
+#else
+    return false;
+#endif
 }
 
 /*
@@ -318,7 +366,8 @@ bool Storage::_flash_erase_ok(void)
  */
 bool Storage::healthy(void)
 {
-    return _initialised && AP_HAL::millis() - _last_empty_ms < 2000;
+    return ((_initialisedType != StorageBackend::None) &&
+            (AP_HAL::millis() - _last_empty_ms < 2000u));
 }
 
 #endif // HAL_USE_EMPTY_STORAGE

libraries/AP_HAL_ChibiOS/Storage.h

@@ -34,6 +34,9 @@
 #define CH_STORAGE_LINE_SIZE (1<<CH_STORAGE_LINE_SHIFT)
 #define CH_STORAGE_NUM_LINES (CH_STORAGE_SIZE/CH_STORAGE_LINE_SIZE)
 
+static_assert(CH_STORAGE_SIZE % CH_STORAGE_LINE_SIZE == 0,
+              "Storage is not multiple of line size");
+
 class ChibiOS::Storage : public AP_HAL::Storage {
 public:
     void init() override {}
@@ -44,13 +47,21 @@ public:
     bool healthy(void) override;
 
 private:
-    volatile bool _initialised;
+    enum class StorageBackend: uint8_t {
+        None,
+        FRAM,
+        Flash,
+        SDCard,
+    };
+    StorageBackend _initialisedType = StorageBackend::None;
     void _storage_create(void);
     void _storage_open(void);
     void _save_backup(void);
     void _mark_dirty(uint16_t loc, uint16_t length);
     uint8_t _buffer[CH_STORAGE_SIZE] __attribute__((aligned(4)));
     Bitmask<CH_STORAGE_NUM_LINES> _dirty_mask;
+    HAL_Semaphore sem;
+    uint8_t tmpline[CH_STORAGE_LINE_SIZE];
 
     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);
@@ -71,14 +82,12 @@ private:
 #endif
 
     void _flash_load(void);
-    void _flash_write(uint16_t line);
+    bool _flash_write(uint16_t line);
 
 #if HAL_WITH_RAMTRON
     AP_RAMTRON fram;
-    bool using_fram;
 #endif
 #ifdef USE_POSIX
-    bool using_filesystem;
     int log_fd;
 #endif
 };