1 files changed, 71 insertions, 35 deletions

diff --git a/machine/minit.c b/machine/minit.c
index 54ff88e..456de60 100644
--- a/machine/minit.c
+++ b/machine/minit.c
@@ -2,46 +2,54 @@
 #include "atomic.h"
 #include "vm.h"
 #include "fp_emulation.h"
+#include "fdt.h"
+#include "uart.h"
+#include "finisher.h"
+#include "disabled_hart_mask.h"
+#include "htif.h"
 #include <string.h>
 #include <limits.h>
 
 pte_t* root_page_table;
-uintptr_t first_free_paddr;
 uintptr_t mem_size;
-uintptr_t num_harts;
 volatile uint64_t* mtime;
 volatile uint32_t* plic_priorities;
 size_t plic_ndevs;
 
 static void mstatus_init()
 {
-  // Enable FPU and set VM mode
-  uintptr_t ms = 0;
-  ms = INSERT_FIELD(ms, MSTATUS_VM, VM_CHOICE);
-  ms = INSERT_FIELD(ms, MSTATUS_FS, 1);
-  ms = INSERT_FIELD(ms, MSTATUS_XS, 1);
-  write_csr(mstatus, ms);
-
-  // Make sure the hart actually supports the VM mode we want
-  ms = read_csr(mstatus);
-  assert(EXTRACT_FIELD(ms, MSTATUS_VM) == VM_CHOICE);
+  // Enable FPU
+  if (supports_extension('D') || supports_extension('F'))
+    write_csr(mstatus, MSTATUS_FS | MSTATUS_XS);
+  else
+    write_csr(mstatus, MSTATUS_XS);
 
   // Enable user/supervisor use of perf counters
-  write_csr(mucounteren, -1);
-  write_csr(mscounteren, -1);
-  write_csr(mie, ~MIP_MTIP); // disable timer; enable other interrupts
+  if (supports_extension('S'))
+    write_csr(scounteren, -1);
+  write_csr(mcounteren, -1);
+
+  // Enable software interrupts
+  write_csr(mie, MIP_MSIP);
+
+  // Disable paging
+  if (supports_extension('S'))
+    write_csr(sptbr, 0);
 }
 
 // send S-mode interrupts and most exceptions straight to S-mode
 static void delegate_traps()
 {
+  if (!supports_extension('S'))
+    return;
+
   uintptr_t interrupts = MIP_SSIP | MIP_STIP | MIP_SEIP;
   uintptr_t exceptions =
     (1U << CAUSE_MISALIGNED_FETCH) |
-    (1U << CAUSE_FAULT_FETCH) |
+    (1U << CAUSE_FETCH_PAGE_FAULT) |
     (1U << CAUSE_BREAKPOINT) |
-    (1U << CAUSE_FAULT_LOAD) |
-    (1U << CAUSE_FAULT_STORE) |
+    (1U << CAUSE_LOAD_PAGE_FAULT) |
+    (1U << CAUSE_STORE_PAGE_FAULT) |
     (1U << CAUSE_BREAKPOINT) |
     (1U << CAUSE_USER_ECALL);
 
@@ -53,11 +61,12 @@ static void delegate_traps()
 
 static void fp_init()
 {
+  if (!supports_extension('D') && !supports_extension('F'))
+    return;
+
   assert(read_csr(mstatus) & MSTATUS_FS);
 
 #ifdef __riscv_flen
-  if (!supports_extension('D'))
-    die("FPU not found; recompile pk with -msoft-float");
   for (int i = 0; i < 32; i++)
     init_fp_reg(i);
   write_csr(fcsr, 0);
@@ -75,16 +84,9 @@ hls_t* hls_init(uintptr_t id)
   return hls;
 }
 
-static uintptr_t sbi_top_paddr()
-{
-  extern char _end;
-  return ROUNDUP((uintptr_t)&_end, RISCV_PGSIZE);
-}
-
 static void memory_init()
 {
   mem_size = mem_size / MEGAPAGE_SIZE * MEGAPAGE_SIZE;
-  first_free_paddr = sbi_top_paddr() + num_harts * RISCV_PGSIZE;
 }
 
 static void hart_init()
@@ -130,34 +132,68 @@ static void hart_plic_init()
   *HLS()->plic_s_thresh = 0;
 }
 
-void init_first_hart()
+static void wake_harts()
 {
+  for (int hart = 0; hart < MAX_HARTS; ++hart)
+    if ((((~disabled_hart_mask & hart_mask) >> hart) & 1))
+      *OTHER_HLS(hart)->ipi = 1; // wakeup the hart
+}
+
+void init_first_hart(uintptr_t hartid, uintptr_t dtb)
+{
+  // Confirm console as early as possible
+  query_uart(dtb);
+  query_htif(dtb);
+
   hart_init();
   hls_init(0); // this might get called again from parse_config_string
-  parse_config_string();
+
+  // Find the power button early as well so die() works
+  query_finisher(dtb);
+
+  query_mem(dtb);
+  query_harts(dtb);
+  query_clint(dtb);
+  query_plic(dtb);
+
+  wake_harts();
+
   plic_init();
   hart_plic_init();
   //prci_test();
   memory_init();
-  boot_loader();
+  boot_loader(dtb);
 }
 
-void init_other_hart()
+void init_other_hart(uintptr_t hartid, uintptr_t dtb)
 {
   hart_init();
   hart_plic_init();
-  boot_other_hart();
+  boot_other_hart(dtb);
 }
 
-void enter_supervisor_mode(void (*fn)(uintptr_t), uintptr_t stack)
+void enter_supervisor_mode(void (*fn)(uintptr_t), uintptr_t arg0, uintptr_t arg1)
 {
+  // Set up a PMP to permit access to all of memory.
+  // Ignore the illegal-instruction trap if PMPs aren't supported.
+  uintptr_t pmpc = PMP_NAPOT | PMP_R | PMP_W | PMP_X;
+  asm volatile ("la t0, 1f\n\t"
+                "csrrw t0, mtvec, t0\n\t"
+                "csrw pmpaddr0, %1\n\t"
+                "csrw pmpcfg0, %0\n\t"
+                ".align 2\n\t"
+                "1: csrw mtvec, t0"
+                : : "r" (pmpc), "r" (-1UL) : "t0");
+
   uintptr_t mstatus = read_csr(mstatus);
   mstatus = INSERT_FIELD(mstatus, MSTATUS_MPP, PRV_S);
   mstatus = INSERT_FIELD(mstatus, MSTATUS_MPIE, 0);
   write_csr(mstatus, mstatus);
   write_csr(mscratch, MACHINE_STACK_TOP() - MENTRY_FRAME_SIZE);
   write_csr(mepc, fn);
-  write_csr(sptbr, (uintptr_t)root_page_table >> RISCV_PGSHIFT);
-  asm volatile ("mv a0, %0; mv sp, %0; mret" : : "r" (stack));
+
+  register uintptr_t a0 asm ("a0") = arg0;
+  register uintptr_t a1 asm ("a1") = arg1;
+  asm volatile ("mret" : : "r" (a0), "r" (a1));
   __builtin_unreachable();
 }