lib: Handle traps when doing unpriv load/store in get_insn()

We can get a page/access trap when doing unpriv load/store in
get_insn() function because on a SMP system Linux swapper running
on HART A can unmap pages from page table used by HART B.

To tackle this we extend get_insn() implementation so that if
we get trap in get_insn() then we redirect it to S-mode as fetch
page/access fault.

Signed-off-by: Anup Patel <[email protected]>

3 files changed, 63 insertions, 45 deletions

diff --git a/lib/sbi/riscv_unpriv.c b/lib/sbi/riscv_unpriv.c
index aa353c0c..ac0e03a7 100644
--- a/lib/sbi/riscv_unpriv.c
+++ b/lib/sbi/riscv_unpriv.c
@@ -97,11 +97,20 @@ void store_u64(u64 *addr, u64 val,
 }
 #endif
 
-ulong get_insn(ulong mepc, ulong *mstatus)
+ulong get_insn(ulong mepc, struct sbi_scratch *scratch,
+	       struct unpriv_trap *trap)
 {
-	register ulong __mepc asm("a2") = mepc;
-	register ulong __mstatus asm("a3");
-	ulong val;
+	ulong __mstatus = 0, val = 0;
+#ifdef __riscv_compressed
+	ulong rvc_mask = 3, tmp;
+#endif
+
+	if (trap) {
+		trap->ilen = 4;
+		trap->cause = 0;
+		trap->tval = 0;
+		sbi_hart_set_trap_info(scratch, trap);
+	}
 #ifndef __riscv_compressed
 	asm("csrrs %[mstatus], " STR(CSR_MSTATUS) ", %[mprv]\n"
 #if __riscv_xlen == 64
@@ -109,37 +118,37 @@ ulong get_insn(ulong mepc, ulong *mstatus)
 #else
 	    STR(LW) " %[insn], (%[addr])\n"
 #endif
-		     "csrw " STR(CSR_MSTATUS) ", %[mstatus]"
+	    "csrw " STR(CSR_MSTATUS) ", %[mstatus]"
 	    : [mstatus] "+&r"(__mstatus), [insn] "=&r"(val)
-	    : [mprv] "r"(MSTATUS_MPRV | MSTATUS_MXR), [addr] "r"(__mepc));
+	    : [mprv] "r"(MSTATUS_MPRV | MSTATUS_MXR), [addr] "r"(mepc));
 #else
-	ulong rvc_mask = 3, tmp;
 	asm("csrrs %[mstatus], " STR(CSR_MSTATUS) ", %[mprv]\n"
-						  "and %[tmp], %[addr], 2\n"
-						  "bnez %[tmp], 1f\n"
-#if __riscv_xlen == 64
-	    STR(LWU) " %[insn], (%[addr])\n"
-#else
-	    STR(LW) " %[insn], (%[addr])\n"
-#endif
-		     "and %[tmp], %[insn], %[rvc_mask]\n"
-		     "beq %[tmp], %[rvc_mask], 2f\n"
-		     "sll %[insn], %[insn], %[xlen_minus_16]\n"
-		     "srl %[insn], %[insn], %[xlen_minus_16]\n"
-		     "j 2f\n"
-		     "1:\n"
-		     "lhu %[insn], (%[addr])\n"
-		     "and %[tmp], %[insn], %[rvc_mask]\n"
-		     "bne %[tmp], %[rvc_mask], 2f\n"
-		     "lhu %[tmp], 2(%[addr])\n"
-		     "sll %[tmp], %[tmp], 16\n"
-		     "add %[insn], %[insn], %[tmp]\n"
-		     "2: csrw " STR(CSR_MSTATUS) ", %[mstatus]"
+	    "lhu %[insn], (%[addr])\n"
+	    "and %[tmp], %[insn], %[rvc_mask]\n"
+	    "bne %[tmp], %[rvc_mask], 2f\n"
+	    "lhu %[tmp], 2(%[addr])\n"
+	    "sll %[tmp], %[tmp], 16\n"
+	    "add %[insn], %[insn], %[tmp]\n"
+	    "2: csrw " STR(CSR_MSTATUS) ", %[mstatus]"
 	    : [mstatus] "+&r"(__mstatus), [insn] "=&r"(val), [tmp] "=&r"(tmp)
-	    : [mprv] "r"(MSTATUS_MPRV | MSTATUS_MXR), [addr] "r"(__mepc),
-	      [rvc_mask] "r"(rvc_mask), [xlen_minus_16] "i"(__riscv_xlen - 16));
+	    : [mprv] "r"(MSTATUS_MPRV | MSTATUS_MXR), [addr] "r"(mepc),
+	      [rvc_mask] "r"(rvc_mask));
 #endif
-	if (mstatus)
-		*mstatus = __mstatus;
+	if (trap) {
+		sbi_hart_set_trap_info(scratch, NULL);
+		switch (trap->cause) {
+		case CAUSE_LOAD_ACCESS:
+			trap->cause = CAUSE_FETCH_ACCESS;
+			trap->tval = mepc;
+			break;
+		case CAUSE_LOAD_PAGE_FAULT:
+			trap->cause = CAUSE_FETCH_PAGE_FAULT;
+			trap->tval = mepc;
+			break;
+		default:
+			break;
+		};
+	}
+
 	return val;
 }
diff --git a/lib/sbi/sbi_illegal_insn.c b/lib/sbi/sbi_illegal_insn.c
index 55418386..b6ba4b05 100644
--- a/lib/sbi/sbi_illegal_insn.c
+++ b/lib/sbi/sbi_illegal_insn.c
@@ -117,10 +117,15 @@ int sbi_illegal_insn_handler(u32 hartid, ulong mcause,
 			     struct sbi_scratch *scratch)
 {
 	ulong insn = csr_read(mbadaddr);
+	struct unpriv_trap uptrap;
 
 	if (unlikely((insn & 3) != 3)) {
-		if (insn == 0)
-			insn = get_insn(regs->mepc, NULL);
+		if (insn == 0) {
+			insn = get_insn(regs->mepc, scratch, &uptrap);
+			if (uptrap.cause)
+				return sbi_trap_redirect(regs, scratch,
+					regs->mepc, uptrap.cause, uptrap.tval);
+		}
 		if ((insn & 3) != 3)
 			return truly_illegal_insn(insn, hartid, mcause, regs,
 						  scratch);
diff --git a/lib/sbi/sbi_misaligned_ldst.c b/lib/sbi/sbi_misaligned_ldst.c
index 1c4e5df4..e0f10bc6 100644
--- a/lib/sbi/sbi_misaligned_ldst.c
+++ b/lib/sbi/sbi_misaligned_ldst.c
@@ -27,10 +27,14 @@ int sbi_misaligned_load_handler(u32 hartid, ulong mcause,
 {
 	union reg_data val;
 	struct unpriv_trap uptrap;
-	ulong insn = get_insn(regs->mepc, NULL);
+	ulong insn = get_insn(regs->mepc, scratch, &uptrap);
 	ulong addr = csr_read(CSR_MTVAL);
 	int i, fp = 0, shift = 0, len = 0;
 
+	if (uptrap.cause)
+		return sbi_trap_redirect(regs, scratch, regs->mepc,
+					 uptrap.cause, uptrap.tval);
+
 	if ((insn & INSN_MASK_LW) == INSN_MATCH_LW) {
 		len   = 4;
 		shift = 8 * (sizeof(ulong) - len);
@@ -100,11 +104,9 @@ int sbi_misaligned_load_handler(u32 hartid, ulong mcause,
 	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 (uptrap.cause)
+			return sbi_trap_redirect(regs, scratch, regs->mepc,
+						 uptrap.cause, uptrap.tval);
 	}
 
 	if (!fp)
@@ -127,10 +129,14 @@ int sbi_misaligned_store_handler(u32 hartid, ulong mcause,
 {
 	union reg_data val;
 	struct unpriv_trap uptrap;
-	ulong insn = get_insn(regs->mepc, NULL);
+	ulong insn = get_insn(regs->mepc, scratch, &uptrap);
 	ulong addr = csr_read(CSR_MTVAL);
 	int i, len = 0;
 
+	if (uptrap.cause)
+		return sbi_trap_redirect(regs, scratch, regs->mepc,
+					 uptrap.cause, uptrap.tval);
+
 	val.data_ulong = GET_RS2(insn, regs);
 
 	if ((insn & INSN_MASK_SW) == INSN_MATCH_SW) {
@@ -190,11 +196,9 @@ int sbi_misaligned_store_handler(u32 hartid, ulong mcause,
 	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;
-		}
+		if (uptrap.cause)
+			return sbi_trap_redirect(regs, scratch, regs->mepc,
+						 uptrap.cause, uptrap.tval);
 	}
 
 	regs->mepc += INSN_LEN(insn);