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

@@ -172,6 +172,7 @@ void Storage::write_block(uint16_t loc, const void *src, size_t n)
     }
     if (memcmp(src, &_buffer[loc], n) != 0) {
         _storage_open();
+        WITH_SEMAPHORE(sem);
         memcpy(&_buffer[loc], src, n);
         _mark_dirty(loc, n);
     }
@@ -200,12 +201,19 @@ 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 (_initialisedType == StorageBackend::FRAM) {
-        if (fram.write(CH_STORAGE_LINE_SIZE*i, &_buffer[CH_STORAGE_LINE_SIZE*i], CH_STORAGE_LINE_SIZE)) {
-            _dirty_mask.clear(i);
+        if (fram.write(CH_STORAGE_LINE_SIZE*i, tmpline, CH_STORAGE_LINE_SIZE)) {
+            write_ok = true;
         }
-        return;
     }
 #endif
 
@@ -221,17 +229,31 @@ void Storage::_timer_tick(void)
         if (AP::FS().fsync(log_fd) != 0) {
             return;
         }
-        _dirty_mask.clear(i);
-        return;
+        write_ok = true;
     }
 #endif
 
 #ifdef STORAGE_FLASH_PAGE
     if (_initialisedType == StorageBackend::Flash) {
         // save to storage backend
-        _flash_write(i);
+        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);
+        }
+    }
 }
 
 /*
@@ -255,13 +277,12 @@ void Storage::_flash_load(void)
 /*
   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)) {
-        // mark the line clean
-        _dirty_mask.clear(line);
-    }
+    return _flash.write(line*CH_STORAGE_LINE_SIZE, CH_STORAGE_LINE_SIZE);
+#else
+    return false;
 #endif
 }
 

libraries/AP_HAL_ChibiOS/Storage.h

@@ -61,6 +61,8 @@ private:
     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);
@@ -81,7 +83,7 @@ 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;