diff --git a/bin/openocd_tcl_wrapper.py b/bin/openocd_tcl_wrapper.py
index 460cfa49a..fad2ae5e2 100644
--- a/bin/openocd_tcl_wrapper.py
+++ b/bin/openocd_tcl_wrapper.py
@@ -133,10 +133,16 @@ class OpenOCD:
             raise Exception("Error: Hart failed to resume")
 
     def step(self):
-        # Set halt bit #TODO save curent value of dcsr
-        self.write_data("DCSR", "0x4")
-        # Resume
-        #self.resume()
+        # Set step bit if it isn't already set
+        dcsr = int(self.read_data("DCSR"), 16)
+        if not (dcsr >> 2) & 0x1:
+            dcsr |= 0x4
+            self.write_data("DCSR", hex(dcsr))
+        # Resume once
+        self.resume()
+        # Unset step bit
+        dcsr &= ~0x4
+        self.write_data("DCSR", hex(dcsr))
 
     def access_register(self, write, regno, addr_size=None):
         data = 1 << 17  # transfer bit always set
diff --git a/src/debug/dm.sv b/src/debug/dm.sv
index 41d5da44f..8423683e7 100644
--- a/src/debug/dm.sv
+++ b/src/debug/dm.sv
@@ -406,11 +406,11 @@ module dm import cvw::*; #(parameter cvw_t P) (
         end
 
         AC_SCAN : begin
-          if ((GPRegNo | FPRegNo) & AcWrite & (Cycle == ScanChainLen)) // Writes to GPR/FPR are shifted in len(GPR) or len(FPR) cycles
+          if (~MiscRegNo & AcWrite & (Cycle == ScanChainLen)) // Writes to CSR/GPR/FPR are shifted in len(CSR/GPR) or len(FPR) cycles
             AcState <= AC_UPDATE;
-          else if ((GPRegNo | FPRegNo) & ~AcWrite & (Cycle == P.LLEN)) // Reads from GPR/FPR are shifted in len(ScanReg) cycles
+          else if (~MiscRegNo & ~AcWrite & (Cycle == P.LLEN)) // Reads from CSR/GPR/FPR are shifted in len(ScanReg) cycles
             AcState <= AC_IDLE;
-          else if (~(GPRegNo | FPRegNo) & (Cycle == ScanChainLen)) // Misc scanchain must be scanned completely
+          else if (MiscRegNo & (Cycle == ScanChainLen)) // Misc scanchain must be scanned completely
             AcState <= AC_IDLE;
           else
             Cycle <= Cycle + 1;
diff --git a/src/privileged/csr.sv b/src/privileged/csr.sv
index 474dd99ce..9973e5dfd 100644
--- a/src/privileged/csr.sv
+++ b/src/privileged/csr.sv
@@ -329,7 +329,7 @@ module csr import cvw::*;  #(parameter cvw_t P) (
   assign InsufficientCSRPrivilegeM = (CSRAdrDM[9:8] == 2'b11 & PrivilegeModeW != P.M_MODE) |
                                      (CSRAdrDM[9:8] == 2'b01 & PrivilegeModeW == P.U_MODE);
   assign IllegalCSRAccessM = ((IllegalCSRCAccessM & IllegalCSRMAccessM & 
-    IllegalCSRSAccessM & IllegalCSRUAccessM /*& IllegalCSRDAccessM*/ |
+    IllegalCSRSAccessM & IllegalCSRUAccessM & IllegalCSRDAccessM |
     InsufficientCSRPrivilegeM) & CSRReadM) | IllegalCSRMWriteReadonlyM;
 
   // Debug module CSR access