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 <anup.patel@wdc.com>

include/sbi/riscv_unpriv.h

@@ -43,6 +43,7 @@ DECLARE_UNPRIVILEGED_LOAD_FUNCTION(u64)
 DECLARE_UNPRIVILEGED_STORE_FUNCTION(u64)
 DECLARE_UNPRIVILEGED_LOAD_FUNCTION(ulong)
 
-ulong get_insn(ulong mepc, ulong *mstatus);
+ulong get_insn(ulong mepc, struct sbi_scratch *scratch,
+	       struct unpriv_trap *trap);
 
 #endif

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;
+	ulong __mstatus = 0, val = 0;
-	register ulong __mstatus asm("a3");
+#ifdef __riscv_compressed
-	ulong val;
+	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"
+	    "lhu %[insn], (%[addr])\n"
-						  "bnez %[tmp], 1f\n"
+	    "and %[tmp], %[insn], %[rvc_mask]\n"
-#if __riscv_xlen == 64
+	    "bne %[tmp], %[rvc_mask], 2f\n"
-	    STR(LWU) " %[insn], (%[addr])\n"
+	    "lhu %[tmp], 2(%[addr])\n"
-#else
+	    "sll %[tmp], %[tmp], 16\n"
-	    STR(LW) " %[insn], (%[addr])\n"
+	    "add %[insn], %[insn], %[tmp]\n"
-#endif
+	    "2: csrw " STR(CSR_MSTATUS) ", %[mstatus]"
-		     "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]"
 	    : [mstatus] "+&r"(__mstatus), [insn] "=&r"(val), [tmp] "=&r"(tmp)
-	    : [mprv] "r"(MSTATUS_MPRV | MSTATUS_MXR), [addr] "r"(__mepc),
+	    : [mprv] "r"(MSTATUS_MPRV | MSTATUS_MXR), [addr] "r"(mepc),
-	      [rvc_mask] "r"(rvc_mask), [xlen_minus_16] "i"(__riscv_xlen - 16));
+	      [rvc_mask] "r"(rvc_mask));
 #endif
-	if (mstatus)
+	if (trap) {
-		*mstatus = __mstatus;
+		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;
 }

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)
+		if (insn == 0) {
-			insn = get_insn(regs->mepc, NULL);
+			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);

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) {
+		if (uptrap.cause)
-			sbi_trap_redirect(regs, scratch, regs->mepc,
+			return sbi_trap_redirect(regs, scratch, regs->mepc,
-					  uptrap.cause, uptrap.tval);
+						 uptrap.cause, uptrap.tval);
-			return 0;
-		}
 	}
 
 	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) {
+		if (uptrap.cause)
-			sbi_trap_redirect(regs, scratch, regs->mepc,
+			return sbi_trap_redirect(regs, scratch, regs->mepc,
-					  uptrap.cause, uptrap.tval);
+						 uptrap.cause, uptrap.tval);
-			return 0;
-		}
 	}
 
 	regs->mepc += INSN_LEN(insn);