diff --git a/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h b/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h index f8498dc907..ae64967618 100644 --- a/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h +++ b/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h @@ -4,7 +4,7 @@ namespace HALSITL { class UARTDriver; class Scheduler; class SITL_State; -class EEPROMStorage; +class Storage; class AnalogIn; class RCInput; class RCOutput; diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp index f90677a7ce..760ac73d23 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp @@ -23,7 +23,7 @@ using namespace HALSITL; -static EEPROMStorage sitlEEPROMStorage; +static Storage sitlStorage; static SITL_State sitlState; static Scheduler sitlScheduler(&sitlState); static RCInput sitlRCInput(&sitlState); @@ -58,7 +58,7 @@ HAL_SITL::HAL_SITL() : &i2c_mgr_instance, &emptySPI, /* spi */ &sitlAnalogIn, /* analogin */ - &sitlEEPROMStorage, /* storage */ + &sitlStorage, /* storage */ &sitlUart0Driver, /* console */ &sitlGPIO, /* gpio */ &sitlRCInput, /* rcinput */ diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp index 1f49d254a9..3dea3ef2a9 100644 --- a/libraries/AP_HAL_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_SITL/Scheduler.cpp @@ -199,6 +199,7 @@ void Scheduler::_run_io_procs() hal.uartE->_timer_tick(); hal.uartF->_timer_tick(); hal.uartG->_timer_tick(); + hal.storage->_timer_tick(); check_thread_stacks(); } diff --git a/libraries/AP_HAL_SITL/Storage.cpp b/libraries/AP_HAL_SITL/Storage.cpp index 5f9b1d09e8..035195e958 100644 --- a/libraries/AP_HAL_SITL/Storage.cpp +++ b/libraries/AP_HAL_SITL/Storage.cpp @@ -1,35 +1,290 @@ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include #include #include #include #include - #include "Storage.h" + using namespace HALSITL; -void EEPROMStorage::_eeprom_open(void) +extern const AP_HAL::HAL& hal; + +void Storage::_storage_open(void) +{ + if (_initialised) { + return; + } + +#if STORAGE_USE_POSIX + // if we have failed filesystem init don't try again + if (log_fd == -1) { + return; + } +#endif + + _dirty_mask.clearall(); + +#if STORAGE_USE_FLASH + // load from storage backend + _flash_load(); +#elif STORAGE_USE_POSIX + log_fd = open(HAL_STORAGE_FILE, O_RDWR|O_CREAT, 0644); + if (log_fd == -1) { + hal.console->printf("open failed of " HAL_STORAGE_FILE "\n"); + return; + } + int ret = read(log_fd, _buffer, HAL_STORAGE_SIZE); + if (ret < 0) { + hal.console->printf("read failed for " HAL_STORAGE_FILE "\n"); + close(log_fd); + log_fd = -1; + return; + } + // pre-fill to full size + if (lseek(log_fd, ret, SEEK_SET) != ret || + write(log_fd, &_buffer[ret], HAL_STORAGE_SIZE-ret) != HAL_STORAGE_SIZE-ret) { + hal.console->printf("setup failed for " HAL_STORAGE_FILE "\n"); + close(log_fd); + log_fd = -1; + return; + } + using_filesystem = true; +#else +#error "No storage system enabled" +#endif + + _initialised = true; +} + +/* + 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) +{ + uint16_t end = loc + length; + for (uint16_t line=loc>>STORAGE_LINE_SHIFT; + line <= end>>STORAGE_LINE_SHIFT; + line++) { + _dirty_mask.set(line); + } +} + +void Storage::read_block(void *dst, uint16_t loc, size_t n) { - if (_eeprom_fd == -1) { - _eeprom_fd = open("eeprom.bin", O_RDWR|O_CREAT|O_CLOEXEC, 0777); - assert(ftruncate(_eeprom_fd, HAL_STORAGE_SIZE) == 0); + if (loc >= sizeof(_buffer)-(n-1)) { + return; } + _storage_open(); + memcpy(dst, &_buffer[loc], n); } -void EEPROMStorage::read_block(void *dst, uint16_t src, size_t n) +void Storage::write_block(uint16_t loc, const void *src, size_t n) { - assert(src < HAL_STORAGE_SIZE && src + n <= HAL_STORAGE_SIZE); - _eeprom_open(); - assert(pread(_eeprom_fd, dst, n, src) == (ssize_t)n); + if (loc >= sizeof(_buffer)-(n-1)) { + return; + } + if (memcmp(src, &_buffer[loc], n) != 0) { + _storage_open(); + memcpy(&_buffer[loc], src, n); + _mark_dirty(loc, n); + } } -void EEPROMStorage::write_block(uint16_t dst, const void *src, size_t n) +void Storage::_timer_tick(void) { - assert(dst < HAL_STORAGE_SIZE); - _eeprom_open(); - assert(pwrite(_eeprom_fd, src, n, dst) == (ssize_t)n); + if (!_initialised) { + return; + } + if (_dirty_mask.empty()) { + _last_empty_ms = AP_HAL::millis(); + return; + } + + // write out the first dirty line. We don't write more + // than one to keep the latency of this call to a minimum + uint16_t i; + for (i=0; i %u\n", page, ret); + return ret; +} + +/* + callback to write data to flash + */ +bool Storage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length) +{ + size_t base_address = sitl_flash_getpageaddr(sector); + bool ret = sitl_flash_write(base_address+offset, data, length); + if (!ret && _flash_erase_ok()) { + // we are getting flash write errors while disarmed. Try + // re-writing all of flash + uint32_t now = AP_HAL::millis(); + 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", + (unsigned)sector, (unsigned)offset, (unsigned)length, (unsigned)ok); + } + } + return ret; +} + +/* + callback to read data from flash + */ +bool Storage::_flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length) +{ + size_t base_address = sitl_flash_getpageaddr(sector); + return sitl_flash_read(base_address+offset, data, length); +} + +/* + callback to erase flash sector + */ +bool Storage::_flash_erase_sector(uint8_t sector) +{ + return sitl_flash_erasepage(sector); +} + +/* + callback to check if erase is allowed + */ +bool Storage::_flash_erase_ok(void) +{ + // only allow erase while disarmed + return !hal.util->get_soft_armed(); +} +#endif // STORAGE_USE_FLASH + +/* + consider storage healthy if we have nothing to write sometime in the + last 2 seconds + */ +bool Storage::healthy(void) +{ + return _initialised && AP_HAL::millis() - _last_empty_ms < 2000; +} + + diff --git a/libraries/AP_HAL_SITL/Storage.h b/libraries/AP_HAL_SITL/Storage.h index 4cffb0ac01..a47eb57e91 100644 --- a/libraries/AP_HAL_SITL/Storage.h +++ b/libraries/AP_HAL_SITL/Storage.h @@ -1,18 +1,70 @@ #pragma once #include +#include #include "AP_HAL_SITL_Namespace.h" +#include -class HALSITL::EEPROMStorage : public AP_HAL::Storage { +#ifndef HAL_STORAGE_FILE +#define HAL_STORAGE_FILE "eeprom.bin" +#endif + +// define which storage system to use. This allows us to test flash storage with --sitl-flash-storage +// configure option +#ifndef STORAGE_USE_FLASH +#define STORAGE_USE_FLASH 0 +#endif + +#define STORAGE_USE_POSIX 1 + +#define STORAGE_LINE_SHIFT 3 + +#define STORAGE_LINE_SIZE (1<