From 6232e2f330f62fbe94b8cbe5aab5c00e95489edc Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Mario=20H=C3=BCttel?= <mario.huettel@gmx.net>
Date: Sun, 6 Sep 2020 01:40:10 +0200
Subject: [PATCH] Issue #18: Store permanent errors in safety backup RAM

---
 stm-firmware/safety/safety-controller.c | 17 ++++-
 stm-firmware/safety/safety-memory.c     | 95 ++++++++++++++++++++++++-
 2 files changed, 109 insertions(+), 3 deletions(-)

diff --git a/stm-firmware/safety/safety-controller.c b/stm-firmware/safety/safety-controller.c
index f38aa2e..a27477e 100644
--- a/stm-firmware/safety/safety-controller.c
+++ b/stm-firmware/safety/safety-controller.c
@@ -208,13 +208,28 @@ int safety_controller_report_error_with_key(enum safety_flag flag, uint32_t key)
 {
 	uint32_t i;
 	int ret = -1;
+	bool old_state;
+	int res;
+	struct error_memory_entry err_mem_entry;
 
 	for (i = 0; i < COUNT_OF(flags); i++) {
 		if (flags[i].flag & flag) {
+			old_state = flags[i].error_state;
 			flags[i].error_state = true;
 			flags[i].error_state_inv = !flags[i].error_state;
 			flags[i].key = key;
-			ret = 0;
+
+			if (flags[i].persistent && !old_state) {
+				err_mem_entry.counter = 1;
+				err_mem_entry.flag_num = i;
+				err_mem_entry.type = SAFETY_MEMORY_ERR_ENTRY_FLAG;
+				res = safety_memory_insert_error_entry(&err_mem_entry);
+				if (res) {
+					ret = -12;
+				}
+			} else {
+				ret = 0;
+			}
 		}
 	}
 
diff --git a/stm-firmware/safety/safety-memory.c b/stm-firmware/safety/safety-memory.c
index 864a8e2..6b21b63 100644
--- a/stm-firmware/safety/safety-memory.c
+++ b/stm-firmware/safety/safety-memory.c
@@ -107,6 +107,11 @@ return_val:
 	return ret;
 }
 
+static void safety_memory_write_header(const struct safety_memory_header *header)
+{
+	backup_ram_write_data(0UL, (uint32_t *)header, wordsize_of(*header));
+}
+
 static void safety_memory_write_new_header(void)
 {
 	struct safety_memory_header header;
@@ -120,7 +125,7 @@ static void safety_memory_write_new_header(void)
 	header.magic_i = ~SAFETY_MEMORY_MAGIC;
 
 	backup_ram_wipe();
-	backup_ram_write_data(0UL, (uint32_t *)&header, wordsize_of(header));
+	safety_memory_write_header(&header);
 }
 
 static int safety_memory_check_crc()
@@ -371,7 +376,93 @@ return_value:
 	return ret;
 }
 
-int safety_memory_insert_error_entry(struct error_memory_entry *entry);
+int safety_memory_insert_error_entry(struct error_memory_entry *entry)
+{
+	int res;
+	int ret = -0xFFFF;
+	uint32_t addr;
+	uint32_t data;
+	bool found;
+	uint32_t input_data;
+	struct error_memory_entry current_entry;
+
+	struct safety_memory_header header;
+
+	input_data = error_memory_entry_to_word(entry);
+
+	if (safety_memory_get_header(&header) != SAFETY_MEMORY_INIT_VALID_MEMORY) {
+		return -2000;
+	}
+
+	if (entry->type == SAFETY_MEMORY_ERR_ENTRY_NOP) {
+		/* Append to end */
+		if ((header.err_memory_end + 1U) < backup_ram_get_size_in_words()) {
+			/* Still fits in memory */
+			backup_ram_write_data(header.err_memory_end, &input_data, 1UL);
+			header.err_memory_end++;
+			safety_memory_write_header(&header);
+			safety_memory_gen_crc();
+			ret = 0;
+		}
+	} else if (entry->type == SAFETY_MEMORY_ERR_ENTRY_FLAG) {
+		found = false;
+		for (addr = header.err_memory_offset; addr < header.err_memory_end; addr++) {
+			res = backup_ram_get_data(addr, &data, 1UL);
+			if (res) {
+				ret = -1;
+				goto return_value;
+			}
+			res = word_to_error_memory_entry(data, &current_entry);
+			if (res) {
+				ret = -2;
+				goto return_value;
+			}
+
+			if (current_entry.type == SAFETY_MEMORY_ERR_ENTRY_FLAG &&
+					current_entry.flag_num == entry->flag_num) {
+				found = true;
+				break;
+			}
+
+			if (current_entry.type == SAFETY_MEMORY_ERR_ENTRY_NOP) {
+				found = true;
+				break;
+			}
+		}
+
+		if (!found) {
+			/* No suitable place found in memory. Append */
+
+			if ((addr + 1) < backup_ram_get_size_in_words()) {
+				backup_ram_write_data(addr, &input_data, 1UL);
+				header.err_memory_end++;
+				safety_memory_write_header(&header);
+			} else {
+				ret = -3;
+				goto return_value;
+			}
+		} else {
+			if (current_entry.type == SAFETY_MEMORY_ERR_ENTRY_NOP) {
+				backup_ram_write_data(addr, &input_data, 1UL);
+			} else {
+				current_entry.counter += entry->counter;
+				if (current_entry.counter < entry->counter)
+					current_entry.counter = 0xFFFF;
+				data = error_memory_entry_to_word(&current_entry);
+				backup_ram_write_data(addr, &data, 1UL);
+			}
+		}
+
+		safety_memory_gen_crc();
+		ret = 0;
+
+	} else {
+		ret = -1001;
+	}
+
+return_value:
+	return ret;
+}
 
 int safety_memory_insert_config_override(struct config_override *config_override);