riscv-sbi/exceptions: handle empty mtval register

JIRA: RTOS-859

riscv-sbi/core/exceptions.c

@@ -280,24 +280,31 @@ static void exceptions_illegalHandler(unsigned int n, exc_context_t *ctx)
 {
 	u64 insn = ctx->mtval;
 	u64 prevMode = (ctx->mstatus & MSTATUS_MPP) >> MSTATUS_MPP_SHIFT;
+	unsigned long unpriv_insn;
 
 	/* Check failing instruction */
 	if (((insn & 3) == 3) && (((insn & 0x7c) >> 2) == 0x1c)) {
 		/* Non-compressed, SYSTEM opcode */
 		exceptions_illSystem(n, ctx);
 	}
-	else if (insn == 0) {
-		/* RV implementation didn't write mtval.
-		 * TODO: get failing instruction through unprivileged access
-		 */
-		exceptions_defaultHandler(n, ctx);
-	}
 	else {
 		if (prevMode == PRV_M) {
 			/* Trapped on illegal instruction in M-mode */
 			exceptions_defaultHandler(n, ctx);
 		}
 		else {
+			if (insn == 0) {
+				/* mepc has virtual address, have to load using MPRV */
+				MPRV_LOAD(lhu, insn, ctx->mepc);
+				/* Check lowest to bits to determine if we are dealing with compressed insn
+				 * Noncompressed RV instructions have bits [1:0] = 11.
+				 */
+				if ((insn & 3) == 3) {
+					/* Noncompressed, load 2nd part */
+					MPRV_LOAD(lhu, unpriv_insn, ctx->mepc + 2);
+					insn = (unpriv_insn << 16) | insn;
+				}
+			}
 			exceptions_redirect(n, insn, ctx);
 		}
 	}

riscv-sbi/include/hart.h

@@ -32,6 +32,26 @@
 	do { \
 		__asm__ volatile ("wfi" ::: "memory"); \
 	} while (0)
+
+
+/* Load value using mstatus privilege modification
+ * This macro allows loads from virtual address in M-Mode.
+ */
+#define MPRV_LOAD(load_inst, reg, addr) \
+	__asm__ volatile ( \
+		/* Set MPRV bit */ \
+		"li a6, (1 << 17)\n\t" \
+		"csrs mstatus, a6\n\t" \
+		".option push\n\t" \
+		".option norvc\n\t" \
+		#load_inst " %0, (%1)\n\t" \
+		".option pop\n\t" /* Clear MPRV bit */ \
+		"csrc mstatus, a6" \
+		: "=r"(reg) \
+		: "r"(addr) \
+		: "a6", "memory" \
+	);
+
 /* clang-format on */