diff --git a/include/sbi/riscv_encoding.h b/include/sbi/riscv_encoding.h
index 1adc0c5..381f305 100644
--- a/include/sbi/riscv_encoding.h
+++ b/include/sbi/riscv_encoding.h
@@ -19,12 +19,15 @@
 #define MSTATUS_HIE			0x00000004
 #define MSTATUS_MIE			0x00000008
 #define MSTATUS_UPIE			0x00000010
-#define MSTATUS_SPIE			0x00000020
+#define MSTATUS_SPIE_SHIFT		5
+#define MSTATUS_SPIE			(1UL << MSTATUS_SPIE_SHIFT)
 #define MSTATUS_HPIE			0x00000040
 #define MSTATUS_MPIE			0x00000080
-#define MSTATUS_SPP			0x00000100
+#define MSTATUS_SPP_SHIFT		8
+#define MSTATUS_SPP			(1UL << MSTATUS_SPP_SHIFT)
 #define MSTATUS_HPP			0x00000600
-#define MSTATUS_MPP			0x00001800
+#define MSTATUS_MPP_SHIFT		11
+#define MSTATUS_MPP			(3UL << MSTATUS_MPP_SHIFT)
 #define MSTATUS_FS			0x00006000
 #define MSTATUS_XS			0x00018000
 #define MSTATUS_MPRV			0x00020000
diff --git a/include/sbi/sbi_trap.h b/include/sbi/sbi_trap.h
index a6b22e4..4e5a054 100644
--- a/include/sbi/sbi_trap.h
+++ b/include/sbi/sbi_trap.h
@@ -51,6 +51,10 @@ struct sbi_trap_regs {
 
 struct sbi_scratch;
 
+int sbi_trap_redirect(struct sbi_trap_regs *regs,
+		      struct sbi_scratch *scratch,
+		      ulong epc, ulong cause, ulong tval);
+
 void sbi_trap_handler(struct sbi_trap_regs *regs,
 		      struct sbi_scratch *scratch);
 
diff --git a/lib/sbi_illegal_insn.c b/lib/sbi_illegal_insn.c
index 9383df4..1bdc759 100644
--- a/lib/sbi_illegal_insn.c
+++ b/lib/sbi_illegal_insn.c
@@ -26,8 +26,7 @@ static int truly_illegal_insn(ulong insn,
 			      struct sbi_trap_regs *regs,
 			      struct sbi_scratch *scratch)
 {
-	/* For now, always fails */
-	return SBI_ENOTSUPP;
+	return sbi_trap_redirect(regs, scratch, regs->mepc, mcause, insn);
 }
 
 static int system_opcode_insn(ulong insn,
@@ -132,7 +131,8 @@ int sbi_illegal_insn_handler(u32 hartid, ulong mcause,
 			insn = get_insn(regs->mepc, &mstatus);
 		}
 		if ((insn & 3) != 3)
-			return SBI_ENOTSUPP;
+			return truly_illegal_insn(insn, hartid, mcause,
+						  regs, scratch);
 	}
 
 	return illegal_insn_table[(insn & 0x7c) >> 2](insn, hartid, mcause,
diff --git a/lib/sbi_trap.c b/lib/sbi_trap.c
index ab78b57..0e731e4 100644
--- a/lib/sbi_trap.c
+++ b/lib/sbi_trap.c
@@ -66,6 +66,49 @@ static void __attribute__((noreturn)) sbi_trap_error(const char *msg,
 	sbi_hart_hang();
 }
 
+int sbi_trap_redirect(struct sbi_trap_regs *regs,
+		      struct sbi_scratch *scratch,
+		      ulong epc, ulong cause, ulong tval)
+{
+	ulong new_mstatus, prev_mode;
+
+	/* Sanity check on previous mode */
+	prev_mode = (regs->mstatus & MSTATUS_MPP) >> MSTATUS_MPP_SHIFT;
+	if (prev_mode != PRV_S && prev_mode != PRV_U)
+		return SBI_ENOTSUPP;
+
+	/* Update S-mode exception info */
+	csr_write(stval, tval);
+	csr_write(sepc, epc);
+	csr_write(scause, cause);
+
+	/* Set MEPC to S-mode exception vector base */
+	regs->mepc = csr_read(stvec);
+
+	/* Initial value of new MSTATUS */
+	new_mstatus = regs->mstatus;
+
+	/* Clear MPP, SPP, SPIE, and SIE */
+	new_mstatus &= ~(MSTATUS_MPP |
+			 MSTATUS_SPP | MSTATUS_SPIE | MSTATUS_SIE);
+
+	/* Set SPP */
+	if (prev_mode == PRV_S)
+		new_mstatus |= (1UL << MSTATUS_SPP_SHIFT);
+
+	/* Set SPIE */
+	if (regs->mstatus & MSTATUS_SIE)
+		new_mstatus |= (1UL << MSTATUS_SPIE_SHIFT);
+
+	/* Set MPP */
+	new_mstatus |= (PRV_S << MSTATUS_MPP_SHIFT);
+
+	/* Set new value in MSTATUS */
+	regs->mstatus = new_mstatus;
+
+	return 0;
+}
+
 void sbi_trap_handler(struct sbi_trap_regs *regs,
 		      struct sbi_scratch *scratch)
 {