diff --git a/libraries/AP_HAL_FLYMAPLE/Storage.cpp b/libraries/AP_HAL_FLYMAPLE/Storage.cpp index fc915912d7..b0334c80c4 100644 --- a/libraries/AP_HAL_FLYMAPLE/Storage.cpp +++ b/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 @@ -22,43 +26,85 @@ #include #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); - if (loc & 1) - return data >> 8; + uint16_t data = eeprom[eeprom_index].read(eeprom_offset >> 1); + if (eeprom_offset & 1) + 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; - uint32_t data = eeprom.read(loc); - data |= (eeprom.read(loc+1) << 16); // second word is the high word - return data; +//hal.console->printf("read_dword %d\n", loc); + uint32_t value; + read_block(&value, loc, sizeof(value)); + 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); - if (loc & 1) + uint16_t data = eeprom[eeprom_index].read(eeprom_offset >> 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; - eeprom.write(loc, value & 0xffff); - eeprom.write(loc+1, value >> 16); +// hal.console->printf("write_dword %d, %d\n", loc, value); + write_block(loc, &value, sizeof(value)); } 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]); diff --git a/libraries/AP_HAL_FLYMAPLE/examples/Storage/Storage.pde b/libraries/AP_HAL_FLYMAPLE/examples/Storage/Storage.pde index a5ec83322f..900fc58662 100644 --- a/libraries/AP_HAL_FLYMAPLE/examples/Storage/Storage.pde +++ b/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();