Add some infrastructure for multicore tests.

When compiling, define the number of harts. This means we only need to
allocate a lot of stack if there are a lot of harts.

2 files changed, 30 insertions, 8 deletions

diff --git a/debug/programs/entry.S b/debug/programs/entry.S
index 866636b..a2ea955 100755
--- a/debug/programs/entry.S
+++ b/debug/programs/entry.S
@@ -1,9 +1,8 @@
-#ifndef ENTRY_S
-#define ENTRY_S
-
 #include "encoding.h"
 
-#define STACK_SIZE 1024
+// Enough stack to store every register in case a trap handler is executed,
+// plus 33 more values.
+#define STACK_SIZE (64 * XLEN / 8)
 
 #if XLEN == 64
 # define LREG ld
@@ -62,10 +61,11 @@ handle_reset:
 .option pop
 
   # Initialize stack pointer.
-  # Support up to 4 harts, with IDs 0--3.
+  # Give each hart STACK_SIZE of stack.
+  # Assume hart IDs are contiguous and start at 0.
   csrr  t0, CSR_MHARTID
   addi  t0, t0, 1
-  li    t1, STACK_SIZE / 4
+  li    t1, STACK_SIZE
   mul   t0, t0, t1
   la    sp, stack_bottom
   add   sp, sp, t0
@@ -194,8 +194,7 @@ loop_forever:
   // Fill the stack with data so we can see if it was overrun.
   .align 4
 stack_bottom:
-  .fill STACK_SIZE/4, 4, 0x22446688
+  .fill NHARTS * STACK_SIZE/4, 4, 0x22446688
 stack_top:
 initialized:
   .word 0
-#endif
diff --git a/debug/programs/init.c b/debug/programs/init.c
index a2b41b0..9933c23 100644
--- a/debug/programs/init.c
+++ b/debug/programs/init.c
@@ -1,7 +1,30 @@
+#include "init.h"
+#include "encoding.h"
+
 int main(void);
 
+trap_handler_t trap_handler[NHARTS] = {0};
+
+void set_trap_handler(trap_handler_t handler)
+{
+    unsigned hartid = csr_read(mhartid);
+    trap_handler[hartid] = handler;
+}
+
+void enable_timer_interrupts()
+{
+    set_csr(mie, MIP_MTIP);
+    set_csr(mstatus, MSTATUS_MIE);
+}
+
 void handle_trap(unsigned int mcause, unsigned int mepc, unsigned int sp)
 {
+    unsigned hartid = csr_read(mhartid);
+    if (trap_handler[hartid]) {
+        trap_handler[hartid](hartid, mcause, mepc, sp);
+        return;
+    }
+
     while (1)
         ;
 }