HAL_ChibiOS: improved selection of storage backend

libraries/AP_HAL_ChibiOS/Storage.cpp

@@ -42,41 +42,47 @@ 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) {
-        if (!fram.read(0, _buffer, CH_STORAGE_SIZE)) {
-            return;
-        }
         _save_backup();
-        _initialised = true;
+        _initialisedType = StorageBackend::FRAM;
+        ::printf("Initialised Storage type=%d\n", _initialisedType);
         return;
     }
-    // allow for FMUv3 with no FRAM chip, fall through to flash storage
+
+#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)
-    // allow for fallback to microSD based storage
+        // if we have failed filesystem init don't try again
-    sdcard_retry();
+        if (log_fd == -1) {
+            return;
+        }
 
+        // use microSD based storage
+        if (sdcard_retry()) {
             log_fd = AP::FS().open(HAL_STORAGE_FILE, O_RDWR|O_CREAT);
             if (log_fd == -1) {
                 ::printf("open failed of " HAL_STORAGE_FILE "\n");
@@ -97,11 +103,16 @@ void Storage::_storage_open(void)
                 log_fd = -1;
                 return;
             }
-    using_filesystem = true;
+            _save_backup();
+            _initialisedType = StorageBackend::SDCard;
+        }
 #endif
 
-    _save_backup();
+    if (_initialisedType != StorageBackend::None) {
-    _initialised = true;
+        ::printf("Initialised Storage type=%d\n", _initialisedType);
+    } else {
+        AP_HAL::panic("Unable to init Storage backend");
+    }
 }
 
 /*
@@ -113,12 +124,13 @@ 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);
         if (fd != -1) {
             AP::FS().write(fd, _buffer, CH_STORAGE_SIZE);
             AP::FS().close(fd);
         }
+    }
 #endif
 }
 
@@ -144,7 +156,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();
@@ -153,7 +165,7 @@ 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) {
@@ -165,7 +177,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()) {
@@ -187,7 +199,7 @@ void Storage::_timer_tick(void)
     }
 
 #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)) {
             _dirty_mask.clear(i);
         }
@@ -196,7 +208,7 @@ void Storage::_timer_tick(void)
 #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;
@@ -213,8 +225,10 @@ void Storage::_timer_tick(void)
 #endif
 
 #ifdef STORAGE_FLASH_PAGE
+    if (_initialisedType == StorageBackend::Flash) {
         // save to storage backend
         _flash_write(i);
+    }
 #endif
 }
 
@@ -229,10 +243,10 @@ void Storage::_flash_load(void)
     ::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
 }
 
@@ -284,10 +298,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
 }
 
 /*
@@ -295,6 +313,7 @@ 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
     for (uint8_t i=0; i<STORAGE_FLASH_RETRIES; i++) {
         if (hal.flash->erasepage(_flash_page+sector)) {
             return true;
@@ -302,6 +321,9 @@ bool Storage::_flash_erase_sector(uint8_t sector)
         hal.scheduler->delay(1);
     }
     return false;
+#else
+    return false;
+#endif
 }
 
 /*
@@ -319,7 +341,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 < 2000)));
 }
 
 /*
@@ -328,12 +351,12 @@ bool Storage::healthy(void)
 bool Storage::erase(void)
 {
 #if HAL_WITH_RAMTRON
-    if (using_fram) {
+    if (_initialisedType == StorageBackend::FRAM) {
         return AP_HAL::Storage::erase();
     }
 #endif
 #ifdef USE_POSIX
-    if (using_filesystem) {
+    if (_initialisedType == StorageBackend::SDCard) {
         return AP_HAL::Storage::erase();
     }
 #endif

libraries/AP_HAL_ChibiOS/Storage.h

@@ -48,7 +48,13 @@ 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);
@@ -79,10 +85,8 @@ private:
 
 #if HAL_WITH_RAMTRON
     AP_RAMTRON fram;
-    bool using_fram;
 #endif
 #ifdef USE_POSIX
-    bool using_filesystem;
     int log_fd;
 #endif
 };