Use IPIs to wake up harts

This has the side effect of testing the IPI mechanism.  Still not sure
this is the best approach to booting, but it works...

4 files changed, 31 insertions, 37 deletions

diff --git a/pk/mentry.S b/pk/mentry.S
index f26e2dd..26ed83c 100644
--- a/pk/mentry.S
+++ b/pk/mentry.S
@@ -175,23 +175,20 @@ mentry:
   csrr a0, mhartid
   bnez a0, .LmultiHart
 
-  # boot hart 0
   csrw mscratch, sp
-  j machine_init
+  j init_first_hart
 
 .LmultiHart:
-  # mhartid may not be contiguous, so generate a hart id using AMOs
-  la a0, num_harts_booted
-  la a1, 1
-  amoadd.w a0, a1, (a0)
-
-  # allocate stack
-  sll t0, a0, RISCV_PGSHIFT
-  add sp, sp, t0
-  csrw mscratch, sp
 
-  # boot this hart
-  j machine_init
+  # wait for IPI to signal that it's time to boot
+  wfi
+  csrr a0, mip
+  andi a0, a0, MIP_MSIP
+  beqz a0, .LmultiHart
+
+  fence
+  csrr sp, mscratch
+  j init_other_hart
 
 .Linterrupt:
   sll a0, a0, 1    # discard MSB
diff --git a/pk/minit.c b/pk/minit.c
index 6ff6be9..712ea67 100644
--- a/pk/minit.c
+++ b/pk/minit.c
@@ -4,7 +4,6 @@
 
 uintptr_t mem_size;
 uint32_t num_harts;
-uint32_t num_harts_booted = 1;
 
 static void mstatus_init()
 {
@@ -55,16 +54,29 @@ static void fp_init()
 #endif
 }
 
-void hls_init(uint32_t hart_id, uintptr_t* csrs)
+void hls_init(uint32_t id, uintptr_t* csrs)
 {
-  hls_t* hls = OTHER_HLS(hart_id);
+  hls_t* hls = OTHER_HLS(id);
   memset(hls, 0, sizeof(*hls));
-  hls->hart_id = hart_id;
+  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;
+}
+
+static void init_hart()
+{
+  mstatus_init();
+  fp_init();
 }
 
-static void init_first_hart()
+void init_first_hart()
 {
+  init_hart();
+
   memset(HLS(), 0, sizeof(*HLS()));
   file_init();
   parse_device_tree();
@@ -77,8 +89,10 @@ static void init_first_hart()
   boot_loader(args);
 }
 
-static void init_other_hart(uint32_t hart_id)
+void init_other_hart()
 {
+  init_hart();
+
   // wait until virtual memory is enabled
   do {
     mb();
@@ -86,22 +100,5 @@ static void init_other_hart(uint32_t hart_id)
 
   write_csr(sptbr, root_page_table);
 
-  // then make sure we're in bounds
-  if (hart_id >= num_harts) {
-    while (1)
-      wfi();
-  }
-
   boot_other_hart();
 }
-
-void machine_init(uint32_t hart_id)
-{
-  mstatus_init();
-  fp_init();
-
-  if (hart_id == 0)
-    init_first_hart();
-  else
-    init_other_hart(hart_id);
-}
diff --git a/pk/mtrap.h b/pk/mtrap.h
index 375d549..61624d3 100644
--- a/pk/mtrap.h
+++ b/pk/mtrap.h
@@ -238,6 +238,7 @@ void hls_init(uint32_t hart_id, uintptr_t* csrs);
 
 // hart-local storage, at top of stack
 #define HLS() ((hls_t*)(MACHINE_STACK_TOP() - HLS_SIZE))
+#define OTHER_STACK_TOP(id) (MACHINE_STACK_TOP() + RISCV_PGSIZE * ((id) - HLS()->hart_id))
 #define OTHER_HLS(id) ((hls_t*)((void*)HLS() + RISCV_PGSIZE * ((id) - HLS()->hart_id)))
 
 #endif // !__ASSEMBLER__
diff --git a/pk/pk.h b/pk/pk.h
index 6aec2ad..08821b4 100644
--- a/pk/pk.h
+++ b/pk/pk.h
@@ -48,7 +48,6 @@ extern "C" {
 extern uintptr_t mem_size;
 extern int have_vm;
 extern uint32_t num_harts;
-extern uint32_t num_harts_booted;
 
 struct mainvars* parse_args(struct mainvars*);
 void printk(const char* s, ...);