From 90a4c57a4232667802b90346bb6d0c823d79ad42 Mon Sep 17 00:00:00 2001
From: Michael Rogenmoser <michael@rogenmoser.us>
Date: Fri, 10 Feb 2023 11:16:38 +0100
Subject: [PATCH] Update critical section code for functionality

---
 include/hal/hmr/hmr_v1.h | 14 ++++++++++++++
 kernel/cluster.c         |  8 +++++++-
 kernel/crt0.S            |  2 ++
 kernel/hmr_synch.c       | 11 +++--------
 4 files changed, 26 insertions(+), 9 deletions(-)

diff --git a/include/hal/hmr/hmr_v1.h b/include/hal/hmr/hmr_v1.h
index 0bf2a7f..c184809 100644
--- a/include/hal/hmr/hmr_v1.h
+++ b/include/hal/hmr/hmr_v1.h
@@ -121,6 +121,16 @@ static inline void hmr_enable_tmr(unsigned int cid, unsigned int tmr_id) {
   pulp_write32(ARCHI_HMR_GLOBAL_ADDR(cid) + HMR_TMR_OFFSET + HMR_TMR_INCREMENT*tmr_id + HMR_TMR_REGS_TMR_ENABLE_REG_OFFSET, 1<<HMR_TMR_REGS_TMR_ENABLE_TMR_ENABLE_BIT);
 }
 
+static inline void hmr_self_enable_tmr() {
+  if (TMR_IS_MAIN_CORE(core_id())) {
+    eu_bar_setup(eu_bar_addr(TMR_BARRIER_ID(TMR_GROUP_ID(core_id()))), TMR_BARRIER_SETUP(TMR_GROUP_ID(core_id())));
+    pulp_write32(ARCHI_HMR_ADDR + HMR_TMR_OFFSET + HMR_TMR_INCREMENT*core_id() + HMR_TMR_REGS_TMR_ENABLE_REG_OFFSET, 1<<HMR_TMR_REGS_TMR_ENABLE_TMR_ENABLE_BIT);
+    while (hmr_get_core_status(0, core_id()) != 1<<HMR_CORE_REGS_CURRENT_MODE_TRIPLE_BIT) {
+      asm volatile ("nop");
+    }
+  }
+}
+
 static inline void hmr_disable_all_tmr(unsigned int cid) {
   hmr_set_tmr_status_all(cid, 0);
 }
@@ -137,6 +147,10 @@ static inline void hmr_set_tmr_config(unsigned int cid, unsigned int tmr_id, boo
     (rapid_recovery ? 1<<HMR_TMR_REGS_TMR_CONFIG_RAPID_RECOVERY_BIT : 0));
 }
 
+static inline unsigned int hmr_get_tmr_config(unsigned int cid, unsigned int tmr_id) {
+  return pulp_read32(ARCHI_HMR_GLOBAL_ADDR(0) + HMR_TMR_OFFSET + HMR_TMR_INCREMENT*tmr_id + HMR_TMR_REGS_TMR_CONFIG_REG_OFFSET);
+}
+
 static inline void hmr_set_tmr_config_all(unsigned int cid, bool delay_resynch, bool setback, bool reload_setback, bool rapid_recovery) {
   pulp_write32(ARCHI_HMR_GLOBAL_ADDR(0) + HMR_TOP_OFFSET + HMR_REGISTERS_TMR_CONFIG_REG_OFFSET,
     (delay_resynch  ? 1<<HMR_REGISTERS_TMR_CONFIG_DELAY_RESYNCH_BIT  : 0) |
diff --git a/kernel/cluster.c b/kernel/cluster.c
index e5adca6..edb7b27 100644
--- a/kernel/cluster.c
+++ b/kernel/cluster.c
@@ -41,7 +41,13 @@ static void cluster_core_init()
     eu_evt_maskSet((1<<PULP_DISPATCH_EVENT) | (1<<PULP_MUTEX_EVENT) | (1<<PULP_HW_BAR_EVENT));
 
 #ifdef ARCHI_HMR
-    eu_irq_maskSet(1<<24 | 1<<23); // Enable resynch and synch requests
+    // Enable resynch and synch requests
+    eu_irq_maskSet(1<<24 | 1<<23);
+    rt_irq_set_handler(24, pos_hmr_tmr_irq);
+    rt_irq_set_handler(23, pos_hmr_tmr_synch);
+    hal_spr_write(0x304, 1<<24|1<<23);
+    hal_irq_enable();
+
     eu_bar_setup(eu_bar_addr(0), hmr_get_active_cores(0));
 #else
     eu_bar_setup(eu_bar_addr(0), (1<<ARCHI_CLUSTER_NB_PE) - 1);
diff --git a/kernel/crt0.S b/kernel/crt0.S
index 3d1f19a..9b6ded9 100644
--- a/kernel/crt0.S
+++ b/kernel/crt0.S
@@ -153,6 +153,7 @@ pos_semihosting_call:
 
 #if defined(ARCHI_HAS_CLUSTER)
 pe_start:
+#if defined(ARCHI_HMR)
     csrr t0, 0xf14
     li   t1, ARCHI_HMR_ADDR + HMR_CORE_OFFSET
     andi t0, t0, 0x01f
@@ -160,6 +161,7 @@ pe_start:
     add  t0, t0, t1
     lw   t1, HMR_CORE_REGS_SP_STORE_REG_OFFSET(t0)
     bnez t1, pos_hmr_tmr_reload
+#endif
     la   x2, cluster_stacks
     lw   x2, 0(x2)
     li   x3, CLUSTER_STACK_SIZE
diff --git a/kernel/hmr_synch.c b/kernel/hmr_synch.c
index c11f220..8cc44bf 100644
--- a/kernel/hmr_synch.c
+++ b/kernel/hmr_synch.c
@@ -205,12 +205,8 @@ void __attribute__((naked)) pos_hmr_tmr_irq() {
 }
 
 void __attribute__((naked)) pos_hmr_tmr_synch() {
-  if (TMR_IS_MAIN_CORE(core_id())) {
-    eu_bar_setup(eu_bar_addr(TMR_BARRIER_ID(TMR_GROUP_ID(core_id()))), TMR_BARRIER_SETUP(TMR_GROUP_ID(core_id())));
-    // eu_bar_setup(eu_bar_addr(0), hmr_get_active_cores(0));
-  }
   pos_hmr_store_state_to_stack();
-  
+
   // store sp to hmr core reg
   __asm__ __volatile__(
     "csrr t0, 0xf14 \n\t" // Read core id
@@ -220,7 +216,7 @@ void __attribute__((naked)) pos_hmr_tmr_synch() {
     "add t0, t0, t1 \n\t"
     "sw sp, " QU(HMR_CORE_REGS_SP_STORE_REG_OFFSET) "(t0) \n\t"
   : : : "memory");
-  
+
   // enter barrier -> this should lock the cores together
   eu_bar_trig_wait_clr(eu_bar_addr(TMR_BARRIER_ID(TMR_GROUP_ID(core_id()))));
 
@@ -240,7 +236,7 @@ int hmr_tmr_critical_section(int (*function_handle)()) {
   int ret = 0; 
   if (TMR_IS_MAIN_CORE(core_id())) {
     // enter critical section
-    hmr_enable_tmr(0, TMR_GROUP_ID(core_id()));
+    hmr_self_enable_tmr();
 
     // do critical stuff
     ret += function_handle();
@@ -249,7 +245,6 @@ int hmr_tmr_critical_section(int (*function_handle)()) {
     hmr_disable_tmr(0, TMR_GROUP_ID(core_id()));
 
   }
-  synch_barrier();
 
   return ret;
 }