From e7ff2cf15013224ee8ecce8e1bda81a9385a6c67 Mon Sep 17 00:00:00 2001
From: Yvan Tortorella <ytortorella@lagrev3.ee.ethz.ch>
Date: Tue, 5 Dec 2023 16:28:58 +0100
Subject: [PATCH] Dual core SMP execution in Carfield.

---
 sw/include/util.h |  7 +++++++
 sw/lib/crt0.S     | 18 +++++++++++++-----
 2 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/sw/include/util.h b/sw/include/util.h
index 46dcedb2..c88fb698 100644
--- a/sw/include/util.h
+++ b/sw/include/util.h
@@ -80,3 +80,10 @@ static inline void *gprw(void *gp) {
     if (!(cond)) return (ret);
 
 #define MIN(a, b) (((a) <= (b)) ? (a) : (b))
+
+// Read hart ID
+static inline unsigned int hart_id() {
+  int hart_id;
+  asm volatile("csrr %0, mhartid" : "=r" (hart_id) : );
+  return hart_id;
+}
diff --git a/sw/lib/crt0.S b/sw/lib/crt0.S
index c16dc390..b4ff8166 100644
--- a/sw/lib/crt0.S
+++ b/sw/lib/crt0.S
@@ -15,8 +15,10 @@ _start:
     csrrc x0, mstatus, 10
 
     // Park SMP harts
-    csrr t0, mhartid
-    beqz t0, 2f
+    // csrr t0, mhartid
+    // beqz t0, 2f
+    // All harts continue.
+    beq t0, t0, 2f
 1:
     wfi
     j 1b
@@ -110,13 +112,19 @@ _fp_init:
 
     // Set FS state to "Clean"
     csrrc x0, mstatus, t1
+    // Non SMP Hart initializes the SoC
+    // SMP Harts jump to main
+    csrr t0, mhartid
+    beqz t0, _soc_init
+    j _program_start
 
-    // Full fence, then init SoC
-    fence
-
+_soc_init:
     // Init SoC, than jump to main
     jal x1, soc_init
 
+_program_start:
+    // Full fence, then go to main
+    fence
     call main
 
 // If main returns, we end up here