From e8f59b6dc656c4f04e3027349b58c1d7532f47ed Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Mario=20H=C3=BCttel?= <mario.huettel@gmx.net>
Date: Mon, 21 Sep 2020 21:10:26 +0200
Subject: [PATCH] Implement automatic resotre of error mem corrupt flag

---
 stm-firmware/safety/safety-controller.c | 60 ++++++++++++++++++++++---
 1 file changed, 53 insertions(+), 7 deletions(-)

diff --git a/stm-firmware/safety/safety-controller.c b/stm-firmware/safety/safety-controller.c
index f36c923..8cc8168 100644
--- a/stm-firmware/safety/safety-controller.c
+++ b/stm-firmware/safety/safety-controller.c
@@ -313,12 +313,7 @@ static enum safety_flag flag_no_to_flag_enum(uint8_t no)
 	return (1U << no);
 }
 
-int safety_controller_report_error(enum safety_flag flag)
-{
-	return safety_controller_report_error_with_key(flag, 0x0UL);
-}
-
-int safety_controller_report_error_with_key(enum safety_flag flag, uint32_t key)
+static int report_error(enum safety_flag flag, uint32_t key, bool prevent_error_mem_enty)
 {
 	uint32_t i;
 	int ret = -1;
@@ -333,7 +328,7 @@ int safety_controller_report_error_with_key(enum safety_flag flag, uint32_t key)
 			flags[i].error_state_inv = !flags[i].error_state;
 			flags[i].key = key;
 
-			if (check_flag_persistent(&flags[i]) && !old_state) {
+			if (check_flag_persistent(&flags[i]) && !old_state && !prevent_error_mem_enty) {
 				err_mem_entry.counter = 1;
 				err_mem_entry.flag_num = flag_enum_to_flag_no(flags[i].flag);
 				err_mem_entry.type = SAFETY_MEMORY_ERR_ENTRY_FLAG;
@@ -349,6 +344,16 @@ int safety_controller_report_error_with_key(enum safety_flag flag, uint32_t key)
 	return ret;
 }
 
+int safety_controller_report_error(enum safety_flag flag)
+{
+	return safety_controller_report_error_with_key(flag, 0x0UL);
+}
+
+int safety_controller_report_error_with_key(enum safety_flag flag, uint32_t key)
+{
+	return report_error(flag, key, false);
+}
+
 void safety_controller_report_timing(enum timing_monitor monitor)
 {
 	volatile struct timing_mon *tim;
@@ -387,9 +392,42 @@ void safety_controller_report_analog_value(enum analog_value_monitor monitor, fl
 
 }
 
+/**
+ * @brief Return the flags, which are set in the error memory
+ * @param flags Flags read from error memory
+ * @return 0 if ok, != 0 if error
+ */
+static enum safety_flag get_safety_flags_from_error_mem(enum safety_flag *flags)
+{
+	uint32_t count;
+	uint32_t idx;
+	int res;
+	enum safety_flag return_flags = 0;
+	struct error_memory_entry entry;
+
+	if (!flags)
+		return -1001;
+
+	res = safety_memory_get_error_entry_count(&count);
+	if (res)
+		return -1;
+
+	for (idx = 0; idx < count; idx++) {
+		res = safety_memory_get_error_entry(idx, &entry);
+		if (entry.type == SAFETY_MEMORY_ERR_ENTRY_FLAG) {
+			return_flags |= flag_no_to_flag_enum(entry.flag_num);
+		}
+	}
+
+	*flags = return_flags;
+	return 0;
+}
+
 void safety_controller_init()
 {
 	enum safety_memory_state found_memory_state;
+	enum safety_flag flags_in_err_mem = ERR_FLAG_NO_FLAG;
+	int res;
 
 	/* Init the safety memory */
 	if (safety_memory_init(&found_memory_state)) {
@@ -407,6 +445,14 @@ void safety_controller_init()
 
 	if (found_memory_state == SAFETY_MEMORY_INIT_CORRUPTED)
 		safety_controller_report_error(ERR_FLAG_SAFETY_MEM_CORRUPT);
+	else if (found_memory_state == SAFETY_MEMORY_INIT_VALID_MEMORY) {
+		/* restore the corrupt flag flag */
+		res = get_safety_flags_from_error_mem(&flags_in_err_mem);
+		if (res)
+			panic_mode();
+		if (flags_in_err_mem & ERR_FLAG_SAFETY_MEM_CORRUPT)
+			report_error(ERR_FLAG_SAFETY_MEM_CORRUPT, 0, true);
+	}
 
 	/* Init default flag states */
 	safety_controller_report_error_with_key(ERR_FLAG_MEAS_ADC_OFF | ERR_FLAG_MEAS_ADC_UNSTABLE,