Make boot loader/SBI changes to support SMP Linux

13 files changed, 183 insertions, 61 deletions

diff --git a/pk/bbl.c b/pk/bbl.c
index f4f0f06..8d52b7d 100644
--- a/pk/bbl.c
+++ b/pk/bbl.c
@@ -1,6 +1,15 @@
 #include "pk.h"
 #include "vm.h"
 
+volatile int elf_loaded;
+
+static void enter_entry_point()
+{
+    write_csr(mepc, current.entry);
+    asm volatile("eret");
+    __builtin_unreachable();
+}
+
 void run_loaded_program(struct mainvars* args)
 {
   if (!current.is_supervisor)
@@ -10,7 +19,15 @@ void run_loaded_program(struct mainvars* args)
 #ifdef PK_ENABLE_LOGO
     print_logo();
 #endif
-    write_csr(mepc, current.entry);
-    asm volatile("eret");
-    __builtin_unreachable();
+    mb();
+    elf_loaded = 1;
+    enter_entry_point();
+}
+
+void boot_other_hart()
+{
+  while (!elf_loaded)
+    ;
+  mb();
+  enter_entry_point();
 }
diff --git a/pk/mcall.h b/pk/mcall.h
index 65db8e1..bae7aa6 100644
--- a/pk/mcall.h
+++ b/pk/mcall.h
@@ -6,7 +6,8 @@
 #define MCALL_SEND_DEVICE_REQUEST 2
 #define MCALL_RECEIVE_DEVICE_RESPONSE 3
 #define MCALL_SEND_IPI 4
-#define MCALL_SHUTDOWN 5
+#define MCALL_CLEAR_IPI 5
+#define MCALL_SHUTDOWN 6
 
 #ifndef __ASSEMBLER__
 
diff --git a/pk/mentry.S b/pk/mentry.S
index 98dc424..e9e794b 100644
--- a/pk/mentry.S
+++ b/pk/mentry.S
@@ -131,12 +131,32 @@ mentry:
   j bad_trap
 
   # Entry point for power-on reset (mtvec + 0x100)
-  # TODO per-hart stacks
+
+  # 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
+
+  # boot hart 0
+  csrw mscratch, sp
+  j machine_init
+
+.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
 
 .Linterrupt:
diff --git a/pk/minit.c b/pk/minit.c
index 47c19b0..a040e2b 100644
--- a/pk/minit.c
+++ b/pk/minit.c
@@ -3,6 +3,7 @@
 
 uintptr_t mem_size;
 uint32_t num_harts;
+uint32_t num_harts_booted = 1;
 
 static void mstatus_init()
 {
@@ -22,21 +23,18 @@ static void mstatus_init()
 
   if (EXTRACT_FIELD(ms, MSTATUS_VM) != VM_CHOICE)
     have_vm = 0;
+
+  clear_csr(mip, MIP_MSIP);
+  set_csr(mie, MIP_MSIP);
 }
 
 static void memory_init()
 {
   if (mem_size == 0)
     panic("could not determine memory capacity");
-}
 
-static void hart_init()
-{
   if (num_harts == 0)
     panic("could not determine number of harts");
-
-  if (num_harts != 1)
-    panic("TODO: SMP support");
 }
 
 static void fp_init()
@@ -54,23 +52,47 @@ static void fp_init()
 #endif
 }
 
-static void mailbox_init()
+static void hls_init(uint32_t hart_id)
 {
-  memset(MAILBOX(), 0, MAILBOX_SIZE);
+  memset(HLS(), 0, sizeof(*HLS()));
+  HLS()->hart_id = hart_id;
 }
 
-void machine_init()
+static void init_first_hart()
 {
-  mailbox_init();
   file_init();
 
   struct mainvars arg_buffer;
   struct mainvars *args = parse_args(&arg_buffer);
 
-  mstatus_init();
   memory_init();
-  hart_init();
-  fp_init();
   vm_init();
   boot_loader(args);
 }
+
+static void init_other_hart()
+{
+  // wait until virtual memory is enabled
+  while (root_page_table == NULL)
+    asm volatile ("" ::: "memory");
+  mb();
+  write_csr(sptbr, root_page_table);
+
+  // then make sure we're in bounds
+  if (HLS()->hart_id >= num_harts)
+    panic("too many harts");
+
+  boot_other_hart();
+}
+
+void machine_init(uint32_t hart_id)
+{
+  hls_init(hart_id);
+  mstatus_init();
+  fp_init();
+
+  if (hart_id == 0)
+    init_first_hart();
+  else
+    init_other_hart();
+}
diff --git a/pk/mtrap.c b/pk/mtrap.c
index c09a16a..77b562a 100644
--- a/pk/mtrap.c
+++ b/pk/mtrap.c
@@ -71,9 +71,9 @@ uintptr_t htif_interrupt(uintptr_t mcause, uintptr_t* regs)
   uintptr_t cmd = FROMHOST_CMD(fromhost);
   uintptr_t data = FROMHOST_DATA(fromhost);
 
-  sbi_device_message* m = MAILBOX()->device_request_queue_head;
+  sbi_device_message* m = HLS()->device_request_queue_head;
   sbi_device_message* prev = NULL;
-  for (size_t i = 0, n = MAILBOX()->device_request_queue_size; i < n; i++) {
+  for (size_t i = 0, n = HLS()->device_request_queue_size; i < n; i++) {
     if (!supervisor_paddr_valid(m, sizeof(*m))
         && EXTRACT_FIELD(read_csr(mstatus), MSTATUS_PRV1) != PRV_M)
       panic("htif: page fault");
@@ -86,16 +86,16 @@ uintptr_t htif_interrupt(uintptr_t mcause, uintptr_t* regs)
       if (prev)
         prev->sbi_private_data = (uintptr_t)next;
       else
-        MAILBOX()->device_request_queue_head = next;
-      MAILBOX()->device_request_queue_size = n-1;
+        HLS()->device_request_queue_head = next;
+      HLS()->device_request_queue_size = n-1;
       m->sbi_private_data = 0;
 
       // enqueue to response queue
-      if (MAILBOX()->device_response_queue_tail)
-        MAILBOX()->device_response_queue_tail->sbi_private_data = (uintptr_t)m;
+      if (HLS()->device_response_queue_tail)
+        HLS()->device_response_queue_tail->sbi_private_data = (uintptr_t)m;
       else
-        MAILBOX()->device_response_queue_head = m;
-      MAILBOX()->device_response_queue_tail = m;
+        HLS()->device_response_queue_head = m;
+      HLS()->device_response_queue_tail = m;
 
       // signal software interrupt
       set_csr(mip, MIP_SSIP);
@@ -109,6 +109,11 @@ uintptr_t htif_interrupt(uintptr_t mcause, uintptr_t* regs)
   panic("htif: no record");
 }
 
+static uintptr_t mcall_hart_id()
+{
+  return HLS()->hart_id;
+}
+
 static uintptr_t mcall_console_putchar(uint8_t ch)
 {
   while (swap_csr(mtohost, TOHOST_CMD(1, 1, ch)) != 0);
@@ -131,7 +136,7 @@ static uintptr_t mcall_console_putchar(uint8_t ch)
 
 static uintptr_t mcall_dev_req(sbi_device_message *m)
 {
-  //printm("req %d %p\n", MAILBOX()->device_request_queue_size, m);
+  //printm("req %d %p\n", HLS()->device_request_queue_size, m);
   if (!supervisor_paddr_valid(m, sizeof(*m))
       && EXTRACT_FIELD(read_csr(mstatus), MSTATUS_PRV1) != PRV_M)
     return -EFAULT;
@@ -142,9 +147,9 @@ static uintptr_t mcall_dev_req(sbi_device_message *m)
   while (swap_csr(mtohost, TOHOST_CMD(m->dev, m->cmd, m->data)) != 0)
     ;
 
-  m->sbi_private_data = (uintptr_t)MAILBOX()->device_request_queue_head;
-  MAILBOX()->device_request_queue_head = m;
-  MAILBOX()->device_request_queue_size++;
+  m->sbi_private_data = (uintptr_t)HLS()->device_request_queue_head;
+  HLS()->device_request_queue_head = m;
+  HLS()->device_request_queue_size++;
 
   return 0;
 }
@@ -153,13 +158,20 @@ static uintptr_t mcall_dev_resp()
 {
   htif_interrupt(0, 0);
 
-  sbi_device_message* m = MAILBOX()->device_response_queue_head;
+  sbi_device_message* m = HLS()->device_response_queue_head;
   if (m) {
     //printm("resp %p\n", m);
     sbi_device_message* next = (void*)atomic_read(&m->sbi_private_data);
-    MAILBOX()->device_response_queue_head = next;
-    if (!next)
-      MAILBOX()->device_response_queue_tail = 0;
+    HLS()->device_response_queue_head = next;
+    if (!next) {
+      HLS()->device_response_queue_tail = 0;
+
+      // only clear SSIP if no other events are pending
+      clear_csr(mip, MIP_SSIP);
+      mb();
+      if (HLS()->ipi_pending)
+        set_csr(mip, MIP_SSIP);
+    }
   }
   return (uintptr_t)m;
 }
@@ -168,10 +180,26 @@ static uintptr_t mcall_send_ipi(uintptr_t recipient)
 {
   if (recipient >= num_harts)
     return -1;
-  write_csr(send_ipi, recipient);
+
+  if (atomic_swap(&OTHER_HLS(recipient)->ipi_pending, 1) == 0) {
+    mb();
+    write_csr(send_ipi, recipient);
+  }
+
   return 0;
 }
 
+static uintptr_t mcall_clear_ipi()
+{
+  // only clear SSIP if no other events are pending
+  if (HLS()->device_response_queue_head == NULL) {
+    clear_csr(mip, MIP_SSIP);
+    mb();
+  }
+
+  return atomic_swap(&HLS()->ipi_pending, 0);
+}
+
 static uintptr_t mcall_shutdown()
 {
   while (1)
@@ -185,7 +213,7 @@ uintptr_t mcall_trap(uintptr_t mcause, uintptr_t* regs)
   switch (n)
   {
     case MCALL_HART_ID:
-      retval = 0; // TODO
+      retval = mcall_hart_id();
       break;
     case MCALL_CONSOLE_PUTCHAR:
       retval = mcall_console_putchar(arg0);
@@ -199,6 +227,9 @@ uintptr_t mcall_trap(uintptr_t mcause, uintptr_t* regs)
     case MCALL_SEND_IPI:
       retval = mcall_send_ipi(arg0);
       break;
+    case MCALL_CLEAR_IPI:
+      retval = mcall_clear_ipi();
+      break;
     case MCALL_SHUTDOWN:
       retval = mcall_shutdown();
       break;
diff --git a/pk/mtrap.h b/pk/mtrap.h
index 891b879..19a7fc3 100644
--- a/pk/mtrap.h
+++ b/pk/mtrap.h
@@ -225,25 +225,31 @@ typedef struct {
   size_t device_request_queue_size;
   sbi_device_message* device_response_queue_head;
   sbi_device_message* device_response_queue_tail;
-} mailbox_t;
+
+  int hart_id;
+  int ipi_pending;
+} hls_t;
 
 #define MACHINE_STACK_TOP() ({ \
   register uintptr_t sp asm ("sp"); \
   (void*)((sp + RISCV_PGSIZE) & -RISCV_PGSIZE); })
-#define MAILBOX() ((mailbox_t*)(MACHINE_STACK_TOP() - MAILBOX_SIZE))
+
+// hart-local storage, at top of stack
+#define HLS() ((hls_t*)(MACHINE_STACK_TOP() - HLS_SIZE))
+#define OTHER_HLS(id) ((hls_t*)((void*)HLS() + RISCV_PGSIZE * ((id) - HLS()->hart_id)))
 
 #endif // !__ASSEMBLER__
 
 #define MACHINE_STACK_SIZE RISCV_PGSIZE
 #define MENTRY_FRAME_SIZE (INTEGER_CONTEXT_SIZE + SOFT_FLOAT_CONTEXT_SIZE \
-                           + MAILBOX_SIZE)
+                           + HLS_SIZE)
 
 #ifdef __riscv_hard_float
 # define SOFT_FLOAT_CONTEXT_SIZE 0
 #else
 # define SOFT_FLOAT_CONTEXT_SIZE (8 * 32)
 #endif
-#define MAILBOX_SIZE 64
+#define HLS_SIZE 64
 #define INTEGER_CONTEXT_SIZE (32 * REGBYTES)
 
 #endif
diff --git a/pk/pk.c b/pk/pk.c
index cc2a761..8adf88e 100644
--- a/pk/pk.c
+++ b/pk/pk.c
@@ -97,3 +97,10 @@ void run_loaded_program(struct mainvars* args)
   write_csr(sscratch, kernel_stack_top);
   start_user(&tf);
 }
+
+void boot_other_hart()
+{
+  // stall all harts besides hart 0
+  while (1)
+    wfi();
+}
diff --git a/pk/pk.h b/pk/pk.h
index ee449aa..9bcf8da 100644
--- a/pk/pk.h
+++ b/pk/pk.h
@@ -45,6 +45,7 @@ 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, ...);
@@ -61,6 +62,7 @@ void handle_fault_load(trapframe_t*);
 void handle_fault_store(trapframe_t*);
 void boot_loader(struct mainvars*);
 void run_loaded_program(struct mainvars*);
+void boot_other_hart();
 
 typedef struct {
   int elf64;
@@ -99,6 +101,11 @@ extern int uarch_counters_enabled;
 extern long uarch_counters[NUM_COUNTERS];
 extern char* uarch_counter_names[NUM_COUNTERS];
 
+static inline void wfi()
+{
+  asm volatile ("wfi" ::: "memory");
+}
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/pk/sbi.S b/pk/sbi.S
index 7d94d04..08d19e4 100644
--- a/pk/sbi.S
+++ b/pk/sbi.S
@@ -5,5 +5,6 @@
 .globl sbi_send_device_request; sbi_send_device_request = -1984
 .globl sbi_receive_device_response; sbi_receive_device_response = -1968
 .globl sbi_send_ipi; sbi_send_ipi = -1952
-.globl sbi_timebase; sbi_timebase = -1936
-.globl sbi_shutdown; sbi_shutdown = -1920
+.globl sbi_clear_ipi; sbi_clear_ipi = -1936
+.globl sbi_timebase; sbi_timebase = -1920
+.globl sbi_shutdown; sbi_shutdown = -1904
diff --git a/pk/sbi.h b/pk/sbi.h
index dbf5e84..7c25588 100644
--- a/pk/sbi.h
+++ b/pk/sbi.h
@@ -12,8 +12,9 @@ unsigned long sbi_query_memory(unsigned long id, memory_block_info *p);
 unsigned long sbi_hart_id(void);
 unsigned long sbi_num_harts(void);
 unsigned long sbi_timebase(void);
-void sbi_send_ipi(uintptr_t hart_id);
-void sbi_console_putchar(unsigned char ch);
+void sbi_send_ipi(unsigned long hart_id);
+unsigned long sbi_clear_ipi(void);
+void sbi_console_putchar(unsigned long ch);
 void sbi_shutdown(void);
 
 typedef struct {
@@ -23,7 +24,7 @@ typedef struct {
   unsigned long sbi_private_data;
 } sbi_device_message;
 
-unsigned long sbi_send_device_request(uintptr_t req);
-uintptr_t sbi_receive_device_response(void);
+unsigned long sbi_send_device_request(unsigned long req);
+unsigned long sbi_receive_device_response(void);
 
 #endif
diff --git a/pk/sbi_entry.S b/pk/sbi_entry.S
index 05d66ad..2583022 100644
--- a/pk/sbi_entry.S
+++ b/pk/sbi_entry.S
@@ -49,6 +49,12 @@ sbi_base:
   ecall
   ret
 
+  # send ipi
+  .align 4
+  li a7, MCALL_CLEAR_IPI
+  ecall
+  ret
+
   # timebase
   .align 4
   li a0, 1000000000 # or, you know, we could provide the correct answer
diff --git a/pk/vm.c b/pk/vm.c
index 467fff9..851f01b 100644
--- a/pk/vm.c
+++ b/pk/vm.c
@@ -18,8 +18,7 @@ typedef struct {
 spinlock_t vm_lock = SPINLOCK_INIT;
 static vmr_t vmrs[MAX_VMR];
 
-typedef uintptr_t pte_t;
-static pte_t* root_page_table;
+pte_t* root_page_table;
 static uintptr_t first_free_page;
 static size_t next_free_page;
 static size_t free_pages;
@@ -161,11 +160,6 @@ static uintptr_t __vm_alloc(size_t npage)
   return 0;
 }
 
-static void flush_tlb()
-{
-  asm volatile("sfence.vm");
-}
-
 int __valid_user_range(uintptr_t vaddr, size_t len)
 {
   if (vaddr + len < vaddr)
@@ -402,7 +396,7 @@ static uintptr_t sbi_top_paddr()
   return ROUNDUP((uintptr_t)&_end, RISCV_PGSIZE);
 }
 
-#define first_free_paddr() (sbi_top_paddr() + RISCV_PGSIZE /* boot stack */)
+#define first_free_paddr() (sbi_top_paddr() + num_harts * RISCV_PGSIZE)
 
 void vm_init()
 {
@@ -424,15 +418,14 @@ void supervisor_vm_init()
   pte_t* middle_pt = (void*)sbi_pt + RISCV_PGSIZE;
 #ifndef __riscv64
   size_t num_middle_pts = 1;
-  root_page_table = middle_pt;
+  pte_t* root_pt = middle_pt;
 #else
   size_t num_middle_pts = (-current.first_user_vaddr - 1) / MEGAPAGE_SIZE + 1;
-  root_page_table = (void*)middle_pt + num_middle_pts * RISCV_PGSIZE;
+  pte_t* root_pt = (void*)middle_pt + num_middle_pts * RISCV_PGSIZE;
   for (size_t i = 0; i < num_middle_pts; i++)
-    root_page_table[(1<<RISCV_PGLEVEL_BITS)-num_middle_pts+i] = ptd_create(((uintptr_t)middle_pt >> RISCV_PGSHIFT) + i);
+    root_pt[(1<<RISCV_PGLEVEL_BITS)-num_middle_pts+i] = ptd_create(((uintptr_t)middle_pt >> RISCV_PGSHIFT) + i);
 #endif
-  memset(middle_pt, 0, root_page_table - middle_pt + RISCV_PGSIZE);
-  write_csr(sptbr, root_page_table);
+  memset(middle_pt, 0, root_pt - middle_pt + RISCV_PGSIZE);
 
   for (uintptr_t vaddr = current.first_user_vaddr, paddr = vaddr + current.bias, end = current.first_vaddr_after_user;
        paddr < mem_size; vaddr += SUPERPAGE_SIZE, paddr += SUPERPAGE_SIZE) {
@@ -441,7 +434,7 @@ void supervisor_vm_init()
     l2_idx += ((vaddr - current.first_user_vaddr) >> l2_shift);
     middle_pt[l2_idx] = pte_create(paddr >> RISCV_PGSHIFT, PROT_READ|PROT_WRITE|PROT_EXEC, 0);
   }
-  current.first_vaddr_after_user += (void*)root_page_table + RISCV_PGSIZE - (void*)sbi_pt;
+  current.first_vaddr_after_user += (void*)root_pt + RISCV_PGSIZE - (void*)sbi_pt;
 
   // map SBI at top of vaddr space
   uintptr_t num_sbi_pages = sbi_top_paddr() / RISCV_PGSIZE;
@@ -457,7 +450,9 @@ void supervisor_vm_init()
   kassert(next_free_page == 0);
   free_pages = 0;
 
-  flush_tlb();
+  mb();
+  root_page_table = root_pt;
+  write_csr(sptbr, root_pt);
 }
 
 uintptr_t pk_vm_init()
diff --git a/pk/vm.h b/pk/vm.h
index 06e0e71..71d9be2 100644
--- a/pk/vm.h
+++ b/pk/vm.h
@@ -46,4 +46,12 @@ uintptr_t do_mremap(uintptr_t addr, size_t old_size, size_t new_size, int flags)
 uintptr_t do_mprotect(uintptr_t addr, size_t length, int prot);
 uintptr_t do_brk(uintptr_t addr);
 
+typedef uintptr_t pte_t;
+extern pte_t* root_page_table;
+
+static inline void flush_tlb()
+{
+  asm volatile("sfence.vm");
+}
+
 #endif