lib: Extend trap redirection for hypervisor v0.5 spec

The hypervisor v0.5 spec introduces two new CSRs for both M-mode
and HS-mode which need to be considered when redirecting traps
hence this patch.

Signed-off-by: Anup Patel <anup.patel@wdc.com>

1 files changed, 37 insertions, 19 deletions

diff --git a/lib/sbi/sbi_trap.c b/lib/sbi/sbi_trap.c
index 7025ad9..d5ab24b 100644
--- a/lib/sbi/sbi_trap.c
+++ b/lib/sbi/sbi_trap.c
@@ -20,12 +20,17 @@
 #include <sbi/sbi_trap.h>
 
 static void __noreturn sbi_trap_error(const char *msg, int rc, u32 hartid,
-				      ulong mcause, ulong mtval,
-				      struct sbi_trap_regs *regs)
+				      ulong mcause, ulong mtval, ulong mtval2,
+				      ulong mtinst, struct sbi_trap_regs *regs)
 {
 	sbi_printf("%s: hart%d: %s (error %d)\n", __func__, hartid, msg, rc);
 	sbi_printf("%s: hart%d: mcause=0x%" PRILX " mtval=0x%" PRILX "\n",
 		   __func__, hartid, mcause, mtval);
+	if (misa_extension('H')) {
+		sbi_printf("%s: hart%d: mtval2=0x%" PRILX
+			   " mtinst=0x%" PRILX "\n",
+			   __func__, hartid, mtval2, mtinst);
+	}
 	sbi_printf("%s: hart%d: mepc=0x%" PRILX " mstatus=0x%" PRILX "\n",
 		   __func__, hartid, regs->mepc, regs->mstatus);
 	sbi_printf("%s: hart%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", __func__,
@@ -80,10 +85,8 @@ int sbi_trap_redirect(struct sbi_trap_regs *regs,
 	ulong hstatus, vsstatus, prev_mode;
 #if __riscv_xlen == 32
 	bool prev_virt = (regs->mstatusH & MSTATUSH_MPV) ? TRUE : FALSE;
-	bool prev_stage2 = (regs->mstatusH & MSTATUSH_MTL) ? TRUE : FALSE;
 #else
 	bool prev_virt = (regs->mstatus & MSTATUS_MPV) ? TRUE : FALSE;
-	bool prev_stage2 = (regs->mstatus & MSTATUS_MTL) ? TRUE : FALSE;
 #endif
 	/* By default, we redirect to HS-mode */
 	bool next_virt = FALSE;
@@ -94,7 +97,7 @@ int sbi_trap_redirect(struct sbi_trap_regs *regs,
 		return SBI_ENOTSUPP;
 
 	/* For certain exceptions from VS/VU-mode we redirect to VS-mode */
-	if (misa_extension('H') && prev_virt && !prev_stage2) {
+	if (misa_extension('H') && prev_virt) {
 		switch (trap->cause) {
 		case CAUSE_FETCH_PAGE_FAULT:
 		case CAUSE_LOAD_PAGE_FAULT:
@@ -106,20 +109,18 @@ int sbi_trap_redirect(struct sbi_trap_regs *regs,
 		};
 	}
 
-	/* Update MSTATUS MPV and MTL bits */
+	/* Update MSTATUS MPV bits */
 #if __riscv_xlen == 32
 	regs->mstatusH &= ~MSTATUSH_MPV;
 	regs->mstatusH |= (next_virt) ? MSTATUSH_MPV : 0UL;
-	regs->mstatusH &= ~MSTATUSH_MTL;
 #else
 	regs->mstatus &= ~MSTATUS_MPV;
 	regs->mstatus |= (next_virt) ? MSTATUS_MPV : 0UL;
-	regs->mstatus &= ~MSTATUS_MTL;
 #endif
 
 	/* Update HSTATUS for VS/VU-mode to HS-mode transition */
 	if (misa_extension('H') && prev_virt && !next_virt) {
-		/* Update HSTATUS SP2P, SP2V, SPV, and STL bits */
+		/* Update HSTATUS SP2P, SP2V, and SPV bits */
 		hstatus = csr_read(CSR_HSTATUS);
 		hstatus &= ~HSTATUS_SP2P;
 		hstatus |= (regs->mstatus & MSTATUS_SPP) ? HSTATUS_SP2P : 0;
@@ -127,9 +128,9 @@ int sbi_trap_redirect(struct sbi_trap_regs *regs,
 		hstatus |= (hstatus & HSTATUS_SPV) ? HSTATUS_SP2V : 0;
 		hstatus &= ~HSTATUS_SPV;
 		hstatus |= (prev_virt) ? HSTATUS_SPV : 0;
-		hstatus &= ~HSTATUS_STL;
-		hstatus |= (prev_stage2) ? HSTATUS_STL : 0;
 		csr_write(CSR_HSTATUS, hstatus);
+		csr_write(CSR_HTVAL, trap->tval2);
+		csr_write(CSR_HTINST, trap->tinst);
 	}
 
 	/* Update exception related CSRs */
@@ -203,8 +204,10 @@ int sbi_trap_redirect(struct sbi_trap_regs *regs,
  * 1. The 'mscratch' CSR is pointing to sbi_scratch of current HART
  * 2. The 'mcause' CSR is having exception/interrupt cause
  * 3. The 'mtval' CSR is having additional trap information
- * 4. Stack pointer (SP) is setup for current HART
- * 5. Interrupts are disabled in MSTATUS CSR
+ * 4. The 'mtval2' CSR is having additional trap information
+ * 5. The 'mtinst' CSR is having decoded trap instruction
+ * 6. Stack pointer (SP) is setup for current HART
+ * 7. Interrupts are disabled in MSTATUS CSR
  *
  * @param regs pointer to register state
  * @param scratch pointer to sbi_scratch of current HART
@@ -216,9 +219,14 @@ void sbi_trap_handler(struct sbi_trap_regs *regs,
 	const char *msg = "trap handler failed";
 	u32 hartid = sbi_current_hartid();
 	ulong mcause = csr_read(CSR_MCAUSE);
-	ulong mtval = csr_read(CSR_MTVAL);
+	ulong mtval = csr_read(CSR_MTVAL), mtval2 = 0, mtinst = 0;
 	struct sbi_trap_info trap, *uptrap;
 
+	if (misa_extension('H')) {
+		mtval2 = csr_read(CSR_MTVAL2);
+		mtinst = csr_read(CSR_MTINST);
+	}
+
 	if (mcause & (1UL << (__riscv_xlen - 1))) {
 		mcause &= ~(1UL << (__riscv_xlen - 1));
 		switch (mcause) {
@@ -237,15 +245,19 @@ void sbi_trap_handler(struct sbi_trap_regs *regs,
 
 	switch (mcause) {
 	case CAUSE_ILLEGAL_INSTRUCTION:
-		rc  = sbi_illegal_insn_handler(hartid, mcause, regs, scratch);
+		rc  = sbi_illegal_insn_handler(hartid, mcause, mtval,
+					       regs, scratch);
 		msg = "illegal instruction handler failed";
 		break;
 	case CAUSE_MISALIGNED_LOAD:
-		rc = sbi_misaligned_load_handler(hartid, mcause, regs, scratch);
+		rc = sbi_misaligned_load_handler(hartid, mcause, mtval,
+						 mtval2, mtinst, regs,
+						 scratch);
 		msg = "misaligned load handler failed";
 		break;
 	case CAUSE_MISALIGNED_STORE:
-		rc  = sbi_misaligned_store_handler(hartid, mcause, regs,
+		rc  = sbi_misaligned_store_handler(hartid, mcause, mtval,
+						   mtval2, mtinst, regs,
 						   scratch);
 		msg = "misaligned store handler failed";
 		break;
@@ -265,10 +277,14 @@ void sbi_trap_handler(struct sbi_trap_regs *regs,
 			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";
@@ -278,13 +294,15 @@ void sbi_trap_handler(struct sbi_trap_regs *regs,
 		trap.epc = regs->mepc;
 		trap.cause = mcause;
 		trap.tval = mtval;
+		trap.tval2 = mtval2;
+		trap.tinst = mtinst;
 		rc = sbi_trap_redirect(regs, &trap, scratch);
 		break;
 	};
 
 trap_error:
 	if (rc) {
-		sbi_trap_error(msg, rc, hartid, mcause, csr_read(CSR_MTVAL),
-			       regs);
+		sbi_trap_error(msg, rc, hartid, mcause, mtval,
+			       mtval2, mtinst, regs);
 	}
 }