clint: move hart wakeup till after all FDT parsing

The clint was wiping out information discovered by the plic.
Initialize hart stacks as they are discovered.
Then fill in clint+plic info
Then wake the harts.

3 files changed, 20 insertions, 6 deletions

diff --git a/machine/fdt.c b/machine/fdt.c
index 60b65c7..d388d04 100644
--- a/machine/fdt.c
+++ b/machine/fdt.c
@@ -166,6 +166,7 @@ void query_mem(uintptr_t fdt)
 ///////////////////////////////////////////// HART SCAN //////////////////////////////////////////
 
 static uint32_t hart_phandles[MAX_HARTS];
+uint64_t hart_mask;
 
 struct hart_scan {
   const struct fdt_scan_node *cpu;
@@ -221,7 +222,9 @@ static void hart_done(const struct fdt_scan_node *node, void *extra)
 
     if (scan->hart < MAX_HARTS) {
       hart_phandles[scan->hart] = scan->phandle;
+      hart_mask |= 1 << scan->hart;
       if (scan->hart >= num_harts) num_harts = scan->hart + 1;
+      hls_init(scan->hart);
     }
   }
 }
@@ -304,10 +307,9 @@ static void clint_done(const struct fdt_scan_node *node, void *extra)
       if (hart_phandles[hart] == phandle)
         break;
     if (hart < MAX_HARTS) {
-      hls_t *hls = hls_init(hart);
+      hls_t *hls = OTHER_HLS(hart);
       hls->ipi = (void*)(scan->reg + index * 4);
       hls->timecmp = (void*)(scan->reg + 0x4000 + (index * 8));
-      *hls->ipi = 1; // wakeup the hart
     }
     value += 4;
   }
@@ -407,10 +409,10 @@ static void plic_done(const struct fdt_scan_node *node, void *extra)
     value += 2;
   }
 #if 0
-  printm("PLIC: prio %x devs %d\n", (uint32_t)(uintptr_t)plic_priorities, plic_ndevs);
+  printm("PLIC: prio %x devs %d\r\n", (uint32_t)(uintptr_t)plic_priorities, plic_ndevs);
   for (int i = 0; i < MAX_HARTS; ++i) {
     hls_t *hls = OTHER_HLS(i);
-    printm("CPU %d: %x %x %x %x\n", i, (uint32_t)(uintptr_t)hls->plic_m_ie, (uint32_t)(uintptr_t)hls->plic_m_thresh, (uint32_t)(uintptr_t)hls->plic_s_ie, (uint32_t)(uintptr_t)hls->plic_s_thresh);
+    printm("CPU %d: %x %x %x %x\r\n", i, (uint32_t)(uintptr_t)hls->plic_m_ie, (uint32_t)(uintptr_t)hls->plic_m_thresh, (uint32_t)(uintptr_t)hls->plic_s_ie, (uint32_t)(uintptr_t)hls->plic_s_thresh);
   }
 #endif
 }
diff --git a/machine/fdt.h b/machine/fdt.h
index 41c1b52..7f44db6 100644
--- a/machine/fdt.h
+++ b/machine/fdt.h
@@ -59,4 +59,7 @@ void query_harts(uintptr_t fdt);
 void query_plic(uintptr_t fdt);
 void query_clint(uintptr_t fdt);
 
+// The hartids of available harts
+extern uint64_t hart_mask;
+
 #endif
diff --git a/machine/minit.c b/machine/minit.c
index 68c29ad..cfe1c43 100644
--- a/machine/minit.c
+++ b/machine/minit.c
@@ -121,6 +121,13 @@ static void hart_plic_init()
   *HLS()->plic_s_thresh = 0;
 }
 
+static void wake_harts()
+{
+  for (int hart = 0; hart < MAX_HARTS; ++hart)
+    if (((hart_mask >> hart) & 1))
+      *OTHER_HLS(hart)->ipi = 1; // wakeup the hart
+}
+
 void init_first_hart(uintptr_t hartid, uintptr_t dtb)
 {
   hart_init();
@@ -128,12 +135,14 @@ void init_first_hart(uintptr_t hartid, uintptr_t dtb)
 
   // Confirm console as early as possible
   query_uart(dtb);
-  printm("SBI console now online\n");
+  printm("SBI console now online\r\n");
 
   query_mem(dtb);
   query_harts(dtb);
-  query_plic(dtb);
   query_clint(dtb);
+  query_plic(dtb);
+
+  wake_harts();
 
   plic_init();
   hart_plic_init();