fix behavior of riscv_hart_mu_p to match TGC_D

gen_input/CoreDSL-Instruction-Set-Description

@@ -1 +1 @@
-Subproject commit 98cddb2999bfc05862a796b157500f7cd0f36ca4
+Subproject commit 8d9a0fb1493b762014c330c71ac8cef96753d302

incl/iss/arch/riscv_hart_mu_p.h

@@ -183,10 +183,10 @@ public:
     };
     using hart_state_type = hart_state<reg_t>;
 
-    constexpr reg_t get_irq_mask(size_t mode) {
+    constexpr reg_t get_irq_wrmask(size_t mode) {
         std::array<const reg_t, 4> m = {{
             0b000100010001, // U mode
-            0,              // S mode
+            0b001100110011, // S mode
             0,
             0b100110011001  // M mode
         }};
@@ -263,6 +263,9 @@ protected:
     virtual iss::status read_mem(phys_addr_t addr, unsigned length, uint8_t *const data);
     virtual iss::status write_mem(phys_addr_t addr, unsigned length, const uint8_t *const data);
 
+    iss::status read_clic(uint64_t addr, unsigned length, uint8_t *const data);
+    iss::status write_clic(uint64_t addr, unsigned length, const uint8_t *const data);
+
     virtual iss::status read_csr(unsigned addr, reg_t &val);
     virtual iss::status write_csr(unsigned addr, reg_t val);
 
@@ -287,10 +290,23 @@ protected:
     std::unordered_map<uint64_t, uint8_t> atomic_reservation;
     std::unordered_map<unsigned, rd_csr_f> csr_rd_cb;
     std::unordered_map<unsigned, wr_csr_f> csr_wr_cb;
+    uint8_t clic_cfg_reg{0};
+    uint32_t clic_info_reg{0};
+    std::array<uint32_t, 32> clic_inttrig_reg;
+    union clic_int_reg_t {
+        struct{
+            uint8_t ip;
+            uint8_t ie;
+            uint8_t attr;
+            uint8_t ctl;
+        };
+        uint32_t raw;
+    };
+    std::vector<clic_int_reg_t> clic_int_reg;
 
 private:
-    iss::status read_reg(unsigned addr, reg_t &val);
+    iss::status read_csr_reg(unsigned addr, reg_t &val);
-    iss::status write_reg(unsigned addr, reg_t val);
+    iss::status write_csr_reg(unsigned addr, reg_t val);
     iss::status read_null(unsigned addr, reg_t &val);
     iss::status write_null(unsigned addr, reg_t val){return iss::status::Ok;}
     iss::status read_cycle(unsigned addr, reg_t &val);
@@ -306,8 +322,11 @@ private:
     iss::status write_ie(unsigned addr, reg_t val);
     iss::status read_ip(unsigned addr, reg_t &val);
     iss::status write_ip(unsigned addr, reg_t val);
+    iss::status write_ideleg(unsigned addr, reg_t val);
+    iss::status write_edeleg(unsigned addr, reg_t val);
     iss::status read_hartid(unsigned addr, reg_t &val);
     iss::status write_epc(unsigned addr, reg_t val);
+    iss::status write_intthresh(unsigned addr, reg_t val);
 
     reg_t mhartid_reg{0x0};
     std::function<iss::status(phys_addr_t, unsigned, uint8_t *const)>mem_read_cb;
@@ -316,6 +335,10 @@ private:
 protected:
     void check_interrupt();
     bool pmp_check(const access_type type, const uint64_t addr, const unsigned len);
+    uint64_t clic_base_addr{0};
+    unsigned clic_num_irq{0};
+    unsigned clic_num_trigger{0};
+    unsigned mcause_max_irq{16};
 };
 
 template <typename BASE, features_e FEAT>
@@ -332,22 +355,22 @@ riscv_hart_mu_p<BASE, FEAT>::riscv_hart_mu_p()
     uart_buf.str("");
     for (unsigned addr = mhpmcounter3; addr <= mhpmcounter31; ++addr){
         csr_rd_cb[addr] = &this_class::read_null;
-        csr_wr_cb[addr] = &this_class::write_reg;
+        csr_wr_cb[addr] = &this_class::write_csr_reg;
     }
     for (unsigned addr = mhpmcounter3h; addr <= mhpmcounter31h; ++addr){
         csr_rd_cb[addr] = &this_class::read_null;
-        csr_wr_cb[addr] = &this_class::write_reg;
+        csr_wr_cb[addr] = &this_class::write_csr_reg;
     }
     for (unsigned addr = mhpmevent3; addr <= mhpmevent31; ++addr){
         csr_rd_cb[addr] = &this_class::read_null;
-        csr_wr_cb[addr] = &this_class::write_reg;
+        csr_wr_cb[addr] = &this_class::write_csr_reg;
     }
     for (unsigned addr = hpmcounter3; addr <= hpmcounter31; ++addr){
         csr_rd_cb[addr] = &this_class::read_null;
     }
     for (unsigned addr = hpmcounter3h; addr <= hpmcounter31h; ++addr){
         csr_rd_cb[addr] = &this_class::read_null;
-        //csr_wr_cb[addr] = &this_class::write_reg;
+        //csr_wr_cb[addr] = &this_class::write_csr_reg;
     }
     // common regs
     const std::array<unsigned, 14> addrs{{
@@ -356,8 +379,8 @@ riscv_hart_mu_p<BASE, FEAT>::riscv_hart_mu_p()
         uepc, utvec, uscratch, ucause, utval,
     }};
     for(auto addr: addrs) {
-        csr_rd_cb[addr] = &this_class::read_reg;
+        csr_rd_cb[addr] = &this_class::read_csr_reg;
-        csr_wr_cb[addr] = &this_class::write_reg;
+        csr_wr_cb[addr] = &this_class::write_csr_reg;
     }
     // special handling & overrides
     csr_rd_cb[time] = &this_class::read_time;
@@ -378,21 +401,12 @@ riscv_hart_mu_p<BASE, FEAT>::riscv_hart_mu_p()
     csr_rd_cb[mstatus] = &this_class::read_status;
     csr_wr_cb[mstatus] = &this_class::write_status;
     csr_wr_cb[mcause] = &this_class::write_cause;
-    csr_rd_cb[ustatus] = &this_class::read_status;
-    csr_wr_cb[ustatus] = &this_class::write_status;
-    csr_wr_cb[ucause] = &this_class::write_cause;
     csr_rd_cb[mtvec] = &this_class::read_tvec;
-    csr_rd_cb[utvec] = &this_class::read_tvec;
     csr_wr_cb[mepc] = &this_class::write_epc;
-    csr_wr_cb[uepc] = &this_class::write_epc;
     csr_rd_cb[mip] = &this_class::read_ip;
     csr_wr_cb[mip] = &this_class::write_ip;
-    csr_rd_cb[uip] = &this_class::read_ip;
-    csr_wr_cb[uip] = &this_class::write_ip;
     csr_rd_cb[mie] = &this_class::read_ie;
     csr_wr_cb[mie] = &this_class::write_ie;
-    csr_rd_cb[uie] = &this_class::read_ie;
-    csr_wr_cb[uie] = &this_class::write_ie;
     csr_rd_cb[mhartid] = &this_class::read_hartid;
     csr_rd_cb[mcounteren] = &this_class::read_null;
     csr_wr_cb[mcounteren] = &this_class::write_null;
@@ -403,35 +417,51 @@ riscv_hart_mu_p<BASE, FEAT>::riscv_hart_mu_p()
 
     if(FEAT & FEAT_PMP){
         for(size_t i=pmpaddr0; i<=pmpaddr15; ++i){
-            csr_rd_cb[i] = &this_class::read_reg;
+            csr_rd_cb[i] = &this_class::read_csr_reg;
-            csr_wr_cb[i] = &this_class::write_reg;
+            csr_wr_cb[i] = &this_class::write_csr_reg;
         }
         for(size_t i=pmpcfg0; i<=pmpcfg3; ++i){
-            csr_rd_cb[i] = &this_class::read_reg;
+            csr_rd_cb[i] = &this_class::read_csr_reg;
-            csr_wr_cb[i] = &this_class::write_reg;
+            csr_wr_cb[i] = &this_class::write_csr_reg;
         }
     }
     if(FEAT & FEAT_EXT_N){
-        csr_rd_cb[mideleg] = &this_class::read_reg;
+        csr_rd_cb[mideleg] = &this_class::read_csr_reg;
-        csr_wr_cb[mideleg] = &this_class::write_reg;
+        csr_wr_cb[mideleg] = &this_class::write_ideleg;
-        csr_rd_cb[medeleg] = &this_class::read_reg;
+        csr_rd_cb[medeleg] = &this_class::read_csr_reg;
-        csr_wr_cb[medeleg] = &this_class::write_reg;
+        csr_wr_cb[medeleg] = &this_class::write_edeleg;
+        csr_rd_cb[uie] = &this_class::read_ie;
+        csr_wr_cb[uie] = &this_class::write_ie;
+        csr_rd_cb[uip] = &this_class::read_ip;
+        csr_wr_cb[uip] = &this_class::write_ip;
+        csr_wr_cb[uepc] = &this_class::write_epc;
+        csr_rd_cb[ustatus] = &this_class::read_status;
+        csr_wr_cb[ustatus] = &this_class::write_status;
+        csr_wr_cb[ucause] = &this_class::write_cause;
+        csr_rd_cb[utvec] = &this_class::read_tvec;
     }
     if(FEAT & FEAT_CLIC) {
-        csr_rd_cb[mtvt] = &this_class::read_reg;
+        csr_rd_cb[mtvt] = &this_class::read_csr_reg;
-        csr_wr_cb[mtvt] = &this_class::write_reg;
+        csr_wr_cb[mtvt] = &this_class::write_csr_reg;
-        csr_rd_cb[mxnti] = &this_class::read_reg;
+        csr_rd_cb[mxnti] = &this_class::read_csr_reg;
-        csr_wr_cb[mxnti] = &this_class::write_reg;
+        csr_wr_cb[mxnti] = &this_class::write_csr_reg;
-        csr_rd_cb[mintstatus] = &this_class::read_reg;
+        csr_rd_cb[mintstatus] = &this_class::read_csr_reg;
-        csr_wr_cb[mintstatus] = &this_class::write_reg;
+        csr_wr_cb[mintstatus] = &this_class::write_csr_reg;
-        csr_rd_cb[mscratchcsw] = &this_class::read_reg;
+        csr_rd_cb[mscratchcsw] = &this_class::read_csr_reg;
-        csr_wr_cb[mscratchcsw] = &this_class::write_reg;
+        csr_wr_cb[mscratchcsw] = &this_class::write_csr_reg;
-        csr_rd_cb[mscratchcswl] = &this_class::read_reg;
+        csr_rd_cb[mscratchcswl] = &this_class::read_csr_reg;
-        csr_wr_cb[mscratchcswl] = &this_class::write_reg;
+        csr_wr_cb[mscratchcswl] = &this_class::write_csr_reg;
-        csr_rd_cb[mintthresh] = &this_class::read_reg;
+        csr_rd_cb[mintthresh] = &this_class::read_csr_reg;
-        csr_wr_cb[mintthresh] = &this_class::write_reg;
+        csr_wr_cb[mintthresh] = &this_class::write_intthresh;
-        csr_rd_cb[mclicbase] = &this_class::read_reg;
+        csr_rd_cb[mclicbase] = &this_class::read_csr_reg;
         csr_wr_cb[mclicbase] = &this_class::write_null;
+
+        clic_base_addr=0xC0000000;
+        clic_num_irq=16;
+        clic_int_reg.resize(clic_num_irq);
+        clic_cfg_reg=0x40;
+        clic_info_reg = (/*CLICINTCTLBITS*/ 4U<<21) + clic_num_irq;
+        mcause_max_irq=clic_num_irq+16;
     }
 }
 
@@ -563,9 +593,13 @@ iss::status riscv_hart_mu_p<BASE, FEAT>::read(const address_type type, const acc
                     fault_data=addr;
                     return iss::Err;
                 }
-                auto res = type==iss::address_type::PHYSICAL?
+                auto phys_addr = type==iss::address_type::PHYSICAL?phys_addr_t{access, space, addr}:BASE::v2p(iss::addr_t{access, type, space, addr});
-                        read_mem( BASE::v2p(phys_addr_t{access, space, addr}), length, data):
+                auto res = iss::Err;
-                        read_mem( BASE::v2p(iss::addr_t{access, type, space, addr}), length, data);
+                if((FEAT & FEAT_CLIC) && access != access_type::FETCH && phys_addr.val>=clic_base_addr && (phys_addr.val+length)<=(clic_base_addr+0x5000)){ //TODO: should be a constant
+                    res = read_clic(phys_addr.val, length, data);
+                } else {
+                    res = read_mem( phys_addr, length, data);
+                }
                 if (unlikely(res != iss::Ok)){
                     this->reg.trap_state = (1 << 31) | (5 << 16); // issue trap 5 (load access fault
                     fault_data=addr;
@@ -653,9 +687,9 @@ iss::status riscv_hart_mu_p<BASE, FEAT>::write(const address_type type, const ac
                     fault_data=addr;
                     return iss::Err;
                 }
-                auto res = type==iss::address_type::PHYSICAL?
+                auto phys_addr = type==iss::address_type::PHYSICAL?phys_addr_t{access, space, addr}:BASE::v2p(iss::addr_t{access, type, space, addr});
-                        write_mem(phys_addr_t{access, space, addr}, length, data):
+                auto res = ((FEAT & FEAT_CLIC) && phys_addr.val>=clic_base_addr && (phys_addr.val+length)<=(clic_base_addr+0x5000))? //TODO: should be a constant
-                        write_mem(BASE::v2p(iss::addr_t{access, type, space, addr}), length, data);
+                        write_clic(phys_addr.val, length, data) : write_mem( phys_addr, length, data);
                 if (unlikely(res != iss::Ok)) {
                     this->reg.trap_state = (1 << 31) | (7 << 16); // issue trap 7 (Store/AMO access fault)
                     fault_data=addr;
@@ -752,7 +786,7 @@ template <typename BASE, features_e FEAT> iss::status riscv_hart_mu_p<BASE, FEAT
     return (this->*(it->second))(addr, val);
 }
 
-template <typename BASE, features_e FEAT> iss::status riscv_hart_mu_p<BASE, FEAT>::read_reg(unsigned addr, reg_t &val) {
+template <typename BASE, features_e FEAT> iss::status riscv_hart_mu_p<BASE, FEAT>::read_csr_reg(unsigned addr, reg_t &val) {
     val = csr[addr];
     return iss::Ok;
 }
@@ -762,7 +796,7 @@ template <typename BASE, features_e FEAT> iss::status riscv_hart_mu_p<BASE, FEAT
     return iss::Ok;
 }
 
-template <typename BASE, features_e FEAT> iss::status riscv_hart_mu_p<BASE, FEAT>::write_reg(unsigned addr, reg_t val) {
+template <typename BASE, features_e FEAT> iss::status riscv_hart_mu_p<BASE, FEAT>::write_csr_reg(unsigned addr, reg_t val) {
     csr[addr] = val;
     return iss::Ok;
 }
@@ -849,13 +883,7 @@ template <typename BASE, features_e FEAT> iss::status riscv_hart_mu_p<BASE, FEAT
 }
 
 template <typename BASE, features_e FEAT> iss::status riscv_hart_mu_p<BASE, FEAT>::write_cause(unsigned addr, reg_t val) {
-    csr[addr] = val & ((1UL<<(traits<BASE>::XLEN-1))|0xf); //TODO: make exception code size configurable
+    csr[addr] = val & ((1UL<<(traits<BASE>::XLEN-1))|(mcause_max_irq-1)); //TODO: make exception code size configurable
-    return iss::Ok;
-}
-
-template <typename BASE, features_e FEAT> iss::status riscv_hart_mu_p<BASE, FEAT>::read_ie(unsigned addr, reg_t &val) {
-    val = csr[mie];
-    val &= csr[mideleg];
     return iss::Ok;
 }
 
@@ -864,34 +892,64 @@ template <typename BASE, features_e FEAT> iss::status riscv_hart_mu_p<BASE, FEAT
     return iss::Ok;
 }
 
+template <typename BASE, features_e FEAT> iss::status riscv_hart_mu_p<BASE, FEAT>::read_ie(unsigned addr, reg_t &val) {
+    auto mask = get_irq_wrmask((addr >> 8) & 0x3);
+    val = csr[mie] & mask;
+    if(this->reg.PRIV!=3)
+        val &= csr[mideleg];
+    return iss::Ok;
+}
+
 template <typename BASE, features_e FEAT> iss::status riscv_hart_mu_p<BASE, FEAT>::write_ie(unsigned addr, reg_t val) {
-    auto req_priv_lvl = (addr >> 8) & 0x3;
+    auto mask = get_irq_wrmask((addr >> 8) & 0x3);
-    auto mask = get_irq_mask(req_priv_lvl);
+    if(this->reg.PRIV==0)
+        mask&= ~(0xff<<4); // STIE and UTIE are read only in user and supervisor mode
     csr[mie] = (csr[mie] & ~mask) | (val & mask);
     check_interrupt();
     return iss::Ok;
 }
 
 template <typename BASE, features_e FEAT> iss::status riscv_hart_mu_p<BASE, FEAT>::read_ip(unsigned addr, reg_t &val) {
-    val = csr[mip];
+    auto mask = get_irq_wrmask((addr >> 8) & 0x3);
-    val &= csr[mideleg];
+    val = csr[mip] & mask;
+    if(this->reg.PRIV!=3)
+        val &= csr[mideleg];
     return iss::Ok;
 }
 
 template <typename BASE, features_e FEAT> iss::status riscv_hart_mu_p<BASE, FEAT>::write_ip(unsigned addr, reg_t val) {
-    auto req_priv_lvl = (addr >> 8) & 0x3;
+    auto mask = get_irq_wrmask((addr >> 8) & 0x3);
-    auto mask = get_irq_mask(req_priv_lvl);
+    mask &= ~(8 << 4); // MTIP is read only
-    mask &= ~(1 << 7); // MTIP is read only
+    if(this->reg.PRIV!=3)
+        mask &= ~(3 << 4); // STIP and UTIP are read only in user and supervisor mode
     csr[mip] = (csr[mip] & ~mask) | (val & mask);
     check_interrupt();
     return iss::Ok;
 }
 
+template <typename BASE, features_e FEAT> iss::status riscv_hart_mu_p<BASE, FEAT>::write_ideleg(unsigned addr, reg_t val) {
+    auto mask = 0b000100010001; // only U mode supported
+    csr[mideleg] = (csr[mideleg] & ~mask) | (val & mask);
+    return iss::Ok;
+}
+
+template <typename BASE, features_e FEAT> iss::status riscv_hart_mu_p<BASE, FEAT>::write_edeleg(unsigned addr, reg_t val) {
+    auto mask = 0xf7f7; // bit 11 (Env call) and 3 (break) are hardwired to 0
+    csr[medeleg] = (csr[medeleg] & ~mask) | (val & mask);
+    return iss::Ok;
+}
+
 template <typename BASE, features_e FEAT> iss::status riscv_hart_mu_p<BASE, FEAT>::write_epc(unsigned addr, reg_t val) {
     csr[addr] = val & get_pc_mask();
     return iss::Ok;
 }
 
+template<typename BASE, features_e FEAT>
+iss::status riscv_hart_mu_p<BASE, FEAT>::write_intthresh(unsigned addr, reg_t val) {
+    csr[addr]= val &0xff;
+    return iss::Ok;
+}
+
 template <typename BASE, features_e FEAT>
 iss::status riscv_hart_mu_p<BASE, FEAT>::read_mem(phys_addr_t paddr, unsigned length, uint8_t *const data) {
     if(mem_read_cb) return mem_read_cb(paddr, length, data);
@@ -995,6 +1053,84 @@ iss::status riscv_hart_mu_p<BASE, FEAT>::write_mem(phys_addr_t paddr, unsigned l
     return iss::Ok;
 }
 
+void read_uint32(uint64_t offs, uint32_t& reg, uint8_t *const data, unsigned length) {
+    auto reg_ptr = reinterpret_cast<uint8_t*>(&reg);
+    switch (offs & 0x3) {
+    case 0:
+        for (auto i = 0U; i < length; ++i)
+            *(data + i) = *(reg_ptr + i);
+    break;
+    case 1:
+        for (auto i = 0U; i < length; ++i)
+            *(data + i) = *(reg_ptr + 1 + i);
+    break;
+    case 2:
+        for (auto i = 0U; i < length; ++i)
+            *(data + i) = *(reg_ptr + 2 + i);
+    break;
+    case 3:
+        *data = *(reg_ptr + 3);
+    break;
+    }
+}
+
+void write_uint32(uint64_t offs, uint32_t& reg, const uint8_t *const data, unsigned length) {
+    auto reg_ptr = reinterpret_cast<uint8_t*>(&reg);
+    switch (offs & 0x3) {
+    case 0:
+        for (auto i = 0U; i < length; ++i)
+            *(reg_ptr + i) = *(data + i);
+    break;
+    case 1:
+        for (auto i = 0U; i < length; ++i)
+            *(reg_ptr + 1 + i) = *(data + i);
+    break;
+    case 2:
+        for (auto i = 0U; i < length; ++i)
+            *(reg_ptr + 2 + i) = *(data + i);
+    break;
+    case 3:
+        *(reg_ptr + 3) = *data ;
+    break;
+    }
+}
+
+template<typename BASE, features_e FEAT>
+iss::status riscv_hart_mu_p<BASE, FEAT>::read_clic(uint64_t addr, unsigned length, uint8_t *const data) {
+    if(addr==clic_base_addr) { // cliccfg
+        *data=clic_cfg_reg;
+        for(auto i=1; i<length; ++i) *(data+i)=0;
+    } else if(addr>=(clic_base_addr+4) && (addr+length)<=(clic_base_addr+4)){ // clicinfo
+        read_uint32(addr, clic_info_reg, data, length);
+    } else if(addr>=(clic_base_addr+0x40) && (addr+length)<=(clic_base_addr+0x40+clic_num_trigger*4)){ // clicinttrig
+        auto offset = ((addr&0x7fff)-0x40)/4;
+        read_uint32(addr, clic_inttrig_reg[offset], data, length);
+    } else if(addr>=(clic_base_addr+0x1000) && (addr+length)<=(clic_base_addr+clic_num_irq*4)){ // clicintip/clicintie/clicintattr/clicintctl
+        auto offset = ((addr&0x7fff)-0x1000)/4;
+        read_uint32(addr, clic_int_reg[offset].raw, data, length);
+    } else {
+        for(auto i = 0U; i<length; ++i) *(data+i)=0;
+    }
+    return iss::Ok;
+}
+
+template<typename BASE, features_e FEAT>
+iss::status riscv_hart_mu_p<BASE, FEAT>::write_clic(uint64_t addr, unsigned length, const uint8_t *const data) {
+    if(addr==clic_base_addr) { // cliccfg
+        clic_cfg_reg = *data;
+        clic_cfg_reg&= 0x7f;
+    } else if(addr>=(clic_base_addr+4) && (addr+length)<=(clic_base_addr+4)){ // clicinfo
+        write_uint32(addr, clic_info_reg, data, length);
+    } else if(addr>=(clic_base_addr+0x40) && (addr+length)<=(clic_base_addr+0xC0)){ // clicinttrig
+        auto offset = ((addr&0x7fff)-0x40)/4;
+        write_uint32(addr, clic_inttrig_reg[offset], data, length);
+    } else if(addr>=(clic_base_addr+0x1000) && (addr+length)<=(clic_base_addr+clic_num_irq*4)){ // clicintip/clicintie/clicintattr/clicintctl
+        auto offset = ((addr&0x7fff)-0x1000)/4;
+        write_uint32(addr, clic_int_reg[offset].raw, data, length);
+    }
+    return iss::Ok;
+}
+
 template <typename BASE, features_e FEAT> inline void riscv_hart_mu_p<BASE, FEAT>::reset(uint64_t address) {
     BASE::reset(address);
     state.mstatus = hart_state_type::mstatus_reset_val;
@@ -1009,7 +1145,7 @@ template <typename BASE, features_e FEAT> void riscv_hart_mu_p<BASE, FEAT>::chec
     auto ena_irq = csr[mip] & csr[mie];
 
     bool mie = state.mstatus.MIE;
-    auto m_enabled = this->reg.PRIV < PRIV_M || (this->reg.PRIV == PRIV_M && mie);
+    auto m_enabled = this->reg.PRIV < PRIV_M ||  mie;
     auto enabled_interrupts = m_enabled ? ena_irq & ~ideleg : 0;
 
     if (enabled_interrupts != 0) {
@@ -1078,7 +1214,7 @@ template <typename BASE, features_e FEAT> uint64_t riscv_hart_mu_p<BASE, FEAT>::
     auto ivec = csr[utvec | (new_priv << 8)];
     // calculate addr// set NEXT_PC to trap addressess to jump to based on MODE
     // bits in mtvec
-    this->reg.NEXT_PC = ivec & ~0x1UL;
+    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 = new_priv;