diff --git a/incl/iss/arch/riscv_hart_common.h b/incl/iss/arch/riscv_hart_common.h
index f8584be..345fbeb 100644
--- a/incl/iss/arch/riscv_hart_common.h
+++ b/incl/iss/arch/riscv_hart_common.h
@@ -188,7 +188,7 @@ enum {
 
 template <typename T> inline bool PTE_TABLE(T PTE) { return (((PTE) & (PTE_V | PTE_R | PTE_W | PTE_X)) == PTE_V); }
 
-enum { PRIV_U = 0, PRIV_S = 1, PRIV_M = 3 };
+enum { PRIV_U = 0, PRIV_S = 1, PRIV_M = 3, PRIV_D = 4};
 
 enum {
     ISA_A = 1,
diff --git a/incl/iss/arch/riscv_hart_m_p.h b/incl/iss/arch/riscv_hart_m_p.h
index f67ad05..bff635d 100644
--- a/incl/iss/arch/riscv_hart_m_p.h
+++ b/incl/iss/arch/riscv_hart_m_p.h
@@ -1127,6 +1127,7 @@ template <typename BASE, features_e FEAT> uint64_t riscv_hart_m_p<BASE, FEAT>::e
     auto trap_id = bit_sub<0, 16>(flags);
     auto cause = bit_sub<16, 15>(flags);
     // calculate effective privilege level
+    unsigned new_priv = PRIV_M;
     if (trap_id == 0) { // exception
         if (cause == 11) cause = 0x8 + PRIV_M; // adjust environment call cause
         // store ret addr in xepc register
@@ -1146,10 +1147,13 @@ template <typename BASE, features_e FEAT> uint64_t riscv_hart_m_p<BASE, FEAT>::e
             csr[mtval] = (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[mtval] = addr;
+            if((FEAT & FEAT_DEBUG) && (csr[dcsr] & 0x8000)) {
+                this->reg.DPC = addr;
+                csr[dcsr] = (csr[dcsr] & ~0x1c3) | (1<<6) | PRIV_M; //FIXME: cause should not be 4 (stepi)
+                new_priv = this->reg.PRIV | PRIV_D;
+            } else {
+                csr[mtval] = addr;
+            }
             break;
         case 4:
         case 6:
@@ -1182,7 +1186,7 @@ template <typename BASE, features_e FEAT> uint64_t riscv_hart_m_p<BASE, FEAT>::e
     this->reg.NEXT_PC = ivec & ~0x3UL;
     if ((ivec & 0x1) == 1 && trap_id != 0) this->reg.NEXT_PC += 4 * cause;
     // reset trap state
-    this->reg.PRIV = PRIV_M;
+    this->reg.PRIV = new_priv;
     this->reg.trap_state = 0;
     std::array<char, 32> buffer;
 #if defined(_MSC_VER)
diff --git a/incl/iss/arch/riscv_hart_mu_p.h b/incl/iss/arch/riscv_hart_mu_p.h
index 32f951d..bd50342 100644
--- a/incl/iss/arch/riscv_hart_mu_p.h
+++ b/incl/iss/arch/riscv_hart_mu_p.h
@@ -1296,7 +1296,7 @@ template <typename BASE, features_e FEAT> uint64_t riscv_hart_mu_p<BASE, FEAT>::
     auto cause = bit_sub<16, 15>(flags);
     if (trap_id == 0 && cause == 11) cause = 0x8 + this->reg.PRIV; // adjust environment call cause
     // calculate effective privilege level
-    auto new_priv = PRIV_M;
+    unsigned new_priv = PRIV_M;
     if (trap_id == 0) { // exception
         if (this->reg.PRIV != PRIV_M && ((csr[medeleg] >> cause) & 0x1) != 0)
             new_priv = PRIV_U;
@@ -1317,10 +1317,13 @@ template <typename BASE, features_e FEAT> uint64_t riscv_hart_mu_p<BASE, FEAT>::
             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;
+            if((FEAT & FEAT_DEBUG) && (csr[dcsr] & 0x8000)) {
+                this->reg.DPC = addr;
+                csr[dcsr] = (csr[dcsr] & ~0x1c3) | (1<<6) | PRIV_M; //FIXME: cause should not be 4 (stepi)
+                new_priv = this->reg.PRIV | PRIV_D;
+            } else {
+                csr[utval | (new_priv << 8)] = addr;
+            }
             break;
         case 4:
         case 6: