diff --git a/incl/iss/arch/riscv_hart_m_p.h b/incl/iss/arch/riscv_hart_m_p.h
index ace8d1f..181dd18 100644
--- a/incl/iss/arch/riscv_hart_m_p.h
+++ b/incl/iss/arch/riscv_hart_m_p.h
@@ -747,8 +747,7 @@ template <typename BASE> iss::status riscv_hart_m_p<BASE>::write_ip(unsigned add
 }
 
 template <typename BASE> iss::status riscv_hart_m_p<BASE>::write_mepc(unsigned addr, reg_t val) {
-    auto mask = get_pc_mask();
-    csr[addr] = val;//(csr[addr] & ~mask) | (val & mask);
+    csr[addr] = val & get_pc_mask();
     return iss::Ok;
 }
 
@@ -891,11 +890,11 @@ template <typename BASE> uint64_t riscv_hart_m_p<BASE>::enter_trap(uint64_t flag
     // calculate effective privilege level
     if (trap_id == 0) { // exception
         // store ret addr in xepc register
-        csr[mepc] = static_cast<reg_t>(addr); // store actual address instruction of exception
+        csr[mepc] = static_cast<reg_t>(addr) & get_pc_mask(); // store actual address instruction of exception
         csr[mtval] = cause==2?((instr & 0x3)==3?instr:instr&0xffff):fault_data;
         fault_data = 0;
     } else {
-        csr[mepc] = this->reg.NEXT_PC; // store next address if interrupt
+        csr[mepc] = this->reg.NEXT_PC & get_pc_mask(); // store next address if interrupt
         this->reg.pending_trap = 0;
     }
     csr[mcause] = (trap_id << 31) + cause;