servo/controller/fw/embed/src/flash.cpp

245 lines
6.1 KiB
C++
Raw Normal View History

2025-04-16 22:51:39 +03:00
#include "flash.h"
#include <stdbool.h>
#include "hal_conf_extra.h"
void flash_unlock(){
// Check if flash is locked
if(!(FLASH->CR & FLASH_CR_LOCK)) {
return; // Already unlocked
}
// Write flash key sequence to unlock
FLASH->KEYR = 0x45670123; // First key
FLASH->KEYR = 0xCDEF89AB; // Second key
}
void flash_lock() {
if(FLASH->CR & FLASH_CR_LOCK) {
return; // Already locked
}
FLASH->CR |= FLASH_CR_LOCK; // Lock flash memory
}
void erase_sector(uint8_t sector){
// Wait if flash is busy
while(FLASH_BUSY);
// Check if flash is locked and unlock if needed
if(FLASH->CR & FLASH_CR_LOCK) {
flash_unlock();
}
// Set sector erase bit and sector number
FLASH->CR |= FLASH_CR_SER;
FLASH->CR &= ~FLASH_CR_SNB;
FLASH->CR |= (sector << FLASH_CR_SNB_Pos) & FLASH_CR_SNB_Msk;
// Start erase
FLASH->CR |= FLASH_CR_STRT;
// Wait for erase to complete
while(FLASH_BUSY);
// Clear sector erase bit
FLASH->CR &= ~FLASH_CR_SER;
}
void flash_program_word(uint32_t address,uint32_t data,uint32_t byte_len){
// Wait if flash is busy
while(FLASH_BUSY);
// Check if flash is locked and unlock if needed
if(FLASH->CR & FLASH_CR_LOCK) {
flash_unlock();
}
// Set program bit 32bit programm size and Write data to address
if(byte_len == 1) {
FLASH_8BYTE;
FLASH->CR |= FLASH_CR_PG;
*(volatile uint8_t*)address = (uint8_t)data;
} else {
FLASH_32BYTE;
FLASH->CR |= FLASH_CR_PG;
*(volatile uint32_t*)address = data;
}
// Wait for programming to complete
while(FLASH_BUSY);
// Clear program bit
FLASH->CR &= ~FLASH_CR_PG;
}
void flash_write(uint32_t addr, FLASH_RECORD* record){
uint32_t* data = (uint32_t*)record;
uint32_t size = FLASH_RECORD_SIZE / 4; //count words in struct
// Wait if flash is busy
while(FLASH_BUSY);
// Check if flash is locked and unlock if needed
if(FLASH->CR & FLASH_CR_LOCK) {
flash_unlock();
}
// Set program bit and write data to flash
FLASH_32BYTE;
FLASH->CR |= FLASH_CR_PG;
for(int i = 0;i < size;i++){
2025-05-05 14:53:37 +03:00
*(volatile uint32_t*)(addr + (i * 4)) = data[i];
2025-04-16 22:51:39 +03:00
}
// Clear program bit
FLASH->CR &= ~FLASH_CR_PG;
2025-05-15 17:38:59 +03:00
write_ptr = addr + (size * 4); //increase variable storing addr
2025-04-16 22:51:39 +03:00
flash_lock();
}
uint8_t flash_read_word(uint32_t address){
// Check if address is valid
if(address < FLASH_BASE || address > FLASH_END) {
return 0;
}
// Read byte from flash memory
return *((volatile uint8_t*)address);
}
// Wait if flash
2025-05-13 18:55:55 +03:00
// bool validata_crc(FLASH_RECORD* crc){
// return crc->crc == 0x6933? true : false;
// }
2025-04-16 22:51:39 +03:00
uint16_t validate_crc16(uint8_t *data, uint32_t length) {
2025-04-18 12:50:07 +03:00
uint16_t crc = 0xFFFF; // start value for CRC MODBUS
2025-04-16 22:51:39 +03:00
while (length--) {
2025-04-18 12:50:07 +03:00
crc ^= *data++; // XOR
2025-04-16 22:51:39 +03:00
for (uint8_t i = 0; i < 8; i++) {
if (crc & 0x0001) {
2025-04-18 12:50:07 +03:00
crc = (crc >> 1) ^ 0xA001; // polynome 0x8005 (reverse)
2025-04-16 22:51:39 +03:00
} else {
crc >>= 1;
}
}
}
2025-04-18 12:50:07 +03:00
return crc;
2025-04-16 22:51:39 +03:00
}
2025-05-15 17:38:59 +03:00
uint16_t calc_crc_struct(FLASH_RECORD* res){
uint8_t arr_res[FLASH_RECORD_SIZE - 2];
uint16_t crc_res;
/* sorting data without CRC */
arr_res[0] = res->data_id;
arr_res[1] = res->data_type;
/* from 32 to 8 bit */
for(int i = 0;i < 4;i++)
arr_res[i + 2] = (uint8_t)(res->value >> i * 4);
crc_res = validate_crc16(arr_res,FLASH_RECORD_SIZE - 2);
return crc_res;
}
2025-04-16 22:51:39 +03:00
/* read struct from FLASH */
void flash_read(uint32_t addr,FLASH_RECORD* ptr){
uint8_t* flash_ptr = (uint8_t*)addr;
uint8_t* dest = (uint8_t*)ptr;
for(int i = 0;i < FLASH_RECORD_SIZE;i++)
dest[i] = flash_ptr[i];
}
void compact_page(){
FLASH_RECORD latest[PARAM_COUNT] = {0};
for(int i = (uint32_t)SECTOR_6;i < (uint32_t)SECTOR_7;i += FLASH_RECORD_SIZE) {
FLASH_RECORD rec;
flash_read(i,&rec);
2025-05-15 17:38:59 +03:00
uint16_t calculated_crc = calc_crc_struct(&rec);
2025-04-16 22:51:39 +03:00
if (calculated_crc == rec.crc && rec.data_id < PARAM_COUNT) {
2025-04-18 12:50:07 +03:00
// if the crc does not match, we check further
2025-04-16 22:51:39 +03:00
latest[rec.data_id] = rec;
}
else
2025-04-18 12:50:07 +03:00
// if
2025-04-16 22:51:39 +03:00
continue;
}
erase_sector(6);
write_ptr = SECTOR_6; // Сброс на начало
for (int i = 0; i < PARAM_COUNT; i++) {
if (latest[i].data_id != 0xFF) {
2025-04-18 12:50:07 +03:00
// alignment
2025-04-16 22:51:39 +03:00
if (write_ptr % 4 != 0) {
write_ptr += (4 - (write_ptr % 4));
}
flash_write(write_ptr, &latest[i]);
}
}
}
2025-05-15 17:38:59 +03:00
void write_param(uint8_t param_id, uint32_t val) {
FLASH_RECORD param_flash;
2025-04-18 12:50:07 +03:00
// __disable_irq(); // Interrupt off
2025-05-15 17:38:59 +03:00
param_flash.data_id = param_id;
param_flash.value = val;
param_flash.data_type = sizeof(uint8_t);
param_flash.crc = calc_crc_struct(&param_flash);
2025-04-16 22:51:39 +03:00
2025-05-15 17:38:59 +03:00
// check alignment
if (write_ptr % 8 != 0) {
write_ptr += (8 - (write_ptr % 8));
2025-04-16 22:51:39 +03:00
}
2025-04-18 12:50:07 +03:00
// check buffer overflow
2025-04-16 22:51:39 +03:00
if (write_ptr + FLASH_RECORD_SIZE >= SECTOR_6_END) {
2025-04-18 12:50:07 +03:00
compact_page(); // after compact_page update
// alignment
2025-05-15 17:38:59 +03:00
if (write_ptr % 8 != 0) {
write_ptr += (8 - (write_ptr % 8));
2025-04-16 22:51:39 +03:00
}
}
2025-04-18 12:50:07 +03:00
flash_write(write_ptr, &param_flash); //inside the function, the write_ptr pointer is automatically incremented by the size of the structure
2025-04-16 22:51:39 +03:00
2025-04-18 12:50:07 +03:00
// __enable_irq(); // Interrupt on
2025-04-16 22:51:39 +03:00
}
2025-05-15 17:38:59 +03:00
2025-04-16 22:51:39 +03:00
FLASH_RECORD* load_params(){
__disable_irq();
static FLASH_RECORD latest[PARAM_COUNT] = {0};
FLASH_RECORD res;
2025-05-15 17:38:59 +03:00
2025-04-16 22:51:39 +03:00
for(uint32_t addr = SECTOR_6;addr < SECTOR_6_END;addr +=FLASH_RECORD_SIZE) {
flash_read(addr,&res);
2025-05-15 17:38:59 +03:00
uint16_t calculated_crc = calc_crc_struct(&res);
if (calculated_crc != res.crc || res.data_id >= PARAM_COUNT) continue;
else{
2025-04-16 22:51:39 +03:00
latest[res.data_id] = res;
2025-05-15 17:38:59 +03:00
write_ptr = addr + FLASH_RECORD_SIZE;
}
2025-04-16 22:51:39 +03:00
}
__enable_irq();
return latest;
2025-05-15 17:38:59 +03:00
}