Add Ssdbltrp

9 files changed, 91 insertions, 16 deletions

diff --git a/riscv/csrs.cc b/riscv/csrs.cc
index e99ae84..01720eb 100644
--- a/riscv/csrs.cc
+++ b/riscv/csrs.cc
@@ -421,6 +421,7 @@ reg_t base_status_csr_t::compute_sstatus_write_mask() const noexcept {
     | (proc->any_custom_extensions() ? SSTATUS_XS : 0)
     | (has_vs ? SSTATUS_VS : 0)
     | (proc->extension_enabled(EXT_ZICFILP) ? SSTATUS_SPELP : 0)
+    | (proc->extension_enabled(EXT_SSDBLTRP) ? SSTATUS_SDT : 0)
     ;
 }
 
@@ -464,12 +465,23 @@ vsstatus_csr_t::vsstatus_csr_t(processor_t* const proc, const reg_t addr):
 }
 
 bool vsstatus_csr_t::unlogged_write(const reg_t val) noexcept {
-  const reg_t newval = (this->val & ~sstatus_write_mask) | (val & sstatus_write_mask);
+  const reg_t hDTE = (state->henvcfg->read() & HENVCFG_DTE);
+  const reg_t adj_write_mask = sstatus_write_mask & ~(hDTE ? 0 : SSTATUS_SDT);
+  reg_t newval = (this->val & ~adj_write_mask) | (val & adj_write_mask);
+
+  newval = (newval & SSTATUS_SDT) ? (newval & ~SSTATUS_SIE) : newval;
+
   if (state->v) maybe_flush_tlb(newval);
   this->val = adjust_sd(newval);
   return true;
 }
 
+reg_t vsstatus_csr_t::read() const noexcept {
+  const reg_t hDTE = state->henvcfg->read() & HENVCFG_DTE;
+  const reg_t adj_read_mask = sstatus_read_mask & ~(hDTE ? 0 : SSTATUS_SDT);
+  return this->val & adj_read_mask;
+}
+
 // implement class sstatus_proxy_csr_t
 sstatus_proxy_csr_t::sstatus_proxy_csr_t(processor_t* const proc, const reg_t addr, mstatus_csr_t_p mstatus):
   base_status_csr_t(proc, addr),
@@ -477,16 +489,26 @@ sstatus_proxy_csr_t::sstatus_proxy_csr_t(processor_t* const proc, const reg_t ad
 }
 
 bool sstatus_proxy_csr_t::unlogged_write(const reg_t val) noexcept {
-  const reg_t new_mstatus = (mstatus->read() & ~sstatus_write_mask) | (val & sstatus_write_mask);
+  const reg_t mDTE = (state->menvcfg->read() & MENVCFG_DTE);
+  const reg_t adj_write_mask = sstatus_write_mask & ~(mDTE ? 0 : SSTATUS_SDT);
+  reg_t new_mstatus = (mstatus->read() & ~adj_write_mask) | (val & adj_write_mask);
+
+  new_mstatus = (new_mstatus & SSTATUS_SDT) ? (new_mstatus & ~SSTATUS_SIE) : new_mstatus;
 
   // On RV32 this will only log the low 32 bits, so make sure we're
   // not modifying anything in the upper 32 bits.
-  assert((sstatus_write_mask & 0xffffffffU) == sstatus_write_mask);
+  assert((adj_write_mask & 0xffffffffU) == adj_write_mask);
 
   mstatus->write(new_mstatus);
   return false; // avoid double logging: already logged by mstatus->write()
 }
 
+reg_t sstatus_proxy_csr_t::read() const noexcept {
+  const reg_t mDTE = state->menvcfg->read() & MENVCFG_DTE;
+  const reg_t adj_read_mask = sstatus_read_mask & ~(mDTE ? 0 : SSTATUS_SDT);
+  return mstatus->read() & adj_read_mask;
+}
+
 // implement class mstatus_csr_t
 mstatus_csr_t::mstatus_csr_t(processor_t* const proc, const reg_t addr):
   base_status_csr_t(proc, addr),
@@ -506,6 +528,7 @@ bool mstatus_csr_t::unlogged_write(const reg_t val) noexcept {
                    | (has_gva ? MSTATUS_GVA : 0)
                    | (has_mpv ? MSTATUS_MPV : 0)
                    | (proc->extension_enabled(EXT_ZICFILP) ? (MSTATUS_SPELP | MSTATUS_MPELP) : 0)
+                   | (proc->extension_enabled(EXT_SSDBLTRP) ? SSTATUS_SDT : 0)
                    ;
 
   const reg_t requested_mpp = proc->legalize_privilege(get_field(val, MSTATUS_MPP));
@@ -1548,7 +1571,7 @@ void henvcfg_csr_t::verify_permissions(insn_t insn, bool write) const {
 }
 
 bool henvcfg_csr_t::unlogged_write(const reg_t val) noexcept {
-  const reg_t mask = menvcfg->read() | ~(MENVCFG_PBMTE | MENVCFG_STCE | MENVCFG_ADUE);
+  const reg_t mask = menvcfg->read() | ~(MENVCFG_PBMTE | MENVCFG_STCE | MENVCFG_ADUE | MENVCFG_DTE);
   return envcfg_csr_t::unlogged_write((masked_csr_t::read() & ~mask) | (val & mask));
 }
 
@@ -1765,3 +1788,13 @@ void ssp_csr_t::verify_permissions(insn_t insn, bool write) const {
   DECLARE_XENVCFG_VARS(SSE);
   require_envcfg(SSE);
 }
+
+mtval2_csr_t::mtval2_csr_t(processor_t* const proc, const reg_t addr):
+  hypervisor_csr_t(proc, addr) {
+}
+
+void mtval2_csr_t::verify_permissions(insn_t insn, bool write) const {
+  basic_csr_t::verify_permissions(insn, write);
+  if (!proc->extension_enabled('H') && !proc->extension_enabled(EXT_SSDBLTRP))
+    throw trap_illegal_instruction(insn.bits());
+}
diff --git a/riscv/csrs.h b/riscv/csrs.h
index 658ffdb..ad165b6 100644
--- a/riscv/csrs.h
+++ b/riscv/csrs.h
@@ -234,13 +234,12 @@ typedef std::shared_ptr<base_status_csr_t> base_status_csr_t_p;
 
 // For vsstatus, which is its own separate architectural register
 // (unlike sstatus)
+// vstatus.sdt is read_only 0 when henvcfg.dte = 0
 class vsstatus_csr_t final: public base_status_csr_t {
  public:
   vsstatus_csr_t(processor_t* const proc, const reg_t addr);
 
-  reg_t read() const noexcept override {
-    return val;
-  }
+  virtual reg_t read() const noexcept override;
 
  protected:
   virtual bool unlogged_write(const reg_t val) noexcept override;
@@ -300,13 +299,12 @@ class rv32_high_csr_t: public csr_t {
   csr_t_p orig;
 };
 
+// sstatus.sdt is read_only 0 when menvcfg.dte = 0
 class sstatus_proxy_csr_t final: public base_status_csr_t {
  public:
   sstatus_proxy_csr_t(processor_t* const proc, const reg_t addr, mstatus_csr_t_p mstatus);
 
-  reg_t read() const noexcept override {
-    return mstatus->read() & sstatus_read_mask;
-  }
+  virtual reg_t read() const noexcept override;
 
  protected:
   virtual bool unlogged_write(const reg_t val) noexcept override;
@@ -480,12 +478,13 @@ class envcfg_csr_t: public masked_csr_t {
 // henvcfg.pbmte is read_only 0 when menvcfg.pbmte = 0
 // henvcfg.stce is read_only 0 when menvcfg.stce = 0
 // henvcfg.hade is read_only 0 when menvcfg.hade = 0
+// henvcfg.dte is read_only 0 when menvcfg.dte = 0
 class henvcfg_csr_t final: public envcfg_csr_t {
  public:
   henvcfg_csr_t(processor_t* const proc, const reg_t addr, const reg_t mask, const reg_t init, csr_t_p menvcfg);
 
   reg_t read() const noexcept override {
-    return (menvcfg->read() | ~(MENVCFG_PBMTE | MENVCFG_STCE | MENVCFG_ADUE)) & masked_csr_t::read();
+    return (menvcfg->read() | ~(MENVCFG_PBMTE | MENVCFG_STCE | MENVCFG_ADUE | MENVCFG_DTE)) & masked_csr_t::read();
   }
 
   virtual void verify_permissions(insn_t insn, bool write) const override;
@@ -880,4 +879,11 @@ class ssp_csr_t final : public masked_csr_t {
   ssp_csr_t(processor_t* const proc, const reg_t addr, const reg_t mask, const reg_t init);
   virtual void verify_permissions(insn_t insn, bool write) const override;
 };
+
+// mtval2 CSR provided by H extension - but required if Ssdbltrp is  implemented
+class mtval2_csr_t: public hypervisor_csr_t {
+ public:
+  mtval2_csr_t(processor_t* const proc, const reg_t addr);
+  virtual void verify_permissions(insn_t insn, bool write) const override;
+};
 #endif
diff --git a/riscv/encoding.h b/riscv/encoding.h
index d5fb1de..675b4f6 100644
--- a/riscv/encoding.h
+++ b/riscv/encoding.h
@@ -2891,6 +2891,7 @@
 #define CAUSE_FETCH_PAGE_FAULT 0xc
 #define CAUSE_LOAD_PAGE_FAULT 0xd
 #define CAUSE_STORE_PAGE_FAULT 0xf
+#define CAUSE_DOUBLE_TRAP 0x10
 #define CAUSE_SOFTWARE_CHECK_FAULT 0x12
 #define CAUSE_HARDWARE_ERROR_FAULT 0x13
 #define CAUSE_FETCH_GUEST_PAGE_FAULT 0x14
@@ -4435,6 +4436,7 @@ DECLARE_CAUSE("machine ecall", CAUSE_MACHINE_ECALL)
 DECLARE_CAUSE("fetch page fault", CAUSE_FETCH_PAGE_FAULT)
 DECLARE_CAUSE("load page fault", CAUSE_LOAD_PAGE_FAULT)
 DECLARE_CAUSE("store page fault", CAUSE_STORE_PAGE_FAULT)
+DECLARE_CAUSE("double trap", CAUSE_DOUBLE_TRAP)
 DECLARE_CAUSE("software check fault", CAUSE_SOFTWARE_CHECK_FAULT)
 DECLARE_CAUSE("hardware error fault", CAUSE_HARDWARE_ERROR_FAULT)
 DECLARE_CAUSE("fetch guest page fault", CAUSE_FETCH_GUEST_PAGE_FAULT)
diff --git a/riscv/insns/dret.h b/riscv/insns/dret.h
index bdcf3db..60aaf21 100644
--- a/riscv/insns/dret.h
+++ b/riscv/insns/dret.h
@@ -7,6 +7,12 @@ p->set_privilege(STATE.dcsr->prv, STATE.dcsr->v);
 if (STATE.prv < PRV_M)
   STATE.mstatus->write(STATE.mstatus->read() & ~MSTATUS_MPRV);
 
+if (STATE.dcsr->prv == PRV_U || STATE.dcsr->v)
+  STATE.mstatus->write(STATE.mstatus->read() & ~MSTATUS_SDT);
+
+if (STATE.dcsr->v && STATE.dcsr->prv == PRV_U)
+  STATE.vsstatus->write(STATE.vsstatus->read() & ~SSTATUS_SDT);
+
 /* We're not in Debug Mode anymore. */
 STATE.debug_mode = false;
 
diff --git a/riscv/insns/mret.h b/riscv/insns/mret.h
index 3fe920c..140ebde 100644
--- a/riscv/insns/mret.h
+++ b/riscv/insns/mret.h
@@ -13,6 +13,10 @@ 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);
+if (prev_prv == PRV_U || prev_virt)
+  s = set_field(s, MSTATUS_SDT, 0);
+if (prev_virt && prev_prv == PRV_U)
+  STATE.vsstatus->write(STATE.vsstatus->read() & ~SSTATUS_SDT);
 STATE.mstatus->write(s);
 if (STATE.mstatush) STATE.mstatush->write(s >> 32); // log mstatush change
 STATE.tcontrol->write((STATE.tcontrol->read() & CSR_TCONTROL_MPTE) ? (CSR_TCONTROL_MPTE | CSR_TCONTROL_MTE) : 0);
diff --git a/riscv/insns/sret.h b/riscv/insns/sret.h
index 23a13b5..fe007d3 100644
--- a/riscv/insns/sret.h
+++ b/riscv/insns/sret.h
@@ -27,5 +27,12 @@ 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);
+
+if (STATE.prv == PRV_S) {
+  s = set_field(s, SSTATUS_SDT, 0);
+  if (!STATE.v && prev_virt && prev_prv == PRV_U)
+    STATE.vsstatus->write(STATE.vsstatus->read() & ~SSTATUS_SDT);
+}
+
 STATE.sstatus->write(s);
 p->set_privilege(prev_prv, prev_virt);
diff --git a/riscv/isa_parser.h b/riscv/isa_parser.h
index afb49f2..783af80 100644
--- a/riscv/isa_parser.h
+++ b/riscv/isa_parser.h
@@ -81,6 +81,7 @@ typedef enum {
   EXT_SSQOSID,
   EXT_ZICFILP,
   EXT_ZICFISS,
+  EXT_SSDBLTRP,
   NUM_ISA_EXTENSIONS
 } isa_extension_t;
 
diff --git a/riscv/processor.cc b/riscv/processor.cc
index 9498b8f..7a6989e 100644
--- a/riscv/processor.cc
+++ b/riscv/processor.cc
@@ -295,7 +295,7 @@ void state_t::reset(processor_t* const proc, reg_t max_isa)
   nonvirtual_scause = std::make_shared<cause_csr_t>(proc, CSR_SCAUSE);
   csrmap[CSR_VSCAUSE] = vscause = std::make_shared<cause_csr_t>(proc, CSR_VSCAUSE);
   csrmap[CSR_SCAUSE] = scause = std::make_shared<virtualized_csr_t>(proc, nonvirtual_scause, vscause);
-  csrmap[CSR_MTVAL2] = mtval2 = std::make_shared<hypervisor_csr_t>(proc, CSR_MTVAL2);
+  csrmap[CSR_MTVAL2] = mtval2 = std::make_shared<mtval2_csr_t>(proc, CSR_MTVAL2);
   csrmap[CSR_MTINST] = mtinst = std::make_shared<hypervisor_csr_t>(proc, CSR_MTINST);
   const reg_t hstatus_init = set_field((reg_t)0, HSTATUS_VSXL, xlen_to_uxl(proc->get_const_xlen()));
   const reg_t hstatus_mask = HSTATUS_VTSR | HSTATUS_VTW
@@ -391,7 +391,8 @@ void state_t::reset(processor_t* const proc, reg_t max_isa)
                               (proc->extension_enabled(EXT_SVPBMT) ? MENVCFG_PBMTE : 0) |
                               (proc->extension_enabled(EXT_SSTC) ? MENVCFG_STCE : 0) |
                               (proc->extension_enabled(EXT_ZICFILP) ? MENVCFG_LPE : 0) |
-                              (proc->extension_enabled(EXT_ZICFISS) ? MENVCFG_SSE : 0);
+                              (proc->extension_enabled(EXT_ZICFISS) ? MENVCFG_SSE : 0) |
+                              (proc->extension_enabled(EXT_SSDBLTRP) ? MENVCFG_DTE : 0);
     const reg_t menvcfg_init = (proc->extension_enabled(EXT_SVPBMT) ? MENVCFG_PBMTE : 0);
     menvcfg = std::make_shared<envcfg_csr_t>(proc, CSR_MENVCFG, menvcfg_mask, menvcfg_init);
     if (xlen == 32) {
@@ -411,7 +412,8 @@ void state_t::reset(processor_t* const proc, reg_t max_isa)
                               (proc->extension_enabled(EXT_SVPBMT) ? HENVCFG_PBMTE : 0) |
                               (proc->extension_enabled(EXT_SSTC) ? HENVCFG_STCE : 0) |
                               (proc->extension_enabled(EXT_ZICFILP) ? HENVCFG_LPE : 0) |
-                              (proc->extension_enabled(EXT_ZICFISS) ? HENVCFG_SSE : 0);
+                              (proc->extension_enabled(EXT_ZICFISS) ? HENVCFG_SSE : 0) |
+                              (proc->extension_enabled(EXT_SSDBLTRP) ? HENVCFG_DTE : 0);
     const reg_t henvcfg_init = (proc->extension_enabled(EXT_SVPBMT) ? HENVCFG_PBMTE : 0);
     henvcfg = std::make_shared<henvcfg_csr_t>(proc, CSR_HENVCFG, henvcfg_mask, henvcfg_init, menvcfg);
     if (xlen == 32) {
@@ -820,6 +822,7 @@ void processor_t::take_trap(trap_t& t, reg_t epc)
   bool curr_virt = state.v;
   const reg_t interrupt_bit = (reg_t)1 << (max_xlen - 1);
   bool interrupt = (bit & interrupt_bit) != 0;
+  bool supv_double_trap = false;
   if (interrupt) {
     vsdeleg = (curr_virt && state.prv <= PRV_S) ? state.hideleg->read() : 0;
     hsdeleg = (state.prv <= PRV_S) ? state.mideleg->read() : 0;
@@ -828,6 +831,14 @@ void processor_t::take_trap(trap_t& t, reg_t epc)
     vsdeleg = (curr_virt && state.prv <= PRV_S) ? (state.medeleg->read() & state.hedeleg->read()) : 0;
     hsdeleg = (state.prv <= PRV_S) ? state.medeleg->read() : 0;
   }
+  // An unexpected trap - a trap when SDT is 1 - traps to M-mode
+  if ((state.prv <= PRV_S && bit < max_xlen) &&
+      (((vsdeleg >> bit) & 1)  || ((hsdeleg >> bit) & 1))) {
+    reg_t s = curr_virt ? state.nonvirtual_sstatus->read() : state.sstatus->read();
+    supv_double_trap = get_field(s, MSTATUS_SDT);
+    if (supv_double_trap)
+      vsdeleg = hsdeleg = 0;
+  }
   if (state.prv <= PRV_S && bit < max_xlen && ((vsdeleg >> bit) & 1)) {
     // Handle the trap in VS-mode
     const reg_t adjusted_cause = interrupt ? bit - 1 : bit;  // VSSIP -> SSIP, etc
@@ -842,6 +853,8 @@ void processor_t::take_trap(trap_t& t, reg_t epc)
     s = set_field(s, MSTATUS_SPP, state.prv);
     s = set_field(s, MSTATUS_SIE, 0);
     s = set_field(s, MSTATUS_SPELP, state.elp);
+    if ((state.menvcfg->read() & MENVCFG_DTE) && (state.henvcfg->read() & HENVCFG_DTE))
+      s = set_field(s, MSTATUS_SDT, 1);
     state.elp = elp_t::NO_LP_EXPECTED;
     state.sstatus->write(s);
     set_privilege(PRV_S, true);
@@ -860,6 +873,8 @@ void processor_t::take_trap(trap_t& t, reg_t epc)
     s = set_field(s, MSTATUS_SPP, state.prv);
     s = set_field(s, MSTATUS_SIE, 0);
     s = set_field(s, MSTATUS_SPELP, state.elp);
+    if (state.menvcfg->read() & MENVCFG_DTE)
+      s = set_field(s, MSTATUS_SDT, 1);
     state.elp = elp_t::NO_LP_EXPECTED;
     state.nonvirtual_sstatus->write(s);
     if (extension_enabled('H')) {
@@ -881,9 +896,9 @@ void processor_t::take_trap(trap_t& t, reg_t epc)
     const bool nmie = !(state.mnstatus && !get_field(state.mnstatus->read(), MNSTATUS_NMIE));
     state.pc = !nmie ? rnmi_trap_handler_address : trap_handler_address;
     state.mepc->write(epc);
-    state.mcause->write(t.cause());
+    state.mcause->write(supv_double_trap ? CAUSE_DOUBLE_TRAP : t.cause());
     state.mtval->write(t.get_tval());
-    state.mtval2->write(t.get_tval2());
+    state.mtval2->write(supv_double_trap ? t.cause() : t.get_tval2());
     state.mtinst->write(t.get_tinst());
 
     reg_t s = state.mstatus->read();
diff --git a/riscv/trap.h b/riscv/trap.h
index 5eb62cf..5ea56e2 100644
--- a/riscv/trap.h
+++ b/riscv/trap.h
@@ -115,6 +115,7 @@ DECLARE_TRAP(CAUSE_MACHINE_ECALL, machine_ecall)
 DECLARE_MEM_TRAP(CAUSE_FETCH_PAGE_FAULT, instruction_page_fault)
 DECLARE_MEM_TRAP(CAUSE_LOAD_PAGE_FAULT, load_page_fault)
 DECLARE_MEM_TRAP(CAUSE_STORE_PAGE_FAULT, store_page_fault)
+DECLARE_TRAP(CAUSE_DOUBLE_TRAP, double_trap)
 DECLARE_MEM_GVA_TRAP(CAUSE_FETCH_GUEST_PAGE_FAULT, instruction_guest_page_fault)
 DECLARE_MEM_GVA_TRAP(CAUSE_LOAD_GUEST_PAGE_FAULT, load_guest_page_fault)
 DECLARE_INST_TRAP(CAUSE_VIRTUAL_INSTRUCTION, virtual_instruction)