diff --git a/libraries/AP_FlashStorage/AP_FlashStorage.cpp b/libraries/AP_FlashStorage/AP_FlashStorage.cpp index 56189a1378..35c3570636 100644 --- a/libraries/AP_FlashStorage/AP_FlashStorage.cpp +++ b/libraries/AP_FlashStorage/AP_FlashStorage.cpp @@ -47,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); @@ -59,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) { @@ -74,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]) { @@ -183,9 +183,11 @@ 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 const uint32_t space_available = flash_sector_size - write_offset; const uint32_t space_required = sizeof(struct block_header) + max_write + reserved_space; @@ -199,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); @@ -229,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 @@ -288,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; @@ -305,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)); } @@ -329,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)); } @@ -375,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); @@ -384,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("new sector unavailable; state=0x%02x\n", (unsigned)header.state); + debug("new sector unavailable; state=0x%02x\n", (unsigned)header.get_state()); return false; } @@ -398,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; } @@ -433,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 +/* + we support 3 different types of flash which have different restrictions + */ +#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_MULTI_WRITE 0 +#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 @@ -98,7 +121,7 @@ public: 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; @@ -114,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); };