diff --git a/include/sbi/riscv_encoding.h b/include/sbi/riscv_encoding.h
index 6dd0d300..1cf624bf 100644
--- a/include/sbi/riscv_encoding.h
+++ b/include/sbi/riscv_encoding.h
@@ -37,12 +37,10 @@
 #define MSTATUS_SXL			_ULL(0x0000000C00000000)
 #define MSTATUS_SBE			_ULL(0x0000001000000000)
 #define MSTATUS_MBE			_ULL(0x0000002000000000)
-#define MSTATUS_MTL			_ULL(0x0000004000000000)
 #define MSTATUS_MPV			_ULL(0x0000008000000000)
 #else
 #define MSTATUSH_SBE			_UL(0x00000010)
 #define MSTATUSH_MBE			_UL(0x00000020)
-#define MSTATUSH_MTL			_UL(0x00000040)
 #define MSTATUSH_MPV			_UL(0x00000080)
 #endif
 #define MSTATUS32_SD			_UL(0x80000000)
@@ -66,7 +64,6 @@
 #define HSTATUS_SP2V			_UL(0x00000200)
 #define HSTATUS_SP2P			_UL(0x00000100)
 #define HSTATUS_SPV			_UL(0x00000080)
-#define HSTATUS_STL			_UL(0x00000040)
 #define HSTATUS_SPRV			_UL(0x00000001)
 
 #define IRQ_S_SOFT			1
diff --git a/include/sbi/sbi_illegal_insn.h b/include/sbi/sbi_illegal_insn.h
index 2dc09888..1e1f74a1 100644
--- a/include/sbi/sbi_illegal_insn.h
+++ b/include/sbi/sbi_illegal_insn.h
@@ -15,7 +15,7 @@
 struct sbi_trap_regs;
 struct sbi_scratch;
 
-int sbi_illegal_insn_handler(u32 hartid, ulong mcause,
+int sbi_illegal_insn_handler(u32 hartid, ulong mcause, ulong insn,
 			     struct sbi_trap_regs *regs,
 			     struct sbi_scratch *scratch);
 
diff --git a/include/sbi/sbi_misaligned_ldst.h b/include/sbi/sbi_misaligned_ldst.h
index c631f763..d95c8455 100644
--- a/include/sbi/sbi_misaligned_ldst.h
+++ b/include/sbi/sbi_misaligned_ldst.h
@@ -16,10 +16,12 @@ struct sbi_trap_regs;
 struct sbi_scratch;
 
 int sbi_misaligned_load_handler(u32 hartid, ulong mcause,
+				ulong addr, ulong tval2, ulong tinst,
 				struct sbi_trap_regs *regs,
 				struct sbi_scratch *scratch);
 
 int sbi_misaligned_store_handler(u32 hartid, ulong mcause,
+				 ulong addr, ulong tval2, ulong tinst,
 				 struct sbi_trap_regs *regs,
 				 struct sbi_scratch *scratch);
 
diff --git a/lib/sbi/sbi_hart.c b/lib/sbi/sbi_hart.c
index 493e7fcb..a704c60c 100644
--- a/lib/sbi/sbi_hart.c
+++ b/lib/sbi/sbi_hart.c
@@ -277,7 +277,6 @@ sbi_hart_switch_mode(unsigned long arg0, unsigned long arg1,
 #if __riscv_xlen == 32
 	if (misa_extension('H')) {
 		valH = csr_read(CSR_MSTATUSH);
-		valH = INSERT_FIELD(valH, MSTATUSH_MTL, 0);
 		if (next_virt)
 			valH = INSERT_FIELD(valH, MSTATUSH_MPV, 1);
 		else
@@ -286,7 +285,6 @@ sbi_hart_switch_mode(unsigned long arg0, unsigned long arg1,
 	}
 #else
 	if (misa_extension('H')) {
-		val = INSERT_FIELD(val, MSTATUS_MTL, 0);
 		if (next_virt)
 			val = INSERT_FIELD(val, MSTATUS_MPV, 1);
 		else
diff --git a/lib/sbi/sbi_illegal_insn.c b/lib/sbi/sbi_illegal_insn.c
index 021ae37c..e3c7809a 100644
--- a/lib/sbi/sbi_illegal_insn.c
+++ b/lib/sbi/sbi_illegal_insn.c
@@ -29,6 +29,8 @@ static int truly_illegal_insn(ulong insn, u32 hartid, ulong mcause,
 	trap.epc = regs->mepc;
 	trap.cause = mcause;
 	trap.tval = insn;
+	trap.tval2 = 0;
+	trap.tinst = 0;
 
 	return sbi_trap_redirect(regs, &trap, scratch);
 }
@@ -131,11 +133,10 @@ static illegal_insn_func illegal_insn_table[32] = {
 	truly_illegal_insn  /* 31 */
 };
 
-int sbi_illegal_insn_handler(u32 hartid, ulong mcause,
+int sbi_illegal_insn_handler(u32 hartid, ulong mcause, ulong insn,
 			     struct sbi_trap_regs *regs,
 			     struct sbi_scratch *scratch)
 {
-	ulong insn = csr_read(CSR_MTVAL);
 	struct sbi_trap_info uptrap;
 
 	if (unlikely((insn & 3) != 3)) {
diff --git a/lib/sbi/sbi_misaligned_ldst.c b/lib/sbi/sbi_misaligned_ldst.c
index 57c7af40..314b2f31 100644
--- a/lib/sbi/sbi_misaligned_ldst.c
+++ b/lib/sbi/sbi_misaligned_ldst.c
@@ -22,12 +22,12 @@ union reg_data {
 };
 
 int sbi_misaligned_load_handler(u32 hartid, ulong mcause,
+				ulong addr, ulong tval2, ulong tinst,
 				struct sbi_trap_regs *regs,
 				struct sbi_scratch *scratch)
 {
 	union reg_data val;
 	struct sbi_trap_info uptrap;
-	ulong addr = csr_read(CSR_MTVAL);
 	int i, fp = 0, shift = 0, len = 0;
 	ulong insn = sbi_get_insn(regs->mepc, scratch, &uptrap);
 
@@ -101,6 +101,8 @@ int sbi_misaligned_load_handler(u32 hartid, ulong mcause,
 		uptrap.epc = regs->mepc;
 		uptrap.cause = mcause;
 		uptrap.tval = addr;
+		uptrap.tval2 = tval2;
+		uptrap.tinst = tinst;
 		return sbi_trap_redirect(regs, &uptrap, scratch);
 	}
 
@@ -129,12 +131,12 @@ int sbi_misaligned_load_handler(u32 hartid, ulong mcause,
 }
 
 int sbi_misaligned_store_handler(u32 hartid, ulong mcause,
+				 ulong addr, ulong tval2, ulong tinst,
 				 struct sbi_trap_regs *regs,
 				 struct sbi_scratch *scratch)
 {
 	union reg_data val;
 	struct sbi_trap_info uptrap;
-	ulong addr = csr_read(CSR_MTVAL);
 	int i, len = 0;
 	ulong insn = sbi_get_insn(regs->mepc, scratch, &uptrap);
 
@@ -199,6 +201,8 @@ int sbi_misaligned_store_handler(u32 hartid, ulong mcause,
 		uptrap.epc = regs->mepc;
 		uptrap.cause = mcause;
 		uptrap.tval = addr;
+		uptrap.tval2 = tval2;
+		uptrap.tinst = tinst;
 		return sbi_trap_redirect(regs, &uptrap, scratch);
 	}
 
diff --git a/lib/sbi/sbi_trap.c b/lib/sbi/sbi_trap.c
index 7025ad98..d5ab24b1 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);
 	}
 }