diff --git a/libraries/AP_FlashStorage/AP_FlashStorage.cpp b/libraries/AP_FlashStorage/AP_FlashStorage.cpp index 16384c18d0..afd6385ceb 100644 --- a/libraries/AP_FlashStorage/AP_FlashStorage.cpp +++ b/libraries/AP_FlashStorage/AP_FlashStorage.cpp @@ -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 #include #include +#include #include #define FLASHSTORAGE_DEBUG 0 @@ -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) 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]) { @@ -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) 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) } } } - - 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; + } + 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); @@ -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) // 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 -#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; };