diff --git a/include/isa.h b/include/isa.h
index 4b3409bfd..5dc8e6781 100644
--- a/include/isa.h
+++ b/include/isa.h
@@ -88,5 +88,10 @@ void isa_difftest_set_mhartid(int n);
 #endif
 void isa_update_mip(unsigned lcofip);
 void isa_update_mhpmcounter_overflow(uint64_t mhpmeventOverflowVec);
+#ifdef CONFIG_RV_IMSIC
+void isa_update_mtopi();
+void isa_update_stopi();
+void isa_update_vstopi();
+#endif
 
 #endif
diff --git a/src/cpu/cpu-exec.c b/src/cpu/cpu-exec.c
index 159e0fe8c..c13352ab7 100644
--- a/src/cpu/cpu-exec.c
+++ b/src/cpu/cpu-exec.c
@@ -27,6 +27,7 @@
 #include <generated/autoconf.h>
 #include <profiling/profiling_control.h>
 #include "../local-include/trigger.h"
+#include "../local-include/aia.h"
 
 /* The assembly code of instructions executed is only output to the screen
  * when the number of instructions executed is less than this value.
diff --git a/src/cpu/difftest/ref.c b/src/cpu/difftest/ref.c
index 82d807a58..0ddd7a3a9 100644
--- a/src/cpu/difftest/ref.c
+++ b/src/cpu/difftest/ref.c
@@ -258,6 +258,11 @@ void difftest_raise_mhpmevent_overflow(uint64_t mhpmeventOverflowVec) {
 void difftest_non_reg_interrupt_pending(void *nonRegInterruptPending) {
   memcpy(&cpu.non_reg_interrupt_pending, nonRegInterruptPending, sizeof(struct NonRegInterruptPending));
   isa_update_mip(cpu.non_reg_interrupt_pending.lcofi_req);
+#ifdef CONFIG_RV_IMSIC
+  isa_update_mtopi();
+  isa_update_stopi();
+  isa_update_vstopi();
+#endif
 }
 
 #ifdef CONFIG_DIFFTEST_STORE_COMMIT
@@ -268,6 +273,13 @@ void difftest_get_store_event_other_info(void *info) {
 
 
 
+void difftest_aia_xtopei(void *xtopei) {
+#ifdef CONFIG_RV_IMSIC
+  memcpy(&cpu.xtopei, xtopei, sizeof(struct Xtopei));
+  isa_update_vstopi();
+#endif
+}
+
 void difftest_enable_debug() {
 #ifdef CONFIG_SHARE
   dynamic_config.debug_difftest = true;
diff --git a/src/isa/riscv64/difftest/ref.c b/src/isa/riscv64/difftest/ref.c
index 10dfd1d88..753455e06 100644
--- a/src/isa/riscv64/difftest/ref.c
+++ b/src/isa/riscv64/difftest/ref.c
@@ -435,3 +435,17 @@ void isa_update_mhpmcounter_overflow(uint64_t mhpmeventOverflowVec) {
   }
 #endif
 }
+
+#ifdef CONFIG_RV_IMSIC
+void isa_update_mtopi() {
+  update_mtopi();
+}
+
+void isa_update_stopi() {
+  update_stopi();
+}
+
+void isa_update_vstopi() {
+  update_vstopi();
+}
+#endif
\ No newline at end of file
diff --git a/src/isa/riscv64/include/isa-def.h b/src/isa/riscv64/include/isa-def.h
index 019431443..55144eba8 100644
--- a/src/isa/riscv64/include/isa-def.h
+++ b/src/isa/riscv64/include/isa-def.h
@@ -53,6 +53,12 @@ struct NonRegInterruptPending {
   bool lcofi_req;
 };
 
+struct Xtopei {
+  uint64_t mtopei;
+  uint64_t stopei;
+  uint64_t vstopei;
+};
+
 struct DebugInfo {
   uint64_t current_pc;
 };
@@ -73,6 +79,9 @@ struct MemEventQueryResult {
 #endif
 
 typedef struct TriggerModule TriggerModule;
+typedef struct IpriosModule IpriosModule;
+typedef struct IpriosSort IpriosSort;
+typedef struct HighestPrioIntr HighestPrioIntr;
 
 typedef struct {
   // Below will be synced by regcpy when run difftest, DO NOT TOUCH
@@ -174,6 +183,17 @@ typedef struct {
 
   trap_info_t trapInfo;
 
+#ifdef CONFIG_RV_IMSIC
+  struct Xtopei xtopei;
+  IpriosModule*  MIprios;
+  IpriosModule*  SIprios;
+  IpriosModule* VSIprios;
+  IpriosSort*    MIpriosSort;
+  IpriosSort*    SIpriosSort;
+  IpriosSort*   VSIpriosSort;
+  HighestPrioIntr* HighestPrioIntr;
+#endif
+
 } riscv64_CPU_state;
 
 // decode
diff --git a/src/isa/riscv64/init.c b/src/isa/riscv64/init.c
index ac1bd0813..a894ef028 100644
--- a/src/isa/riscv64/init.c
+++ b/src/isa/riscv64/init.c
@@ -32,6 +32,9 @@ void init_csr();
 #ifdef CONFIG_RV_SDTRIG
 void init_trigger();
 #endif // CONFIG_RV_SDTRIG
+#ifdef CONFIG_RV_IMSIC
+void init_iprio();
+#endif
 
 #if !defined(CONFIG_SHARE) || defined(CONFIG_LIGHTQS)
 void init_clint();
@@ -202,6 +205,10 @@ void init_isa() {
   init_trigger();
 #endif // CONFIG_RV_SDTRIG
 
+#ifdef CONFIG_RV_IMSIC
+  init_iprio();
+#endif
+
 #define MSTATEEN0_RESET  0xdc00000000000001ULL
 #define HSTATEEN0_RESET  0xdc00000000000001ULL
 #define SSTATEEN0_RESET  0x0000000000000001ULL
diff --git a/src/isa/riscv64/local-include/aia.h b/src/isa/riscv64/local-include/aia.h
new file mode 100644
index 000000000..26bec7f33
--- /dev/null
+++ b/src/isa/riscv64/local-include/aia.h
@@ -0,0 +1,53 @@
+#ifdef CONFIG_RV_IMSIC
+
+#ifndef __AIA_H__
+#define __AIA_H__
+
+#include "common.h"
+#include "csr.h"
+#include "intr.h"
+
+// 0,4,8 is reserved
+#define IPRIO_ENABLE_NUM 61
+#define IPRIO_NUM 8
+
+typedef struct {
+  word_t val;
+} Iprio;
+
+typedef struct IpriosModule {
+  Iprio iprios[IPRIO_NUM];
+} IpriosModule;
+
+typedef struct {
+  bool enable;
+  uint8_t priority;
+} IpriosEnable;
+
+typedef struct HighestPrioIntr {
+  uint8_t idx;
+  uint8_t priority;
+} HighestPrioIntr;
+
+typedef struct IpriosSort {
+  IpriosEnable ipriosEnable[IPRIO_ENABLE_NUM];
+} IpriosSort;
+
+typedef struct {
+  uint8_t hviprios[24];
+} Hviprios;
+
+bool iprio_is_zero(IpriosModule* iprios);
+void set_iprios_sort(uint64_t topi_gather, IpriosSort* iprios_sort, IpriosModule* iprios);
+void set_viprios_sort(uint64_t topi_gather);
+uint8_t high_iprio(IpriosSort* ipriosSort, uint8_t xei);
+uint8_t get_prio_idx_in_group(uint8_t irq);
+
+extern int interrupt_default_prio[IPRIO_ENABLE_NUM];
+
+bool no_mtopi();
+bool no_stopi();
+bool no_vstopi();
+
+#endif // __AIA_H__
+#endif // CONFIG_RV_IMSIC
\ No newline at end of file
diff --git a/src/isa/riscv64/local-include/csr.h b/src/isa/riscv64/local-include/csr.h
index cdf8ec9ca..4cf41c2d1 100644
--- a/src/isa/riscv64/local-include/csr.h
+++ b/src/isa/riscv64/local-include/csr.h
@@ -1249,6 +1249,14 @@ CSR_STRUCT_START(hvictl)
 CSR_STRUCT_END(hvictl)
 
 CSR_STRUCT_START(hviprio1)
+  uint64_t intr0  : 8; // [7 : 0] reserved
+  uint64_t ssi    : 8; // [16: 8]
+  uint64_t intr4  : 8; // [23:16] reserved
+  uint64_t sti    : 8; // [31:24]
+  uint64_t intr8  : 8; // [39:32] reserved
+  uint64_t coi    : 8; // [47:40]
+  uint64_t intr14 : 8; // [55:48]
+  uint64_t intr15 : 8; // [63:56]
 CSR_STRUCT_END(hviprio1)
 
 CSR_STRUCT_START(hviprio2)
@@ -1524,6 +1532,11 @@ void csr_prepare();
 
 word_t gen_status_sd(word_t status);
 word_t get_mip();
+#ifdef CONFIG_RV_IMSIC
+void update_mtopi();
+void update_stopi();
+void update_vstopi();
+#endif
 
 /** PMP **/
 uint8_t pmpcfg_from_index(int idx);
diff --git a/src/isa/riscv64/system/aia.c b/src/isa/riscv64/system/aia.c
new file mode 100644
index 000000000..6d4772677
--- /dev/null
+++ b/src/isa/riscv64/system/aia.c
@@ -0,0 +1,165 @@
+#include "isa.h"
+#include "cpu/cpu.h"
+#include "../local-include/aia.h"
+
+#ifdef CONFIG_RV_IMSIC
+
+int interrupt_default_prio[IPRIO_ENABLE_NUM] = {
+  // custom highest group
+  63, 31, 62,
+  61, 30, 60,
+  // local high group
+  47, 23, 46,
+  45, 22, 44,
+  43, 21, 42,
+  41, 20, 40,
+  // custom middle high group
+  59, 29, 58,
+  57, 28, 56,
+  // priv arch group
+  IRQ_MEIP, IRQ_MSIP, IRQ_MTIP,
+  IRQ_SEIP, IRQ_SSIP, IRQ_STIP,
+  IRQ_SGEI,
+  IRQ_VSEIP, IRQ_VSSIP, IRQ_VSTIP,
+  IRQ_LCOFI, 14, 15,
+  // custom middle low group
+  55, 27, 54,
+  53, 26, 52,
+  // local low group
+  39, 19, 38,
+  37, 18, 36,
+  35, 17, 34,
+  33, 16, 32,
+  // custom lowest group
+  51, 25, 50,
+  49, 24, 48
+};
+
+bool iprio_is_zero(IpriosModule* iprios) {
+  bool is_zero = true;
+  for (int i = 0; i < IPRIO_NUM; i++) {
+    is_zero &= iprios->iprios[i].val == 0;
+  }
+  return is_zero;
+}
+
+uint8_t get_prio_idx_in_group(uint8_t irq) {
+  uint8_t idx = 0;
+  for (int i = 0; i < IPRIO_ENABLE_NUM; i++) {
+    if (irq == interrupt_default_prio[i]) {
+      idx = i;
+    }
+  }
+  return idx;
+}
+
+bool intr_enable(uint64_t topi_gather, uint64_t idx) {
+  return (topi_gather >> interrupt_default_prio[idx]) & 0x1;
+}
+
+void set_iprios_sort(uint64_t topi_gather, IpriosSort* iprios_sort, IpriosModule* iprios) {
+  for (int i = 0; i < IPRIO_ENABLE_NUM; i++) {
+    if (intr_enable(topi_gather, i)) {
+      iprios_sort->ipriosEnable[i].enable = true;
+      iprios_sort->ipriosEnable[i].priority = (iprios->iprios[interrupt_default_prio[i]/8].val >> (8 * (interrupt_default_prio[i]%8))) & 0xff;
+    } else {
+      iprios_sort->ipriosEnable[i].enable = false;
+      iprios_sort->ipriosEnable[i].priority = 0;
+    }
+  }
+}
+
+void handle_irq_hviprio(uint8_t irq, uint64_t topi, uint8_t priority) {
+  word_t idx = get_prio_idx_in_group(irq);
+  if (intr_enable(topi, idx)) {
+    cpu.VSIpriosSort->ipriosEnable[idx].enable = true;
+    cpu.VSIpriosSort->ipriosEnable[idx].priority = priority;
+  }
+}
+
+word_t get_hviprio2(uint8_t idx) {
+  return (hviprio2->val >> 8 * idx) & 0xf;
+}
+
+void set_hviprios(Hviprios hprios) {
+  hprios.hviprios[1] = hviprio1->ssi;
+  hprios.hviprios[5] = hviprio1->sti;
+  hprios.hviprios[13] = hviprio1->coi;
+  hprios.hviprios[14] = hviprio1->intr14;
+  hprios.hviprios[15] = hviprio1->intr15;
+  for (int i = 0; i < 8; i++) {
+    hprios.hviprios[i+16] = get_hviprio2(i);
+  }
+}
+
+void set_viprios_sort(uint64_t topi_gather) {
+  Hviprios hviprios;
+  for (int i = 0; i < 24; i++) {
+    hviprios.hviprios[i] = 0;
+  }
+  
+  set_hviprios(hviprios);
+  for (int i = 0; i < 24; i++) {
+    handle_irq_hviprio(i, topi_gather, hviprios.hviprios[i]);
+  }
+}
+
+uint8_t high_iprio(IpriosSort* ipriosSort, uint8_t xei) {
+  uint8_t high_prio_idx = cpu.HighestPrioIntr->idx;
+
+  for (int i = 1; i < IPRIO_ENABLE_NUM; i ++) {
+    bool left_enable  = ipriosSort->ipriosEnable[high_prio_idx].enable;
+    bool right_enable = ipriosSort->ipriosEnable[i].enable;
+
+    bool left_disenable = !left_enable;
+
+    uint8_t left_priority = ipriosSort->ipriosEnable[high_prio_idx].priority;
+    uint8_t right_priority = ipriosSort->ipriosEnable[i].priority;
+
+    bool left_priority_is_zero = left_priority == 0;
+    bool right_priority_is_zero = right_priority == 0;
+
+    bool left_priority_is_not_zero = !left_priority_is_zero;
+    bool right_priority_is_not_zero = !right_priority_is_zero;
+
+    bool left_leq_xei = high_prio_idx <= get_prio_idx_in_group(xei);
+    bool right_leq_xei = i <= get_prio_idx_in_group(xei);
+
+    bool left_great_xei = !left_leq_xei;
+
+    bool left_leq_right = left_priority <= right_priority;
+    bool left_great_right = !left_leq_right;
+
+    if (left_disenable && right_enable) {
+      high_prio_idx = i;
+    } else if (left_enable && right_enable) {
+      if (left_priority_is_zero && right_priority_is_not_zero) {
+        if (left_great_xei || right_leq_xei) {
+          high_prio_idx = i;
+        }
+      } else if (left_priority_is_not_zero && right_priority_is_zero) {
+        if (left_great_xei && right_leq_xei) {
+          high_prio_idx = i;
+        }
+      } else if (left_priority_is_not_zero && right_priority_is_not_zero) {
+        if (left_great_right) {
+          high_prio_idx = i;
+        }
+      }
+    }
+  }
+
+  return high_prio_idx;
+}
+
+bool no_mtopi() {
+  return mtopi->iid == 0;
+}
+bool no_stopi() {
+  return stopi->iid == 0;
+}
+bool no_vstopi() {
+  return vstopi->iid == 0;
+}
+
+#endif
diff --git a/src/isa/riscv64/system/intr.c b/src/isa/riscv64/system/intr.c
index d4815b0aa..5bab6864e 100644
--- a/src/isa/riscv64/system/intr.c
+++ b/src/isa/riscv64/system/intr.c
@@ -19,6 +19,7 @@
 #include "../local-include/trigger.h"
 #include "../local-include/csr.h"
 #include "../local-include/intr.h"
+#include "../local-include/aia.h"
 
 void update_mmu_state();
 
@@ -137,9 +138,23 @@ word_t raise_intr(word_t NO, vaddr_t epc) {
   bool delegVS = intr_deleg_VS(NO);
   delegM = !delegS && !delegVS && !isNMI;
   delegS &= !delegVS;
+#ifdef CONFIG_RV_IMSIC
+  if (NO & INTR_BIT) {
+    delegS  =  no_mtopi() && !no_stopi() && !isNMI;
+    delegVS =  no_mtopi() &&  no_stopi() && !no_vstopi() && !isNMI;
+    delegM = !delegS && !delegVS && !isNMI;
+  }
+#endif
   bool vs_EX_DT = MUXDEF(CONFIG_RV_SSDBLTRP, delegVS && vsstatus->sdt, false);
   m_EX_DT = MUXDEF(CONFIG_RV_SMDBLTRP, delegM && mstatus->mdt, false);
   if ((delegVS && !vs_EX_DT) || (virtualInterruptIsHvictlInject && !isNMI)){
+#ifdef CONFIG_RV_IMSIC
+    if ((NO & INTR_BIT) && (NO & (MIP_SSIP | MIP_STIP | MIP_SEIP))) {
+      vscause->val = ((NO & (~INTR_BIT)) - 1) | INTR_BIT;
+    } else {
+      vscause->val = NO;
+    }
+#else
     if (virtualInterruptIsHvictlInject) {
       vscause->val = NO | INTR_BIT;
 #ifdef CONFIG_RV_IMSIC
@@ -148,6 +163,7 @@ word_t raise_intr(word_t NO, vaddr_t epc) {
     } else {
       vscause->val = NO & INTR_BIT ? ((NO & (~INTR_BIT)) - 1) | INTR_BIT : NO;
     }
+#endif // CONFIG_RV_IMSIC
     vsepc->val = epc;
     vsstatus->spp = cpu.mode;
     vsstatus->spie = vsstatus->sie;
diff --git a/src/isa/riscv64/system/priv.c b/src/isa/riscv64/system/priv.c
index 7f1361977..02bf1c897 100644
--- a/src/isa/riscv64/system/priv.c
+++ b/src/isa/riscv64/system/priv.c
@@ -18,6 +18,7 @@
 #include "../local-include/rtl.h"
 #include "../local-include/intr.h"
 #include "../local-include/trigger.h"
+#include "../local-include/aia.h"
 #include <cpu/cpu.h>
 #include <cpu/difftest.h>
 #include <memory/paddr.h>
@@ -94,6 +95,33 @@ void init_trigger() {
 }
 #endif // CONFIG_RV_SDTRIG
 
+#ifdef CONFIG_RV_IMSIC
+void init_iprio() {
+  cpu.MIprios  = (IpriosModule*) malloc(sizeof (IpriosModule));
+  cpu.SIprios  = (IpriosModule*) malloc(sizeof (IpriosModule));
+  cpu.VSIprios = (IpriosModule*) malloc(sizeof (IpriosModule));
+  cpu.MIpriosSort  = (IpriosSort*) malloc(sizeof (IpriosSort));
+  cpu.SIpriosSort  = (IpriosSort*) malloc(sizeof (IpriosSort));
+  cpu.VSIpriosSort = (IpriosSort*) malloc(sizeof (IpriosSort));
+  cpu.HighestPrioIntr = (HighestPrioIntr*) malloc(sizeof (HighestPrioIntr));
+  cpu.HighestPrioIntr->idx = 0;
+  cpu.HighestPrioIntr->priority = 0;
+  for (int i = 0; i < IPRIO_NUM; i++) {
+    cpu.MIprios->iprios[i].val = 0;
+    cpu.SIprios->iprios[i].val = 0;
+    cpu.VSIprios->iprios[i].val = 0;
+  }
+  for (int i = 0; i < IPRIO_ENABLE_NUM; i++) {
+    cpu.MIpriosSort->ipriosEnable[i].enable = false;
+    cpu.SIpriosSort->ipriosEnable[i].enable = false;
+    cpu.VSIpriosSort->ipriosEnable[i].enable = false;
+    cpu.MIpriosSort->ipriosEnable[i].priority = 0;
+    cpu.SIpriosSort->ipriosEnable[i].priority = 0;
+    cpu.VSIpriosSort->ipriosEnable[i].priority = 0;
+  }
+}
+#endif
+
 // check s/h/mcounteren for counters, throw exception if counter is not enabled.
 // also check h/mcounteren h/menvcfg for sstc
 static inline bool csr_counter_enable_check(uint32_t addr) {
@@ -794,6 +822,16 @@ static inline void set_vsie(word_t src) {
 }
 #endif // CONFIG_RVH
 
+#ifdef CONFIG_RVH
+static inline word_t get_hie() {
+  word_t tmp = 0;
+
+  tmp = mie->val & HIE_RMASK & (mideleg->val | MIDELEG_FORCED_MASK);
+
+  return tmp;
+}
+#endif
+
 inline word_t get_mip() {
   word_t tmp = 0;
 
@@ -977,6 +1015,16 @@ static inline void set_vsip(word_t src) {
 }
 #endif // CONFIG_RVH
 
+#ifdef CONFIG_RVH
+static inline word_t get_hip() {
+  word_t tmp = 0;
+
+  tmp = ((get_mip() & HIP_RMASK) | (hvip->val & MIP_VSSIP)) & (mideleg->val | MIDELEG_FORCED_MASK);
+
+  return tmp;
+}
+#endif
+
 static inline void update_counter_mcountinhibit(word_t old, word_t new) {
   #ifdef CONFIG_RV_CSR_MCOUNTINHIBIT_CNTR
     bool old_cy = old & 0x1;
@@ -998,6 +1046,169 @@ static inline void update_counter_mcountinhibit(word_t old, word_t new) {
     }
   #endif // CONFIG_RV_CSR_MCOUNTINHIBIT_CNTR
 }
+
+#ifdef CONFIG_RV_IMSIC
+inline void update_mtopi() {
+  bool miprios_is_zero = iprio_is_zero(cpu.MIprios);
+  uint64_t mtopi_gather = get_mip() & mie->val & (~(mideleg->val));
+  bool mtopi_is_not_zero = mtopi_gather != 0;
+  
+  set_iprios_sort(mtopi_gather, cpu.MIpriosSort, cpu.MIprios);
+  uint8_t m_iid_idx = high_iprio(cpu.MIpriosSort, IRQ_MEIP);
+  uint8_t m_iid_num = interrupt_default_prio[m_iid_idx];
+  uint8_t m_prio_num = cpu.MIpriosSort->ipriosEnable[m_iid_idx].priority;
+
+  bool m_iid_default_prio_high_MEI = m_iid_idx < get_prio_idx_in_group(IRQ_MEIP);
+  bool m_iid_default_prio_low_MEI = m_iid_idx > get_prio_idx_in_group(IRQ_MEIP);
+
+  if (mtopi_is_not_zero) {
+    mtopi->iid = m_iid_num;
+    if (miprios_is_zero) {
+      mtopi->iprio = 1;
+    } else {
+      if ((m_prio_num >= 1) && (m_prio_num <= 255)) {
+        mtopi->iprio = m_prio_num;
+      } else if ((m_prio_num > 255) || ((m_prio_num == 0) && (m_iid_default_prio_low_MEI))) {
+        mtopi->iprio = 255;
+      } else if ((m_prio_num == 0) && m_iid_default_prio_high_MEI) {
+        mtopi->iprio = 0;
+      }
+    } 
+  } else {
+    mtopi->val = 0;
+  }
+}
+#endif
+
+#ifdef CONFIG_RV_IMSIC
+inline void update_stopi() {
+  bool siprios_is_zero = iprio_is_zero(cpu.SIprios);
+  hip_t read_hip = (hip_t)get_hip();
+  sip_t read_sip = (sip_t)non_vmode_get_sip();
+  hie_t read_hie = (hie_t)get_hie();
+  sie_t read_sie = (sie_t)non_vmode_get_sie();
+
+  uint64_t stopi_gather = (read_hip.val | read_sip.val) & (read_hie.val | read_sie.val) & (~(hideleg->val));
+  bool stopi_is_not_zero = stopi_gather != 0;
+  set_iprios_sort(stopi_gather, cpu.SIpriosSort, cpu.SIprios);
+
+  uint8_t s_iid_idx = high_iprio(cpu.SIpriosSort, IRQ_SEIP);
+  uint8_t s_iid_num = interrupt_default_prio[s_iid_idx];
+  uint8_t s_prio_num = cpu.SIpriosSort->ipriosEnable[s_iid_idx].priority;
+
+  bool s_iid_default_prio_high_SEI = s_iid_idx < get_prio_idx_in_group(IRQ_SEIP);
+  bool s_iid_default_prio_low_SEI = s_iid_idx > get_prio_idx_in_group(IRQ_SEIP);
+
+  if (stopi_is_not_zero) {
+    stopi->iid = s_iid_num;
+    if (siprios_is_zero) {
+      stopi->iprio = 1;
+    } else {
+      if ((s_prio_num >= 1) && (s_prio_num <= 255)) {
+        stopi->iprio = s_prio_num;
+      } else if ((s_prio_num > 255) || ((s_prio_num == 0) && (s_iid_default_prio_low_SEI))) {
+        stopi->iprio = 255;
+      } else if ((s_prio_num == 0) && s_iid_default_prio_high_SEI) {
+        stopi->iprio = 0;
+      }
+    }
+  } else {
+    stopi->val = 0;
+  }
+}
+#endif
+
+#ifdef CONFIG_RV_IMSIC
+inline void update_vstopi() {
+  vsip_t read_vsip = (vsip_t)get_vsip();
+  vsie_t read_vsie = (vsie_t)get_vsie();
+
+  bool candidate1 = read_vsip.seip && read_vsie.seie && (hstatus->vgein != 0) && (cpu.xtopei.vstopei != 0);
+  bool candidate2 = read_vsip.seip && read_vsie.seie && (hstatus->vgein == 0) && (hvictl->iid == 9) && (hvictl->iprio != 0);
+  bool candidate3 = read_vsip.seip && read_vsie.seie && !candidate1 && !candidate2;
+  bool candidate4 = !hvictl->vti && (read_vsie.val & read_vsip.val & 0xfffffffffffffdff);
+  bool candidate5 = hvictl->vti && (hvictl->iid != 9);
+  bool candidate_no_valid = !candidate1 && !candidate2 && !candidate3 && !candidate4 && !candidate5;
+
+  uint64_t vstopi_gather = get_vsip() & get_vsie();
+  set_iprios_sort(vstopi_gather, cpu.VSIpriosSort, cpu.VSIprios);
+  set_viprios_sort(vstopi_gather);
+
+  uint8_t vs_iid_idx = high_iprio(cpu.VSIpriosSort, IRQ_VSEIP);
+  uint8_t vs_iid_num = interrupt_default_prio[vs_iid_idx];
+  uint8_t vs_prio_num = cpu.VSIpriosSort->ipriosEnable[vs_iid_idx].priority;
+
+  uint8_t iid_candidate123 = IRQ_SEIP;
+  uint8_t iid_candidate45 = 0;
+  uint16_t iprio_candidate123 = 0;
+  uint16_t iprio_candidate45 = 0;
+
+  if (candidate1) {
+    vstopei_t* vstopei_tmp = (vstopei_t*)cpu.xtopei.vstopei;
+    iprio_candidate123 = vstopei_tmp->iprio;
+  } else if (candidate2) {
+    iprio_candidate123 = hvictl->iprio;
+  } else if (candidate3) {
+    iprio_candidate123 = 256;
+  }
+
+  if (candidate4) {
+    iid_candidate45 = vs_iid_num;
+    iprio_candidate45 = vs_prio_num;
+  } else if (candidate5) {
+    iid_candidate45 = hvictl->iid;
+    iprio_candidate45 = hvictl->iprio;
+  }
+
+  bool candidate123 = candidate1 || candidate2 || candidate3;
+  bool candidate45 = candidate4 || candidate5;
+  bool candidate123_high_candidate45 = false;
+  bool candidate123_low_candidate45 = false;
+
+  if (candidate123 && candidate4) {
+    candidate123_high_candidate45 = (iprio_candidate123 < iprio_candidate45) || ((iprio_candidate123 == iprio_candidate45) && (get_prio_idx_in_group(iid_candidate123) <= get_prio_idx_in_group(iid_candidate45)));
+    candidate123_low_candidate45  = (iprio_candidate123 > iprio_candidate45) || ((iprio_candidate123 == iprio_candidate45) && (get_prio_idx_in_group(iid_candidate123) > get_prio_idx_in_group(iid_candidate45)));
+  } else if (candidate123 && candidate5) {
+    candidate123_high_candidate45 = (iprio_candidate123 < iprio_candidate45) || ((iprio_candidate123 == iprio_candidate45) && hvictl->dpr);
+    candidate123_low_candidate45  = (iprio_candidate123 > iprio_candidate45) || ((iprio_candidate123 == iprio_candidate45) && !hvictl->dpr);
+  } else if (candidate123 && !candidate45) {
+    candidate123_high_candidate45 = true;
+  } else if (!candidate123 && candidate45) {
+    candidate123_low_candidate45 = true;
+  }
+
+  uint8_t iid_candidate = 0;
+  uint16_t iprio_candidate = 0;
+
+  if (candidate123_high_candidate45) {
+    iid_candidate = iid_candidate123;
+    iprio_candidate = iprio_candidate123;
+  } else if (candidate123_low_candidate45) {
+    iid_candidate = iid_candidate45;
+    iprio_candidate = iprio_candidate45;
+  }
+
+  if (candidate_no_valid) {
+    vstopi->val = 0;
+  } else {
+    vstopi->iid = iid_candidate;
+    if (iprio_candidate > 255) {
+      vstopi->iprio = 255;
+    } else if (candidate123_low_candidate45 && candidate5 && !hvictl->ipriom) {
+      vstopi->iprio = 1;
+    } else if ((candidate123_high_candidate45 && (iprio_candidate <= 255)) || (candidate123_low_candidate45 && candidate4) || (candidate123_low_candidate45 && candidate5 && hvictl->ipriom)) {
+      vstopi->iprio = iprio_candidate & 0xff;
+    }
+  }
+}
+#endif
+
+#ifdef CONFIG_RV_IMSIC
+bool iselect_is_major_ip(uint64_t iselect) {
+  return (iselect > ISELECT_2F_MASK) && (iselect <= ISELECT_3F_MASK) && !(iselect & 0x1);
+}
+#endif
+
 static word_t csr_read(uint32_t csrid) {
   word_t *src = csr_decode(csrid);
   switch (csrid) {
@@ -1088,6 +1299,20 @@ static word_t csr_read(uint32_t csrid) {
     case CSR_SISELECT:
       IFDEF(CONFIG_RVH, if (cpu.v) return vsiselect->val);
       return siselect->val;
+    case CSR_STOPI:
+      if (cpu.v) return vstopi->val;
+      return stopi->val;
+    case CSR_STOPEI:
+      if (cpu.v) return cpu.xtopei.vstopei;
+      return cpu.xtopei.stopei;
+    case CSR_SIREG:
+    {
+      bool siselect_is_major_ip = iselect_is_major_ip(siselect->val);
+      if (siselect_is_major_ip) {
+        return cpu.SIprios->iprios[(siselect->val - ISELECT_2F_MASK - 1) >> 1].val;
+      }
+      return 0;
+    }
 #endif // CONFIG_RV_IMSIC
     case CSR_SATP:
       IFDEF(CONFIG_RVH, if (cpu.v) return vsatp->val);
@@ -1108,7 +1333,7 @@ static word_t csr_read(uint32_t csrid) {
     case CSR_VSIP: return get_vsip();
     case CSR_HEDELEG: return hedeleg->val & HEDELEG_MASK;
     case CSR_HIDELEG: return hideleg->val & HIDELEG_MASK;
-    case CSR_HIE: return mie->val & HIE_RMASK & (mideleg->val | MIDELEG_FORCED_MASK);
+    case CSR_HIE: return get_hie();
     case CSR_HGEIE: return hgeie->val & ~(0x1UL);
 #ifdef CONFIG_RV_AIA
     case CSR_HVIEN: return hvien->val & HVIEN_MSAK;
@@ -1126,9 +1351,20 @@ static word_t csr_read(uint32_t csrid) {
     case CSR_HSTATEEN0: return hstateen0->val & mstateen0->val;
 #endif // CONFIG_RV_SMSTATEEN
 
-    case CSR_HIP: return ((get_mip() & HIP_RMASK) | (hvip->val & MIP_VSSIP)) & (mideleg->val | MIDELEG_FORCED_MASK);
+    case CSR_HIP: return get_hip();
     case CSR_HVIP: return hvip->val & HVIP_MASK;
     case CSR_HGEIP: return hgeip->val & ~(0x1UL);
+#ifdef CONFIG_RV_IMSIC
+    case CSR_VSTOPEI: return cpu.xtopei.vstopei;
+    case CSR_VSIREG:
+    {
+      bool vsiselect_is_major_ip = iselect_is_major_ip(siselect->val);
+      if (vsiselect_is_major_ip) {
+        return cpu.SIprios->iprios[(siselect->val - ISELECT_2F_MASK - 1) >> 1].val;
+      }
+      return 0;
+    }
+#endif
 #endif // CONFIG_RVH
 
     /************************* Machine-Level CSRs *************************/
@@ -1144,6 +1380,17 @@ static word_t csr_read(uint32_t csrid) {
 #ifdef CONFIG_RV_AIA
     case CSR_MVIEN: return mvien->val & MVIEN_MASK;
     case CSR_MVIP: return get_mvip();
+#ifdef CONFIG_RV_IMSIC
+    case CSR_MTOPEI: return cpu.xtopei.mtopei;
+    case CSR_MIREG:
+    {
+      bool miselect_is_major_ip = iselect_is_major_ip(miselect->val);
+      if (miselect_is_major_ip) {
+        return cpu.MIprios->iprios[(miselect->val - ISELECT_2F_MASK - 1) >> 1].val;
+      }
+      return 0;
+    }
+#endif
 #endif // CONFIG_RV_AIA
 
     case CSR_MIP:
@@ -1423,6 +1670,15 @@ static void csr_write(uint32_t csrid, word_t src) {
 
 #ifdef CONFIG_RV_IMSIC
     case CSR_STOPI: return;
+    case CSR_STOPEI: return;
+    case CSR_SIREG:
+    {
+      if (cpu.v) { break; }
+      if (iselect_is_major_ip(siselect->val)) {
+        cpu.SIprios->iprios[(siselect->val - ISELECT_2F_MASK - 1) >> 1].val = src;
+      }
+      break;
+    }
 #endif // CONFIG_RV_IMSIC
 
 
@@ -1520,6 +1776,8 @@ static void csr_write(uint32_t csrid, word_t src) {
 
 #ifdef CONFIG_RV_IMSIC
     case CSR_VSTOPI: return;
+    case CSR_VSTOPEI: return;
+    case CSR_VSIREG: return;
 #endif // CONFIG_RV_IMSIC
 
 #endif // CONFIG_RVH
@@ -1797,6 +2055,14 @@ static void csr_write(uint32_t csrid, word_t src) {
 
 #ifdef CONFIG_RV_IMSIC
     case CSR_MTOPI: return;
+    case CSR_MTOPEI: return;
+    case CSR_MIREG:
+    {
+      if (iselect_is_major_ip(miselect->val)) {
+        cpu.MIprios->iprios[(miselect->val - ISELECT_2F_MASK - 1) >> 1].val = src;
+      }
+      break;
+    }
 #endif // CONFIG_RV_IMSIC
 
     /************************* All Others Normal CSRs *************************/
@@ -1831,6 +2097,17 @@ static void csr_write(uint32_t csrid, word_t src) {
       is_write(mie) || is_write(sie) || is_write(mip) || is_write(sip)) {
     set_sys_state_flag(SYS_STATE_UPDATE);
   }
+
+#ifdef CONFIG_RV_IMSIC
+  if (is_write(mideleg) || is_write(hideleg) || is_write(hstatus) || is_write(hvictl) ||
+      is_write(mip) || is_write(mvip) || is_write(hvip) || is_write(hip) || is_write(sip) || is_write(vsip) ||
+      is_write(mie) || is_write(mvien) || is_write(hvien) || is_write(hie) || is_write(sie) || is_write(vsie) ||
+      is_write(mireg) || is_write(sireg)) {
+    update_mtopi();
+    update_stopi();
+    update_vstopi();
+  }
+#endif
 }
 
 static inline bool satp_permit_check(const word_t *dest_access){