Change boot procedure to not rely on IPIs

That procedure doesn't work correctly if hart 0 comes out of reset while
hart 1 is still in reset.

4 files changed, 32 insertions, 18 deletions

diff --git a/pk/devicetree.c b/pk/devicetree.c
index 3ea33d0..242f013 100644
--- a/pk/devicetree.c
+++ b/pk/devicetree.c
@@ -21,6 +21,7 @@ static void fdt_handle_cpu(const char* isa, uint32_t* reg_addr, uint32_t reg_len
   uintptr_t* base_addr = (uintptr_t*)(uintptr_t)fdt_read_uint64(reg_addr);
   debug_printk("at %p, ", base_addr);
   uintptr_t hart_id = *(uintptr_t*)(base_addr + CSR_MHARTID);
+  kassert(hart_id < MAX_HARTS);
   debug_printk("found hart %ld\n", hart_id);
   hls_init(hart_id, base_addr);
   num_harts++;
diff --git a/pk/mentry.S b/pk/mentry.S
index 26ed83c..7f0fc61 100644
--- a/pk/mentry.S
+++ b/pk/mentry.S
@@ -166,28 +166,35 @@ mentry:
   li x30, 0
   li x31, 0
 
+  csrr a0, mhartid
+  bnez a0, .LmultiHart
+
   # sp <- end of first full page after the end of the binary
   la sp, _end + 2*RISCV_PGSIZE - 1
   li t0, -RISCV_PGSIZE
   and sp, sp, t0
   addi sp, sp, -MENTRY_FRAME_SIZE
 
-  csrr a0, mhartid
-  bnez a0, .LmultiHart
-
   csrw mscratch, sp
   j init_first_hart
 
 .LmultiHart:
+  # make sure our hart id is within a valid range
+  li a1, MAX_HARTS
+  bgeu a0, a1, .LmultiHart
 
-  # wait for IPI to signal that it's time to boot
-  wfi
-  csrr a0, mip
-  andi a0, a0, MIP_MSIP
-  beqz a0, .LmultiHart
-
+  # signal we're ready to boot
+  csrw mscratch, x0
   fence
-  csrr sp, mscratch
+  li a1, 1
+  sll a1, a1, a0
+  la a2, booted_harts_mask
+  amoor.w x0, a1, (a2)
+
+  # wait for main hart to grant us a stack
+1:csrr sp, mscratch
+  beqz sp, 1b
+
   j init_other_hart
 
 .Linterrupt:
diff --git a/pk/minit.c b/pk/minit.c
index 712ea67..91e2c9c 100644
--- a/pk/minit.c
+++ b/pk/minit.c
@@ -2,6 +2,7 @@
 #include "mtrap.h"
 #include "devicetree.h"
 
+volatile uint32_t booted_harts_mask;
 uintptr_t mem_size;
 uint32_t num_harts;
 
@@ -61,10 +62,13 @@ void hls_init(uint32_t id, uintptr_t* csrs)
   hls->hart_id = id;
   hls->csrs = csrs;
 
-  // tell the hart where its stack is, then wake it up
-  csrs[CSR_MSCRATCH] = (uintptr_t)(OTHER_STACK_TOP(id) - MENTRY_FRAME_SIZE);
-  mb();
-  csrs[CSR_MIPI] = 0;
+  if (id != 0) {
+    while (((booted_harts_mask >> id) & 1) == 0)
+      ;
+    mb();
+    // wake up the hart by granting it a stack
+    csrs[CSR_MSCRATCH] = (uintptr_t)(OTHER_STACK_TOP(id) - MENTRY_FRAME_SIZE);
+  }
 }
 
 static void init_hart()
@@ -94,10 +98,9 @@ void init_other_hart()
   init_hart();
 
   // wait until virtual memory is enabled
-  do {
-    mb();
-  } while (root_page_table == NULL);
-
+  while (*(pte_t* volatile*)&root_page_table == NULL)
+    ;
+  mb();
   write_csr(sptbr, root_page_table);
 
   boot_other_hart();
diff --git a/pk/pk.h b/pk/pk.h
index 08821b4..b8e9772 100644
--- a/pk/pk.h
+++ b/pk/pk.h
@@ -3,6 +3,8 @@
 #ifndef _PK_H
 #define _PK_H
 
+#define MAX_HARTS 32 // coupled to width of booted_harts_mask
+
 #ifndef __ASSEMBLER__
 
 #define debug_printk(s, ...) //printk(s, __VA_ARGS__)
@@ -48,6 +50,7 @@ extern "C" {
 extern uintptr_t mem_size;
 extern int have_vm;
 extern uint32_t num_harts;
+extern volatile uint32_t booted_harts_mask;
 
 struct mainvars* parse_args(struct mainvars*);
 void printk(const char* s, ...);