3 files changed, 80 insertions, 21 deletions

diff --git a/src/arch/riscv/isa/decoder.isa b/src/arch/riscv/isa/decoder.isa
index e3992d712..d8f3395e3 100644
--- a/src/arch/riscv/isa/decoder.isa
+++ b/src/arch/riscv/isa/decoder.isa
@@ -1699,36 +1699,28 @@ decode QUADRANT default Unknown::unknown() {
             }
             format CSROp {
                 0x1: csrrw({{
-                    Rd = xc->readMiscReg(csr);
-                    xc->setMiscReg(csr, Rs1);
+                    Rd = data;
+                    data = Rs1;
                 }}, IsNonSpeculative, No_OpClass);
                 0x2: csrrs({{
-                    Rd = xc->readMiscReg(csr);
-                    if (Rs1 != 0) {
-                        xc->setMiscReg(csr, Rd | Rs1);
-                    }
+                    Rd = data;
+                    data |= Rs1;
                 }}, IsNonSpeculative, No_OpClass);
                 0x3: csrrc({{
-                    Rd = xc->readMiscReg(csr);
-                    if (Rs1 != 0) {
-                        xc->setMiscReg(csr, Rd & ~Rs1);
-                    }
+                    Rd = data;
+                    data &= ~Rs1;
                 }}, IsNonSpeculative, No_OpClass);
                 0x5: csrrwi({{
-                    Rd = xc->readMiscReg(csr);
-                    xc->setMiscReg(csr, uimm);
+                    Rd = data;
+                    data = uimm;
                 }}, IsNonSpeculative, No_OpClass);
                 0x6: csrrsi({{
-                    Rd = xc->readMiscReg(csr);
-                    if (uimm != 0) {
-                        xc->setMiscReg(csr, Rd | uimm);
-                    }
+                    Rd = data;
+                    data |= uimm;
                 }}, IsNonSpeculative, No_OpClass);
                 0x7: csrrci({{
-                    Rd = xc->readMiscReg(csr);
-                    if (uimm != 0) {
-                        xc->setMiscReg(csr, Rd & ~uimm);
-                    }
+                    Rd = data;
+                    data &= ~uimm;
                 }}, IsNonSpeculative, No_OpClass);
             }
         }
diff --git a/src/arch/riscv/isa/formats/standard.isa b/src/arch/riscv/isa/formats/standard.isa
index 2e0e3edd4..e69ad7ee5 100644
--- a/src/arch/riscv/isa/formats/standard.isa
+++ b/src/arch/riscv/isa/formats/standard.isa
@@ -210,6 +210,72 @@ def template JumpExecute {{
     }
 }};
 
+def template CSRExecute {{
+    Fault
+    %(class_name)s::execute(ExecContext *xc,
+        Trace::InstRecord *traceData) const
+    {
+        Fault fault = NoFault;
+
+        %(op_decl)s;
+        %(op_rd)s;
+
+        MiscReg data, olddata;
+        switch (csr) {
+          case CSR_FCSR:
+            olddata = xc->readMiscReg(MISCREG_FFLAGS) |
+                      (xc->readMiscReg(MISCREG_FRM) << FRM_OFFSET);
+            break;
+          default:
+            if (CSRData.find(csr) != CSRData.end()) {
+                olddata = xc->readMiscReg(CSRData.at(csr).physIndex);
+            } else {
+                std::string error = csprintf("Illegal CSR index %#x\n", csr);
+                fault = make_shared<IllegalInstFault>(error);
+                olddata = 0;
+            }
+            break;
+        }
+        auto mask = CSRMasks.find(csr);
+        if (mask != CSRMasks.end())
+            olddata &= mask->second;
+        DPRINTF(RiscvMisc, "Reading CSR %s: %#x\n", CSRData.at(csr).name,
+                olddata);
+        data = olddata;
+
+        if (fault == NoFault) {
+            %(code)s;
+            if (fault == NoFault) {
+                if (mask != CSRMasks.end())
+                    data &= mask->second;
+                if (data != olddata) {
+                    if (bits(csr, 11, 10) == 0x3) {
+                        std::string error = csprintf("CSR %s is read-only\n",
+                                                     CSRData.at(csr).name);
+                        fault = make_shared<IllegalInstFault>(error);
+                    } else {
+                        DPRINTF(RiscvMisc, "Writing %#x to CSR %s.\n", data,
+                                CSRData.at(csr).name);
+                        switch (csr) {
+                          case CSR_FCSR:
+                            xc->setMiscReg(MISCREG_FFLAGS, bits(data, 4, 0));
+                            xc->setMiscReg(MISCREG_FRM, bits(data, 7, 5));
+                            break;
+                          default:
+                            xc->setMiscReg(CSRData.at(csr).physIndex, data);
+                            break;
+                        }
+                    }
+                }
+            }
+            if (fault == NoFault) {
+                %(op_wb)s;
+            }
+        }
+        return fault;
+    }
+}};
+
 def format ROp(code, *opt_flags) {{
     iop = InstObjParams(name, Name, 'RegOp', code, opt_flags)
     header_output = BasicDeclare.subst(iop)
@@ -301,5 +367,5 @@ def format CSROp(code, *opt_flags) {{
     header_output = BasicDeclare.subst(iop)
     decoder_output = BasicConstructor.subst(iop)
     decode_block = BasicDecode.subst(iop)
-    exec_output = BasicExecute.subst(iop)
+    exec_output = CSRExecute.subst(iop)
 }};
diff --git a/src/arch/riscv/isa/includes.isa b/src/arch/riscv/isa/includes.isa
index 9f3d99fb5..d992685e1 100644
--- a/src/arch/riscv/isa/includes.isa
+++ b/src/arch/riscv/isa/includes.isa
@@ -87,6 +87,7 @@ output exec {{
 #include "base/condcodes.hh"
 #include "cpu/base.hh"
 #include "cpu/exetrace.hh"
+#include "debug/RiscvMisc.hh"
 #include "mem/packet.hh"
 #include "mem/packet_access.hh"
 #include "mem/request.hh"