WIP on SBI

5 files changed, 61 insertions, 63 deletions

diff --git a/dummy_payload/dummy_entry.S b/dummy_payload/dummy_entry.S
index 46c0b7f..92d4652 100644
--- a/dummy_payload/dummy_entry.S
+++ b/dummy_payload/dummy_entry.S
@@ -7,13 +7,13 @@ _start:
 1:
   lbu a0, (s0)
   beqz a0, 1f
-  li a7, MCALL_CONSOLE_PUTCHAR
+  li a7, SBI_CONSOLE_PUTCHAR
   ecall
   add s0, s0, 1
   j 1b
 
 1:
-  li a7, MCALL_SHUTDOWN
+  li a7, SBI_SHUTDOWN
   ecall
 
   .data
diff --git a/machine/mcall.h b/machine/mcall.h
index a704480..6a2c037 100644
--- a/machine/mcall.h
+++ b/machine/mcall.h
@@ -1,13 +1,14 @@
-#ifndef _RISCV_MCALL_H
-#define _RISCV_MCALL_H
+#ifndef _RISCV_SBI_H
+#define _RISCV_SBI_H
 
-#define MCALL_CONSOLE_PUTCHAR 1
-#define MCALL_CONSOLE_GETCHAR 2
-#define MCALL_SEND_IPI 4
-#define MCALL_CLEAR_IPI 5
-#define MCALL_SHUTDOWN 6
-#define MCALL_SET_TIMER 7
-#define MCALL_REMOTE_SFENCE_VM 8
-#define MCALL_REMOTE_FENCE_I 9
+#define SBI_SET_TIMER 0
+#define SBI_CONSOLE_PUTCHAR 1
+#define SBI_CONSOLE_GETCHAR 2
+#define SBI_CLEAR_IPI 3
+#define SBI_SEND_IPI 4
+#define SBI_REMOTE_FENCE_I 5
+#define SBI_REMOTE_SFENCE_VMA 6
+#define SBI_REMOTE_SFENCE_VMA_ASID 7
+#define SBI_SHUTDOWN 8
 
 #endif
diff --git a/machine/mentry.S b/machine/mentry.S
index 6e77d5e..5ff5c68 100644
--- a/machine/mentry.S
+++ b/machine/mentry.S
@@ -6,6 +6,7 @@
   .data
   .align 6
 trap_table:
+#define BAD_TRAP_VECTOR 0
   .word bad_trap
   .word bad_trap
   .word illegal_insn_trap
@@ -60,11 +61,11 @@ trap_vector:
 1:
   # Is it an IPI?
   li a0, IRQ_M_SOFT * 2
-  bne a0, a1, bad_trap
+  bne a0, a1, .Lbad_trap
 
   # Yes.  First, clear the MIPI bit.
   LOAD a0, MENTRY_IPI_OFFSET(sp)
-  STORE x0, (a0)
+  sw x0, (a0)
   fence
 
   # Now, decode the cause(s).
@@ -83,7 +84,7 @@ trap_vector:
   beqz a1, 1f
   fence.i
 1:
-  andi a1, a0, IPI_SFENCE_VM
+  andi a1, a0, IPI_SFENCE_VMA
   beqz a1, 1f
   sfence.vma
 1:
@@ -188,6 +189,10 @@ restore_regs:
   li a1, TRAP_FROM_MACHINE_MODE_VECTOR
   j .Lhandle_trap_in_machine_mode
 
+.Lbad_trap:
+  li a1, BAD_TRAP_VECTOR
+  j .Lhandle_trap_in_machine_mode
+
   .globl __redirect_trap
 __redirect_trap:
   # reset sp to top of M-mode stack
diff --git a/machine/mtrap.c b/machine/mtrap.c
index fb6c3f9..b9adebf 100644
--- a/machine/mtrap.c
+++ b/machine/mtrap.c
@@ -9,9 +9,9 @@
 #include <stdarg.h>
 #include <stdio.h>
 
-void __attribute__((noreturn)) bad_trap()
+void __attribute__((noreturn)) bad_trap(uintptr_t* regs, uintptr_t dummy, uintptr_t mepc)
 {
-  die("machine mode: unhandlable trap %d @ %p", read_csr(mcause), read_csr(mepc));
+  die("machine mode: unhandlable trap %d @ %p", read_csr(mcause), mepc);
 }
 
 static uintptr_t mcall_console_putchar(uint8_t ch)
@@ -45,10 +45,9 @@ void printm(const char* s, ...)
 
 static void send_ipi(uintptr_t recipient, int event)
 {
-  if ((atomic_or(&OTHER_HLS(recipient)->mipi_pending, event) & event) == 0) {
-    mb();
-    *OTHER_HLS(recipient)->ipi = 1;
-  }
+  atomic_or(&OTHER_HLS(recipient)->mipi_pending, event);
+  mb();
+  *OTHER_HLS(recipient)->ipi = 1;
 }
 
 static uintptr_t mcall_console_getchar()
@@ -77,21 +76,24 @@ static uintptr_t mcall_set_timer(uint64_t when)
 static void send_ipi_many(uintptr_t* pmask, int event)
 {
   _Static_assert(MAX_HARTS <= 8 * sizeof(*pmask), "# harts > uintptr_t bits");
-  uintptr_t mask = -1;
+  uintptr_t mask = ((uintptr_t)1 << num_harts) - 1;
   if (pmask)
-    mask = load_uintptr_t(pmask, read_csr(mepc));
+    mask &= load_uintptr_t(pmask, read_csr(mepc));
 
   // send IPIs to everyone
-  for (ssize_t i = num_harts-1; i >= 0; i--)
-    if ((mask >> i) & 1)
+  for (uintptr_t i = 0, m = mask; m; i++, m >>= 1)
+    if (m & 1)
       send_ipi(i, event);
 
+  if (event == IPI_SOFT)
+    return;
+
   // wait until all events have been handled.
   // prevent deadlock by consuming incoming IPIs.
   uint32_t incoming_ipi = 0;
-  for (ssize_t i = num_harts-1; i >= 0; i--)
-    if ((mask >> i) & 1)
-      while (OTHER_HLS(i)->ipi)
+  for (uintptr_t i = 0, m = mask; m; i++, m >>= 1)
+    if (m & 1)
+      while (*OTHER_HLS(i)->ipi)
         incoming_ipi |= atomic_swap(HLS()->ipi, 0);
 
   // if we got an IPI, restore it; it will be taken after returning
@@ -101,49 +103,40 @@ static void send_ipi_many(uintptr_t* pmask, int event)
   }
 }
 
-static uintptr_t mcall_remote_sfence_vm(uintptr_t* hart_mask)
-{
-  // ignore the ASID and do a global flush.
-  // this allows us to avoid queueing a message.
-  send_ipi_many(hart_mask, IPI_SFENCE_VM);
-  return 0;
-}
-
-static uintptr_t mcall_remote_fence_i(uintptr_t* hart_mask)
-{
-  send_ipi_many(hart_mask, IPI_FENCE_I);
-  return 0;
-}
-
 void mcall_trap(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc)
 {
-  uintptr_t n = regs[17], arg0 = regs[10], arg1 = regs[11], retval;
-  uintptr_t ipi_type = 0;
+  write_csr(mepc, mepc + 4);
+
+  uintptr_t n = regs[17], arg0 = regs[10], arg1 = regs[11], retval, ipi_type;
 
   switch (n)
   {
-    case MCALL_CONSOLE_PUTCHAR:
+    case SBI_CONSOLE_PUTCHAR:
       retval = mcall_console_putchar(arg0);
       break;
-    case MCALL_CONSOLE_GETCHAR:
+    case SBI_CONSOLE_GETCHAR:
       retval = mcall_console_getchar();
       break;
-    case MCALL_SEND_IPI:
+    case SBI_SEND_IPI:
       ipi_type = IPI_SOFT;
-    case MCALL_REMOTE_SFENCE_VM:
-      ipi_type = IPI_SFENCE_VM;
-    case MCALL_REMOTE_FENCE_I:
+      goto send_ipi;
+    case SBI_REMOTE_SFENCE_VMA:
+    case SBI_REMOTE_SFENCE_VMA_ASID:
+      ipi_type = IPI_SFENCE_VMA;
+      goto send_ipi;
+    case SBI_REMOTE_FENCE_I:
       ipi_type = IPI_FENCE_I;
+send_ipi:
       send_ipi_many((uintptr_t*)arg0, ipi_type);
       retval = 0;
       break;
-    case MCALL_CLEAR_IPI:
+    case SBI_CLEAR_IPI:
       retval = mcall_clear_ipi();
       break;
-    case MCALL_SHUTDOWN:
+    case SBI_SHUTDOWN:
       retval = mcall_shutdown();
       break;
-    case MCALL_SET_TIMER:
+    case SBI_SET_TIMER:
 #if __riscv_xlen == 32
       retval = mcall_set_timer(arg0 + ((uint64_t)arg1 << 32));
 #else
@@ -155,7 +148,6 @@ void mcall_trap(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc)
       break;
   }
   regs[10] = retval;
-  write_csr(mepc, mepc + 4);
 }
 
 void redirect_trap(uintptr_t epc, uintptr_t mstatus)
@@ -175,7 +167,7 @@ void redirect_trap(uintptr_t epc, uintptr_t mstatus)
   return __redirect_trap();
 }
 
-static void machine_page_fault(uintptr_t* regs, uintptr_t mepc)
+static void machine_page_fault(uintptr_t* regs, uintptr_t dummy, uintptr_t mepc)
 {
   // MPRV=1 iff this trap occurred while emulating an instruction on behalf
   // of a lower privilege level. In that case, a2=epc and a3=mstatus.
@@ -183,7 +175,7 @@ static void machine_page_fault(uintptr_t* regs, uintptr_t mepc)
     write_csr(sbadaddr, read_csr(mbadaddr));
     return redirect_trap(regs[12], regs[13]);
   }
-  bad_trap();
+  bad_trap(regs, dummy, mepc);
 }
 
 void trap_from_machine_mode(uintptr_t* regs, uintptr_t dummy, uintptr_t mepc)
@@ -194,8 +186,8 @@ void trap_from_machine_mode(uintptr_t* regs, uintptr_t dummy, uintptr_t mepc)
   {
     case CAUSE_FAULT_LOAD:
     case CAUSE_FAULT_STORE:
-      return machine_page_fault(regs, mepc);
+      return machine_page_fault(regs, dummy, mepc);
     default:
-      bad_trap();
+      bad_trap(regs, dummy, mepc);
   }
 }
diff --git a/machine/mtrap.h b/machine/mtrap.h
index 6f38cf6..3700a6d 100644
--- a/machine/mtrap.h
+++ b/machine/mtrap.h
@@ -36,10 +36,10 @@ extern volatile uint32_t* plic_priorities;
 extern size_t plic_ndevs;
 
 typedef struct {
-  uint32_t* ipi;
+  volatile uint32_t* ipi;
   volatile int mipi_pending;
 
-  uint64_t* timecmp;
+  volatile uint64_t* timecmp;
 
   volatile uint32_t* plic_m_thresh;
   volatile uintptr_t* plic_m_ie;
@@ -75,9 +75,9 @@ static inline void wfi()
 
 #endif // !__ASSEMBLER__
 
-#define IPI_SOFT      0x1
-#define IPI_FENCE_I   0x2
-#define IPI_SFENCE_VM 0x4
+#define IPI_SOFT       0x1
+#define IPI_FENCE_I    0x2
+#define IPI_SFENCE_VMA 0x4
 
 #define MACHINE_STACK_SIZE RISCV_PGSIZE
 #define MENTRY_HLS_OFFSET (INTEGER_CONTEXT_SIZE + SOFT_FLOAT_CONTEXT_SIZE)