lib: Optimize unpriv load/store implementation

This patch optimize unpriv load/store implementation by having
dedicated unpriv trap handler (just like KVM RISC-V).

As a result of this optimization:
1. We have reduced roughly 13+ instruction in all unpriv load/store
   functions. The reduced instruction also include two function calls.
2. Per-HART trap info pointer in scratch space is now redundant
   hence removed.
3. The sbi_trap_handler() is now much cleaner because we don't have
   to handle unpriv load/store traps.

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

5 files changed, 124 insertions, 122 deletions

diff --git a/lib/sbi/objects.mk b/lib/sbi/objects.mk
index 5f1bdbf..7e700fa 100644
--- a/lib/sbi/objects.mk
+++ b/lib/sbi/objects.mk
@@ -37,3 +37,4 @@ libsbi-objs-y += sbi_timer.o
 libsbi-objs-y += sbi_tlb.o
 libsbi-objs-y += sbi_trap.o
 libsbi-objs-y += sbi_unpriv.o
+libsbi-objs-y += sbi_unpriv_trap.o
diff --git a/lib/sbi/sbi_hart.c b/lib/sbi/sbi_hart.c
index e71725c..b0f9919 100644
--- a/lib/sbi/sbi_hart.c
+++ b/lib/sbi/sbi_hart.c
@@ -17,6 +17,11 @@
 #include <sbi/sbi_hart.h>
 #include <sbi/sbi_platform.h>
 
+extern void __sbi_unpriv_trap(void);
+extern void __sbi_unpriv_trap_hext(void);
+
+void (*sbi_hart_unpriv_trap)(void) = &__sbi_unpriv_trap;
+
 static void mstatus_init(struct sbi_scratch *scratch, u32 hartid)
 {
 	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
@@ -203,17 +208,13 @@ static int pmp_init(struct sbi_scratch *scratch, u32 hartid)
 	return 0;
 }
 
-static unsigned long trap_info_offset;
-
 int sbi_hart_init(struct sbi_scratch *scratch, u32 hartid, bool cold_boot)
 {
 	int rc;
 
 	if (cold_boot) {
-		trap_info_offset = sbi_scratch_alloc_offset(__SIZEOF_POINTER__,
-							    "HART_TRAP_INFO");
-		if (!trap_info_offset)
-			return SBI_ENOMEM;
+		if (misa_extension('H'))
+			sbi_hart_unpriv_trap = &__sbi_unpriv_trap_hext;
 	}
 
 	mstatus_init(scratch, hartid);
@@ -229,29 +230,6 @@ int sbi_hart_init(struct sbi_scratch *scratch, u32 hartid, bool cold_boot)
 	return pmp_init(scratch, hartid);
 }
 
-void *sbi_hart_get_trap_info(struct sbi_scratch *scratch)
-{
-	unsigned long *trap_info;
-
-	if (!trap_info_offset)
-		return NULL;
-
-	trap_info = sbi_scratch_offset_ptr(scratch, trap_info_offset);
-
-	return (void *)(*trap_info);
-}
-
-void sbi_hart_set_trap_info(struct sbi_scratch *scratch, void *data)
-{
-	unsigned long *trap_info;
-
-	if (!trap_info_offset)
-		return;
-
-	trap_info = sbi_scratch_offset_ptr(scratch, trap_info_offset);
-	*trap_info = (unsigned long)data;
-}
-
 void __attribute__((noreturn)) sbi_hart_hang(void)
 {
 	while (1)
diff --git a/lib/sbi/sbi_trap.c b/lib/sbi/sbi_trap.c
index b4ba2ac..9aa6b85 100644
--- a/lib/sbi/sbi_trap.c
+++ b/lib/sbi/sbi_trap.c
@@ -220,7 +220,7 @@ void sbi_trap_handler(struct sbi_trap_regs *regs,
 	u32 hartid = current_hartid();
 	ulong mcause = csr_read(CSR_MCAUSE);
 	ulong mtval = csr_read(CSR_MTVAL), mtval2 = 0, mtinst = 0;
-	struct sbi_trap_info trap, *uptrap;
+	struct sbi_trap_info trap;
 
 	if (misa_extension('H')) {
 		mtval2 = csr_read(CSR_MTVAL2);
@@ -266,29 +266,6 @@ void sbi_trap_handler(struct sbi_trap_regs *regs,
 		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;
-			uptrap->epc = regs->mepc;
-			regs->mepc += 4;
-			uptrap->cause = mcause;
-			uptrap->tval = mtval;
-			uptrap->tval2 = mtval2;
-			uptrap->tinst = mtinst;
-		} else {
-			trap.epc = regs->mepc;
-			trap.cause = mcause;
-			trap.tval = mtval;
-			trap.tval2 = mtval2;
-			trap.tinst = mtinst;
-			rc = sbi_trap_redirect(regs, &trap, scratch);
-		}
-		msg = "page/access fault handler failed";
-		break;
 	default:
 		/* If the trap came from S or U mode, redirect it there */
 		trap.epc = regs->mepc;
diff --git a/lib/sbi/sbi_unpriv.c b/lib/sbi/sbi_unpriv.c
index 77ff2e9..1724420 100644
--- a/lib/sbi/sbi_unpriv.c
+++ b/lib/sbi/sbi_unpriv.c
@@ -19,25 +19,30 @@
 			     struct sbi_scratch *scratch,                     \
 			     struct sbi_trap_info *trap)                      \
 	{                                                                     \
-		register ulong __mstatus asm("a2");                           \
-		type val = 0;                                                 \
-		trap->epc = 0;                                                \
+		register ulong tinfo asm("a3");                               \
+		register ulong ttmp asm("a4");                                \
+		register ulong mstatus asm("a5");                             \
+		register ulong mtvec asm("a6") = sbi_hart_unpriv_trap_addr(); \
+		type ret = 0;                                                 \
 		trap->cause = 0;                                              \
-		trap->tval = 0;                                               \
-		trap->tval2 = 0;                                              \
-		trap->tinst = 0;                                              \
-		sbi_hart_set_trap_info(scratch, trap);                        \
 		asm volatile(                                                 \
-			"csrrs %0, " STR(CSR_MSTATUS) ", %3\n"                \
+			"add %[tinfo], %[taddr], zero\n"                      \
+			"add %[ttmp], %[taddr], zero\n"                       \
+			"csrrw %[mtvec], " STR(CSR_MTVEC) ", %[mtvec]\n"      \
+			"csrrs %[mstatus], " STR(CSR_MSTATUS) ", %[mprv]\n"   \
 			".option push\n"                                      \
 			".option norvc\n"                                     \
-			#insn " %1, %2\n"                                     \
+			#insn " %[ret], %[addr]\n"                            \
 			".option pop\n"                                       \
-			"csrw " STR(CSR_MSTATUS) ", %0"                       \
-		    : "+&r"(__mstatus), "=&r"(val)                            \
-		    : "m"(*addr), "r"(MSTATUS_MPRV));                         \
-		sbi_hart_set_trap_info(scratch, NULL);                        \
-		return val;                                                   \
+			"csrw " STR(CSR_MSTATUS) ", %[mstatus]\n"             \
+			"csrw " STR(CSR_MTVEC) ", %[mtvec]"                   \
+		    : [mstatus] "+&r"(mstatus), [mtvec] "+&r"(mtvec),         \
+		      [tinfo] "+&r"(tinfo), [ttmp] "+&r"(ttmp),               \
+		      [ret] "=&r"(ret)                                        \
+		    : [addr] "m"(*addr), [mprv] "r"(MSTATUS_MPRV),            \
+		      [taddr] "r"((ulong)trap)                                \
+		    : "memory");                                              \
+		return ret;                                                   \
 	}
 
 #define DEFINE_UNPRIVILEGED_STORE_FUNCTION(type, insn)                        \
@@ -45,23 +50,27 @@
 			      struct sbi_scratch *scratch,                    \
 			      struct sbi_trap_info *trap)                     \
 	{                                                                     \
-		register ulong __mstatus asm("a3");                           \
-		trap->epc = 0;                                                \
+		register ulong tinfo asm("a3");                               \
+		register ulong ttmp asm("a4");                                \
+		register ulong mstatus asm("a5");                             \
+		register ulong mtvec asm("a6") = sbi_hart_unpriv_trap_addr(); \
 		trap->cause = 0;                                              \
-		trap->tval = 0;                                               \
-		trap->tval2 = 0;                                              \
-		trap->tinst = 0;                                              \
-		sbi_hart_set_trap_info(scratch, trap);                        \
 		asm volatile(                                                 \
-			"csrrs %0, " STR(CSR_MSTATUS) ", %3\n"                \
+			"add %[tinfo], %[taddr], zero\n"                      \
+			"add %[ttmp], %[taddr], zero\n"                       \
+			"csrrw %[mtvec], " STR(CSR_MTVEC) ", %[mtvec]\n"      \
+			"csrrs %[mstatus], " STR(CSR_MSTATUS) ", %[mprv]\n"   \
 			".option push\n"                                      \
 			".option norvc\n"                                     \
-			#insn " %1, %2\n"                                     \
+			#insn " %[val], %[addr]\n"                            \
 			".option pop\n"                                       \
-			"csrw " STR(CSR_MSTATUS) ", %0"                       \
-			: "+&r"(__mstatus)                                    \
-			: "r"(val), "m"(*addr), "r"(MSTATUS_MPRV));           \
-		sbi_hart_set_trap_info(scratch, NULL);                        \
+			"csrw " STR(CSR_MSTATUS) ", %[mstatus]\n"             \
+			"csrw " STR(CSR_MTVEC) ", %[mtvec]"                   \
+		    : [mstatus] "+&r"(mstatus), [mtvec] "+&r"(mtvec),         \
+		      [tinfo] "+&r"(tinfo), [ttmp] "+&r"(ttmp)                \
+		    : [addr] "m"(*addr), [mprv] "r"(MSTATUS_MPRV),            \
+		      [taddr] "r"((ulong)trap), [val] "r"(val)                \
+		    : "memory");              \
 	}
 
 DEFINE_UNPRIVILEGED_LOAD_FUNCTION(u8, lbu)
@@ -113,52 +122,33 @@ void sbi_store_u64(u64 *addr, u64 val,
 ulong sbi_get_insn(ulong mepc, struct sbi_scratch *scratch,
 		   struct sbi_trap_info *trap)
 {
-	ulong __mstatus = 0, val = 0;
-#ifdef __riscv_compressed
-	ulong rvc_mask = 3, tmp;
-#endif
+	register ulong tinfo asm("a3");
+	register ulong ttmp asm("a4");
+	register ulong mstatus asm("a5");
+	register ulong mtvec asm("a6") = sbi_hart_unpriv_trap_addr();
+	ulong insn = 0;
 
-	trap->epc = 0;
 	trap->cause = 0;
-	trap->tval = 0;
-	trap->tval2 = 0;
-	trap->tinst = 0;
-	sbi_hart_set_trap_info(scratch, trap);
 
-#ifndef __riscv_compressed
-	asm("csrrs %[mstatus], " STR(CSR_MSTATUS) ", %[mprv]\n"
-	    ".option push\n"
-	    ".option norvc\n"
-#if __riscv_xlen == 64
-	    STR(LWU) " %[insn], (%[addr])\n"
-#else
-	    STR(LW) " %[insn], (%[addr])\n"
-#endif
-	    ".option pop\n"
-	    "csrw " STR(CSR_MSTATUS) ", %[mstatus]"
-	    : [mstatus] "+&r"(__mstatus), [insn] "=&r"(val)
-	    : [mprv] "r"(MSTATUS_MPRV | MSTATUS_MXR), [addr] "r"(mepc));
-#else
-	asm("csrrs %[mstatus], " STR(CSR_MSTATUS) ", %[mprv]\n"
-	    ".option push\n"
-	    ".option norvc\n"
+	asm volatile(
+	    "add %[tinfo], %[taddr], zero\n"
+	    "csrrw %[mtvec], " STR(CSR_MTVEC) ", %[mtvec]\n"
+	    "csrrs %[mstatus], " STR(CSR_MSTATUS) ", %[mprv]\n"
 	    "lhu %[insn], (%[addr])\n"
-	    ".option pop\n"
-	    "and %[tmp], %[insn], %[rvc_mask]\n"
-	    "bne %[tmp], %[rvc_mask], 2f\n"
-	    ".option push\n"
-	    ".option norvc\n"
-	    "lhu %[tmp], 2(%[addr])\n"
-	    ".option pop\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));
-#endif
-
-	sbi_hart_set_trap_info(scratch, NULL);
+	    "andi %[ttmp], %[insn], 3\n"
+	    "addi %[ttmp], %[ttmp], -3\n"
+	    "bne %[ttmp], zero, 2f\n"
+	    "lhu %[ttmp], 2(%[addr])\n"
+	    "sll %[ttmp], %[ttmp], 16\n"
+	    "add %[insn], %[insn], %[ttmp]\n"
+	    "2: csrw " STR(CSR_MSTATUS) ", %[mstatus]\n"
+	    "csrw " STR(CSR_MTVEC) ", %[mtvec]"
+	    : [mstatus] "+&r"(mstatus), [mtvec] "+&r"(mtvec),
+	      [tinfo] "+&r"(tinfo), [ttmp] "+&r"(ttmp),
+	      [insn] "=&r"(insn)
+	    : [mprv] "r"(MSTATUS_MPRV | MSTATUS_MXR),
+	      [taddr] "r"((ulong)trap), [addr] "r"(mepc)
+	    : "memory");
 
 	switch (trap->cause) {
 	case CAUSE_LOAD_ACCESS:
@@ -177,5 +167,5 @@ ulong sbi_get_insn(ulong mepc, struct sbi_scratch *scratch,
 		break;
 	};
 
-	return val;
+	return insn;
 }
diff --git a/lib/sbi/sbi_unpriv_trap.S b/lib/sbi/sbi_unpriv_trap.S
new file mode 100644
index 0000000..180c376
--- /dev/null
+++ b/lib/sbi/sbi_unpriv_trap.S
@@ -0,0 +1,56 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/sbi_trap.h>
+
+	/*
+	 * We assume that faulting unpriv load/store instruction is
+	 * is 4-byte long and blindly increment SEPC by 4.
+	 *
+	 * The trap info will be saved as follows:
+	 * A3 <- pointer struct sbi_trap_info
+	 * A4 <- temporary
+	 */
+
+	.align 3
+	.global __sbi_unpriv_trap
+__sbi_unpriv_trap:
+	/* Without H-extension so, MTVAL2 and MTINST CSRs not available */
+	csrr	a4, CSR_MEPC
+	REG_S	a4, SBI_TRAP_INFO_OFFSET(epc)(a3)
+	csrr	a4, CSR_MCAUSE
+	REG_S	a4, SBI_TRAP_INFO_OFFSET(cause)(a3)
+	csrr	a4, CSR_MTVAL
+	REG_S	a4, SBI_TRAP_INFO_OFFSET(tval)(a3)
+	REG_S	zero, SBI_TRAP_INFO_OFFSET(tval2)(a3)
+	REG_S	zero, SBI_TRAP_INFO_OFFSET(tinst)(a3)
+	csrr	a4, CSR_MEPC
+	addi	a4, a4, 4
+	csrw	CSR_MEPC, a4
+	mret
+
+	.align 3
+	.global __sbi_unpriv_trap_hext
+__sbi_unpriv_trap_hext:
+	/* With H-extension so, MTVAL2 and MTINST CSRs available */
+	csrr	a4, CSR_MEPC
+	REG_S	a4, SBI_TRAP_INFO_OFFSET(epc)(a3)
+	csrr	a4, CSR_MCAUSE
+	REG_S	a4, SBI_TRAP_INFO_OFFSET(cause)(a3)
+	csrr	a4, CSR_MTVAL
+	REG_S	a4, SBI_TRAP_INFO_OFFSET(tval)(a3)
+	csrr	a4, CSR_MTVAL2
+	REG_S	a4, SBI_TRAP_INFO_OFFSET(tval2)(a3)
+	csrr	a4, CSR_MTINST
+	REG_S	a4, SBI_TRAP_INFO_OFFSET(tinst)(a3)
+	csrr	a4, CSR_MEPC
+	addi	a4, a4, 4
+	csrw	CSR_MEPC, a4
+	mret