HAL_SITL: enable optional storage in flash
this brings storage in SITL very close to storage in ChibiOS
This commit is contained in:
Andrew Tridgell
parent
0332057cbe
commit
751dade9a8
@ -4,7 +4,7 @@ namespace HALSITL {
|
||||
class UARTDriver;
|
||||
class Scheduler;
|
||||
class SITL_State;
|
||||
class EEPROMStorage;
|
||||
class Storage;
|
||||
class AnalogIn;
|
||||
class RCInput;
|
||||
class RCOutput;
|
||||
|
||||
@ -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 */
|
||||
|
||||
@ -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();
|
||||
}
|
||||
|
||||
@ -1,35 +1,290 @@
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
|
||||
#include <assert.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "Storage.h"
|
||||
|
||||
using namespace HALSITL;
|
||||
|
||||
void EEPROMStorage::_eeprom_open(void)
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
void Storage::_storage_open(void)
|
||||
{
|
||||
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 (_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 EEPROMStorage::read_block(void *dst, uint16_t src, size_t n)
|
||||
void Storage::read_block(void *dst, uint16_t loc, 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;
|
||||
}
|
||||
_storage_open();
|
||||
memcpy(dst, &_buffer[loc], n);
|
||||
}
|
||||
|
||||
void EEPROMStorage::write_block(uint16_t dst, const void *src, size_t n)
|
||||
void Storage::write_block(uint16_t loc, const void *src, size_t n)
|
||||
{
|
||||
assert(dst < HAL_STORAGE_SIZE);
|
||||
_eeprom_open();
|
||||
assert(pwrite(_eeprom_fd, src, n, dst) == (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 Storage::_timer_tick(void)
|
||||
{
|
||||
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<STORAGE_NUM_LINES; i++) {
|
||||
if (_dirty_mask.get(i)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (i == STORAGE_NUM_LINES) {
|
||||
// this shouldn't be possible
|
||||
return;
|
||||
}
|
||||
|
||||
#if STORAGE_USE_POSIX
|
||||
if (using_filesystem && log_fd != -1) {
|
||||
uint32_t offset = STORAGE_LINE_SIZE*i;
|
||||
if (lseek(log_fd, offset, SEEK_SET) != offset) {
|
||||
return;
|
||||
}
|
||||
if (write(log_fd, &_buffer[offset], STORAGE_LINE_SIZE) != STORAGE_LINE_SIZE) {
|
||||
return;
|
||||
}
|
||||
_dirty_mask.clear(i);
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if STORAGE_USE_FLASH
|
||||
// save to storage backend
|
||||
_flash_write(i);
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
load all data from flash
|
||||
*/
|
||||
void Storage::_flash_load(void)
|
||||
{
|
||||
#if STORAGE_USE_FLASH
|
||||
if (!_flash.init()) {
|
||||
AP_HAL::panic("unable to init flash storage");
|
||||
}
|
||||
#else
|
||||
AP_HAL::panic("unable to init storage");
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
write one storage line. This also updates _dirty_mask.
|
||||
*/
|
||||
void Storage::_flash_write(uint16_t line)
|
||||
{
|
||||
#if STORAGE_USE_FLASH
|
||||
if (_flash.write(line*STORAGE_LINE_SIZE, STORAGE_LINE_SIZE)) {
|
||||
// mark the line clean
|
||||
_dirty_mask.clear(line);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
#if STORAGE_USE_FLASH
|
||||
/*
|
||||
emulate writing to flash
|
||||
*/
|
||||
static int flash_fd = -1;
|
||||
|
||||
static uint32_t sitl_flash_getpageaddr(uint32_t page)
|
||||
{
|
||||
return page * HAL_STORAGE_SIZE;
|
||||
}
|
||||
|
||||
static void sitl_flash_open(void)
|
||||
{
|
||||
if (flash_fd == -1) {
|
||||
flash_fd = open("flash.dat", O_RDWR, 0644);
|
||||
if (flash_fd == -1) {
|
||||
flash_fd = open("flash.dat", O_RDWR|O_CREAT, 0644);
|
||||
if (flash_fd == -1) {
|
||||
AP_HAL::panic("Failed to open flash.dat");
|
||||
}
|
||||
if (ftruncate(flash_fd, 2*HAL_STORAGE_SIZE) != 0) {
|
||||
AP_HAL::panic("Failed to create flash.dat");
|
||||
}
|
||||
uint8_t fill[HAL_STORAGE_SIZE*2];
|
||||
memset(fill, 0xff, sizeof(fill));
|
||||
pwrite(flash_fd, fill, sizeof(fill), 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool sitl_flash_write(uint32_t addr, const uint8_t *data, uint32_t length)
|
||||
{
|
||||
sitl_flash_open();
|
||||
uint8_t old[length];
|
||||
if (pread(flash_fd, old, length, addr) != length) {
|
||||
AP_HAL::panic("Failed to read flash.dat");
|
||||
}
|
||||
// check that we are only ever clearing bits (real flash storage can only ever clear bits,
|
||||
// except for an erase
|
||||
for (uint32_t i=0; i<length; i++) {
|
||||
if (data[i] & ~old[i]) {
|
||||
AP_HAL::panic("Attempt to set flash byte 0x%02x from 0x%02x at %u\n", data[i], old[i], addr+i);
|
||||
}
|
||||
}
|
||||
return pwrite(flash_fd, data, length, addr) == length;
|
||||
}
|
||||
|
||||
static bool sitl_flash_read(uint32_t addr, uint8_t *data, uint32_t length)
|
||||
{
|
||||
sitl_flash_open();
|
||||
return pread(flash_fd, data, length, addr) == length;
|
||||
}
|
||||
|
||||
static bool sitl_flash_erasepage(uint32_t page)
|
||||
{
|
||||
uint8_t fill[HAL_STORAGE_SIZE];
|
||||
memset(fill, 0xff, sizeof(fill));
|
||||
sitl_flash_open();
|
||||
bool ret = pwrite(flash_fd, fill, sizeof(fill), page * HAL_STORAGE_SIZE) == sizeof(fill);
|
||||
printf("erase %u -> %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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -1,18 +1,70 @@
|
||||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Common/Bitmask.h>
|
||||
#include "AP_HAL_SITL_Namespace.h"
|
||||
#include <AP_FlashStorage/AP_FlashStorage.h>
|
||||
|
||||
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<<STORAGE_LINE_SHIFT)
|
||||
#define STORAGE_NUM_LINES (HAL_STORAGE_SIZE/STORAGE_LINE_SIZE)
|
||||
|
||||
class HALSITL::Storage : public AP_HAL::Storage {
|
||||
public:
|
||||
EEPROMStorage() {
|
||||
_eeprom_fd = -1;
|
||||
}
|
||||
void init() {}
|
||||
void read_block(void *dst, uint16_t src, size_t n);
|
||||
void write_block(uint16_t dst, const void* src, size_t n);
|
||||
void init() override {}
|
||||
void read_block(void *dst, uint16_t src, size_t n) override;
|
||||
void write_block(uint16_t dst, const void* src, size_t n) override;
|
||||
|
||||
void _timer_tick(void) override;
|
||||
bool healthy(void) override;
|
||||
|
||||
private:
|
||||
int _eeprom_fd;
|
||||
void _eeprom_open(void);
|
||||
volatile bool _initialised;
|
||||
void _storage_create(void);
|
||||
void _storage_open(void);
|
||||
void _save_backup(void);
|
||||
void _mark_dirty(uint16_t loc, uint16_t length);
|
||||
uint8_t _buffer[HAL_STORAGE_SIZE] __attribute__((aligned(4)));
|
||||
Bitmask _dirty_mask{STORAGE_NUM_LINES};
|
||||
|
||||
#if STORAGE_USE_FLASH
|
||||
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);
|
||||
bool _flash_erase_sector(uint8_t sector);
|
||||
bool _flash_erase_ok(void);
|
||||
#endif
|
||||
|
||||
bool _flash_failed;
|
||||
uint32_t _last_re_init_ms;
|
||||
uint32_t _last_empty_ms;
|
||||
|
||||
#if STORAGE_USE_FLASH
|
||||
AP_FlashStorage _flash{_buffer,
|
||||
HAL_STORAGE_SIZE,
|
||||
FUNCTOR_BIND_MEMBER(&Storage::_flash_write_data, bool, uint8_t, uint32_t, const uint8_t *, uint16_t),
|
||||
FUNCTOR_BIND_MEMBER(&Storage::_flash_read_data, bool, uint8_t, uint32_t, uint8_t *, uint16_t),
|
||||
FUNCTOR_BIND_MEMBER(&Storage::_flash_erase_sector, bool, uint8_t),
|
||||
FUNCTOR_BIND_MEMBER(&Storage::_flash_erase_ok, bool)};
|
||||
#endif
|
||||
|
||||
void _flash_load(void);
|
||||
void _flash_write(uint16_t line);
|
||||
|
||||
#if STORAGE_USE_POSIX
|
||||
bool using_filesystem;
|
||||
int log_fd;
|
||||
#endif
|
||||
};
|
||||
|
||||
Loading…
Reference in New Issue
Block a user