AP_HAL_FLYMAPLE: Improvements to FLYMAPLEStorage

Fix a number of bugs and expand storage to an emulated 4kb, as needed by AP.

libraries/AP_HAL_FLYMAPLE/Storage.cpp

@@ -14,6 +14,10 @@
  */
 /*
   Flymaple port by Mike McCauley
+  Partly based on EEPROM.*, flash_stm* copied from AeroQuad_v3.2
+  This uses n*2*2k pages of FLASH ROM to emulate an EEPROM
+  This storage is retained after power down, and survives reloading of firmware
+  All multi-byte accesses are reduced to single byte access so that can span EEPROM block boundaries
  */
 #include <AP_HAL.h>
 
@@ -22,43 +26,85 @@
 #include <string.h>
 #include "Storage.h"
 #include "FlymapleWirish.h"
-// EEPROM.*, flash_stm* copied from AeroQuad_v3.2
 #include "utility/EEPROM.h"
 
 using namespace AP_HAL_FLYMAPLE_NS;
 
-static EEPROMClass eeprom;
+extern const AP_HAL::HAL& hal;
+
+// The EEPROM class uses 2x2k FLASH ROM pages to emulate 1k of EEPROM.
+// AP needs at last 4k of EEPROM, so we define multiple EEPROMClass, one for each
+// 1k of EEPROM address space
+
+// This is the number of 1k EEPROM blocks we need
+const uint8_t num_eeprom_blocks = 4;
+
+// This is the addres of the LAST 2k FLASH ROM page to be used to implement the EEPROM
+// FLASH ROM page use grows downwards from here
+// It is in fact the lat page in the available FLASH ROM
+const uint32_t last_flash_page = 0x0807F800;
+
+// This is the size of each FLASH ROM page
+const uint32 pageSize  = 0x800;
+
+// This defines the base addresses of the 2 FLASH ROM pages that will be used to emulate EEPROM
+// These are the last 2 2k pages in the FLASH ROM address space on the RET6 used by Flymaple
+// This will effectively provide a total of 1kb of emulated EEPROM storage
+const uint32 pageBase0 = 0x0807F000;
+const uint32 pageBase1 = 0x0807F800;
+
+static EEPROMClass eeprom[num_eeprom_blocks];
 
 FLYMAPLEStorage::FLYMAPLEStorage()
 {}
 
 void FLYMAPLEStorage::init(void*)
 {
-    eeprom.init(0x801F000, 0x801F800, 0x800);
+    for (int i = 0; i < num_eeprom_blocks; i++)
+    {
+	uint16 result = eeprom[i].init(last_flash_page - (2*i * pageSize), 
+				       last_flash_page - (((2*i)+1) * pageSize), 
+				       pageSize);
+	if (result != EEPROM_OK)
+	    hal.console->printf("FLYMAPLEStorage::init eeprom.init[%d] failed: %x\n", i, result);
+    }
 }
 
 uint8_t FLYMAPLEStorage::read_byte(uint16_t loc){
+//hal.console->printf("read_byte %d\n", loc);
+    uint16_t eeprom_index = loc >> 10;
+    uint16_t eeprom_offset = loc & 0x3ff;
+    if (eeprom_index >= num_eeprom_blocks)
+    {
+	hal.console->printf("FLYMAPLEStorage::read_byte loc %d out of range\n", loc);
+	return 0xff; // What else?
+    }
+
     // 'bytes' are packed 2 per word
     // Read existing dataword and change upper or lower byte
-    uint16_t data = eeprom.read(loc >> 1);
+    uint16_t data = eeprom[eeprom_index].read(eeprom_offset >> 1);
-    if (loc & 1)
+    if (eeprom_offset & 1)
-	return data >> 8;
+	return data >> 8; // Odd, upper byte
     else
-	return data & 0xff;
+	return data & 0xff; // Even lower byte
 }
 
 uint16_t FLYMAPLEStorage::read_word(uint16_t loc){
-    return eeprom.read(loc);
+//hal.console->printf("read_word %d\n", loc);
+    uint16_t value;
+    read_block(&value, loc, sizeof(value));
+    return value;
 }
 
 uint32_t FLYMAPLEStorage::read_dword(uint16_t loc){
-    loc <<= 1;
+//hal.console->printf("read_dword %d\n", loc);
-    uint32_t data = eeprom.read(loc);
+    uint32_t value;
-    data |= (eeprom.read(loc+1) << 16); // second word is the high word
+    read_block(&value, loc, sizeof(value));
-    return data;
+    return value;
 }
 
 void FLYMAPLEStorage::read_block(void* dst, uint16_t src, size_t n) {
+//    hal.console->printf("read_block %d %d\n", src, n);
     // Treat as a block of bytes
     for (size_t i = 0; i < n; i++)
 	((uint8_t*)dst)[i] = read_byte(src+i);
@@ -66,30 +112,41 @@ void FLYMAPLEStorage::read_block(void* dst, uint16_t src, size_t n) {
 
 void FLYMAPLEStorage::write_byte(uint16_t loc, uint8_t value)
 {
+//    hal.console->printf("write_byte %d, %d\n", loc, value);
+
+    uint16_t eeprom_index = loc >> 10;
+    uint16_t eeprom_offset = loc & 0x3ff;
+    if (eeprom_index >= num_eeprom_blocks)
+    {
+	hal.console->printf("FLYMAPLEStorage::write_byte loc %d out of range\n", loc);
+	return;
+    }
+    
     // 'bytes' are packed 2 per word
     // Read existing data word and change upper or lower byte
-    uint16_t data = eeprom.read(loc >> 1);
+    uint16_t data = eeprom[eeprom_index].read(eeprom_offset >> 1);
-    if (loc & 1)
+    if (eeprom_offset & 1)
 	data = (data & 0x00ff) | (value << 8); // Odd, upper byte
     else
 	data = (data & 0xff00) | value; // Even, lower byte
-    eeprom.write(loc >> 1, data);
+    eeprom[eeprom_index].write(eeprom_offset >> 1, data);
 }
 
 void FLYMAPLEStorage::write_word(uint16_t loc, uint16_t value)
 {
-    eeprom.write(loc, value);
+//    hal.console->printf("write_word %d, %d\n", loc, value);
+    write_block(loc, &value, sizeof(value));
 }
 
 void FLYMAPLEStorage::write_dword(uint16_t loc, uint32_t value)
 {
-    loc <<= 1;
+//    hal.console->printf("write_dword %d, %d\n", loc, value);
-    eeprom.write(loc, value & 0xffff);
+    write_block(loc, &value, sizeof(value));
-    eeprom.write(loc+1, value >> 16);
 }
 
 void FLYMAPLEStorage::write_block(uint16_t loc, const void* src, size_t n)
 {
+//    hal.console->printf("write_block %d, %d\n", loc, n);
     // Treat as a block of bytes
     for (size_t i = 0; i < n; i++)
 	write_byte(loc+i, ((uint8_t*)src)[i]);

libraries/AP_HAL_FLYMAPLE/examples/Storage/Storage.pde

@@ -9,7 +9,7 @@
 
 const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
 
-uint8_t fibs[] = { 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144 };
+uint8_t fibs[] = { 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 0 };
 
 void test_erase() {
     hal.console->printf_P(PSTR("erasing... "));
@@ -46,6 +46,9 @@ void test_readback() {
 void setup (void) {
     hal.scheduler->delay(5000);
     hal.console->printf_P(PSTR("Starting AP_HAL_FLYMAPLE::Storage test\r\n"));
+
+    hal.console->printf("test %d\n", i);
+    test_readback(); // Test what was left from the last run, possibly after power off
     test_erase();
     test_write();
     test_readback();