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

@@ -6,7 +6,7 @@ namespace PX4 {
 	class PX4ConsoleDriver;
 	class PX4Scheduler;
 	class PX4UARTDriver;
-	class PX4EEPROMStorage;
+	class PX4Storage;
 	class PX4RCInput;
 	class PX4RCOutput;
 	class PX4AnalogIn;

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;

libraries/AP_HAL_PX4/Scheduler.cpp

@@ -16,6 +16,7 @@
 #include <systemlib/systemlib.h>
 #include <poll.h>
 #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;
 }

libraries/AP_HAL_PX4/Storage.cpp

@@ -7,106 +7,190 @@
 #include <fcntl.h>
 #include <unistd.h>
 #include <errno.h>
+#include <stdio.h>
 
 #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 STORAGE_DIR "/fs/microsd/APM"
-#define EEPROM_FILE EEPROM_DIR "/" SKETCHNAME ".eeprom"
+#define STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".stg"
 
 extern const AP_HAL::HAL& hal;
 
-/* NuttX doesn't have pread/pwrite */
+void PX4Storage::_storage_create(void)
-static ssize_t pread(int fd, void *buf, size_t count, off_t ofs)
 {
-	lseek(fd, ofs, SEEK_SET);
+	mkdir(STORAGE_DIR, 0777);
-	return read(fd, buf, count);
+	unlink(STORAGE_FILE);
-}
+	_fd = open(STORAGE_FILE, O_RDWR|O_CREAT|O_TRUNC, 0666);
-
+	if (_fd == -1) {
-static ssize_t pwrite(int fd, const void *buf, size_t count, off_t ofs)
+		hal.scheduler->panic("Failed to create " STORAGE_FILE);
-{
+	}
-	lseek(fd, ofs, SEEK_SET);
+	for (uint16_t loc=0; loc<sizeof(_buffer); loc += PX4_STORAGE_MAX_WRITE) {
-	return write(fd, buf, count);
+		if (write(_fd, &_buffer[loc], PX4_STORAGE_MAX_WRITE) != PX4_STORAGE_MAX_WRITE) {
-}
+			hal.scheduler->panic("Error filling " STORAGE_FILE);			
-
-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);
 		}
-		if (lseek(_eeprom_fd, 0, SEEK_END) < EEPROM_SIZE) {
+	}
-			char c = 0;
+	// ensure the directory is updated with the new size
-			assert(pwrite(_eeprom_fd, &c, 1, EEPROM_SIZE-1) == 1);
+	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_NUM_LINES; i++) {
+		if (_dirty_mask & (1<<i)) {
+			// mark that line clean
+			_dirty_mask &= ~(1<<i);
+			break;
+		}
+	}
+	if (i == PX4_STORAGE_NUM_LINES) {
+		return;
+	}
+	// see how many lines to write
+	for (n=1; (i+n) < PX4_STORAGE_NUM_LINES && 
+		     n < (1024>>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<<PX4_STORAGE_LINE_SHIFT, SEEK_SET) == (i<<PX4_STORAGE_LINE_SHIFT)) {
+		write(_fd, &_buffer[i<<PX4_STORAGE_LINE_SHIFT], n<<PX4_STORAGE_LINE_SHIFT);
+		if (_dirty_mask == 0) {
+			fsync(_fd);
 		}
 	}
 }
 
-uint8_t PX4EEPROMStorage::read_byte(uint16_t loc) 
+#endif // CONFIG_HAL_BOARD
-{
-	uint8_t value;
-	_eeprom_open();
-	assert(pread(_eeprom_fd, &value, 1, loc) == 1);
-	return value;
-}
-
-uint16_t PX4EEPROMStorage::read_word(uint16_t loc) 
-{
-	uint16_t value;
-	assert(loc < EEPROM_SIZE);
-	_eeprom_open();
-	assert(pread(_eeprom_fd, &value, 2, loc) == 2);
-	return value;
-}
-
-uint32_t PX4EEPROMStorage::read_dword(uint16_t loc) 
-{
-	uint32_t value;
-	assert(loc < EEPROM_SIZE);
-	_eeprom_open();
-	assert(pread(_eeprom_fd, &value, 4, loc) == 4);
-	return value;
-}
-
-void PX4EEPROMStorage::read_block(void *dst, uint16_t src, size_t n) 
-{
-	assert(src < EEPROM_SIZE && src + n < EEPROM_SIZE);
-	_eeprom_open();
-	assert(pread(_eeprom_fd, dst, n, src) == (ssize_t)n);
-}
-
-void PX4EEPROMStorage::write_byte(uint16_t loc, uint8_t value) 
-{
-	assert(loc < EEPROM_SIZE);
-	_eeprom_open();
-	assert(pwrite(_eeprom_fd, &value, 1, loc) == 1);
-}
-
-void PX4EEPROMStorage::write_word(uint16_t loc, uint16_t value) 
-{
-	assert(loc < EEPROM_SIZE);
-	_eeprom_open();
-	assert(pwrite(_eeprom_fd, &value, 2, loc) == 2);
-}
-
-void PX4EEPROMStorage::write_dword(uint16_t loc, uint32_t value) 
-{
-	assert(loc < EEPROM_SIZE);
-	_eeprom_open();
-	assert(pwrite(_eeprom_fd, &value, 4, loc) == 4);
-}
-
-void PX4EEPROMStorage::write_block(uint16_t dst, void *src, size_t n) 
-{
-	assert(dst < EEPROM_SIZE);
-	_eeprom_open();
-	assert(pwrite(_eeprom_fd, src, n, dst) == (ssize_t)n);
-}
-
-#endif

libraries/AP_HAL_PX4/Storage.h

@@ -6,10 +6,16 @@
 #include <AP_HAL.h>
 #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<<PX4_STORAGE_LINE_SHIFT)
+#define PX4_STORAGE_NUM_LINES (PX4_STORAGE_SIZE/PX4_STORAGE_LINE_SIZE)
+
+class PX4::PX4Storage : public AP_HAL::Storage {
 public:
-    PX4EEPROMStorage() {
+    PX4Storage() {
-	    _eeprom_fd = -1;
+	    _fd = -1;
     }
     void init(void* machtnichts) {}
     uint8_t  read_byte(uint16_t loc);
@@ -22,9 +28,15 @@ public:
     void write_dword(uint16_t loc, uint32_t value);
     void write_block(uint16_t dst, void* src, size_t n);
 
+    void _timer_tick(void);
+
 private:
-    int _eeprom_fd;
+    int _fd;
-    void _eeprom_open(void);
+    void _storage_create(void);
+    void _storage_open(void);
+    void _mark_dirty(uint16_t loc, uint16_t length);
+    uint8_t _buffer[PX4_STORAGE_SIZE];
+    volatile uint32_t _dirty_mask;
 };
 
 #endif // __AP_HAL_PX4_STORAGE_H__