AP_FlashStorage: fixed two bugs found by night-ghost

this fixes two issues found by night-ghost. The first occurs if a 16
bit flash write is not atomic and only the first byte is written. Then
we could end up declaring the data invalid on reboot and erasing.

The second is a race in switching sectors. If power off occurs at the
wrong time we would trigger en erase on reboot.
This commit is contained in:
Andrew Tridgell 2018-01-29 19:23:19 +11:00 committed by Randy Mackay
parent 8da1252959
commit 1513246040

View File

@ -232,15 +232,29 @@ bool AP_FlashStorage::load_sector(uint8_t sector)
write_offset = ofs; write_offset = ofs;
return true; return true;
case BLOCK_STATE_VALID:
case BLOCK_STATE_WRITING: { case BLOCK_STATE_WRITING: {
uint16_t block_ofs = header.block_num*block_size; /*
we were interrupted while writing a block. We can't
re-use the data in this block as it may have some bits
that are not set to 1, so by flash rules can't be set to
an arbitrary value. So we skip over this block, leaving
a gap. The gap size is limited to (7+1)*8=64 bytes. That
gap won't be recovered until we next do an erase of this
sector
*/
uint16_t block_nbytes = (header.num_blocks_minus_one+1)*block_size; uint16_t block_nbytes = (header.num_blocks_minus_one+1)*block_size;
ofs += block_nbytes + sizeof(header);
break;
}
case BLOCK_STATE_VALID: {
uint16_t block_nbytes = (header.num_blocks_minus_one+1)*block_size;
uint16_t block_ofs = header.block_num*block_size;
if (block_ofs + block_nbytes > storage_size) { if (block_ofs + block_nbytes > storage_size) {
// the data is invalid (out of range)
return false; return false;
} }
if (state == BLOCK_STATE_VALID && if (!flash_read(sector, ofs+sizeof(header), &mem_buffer[block_ofs], block_nbytes)) {
!flash_read(sector, ofs+sizeof(header), &mem_buffer[block_ofs], block_nbytes)) {
return false; return false;
} }
//debug("read at %u for %u\n", block_ofs, block_nbytes); //debug("read at %u for %u\n", block_ofs, block_nbytes);
@ -349,16 +363,18 @@ bool AP_FlashStorage::switch_sectors(void)
return false; return false;
} }
// mark it in-use // mark current sector as full. This needs to be done before we
header.state = SECTOR_STATE_IN_USE; // mark the new sector as in-use so that a power failure between
if (!flash_write(new_sector, 0, (const uint8_t *)&header, sizeof(header))) { // 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;
if (!flash_write(current_sector, 0, (const uint8_t *)&header, sizeof(header))) {
return false; return false;
} }
// mark new sector as in-use
// mark current sector as full header.state = SECTOR_STATE_IN_USE;
header.state = SECTOR_STATE_FULL; if (!flash_write(new_sector, 0, (const uint8_t *)&header, sizeof(header))) {
if (!flash_write(current_sector, 0, (const uint8_t *)&header, sizeof(header))) {
return false; return false;
} }