WIP on priv-1.10

3 files changed, 177 insertions, 61 deletions

diff --git a/encoding.h b/encoding.h
index 8aeac4c..13930e8 100644
--- a/encoding.h
+++ b/encoding.h
@@ -19,7 +19,6 @@
 #define MSTATUS_MPRV        0x00020000
 #define MSTATUS_PUM         0x00040000
 #define MSTATUS_MXR         0x00080000
-#define MSTATUS_VM          0x1F000000
 #define MSTATUS32_SD        0x80000000
 #define MSTATUS64_SD        0x8000000000000000
 
@@ -107,12 +106,19 @@
 #define PRV_H 2
 #define PRV_M 3
 
-#define VM_MBARE 0
-#define VM_MBB   1
-#define VM_MBBID 2
-#define VM_SV32  8
-#define VM_SV39  9
-#define VM_SV48  10
+#define SPTBR32_MODE 0x80000000
+#define SPTBR32_ASID 0x7FC00000
+#define SPTBR32_PPN  0x003FFFFF
+#define SPTBR64_MODE 0xF000000000000000
+#define SPTBR64_ASID 0x0FFFF00000000000
+#define SPTBR64_PPN  0x00000FFFFFFFFFFF
+
+#define SPTBR_MODE_OFF  0
+#define SPTBR_MODE_SV32 1
+#define SPTBR_MODE_SV39 8
+#define SPTBR_MODE_SV48 9
+#define SPTBR_MODE_SV57 10
+#define SPTBR_MODE_SV64 11
 
 #define IRQ_S_SOFT   1
 #define IRQ_H_SOFT   2
@@ -154,10 +160,12 @@
 # define MSTATUS_SD MSTATUS64_SD
 # define SSTATUS_SD SSTATUS64_SD
 # define RISCV_PGLEVEL_BITS 9
+# define SPTBR_MODE SPTBR64_MODE
 #else
 # define MSTATUS_SD MSTATUS32_SD
 # define SSTATUS_SD SSTATUS32_SD
 # define RISCV_PGLEVEL_BITS 10
+# define SPTBR_MODE SPTBR32_MODE
 #endif
 #define RISCV_PGSHIFT 12
 #define RISCV_PGSIZE (1 << RISCV_PGSHIFT)
@@ -171,30 +179,18 @@
   __tmp; })
 
 #define write_csr(reg, val) ({ \
-  if (__builtin_constant_p(val) && (unsigned long)(val) < 32) \
-    asm volatile ("csrw " #reg ", %0" :: "i"(val)); \
-  else \
-    asm volatile ("csrw " #reg ", %0" :: "r"(val)); })
+  asm volatile ("csrw " #reg ", %0" :: "rK"(val)); })
 
 #define swap_csr(reg, val) ({ unsigned long __tmp; \
-  if (__builtin_constant_p(val) && (unsigned long)(val) < 32) \
-    asm volatile ("csrrw %0, " #reg ", %1" : "=r"(__tmp) : "i"(val)); \
-  else \
-    asm volatile ("csrrw %0, " #reg ", %1" : "=r"(__tmp) : "r"(val)); \
+  asm volatile ("csrrw %0, " #reg ", %1" : "=r"(__tmp) : "rK"(val)); \
   __tmp; })
 
 #define set_csr(reg, bit) ({ unsigned long __tmp; \
-  if (__builtin_constant_p(bit) && (unsigned long)(bit) < 32) \
-    asm volatile ("csrrs %0, " #reg ", %1" : "=r"(__tmp) : "i"(bit)); \
-  else \
-    asm volatile ("csrrs %0, " #reg ", %1" : "=r"(__tmp) : "r"(bit)); \
+  asm volatile ("csrrs %0, " #reg ", %1" : "=r"(__tmp) : "rK"(bit)); \
   __tmp; })
 
 #define clear_csr(reg, bit) ({ unsigned long __tmp; \
-  if (__builtin_constant_p(bit) && (unsigned long)(bit) < 32) \
-    asm volatile ("csrrc %0, " #reg ", %1" : "=r"(__tmp) : "i"(bit)); \
-  else \
-    asm volatile ("csrrc %0, " #reg ", %1" : "=r"(__tmp) : "r"(bit)); \
+  asm volatile ("csrrc %0, " #reg ", %1" : "=r"(__tmp) : "rK"(bit)); \
   __tmp; })
 
 #define rdtime() read_csr(time)
@@ -208,7 +204,7 @@
 #endif
 
 #endif
-/* Automatically generated by parse-opcodes */
+/* Automatically generated by parse-opcodes.  */
 #ifndef RISCV_ENCODING_H
 #define RISCV_ENCODING_H
 #define MATCH_BEQ 0x63
@@ -397,8 +393,8 @@
 #define MASK_MRET  0xffffffff
 #define MATCH_DRET 0x7b200073
 #define MASK_DRET  0xffffffff
-#define MATCH_SFENCE_VM 0x10400073
-#define MASK_SFENCE_VM  0xfff07fff
+#define MATCH_SFENCE_VMA 0x12000073
+#define MASK_SFENCE_VMA  0xfe007fff
 #define MATCH_WFI 0x10500073
 #define MASK_WFI  0xffffffff
 #define MATCH_CSRRW 0x1073
@@ -457,6 +453,34 @@
 #define MASK_FCVT_D_S  0xfff0007f
 #define MATCH_FSQRT_D 0x5a000053
 #define MASK_FSQRT_D  0xfff0007f
+#define MATCH_FADD_Q 0x6000053
+#define MASK_FADD_Q  0xfe00007f
+#define MATCH_FSUB_Q 0xe000053
+#define MASK_FSUB_Q  0xfe00007f
+#define MATCH_FMUL_Q 0x16000053
+#define MASK_FMUL_Q  0xfe00007f
+#define MATCH_FDIV_Q 0x1e000053
+#define MASK_FDIV_Q  0xfe00007f
+#define MATCH_FSGNJ_Q 0x26000053
+#define MASK_FSGNJ_Q  0xfe00707f
+#define MATCH_FSGNJN_Q 0x26001053
+#define MASK_FSGNJN_Q  0xfe00707f
+#define MATCH_FSGNJX_Q 0x26002053
+#define MASK_FSGNJX_Q  0xfe00707f
+#define MATCH_FMIN_Q 0x2e000053
+#define MASK_FMIN_Q  0xfe00707f
+#define MATCH_FMAX_Q 0x2e001053
+#define MASK_FMAX_Q  0xfe00707f
+#define MATCH_FCVT_S_Q 0x40300053
+#define MASK_FCVT_S_Q  0xfff0007f
+#define MATCH_FCVT_Q_S 0x46000053
+#define MASK_FCVT_Q_S  0xfff0007f
+#define MATCH_FCVT_D_Q 0x42300053
+#define MASK_FCVT_D_Q  0xfff0007f
+#define MATCH_FCVT_Q_D 0x46100053
+#define MASK_FCVT_Q_D  0xfff0007f
+#define MATCH_FSQRT_Q 0x5e000053
+#define MASK_FSQRT_Q  0xfff0007f
 #define MATCH_FLE_S 0xa0000053
 #define MASK_FLE_S  0xfe00707f
 #define MATCH_FLT_S 0xa0001053
@@ -469,6 +493,12 @@
 #define MASK_FLT_D  0xfe00707f
 #define MATCH_FEQ_D 0xa2002053
 #define MASK_FEQ_D  0xfe00707f
+#define MATCH_FLE_Q 0xa6000053
+#define MASK_FLE_Q  0xfe00707f
+#define MATCH_FLT_Q 0xa6001053
+#define MASK_FLT_Q  0xfe00707f
+#define MATCH_FEQ_Q 0xa6002053
+#define MASK_FEQ_Q  0xfe00707f
 #define MATCH_FCVT_W_S 0xc0000053
 #define MASK_FCVT_W_S  0xfff0007f
 #define MATCH_FCVT_WU_S 0xc0100053
@@ -493,6 +523,18 @@
 #define MASK_FMV_X_D  0xfff0707f
 #define MATCH_FCLASS_D 0xe2001053
 #define MASK_FCLASS_D  0xfff0707f
+#define MATCH_FCVT_W_Q 0xc6000053
+#define MASK_FCVT_W_Q  0xfff0007f
+#define MATCH_FCVT_WU_Q 0xc6100053
+#define MASK_FCVT_WU_Q  0xfff0007f
+#define MATCH_FCVT_L_Q 0xc6200053
+#define MASK_FCVT_L_Q  0xfff0007f
+#define MATCH_FCVT_LU_Q 0xc6300053
+#define MASK_FCVT_LU_Q  0xfff0007f
+#define MATCH_FMV_X_Q 0xe6000053
+#define MASK_FMV_X_Q  0xfff0707f
+#define MATCH_FCLASS_Q 0xe6001053
+#define MASK_FCLASS_Q  0xfff0707f
 #define MATCH_FCVT_S_W 0xd0000053
 #define MASK_FCVT_S_W  0xfff0007f
 #define MATCH_FCVT_S_WU 0xd0100053
@@ -513,14 +555,28 @@
 #define MASK_FCVT_D_LU  0xfff0007f
 #define MATCH_FMV_D_X 0xf2000053
 #define MASK_FMV_D_X  0xfff0707f
+#define MATCH_FCVT_Q_W 0xd6000053
+#define MASK_FCVT_Q_W  0xfff0007f
+#define MATCH_FCVT_Q_WU 0xd6100053
+#define MASK_FCVT_Q_WU  0xfff0007f
+#define MATCH_FCVT_Q_L 0xd6200053
+#define MASK_FCVT_Q_L  0xfff0007f
+#define MATCH_FCVT_Q_LU 0xd6300053
+#define MASK_FCVT_Q_LU  0xfff0007f
+#define MATCH_FMV_Q_X 0xf6000053
+#define MASK_FMV_Q_X  0xfff0707f
 #define MATCH_FLW 0x2007
 #define MASK_FLW  0x707f
 #define MATCH_FLD 0x3007
 #define MASK_FLD  0x707f
+#define MATCH_FLQ 0x4007
+#define MASK_FLQ  0x707f
 #define MATCH_FSW 0x2027
 #define MASK_FSW  0x707f
 #define MATCH_FSD 0x3027
 #define MASK_FSD  0x707f
+#define MATCH_FSQ 0x4027
+#define MASK_FSQ  0x707f
 #define MATCH_FMADD_S 0x43
 #define MASK_FMADD_S  0x600007f
 #define MATCH_FMSUB_S 0x47
@@ -537,6 +593,14 @@
 #define MASK_FNMSUB_D  0x600007f
 #define MATCH_FNMADD_D 0x200004f
 #define MASK_FNMADD_D  0x600007f
+#define MATCH_FMADD_Q 0x6000043
+#define MASK_FMADD_Q  0x600007f
+#define MATCH_FMSUB_Q 0x6000047
+#define MASK_FMSUB_Q  0x600007f
+#define MATCH_FNMSUB_Q 0x600004b
+#define MASK_FNMSUB_Q  0x600007f
+#define MATCH_FNMADD_Q 0x600004f
+#define MASK_FNMADD_Q  0x600007f
 #define MATCH_C_NOP 0x1
 #define MASK_C_NOP  0xffff
 #define MATCH_C_ADDI16SP 0x6101
@@ -707,6 +771,7 @@
 #define CSR_SSTATUS 0x100
 #define CSR_SIE 0x104
 #define CSR_STVEC 0x105
+#define CSR_SCOUNTEREN 0x106
 #define CSR_SSCRATCH 0x140
 #define CSR_SEPC 0x141
 #define CSR_SCAUSE 0x142
@@ -719,6 +784,7 @@
 #define CSR_MIDELEG 0x303
 #define CSR_MIE 0x304
 #define CSR_MTVEC 0x305
+#define CSR_MCOUNTEREN 0x306
 #define CSR_MSCRATCH 0x340
 #define CSR_MEPC 0x341
 #define CSR_MCAUSE 0x342
@@ -762,8 +828,6 @@
 #define CSR_MHPMCOUNTER29 0xb1d
 #define CSR_MHPMCOUNTER30 0xb1e
 #define CSR_MHPMCOUNTER31 0xb1f
-#define CSR_MUCOUNTEREN 0x320
-#define CSR_MSCOUNTEREN 0x321
 #define CSR_MHPMEVENT3 0x323
 #define CSR_MHPMEVENT4 0x324
 #define CSR_MHPMEVENT5 0x325
@@ -967,7 +1031,7 @@ DECLARE_INSN(sret, MATCH_SRET, MASK_SRET)
 DECLARE_INSN(hret, MATCH_HRET, MASK_HRET)
 DECLARE_INSN(mret, MATCH_MRET, MASK_MRET)
 DECLARE_INSN(dret, MATCH_DRET, MASK_DRET)
-DECLARE_INSN(sfence_vm, MATCH_SFENCE_VM, MASK_SFENCE_VM)
+DECLARE_INSN(sfence_vma, MATCH_SFENCE_VMA, MASK_SFENCE_VMA)
 DECLARE_INSN(wfi, MATCH_WFI, MASK_WFI)
 DECLARE_INSN(csrrw, MATCH_CSRRW, MASK_CSRRW)
 DECLARE_INSN(csrrs, MATCH_CSRRS, MASK_CSRRS)
@@ -997,12 +1061,29 @@ DECLARE_INSN(fmax_d, MATCH_FMAX_D, MASK_FMAX_D)
 DECLARE_INSN(fcvt_s_d, MATCH_FCVT_S_D, MASK_FCVT_S_D)
 DECLARE_INSN(fcvt_d_s, MATCH_FCVT_D_S, MASK_FCVT_D_S)
 DECLARE_INSN(fsqrt_d, MATCH_FSQRT_D, MASK_FSQRT_D)
+DECLARE_INSN(fadd_q, MATCH_FADD_Q, MASK_FADD_Q)
+DECLARE_INSN(fsub_q, MATCH_FSUB_Q, MASK_FSUB_Q)
+DECLARE_INSN(fmul_q, MATCH_FMUL_Q, MASK_FMUL_Q)
+DECLARE_INSN(fdiv_q, MATCH_FDIV_Q, MASK_FDIV_Q)
+DECLARE_INSN(fsgnj_q, MATCH_FSGNJ_Q, MASK_FSGNJ_Q)
+DECLARE_INSN(fsgnjn_q, MATCH_FSGNJN_Q, MASK_FSGNJN_Q)
+DECLARE_INSN(fsgnjx_q, MATCH_FSGNJX_Q, MASK_FSGNJX_Q)
+DECLARE_INSN(fmin_q, MATCH_FMIN_Q, MASK_FMIN_Q)
+DECLARE_INSN(fmax_q, MATCH_FMAX_Q, MASK_FMAX_Q)
+DECLARE_INSN(fcvt_s_q, MATCH_FCVT_S_Q, MASK_FCVT_S_Q)
+DECLARE_INSN(fcvt_q_s, MATCH_FCVT_Q_S, MASK_FCVT_Q_S)
+DECLARE_INSN(fcvt_d_q, MATCH_FCVT_D_Q, MASK_FCVT_D_Q)
+DECLARE_INSN(fcvt_q_d, MATCH_FCVT_Q_D, MASK_FCVT_Q_D)
+DECLARE_INSN(fsqrt_q, MATCH_FSQRT_Q, MASK_FSQRT_Q)
 DECLARE_INSN(fle_s, MATCH_FLE_S, MASK_FLE_S)
 DECLARE_INSN(flt_s, MATCH_FLT_S, MASK_FLT_S)
 DECLARE_INSN(feq_s, MATCH_FEQ_S, MASK_FEQ_S)
 DECLARE_INSN(fle_d, MATCH_FLE_D, MASK_FLE_D)
 DECLARE_INSN(flt_d, MATCH_FLT_D, MASK_FLT_D)
 DECLARE_INSN(feq_d, MATCH_FEQ_D, MASK_FEQ_D)
+DECLARE_INSN(fle_q, MATCH_FLE_Q, MASK_FLE_Q)
+DECLARE_INSN(flt_q, MATCH_FLT_Q, MASK_FLT_Q)
+DECLARE_INSN(feq_q, MATCH_FEQ_Q, MASK_FEQ_Q)
 DECLARE_INSN(fcvt_w_s, MATCH_FCVT_W_S, MASK_FCVT_W_S)
 DECLARE_INSN(fcvt_wu_s, MATCH_FCVT_WU_S, MASK_FCVT_WU_S)
 DECLARE_INSN(fcvt_l_s, MATCH_FCVT_L_S, MASK_FCVT_L_S)
@@ -1015,6 +1096,12 @@ DECLARE_INSN(fcvt_l_d, MATCH_FCVT_L_D, MASK_FCVT_L_D)
 DECLARE_INSN(fcvt_lu_d, MATCH_FCVT_LU_D, MASK_FCVT_LU_D)
 DECLARE_INSN(fmv_x_d, MATCH_FMV_X_D, MASK_FMV_X_D)
 DECLARE_INSN(fclass_d, MATCH_FCLASS_D, MASK_FCLASS_D)
+DECLARE_INSN(fcvt_w_q, MATCH_FCVT_W_Q, MASK_FCVT_W_Q)
+DECLARE_INSN(fcvt_wu_q, MATCH_FCVT_WU_Q, MASK_FCVT_WU_Q)
+DECLARE_INSN(fcvt_l_q, MATCH_FCVT_L_Q, MASK_FCVT_L_Q)
+DECLARE_INSN(fcvt_lu_q, MATCH_FCVT_LU_Q, MASK_FCVT_LU_Q)
+DECLARE_INSN(fmv_x_q, MATCH_FMV_X_Q, MASK_FMV_X_Q)
+DECLARE_INSN(fclass_q, MATCH_FCLASS_Q, MASK_FCLASS_Q)
 DECLARE_INSN(fcvt_s_w, MATCH_FCVT_S_W, MASK_FCVT_S_W)
 DECLARE_INSN(fcvt_s_wu, MATCH_FCVT_S_WU, MASK_FCVT_S_WU)
 DECLARE_INSN(fcvt_s_l, MATCH_FCVT_S_L, MASK_FCVT_S_L)
@@ -1025,10 +1112,17 @@ DECLARE_INSN(fcvt_d_wu, MATCH_FCVT_D_WU, MASK_FCVT_D_WU)
 DECLARE_INSN(fcvt_d_l, MATCH_FCVT_D_L, MASK_FCVT_D_L)
 DECLARE_INSN(fcvt_d_lu, MATCH_FCVT_D_LU, MASK_FCVT_D_LU)
 DECLARE_INSN(fmv_d_x, MATCH_FMV_D_X, MASK_FMV_D_X)
+DECLARE_INSN(fcvt_q_w, MATCH_FCVT_Q_W, MASK_FCVT_Q_W)
+DECLARE_INSN(fcvt_q_wu, MATCH_FCVT_Q_WU, MASK_FCVT_Q_WU)
+DECLARE_INSN(fcvt_q_l, MATCH_FCVT_Q_L, MASK_FCVT_Q_L)
+DECLARE_INSN(fcvt_q_lu, MATCH_FCVT_Q_LU, MASK_FCVT_Q_LU)
+DECLARE_INSN(fmv_q_x, MATCH_FMV_Q_X, MASK_FMV_Q_X)
 DECLARE_INSN(flw, MATCH_FLW, MASK_FLW)
 DECLARE_INSN(fld, MATCH_FLD, MASK_FLD)
+DECLARE_INSN(flq, MATCH_FLQ, MASK_FLQ)
 DECLARE_INSN(fsw, MATCH_FSW, MASK_FSW)
 DECLARE_INSN(fsd, MATCH_FSD, MASK_FSD)
+DECLARE_INSN(fsq, MATCH_FSQ, MASK_FSQ)
 DECLARE_INSN(fmadd_s, MATCH_FMADD_S, MASK_FMADD_S)
 DECLARE_INSN(fmsub_s, MATCH_FMSUB_S, MASK_FMSUB_S)
 DECLARE_INSN(fnmsub_s, MATCH_FNMSUB_S, MASK_FNMSUB_S)
@@ -1037,6 +1131,10 @@ DECLARE_INSN(fmadd_d, MATCH_FMADD_D, MASK_FMADD_D)
 DECLARE_INSN(fmsub_d, MATCH_FMSUB_D, MASK_FMSUB_D)
 DECLARE_INSN(fnmsub_d, MATCH_FNMSUB_D, MASK_FNMSUB_D)
 DECLARE_INSN(fnmadd_d, MATCH_FNMADD_D, MASK_FNMADD_D)
+DECLARE_INSN(fmadd_q, MATCH_FMADD_Q, MASK_FMADD_Q)
+DECLARE_INSN(fmsub_q, MATCH_FMSUB_Q, MASK_FMSUB_Q)
+DECLARE_INSN(fnmsub_q, MATCH_FNMSUB_Q, MASK_FNMSUB_Q)
+DECLARE_INSN(fnmadd_q, MATCH_FNMADD_Q, MASK_FNMADD_Q)
 DECLARE_INSN(c_nop, MATCH_C_NOP, MASK_C_NOP)
 DECLARE_INSN(c_addi16sp, MATCH_C_ADDI16SP, MASK_C_ADDI16SP)
 DECLARE_INSN(c_jr, MATCH_C_JR, MASK_C_JR)
@@ -1143,6 +1241,7 @@ DECLARE_CSR(hpmcounter31, CSR_HPMCOUNTER31)
 DECLARE_CSR(sstatus, CSR_SSTATUS)
 DECLARE_CSR(sie, CSR_SIE)
 DECLARE_CSR(stvec, CSR_STVEC)
+DECLARE_CSR(scounteren, CSR_SCOUNTEREN)
 DECLARE_CSR(sscratch, CSR_SSCRATCH)
 DECLARE_CSR(sepc, CSR_SEPC)
 DECLARE_CSR(scause, CSR_SCAUSE)
@@ -1155,6 +1254,7 @@ DECLARE_CSR(medeleg, CSR_MEDELEG)
 DECLARE_CSR(mideleg, CSR_MIDELEG)
 DECLARE_CSR(mie, CSR_MIE)
 DECLARE_CSR(mtvec, CSR_MTVEC)
+DECLARE_CSR(mcounteren, CSR_MCOUNTEREN)
 DECLARE_CSR(mscratch, CSR_MSCRATCH)
 DECLARE_CSR(mepc, CSR_MEPC)
 DECLARE_CSR(mcause, CSR_MCAUSE)
@@ -1198,8 +1298,6 @@ DECLARE_CSR(mhpmcounter28, CSR_MHPMCOUNTER28)
 DECLARE_CSR(mhpmcounter29, CSR_MHPMCOUNTER29)
 DECLARE_CSR(mhpmcounter30, CSR_MHPMCOUNTER30)
 DECLARE_CSR(mhpmcounter31, CSR_MHPMCOUNTER31)
-DECLARE_CSR(mucounteren, CSR_MUCOUNTEREN)
-DECLARE_CSR(mscounteren, CSR_MSCOUNTEREN)
 DECLARE_CSR(mhpmevent3, CSR_MHPMEVENT3)
 DECLARE_CSR(mhpmevent4, CSR_MHPMEVENT4)
 DECLARE_CSR(mhpmevent5, CSR_MHPMEVENT5)
diff --git a/p/riscv_test.h b/p/riscv_test.h
index 1da083c..9f17ae5 100644
--- a/p/riscv_test.h
+++ b/p/riscv_test.h
@@ -37,11 +37,6 @@
   RVTEST_ENABLE_SUPERVISOR;                                             \
   .endm
 
-#define RVTEST_RV64SV                                                   \
-  .macro init;                                                          \
-  RVTEST_ENABLE_SUPERVISOR;                                             \
-  .endm
-
 #define RVTEST_RV32M                                                    \
   .macro init;                                                          \
   RVTEST_ENABLE_MACHINE;                                                \
@@ -58,6 +53,13 @@
 # define CHECK_XLEN csrr a0, misa; bgez a0, 1f; RVTEST_PASS; 1:
 #endif
 
+#define INIT_SPTBR                                                      \
+  csrr a0, misa;                                                        \
+  slli a0, a0, (__riscv_xlen - 1) - ('S' - 'A');                        \
+  bgez a0, 1f;                                                          \
+  csrwi sptbr, 0;                                                       \
+1:
+
 #define RVTEST_ENABLE_SUPERVISOR                                        \
   li a0, MSTATUS_MPP & (MSTATUS_MPP >> 1);                              \
   csrs mstatus, a0;                                                     \
@@ -121,6 +123,7 @@ handle_exception:                                                       \
         j write_tohost;                                                 \
 reset_vector:                                                           \
         RISCV_MULTICORE_DISABLE;                                        \
+        INIT_SPTBR;                                                     \
         li TESTNUM, 0;                                                  \
         la t0, trap_vector;                                             \
         csrw mtvec, t0;                                                 \
diff --git a/v/vm.c b/v/vm.c
index 1aa068a..2e057fb 100644
--- a/v/vm.c
+++ b/v/vm.c
@@ -9,20 +9,21 @@
 void trap_entry();
 void pop_tf(trapframe_t*);
 
-volatile uint64_t tohost __attribute__((aligned(64)));
-volatile uint64_t fromhost __attribute__((aligned(64)));
+volatile uint64_t tohost;
+volatile uint64_t fromhost;
 
 static void do_tohost(uint64_t tohost_value)
 {
+  while (tohost)
+    fromhost = 0;
   tohost = tohost_value;
-  while (fromhost == 0)
-    ;
-  fromhost = 0;
 }
 
 #define pa2kva(pa) ((void*)(pa) - DRAM_BASE - MEGAPAGE_SIZE)
 #define uva2kva(pa) ((void*)(pa) - MEGAPAGE_SIZE)
 
+#define flush_page(addr) asm volatile ("sfence.vma %0" : : "r" (addr))
+
 static uint64_t lfsr63(uint64_t x)
 {
   uint64_t bit = (x ^ (x >> 1)) & 1;
@@ -61,15 +62,13 @@ void wtf()
 
 #define l1pt pt[0]
 #define user_l2pt pt[1]
-#define kernel_l2pt pt[2]
 #if __riscv_xlen == 64
-# define NPT 5
+# define NPT 4
+#define kernel_l2pt pt[2]
 # define user_l3pt pt[3]
-# define kernel_l3pt pt[4]
 #else
-# define NPT 3
+# define NPT 2
 # define user_l3pt user_l2pt
-# define kernel_l3pt kernel_l2pt
 #endif
 pte_t pt[NPT][PTES_PER_PT] __attribute__((aligned(PGSIZE)));
 
@@ -119,24 +118,39 @@ static void evict(unsigned long addr)
   }
 }
 
-void handle_fault(unsigned long addr)
+void handle_fault(uintptr_t addr, uintptr_t cause)
 {
   assert(addr >= PGSIZE && addr < MAX_TEST_PAGES * PGSIZE);
   addr = addr/PGSIZE*PGSIZE;
 
+  if (user_l3pt[addr/PGSIZE]) {
+    if (!(user_l3pt[addr/PGSIZE] & PTE_A)) {
+      user_l3pt[addr/PGSIZE] |= PTE_A;
+    } else {
+      assert(!(user_l3pt[addr/PGSIZE] & PTE_D) && cause == CAUSE_FAULT_STORE);
+      user_l3pt[addr/PGSIZE] |= PTE_D;
+    }
+    flush_page(addr);
+    return;
+  }
+
   freelist_t* node = freelist_head;
   assert(node);
   freelist_head = node->next;
   if (freelist_head == freelist_tail)
     freelist_tail = 0;
 
-  user_l3pt[addr/PGSIZE] = (node->addr >> PGSHIFT << PTE_PPN_SHIFT) | PTE_V | PTE_U | PTE_R | PTE_W | PTE_X;
-  asm volatile ("sfence.vm");
+  uintptr_t new_pte = (node->addr >> PGSHIFT << PTE_PPN_SHIFT) | PTE_V | PTE_U | PTE_R | PTE_W | PTE_X;
+  user_l3pt[addr/PGSIZE] = new_pte | PTE_A | PTE_D;
+  flush_page(addr);
 
   assert(user_mapping[addr/PGSIZE].addr == 0);
   user_mapping[addr/PGSIZE] = *node;
   memcpy((void*)addr, uva2kva(addr), PGSIZE);
 
+  user_l3pt[addr/PGSIZE] = new_pte;
+  flush_page(addr);
+
   __builtin___clear_cache(0,0);
 }
 
@@ -165,7 +179,7 @@ void handle_trap(trapframe_t* tf)
     tf->epc += 4;
   }
   else if (tf->cause == CAUSE_FAULT_FETCH || tf->cause == CAUSE_FAULT_LOAD || tf->cause == CAUSE_FAULT_STORE)
-    handle_fault(tf->badvaddr);
+    handle_fault(tf->badvaddr, tf->cause);
   else
     assert(!"unexpected exception");
 
@@ -196,18 +210,23 @@ void vm_boot(uintptr_t test_addr)
 
   _Static_assert(SIZEOF_TRAPFRAME_T == sizeof(trapframe_t), "???");
 
-#if MAX_TEST_PAGES > PTES_PER_PT
+#if (MAX_TEST_PAGES > PTES_PER_PT) || (DRAM_BASE % MEGAPAGE_SIZE) != 0
 # error
 #endif
-  write_csr(sptbr, (uintptr_t)l1pt >> PGSHIFT);
-  // map kernel to uppermost megapage
-  l1pt[PTES_PER_PT-1] = ((pte_t)kernel_l2pt >> PGSHIFT << PTE_PPN_SHIFT) | PTE_V;
   // map user to lowermost megapage
   l1pt[0] = ((pte_t)user_l2pt >> PGSHIFT << PTE_PPN_SHIFT) | PTE_V;
+  // map kernel to uppermost megapage
 #if __riscv_xlen == 64
-  kernel_l2pt[PTES_PER_PT-1] = ((pte_t)kernel_l3pt >> PGSHIFT << PTE_PPN_SHIFT) | PTE_V;
+  l1pt[PTES_PER_PT-1] = ((pte_t)kernel_l2pt >> PGSHIFT << PTE_PPN_SHIFT) | PTE_V;
+  kernel_l2pt[PTES_PER_PT-1] = (DRAM_BASE/RISCV_PGSIZE << PTE_PPN_SHIFT) | PTE_V | PTE_R | PTE_W | PTE_X | PTE_A | PTE_D;
   user_l2pt[0] = ((pte_t)user_l3pt >> PGSHIFT << PTE_PPN_SHIFT) | PTE_V;
+  uintptr_t vm_choice = SPTBR_MODE_SV39;
+#else
+  l1pt[PTES_PER_PT-1] = (DRAM_BASE/RISCV_PGSIZE << PTE_PPN_SHIFT) | PTE_V | PTE_R | PTE_W | PTE_X | PTE_A | PTE_D;
+  uintptr_t vm_choice = SPTBR_MODE_SV32;
 #endif
+  write_csr(sptbr, ((uintptr_t)l1pt >> PGSHIFT) |
+                   (vm_choice * (SPTBR_MODE & ~(SPTBR_MODE<<1))));
 
   // set up supervisor trap handling
   write_csr(stvec, pa2kva(trap_entry));
@@ -217,10 +236,8 @@ void vm_boot(uintptr_t test_addr)
     (1 << CAUSE_FAULT_FETCH) |
     (1 << CAUSE_FAULT_LOAD) |
     (1 << CAUSE_FAULT_STORE));
-  // on ERET, user mode; FPU on; accelerator on; VM on
-  int vm_choice = sizeof(long) == 8 ? VM_SV39 : VM_SV32;
-  write_csr(mstatus, MSTATUS_FS | MSTATUS_XS |
-                     (vm_choice * (MSTATUS_VM & ~(MSTATUS_VM<<1))));
+  // FPU on; accelerator on
+  write_csr(mstatus, MSTATUS_FS | MSTATUS_XS);
   write_csr(mie, 0);
 
   random = 1 + (random % MAX_TEST_PAGES);
@@ -231,8 +248,6 @@ void vm_boot(uintptr_t test_addr)
     freelist_nodes[i].addr = DRAM_BASE + (MAX_TEST_PAGES + random)*PGSIZE;
     freelist_nodes[i].next = pa2kva(&freelist_nodes[i+1]);
     random = LFSR_NEXT(random);
-
-    kernel_l3pt[i] = ((i + DRAM_BASE/RISCV_PGSIZE) << PTE_PPN_SHIFT) | PTE_V | PTE_R | PTE_W | PTE_X;
   }
   freelist_nodes[MAX_TEST_PAGES-1].next = 0;