Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_FlashStorage: resync for 4.0 update

This commit is contained in:
Andrew Tridgell 2020-05-11 16:46:43 +10:00
parent e291bd1471
commit 74444a28cc
2 changed files with 323 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

265
libraries/AP_FlashStorage/AP_FlashStorage.cpp
View File

@ -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 @@
#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
@ -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)
}
// 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]) {
@ -140,6 +141,23 @@ 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)
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;
@ -180,26 +202,44 @@ 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;
struct PACKED {
struct block_header header;
uint8_t data[max_write];
} blk;
#if AP_FLASHSTORAGE_MULTI_WRITE
if (!flash_write(current_sector, write_offset, (uint8_t*)&header, sizeof(header))) {
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;
}
if (!flash_write(current_sector, write_offset+sizeof(blk.header), blk.data, block_nbytes)) {
return false;
}
blk.header.state = BLOCK_STATE_VALID;
if (!flash_write(current_sector, write_offset, (uint8_t*)&blk.header, sizeof(blk.header))) {
return false;
}
#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
if (!flash_write(current_sector, write_offset+sizeof(header), &mem_buffer[block_ofs], block_nbytes)) {
return false;
}
header.state = BLOCK_STATE_VALID;
if (!flash_write(current_sector, write_offset, (uint8_t*)&header, sizeof(header))) {
return false;
}
write_offset += sizeof(header) + block_nbytes;
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);
@ -210,6 +250,8 @@ bool AP_FlashStorage::write(uint16_t offset, uint16_t length)
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)
// 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)
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)
// 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)
}
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)
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)
// 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)
}
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

118
libraries/AP_FlashStorage/AP_FlashStorage.h
View File

@ -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 @@
#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 @@
*/
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:
// 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:
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:
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;
};