Merge pull request #363 from tinymovr/firmware/validate_nvm_write

Validate data written to nvm
tinymovr · Jul 8, 2024 · 780ecbe · 780ecbe
2 parents 4a12c9c + 057534b
commit 780ecbe
Show file tree

Hide file tree

Showing 5 changed files with 87 additions and 52 deletions.
diff --git 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
@@ -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
@@ -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<SETTINGS_PAGE_COUNT; i++)
-		{
-			flash_erase_page(SETTINGS_PAGE + i);
-		}
+    {
+		nvm_erase();
 		system_reset();
 	}
 }
diff --git a/firmware/src/nvm/nvm.h b/firmware/src/nvm/nvm.h
@@ -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.
@@ -38,6 +37,7 @@ struct NVMStruct {
     CANConfig can_config;
     TrajPlannerConfig traj_planner_config;
     char version[16];
+    uint32_t checksum;
 };
 
 #define SETTINGS_PAGE (120)
@@ -50,4 +50,6 @@ extern const uint32_t config_size;
 bool nvm_save_config(void);
 bool nvm_load_config(void);
 void nvm_erase(void);
+void nvm_erase_and_reset(void);
+uint32_t calculate_checksum(const uint8_t *data, size_t len);
 
diff --git a/studio/Python/tests/test_board_config.py → studio/Python/tests/test_nvm.py b/studio/Python/tests/test_board_config.py → studio/Python/tests/test_nvm.py
@@ -30,7 +30,7 @@
 tsleep = 0.30
 
 
-class TestBoardConfig(TMTestCase):
+class TestNVM(TMTestCase):
     def test_a_state_errors(self):
         """
         Test state transitions