mirror of https://github.com/ArduPilot/ardupilot
AP_FlashStorage: support STM32G4
needs to write in multiples of 8 bytes, and can only write if all 1s
This commit is contained in:
parent
af64b86c9e
commit
ec8abf0b2f
|
@ -183,7 +183,7 @@ 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 AP_FLASHSTORAGE_TYPE != AP_FLASHSTORAGE_TYPE_H7 && AP_FLASHSTORAGE_TYPE != AP_FLASHSTORAGE_TYPE_G4
|
||||
if (length < n) {
|
||||
n = length;
|
||||
}
|
||||
|
@ -232,7 +232,7 @@ bool AP_FlashStorage::write(uint16_t offset, uint16_t length)
|
|||
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
|
||||
#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_H7 || AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_G4
|
||||
blk.header.state = BLOCK_STATE_VALID;
|
||||
if (!flash_write(current_sector, write_offset, (uint8_t*)&blk, sizeof(blk.header) + max_write)) {
|
||||
return false;
|
||||
|
@ -313,6 +313,9 @@ bool AP_FlashStorage::load_sector(uint8_t sector)
|
|||
#if AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_H7
|
||||
// offsets must be advanced to a multiple of 32 on H7
|
||||
ofs = (ofs + 31U) & ~31U;
|
||||
#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_G4
|
||||
// offsets must be advanced to a multiple of 8 on G4
|
||||
ofs = (ofs + 7U) & ~7U;
|
||||
#endif
|
||||
}
|
||||
write_offset = ofs;
|
||||
|
@ -535,6 +538,76 @@ void AP_FlashStorage::sector_header::set_state(SectorState state)
|
|||
}
|
||||
}
|
||||
|
||||
#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_G4
|
||||
/*
|
||||
G4 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 == 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)
|
||||
{
|
||||
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
|
||||
|
|
|
@ -45,6 +45,7 @@
|
|||
#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
|
||||
#define AP_FLASHSTORAGE_TYPE_G4 4 // G4
|
||||
|
||||
#ifndef AP_FLASHSTORAGE_TYPE
|
||||
#if defined(STM32F1) || defined(STM32F3)
|
||||
|
@ -58,7 +59,12 @@
|
|||
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
|
||||
#elif defined(STM32G4)
|
||||
/*
|
||||
STM32G4 can only write in 8 byte chunks, and must only write when all bits are 1
|
||||
*/
|
||||
#define AP_FLASHSTORAGE_TYPE AP_FLASHSTORAGE_TYPE_G4
|
||||
#else // F4, F7
|
||||
/*
|
||||
STM32HF4 and STM32H7 can update bits from 1 to 0
|
||||
*/
|
||||
|
@ -75,6 +81,10 @@ private:
|
|||
// 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;
|
||||
#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_G4
|
||||
// write in 8 byte chunks, with 2 byte header
|
||||
static const uint8_t block_size = 6;
|
||||
static const uint8_t max_write = block_size;
|
||||
#else
|
||||
static const uint8_t block_size = 8;
|
||||
static const uint8_t max_write = 64;
|
||||
|
@ -143,6 +153,8 @@ private:
|
|||
static const uint32_t signature = 0x51;
|
||||
#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_H7
|
||||
static const uint32_t signature = 0x51685B62;
|
||||
#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_G4
|
||||
static const uint32_t signature = 0x1586B562;
|
||||
#else
|
||||
#error "Unknown AP_FLASHSTORAGE_TYPE"
|
||||
#endif
|
||||
|
@ -174,6 +186,14 @@ private:
|
|||
uint32_t state3;
|
||||
uint32_t signature3;
|
||||
uint32_t pad3[6];
|
||||
#elif AP_FLASHSTORAGE_TYPE == AP_FLASHSTORAGE_TYPE_G4
|
||||
// needs to be 24 bytes on G4 to support 3 states
|
||||
uint32_t state1;
|
||||
uint32_t signature1;
|
||||
uint32_t state2;
|
||||
uint32_t signature2;
|
||||
uint32_t state3;
|
||||
uint32_t signature3;
|
||||
#endif
|
||||
bool signature_ok(void) const;
|
||||
SectorState get_state() const;
|
||||
|
|
Loading…
Reference in New Issue