diff --git a/incl/iss/arch/riscv_hart_m_p.h b/incl/iss/arch/riscv_hart_m_p.h
index 4a7046a..f9b4784 100644
--- a/incl/iss/arch/riscv_hart_m_p.h
+++ b/incl/iss/arch/riscv_hart_m_p.h
@@ -1015,7 +1015,7 @@ template <typename BASE, features_e FEAT> uint64_t riscv_hart_m_p<BASE, FEAT>::e
             //TODO: implement debug mode behavior
             // csr[dpc] = addr;
             // csr[dcsr] = (csr[dcsr] & ~0x1c3) | (1<<6) | PRIV_M; //FIXME: cause should not be 4 (stepi)
-            csr[mtval] = 0;
+            csr[mtval] = addr;
             break;
         case 4:
         case 6:
diff --git a/incl/iss/arch/riscv_hart_msu_vp.h b/incl/iss/arch/riscv_hart_msu_vp.h
index aa88e6d..efbe605 100644
--- a/incl/iss/arch/riscv_hart_msu_vp.h
+++ b/incl/iss/arch/riscv_hart_msu_vp.h
@@ -1276,7 +1276,27 @@ template <typename BASE> uint64_t riscv_hart_msu_vp<BASE>::enter_trap(uint64_t f
          * access, or page-fault exception occurs, mtval is written with the
          * faulting effective address.
          */
-        csr[utval | (new_priv << 8)] = cause==2?((instr & 0x3)==3?instr:instr&0xffff):fault_data;
+        switch(cause){
+        case 0:
+            csr[utval | (new_priv << 8)] = static_cast<reg_t>(addr);
+            break;
+        case 2:
+            csr[utval | (new_priv << 8)] = (instr & 0x3)==3?instr:instr&0xffff;
+            break;
+        case 3:
+            //TODO: implement debug mode behavior
+            // csr[dpc] = addr;
+            // csr[dcsr] = (csr[dcsr] & ~0x1c3) | (1<<6) | PRIV_M; //FIXME: cause should not be 4 (stepi)
+            csr[utval | (new_priv << 8)] = addr;
+            break;
+        case 4:
+        case 6:
+        case 7:
+            csr[utval | (new_priv << 8)] = fault_data;
+            break;
+        default:
+            csr[utval | (new_priv << 8)] = 0;
+        }
         fault_data = 0;
     } else {
         if (cur_priv != PRIV_M && ((csr[mideleg] >> cause) & 0x1) != 0)
diff --git a/incl/iss/arch/riscv_hart_mu_p.h b/incl/iss/arch/riscv_hart_mu_p.h
index 2444cd7..f48a28e 100644
--- a/incl/iss/arch/riscv_hart_mu_p.h
+++ b/incl/iss/arch/riscv_hart_mu_p.h
@@ -1294,7 +1294,7 @@ template <typename BASE, features_e FEAT> uint64_t riscv_hart_mu_p<BASE, FEAT>::
             //TODO: implement debug mode behavior
             // csr[dpc] = addr;
             // csr[dcsr] = (csr[dcsr] & ~0x1c3) | (1<<6) | PRIV_M; //FIXME: cause should not be 4 (stepi)
-            csr[utval | (new_priv << 8)] = 0;
+            csr[utval | (new_priv << 8)] = addr;
             break;
         case 4:
         case 6: