lib: Handle page/access fault caused by unpriv load/store

The unpriv load/store instruction from M-mode can cause page/access
fault to M-mode if S-mode page table did not have mappings OR it did
not have PMP access permission.

To tackle this, we redirect trap back to S-mode if unpriv load/store
instruction traps in M-mode.

Signed-off-by: Anup Patel <anup.patel@wdc.com>
Reviewed-by: Atish Patra <atish.patra@wdc.com>

9 files changed, 131 insertions, 44 deletions

diff --git a/include/sbi/riscv_unpriv.h b/include/sbi/riscv_unpriv.h
index ed2dbcc..0fad27d 100644
--- a/include/sbi/riscv_unpriv.h
+++ b/include/sbi/riscv_unpriv.h
@@ -12,11 +12,23 @@
 
 #include <sbi/sbi_types.h>
 
-#define DECLARE_UNPRIVILEGED_LOAD_FUNCTION(type) \
-	type load_##type(const type *addr);
+struct sbi_scratch;
 
-#define DECLARE_UNPRIVILEGED_STORE_FUNCTION(type) \
-	void store_##type(type *addr, type val);
+struct unpriv_trap {
+	unsigned long ilen;
+	unsigned long cause;
+	unsigned long tval;
+};
+
+#define DECLARE_UNPRIVILEGED_LOAD_FUNCTION(type)       \
+	type load_##type(const type *addr,             \
+			 struct sbi_scratch *scratch,  \
+			 struct unpriv_trap *trap);
+
+#define DECLARE_UNPRIVILEGED_STORE_FUNCTION(type)      \
+	void store_##type(type *addr, type val,        \
+			  struct sbi_scratch *scratch, \
+			  struct unpriv_trap *trap);
 
 DECLARE_UNPRIVILEGED_LOAD_FUNCTION(u8)
 DECLARE_UNPRIVILEGED_LOAD_FUNCTION(u16)
diff --git a/include/sbi/sbi_error.h b/include/sbi/sbi_error.h
index db3d6dd..3ad78db 100644
--- a/include/sbi/sbi_error.h
+++ b/include/sbi/sbi_error.h
@@ -25,6 +25,7 @@
 #define SBI_EILL	-10
 #define SBI_ENOSPC	-11
 #define SBI_ENOMEM	-12
+#define SBI_ETRAP	-13
 
 /* clang-format on */
 
diff --git a/include/sbi/sbi_ipi.h b/include/sbi/sbi_ipi.h
index b8afb5b..53468b2 100644
--- a/include/sbi/sbi_ipi.h
+++ b/include/sbi/sbi_ipi.h
@@ -10,6 +10,7 @@
 #ifndef __SBI_IPI_H__
 #define __SBI_IPI_H__
 
+#include <sbi/riscv_unpriv.h>
 #include <sbi/sbi_types.h>
 
 /* clang-format off */
@@ -28,8 +29,8 @@ struct sbi_ipi_data {
 	unsigned long ipi_type;
 };
 
-int sbi_ipi_send_many(struct sbi_scratch *scratch, ulong *pmask, u32 event,
-		      void *data);
+int sbi_ipi_send_many(struct sbi_scratch *scratch, struct unpriv_trap *uptrap,
+		      ulong *pmask, u32 event, void *data);
 
 void sbi_ipi_clear_smode(struct sbi_scratch *scratch);
 
diff --git a/lib/riscv_unpriv.c b/lib/riscv_unpriv.c
index 75d8f83..aa353c0 100644
--- a/lib/riscv_unpriv.c
+++ b/lib/riscv_unpriv.c
@@ -10,59 +10,90 @@
 #include <sbi/riscv_encoding.h>
 #include <sbi/riscv_unpriv.h>
 #include <sbi/sbi_bits.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_scratch.h>
 
-#define DEFINE_UNPRIVILEGED_LOAD_FUNCTION(type, insn)                         \
-	type load_##type(const type *addr)                                    \
+#define DEFINE_UNPRIVILEGED_LOAD_FUNCTION(type, insn, insnlen)                \
+	type load_##type(const type *addr,                                    \
+			struct sbi_scratch *scratch,                          \
+			struct unpriv_trap *trap)                             \
 	{                                                                     \
 		register ulong __mstatus asm("a2");                           \
-		type val;                                                     \
+		type val = 0;                                                 \
+		trap->ilen = insnlen;                                         \
+		trap->cause = 0;                                              \
+		trap->tval = 0;                                               \
+		sbi_hart_set_trap_info(scratch, trap);                        \
 		asm volatile(                                                 \
 			"csrrs %0, " STR(CSR_MSTATUS) ", %3\n"                \
 			#insn " %1, %2\n"                                     \
 			"csrw " STR(CSR_MSTATUS) ", %0"                       \
 		    : "+&r"(__mstatus), "=&r"(val)                            \
 		    : "m"(*addr), "r"(MSTATUS_MPRV));                         \
+		sbi_hart_set_trap_info(scratch, NULL);                        \
 		return val;                                                   \
 	}
 
-#define DEFINE_UNPRIVILEGED_STORE_FUNCTION(type, insn)                        \
-	void store_##type(type *addr, type val)                               \
+#define DEFINE_UNPRIVILEGED_STORE_FUNCTION(type, insn, insnlen)               \
+	void store_##type(type *addr, type val,                               \
+			struct sbi_scratch *scratch,                          \
+			struct unpriv_trap *trap)                             \
 	{                                                                     \
 		register ulong __mstatus asm("a3");                           \
+		trap->ilen = insnlen;                                         \
+		trap->cause = 0;                                              \
+		trap->tval = 0;                                               \
+		sbi_hart_set_trap_info(scratch, trap);                        \
 		asm volatile(                                                 \
 			"csrrs %0, " STR(CSR_MSTATUS) ", %3\n"                \
 			#insn " %1, %2\n"                                     \
 			"csrw " STR(CSR_MSTATUS) ", %0"                       \
 			: "+&r"(__mstatus)                                    \
 			: "r"(val), "m"(*addr), "r"(MSTATUS_MPRV));           \
+		sbi_hart_set_trap_info(scratch, NULL);                        \
 	}
 
-DEFINE_UNPRIVILEGED_LOAD_FUNCTION(u8, lbu)
-DEFINE_UNPRIVILEGED_LOAD_FUNCTION(u16, lhu)
-DEFINE_UNPRIVILEGED_LOAD_FUNCTION(s8, lb)
-DEFINE_UNPRIVILEGED_LOAD_FUNCTION(s16, lh)
-DEFINE_UNPRIVILEGED_LOAD_FUNCTION(s32, lw)
-DEFINE_UNPRIVILEGED_STORE_FUNCTION(u8, sb)
-DEFINE_UNPRIVILEGED_STORE_FUNCTION(u16, sh)
-DEFINE_UNPRIVILEGED_STORE_FUNCTION(u32, sw)
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(u8, lbu, 4)
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(u16, lhu, 4)
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(s8, lb, 4)
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(s16, lh, 4)
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(s32, lw, 2)
+DEFINE_UNPRIVILEGED_STORE_FUNCTION(u8, sb, 4)
+DEFINE_UNPRIVILEGED_STORE_FUNCTION(u16, sh, 4)
+DEFINE_UNPRIVILEGED_STORE_FUNCTION(u32, sw, 2)
 #if __riscv_xlen == 64
-DEFINE_UNPRIVILEGED_LOAD_FUNCTION(u32, lwu)
-DEFINE_UNPRIVILEGED_LOAD_FUNCTION(u64, ld)
-DEFINE_UNPRIVILEGED_STORE_FUNCTION(u64, sd)
-DEFINE_UNPRIVILEGED_LOAD_FUNCTION(ulong, ld)
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(u32, lwu, 4)
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(u64, ld, 2)
+DEFINE_UNPRIVILEGED_STORE_FUNCTION(u64, sd, 2)
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(ulong, ld, 2)
 #else
-DEFINE_UNPRIVILEGED_LOAD_FUNCTION(u32, lw)
-DEFINE_UNPRIVILEGED_LOAD_FUNCTION(ulong, lw)
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(u32, lw, 2)
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(ulong, lw, 2)
 
-u64 load_u64(const u64 *addr)
+u64 load_u64(const u64 *addr,
+	     struct sbi_scratch *scratch, struct unpriv_trap *trap)
 {
-	return load_u32((u32 *)addr) + ((u64)load_u32((u32 *)addr + 1) << 32);
+	u64 ret = load_u32((u32 *)addr, scratch, trap);
+
+	if (trap->cause)
+		return 0;
+	ret |= ((u64)load_u32((u32 *)addr + 1, scratch, trap) << 32);
+	if (trap->cause)
+		return 0;
+
+	return ret;
 }
 
-void store_u64(u64 *addr, u64 val)
+void store_u64(u64 *addr, u64 val,
+	       struct sbi_scratch *scratch, struct unpriv_trap *trap)
 {
-	store_u32((u32 *)addr, val);
-	store_u32((u32 *)addr + 1, val >> 32);
+	store_u32((u32 *)addr, val, scratch, trap);
+	if (trap->cause)
+		return;
+
+	store_u32((u32 *)addr + 1, val >> 32, scratch, trap);
+	if (trap->cause)
+		return;
 }
 #endif
 
diff --git a/lib/sbi_ecall.c b/lib/sbi_ecall.c
index a8ad40c..50c05d6 100644
--- a/lib/sbi_ecall.c
+++ b/lib/sbi_ecall.c
@@ -34,6 +34,7 @@ int sbi_ecall_handler(u32 hartid, ulong mcause, struct sbi_trap_regs *regs,
 		      struct sbi_scratch *scratch)
 {
 	int ret = SBI_ENOTSUPP;
+	struct unpriv_trap uptrap;
 	struct sbi_tlb_info tlb_info;
 
 	switch (regs->a7) {
@@ -59,11 +60,11 @@ int sbi_ecall_handler(u32 hartid, ulong mcause, struct sbi_trap_regs *regs,
 		ret = 0;
 		break;
 	case SBI_ECALL_SEND_IPI:
-		ret = sbi_ipi_send_many(scratch, (ulong *)regs->a0,
+		ret = sbi_ipi_send_many(scratch, &uptrap, (ulong *)regs->a0,
 					SBI_IPI_EVENT_SOFT, NULL);
 		break;
 	case SBI_ECALL_REMOTE_FENCE_I:
-		ret = sbi_ipi_send_many(scratch, (ulong *)regs->a0,
+		ret = sbi_ipi_send_many(scratch, &uptrap, (ulong *)regs->a0,
 					SBI_IPI_EVENT_FENCE_I, NULL);
 		break;
 	case SBI_ECALL_REMOTE_SFENCE_VMA:
@@ -71,7 +72,7 @@ int sbi_ecall_handler(u32 hartid, ulong mcause, struct sbi_trap_regs *regs,
 		tlb_info.size  = (unsigned long)regs->a2;
 		tlb_info.type  = SBI_TLB_FLUSH_VMA;
 
-		ret = sbi_ipi_send_many(scratch, (ulong *)regs->a0,
+		ret = sbi_ipi_send_many(scratch, &uptrap, (ulong *)regs->a0,
 					SBI_IPI_EVENT_SFENCE_VMA, &tlb_info);
 		break;
 	case SBI_ECALL_REMOTE_SFENCE_VMA_ASID:
@@ -80,7 +81,7 @@ int sbi_ecall_handler(u32 hartid, ulong mcause, struct sbi_trap_regs *regs,
 		tlb_info.asid  = (unsigned long)regs->a3;
 		tlb_info.type  = SBI_TLB_FLUSH_VMA_ASID;
 
-		ret = sbi_ipi_send_many(scratch, (ulong *)regs->a0,
+		ret = sbi_ipi_send_many(scratch, &uptrap, (ulong *)regs->a0,
 					SBI_IPI_EVENT_SFENCE_VMA_ASID,
 					&tlb_info);
 		break;
@@ -96,6 +97,10 @@ int sbi_ecall_handler(u32 hartid, ulong mcause, struct sbi_trap_regs *regs,
 
 	if (!ret) {
 		regs->mepc += 4;
+	} else if (ret == SBI_ETRAP) {
+		ret = 0;
+		sbi_trap_redirect(regs, scratch, regs->mepc,
+				  uptrap.cause, uptrap.tval);
 	}
 
 	return ret;
diff --git a/lib/sbi_ipi.c b/lib/sbi_ipi.c
index da6b06d..48d5b22 100644
--- a/lib/sbi_ipi.c
+++ b/lib/sbi_ipi.c
@@ -58,15 +58,18 @@ done:
 	return 0;
 }
 
-int sbi_ipi_send_many(struct sbi_scratch *scratch, ulong *pmask, u32 event,
-		      void *data)
+int sbi_ipi_send_many(struct sbi_scratch *scratch, struct unpriv_trap *uptrap,
+		      ulong *pmask, u32 event, void *data)
 {
 	ulong i, m;
 	ulong mask = sbi_hart_available_mask();
 	u32 hartid = sbi_current_hartid();
 
-	if (pmask)
-		mask &= load_ulong(pmask);
+	if (pmask) {
+		mask &= load_ulong(pmask, scratch, uptrap);
+		if (uptrap->cause)
+			return SBI_ETRAP;
+	}
 
 	/* send IPIs to every other hart on the set */
 	for (i = 0, m = mask; m; i++, m >>= 1)
diff --git a/lib/sbi_misaligned_ldst.c b/lib/sbi_misaligned_ldst.c
index e93464f..7f911c3 100644
--- a/lib/sbi_misaligned_ldst.c
+++ b/lib/sbi_misaligned_ldst.c
@@ -26,6 +26,7 @@ int sbi_misaligned_load_handler(u32 hartid, ulong mcause,
 				struct sbi_scratch *scratch)
 {
 	union reg_data val;
+	struct unpriv_trap uptrap;
 	ulong insn = get_insn(regs->mepc, NULL);
 	ulong addr = csr_read(CSR_MTVAL);
 	int i, fp = 0, shift = 0, len = 0;
@@ -91,8 +92,15 @@ int sbi_misaligned_load_handler(u32 hartid, ulong mcause,
 		return SBI_EILL;
 
 	val.data_u64 = 0;
-	for (i = 0; i < len; i++)
-		val.data_bytes[i] = load_u8((void *)(addr + i));
+	for (i = 0; i < len; i++) {
+		val.data_bytes[i] = load_u8((void *)(addr + i),
+					    scratch, &uptrap);
+		if (uptrap.cause) {
+			sbi_trap_redirect(regs, scratch, regs->mepc,
+					  uptrap.cause, uptrap.tval);
+			return 0;
+		}
+	}
 
 	if (!fp)
 		SET_RD(insn, regs, val.data_ulong << shift >> shift);
@@ -111,6 +119,7 @@ int sbi_misaligned_store_handler(u32 hartid, ulong mcause,
 				 struct sbi_scratch *scratch)
 {
 	union reg_data val;
+	struct unpriv_trap uptrap;
 	ulong insn = get_insn(regs->mepc, NULL);
 	ulong addr = csr_read(CSR_MTVAL);
 	int i, len = 0;
@@ -166,8 +175,15 @@ int sbi_misaligned_store_handler(u32 hartid, ulong mcause,
 	} else
 		return SBI_EILL;
 
-	for (i = 0; i < len; i++)
-		store_u8((void *)(addr + i), val.data_bytes[i]);
+	for (i = 0; i < len; i++) {
+		store_u8((void *)(addr + i), val.data_bytes[i],
+			 scratch, &uptrap);
+		if (uptrap.cause) {
+			sbi_trap_redirect(regs, scratch, regs->mepc,
+					  uptrap.cause, uptrap.tval);
+			return 0;
+		}
+	}
 
 	regs->mepc += INSN_LEN(insn);
 
diff --git a/lib/sbi_system.c b/lib/sbi_system.c
index 3381d40..2cb30d4 100644
--- a/lib/sbi_system.c
+++ b/lib/sbi_system.c
@@ -39,7 +39,7 @@ sbi_system_shutdown(struct sbi_scratch *scratch, u32 type)
 
 	/* If that fails (or is not implemented) send an IPI on every
 	 * hart to hang and then hang the current hart */
-	sbi_ipi_send_many(scratch, NULL, SBI_IPI_EVENT_HALT, NULL);
+	sbi_ipi_send_many(scratch, NULL, NULL, SBI_IPI_EVENT_HALT, NULL);
 
 	sbi_hart_hang();
 }
diff --git a/lib/sbi_trap.c b/lib/sbi_trap.c
index 4a448e5..82f7b65 100644
--- a/lib/sbi_trap.c
+++ b/lib/sbi_trap.c
@@ -9,6 +9,7 @@
 
 #include <sbi/riscv_asm.h>
 #include <sbi/riscv_encoding.h>
+#include <sbi/riscv_unpriv.h>
 #include <sbi/sbi_console.h>
 #include <sbi/sbi_ecall.h>
 #include <sbi/sbi_error.h>
@@ -134,11 +135,12 @@ int sbi_trap_redirect(struct sbi_trap_regs *regs, struct sbi_scratch *scratch,
  */
 void sbi_trap_handler(struct sbi_trap_regs *regs, struct sbi_scratch *scratch)
 {
-	int rc		= SBI_ENOTSUPP;
+	int rc = SBI_ENOTSUPP;
 	const char *msg = "trap handler failed";
 	u32 hartid = sbi_current_hartid();
 	ulong mcause = csr_read(CSR_MCAUSE);
 	ulong mtval = csr_read(CSR_MTVAL);
+	struct unpriv_trap *uptrap;
 
 	if (mcause & (1UL << (__riscv_xlen - 1))) {
 		mcause &= ~(1UL << (__riscv_xlen - 1));
@@ -175,6 +177,22 @@ void sbi_trap_handler(struct sbi_trap_regs *regs, struct sbi_scratch *scratch)
 		rc  = sbi_ecall_handler(hartid, mcause, regs, scratch);
 		msg = "ecall handler failed";
 		break;
+	case CAUSE_LOAD_ACCESS:
+	case CAUSE_STORE_ACCESS:
+	case CAUSE_LOAD_PAGE_FAULT:
+	case CAUSE_STORE_PAGE_FAULT:
+		uptrap = sbi_hart_get_trap_info(scratch);
+		if ((regs->mstatus & MSTATUS_MPRV) && uptrap) {
+			rc = 0;
+			regs->mepc += uptrap->ilen;
+			uptrap->cause = mcause;
+			uptrap->tval = mtval;
+		} else {
+			rc = sbi_trap_redirect(regs, scratch, regs->mepc,
+					       mcause, mtval);
+		}
+		msg = "page/access fault handler failed";
+		break;
 	default:
 		/* If the trap came from S or U mode, redirect it there */
 		rc = sbi_trap_redirect(regs, scratch, regs->mepc, mcause, mtval);