diff --git a/firmware/src/can/can_endpoints.c b/firmware/src/can/can_endpoints.c index cc4f6cfc..9e843035 100644 --- a/firmware/src/can/can_endpoints.c +++ b/firmware/src/can/can_endpoints.c @@ -163,7 +163,7 @@ uint8_t avlos_save_config(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command uint8_t avlos_erase_config(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd) { - nvm_erase(); + nvm_erase_and_reset(); return AVLOS_RET_CALL; } diff --git a/firmware/src/nvm/flash_func.c b/firmware/src/nvm/flash_func.c index c015dd4e..5485c994 100644 --- a/firmware/src/nvm/flash_func.c +++ b/firmware/src/nvm/flash_func.c @@ -22,7 +22,10 @@ /// @retval None /// //============================================================================== -TM_RAMFUNC void flash_erase_page(uint32_t page_num) +#if defined(__GNUC__) + __attribute__((optimize("-O0"))) +#endif +PAC5XXX_RAMFUNC void flash_erase_page(uint32_t page_num) { // Enable Write Access to FLash Controller PAC55XX_MEMCTL->FLASHLOCK = FLASH_LOCK_ALLOW_WRITE_ERASE_FLASH; @@ -49,7 +52,10 @@ TM_RAMFUNC void flash_erase_page(uint32_t page_num) /// @retval None /// //============================================================================== -TM_RAMFUNC void flash_erase_key(uint32_t key) +#if defined(__GNUC__) + __attribute__((optimize("-O0"))) +#endif +PAC5XXX_RAMFUNC void flash_erase_key(uint32_t key) { // Enable Write Access to FLash Controller PAC55XX_MEMCTL->FLASHLOCK = FLASH_LOCK_ALLOW_WRITE_ERASE_FLASH; diff --git a/firmware/src/nvm/nvm.c b/firmware/src/nvm/nvm.c index a6f35222..efb7dee5 100644 --- a/firmware/src/nvm/nvm.c +++ b/firmware/src/nvm/nvm.c @@ -1,4 +1,3 @@ - // * This file is part of the Tinymovr-Firmware distribution // * (https://github.com/yconst/tinymovr-firmware). // * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou. @@ -25,64 +24,92 @@ static struct NVMStruct s; const uint32_t config_size = sizeof(struct NVMStruct); +uint32_t calculate_checksum(const uint8_t *data, size_t len) +{ + uint32_t checksum = 0; + for (size_t i = 0; i < len; ++i) + { + checksum += data[i]; + } + return ~checksum + 1; +} + bool nvm_save_config(void) { - bool commited = false; - uint8_t data[sizeof(struct NVMStruct)]; - s.node_id_1 = CAN_get_ID(); - s.node_id_2 = CAN_get_ID(); - frames_get_config(&(s.frames_config)); - s.adc_config = *ADC_get_config(); - s.motor_config = *motor_get_config(); - sensors_get_config(&(s.sensors_config)); - observers_get_config(&(s.observers_config)); - s.controller_config = *controller_get_config(); - s.can_config = *CAN_get_config(); - s.traj_planner_config = *traj_planner_get_config(); - strlcpy(s.version, GIT_VERSION, sizeof(s.version)); - memcpy(data, &s, sizeof(struct NVMStruct)); - if (CONTROLLER_STATE_IDLE == controller_get_state()) - { - uint8_t *dataBuffer = data; - __disable_irq(); - flash_erase_page(SETTINGS_PAGE); - flash_write((uint8_t *)SETTINGS_PAGE_HEX, dataBuffer, sizeof(struct NVMStruct)); - __enable_irq(); - commited = true; - } - return commited; + uint8_t data[sizeof(struct NVMStruct)]; + uint8_t readback_data[sizeof(struct NVMStruct)]; + + s.node_id_1 = CAN_get_ID(); + s.node_id_2 = CAN_get_ID(); + frames_get_config(&(s.frames_config)); + s.adc_config = *ADC_get_config(); + s.motor_config = *motor_get_config(); + sensors_get_config(&(s.sensors_config)); + observers_get_config(&(s.observers_config)); + s.controller_config = *controller_get_config(); + s.can_config = *CAN_get_config(); + s.traj_planner_config = *traj_planner_get_config(); + strncpy(s.version, GIT_VERSION, sizeof(s.version)); + + memcpy(data, &s, sizeof(struct NVMStruct) - sizeof(s.checksum)); + s.checksum = calculate_checksum(data, sizeof(struct NVMStruct) - sizeof(s.checksum)); + memcpy(data + sizeof(struct NVMStruct) - sizeof(s.checksum), &s.checksum, sizeof(s.checksum)); + + if (CONTROLLER_STATE_IDLE == controller_get_state()) + { + uint8_t *dataBuffer = data; + __disable_irq(); + nvm_erase(); + flash_write((uint8_t *)SETTINGS_PAGE_HEX, dataBuffer, sizeof(struct NVMStruct)); + __enable_irq(); + + memcpy(readback_data, (uint8_t *)SETTINGS_PAGE_HEX, sizeof(struct NVMStruct)); + if (memcmp(data, readback_data, sizeof(struct NVMStruct)) == 0) + { + return true; + } + } + return false; } bool nvm_load_config(void) { - memcpy(&s, (uint8_t *)SETTINGS_PAGE_HEX, sizeof(struct NVMStruct)); - // TODO: Also validate checksum - bool loaded = false; - char static_version[16]; - strlcpy(static_version, GIT_VERSION, sizeof(static_version)); - if (strcmp(s.version, static_version) == 0) + memcpy(&s, (uint8_t *)SETTINGS_PAGE_HEX, sizeof(struct NVMStruct)); + uint32_t calculated_checksum = calculate_checksum((uint8_t *)&s, sizeof(struct NVMStruct) - sizeof(s.checksum)); + if (calculated_checksum != s.checksum) + { + return false; + } + + if (strncmp(s.version, GIT_VERSION, sizeof(s.version)) == 0) + { + frames_restore_config(&s.frames_config); + ADC_restore_config(&s.adc_config); + motor_restore_config(&s.motor_config); + sensors_restore_config(&s.sensors_config); + observers_restore_config(&s.observers_config); + controller_restore_config(&s.controller_config); + CAN_restore_config(&s.can_config); + traj_planner_restore_config(&s.traj_planner_config); + return true; + } + return false; +} + +void nvm_erase(void) +{ + for (uint8_t i = 0; i < SETTINGS_PAGE_COUNT; i++) { - frames_restore_config(&s.frames_config); - ADC_restore_config(&s.adc_config); - motor_restore_config(&s.motor_config); - sensors_restore_config(&s.sensors_config); - observers_restore_config(&s.observers_config); - controller_restore_config(&s.controller_config); - CAN_restore_config(&s.can_config); - traj_planner_restore_config(&s.traj_planner_config); - loaded = true; + flash_erase_page(SETTINGS_PAGE + i); } - return loaded; } -void nvm_erase(void) +// This separate function is needed to interface with the protocol +void nvm_erase_and_reset(void) { if (CONTROLLER_STATE_IDLE == controller_get_state()) - { - for (uint8_t i=0; i