1 files changed, 44 insertions, 53 deletions

diff --git a/bbl/bbl.c b/bbl/bbl.c
index 487a416..1b96a9d 100644
--- a/bbl/bbl.c
+++ b/bbl/bbl.c
@@ -4,73 +4,64 @@
 #include "vm.h"
 #include "bits.h"
 #include "config.h"
+#include "fdt.h"
 #include <string.h>
 
-static kernel_elf_info info;
-static volatile int elf_loaded;
+static const void* entry_point;
+long disabled_hart_mask;
 
-static void supervisor_vm_init()
+static uintptr_t dtb_output()
 {
-  uintptr_t highest_va = DRAM_BASE - first_free_paddr;
-  mem_size = MIN(mem_size, highest_va - info.first_user_vaddr) & -MEGAPAGE_SIZE;
+  extern char _payload_end;
+  uintptr_t end = (uintptr_t) &_payload_end;
+  return (end + MEGAPAGE_SIZE - 1) / MEGAPAGE_SIZE * MEGAPAGE_SIZE;
+}
 
-  pte_t* sbi_pt = (pte_t*)(info.first_vaddr_after_user + info.load_offset);
-  memset(sbi_pt, 0, RISCV_PGSIZE);
-  pte_t* middle_pt = (void*)sbi_pt + RISCV_PGSIZE;
-#if __riscv_xlen == 32
-  size_t num_middle_pts = 1;
-  pte_t* root_pt = middle_pt;
-  memset(root_pt, 0, RISCV_PGSIZE);
-#else
-  size_t num_middle_pts = (-info.first_user_vaddr - 1) / GIGAPAGE_SIZE + 1;
-  pte_t* root_pt = (void*)middle_pt + num_middle_pts * RISCV_PGSIZE;
-  memset(middle_pt, 0, (num_middle_pts + 1) * RISCV_PGSIZE);
-  for (size_t i = 0; i < num_middle_pts; i++)
-    root_pt[(1<<RISCV_PGLEVEL_BITS)-num_middle_pts+i] = ptd_create(((uintptr_t)middle_pt >> RISCV_PGSHIFT) + i);
-#endif
+static void filter_dtb(uintptr_t source)
+{
+  uintptr_t dest = dtb_output();
+  uint32_t size = fdt_size(source);
+  memcpy((void*)dest, (void*)source, size);
 
-  for (uintptr_t vaddr = info.first_user_vaddr, paddr = vaddr + info.load_offset, end = info.first_vaddr_after_user;
-       paddr < DRAM_BASE + mem_size; vaddr += MEGAPAGE_SIZE, paddr += MEGAPAGE_SIZE) {
-    int l2_shift = RISCV_PGLEVEL_BITS + RISCV_PGSHIFT;
-    size_t l2_idx = (info.first_user_vaddr >> l2_shift) & ((1 << RISCV_PGLEVEL_BITS)-1);
-    l2_idx += ((vaddr - info.first_user_vaddr) >> l2_shift);
-    middle_pt[l2_idx] = pte_create(paddr >> RISCV_PGSHIFT, PTE_G | PTE_R | PTE_W | PTE_X);
-  }
+  // Remove information from the chained FDT
+  filter_harts(dest, &disabled_hart_mask);
+  filter_plic(dest);
+  filter_compat(dest, "riscv,clint0");
+  filter_compat(dest, "riscv,debug-013");
+}
+
+void boot_other_hart(uintptr_t unused __attribute__((unused)))
+{
+  const void* entry;
+  do {
+    entry = entry_point;
+    mb();
+  } while (!entry);
 
-  // map SBI at top of vaddr space
-  extern char _sbi_end;
-  uintptr_t num_sbi_pages = ((uintptr_t)&_sbi_end - DRAM_BASE - 1) / RISCV_PGSIZE + 1;
-  assert(num_sbi_pages <= (1 << RISCV_PGLEVEL_BITS));
-  for (uintptr_t i = 0; i < num_sbi_pages; i++) {
-    uintptr_t idx = (1 << RISCV_PGLEVEL_BITS) - num_sbi_pages + i;
-    sbi_pt[idx] = pte_create((DRAM_BASE / RISCV_PGSIZE) + i, PTE_G | PTE_R | PTE_X);
+  long hartid = read_csr(mhartid);
+  if ((1 << hartid) & disabled_hart_mask) {
+    while (1) {
+      __asm__ volatile("wfi");
+#ifdef __riscv_div
+      __asm__ volatile("div x0, x0, x0");
+#endif
+    }
   }
-  pte_t* sbi_pte = middle_pt + ((num_middle_pts << RISCV_PGLEVEL_BITS)-1);
-  assert(!*sbi_pte);
-  *sbi_pte = ptd_create((uintptr_t)sbi_pt >> RISCV_PGSHIFT);
 
-  mb();
-  root_page_table = root_pt;
-  write_csr(sptbr, (uintptr_t)root_pt >> RISCV_PGSHIFT);
+  enter_supervisor_mode(entry, hartid, dtb_output());
 }
 
-void boot_loader()
+void boot_loader(uintptr_t dtb)
 {
-  extern char _payload_start, _payload_end;
-  load_kernel_elf(&_payload_start, &_payload_end - &_payload_start, &info);
-  supervisor_vm_init();
+  extern char _payload_start;
+  filter_dtb(dtb);
 #ifdef PK_ENABLE_LOGO
   print_logo();
 #endif
+#ifdef PK_PRINT_DEVICE_TREE
+  fdt_print(dtb_output());
+#endif
   mb();
-  elf_loaded = 1;
-  enter_supervisor_mode((void *)info.entry, 0);
-}
-
-void boot_other_hart()
-{
-  while (!elf_loaded)
-    ;
-  mb();
-  enter_supervisor_mode((void *)info.entry, 0);
+  entry_point = &_payload_start;
+  boot_other_hart(0);
 }