include: Extend get_insn() to read instruction from VS/VU mode

Current implementation of get_insn() is not suitable for reading
instruction from VS/VU mode because we have to set SSTATUS_MXR bit
in VSSTATUS CSR for reading instruction from VS/VU mode.

This patch extends get_insn() to read instruction from VS/VU mode.

Signed-off-by: Anup Patel <anup.patel@wdc.com>
Signed-off-by: Alistair Francis <alistair.francis@wdc.com>

3 files changed, 46 insertions, 27 deletions

diff --git a/lib/sbi/riscv_unpriv.c b/lib/sbi/riscv_unpriv.c
index ac0e03a..ef70e3a 100644
--- a/lib/sbi/riscv_unpriv.c
+++ b/lib/sbi/riscv_unpriv.c
@@ -97,20 +97,22 @@ void store_u64(u64 *addr, u64 val,
 }
 #endif
 
-ulong get_insn(ulong mepc, struct sbi_scratch *scratch,
+ulong get_insn(ulong mepc, bool virt, struct sbi_scratch *scratch,
 	       struct unpriv_trap *trap)
 {
-	ulong __mstatus = 0, val = 0;
+	ulong __mstatus = 0, __vsstatus = 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);
-	}
+	trap->ilen = 4;
+	trap->cause = 0;
+	trap->tval = 0;
+	sbi_hart_set_trap_info(scratch, trap);
+
+	if (virt)
+		__vsstatus = csr_read_set(CSR_VSSTATUS, SSTATUS_MXR);
+
 #ifndef __riscv_compressed
 	asm("csrrs %[mstatus], " STR(CSR_MSTATUS) ", %[mprv]\n"
 #if __riscv_xlen == 64
@@ -134,21 +136,23 @@ ulong get_insn(ulong mepc, struct sbi_scratch *scratch,
 	    : [mprv] "r"(MSTATUS_MPRV | MSTATUS_MXR), [addr] "r"(mepc),
 	      [rvc_mask] "r"(rvc_mask));
 #endif
-	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;
-		};
-	}
+
+	if (virt)
+		csr_write(CSR_VSSTATUS, __vsstatus);
+
+	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 7ea42f7..e75c13c 100644
--- a/lib/sbi/sbi_illegal_insn.c
+++ b/lib/sbi/sbi_illegal_insn.c
@@ -130,12 +130,17 @@ int sbi_illegal_insn_handler(u32 hartid, ulong mcause,
 			     struct sbi_trap_regs *regs,
 			     struct sbi_scratch *scratch)
 {
-	ulong insn = csr_read(mbadaddr);
+	ulong insn = csr_read(CSR_MTVAL);
+#if __riscv_xlen == 32
+	bool virt = (regs->mstatusH & MSTATUSH_MPV) ? TRUE : FALSE;
+#else
+	bool virt = (regs->mstatus & MSTATUS_MPV) ? TRUE : FALSE;
+#endif
 	struct unpriv_trap uptrap;
 
 	if (unlikely((insn & 3) != 3)) {
 		if (insn == 0) {
-			insn = get_insn(regs->mepc, scratch, &uptrap);
+			insn = get_insn(regs->mepc, virt, scratch, &uptrap);
 			if (uptrap.cause)
 				return sbi_trap_redirect(regs, scratch,
 					regs->mepc, uptrap.cause, uptrap.tval);
diff --git a/lib/sbi/sbi_misaligned_ldst.c b/lib/sbi/sbi_misaligned_ldst.c
index e0f10bc..10c467c 100644
--- a/lib/sbi/sbi_misaligned_ldst.c
+++ b/lib/sbi/sbi_misaligned_ldst.c
@@ -27,9 +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, scratch, &uptrap);
 	ulong addr = csr_read(CSR_MTVAL);
 	int i, fp = 0, shift = 0, len = 0;
+#if __riscv_xlen == 32
+	bool virt = (regs->mstatusH & MSTATUSH_MPV) ? TRUE : FALSE;
+#else
+	bool virt = (regs->mstatus & MSTATUS_MPV) ? TRUE : FALSE;
+#endif
+	ulong insn = get_insn(regs->mepc, virt, scratch, &uptrap);
 
 	if (uptrap.cause)
 		return sbi_trap_redirect(regs, scratch, regs->mepc,
@@ -129,9 +134,14 @@ int sbi_misaligned_store_handler(u32 hartid, ulong mcause,
 {
 	union reg_data val;
 	struct unpriv_trap uptrap;
-	ulong insn = get_insn(regs->mepc, scratch, &uptrap);
 	ulong addr = csr_read(CSR_MTVAL);
 	int i, len = 0;
+#if __riscv_xlen == 32
+	bool virt = (regs->mstatusH & MSTATUSH_MPV) ? TRUE : FALSE;
+#else
+	bool virt = (regs->mstatus & MSTATUS_MPV) ? TRUE : FALSE;
+#endif
+	ulong insn = get_insn(regs->mepc, virt, scratch, &uptrap);
 
 	if (uptrap.cause)
 		return sbi_trap_redirect(regs, scratch, regs->mepc,