You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
129 lines
4.3 KiB
129 lines
4.3 KiB
/* |
|
This program is free software: you can redistribute it and/or modify |
|
it under the terms of the GNU General Public License as published by |
|
the Free Software Foundation, either version 3 of the License, or |
|
(at your option) any later version. |
|
|
|
This program is distributed in the hope that it will be useful, |
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
GNU General Public License for more details. |
|
|
|
You should have received a copy of the GNU General Public License |
|
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
*/ |
|
/* |
|
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/AP_HAL.h> |
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE |
|
|
|
#include <string.h> |
|
#include "Storage.h" |
|
#include "FlymapleWirish.h" |
|
#include "utility/EEPROM.h" |
|
|
|
using namespace AP_HAL_FLYMAPLE_NS; |
|
|
|
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 address 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() |
|
{ |
|
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[eeprom_index].read(eeprom_offset >> 1); |
|
if (eeprom_offset & 1) |
|
return data >> 8; // Odd, upper byte |
|
else |
|
return data & 0xff; // Even lower byte |
|
} |
|
|
|
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); |
|
} |
|
|
|
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[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[eeprom_index].write(eeprom_offset >> 1, data); |
|
} |
|
|
|
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]); |
|
} |
|
|
|
#endif
|
|
|