From 211b7501fb269de7603c5af0dd74924b44292ad9 Mon Sep 17 00:00:00 2001
From: Felipe Martinez <pipernene@gmail.com>
Date: Wed, 18 Dec 2024 22:10:39 +0000
Subject: [PATCH] Fix state loads

---
 include/memory.h                  |  4 +++-
 src/memory.c                      |  6 +++---
 src/nrf52832.c                    |  4 ++--
 src/peripherals/dcb.c             |  1 +
 src/peripherals/dwt.c             |  1 +
 src/peripherals/nrf52832/ccm.c    |  1 +
 src/peripherals/nrf52832/clock.c  |  1 +
 src/peripherals/nrf52832/comp.c   |  1 +
 src/peripherals/nrf52832/ecb.c    |  1 +
 src/peripherals/nrf52832/gpio.c   |  1 +
 src/peripherals/nrf52832/gpiote.c |  1 +
 src/peripherals/nrf52832/power.c  | 23 ++++++++++++-----------
 src/peripherals/nrf52832/ppi.c    |  1 +
 src/peripherals/nrf52832/radio.c  |  1 +
 src/peripherals/nrf52832/rng.c    |  1 +
 src/peripherals/nrf52832/saadc.c  |  1 +
 src/peripherals/nrf52832/temp.c   |  1 +
 src/peripherals/nrf52832/wdt.c    |  1 +
 src/peripherals/nvic.c            |  2 ++
 src/peripherals/scb.c             |  4 +++-
 20 files changed, 39 insertions(+), 18 deletions(-)

diff --git a/include/memory.h b/include/memory.h
index 558d1cf..4cb8b74 100644
--- a/include/memory.h
+++ b/include/memory.h
@@ -51,6 +51,8 @@ typedef enum {
 #define OP_INVALID_SIZE return MEMREG_RESULT_INVALID_SIZE
 #endif
 
+#define OP_IGNORE_LOAD_DATA if ((op) == OP_LOAD_DATA) { return MEMREG_RESULT_OK; }
+
 #define OP_ASSERT_SIZE(op, size)  if ((op) != OP_READ_##size && (op) != OP_WRITE_##size) { OP_INVALID_SIZE; }
 #define OP_ASSERT_READ(op)  if ((op) < 0) { OP_INVALID_ACCESS; }
 #define OP_ASSERT_WRITE(op) if ((op) > 0) { OP_INVALID_ACCESS; }
@@ -90,11 +92,11 @@ typedef struct memory_map_t memory_map_t;
 
 memory_map_t *memory_map_new();
 void memory_map_free(memory_map_t *);
-void memory_map_reset(memory_map_t *);
 void memory_map_add_region(memory_map_t *, memreg_t *region);
 memreg_t *memory_map_get_region(memory_map_t *, uint32_t addr);
 
 void memory_map_do_operation(memory_map_t *, uint32_t addr, memreg_op_t op, uint32_t *value);
+void memory_map_do_operation_all(memory_map_t *, memreg_op_t op);
 uint32_t memory_map_find_data(memory_map_t *map, uint32_t start_addr, uint32_t search_length, const uint8_t *data, size_t data_size);
 
 inline static uint32_t memory_map_read(memory_map_t *map, uint32_t addr)
diff --git a/src/memory.c b/src/memory.c
index a5e6e08..f5b5506 100644
--- a/src/memory.c
+++ b/src/memory.c
@@ -182,7 +182,7 @@ void memory_map_free(memory_map_t *map)
     assert(false); // TODO: Implement
 }
 
-void memory_map_reset(memory_map_t *map)
+void memory_map_do_operation_all(memory_map_t *map, memreg_op_t op)
 {
     for (size_t i = 0; i < BUCKET_COUNT(1); i++)
     {
@@ -191,7 +191,7 @@ void memory_map_reset(memory_map_t *map)
             membucket_t *bucket = &map->buckets[i][j];
 
             for (size_t k = 0; k < bucket->regions_count; k++)
-                memreg_reset(bucket->regions[k]);
+                bucket->regions[k]->operation(0, 0, NULL, op, bucket->regions[k]->userdata);
         }
     }
 }
@@ -259,7 +259,7 @@ void memory_map_do_operation(memory_map_t *map, uint32_t addr, memreg_op_t op, u
         }
     }
 
-    if (handled || op == OP_LOAD_DATA) // OP_LOAD_DATA is optional
+    if (handled)
         return;
 
     switch (result)
diff --git a/src/nrf52832.c b/src/nrf52832.c
index d8ba9cd..13b0ad8 100644
--- a/src/nrf52832.c
+++ b/src/nrf52832.c
@@ -170,7 +170,7 @@ void nrf52832_reset(NRF52832_t *nrf52832)
 {
     nrf52832->cycle_counter = 0;
 
-    memory_map_reset(nrf52832->mem);
+    memory_map_do_operation_all(nrf52832->mem, OP_RESET);
     pins_reset(nrf52832->pins);
     bus_spi_reset(nrf52832->bus_spi);
     i2c_reset(nrf52832->bus_i2c);
@@ -259,5 +259,5 @@ uint64_t nrf52832_get_cycle_counter(NRF52832_t *nrf)
 
 void nrf52832_reload_state(NRF52832_t *nrf)
 {
-    memory_map_do_operation(nrf->mem, 0, OP_LOAD_DATA, NULL);
+    memory_map_do_operation_all(nrf->mem, OP_LOAD_DATA);
 }
diff --git a/src/peripherals/dcb.c b/src/peripherals/dcb.c
index cadbcd0..652f101 100644
--- a/src/peripherals/dcb.c
+++ b/src/peripherals/dcb.c
@@ -20,6 +20,7 @@ OPERATION(dcb)
         return MEMREG_RESULT_OK;
     }
 
+    OP_IGNORE_LOAD_DATA
     OP_ASSERT_SIZE(op, WORD);
 
     switch (offset)
diff --git a/src/peripherals/dwt.c b/src/peripherals/dwt.c
index c0c19ef..91f7765 100644
--- a/src/peripherals/dwt.c
+++ b/src/peripherals/dwt.c
@@ -21,6 +21,7 @@ OPERATION(dwt)
         return MEMREG_RESULT_OK;
     }
 
+    OP_IGNORE_LOAD_DATA
     OP_ASSERT_SIZE(op, WORD);
 
     switch (offset)
diff --git a/src/peripherals/nrf52832/ccm.c b/src/peripherals/nrf52832/ccm.c
index baf3f31..28e8194 100644
--- a/src/peripherals/nrf52832/ccm.c
+++ b/src/peripherals/nrf52832/ccm.c
@@ -14,6 +14,7 @@ OPERATION(ccm)
         return MEMREG_RESULT_OK;
     }
 
+    OP_IGNORE_LOAD_DATA
     OP_ASSERT_SIZE(op, WORD);
 
     return MEMREG_RESULT_OK;
diff --git a/src/peripherals/nrf52832/clock.c b/src/peripherals/nrf52832/clock.c
index 0f32544..f845be1 100644
--- a/src/peripherals/nrf52832/clock.c
+++ b/src/peripherals/nrf52832/clock.c
@@ -56,6 +56,7 @@ OPERATION(clock)
         return MEMREG_RESULT_OK;
     }
 
+    OP_IGNORE_LOAD_DATA
     OP_ASSERT_SIZE(op, WORD);
 
     switch (offset)
diff --git a/src/peripherals/nrf52832/comp.c b/src/peripherals/nrf52832/comp.c
index 575e0a0..ad26e4a 100644
--- a/src/peripherals/nrf52832/comp.c
+++ b/src/peripherals/nrf52832/comp.c
@@ -10,6 +10,7 @@ OPERATION(comp)
     if (op == OP_RESET)
         return MEMREG_RESULT_OK;
 
+    OP_IGNORE_LOAD_DATA
     OP_ASSERT_SIZE(op, WORD);
 
     // COMP_t *comp = (COMP_t *)userdata;
diff --git a/src/peripherals/nrf52832/ecb.c b/src/peripherals/nrf52832/ecb.c
index 7cb1e54..be9ae97 100644
--- a/src/peripherals/nrf52832/ecb.c
+++ b/src/peripherals/nrf52832/ecb.c
@@ -56,6 +56,7 @@ OPERATION(ecb)
         return MEMREG_RESULT_OK;
     }
 
+    OP_IGNORE_LOAD_DATA
     OP_ASSERT_SIZE(op, WORD);
 
     switch (offset)
diff --git a/src/peripherals/nrf52832/gpio.c b/src/peripherals/nrf52832/gpio.c
index 156b89f..7996e5f 100644
--- a/src/peripherals/nrf52832/gpio.c
+++ b/src/peripherals/nrf52832/gpio.c
@@ -64,6 +64,7 @@ OPERATION(gpio)
         return MEMREG_RESULT_OK;
     }
 
+    OP_IGNORE_LOAD_DATA
     OP_ASSERT_SIZE(op, WORD);
 
     // PIN_CNF[n]
diff --git a/src/peripherals/nrf52832/gpiote.c b/src/peripherals/nrf52832/gpiote.c
index 1e4f4f0..c347c94 100644
--- a/src/peripherals/nrf52832/gpiote.c
+++ b/src/peripherals/nrf52832/gpiote.c
@@ -118,6 +118,7 @@ OPERATION(gpiote)
         return MEMREG_RESULT_OK;
     }
 
+    OP_IGNORE_LOAD_DATA
     OP_ASSERT_SIZE(op, WORD);
 
     switch (offset)
diff --git a/src/peripherals/nrf52832/power.c b/src/peripherals/nrf52832/power.c
index 6249545..fce3a68 100644
--- a/src/peripherals/nrf52832/power.c
+++ b/src/peripherals/nrf52832/power.c
@@ -18,21 +18,22 @@ OPERATION(power)
         return MEMREG_RESULT_OK;
     }
 
+    OP_IGNORE_LOAD_DATA
     OP_ASSERT_SIZE(op, WORD);
 
     switch (offset)
     {
-        case 0x400: // RESETREAS
-            if (OP_IS_READ(op))
-                *value = power->resetreason;
-            else
-                power->resetreason &= ~*value;
-
-            return MEMREG_RESULT_OK;
-        
-        case 0x578: // DCDCEN
-            // Do nothing
-            return MEMREG_RESULT_OK;
+    case 0x400: // RESETREAS
+        if (OP_IS_READ(op))
+            *value = power->resetreason;
+        else
+            power->resetreason &= ~*value;
+
+        return MEMREG_RESULT_OK;
+
+    case 0x578: // DCDCEN
+        // Do nothing
+        return MEMREG_RESULT_OK;
     }
 
     return MEMREG_RESULT_UNHANDLED;
diff --git a/src/peripherals/nrf52832/ppi.c b/src/peripherals/nrf52832/ppi.c
index 7ead7de..62579a4 100644
--- a/src/peripherals/nrf52832/ppi.c
+++ b/src/peripherals/nrf52832/ppi.c
@@ -87,6 +87,7 @@ OPERATION(ppi)
         return MEMREG_RESULT_OK;
     }
 
+    OP_IGNORE_LOAD_DATA
     OP_ASSERT_SIZE(op, WORD);
 
     switch (offset)
diff --git a/src/peripherals/nrf52832/radio.c b/src/peripherals/nrf52832/radio.c
index 8644df5..6c57cc8 100644
--- a/src/peripherals/nrf52832/radio.c
+++ b/src/peripherals/nrf52832/radio.c
@@ -441,6 +441,7 @@ OPERATION(radio)
         return MEMREG_RESULT_OK;
     }
 
+    OP_IGNORE_LOAD_DATA //TODO: Implement
     OP_ASSERT_SIZE(op, WORD);
 
     switch (offset)
diff --git a/src/peripherals/nrf52832/rng.c b/src/peripherals/nrf52832/rng.c
index a6e0f32..2f29a20 100644
--- a/src/peripherals/nrf52832/rng.c
+++ b/src/peripherals/nrf52832/rng.c
@@ -55,6 +55,7 @@ OPERATION(rng)
         return MEMREG_RESULT_OK;
     }
 
+    OP_IGNORE_LOAD_DATA
     OP_ASSERT_SIZE(op, WORD);
 
     switch (offset)
diff --git a/src/peripherals/nrf52832/saadc.c b/src/peripherals/nrf52832/saadc.c
index 04d0863..8993795 100644
--- a/src/peripherals/nrf52832/saadc.c
+++ b/src/peripherals/nrf52832/saadc.c
@@ -23,6 +23,7 @@ OPERATION(saadc)
         return MEMREG_RESULT_OK;
     }
 
+    OP_IGNORE_LOAD_DATA
     OP_ASSERT_SIZE(op, WORD);
 
     // TODO: Implement
diff --git a/src/peripherals/nrf52832/temp.c b/src/peripherals/nrf52832/temp.c
index ecc412b..31f67ae 100644
--- a/src/peripherals/nrf52832/temp.c
+++ b/src/peripherals/nrf52832/temp.c
@@ -20,6 +20,7 @@ OPERATION(temp)
         return MEMREG_RESULT_OK;
     }
 
+    OP_IGNORE_LOAD_DATA
     OP_ASSERT_SIZE(op, WORD);
 
     if (offset >= 0x520 && offset <= 0x534)
diff --git a/src/peripherals/nrf52832/wdt.c b/src/peripherals/nrf52832/wdt.c
index 643171d..0d2b4a6 100644
--- a/src/peripherals/nrf52832/wdt.c
+++ b/src/peripherals/nrf52832/wdt.c
@@ -24,6 +24,7 @@ OPERATION(wdt)
         return MEMREG_RESULT_OK;
     }
 
+    OP_IGNORE_LOAD_DATA
     OP_ASSERT_SIZE(op, WORD);
 
     // TODO: Make watchdog do something
diff --git a/src/peripherals/nvic.c b/src/peripherals/nvic.c
index 083bb74..7a53592 100644
--- a/src/peripherals/nvic.c
+++ b/src/peripherals/nvic.c
@@ -28,6 +28,8 @@ OPERATION(nvic)
         return MEMREG_RESULT_OK;
     }
 
+    OP_IGNORE_LOAD_DATA
+
     // NVIC_ISER[n] and NVIC_ICER[n]
     if (offset <= 0x40 || (offset >= 0x80 && offset <= 0xBC))
     {
diff --git a/src/peripherals/scb.c b/src/peripherals/scb.c
index 8d89b7f..18c51fb 100644
--- a/src/peripherals/scb.c
+++ b/src/peripherals/scb.c
@@ -36,6 +36,8 @@ OPERATION(scb)
         return MEMREG_RESULT_OK;
     }
 
+    OP_IGNORE_LOAD_DATA
+
     switch (offset)
     {
     case 0x00: // CPUID
@@ -44,7 +46,7 @@ OPERATION(scb)
 
         *value = 0x410FC241; // ARM Cortex-M4
         return MEMREG_RESULT_OK;
-    
+
     case 0x04: // ICSR
         OP_ASSERT_SIZE(op, WORD);