Zicfilp: Preserve expected landing pad state on traps

6 files changed, 26 insertions, 4 deletions

diff --git a/riscv/csrs.cc b/riscv/csrs.cc
index 50fb1a2..728d671 100644
--- a/riscv/csrs.cc
+++ b/riscv/csrs.cc
@@ -1337,10 +1337,12 @@ bool dcsr_csr_t::unlogged_write(const reg_t val) noexcept {
   return true;
 }
 
-void dcsr_csr_t::write_cause_and_prv(uint8_t cause, reg_t prv, bool v) noexcept {
+void dcsr_csr_t::update_fields(const uint8_t cause, const reg_t prv,
+                               const bool v, const elp_t pelp) noexcept {
   this->cause = cause;
   this->prv = prv;
   this->v = v;
+  this->pelp = pelp;
   log_write();
 }
 
diff --git a/riscv/csrs.h b/riscv/csrs.h
index a472801..990c301 100644
--- a/riscv/csrs.h
+++ b/riscv/csrs.h
@@ -681,7 +681,8 @@ class dcsr_csr_t: public csr_t {
   dcsr_csr_t(processor_t* const proc, const reg_t addr);
   virtual void verify_permissions(insn_t insn, bool write) const override;
   virtual reg_t read() const noexcept override;
-  void write_cause_and_prv(uint8_t cause, reg_t prv, bool v) noexcept;
+  void update_fields(const uint8_t cause, const reg_t prv,
+                     const bool v, const elp_t pelp) noexcept;
  protected:
   virtual bool unlogged_write(const reg_t val) noexcept override;
  public:
diff --git a/riscv/insns/dret.h b/riscv/insns/dret.h
index 2abcc7d..bdcf3db 100644
--- a/riscv/insns/dret.h
+++ b/riscv/insns/dret.h
@@ -1,5 +1,8 @@
 require(STATE.debug_mode);
 set_pc_and_serialize(STATE.dpc->read());
+if (ZICFILP_xLPE(STATE.dcsr->v, STATE.dcsr->prv)) {
+  STATE.elp = STATE.dcsr->pelp;
+}
 p->set_privilege(STATE.dcsr->prv, STATE.dcsr->v);
 if (STATE.prv < PRV_M)
   STATE.mstatus->write(STATE.mstatus->read() & ~MSTATUS_MPRV);
diff --git a/riscv/insns/mret.h b/riscv/insns/mret.h
index 7cb1f62..1133e86 100644
--- a/riscv/insns/mret.h
+++ b/riscv/insns/mret.h
@@ -9,6 +9,10 @@ s = set_field(s, MSTATUS_MIE, get_field(s, MSTATUS_MPIE));
 s = set_field(s, MSTATUS_MPIE, 1);
 s = set_field(s, MSTATUS_MPP, p->extension_enabled('U') ? PRV_U : PRV_M);
 s = set_field(s, MSTATUS_MPV, 0);
+if (ZICFILP_xLPE(prev_virt, prev_prv)) {
+  STATE.elp = static_cast<elp_t>(get_field(s, MSTATUS_MPELP));
+  s = set_field(s, MSTATUS_MPELP, elp_t::NO_LP_EXPECTED);
+}
 STATE.mstatus->write(s);
 if (STATE.mstatush) STATE.mstatush->write(s >> 32); // log mstatush change
 p->set_privilege(prev_prv, prev_virt);
diff --git a/riscv/insns/sret.h b/riscv/insns/sret.h
index 4c7305d..aeaf087 100644
--- a/riscv/insns/sret.h
+++ b/riscv/insns/sret.h
@@ -13,7 +13,6 @@ reg_t prev_prv = get_field(s, MSTATUS_SPP);
 s = set_field(s, MSTATUS_SIE, get_field(s, MSTATUS_SPIE));
 s = set_field(s, MSTATUS_SPIE, 1);
 s = set_field(s, MSTATUS_SPP, PRV_U);
-STATE.sstatus->write(s);
 bool prev_virt = STATE.v;
 if (!STATE.v) {
   if (p->extension_enabled('H')) {
@@ -24,4 +23,9 @@ if (!STATE.v) {
 
   STATE.mstatus->write(set_field(STATE.mstatus->read(), MSTATUS_MPRV, 0));
 }
+if (ZICFILP_xLPE(prev_virt, prev_prv)) {
+  STATE.elp = static_cast<elp_t>(get_field(s, SSTATUS_SPELP));
+  s = set_field(s, SSTATUS_SPELP, elp_t::NO_LP_EXPECTED);
+}
+STATE.sstatus->write(s);
 p->set_privilege(prev_prv, prev_virt);
diff --git a/riscv/processor.cc b/riscv/processor.cc
index 48e5f16..4f78879 100644
--- a/riscv/processor.cc
+++ b/riscv/processor.cc
@@ -823,8 +823,10 @@ const char* processor_t::get_privilege_string()
 
 void processor_t::enter_debug_mode(uint8_t cause)
 {
+  const bool has_zicfilp = extension_enabled(EXT_ZICFILP);
   state.debug_mode = true;
-  state.dcsr->write_cause_and_prv(cause, state.prv, state.v);
+  state.dcsr->update_fields(cause, state.prv, state.v, state.elp);
+  state.elp = elp_t::NO_LP_EXPECTED;
   set_privilege(PRV_M, false);
   state.dpc->write(state.pc);
   state.pc = DEBUG_ROM_ENTRY;
@@ -893,6 +895,8 @@ void processor_t::take_trap(trap_t& t, reg_t epc)
     s = set_field(s, MSTATUS_SPIE, get_field(s, MSTATUS_SIE));
     s = set_field(s, MSTATUS_SPP, state.prv);
     s = set_field(s, MSTATUS_SIE, 0);
+    s = set_field(s, MSTATUS_SPELP, state.elp);
+    state.elp = elp_t::NO_LP_EXPECTED;
     state.sstatus->write(s);
     set_privilege(PRV_S, true);
   } else if (state.prv <= PRV_S && bit < max_xlen && ((hsdeleg >> bit) & 1)) {
@@ -909,6 +913,8 @@ void processor_t::take_trap(trap_t& t, reg_t epc)
     s = set_field(s, MSTATUS_SPIE, get_field(s, MSTATUS_SIE));
     s = set_field(s, MSTATUS_SPP, state.prv);
     s = set_field(s, MSTATUS_SIE, 0);
+    s = set_field(s, MSTATUS_SPELP, state.elp);
+    state.elp = elp_t::NO_LP_EXPECTED;
     state.nonvirtual_sstatus->write(s);
     if (extension_enabled('H')) {
       s = state.hstatus->read();
@@ -940,6 +946,8 @@ void processor_t::take_trap(trap_t& t, reg_t epc)
     s = set_field(s, MSTATUS_MIE, 0);
     s = set_field(s, MSTATUS_MPV, curr_virt);
     s = set_field(s, MSTATUS_GVA, t.has_gva());
+    s = set_field(s, MSTATUS_MPELP, state.elp);
+    state.elp = elp_t::NO_LP_EXPECTED;
     state.mstatus->write(s);
     if (state.mstatush) state.mstatush->write(s >> 32);  // log mstatush change
     set_privilege(PRV_M, false);