diff --git a/libraries/AP_HAL_ChibiOS/Storage.cpp b/libraries/AP_HAL_ChibiOS/Storage.cpp index 5eb9962840..384b94a1f0 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.cpp +++ b/libraries/AP_HAL_ChibiOS/Storage.cpp @@ -42,66 +42,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 (using_fram) { - if (!fram.read(0, _buffer, CH_STORAGE_SIZE)) { + if (fram.init() && fram.read(0, _buffer, CH_STORAGE_SIZE)) { + _save_backup(); + _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; - return; - } - // allow for FMUv3 with no FRAM chip, fall through to flash storage + + // 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"); + 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 - // load from storage backend - _flash_load(); -#elif defined(USE_POSIX) - // 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) { - ::printf("open failed of " HAL_STORAGE_FILE "\n"); - return; + if (_initialisedType != StorageBackend::None) { + ::printf("Initialised Storage type=%d\n", _initialisedType); + } else { + AP_HAL::panic("Unable to init Storage backend"); } - 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; - } - using_filesystem = true; -#endif - - _save_backup(); - _initialised = true; } /* @@ -113,11 +124,12 @@ void Storage::_save_backup(void) { #ifdef USE_POSIX // allow for fallback to microSD based storage - 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); + 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 - // save to storage backend - _flash_write(i); + 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; ierasepage(_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 diff --git a/libraries/AP_HAL_ChibiOS/Storage.h b/libraries/AP_HAL_ChibiOS/Storage.h index 80805baa1d..0be0fd1fef 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.h +++ b/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 };