zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_FlashStorage: resync for 4.0 update

copter407
Andrew Tridgell 5 years ago
parent
e291bd1471
commit
74444a28cc
2 changed files with 320 additions and 69 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 269
      libraries/AP_FlashStorage/AP_FlashStorage.cpp
  2. 120
      libraries/AP_FlashStorage/AP_FlashStorage.h

269
libraries/AP_FlashStorage/AP_FlashStorage.cpp
Unescape View File

@ -1,5 +1,5 @@ @@ -1,5 +1,5 @@
/*
Please contribute your ideas! See http://dev.ardupilot.org for details
Please contribute your ideas! See https://dev.ardupilot.org for details
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
@ -18,6 +18,7 @@ @@ -18,6 +18,7 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_FlashStorage/AP_FlashStorage.h>
#include <AP_Math/AP_Math.h>
#include <AP_InternalError/AP_InternalError.h>
#include <stdio.h>
#define FLASHSTORAGE_DEBUG 0
@ -46,7 +47,7 @@ AP_FlashStorage::AP_FlashStorage(uint8_t *_mem_buffer, @@ -46,7 +47,7 @@ AP_FlashStorage::AP_FlashStorage(uint8_t *_mem_buffer,
bool AP_FlashStorage::init(void)
{
debug("running init()\n");
// start with empty memory buffer
memset(mem_buffer, 0, storage_size);
@ -58,8 +59,8 @@ bool AP_FlashStorage::init(void) @@ -58,8 +59,8 @@ bool AP_FlashStorage::init(void)
if (!flash_read(i, 0, (uint8_t *)&header[i], sizeof(header[i]))) {
return false;
}
bool bad_header = (header[i].signature != signature);
enum SectorState state = (enum SectorState)header[i].state;
bool bad_header = !header[i].signature_ok();
enum SectorState state = header[i].get_state();
if (state != SECTOR_STATE_AVAILABLE &&
state != SECTOR_STATE_IN_USE &&
state != SECTOR_STATE_FULL) {
@ -73,7 +74,7 @@ bool AP_FlashStorage::init(void) @@ -73,7 +74,7 @@ bool AP_FlashStorage::init(void)
}
// work out the first sector to read from using sector states
enum SectorState states[2] {(enum SectorState)header[0].state, (enum SectorState)header[1].state};
enum SectorState states[2] {header[0].get_state(), header[1].get_state()};
uint8_t first_sector;
if (states[0] == states[1]) {
@ -139,7 +140,24 @@ bool AP_FlashStorage::init(void) @@ -139,7 +140,24 @@ bool AP_FlashStorage::init(void)
bool AP_FlashStorage::switch_full_sector(void)
{
debug("running switch_full_sector()\n");
if (in_switch_full_sector) {
INTERNAL_ERROR(AP_InternalError::error_t::switch_full_sector_recursion);
return false;
}
in_switch_full_sector = true;
bool ret = protected_switch_full_sector();
in_switch_full_sector = false;
return ret;
}
// protected_switch_full_sector is protected by switch_full_sector to
// avoid an infinite recursion problem; switch_full_sectory calls
// write() which can call switch_full_sector. This has been seen in
// practice, and while it might be caused by corruption... corruption
// happens.
bool AP_FlashStorage::protected_switch_full_sector(void)
{
// clear any write error
write_error = false;
reserved_space = 0;
@ -165,11 +183,15 @@ bool AP_FlashStorage::write(uint16_t offset, uint16_t length) @@ -165,11 +183,15 @@ bool AP_FlashStorage::write(uint16_t offset, uint16_t length)
while (length > 0) {
uint8_t n = max_write;
#if AP_FLASHSTORAGE_TYPE != AP_FLASHSTORAGE_TYPE_H7
if (length < n) {
n = length;
}
#endif
if (write_offset > flash_sector_size - (sizeof(struct block_header) + max_write + reserved_space)) {
const uint32_t space_available = flash_sector_size - write_offset;
const uint32_t space_required = sizeof(struct block_header) + max_write + reserved_space;
if (space_available < space_required) {
if (!switch_sectors()) {
if (!flash_erase_ok()) {
return false;
@ -179,27 +201,45 @@ bool AP_FlashStorage::write(uint16_t offset, uint16_t length) @@ -179,27 +201,45 @@ bool AP_FlashStorage::write(uint16_t offset, uint16_t length)
}
}
}
struct block_header header;
header.state = BLOCK_STATE_WRITING;
header.block_num = offset / block_size;
header.num_blocks_minus_one = ((n + (block_size - 1)) / block_size)-1;
uint16_t block_ofs = header.block_num*block_size;
uint16_t block_nbytes = (header.num_blocks_minus_one+1)*block_size;
#if AP_FLASHSTORAGE_MULTI_WRITE
if (!flash_write(current_sector, write_offset, (uint8_t*)&header, sizeof(header))) {
struct PACKED {
struct block_header header;
uint8_t data[max_write];
} blk;
blk.header.state = BLOCK_STATE_WRITING;
blk.header.block_num = offset / block_size;
blk.header.num_blocks_minus_one = ((n + (block_size - 1)) / block_size)-1;
uint16_t block_ofs = blk.header.block_num*block_size;
uint16_t block_nbytes = (blk.header.num_blocks_minus_one+1)*block_size;
memcpy(blk.data, &mem_buffer[block_ofs], block_nbytes);
#if AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_F4
if (!flash_write(current_sector, write_offset, (uint8_t*)&blk.header, sizeof(blk.header))) {
return false;
}
#endif
if (!flash_write(current_sector, write_offset+sizeof(header), &mem_buffer[block_ofs], block_nbytes)) {
if (!flash_write(current_sector, write_offset+sizeof(blk.header), blk.data, block_nbytes)) {
return false;
}
header.state = BLOCK_STATE_VALID;
if (!flash_write(current_sector, write_offset, (uint8_t*)&header, sizeof(header))) {
blk.header.state = BLOCK_STATE_VALID;
if (!flash_write(current_sector, write_offset, (uint8_t*)&blk.header, sizeof(blk.header))) {
return false;
}
write_offset += sizeof(header) + block_nbytes;
#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_F1
blk.header.state = BLOCK_STATE_VALID;
if (!flash_write(current_sector, write_offset, (uint8_t*)&blk, sizeof(blk.header) + block_nbytes)) {
return false;
}
#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_H7
blk.header.state = BLOCK_STATE_VALID;
if (!flash_write(current_sector, write_offset, (uint8_t*)&blk, sizeof(blk.header) + max_write)) {
return false;
}
#endif
write_offset += sizeof(blk.header) + block_nbytes;
uint8_t n2 = block_nbytes - (offset % block_size);
//debug("write_block at %u for %u n2=%u\n", block_ofs, block_nbytes, n2);
@ -209,7 +249,9 @@ bool AP_FlashStorage::write(uint16_t offset, uint16_t length) @@ -209,7 +249,9 @@ bool AP_FlashStorage::write(uint16_t offset, uint16_t length)
offset += n2;
length -= n2;
}
//debug("write_offset %u\n", write_offset);
// handle wrap to next sector
// write data
// write header word
@ -268,6 +310,10 @@ bool AP_FlashStorage::load_sector(uint8_t sector) @@ -268,6 +310,10 @@ bool AP_FlashStorage::load_sector(uint8_t sector)
// invalid state
return false;
}
#if AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_H7
// offsets must be advanced to a multiple of 32 on H7
ofs = (ofs + 31U) & ~31U;
#endif
}
write_offset = ofs;
return true;
@ -285,8 +331,7 @@ bool AP_FlashStorage::erase_sector(uint8_t sector, bool mark_available) @@ -285,8 +331,7 @@ bool AP_FlashStorage::erase_sector(uint8_t sector, bool mark_available)
return true;
}
struct sector_header header;
header.signature = signature;
header.state = SECTOR_STATE_AVAILABLE;
header.set_state(SECTOR_STATE_AVAILABLE);
return flash_write(sector, 0, (const uint8_t *)&header, sizeof(header));
}
@ -309,8 +354,7 @@ bool AP_FlashStorage::erase_all(void) @@ -309,8 +354,7 @@ bool AP_FlashStorage::erase_all(void)
// mark current sector as in-use
struct sector_header header;
header.signature = signature;
header.state = SECTOR_STATE_IN_USE;
header.set_state(SECTOR_STATE_IN_USE);
return flash_write(current_sector, 0, (const uint8_t *)&header, sizeof(header));
}
@ -355,7 +399,6 @@ bool AP_FlashStorage::switch_sectors(void) @@ -355,7 +399,6 @@ bool AP_FlashStorage::switch_sectors(void)
}
struct sector_header header;
header.signature = signature;
uint8_t new_sector = current_sector ^ 1;
debug("switching to sector %u\n", new_sector);
@ -364,13 +407,13 @@ bool AP_FlashStorage::switch_sectors(void) @@ -364,13 +407,13 @@ bool AP_FlashStorage::switch_sectors(void)
if (!flash_read(new_sector, 0, (uint8_t *)&header, sizeof(header))) {
return false;
}
if (header.signature != signature) {
if (!header.signature_ok()) {
write_error = true;
return false;
}
if (SECTOR_STATE_AVAILABLE != (enum SectorState)header.state) {
if (SECTOR_STATE_AVAILABLE != header.get_state()) {
write_error = true;
debug("both sectors full\n");
debug("new sector unavailable; state=0x%02x\n", (unsigned)header.get_state());
return false;
}
@ -378,13 +421,13 @@ bool AP_FlashStorage::switch_sectors(void) @@ -378,13 +421,13 @@ bool AP_FlashStorage::switch_sectors(void)
// mark the new sector as in-use so that a power failure between
// the two steps doesn't leave us with an erase on the
// reboot. Thanks to night-ghost for spotting this.
header.state = SECTOR_STATE_FULL;
header.set_state(SECTOR_STATE_FULL);
if (!flash_write(current_sector, 0, (const uint8_t *)&header, sizeof(header))) {
return false;
}
// mark new sector as in-use
header.state = SECTOR_STATE_IN_USE;
header.set_state(SECTOR_STATE_IN_USE);
if (!flash_write(new_sector, 0, (const uint8_t *)&header, sizeof(header))) {
return false;
}
@ -413,3 +456,163 @@ bool AP_FlashStorage::re_initialise(void) @@ -413,3 +456,163 @@ bool AP_FlashStorage::re_initialise(void)
}
return write_all();
}
#if AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_H7
/*
H7 specific sector header functions
*/
bool AP_FlashStorage::sector_header::signature_ok(void) const
{
for (uint8_t i=0; i<ARRAY_SIZE(pad1); i++) {
if (pad1[i] != 0xFFFFFFFFU || pad2[i] != 0xFFFFFFFFU || pad3[i] != 0xFFFFFFFFU) {
return false;
}
}
return signature1 == signature;
}
AP_FlashStorage::SectorState AP_FlashStorage::sector_header::get_state(void) const
{
if (state1 == 0xFFFFFFF1 &&
state2 == 0xFFFFFFFF &&
state3 == 0xFFFFFFFF &&
signature1 == signature &&
signature2 == 0xFFFFFFFF &&
signature3 == 0xFFFFFFFF) {
return SECTOR_STATE_AVAILABLE;
}
if (state1 == 0xFFFFFFF1 &&
state2 == 0xFFFFFFF2 &&
state3 == 0xFFFFFFFF &&
signature1 == signature &&
signature2 == signature &&
signature3 == 0xFFFFFFFF) {
return SECTOR_STATE_IN_USE;
}
if (state1 == 0xFFFFFFF1 &&
state2 == 0xFFFFFFF2 &&
state3 == 0xFFFFFFF3 &&
signature1 == signature &&
signature2 == signature &&
signature3 == signature) {
return SECTOR_STATE_FULL;
}
return SECTOR_STATE_INVALID;
}
void AP_FlashStorage::sector_header::set_state(SectorState state)
{
memset(pad1, 0xff, sizeof(pad1));
memset(pad2, 0xff, sizeof(pad2));
memset(pad3, 0xff, sizeof(pad3));
switch (state) {
case SECTOR_STATE_AVAILABLE:
signature1 = signature;
signature2 = 0xFFFFFFFF;
signature3 = 0xFFFFFFFF;
state1 = 0xFFFFFFF1;
state2 = 0xFFFFFFFF;
state3 = 0xFFFFFFFF;
break;
case SECTOR_STATE_IN_USE:
signature1 = signature;
signature2 = signature;
signature3 = 0xFFFFFFFF;
state1 = 0xFFFFFFF1;
state2 = 0xFFFFFFF2;
state3 = 0xFFFFFFFF;
break;
case SECTOR_STATE_FULL:
signature1 = signature;
signature2 = signature;
signature3 = signature;
state1 = 0xFFFFFFF1;
state2 = 0xFFFFFFF2;
state3 = 0xFFFFFFF3;
break;
default:
break;
}
}
#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_F1
/*
F1/F3 specific sector header functions
*/
bool AP_FlashStorage::sector_header::signature_ok(void) const
{
return signature1 == signature;
}
AP_FlashStorage::SectorState AP_FlashStorage::sector_header::get_state(void) const
{
if (state1 == 0xFFFFFFFF) {
return SECTOR_STATE_AVAILABLE;
}
if (state1 == 0xFFFFFFF1) {
return SECTOR_STATE_IN_USE;
}
if (state1 == 0xFFF2FFF1) {
return SECTOR_STATE_FULL;
}
return SECTOR_STATE_INVALID;
}
void AP_FlashStorage::sector_header::set_state(SectorState state)
{
signature1 = signature;
switch (state) {
case SECTOR_STATE_AVAILABLE:
state1 = 0xFFFFFFFF;
break;
case SECTOR_STATE_IN_USE:
state1 = 0xFFFFFFF1;
break;
case SECTOR_STATE_FULL:
state1 = 0xFFF2FFF1;
break;
default:
break;
}
}
#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_F4
/*
F4 specific sector header functions
*/
bool AP_FlashStorage::sector_header::signature_ok(void) const
{
return signature1 == signature;
}
AP_FlashStorage::SectorState AP_FlashStorage::sector_header::get_state(void) const
{
if (state1 == 0xFF) {
return SECTOR_STATE_AVAILABLE;
}
if (state1 == 0xFE) {
return SECTOR_STATE_IN_USE;
}
if (state1 == 0xFC) {
return SECTOR_STATE_FULL;
}
return SECTOR_STATE_INVALID;
}
void AP_FlashStorage::sector_header::set_state(SectorState state)
{
signature1 = signature;
switch (state) {
case SECTOR_STATE_AVAILABLE:
state1 = 0xFF;
break;
case SECTOR_STATE_IN_USE:
state1 = 0xFE;
break;
case SECTOR_STATE_FULL:
state1 = 0xFC;
break;
default:
break;
}
}
#endif

120
libraries/AP_FlashStorage/AP_FlashStorage.h
Unescape View File

@ -1,5 +1,5 @@ @@ -1,5 +1,5 @@
/*
Please contribute your ideas! See http://dev.ardupilot.org for details
Please contribute your ideas! See https://dev.ardupilot.org for details
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
@ -39,14 +39,31 @@ @@ -39,14 +39,31 @@
#include <AP_HAL/AP_HAL.h>
#if defined(STM32F1)
/*
the STM32F1 can't change individual bits from 1 to 0 unless all bits in
the 16 bit word are 1
we support 3 different types of flash which have different restrictions
*/
#define AP_FLASHSTORAGE_MULTI_WRITE 0
#define AP_FLASHSTORAGE_TYPE_F1 1 // F1 and F3
#define AP_FLASHSTORAGE_TYPE_F4 2 // F4 and F7
#define AP_FLASHSTORAGE_TYPE_H7 3 // H7
#ifndef AP_FLASHSTORAGE_TYPE
#if defined(STM32F1) || defined(STM32F3)
/*
the STM32F1 and STM32F3 can't change individual bits from 1 to 0
unless all bits in the 16 bit word are 1
*/
#define AP_FLASHSTORAGE_TYPE AP_FLASHSTORAGE_TYPE_F1
#elif defined(STM32H7)
/*
STM32H7 can only write in 32 byte chunks, and must only write when all bits are 1
*/
#define AP_FLASHSTORAGE_TYPE AP_FLASHSTORAGE_TYPE_H7
#else
#define AP_FLASHSTORAGE_MULTI_WRITE 1
/*
STM32HF4 and STM32H7 can update bits from 1 to 0
*/
#define AP_FLASHSTORAGE_TYPE AP_FLASHSTORAGE_TYPE_F4
#endif
#endif
/*
@ -54,9 +71,15 @@ @@ -54,9 +71,15 @@
*/
class AP_FlashStorage {
private:
#if AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_H7
// need to write in 32 byte chunks, with 2 byte header
static const uint8_t block_size = 30;
static const uint8_t max_write = block_size;
#else
static const uint8_t block_size = 8;
static const uint16_t num_blocks = HAL_STORAGE_SIZE / block_size;
static const uint8_t max_write = 64;
#endif
static const uint16_t num_blocks = (HAL_STORAGE_SIZE+(block_size-1)) / block_size;
public:
// caller provided function to write to a flash sector
@ -82,18 +105,23 @@ public: @@ -82,18 +105,23 @@ public:
// initialise storage, filling mem_buffer with current contents
bool init(void);
// erase sectors and re-initialise
bool erase(void) WARN_IF_UNUSED {
return erase_all();
}
// re-initialise storage, using current mem_buffer
bool re_initialise(void);
bool re_initialise(void) WARN_IF_UNUSED;
// switch full sector - should only be called when safe to have CPU
// offline for considerable periods as an erase will be needed
bool switch_full_sector(void);
bool switch_full_sector(void) WARN_IF_UNUSED;
// write some data to storage from mem_buffer
bool write(uint16_t offset, uint16_t length);
bool write(uint16_t offset, uint16_t length) WARN_IF_UNUSED;
// fixed storage size
static const uint16_t storage_size = block_size * num_blocks;
static const uint16_t storage_size = HAL_STORAGE_SIZE;
private:
uint8_t *mem_buffer;
@ -109,34 +137,47 @@ private: @@ -109,34 +137,47 @@ private:
bool write_error;
// 24 bit signature
#if AP_FLASHSTORAGE_MULTI_WRITE
#if AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_F4
static const uint32_t signature = 0x51685B;
#else
#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_F1
static const uint32_t signature = 0x51;
#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_H7
static const uint32_t signature = 0x51685B62;
#else
#error "Unknown AP_FLASHSTORAGE_TYPE"
#endif
// 8 bit sector states
// sector states, representation depends on storage type
enum SectorState {
#if AP_FLASHSTORAGE_MULTI_WRITE
SECTOR_STATE_AVAILABLE = 0xFF,
SECTOR_STATE_IN_USE = 0xFE,
SECTOR_STATE_FULL = 0xFC
#else
SECTOR_STATE_AVAILABLE = 0xFFFFFFFF,
SECTOR_STATE_IN_USE = 0xFFFFFFF1,
SECTOR_STATE_FULL = 0xFFF2FFF1,
#endif
SECTOR_STATE_AVAILABLE = 1,
SECTOR_STATE_IN_USE = 2,
SECTOR_STATE_FULL = 3,
SECTOR_STATE_INVALID = 4
};
// header in first word of each sector
struct sector_header {
#if AP_FLASHSTORAGE_MULTI_WRITE
uint32_t state:8;
uint32_t signature:24;
#else
uint32_t state:32;
uint32_t signature:16;
#if AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_F4
uint32_t state1:8;
uint32_t signature1:24;
#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_F1
uint32_t state1:32;
uint32_t signature1:16;
#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_H7
// needs to be 96 bytes on H7 to support 3 states
uint32_t state1;
uint32_t signature1;
uint32_t pad1[6];
uint32_t state2;
uint32_t signature2;
uint32_t pad2[6];
uint32_t state3;
uint32_t signature3;
uint32_t pad3[6];
#endif
bool signature_ok(void) const;
SectorState get_state() const;
void set_state(SectorState state);
};
@ -157,20 +198,27 @@ private: @@ -157,20 +198,27 @@ private:
static const uint32_t reserve_size = (storage_size / max_write) * (sizeof(block_header) + max_write) + max_write;
// load data from a sector
bool load_sector(uint8_t sector);
bool load_sector(uint8_t sector) WARN_IF_UNUSED;
// erase a sector and write header
bool erase_sector(uint8_t sector, bool mark_available);
bool erase_sector(uint8_t sector, bool mark_available) WARN_IF_UNUSED;
// erase all sectors and reset
bool erase_all();
bool erase_all() WARN_IF_UNUSED;
// write all of mem_buffer to current sector
bool write_all();
bool write_all() WARN_IF_UNUSED;
// return true if all bytes are zero
bool all_zero(uint16_t ofs, uint16_t size);
bool all_zero(uint16_t ofs, uint16_t size) WARN_IF_UNUSED;
// switch to next sector for writing
bool switch_sectors(void);
bool switch_sectors(void) WARN_IF_UNUSED;
// _switch_full_sector is protected by switch_full_sector to avoid
// an infinite recursion problem; switch_full_sectory calls
// write() which can call switch_full_sector. This has been seen
// in practice.
bool protected_switch_full_sector(void) WARN_IF_UNUSED;
bool in_switch_full_sector;
};

Write Preview
Loading…
Cancel
Save