void readEEPROM(void) { // Sanity check if (!isEEPROMContentValid()) failureMode(FAILURE_INVALID_EEPROM_CONTENTS); suspendRxSignal(); // Read flash memcpy(&masterConfig, (char *) CONFIG_START_FLASH_ADDRESS, sizeof(master_t)); if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) // sanity check masterConfig.current_profile_index = 0; setProfile(masterConfig.current_profile_index); if (currentProfile->defaultRateProfileIndex > MAX_CONTROL_RATE_PROFILE_COUNT - 1) // sanity check currentProfile->defaultRateProfileIndex = 0; setControlRateProfile(currentProfile->defaultRateProfileIndex); validateAndFixConfig(); activateConfig(); resumeRxSignal(); }
void ensureEEPROMContainsValidData(void) { if (isEEPROMContentValid()) { return; } resetEEPROM(); }
bool writeEEPROM(void) { // Generate compile time error if the config does not fit in the reserved area of flash. BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG); FLASH_Status status = 0; uint32_t wordOffset; int8_t attemptsRemaining = 3; bool changed; // prepare checksum/version constants masterConfig.version = EEPROM_CONF_VERSION; masterConfig.size = sizeof(master_t); masterConfig.magic_be = 0xBE; masterConfig.magic_ef = 0xEF; masterConfig.chk = 0; // erase checksum before recalculating masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t)); changed = (memcmp((const master_t *)CONFIG_START_FLASH_ADDRESS, &masterConfig, sizeof(master_t)) != 0); // write it if(changed) { FLASH_Unlock(); while (attemptsRemaining--) { #ifdef STM32F303 FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR); #endif #ifdef STM32F10X FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR); #endif for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) { if (wordOffset % FLASH_PAGE_SIZE == 0) { status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset); if (status != FLASH_COMPLETE) { break; } } status = FLASH_ProgramWord(CONFIG_START_FLASH_ADDRESS + wordOffset, *(uint32_t *) ((char *) &masterConfig + wordOffset)); if (status != FLASH_COMPLETE) { break; } } if (status == FLASH_COMPLETE) { break; } } FLASH_Lock(); // Flash write failed - just die now if (status != FLASH_COMPLETE || !isEEPROMContentValid()) { failureMode(10); } } return changed; }
void readEEPROM(void) { // Sanity check if (!isEEPROMContentValid()) failureMode(10); // Read flash memcpy(&masterConfig, (char *) FLASH_WRITE_ADDR, sizeof(master_t)); // Copy current profile if (masterConfig.current_profile_index > 2) // sanity check masterConfig.current_profile_index = 0; memcpy(¤tProfile, &masterConfig.profile[masterConfig.current_profile_index], sizeof(profile_t)); validateAndFixConfig(); activateConfig();
void readEEPROM(void) { // Sanity check if (!isEEPROMContentValid()) failureMode(10); // Read flash memcpy(&masterConfig, (char *) flashWriteAddress, sizeof(master_t)); // Copy current profile if (masterConfig.current_profile_index > 2) // sanity check masterConfig.current_profile_index = 0; setProfile(masterConfig.current_profile_index); validateAndFixConfig(); activateConfig(); }
void writeEEPROM(void) { // Generate compile time error if the config does not fit in the reserved area of flash. BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG); FLASH_Status status = 0; uint32_t wordOffset; int8_t attemptsRemaining = 3; suspendRxSignal(); // prepare checksum/version constants masterConfig.version = EEPROM_CONF_VERSION; masterConfig.size = sizeof(master_t); masterConfig.magic_be = 0xBE; masterConfig.magic_ef = 0xEF; masterConfig.chk = 0; // erase checksum before recalculating masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t)); // write it FLASH_Unlock(); while (attemptsRemaining--) { #ifdef STM32F40_41xxx FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); #endif #ifdef STM32F303 FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR); #endif #ifdef STM32F10X FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR); #endif for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) { if (wordOffset % FLASH_PAGE_SIZE == 0) { #if defined(STM32F40_41xxx) status = FLASH_EraseSector(FLASH_Sector_8, VoltageRange_3); //0x08080000 to 0x080A0000 #elif defined (STM32F411xE) status = FLASH_EraseSector(FLASH_Sector_7, VoltageRange_3); //0x08060000 to 0x08080000 #else status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset); #endif if (status != FLASH_COMPLETE) { break; } } status = FLASH_ProgramWord(CONFIG_START_FLASH_ADDRESS + wordOffset, *(uint32_t *) ((char *) &masterConfig + wordOffset)); if (status != FLASH_COMPLETE) { break; } } if (status == FLASH_COMPLETE) { break; } } FLASH_Lock(); // Flash write failed - just die now if (status != FLASH_COMPLETE || !isEEPROMContentValid()) { failureMode(FAILURE_FLASH_WRITE_FAILED); } resumeRxSignal(); }
static bool writeSettingsToEEPROM(void) { config_streamer_t streamer; config_streamer_init(&streamer); config_streamer_start(&streamer, (uintptr_t)&__config_start, &__config_end - &__config_start); uint8_t chk = 0; configHeader_t header = { .eepromConfigVersion = EEPROM_CONF_VERSION, .boardIdentifier = TARGET_BOARD_IDENTIFIER, }; config_streamer_write(&streamer, (uint8_t *)&header, sizeof(header)); chk = updateChecksum(chk, (uint8_t *)&header, sizeof(header)); // write the transitional masterConfig record config_streamer_write(&streamer, (uint8_t *)&masterConfig, sizeof(masterConfig)); chk = updateChecksum(chk, (uint8_t *)&masterConfig, sizeof(masterConfig)); PG_FOREACH(reg) { const uint16_t regSize = pgSize(reg); configRecord_t record = { .size = sizeof(configRecord_t) + regSize, .pgn = pgN(reg), .version = pgVersion(reg), .flags = 0 }; if (pgIsSystem(reg)) { // write the only instance record.flags |= CR_CLASSICATION_SYSTEM; config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record)); chk = updateChecksum(chk, (uint8_t *)&record, sizeof(record)); config_streamer_write(&streamer, reg->address, regSize); chk = updateChecksum(chk, reg->address, regSize); } else { // write one instance for each profile for (uint8_t profileIndex = 0; profileIndex < MAX_PROFILE_COUNT; profileIndex++) { record.flags = 0; record.flags |= ((profileIndex + 1) & CR_CLASSIFICATION_MASK); config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record)); chk = updateChecksum(chk, (uint8_t *)&record, sizeof(record)); const uint8_t *address = reg->address + (regSize * profileIndex); config_streamer_write(&streamer, address, regSize); chk = updateChecksum(chk, address, regSize); } } } configFooter_t footer = { .terminator = 0, }; config_streamer_write(&streamer, (uint8_t *)&footer, sizeof(footer)); chk = updateChecksum(chk, (uint8_t *)&footer, sizeof(footer)); // append checksum now chk = ~chk; config_streamer_write(&streamer, &chk, sizeof(chk)); config_streamer_flush(&streamer); bool success = config_streamer_finish(&streamer) == 0; return success; } void writeConfigToEEPROM(void) { bool success = false; // write it for (int attempt = 0; attempt < 3 && !success; attempt++) { if (writeSettingsToEEPROM()) { success = true; } } if (success && isEEPROMContentValid()) { return; } // Flash write failed - just die now failureMode(FAILURE_FLASH_WRITE_FAILED); }
// FIXME: HAL for now this will only work for F4/F7 as flash layout is different void writeEEPROM(void) { // Generate compile time error if the config does not fit in the reserved area of flash. BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG); HAL_StatusTypeDef status; uint32_t wordOffset; int8_t attemptsRemaining = 3; suspendRxSignal(); // prepare checksum/version constants masterConfig.version = EEPROM_CONF_VERSION; masterConfig.size = sizeof(master_t); masterConfig.magic_be = 0xBE; masterConfig.magic_ef = 0xEF; masterConfig.chk = 0; // erase checksum before recalculating masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t)); // write it /* Unlock the Flash to enable the flash control register access *************/ HAL_FLASH_Unlock(); while (attemptsRemaining--) { /* Fill EraseInit structure*/ FLASH_EraseInitTypeDef EraseInitStruct = {0}; EraseInitStruct.TypeErase = FLASH_TYPEERASE_SECTORS; EraseInitStruct.VoltageRange = FLASH_VOLTAGE_RANGE_3; // 2.7-3.6V EraseInitStruct.Sector = (FLASH_SECTOR_TOTAL-1); EraseInitStruct.NbSectors = 1; uint32_t SECTORError; status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError); if (status != HAL_OK) { continue; } else { for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) { status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, CONFIG_START_FLASH_ADDRESS + wordOffset, *(uint32_t *) ((char *) &masterConfig + wordOffset)); if(status != HAL_OK) { break; } } } if (status == HAL_OK) { break; } } HAL_FLASH_Lock(); // Flash write failed - just die now if (status != HAL_OK || !isEEPROMContentValid()) { failureMode(FAILURE_FLASH_WRITE_FAILED); } resumeRxSignal(); }