4 files changed, 106 insertions, 0 deletions
diff --git a/riscv/csrs.cc b/riscv/csrs.cc
index b8fa920..c2bdf0a 100644
--- a/riscv/csrs.cc
+++ b/riscv/csrs.cc
@@ -1244,3 +1244,64 @@ bool vxsat_csr_t::unlogged_write(const reg_t val) noexcept {
   dirty_vs_state;
   return masked_csr_t::unlogged_write(val);
 }
+
+// implement class hstateen_csr_t
+hstateen_csr_t::hstateen_csr_t(processor_t* const proc, const reg_t addr, const reg_t mask,
+                               const reg_t init, uint8_t index):
+  masked_csr_t(proc, addr, mask, init),
+  index(index) {
+}
+
+reg_t hstateen_csr_t::read() const noexcept {
+  // For every bit in an mstateen CSR that is zero (whether read-only zero or set to zero),
+  // the same bit appears as read-only zero in the matching hstateen and sstateen CSRs
+  return masked_csr_t::read() & state->mstateen[index]->read();
+}
+
+bool hstateen_csr_t::unlogged_write(const reg_t val) noexcept {
+  // For every bit in an mstateen CSR that is zero (whether read-only zero or set to zero),
+  // the same bit appears as read-only zero in the matching hstateen and sstateen CSRs
+  return masked_csr_t::unlogged_write(val & state->mstateen[index]->read());
+}
+
+void hstateen_csr_t::verify_permissions(insn_t insn, bool write) const {
+  masked_csr_t::verify_permissions(insn, write);
+
+  if ((state->prv < PRV_M) && !(state->mstateen[index]->read() & MSTATEEN_HSTATEEN))
+    throw trap_illegal_instruction(insn.bits());
+}
+
+// implement class sstateen_csr_t
+sstateen_csr_t::sstateen_csr_t(processor_t* const proc, const reg_t addr, const reg_t mask,
+                               const reg_t init, uint8_t index):
+  hstateen_csr_t(proc, addr, mask, init, index) {
+}
+
+reg_t sstateen_csr_t::read() const noexcept {
+  // For every bit in an mstateen CSR that is zero (whether read-only zero or set to zero),
+  // the same bit appears as read-only zero in the matching hstateen and sstateen CSRs
+  // For every bit in an hstateen CSR that is zero (whether read-only zero or set to zero),
+  // the same bit appears as read-only zero in sstateen when accessed in VS-mode
+  if (state->v)
+    return hstateen_csr_t::read() & state->hstateen[index]->read();
+  else
+    return hstateen_csr_t::read();
+}
+
+bool sstateen_csr_t::unlogged_write(const reg_t val) noexcept {
+  // For every bit in an mstateen CSR that is zero (whether read-only zero or set to zero),
+  // the same bit appears as read-only zero in the matching hstateen and sstateen CSRs
+  // For every bit in an hstateen CSR that is zero (whether read-only zero or set to zero),
+  // the same bit appears as read-only zero in sstateen when accessed in VS-mode
+  if (state->v)
+    return hstateen_csr_t::unlogged_write(val & state->hstateen[index]->read());
+  else
+    return hstateen_csr_t::unlogged_write(val);
+}
+
+void sstateen_csr_t::verify_permissions(insn_t insn, bool write) const {
+  hstateen_csr_t::verify_permissions(insn, write);
+
+  if (state->v && !(state->hstateen[index]->read() & HSTATEEN_SSTATEEN))
+      throw trap_virtual_instruction(insn.bits());
+}
diff --git a/riscv/csrs.h b/riscv/csrs.h
index 03a3ed6..15f868c 100644
--- a/riscv/csrs.h
+++ b/riscv/csrs.h
@@ -676,4 +676,23 @@ class vxsat_csr_t: public masked_csr_t {
   virtual bool unlogged_write(const reg_t val) noexcept override;
 };
 
+class hstateen_csr_t: public masked_csr_t {
+ public:
+  hstateen_csr_t(processor_t* const proc, const reg_t addr, const reg_t mask, const reg_t init, uint8_t index);
+  virtual reg_t read() const noexcept override;
+  virtual void verify_permissions(insn_t insn, bool write) const override;
+ protected:
+  virtual bool unlogged_write(const reg_t val) noexcept override;
+protected:
+  uint8_t index;
+};
+
+class sstateen_csr_t: public hstateen_csr_t {
+ public:
+  sstateen_csr_t(processor_t* const proc, const reg_t addr, const reg_t mask, const reg_t init, uint8_t index);
+  virtual reg_t read() const noexcept override;
+  virtual void verify_permissions(insn_t insn, bool write) const override;
+ protected:
+  virtual bool unlogged_write(const reg_t val) noexcept override;
+};
 #endif
diff --git a/riscv/processor.cc b/riscv/processor.cc
index 7ffc8d1..1c33911 100644
--- a/riscv/processor.cc
+++ b/riscv/processor.cc
@@ -397,6 +397,28 @@ void state_t::reset(processor_t* const proc, reg_t max_isa)
   const reg_t henvcfg_init = (proc->extension_enabled(EXT_SVPBMT) ? HENVCFG_PBMTE : 0);
   csrmap[CSR_HENVCFG] = henvcfg = std::make_shared<henvcfg_csr_t>(proc, CSR_HENVCFG, henvcfg_mask, henvcfg_init, menvcfg);
 
+  if (proc->extension_enabled_const(EXT_SMSTATEEN)) {
+    const reg_t sstateen0_mask = (proc->extension_enabled(EXT_ZFINX) ? SSTATEEN0_FCSR : 0) | SSTATEEN0_CS;
+    const reg_t hstateen0_mask = sstateen0_mask | HSTATEEN0_SENVCFG | HSTATEEN_SSTATEEN;
+    const reg_t mstateen0_mask = hstateen0_mask;
+    for (int i = 0; i < 4; i++) {
+      const reg_t mstateen_mask = i == 0 ? mstateen0_mask : MSTATEEN_HSTATEEN;
+      csrmap[CSR_MSTATEEN0 + i] = mstateen[i] = std::make_shared<masked_csr_t>(proc, CSR_MSTATEEN0 + i, mstateen_mask, 0);
+      if (xlen == 32) {
+        csrmap[CSR_MSTATEEN0H + i] = std::make_shared<rv32_high_csr_t>(proc, CSR_MSTATEEN0H + i, -1, mstateen[i]);
+      }
+
+      const reg_t hstateen_mask = i == 0 ? hstateen0_mask : HSTATEEN_SSTATEEN;
+      csrmap[CSR_HSTATEEN0 + i] = hstateen[i] = std::make_shared<hstateen_csr_t>(proc, CSR_HSTATEEN0 + i, hstateen_mask, 0, i);
+      if (xlen == 32) {
+        csrmap[CSR_HSTATEEN0H + i] = std::make_shared<rv32_high_csr_t>(proc, CSR_HSTATEEN0H + i, -1, hstateen[i]);
+      }
+
+      const reg_t sstateen_mask = i == 0 ? sstateen0_mask : 0;
+      csrmap[CSR_SSTATEEN0 + i] = sstateen[i] = std::make_shared<sstateen_csr_t>(proc, CSR_HSTATEEN0 + i, sstateen_mask, 0, i);
+    }
+  }
+
   serialized = false;
 
 #ifdef RISCV_ENABLE_COMMITLOG
diff --git a/riscv/processor.h b/riscv/processor.h
index 0c6a6b2..727c404 100644
--- a/riscv/processor.h
+++ b/riscv/processor.h
@@ -186,6 +186,10 @@ struct state_t
   csr_t_p senvcfg;
   csr_t_p henvcfg;
 
+  csr_t_p mstateen[4];
+  csr_t_p sstateen[4];
+  csr_t_p hstateen[4];
+
   bool serialized; // whether timer CSRs are in a well-defined state
 
   // When true, execute a single instruction and then enter debug mode.  This