From 952df2fced50ecd8fdd83a63ed2c504623fcd20a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 Jan 2013 16:37:11 +1100 Subject: [PATCH] HAL_PX4: new buffered storeage driver for microsd cards this does all IO in the timer thread, avoids writes that don't change data, and does all writes in multiples of 128 byte chunks. This should be about as friendly to SD cards as we can get. --- libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h | 2 +- libraries/AP_HAL_PX4/HAL_PX4_Class.cpp | 2 +- libraries/AP_HAL_PX4/Scheduler.cpp | 4 + libraries/AP_HAL_PX4/Storage.cpp | 260 +++++++++++++------- libraries/AP_HAL_PX4/Storage.h | 22 +- 5 files changed, 195 insertions(+), 95 deletions(-) diff --git a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h b/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h index c47e3e2371..b9ff6582a3 100644 --- a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h +++ b/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h @@ -6,7 +6,7 @@ namespace PX4 { class PX4ConsoleDriver; class PX4Scheduler; class PX4UARTDriver; - class PX4EEPROMStorage; + class PX4Storage; class PX4RCInput; class PX4RCOutput; class PX4AnalogIn; diff --git a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp index 752d7ac75c..6b8f26c24c 100644 --- a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp +++ b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp @@ -35,7 +35,7 @@ static Empty::EmptyGPIO gpioDriver; static PX4ConsoleDriver consoleDriver; static PX4Scheduler schedulerInstance; -static PX4EEPROMStorage storageDriver; +static PX4Storage storageDriver; static PX4RCInput rcinDriver; static PX4RCOutput rcoutDriver; static PX4AnalogIn analogIn; diff --git a/libraries/AP_HAL_PX4/Scheduler.cpp b/libraries/AP_HAL_PX4/Scheduler.cpp index a918fd9497..8e8b2991e4 100644 --- a/libraries/AP_HAL_PX4/Scheduler.cpp +++ b/libraries/AP_HAL_PX4/Scheduler.cpp @@ -16,6 +16,7 @@ #include #include #include "UARTDriver.h" +#include "Storage.h" using namespace PX4; @@ -179,6 +180,9 @@ void *PX4Scheduler::_timer_thread(void) // process any pending serial bytes ((PX4UARTDriver *)hal.uartA)->_timer_tick(); ((PX4UARTDriver *)hal.uartB)->_timer_tick(); + + // process any pending storage writes + ((PX4Storage *)hal.storage)->_timer_tick(); } return NULL; } diff --git a/libraries/AP_HAL_PX4/Storage.cpp b/libraries/AP_HAL_PX4/Storage.cpp index a13c302bb1..420af55f0c 100644 --- a/libraries/AP_HAL_PX4/Storage.cpp +++ b/libraries/AP_HAL_PX4/Storage.cpp @@ -7,106 +7,190 @@ #include #include #include +#include #include "Storage.h" using namespace PX4; -#define EEPROM_SIZE 4096 +/* + This stores 'eeprom' data on the SD card, with a 4k size, and a + in-memory buffer. This keeps the latency down. + */ -// name the eeprom file after the sketch so you can use the same sd +// name the storage file after the sketch so you can use the same sd // card for ArduCopter and ArduPlane -#define EEPROM_DIR "/fs/microsd/APM" -#define EEPROM_FILE EEPROM_DIR "/" SKETCHNAME ".eeprom" +#define STORAGE_DIR "/fs/microsd/APM" +#define STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".stg" extern const AP_HAL::HAL& hal; -/* NuttX doesn't have pread/pwrite */ -static ssize_t pread(int fd, void *buf, size_t count, off_t ofs) +void PX4Storage::_storage_create(void) { - lseek(fd, ofs, SEEK_SET); - return read(fd, buf, count); -} - -static ssize_t pwrite(int fd, const void *buf, size_t count, off_t ofs) -{ - lseek(fd, ofs, SEEK_SET); - return write(fd, buf, count); -} - -void PX4EEPROMStorage::_eeprom_open(void) -{ - if (_eeprom_fd == -1) { - mkdir(EEPROM_DIR, 0777); - _eeprom_fd = open(EEPROM_FILE, O_RDWR|O_CREAT, 0777); - if (_eeprom_fd == -1) { - hal.scheduler->panic("Failed to open " EEPROM_FILE); + mkdir(STORAGE_DIR, 0777); + unlink(STORAGE_FILE); + _fd = open(STORAGE_FILE, O_RDWR|O_CREAT|O_TRUNC, 0666); + if (_fd == -1) { + hal.scheduler->panic("Failed to create " STORAGE_FILE); + } + for (uint16_t loc=0; locpanic("Error filling " STORAGE_FILE); } - if (lseek(_eeprom_fd, 0, SEEK_END) < EEPROM_SIZE) { - char c = 0; - assert(pwrite(_eeprom_fd, &c, 1, EEPROM_SIZE-1) == 1); + } + // ensure the directory is updated with the new size + fsync(_fd); +} + +void PX4Storage::_storage_open(void) +{ + if (_fd != -1) { + return; + } + + _dirty_mask = 0; + _fd = open(STORAGE_FILE, O_RDWR); + if (_fd == -1) { + _storage_create(); + } else if (read(_fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) { + close(_fd); + _fd = -1; + _storage_create(); + } +} + +void PX4Storage::_mark_dirty(uint16_t loc, uint16_t length) +{ + uint16_t end = loc + length; + while (loc < end) { + uint8_t line = (loc >> PX4_STORAGE_LINE_SHIFT); + _dirty_mask |= 1 << line; + loc += PX4_STORAGE_LINE_SIZE; + } +} + +uint8_t PX4Storage::read_byte(uint16_t loc) +{ + if (loc >= sizeof(_buffer)) { + return 0; + } + _storage_open(); + return _buffer[loc]; +} + +uint16_t PX4Storage::read_word(uint16_t loc) +{ + uint16_t value; + if (loc >= sizeof(_buffer)-(sizeof(value)-1)) { + return 0; + } + _storage_open(); + memcpy(&value, &_buffer[loc], sizeof(value)); + return value; +} + +uint32_t PX4Storage::read_dword(uint16_t loc) +{ + uint32_t value; + if (loc >= sizeof(_buffer)-(sizeof(value)-1)) { + return 0; + } + _storage_open(); + memcpy(&value, &_buffer[loc], sizeof(value)); + return value; +} + +void PX4Storage::read_block(void *dst, uint16_t loc, size_t n) +{ + if (loc >= sizeof(_buffer)-(n-1)) { + return; + } + _storage_open(); + memcpy(dst, &_buffer[loc], n); +} + +void PX4Storage::write_byte(uint16_t loc, uint8_t value) +{ + if (loc >= sizeof(_buffer)) { + return; + } + if (_buffer[loc] != value) { + _storage_open(); + _buffer[loc] = value; + _mark_dirty(loc, sizeof(value)); + } +} + +void PX4Storage::write_word(uint16_t loc, uint16_t value) +{ + if (loc >= sizeof(_buffer)-(sizeof(value)-1)) { + return; + } + if (memcmp(&value, &_buffer[loc], sizeof(value)) != 0) { + _storage_open(); + memcpy(&_buffer[loc], &value, sizeof(value)); + _mark_dirty(loc, sizeof(value)); + } +} + +void PX4Storage::write_dword(uint16_t loc, uint32_t value) +{ + if (loc >= sizeof(_buffer)-(sizeof(value)-1)) { + return; + } + if (memcmp(&value, &_buffer[loc], sizeof(value)) != 0) { + _storage_open(); + memcpy(&_buffer[loc], &value, sizeof(value)); + _mark_dirty(loc, sizeof(value)); + } +} + +void PX4Storage::write_block(uint16_t loc, void *src, size_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 PX4Storage::_timer_tick(void) +{ + if (_fd == -1 || _dirty_mask == 0) { + return; + } + // write out the first dirty set of lines. We don't write more + // than one to keep the latency of this call to a minimum + uint8_t i, n; + for (i=0; i>PX4_STORAGE_LINE_SHIFT); n++) { + if (!(_dirty_mask & (1<<(n+i)))) { + break; + } + // mark that line clean + _dirty_mask &= ~(1<<(i+n)); + } + + // write them + if (lseek(_fd, i< #include "AP_HAL_PX4_Namespace.h" -class PX4::PX4EEPROMStorage : public AP_HAL::Storage { +#define PX4_STORAGE_SIZE 4096 +#define PX4_STORAGE_MAX_WRITE 1024 +#define PX4_STORAGE_LINE_SHIFT 7 +#define PX4_STORAGE_LINE_SIZE (1<