HAL_PX4: added a storage driver

stores 'eeprom' to a sdcard file

libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h

@@ -6,6 +6,7 @@ namespace PX4 {
 	class PX4ConsoleDriver;
 	class PX4Scheduler;
 	class PX4UARTDriver;
+	class PX4EEPROMStorage;
 }
 
 #endif //__AP_HAL_PX4_NAMESPACE_H__

libraries/AP_HAL_PX4/HAL_PX4_Class.cpp

@@ -10,6 +10,7 @@
 #include "Console.h"
 #include "Scheduler.h"
 #include "UARTDriver.h"
+#include "Storage.h"
 
 #include <AP_HAL_Empty.h>
 #include <AP_HAL_Empty_Private.h>
@@ -21,7 +22,6 @@ using namespace PX4;
 static Empty::EmptyI2CDriver  i2cDriver;
 static Empty::EmptySPIDeviceManager spiDeviceManager;
 static Empty::EmptyAnalogIn analogIn;
-static Empty::EmptyStorage storageDriver;
 static Empty::EmptyGPIO gpioDriver;
 static Empty::EmptyRCInput rcinDriver;
 static Empty::EmptyRCOutput rcoutDriver;
@@ -29,6 +29,7 @@ static Empty::EmptyUtil utilInstance;
 
 static PX4ConsoleDriver consoleDriver;
 static PX4Scheduler schedulerInstance;
+static PX4EEPROMStorage storageDriver;
 
 // only one real UART driver for now
 static PX4UARTDriver uartADriver;

libraries/AP_HAL_PX4/Storage.cpp

@@ -0,0 +1,107 @@
+#include <AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+
+#include <assert.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <errno.h>
+
+#include "Storage.h"
+using namespace PX4;
+
+#define EEPROM_SIZE 4096
+#define EEPROM_FILE "/fs/microsd/apm-eeprom"
+
+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)
+{
+	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) {
+		_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;
+			assert(pwrite(_eeprom_fd, &c, 1, EEPROM_SIZE-1) == 1);
+		}
+	}
+}
+
+uint8_t PX4EEPROMStorage::read_byte(uint16_t loc) 
+{
+	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

@@ -0,0 +1,30 @@
+
+
+#ifndef __AP_HAL_PX4_STORAGE_H__
+#define __AP_HAL_PX4_STORAGE_H__
+
+#include <AP_HAL.h>
+#include "AP_HAL_PX4_Namespace.h"
+
+class PX4::PX4EEPROMStorage : public AP_HAL::Storage {
+public:
+    PX4EEPROMStorage() {
+	    _eeprom_fd = -1;
+    }
+    void init(void* machtnichts) {}
+    uint8_t  read_byte(uint16_t loc);
+    uint16_t read_word(uint16_t loc);
+    uint32_t read_dword(uint16_t loc);
+    void     read_block(void *dst, uint16_t src, size_t n);
+
+    void write_byte(uint16_t loc, uint8_t value);
+    void write_word(uint16_t loc, uint16_t value);
+    void write_dword(uint16_t loc, uint32_t value);
+    void write_block(uint16_t dst, void* src, size_t n);
+
+private:
+    int _eeprom_fd;
+    void _eeprom_open(void);
+};
+
+#endif // __AP_HAL_PX4_STORAGE_H__