diff --git a/bin/hw_debug_test.py b/bin/hw_debug_test.py
index f7aad2938..5febadc2a 100755
--- a/bin/hw_debug_test.py
+++ b/bin/hw_debug_test.py
@@ -35,6 +35,7 @@ from openocd_tcl_wrapper import OpenOCD
 random_stimulus = True
 random_order = False
 
+
 def main():
     with OpenOCD() as cvw:
         registers = dict.fromkeys(cvw.register_translations.keys(),[])
diff --git a/bin/openocd_tcl_wrapper.py b/bin/openocd_tcl_wrapper.py
index d035a60b8..460cfa49a 100644
--- a/bin/openocd_tcl_wrapper.py
+++ b/bin/openocd_tcl_wrapper.py
@@ -131,14 +131,12 @@ class OpenOCD:
         dmstat = int(self.read_dmi("0x11"), 16)  # Check resumeack bit
         if not ((dmstat >> 16) & 0x3):
             raise Exception("Error: Hart failed to resume")
-        self.write_dmi("0x10", "0x40000001")  # Clear resumeack bit
 
     def step(self):
-        self.write_dmi("0x10", "0xC0000001")
-        # BOZO: checking resumeack after halt is pointless until sdext halt method is added
-        dmstat = int(self.read_dmi("0x11"), 16)
-        if not ((dmstat >> 16) & 0x3):
-            raise Exception("Error: Hart failed to resume")
+        # Set halt bit #TODO save curent value of dcsr
+        self.write_data("DCSR", "0x4")
+        # Resume
+        #self.resume()
 
     def access_register(self, write, regno, addr_size=None):
         data = 1 << 17  # transfer bit always set
diff --git a/config/shared/debug.vh b/config/shared/debug.vh
index 35a5daca3..d392665ce 100755
--- a/config/shared/debug.vh
+++ b/config/shared/debug.vh
@@ -101,6 +101,20 @@
 `define CMDTYPE 31:24
 `define CONTROL 23:0
 
+// DCSR
+`define EBREAKVS  17
+`define EBREAKVU  16
+`define EBREAKM   15
+`define EBREAKS   13
+`define EBREAKU   12
+`define STEPIE    11
+`define STOPCOUNT 10
+`define STOPTIME  9
+`define V         5
+`define MPRVEN    4
+`define STEP      2
+`define PRV       1:0
+
 //// Abstract Commands
 // cmderr
 `define CMDERR_NONE          3'h0
diff --git a/src/debug/dm.sv b/src/debug/dm.sv
index 47d61a10c..41d5da44f 100644
--- a/src/debug/dm.sv
+++ b/src/debug/dm.sv
@@ -26,14 +26,10 @@
 ////////////////////////////////////////////////////////////////////////////////////////////////
 
 // TODO List:
-// fix CSR scanning
 // determine config/permissions of all CSRs
-// DCSR and DPC should exist even when not privileged_supported
-// test all combinations of XLEN/FLEN
 // improve halting / implement "debug mode"
 //// Debug Mode = M-mode with stalled pipe
 // Flush pipe with NOPs during halt?
-// implement better steps
 // Alias DPC to PCF/PCNextF?
 
 // (stretch) add system bus access?
@@ -51,8 +47,14 @@ module dm import cvw::*; #(parameter cvw_t P) (
 
   // Platform reset signal
   output logic            NdmReset,
-  // Core hazard signal
-  output logic            DebugStall,
+  // Core control signals
+  input  logic            ResumeAck,      // Signals Hart has been resumed
+  input  logic            HaveReset,      // Signals Hart has been reset
+  input  logic            DebugMode,      // Signals core is halted
+  output logic            HaltReq,        // Initiates core halt
+  output logic            ResumeReq,      // Initiates core resume
+  output logic            HaltOnReset,    // Halts core immediately on hart reset
+  output logic            AckHaveReset,   // Clears HaveReset status
 
   // Scan Chain
   output logic            DebugScanEn,    // puts scannable flops into scan mode
@@ -83,28 +85,16 @@ module dm import cvw::*; #(parameter cvw_t P) (
   logic [1:0]             RspOP;
 
   // JTAG ID for Wally:  
-  // [31:27] = 1 (4 bits)
-  // ver [27:12] = 0x2A (42)
-  // JEDEC number [11:1] = 000_0000_0010 (Open HW Group)
-  // [0] set to 1
-  localparam JTAG_DEVICE_ID = 32'h1002_A005; 
+  // Version [31:28] = 0x1 : 0001
+  // PartNumber [27:12] = 0x2A : 00000000_00101010
+  // JEDEC number [11:1] = 0x602 : Bank 13 (1100) Open HW Group (0000010) 
+  // [0] = 1
+  localparam JTAG_DEVICE_ID = 32'h1002AC05; 
 
   dtm #(`ADDR_WIDTH, JTAG_DEVICE_ID) dtm (.clk, .tck, .tdi, .tms, .tdo,
     .ReqReady, .ReqValid, .ReqAddress, .ReqData, .ReqOP, .RspReady,
     .RspValid, .RspData, .RspOP);
 
-  // Core control signals
-  logic HaltReq;
-  logic ResumeReq;
-  logic HaltOnReset;
-  logic Halted;
-  logic AckHaveReset;
-
-  hartcontrol hartcontrol(.clk, .rst(rst | ~DmActive), .NdmReset, .AckHaveReset, .HaltReq,
-    .ResumeReq, .HaltOnReset, .DebugStall, .Halted, .AllRunning, .AnyRunning, 
-    .AllHalted, .AnyHalted, .AllResumeAck, .AnyResumeAck, .AllHaveReset, .AnyHaveReset);
-
-
   enum logic [3:0] {INACTIVE, IDLE, ACK, R_DATA, W_DATA, DMSTATUS, W_DMCONTROL, R_DMCONTROL, 
 		    W_ABSTRACTCS, R_ABSTRACTCS, ABST_COMMAND, R_SYSBUSCS, READ_ZERO,
 		    INVALID} State;
@@ -149,7 +139,7 @@ module dm import cvw::*; #(parameter cvw_t P) (
   //// DM register fields
   // DMControl
   logic              AckUnavail;
-  logic              DmActive;       // This bit is used to (de)activate the DM. Toggling acts as reset
+  logic              DmActive;       // This bit is used to (de)activate the DM. Toggling off/on acts as reset
   // DMStatus
   logic              StickyUnavail;
   logic              ImpEBreak;
@@ -157,10 +147,10 @@ module dm import cvw::*; #(parameter cvw_t P) (
   logic              AnyHaveReset;
   logic              AllResumeAck;
   logic              AnyResumeAck;
-  logic              AllNonExistent; // TODO
-  logic              AnyNonExistent;
-  logic              AllUnavail;     // TODO
-  logic              AnyUnavail;
+  const logic        AllNonExistent = 0;
+  const logic        AnyNonExistent = 0;
+  const logic        AllUnavail = 0;
+  const logic        AnyUnavail = 0;
   logic              AllRunning;
   logic              AnyRunning;
   logic              AllHalted;
@@ -177,6 +167,18 @@ module dm import cvw::*; #(parameter cvw_t P) (
   logic [2:0]        CmdErr;
   const logic [3:0]  DataCount = (P.LLEN/32);
 
+  // Core control signals
+  assign AllHaveReset = HaveReset;
+  assign AnyHaveReset = HaveReset;
+  assign AnyHalted = DebugMode;
+  assign AllHalted = DebugMode;
+  assign AnyRunning = ~DebugMode;
+  assign AllRunning = ~DebugMode;
+  // I believe resumeack is used to determine when a resume is requested but never completes
+  // It's pretty worthless in this implementation (complain to the debug working group)
+  assign AllResumeAck = ResumeAck;
+  assign AnyResumeAck = ResumeAck;
+  
   // See spec 3.14.2
   assign DMControl = {2'b0, 1'b0, 2'b0, 1'b0, 10'b0, 10'b0, 4'b0, NdmReset, DmActive};
 
@@ -290,7 +292,7 @@ module dm import cvw::*; #(parameter cvw_t P) (
             //  hartreset, ackhavereset, setresethaltreq, and clrresethaltreq. The others must be written 0
             case ({ReqData[`RESUMEREQ],ReqData[`ACKHAVERESET],ReqData[`SETRESETHALTREQ],ReqData[`CLRRESETHALTREQ]})
               4'b0000 :; // None
-              4'b1000 : ResumeReq <= 1;
+              4'b1000 : ResumeReq <= ~ReqData[`HALTREQ]; // Ignore ResumeReq if HaltReq
               4'b0100 : AckHaveReset <= 1;
               4'b0010 : HaltOnReset <= 1;
               4'b0001 : HaltOnReset <= 0;
@@ -338,7 +340,7 @@ module dm import cvw::*; #(parameter cvw_t P) (
           if (CmdErr != `CMDERR_NONE); // If CmdErr, do nothing
           else if (Busy)
             CmdErr <= `CMDERR_BUSY; // If Busy, set CmdErr, do nothing
-          else if (~Halted)
+          else if (~DebugMode)
             CmdErr <= `CMDERR_HALTRESUME; // If not halted, set CmdErr, do nothing
           else begin
             case (ReqData[`CMDTYPE])
diff --git a/src/debug/hartcontrol.sv b/src/debug/dmc.sv
similarity index 55%
rename from src/debug/hartcontrol.sv
rename to src/debug/dmc.sv
index 9dccb074d..4d29b99e8 100644
--- a/src/debug/hartcontrol.sv
+++ b/src/debug/dmc.sv
@@ -1,10 +1,10 @@
 ///////////////////////////////////////////
-// hartcontrol.sv
+// dmc.sv
 //
 // Written: matthew.n.otto@okstate.edu 10 May 2024
 // Modified: 
 //
-// Purpose: Controls the state of connected hart
+// Purpose: Controls pipeline during Debug Mode
 // 
 // Documentation: RISC-V System on Chip Design 
 //
@@ -30,72 +30,43 @@
 // Note: This module controls all of the per-hart debug state.
 // In a multihart system, this module should be instantiated under wallypipelinedcore
 
-module hartcontrol(
-  input  logic clk, rst,
-  input  logic NdmReset,     // Triggers HaltOnReset behavior
-  input  logic AckHaveReset, // Clears *HaveReset status
-
-  input  logic HaltReq,      // Initiate core halt
+module dmc(
+  input  logic clk, reset,
+  input  logic Step,
+  input  logic HaltReq,      // Initiates core halt
   input  logic ResumeReq,    // Initiates core resume
   input  logic HaltOnReset,  // Halts core immediately on hart reset
+  input  logic AckHaveReset, // Clears HaveReset status
 
-  output logic DebugStall,   // Stall signal goes to hazard unit
-
-  // DMStatus bits
-  output logic Halted,
-  output logic AllRunning,
-  output logic AnyRunning,
-  output logic AllHalted,
-  output logic AnyHalted,
-  output logic AllResumeAck,
-  output logic AnyResumeAck,
-  output logic AllHaveReset,
-  output logic AnyHaveReset
+  output logic DebugMode,
+  output logic ResumeAck,    // Signals Hart has been resumed
+  output logic HaveReset,    // Signals Hart has been reset
+  output logic DebugStall    // Stall signal goes to hazard unit
 );
   enum logic {RUNNING, HALTED} State;
-  
-  assign AnyHaveReset = AllHaveReset;
 
   always_ff @(posedge clk) begin
-    if (NdmReset)
-      AllHaveReset <= 1;
+    if (reset)
+      HaveReset <= 1;
     else if (AckHaveReset)
-      AllHaveReset <= 0;
+      HaveReset <= 0;
   end
-  
-
-  assign Halted = DebugStall;
-  assign AllRunning = ~DebugStall;
-  assign AnyRunning = ~DebugStall;
-  assign AllHalted = DebugStall;
-  assign AnyHalted = DebugStall;
-  // BOZO: when sdext is implemented (proper step support is added)
-  //       change ResumeReq to be ignored when HaltReq
-  //       but ResumeReq should still always clear *ResumeAck
-  assign AnyResumeAck = AllResumeAck;
 
+  assign DebugMode = (State != RUNNING); // TODO: update this
   assign DebugStall = (State == HALTED);
 
   always_ff @(posedge clk) begin
-    if (rst)
-      State <= RUNNING;
-    else if (NdmReset)
+    if (reset)
       State <= HaltOnReset ? HALTED : RUNNING;
     else begin
       case (State)
         RUNNING : begin
-          if (HaltReq) begin
-            State <= HALTED;
-          end else if (ResumeReq) begin
-            AllResumeAck <= 0;
-          end
+          State <= Step | HaltReq ? HALTED : RUNNING;
         end
 
         HALTED : begin
-          if (ResumeReq) begin
-            State <= RUNNING;
-            AllResumeAck <= 1;
-          end
+          State <= ResumeReq ? RUNNING : HALTED;
+          ResumeAck <= ResumeReq ? 1 : ResumeAck;
         end
       endcase
     end
diff --git a/src/debug/rad.sv b/src/debug/rad.sv
index 58175f68e..e09b5f4d5 100644
--- a/src/debug/rad.sv
+++ b/src/debug/rad.sv
@@ -106,7 +106,7 @@ module rad import cvw::*; #(parameter cvw_t P) (
       `MIP_REGNO : begin
         ShiftCount = P.LLEN - 1;
         CSRegNo = 1;
-        RegReadOnly = 1; // TODO: eventually DCSR (any maybe others) will be RW
+        //RegReadOnly = 1;
       end
 
       [`HPMCOUNTERBASE_REGNO:`TIME_REGNO],
diff --git a/src/ifu/ifu.sv b/src/ifu/ifu.sv
index 637fa231d..4a9ead7ad 100644
--- a/src/ifu/ifu.sv
+++ b/src/ifu/ifu.sv
@@ -145,7 +145,7 @@ module ifu import cvw::*;  #(parameter cvw_t P) (
   logic [LINELEN-1:0]          FetchBuffer;
   logic [31:0]                 ShiftUncachedInstr;
   // Debug scan chain
-  logic                        DSCR;                                     // Debug Scan Chain Register
+  logic                        DebugScanChainReg;                                     // Debug Scan Chain Register
   
   assign PCFExt = {2'b00, PCSpillF};
 
@@ -411,22 +411,22 @@ module ifu import cvw::*;  #(parameter cvw_t P) (
   if (P.ZICSR_SUPPORTED | P.A_SUPPORTED) begin
     mux2    #(32)     FlushInstrMMux(InstrE, nop, FlushM, NextInstrE);
     if (P.DEBUG_SUPPORTED)
-      flopenrs #(32)     InstrMReg(clk, reset, ~StallM, NextInstrE, InstrM, DebugScanEn, DSCR, DebugScanOut);
+      flopenrs #(32)     InstrMReg(clk, reset, ~StallM, NextInstrE, InstrM, DebugScanEn, DebugScanChainReg, DebugScanOut);
     else
       flopenr #(32)     InstrMReg(clk, reset, ~StallM, NextInstrE, InstrM);
   end else begin
     assign InstrM = '0;
-    assign DebugScanOut = DSCR;
+    assign DebugScanOut = DebugScanChainReg;
   end
   // PCM is only needed with CSRs or branch prediction
   if (P.ZICSR_SUPPORTED | P.BPRED_SUPPORTED)
     if (P.DEBUG_SUPPORTED)
-      flopenrs #(P.XLEN) PCMReg(clk, reset, ~StallM, PCE, PCM, DebugScanEn, DebugScanIn, DSCR);
+      flopenrs #(P.XLEN) PCMReg(clk, reset, ~StallM, PCE, PCM, DebugScanEn, DebugScanIn, DebugScanChainReg);
     else
       flopenr #(P.XLEN) PCMReg(clk, reset, ~StallM, PCE, PCM);
   else begin
     assign PCM = '0;
-    assign DSCR = DebugScanIn;
+    assign DebugScanChainReg = DebugScanIn;
   end
   
   // If compressed instructions are supported, increment PCLink by 2 or 4 for a jal.  Otherwise, just by 4
diff --git a/src/privileged/csr.sv b/src/privileged/csr.sv
index 679bd4170..474dd99ce 100644
--- a/src/privileged/csr.sv
+++ b/src/privileged/csr.sv
@@ -93,6 +93,8 @@ module csr import cvw::*;  #(parameter cvw_t P) (
   output logic [P.XLEN-1:0]        CSRReadValW,               // value read from CSR
   output logic                     IllegalCSRAccessM,         // Illegal CSR access: CSR doesn't exist or is inaccessible at this privilege level
   output logic                     BigEndianM,                // memory access is big-endian based on privilege mode and STATUS register endian fields
+  // Debug Mode output
+  output logic                     Step,
   // Debug scan chain
   input  logic                     DebugSel,
   input  logic [11:0]              DebugRegAddr,
@@ -106,7 +108,7 @@ module csr import cvw::*;  #(parameter cvw_t P) (
   localparam MIP = 12'h344;
   localparam SIP = 12'h144;
   
-  logic [P.XLEN-1:0]       CSRMReadValM, CSRSReadValM, CSRUReadValM, CSRCReadValM;
+  logic [P.XLEN-1:0]       CSRMReadValM, CSRSReadValM, CSRUReadValM, CSRCReadValM, CSRDReadValM;
   logic [P.XLEN-1:0]       CSRReadValM;  
   logic [P.XLEN-1:0]       CSRSrcM;
   logic [P.XLEN-1:0]       CSRRWM, CSRRSM, CSRRCM;  
@@ -116,13 +118,14 @@ module csr import cvw::*;  #(parameter cvw_t P) (
   logic [P.XLEN-1:0]       MEPC_REGW, SEPC_REGW;
   logic [31:0]             MCOUNTINHIBIT_REGW, MCOUNTEREN_REGW, SCOUNTEREN_REGW;
   logic                    WriteMSTATUSM, WriteMSTATUSHM, WriteSSTATUSM;
-  logic                    CSRMWriteM, CSRSWriteM, CSRUWriteM;
+  logic                    CSRWriteDM;
+  logic                    CSRMWriteM, CSRSWriteM, CSRUWriteM, CSRDWriteM;
   logic                    UngatedCSRMWriteM;
   logic                    WriteFRMM, WriteFFLAGSM;
   logic [P.XLEN-1:0]       UnalignedNextEPCM, NextEPCM, NextMtvalM;
   logic [4:0]              NextCauseM;
   logic [11:0]             CSRAdrM, CSRAdrDM;
-  logic                    IllegalCSRCAccessM, IllegalCSRMAccessM, IllegalCSRSAccessM, IllegalCSRUAccessM;
+  logic                    IllegalCSRCAccessM, IllegalCSRMAccessM, IllegalCSRSAccessM, IllegalCSRUAccessM, IllegalCSRDAccessM;
   logic                    InsufficientCSRPrivilegeM;
   logic                    IllegalCSRMWriteReadonlyM;
   logic [P.XLEN-1:0]       CSRReadVal2M;
@@ -137,7 +140,6 @@ module csr import cvw::*;  #(parameter cvw_t P) (
   logic [P.XLEN-1:0]       SENVCFG_REGW;
   logic                    ENVCFG_STCE; // supervisor timer counter enable
   logic                    ENVCFG_FIOM; // fence implies io (presently not used)
-  logic                    CSRMWriteDM;
 
   // only valid unflushed instructions can access CSRs
   assign InstrValidNotFlushedM = InstrValidM & ~StallW & ~FlushW;
@@ -212,10 +214,11 @@ module csr import cvw::*;  #(parameter cvw_t P) (
   assign NextEPCM = P.ZCA_SUPPORTED ? {UnalignedNextEPCM[P.XLEN-1:1], 1'b0} : {UnalignedNextEPCM[P.XLEN-1:2], 2'b00}; // 3.1.15 alignment
   assign NextCauseM = TrapM ? {InterruptM, CauseM}: {CSRWriteValDM[P.XLEN-1], CSRWriteValDM[3:0]};
   assign NextMtvalM = TrapM ? NextFaultMtvalM : CSRWriteValDM;
-  assign UngatedCSRMWriteM = CSRWriteM & (PrivilegeModeW == P.M_MODE);
+  assign UngatedCSRMWriteM = CSRWriteDM & (PrivilegeModeW == P.M_MODE);
   assign CSRMWriteM = UngatedCSRMWriteM & InstrValidNotFlushedM;
-  assign CSRSWriteM = CSRWriteM & (|PrivilegeModeW) & InstrValidNotFlushedM;
-  assign CSRUWriteM = CSRWriteM  & InstrValidNotFlushedM;
+  assign CSRSWriteM = CSRWriteDM & (|PrivilegeModeW) & InstrValidNotFlushedM;
+  assign CSRUWriteM = CSRWriteDM & InstrValidNotFlushedM;
+  assign CSRDWriteM = CSRWriteDM;
   assign MTrapM = TrapM & (NextPrivilegeModeM == P.M_MODE);
   assign STrapM = TrapM & (NextPrivilegeModeM == P.S_MODE) & P.S_SUPPORTED;
 
@@ -283,7 +286,7 @@ module csr import cvw::*;  #(parameter cvw_t P) (
   
   if (P.ZICNTR_SUPPORTED) begin:counters
     csrc #(P) counters(.clk, .reset, .StallE, .StallM, .FlushM,
-      .InstrValidNotFlushedM, .LoadStallD, .StoreStallD, .CSRWriteM, .CSRMWriteM,
+      .InstrValidNotFlushedM, .LoadStallD, .StoreStallD, .CSRWriteM(CSRWriteDM), .CSRMWriteM,
       .BPDirPredWrongM, .BTAWrongM, .RASPredPCWrongM, .IClassWrongM, .BPWrongM,
       .IClassM, .DCacheMiss, .DCacheAccess, .ICacheMiss, .ICacheAccess, .sfencevmaM,
       .InterruptM, .ExceptionM, .InvalidateICacheM, .ICacheStallF, .DCacheStallM, .DivBusyE, .FDivBusyE,
@@ -295,6 +298,16 @@ module csr import cvw::*;  #(parameter cvw_t P) (
     assign IllegalCSRCAccessM = 1'b1; // counters aren't enabled
   end
 
+  if (P.DEBUG_SUPPORTED) begin:csrd
+    csrd #(P) csrd(.clk, .reset,
+    .CSRWriteDM, .CSRAdrM(CSRAdrDM), .CSRWriteValM(CSRWriteValDM), .CSRDReadValM, .IllegalCSRDAccessM,
+    .Step
+    );
+  end else begin
+    assign CSRDReadValM = '0;
+    assign IllegalCSRDAccessM = 1'b1; // Debug isn't supported
+  end
+
    // Broadcast appropriate environment configuration based on privilege mode
   assign ENVCFG_STCE =  MENVCFG_REGW[63]; // supervisor timer counter enable
   assign ENVCFG_PBMTE = MENVCFG_REGW[62]; // page-based memory types enable
@@ -308,7 +321,7 @@ module csr import cvw::*;  #(parameter cvw_t P) (
                                                                        (MENVCFG_REGW[0] & SENVCFG_REGW[0]);
 
   // merge CSR Reads
-  assign CSRReadValM = CSRUReadValM | CSRSReadValM | CSRMReadValM | CSRCReadValM; 
+  assign CSRReadValM = CSRUReadValM | CSRSReadValM | CSRMReadValM | CSRCReadValM | CSRDReadValM; 
   flopenrc #(P.XLEN) CSRValWReg(clk, reset, FlushW, ~StallW, CSRReadValM, CSRReadValW);
 
   // merge illegal accesses: illegal if none of the CSR addresses is legal or privilege is insufficient
@@ -316,19 +329,19 @@ 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 |
+    IllegalCSRSAccessM & IllegalCSRUAccessM /*& IllegalCSRDAccessM*/ |
     InsufficientCSRPrivilegeM) & CSRReadM) | IllegalCSRMWriteReadonlyM;
 
   // Debug module CSR access
   // TODO: should DM be able to access CSRs when hart isn't in M mode?
   if (P.DEBUG_SUPPORTED) begin
     assign CSRAdrDM = DebugSel ? DebugRegAddr : CSRAdrM;
-    //assign CSRMWriteDM = DebugSel ? DebugRegUpdate : CSRMWriteM; // TODO: add write support
+    assign CSRWriteDM = DebugSel ? DebugRegUpdate : CSRWriteM; // TODO: add write support
     assign CSRWriteValDM = DebugSel ? DebugCSRScanVal : CSRWriteValM;
     flopenrs #(P.XLEN) GPScanReg(.clk, .reset, .en(DebugCapture), .d(CSRReadValM), .q(DebugCSRScanVal), .scan(DebugScanEn), .scanin(DebugScanIn), .scanout(DebugScanOut));
   end else begin
     assign CSRAdrDM = CSRAdrM;
-    //assign CSRMWriteDM = CSRMWriteM;
+    assign CSRWriteDM = CSRWriteM;
     assign CSRWriteValDM = CSRWriteValM;
   end
 endmodule
diff --git a/src/privileged/csrd.sv b/src/privileged/csrd.sv
new file mode 100644
index 000000000..c405fdd5e
--- /dev/null
+++ b/src/privileged/csrd.sv
@@ -0,0 +1,99 @@
+///////////////////////////////////////////
+// csrd.sv
+//
+// Written: matthew.n.otto@okstate.edu
+// Created: 13 June 2024
+//
+// Purpose: Debug Control and Status Registers
+//          See RISC-V Debug Specification (4.10)
+//
+// A component of the CORE-V-WALLY configurable RISC-V project.
+// https://github.com/openhwgroup/cvw
+// 
+// Copyright (C) 2021-24 Harvey Mudd College & Oklahoma State University
+//
+// SPDX-License-Identifier: Apache-2.0 WITH SHL-2.1
+//
+// Licensed under the Solderpad Hardware License v 2.1 (the “License”); you may not use this file 
+// except in compliance with the License, or, at your option, the Apache License Version 2.0. You 
+// may obtain a copy of the License at
+//
+// https://solderpad.org/licenses/SHL-2.1/
+//
+// Unless required by applicable law or agreed to in writing, any work distributed under the 
+// License is distributed on an “AS IS” BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, 
+// either express or implied. See the License for the specific language governing permissions 
+// and limitations under the License.
+////////////////////////////////////////////////////////////////////////////////////////////////
+
+module csrd import cvw::*;  #(parameter cvw_t P) (
+  input  logic              clk, reset,
+  input  logic              CSRWriteDM,
+  input  logic [11:0]       CSRAdrM,
+  input  logic [P.XLEN-1:0] CSRWriteValM,
+  output logic [P.XLEN-1:0] CSRDReadValM,
+  output logic              IllegalCSRDAccessM,
+
+  output logic              Step
+);
+  `include "debug.vh"
+
+  localparam DCSR = 12'h7B0;  // Debug Control and Status Register 
+  localparam DPC  = 12'h7B1;  // Debug PC 
+
+  // TODO: these registers are only accessible from Debug Mode.
+  logic [31:0] DCSR_REGW;
+  logic [31:0] DPC_REGW;
+  logic WriteDCSRM;
+  logic WriteDPCM;
+
+  // DCSR fields
+  const logic [3:0] DebugVer = 4;
+  const logic       ebreakVS = 0;
+  const logic       ebreakVU = 0;
+  logic             ebreakM;
+  logic             ebreakS;
+  logic             ebreakU;
+  const logic       StepIE = 0;
+  const logic       StopCount = 0;
+  const logic       StopTime = 0;
+  logic [2:0]       Cause;     // TODO: give reason for entering debug mode
+  const logic       V = 0;
+  const logic       MPrvEn = 0;
+  logic             NMIP;      // pending non-maskable interrupt
+  logic [1:0]       Prv;
+
+
+
+  assign WriteDCSRM = CSRWriteDM & (CSRAdrM == DCSR);
+  assign WriteDPCM  = CSRWriteDM & (CSRAdrM == DPC);
+
+  always_ff @(posedge clk) begin
+    if (reset)
+      Prv <= 3;
+    //else if (Halt) // TODO: trigger when hart enters debug mode
+    //  Prv <= // hart priv mode
+    else if (WriteDCSRM)
+      Prv <= CSRWriteValM[`PRV]; // TODO: overwrite hart privilege mode
+  end
+
+  flopenr ebreakreg(clk, reset, WriteDCSRM, 
+    {CSRWriteValM[`EBREAKM], CSRWriteValM[`EBREAKS], CSRWriteValM[`EBREAKU], CSRWriteValM[`STEP]}, 
+    {ebreakM, ebreakS, ebreakU, Step});
+
+
+  assign DCSR_REGW = {4'b0100, 10'b0, ebreakVS, ebreakVU, ebreakM, 1'b0, ebreakS, ebreakU, StepIE,
+                      StopCount, StopTime, Cause, V, MPrvEn, NMIP, Step, Prv};
+  assign DPC_REGW = {32'hd099f00d};
+
+  always_comb begin
+    CSRDReadValM = 0;
+    IllegalCSRDAccessM = 0;
+    case (CSRAdrM)
+      DCSR   : CSRDReadValM = DCSR_REGW;
+      DPC    : CSRDReadValM = DPC_REGW;
+      default: IllegalCSRDAccessM = 1'b1;
+    endcase
+  end
+
+endmodule
\ No newline at end of file
diff --git a/src/privileged/csrm.sv b/src/privileged/csrm.sv
index 692ead2b2..9bb5a92e5 100644
--- a/src/privileged/csrm.sv
+++ b/src/privileged/csrm.sv
@@ -58,8 +58,6 @@ module csrm  import cvw::*;  #(parameter cvw_t P) (
   logic                            WriteMTVECM, WriteMEDELEGM, WriteMIDELEGM;
   logic                            WriteMSCRATCHM, WriteMEPCM, WriteMCAUSEM, WriteMTVALM;
   logic                            WriteMCOUNTERENM, WriteMCOUNTINHIBITM;
-  logic [31:0]                     DCSR_REGW, DPC_REGW;
-  logic                            WriteDPCM, WriteDCSRM;
 
   // Machine CSRs
   localparam MVENDORID     = 12'hF11;
@@ -91,10 +89,6 @@ module csrm  import cvw::*;  #(parameter cvw_t P) (
   localparam TDATA1        = 12'h7A1;
   localparam TDATA2        = 12'h7A2;
   localparam TDATA3        = 12'h7A3;
-  localparam DCSR          = 12'h7B0;  // Debug Control and Status Register 
-  localparam DPC           = 12'h7B1;  // Debug PC 
-  localparam DSCRATCH0     = 12'h7B2;  // Debug Scratch Register 0 (opt)
-  localparam DSCRATCH1     = 12'h7B3;  // Debug Scratch Register 1 (opt)
   // Constants
   localparam ZERO = {(P.XLEN){1'b0}};
   // when compressed instructions are supported, there can't be misaligned instructions
@@ -132,9 +126,6 @@ module csrm  import cvw::*;  #(parameter cvw_t P) (
 
   // MISA is hardwired.  Spec says it could be written to disable features, but this is not supported by Wally
   assign MISA_REGW = {(P.XLEN == 32 ? 2'b01 : 2'b10), {(P.XLEN-28){1'b0}}, MISA_26[25:0]};
-  // Debug registers (stubbed out)
-  assign DPC_REGW = {32'hd099f00d};
-  assign DCSR_REGW = {32'hdeadbeef};
 
   // MHARTID is hardwired. It only exists as a signal so that the testbench can easily see it.
   assign MHARTID_REGW = '0;
@@ -151,10 +142,8 @@ module csrm  import cvw::*;  #(parameter cvw_t P) (
   assign WriteMTVALM         = MTrapM | (CSRMWriteM & (CSRAdrM == MTVAL));
   assign WriteMCOUNTERENM    = CSRMWriteM & (CSRAdrM == MCOUNTEREN);
   assign WriteMCOUNTINHIBITM = CSRMWriteM & (CSRAdrM == MCOUNTINHIBIT);
-  assign WriteDPCM           = CSRMWriteM & (CSRAdrM == DPC);
-  assign WriteDCSRM          = CSRMWriteM & (CSRAdrM == DCSR);
 
-  assign IllegalCSRMWriteReadonlyM = UngatedCSRMWriteM & (CSRAdrM == MVENDORID | CSRAdrM == MARCHID | CSRAdrM == MIMPID | CSRAdrM == MHARTID | CSRAdrM == MCONFIGPTR); // TODO: add DPC
+  assign IllegalCSRMWriteReadonlyM = UngatedCSRMWriteM & (CSRAdrM == MVENDORID | CSRAdrM == MARCHID | CSRAdrM == MIMPID | CSRAdrM == MHARTID | CSRAdrM == MCONFIGPTR);
 
   // CSRs
   flopenr #(P.XLEN) MTVECreg(clk, reset, WriteMTVECM, {CSRWriteValM[P.XLEN-1:2], 1'b0, CSRWriteValM[0]}, MTVEC_REGW); 
@@ -171,12 +160,6 @@ module csrm  import cvw::*;  #(parameter cvw_t P) (
   if (P.U_SUPPORTED) begin: mcounteren // MCOUNTEREN only exists when user mode is supported
     flopenr #(32)   MCOUNTERENreg(clk, reset, WriteMCOUNTERENM, CSRWriteValM[31:0], MCOUNTEREN_REGW);
   end else assign MCOUNTEREN_REGW = '0;
-  if (P.DEBUG_SUPPORTED) begin
-    //flopenr #(32) DPCreg(clk, reset, WriteDPCM, CSRWriteValM[31:0], DPC_REGW); // TODO: update DPC from PC (M?)
-    //flopenr #(32) DCSRreg(clk, reset, WriteDCSRM, CSRWriteValM[31:0], DCSR_REGW); // TODO: control writes to DCSR
-  end else begin
-    assign {DPC_REGW,DCSR_REGW} = '0;
-  end
 
   // MENVCFG register
   if (P.U_SUPPORTED) begin // menvcfg only exists if there is a lower privilege to control
@@ -253,10 +236,6 @@ module csrm  import cvw::*;  #(parameter cvw_t P) (
       MENVCFGH:      if (P.U_SUPPORTED & P.XLEN==32) CSRMReadValM = MENVCFGH_REGW;
                      else IllegalCSRMAccessM = 1'b1;
       MCOUNTINHIBIT: CSRMReadValM = {{(P.XLEN-32){1'b0}}, MCOUNTINHIBIT_REGW};
-      DCSR:          if (P.DEBUG_SUPPORTED) CSRMReadValM = DCSR_REGW;
-                     else IllegalCSRMAccessM = 1'b1;
-      DPC:           if (P.DEBUG_SUPPORTED) CSRMReadValM = DPC_REGW;
-                     else IllegalCSRMAccessM = 1'b1;
       default:       IllegalCSRMAccessM = 1'b1;
     endcase
   end
diff --git a/src/privileged/privileged.sv b/src/privileged/privileged.sv
index e1fada421..7b1706fe9 100644
--- a/src/privileged/privileged.sv
+++ b/src/privileged/privileged.sv
@@ -97,6 +97,8 @@ module privileged import cvw::*;  #(parameter cvw_t P) (
   output logic              BigEndianM,                                     // Use big endian in current privilege mode
   // Fault outputs                                                         
   output logic              wfiM, IntPendingM,                              // Stall in Memory stage for WFI until interrupt pending or timeout
+  // Debuge Mode
+  output logic              Step,
   // Debug scan chain
   input  logic              DebugSel,
   input  logic [11:0]       DebugRegAddr,
@@ -155,7 +157,7 @@ module privileged import cvw::*;  #(parameter cvw_t P) (
     .SATP_REGW, .PMPCFG_ARRAY_REGW, .PMPADDR_ARRAY_REGW,
     .SetFflagsM, .FRM_REGW, .ENVCFG_CBE, .ENVCFG_PBMTE, .ENVCFG_ADUE,
     .EPCM, .TrapVectorM,
-    .CSRReadValW, .IllegalCSRAccessM, .BigEndianM,
+    .CSRReadValW, .IllegalCSRAccessM, .BigEndianM, .Step,
     .DebugSel, .DebugRegAddr, .DebugCapture, .DebugRegUpdate, .DebugScanEn, .DebugScanIn, .DebugScanOut);
 
   // pipeline early-arriving trap sources
diff --git a/src/wally/wallypipelinedcore.sv b/src/wally/wallypipelinedcore.sv
index 0445cd961..5a75f5770 100644
--- a/src/wally/wallypipelinedcore.sv
+++ b/src/wally/wallypipelinedcore.sv
@@ -45,7 +45,14 @@ module wallypipelinedcore import cvw::*; #(parameter cvw_t P) (
    output logic [3:0]            HPROT,
    output logic [1:0]            HTRANS,
    output logic                  HMASTLOCK,
-   input  logic                  DebugStall,
+   // Debug Mode Control
+   input  logic                  HaltReq,
+   input  logic                  ResumeReq,
+   input  logic                  HaltOnReset,
+   input  logic                  AckHaveReset,
+   output logic                  ResumeAck,
+   output logic                  HaveReset,
+   output logic                  DebugMode,
    // Debug scan chain
    input  logic                  DebugScanEn,    // puts scannable flops into scan mode
    output logic                  DebugScanOut,   // (misc) scan chain data out
@@ -65,6 +72,7 @@ module wallypipelinedcore import cvw::*; #(parameter cvw_t P) (
   logic                          StallF, StallD, StallE, StallM, StallW;
   logic                          FlushD, FlushE, FlushM, FlushW;
   logic                          TrapM, RetM;
+  logic                          DebugStall, Step;
 
   //  signals that must connect through DP
   logic                          IntDivE, W64E;
@@ -184,7 +192,7 @@ module wallypipelinedcore import cvw::*; #(parameter cvw_t P) (
   logic                          wfiM, IntPendingM;
 
   // Debug register scan chain interconnects
-  logic [2:0]                    ScanReg;
+  logic [2:0]                    DebugScanReg;
 
   // instruction fetch unit: PC, branch prediction, instruction cache
   ifu #(P) ifu(.clk, .reset,
@@ -208,7 +216,7 @@ module wallypipelinedcore import cvw::*; #(parameter cvw_t P) (
     .STATUS_MPP, .ENVCFG_PBMTE, .ENVCFG_ADUE, .ITLBWriteF, .sfencevmaM, .ITLBMissF,
     // pmp/pma (inside mmu) signals. 
     .PMPCFG_ARRAY_REGW,  .PMPADDR_ARRAY_REGW, .InstrAccessFaultF, .InstrUpdateDAF,
-    .DebugScanEn(DebugScanEn & MiscSel), .DebugScanIn(ScanReg[0]), .DebugScanOut(ScanReg[1]));
+    .DebugScanEn(DebugScanEn & MiscSel), .DebugScanIn(DebugScanReg[0]), .DebugScanOut(DebugScanReg[1]));
     
   // integer execution unit: integer register file, datapath and controller
   ieu #(P) ieu(.clk, .reset,
@@ -234,7 +242,7 @@ module wallypipelinedcore import cvw::*; #(parameter cvw_t P) (
      .StallD, .StallE, .StallM, .StallW, .FlushD, .FlushE, .FlushM, .FlushW,
      .StructuralStallD, .LoadStallD, .StoreStallD, .PCSrcE,
      .CSRReadM, .CSRWriteM, .PrivilegedM, .CSRWriteFenceM, .InvalidateICacheM,
-     .DebugScanEn, .DebugScanIn(ScanReg[1]), .GPRScanIn(DebugScanIn), .DebugScanOut(ScanReg[2]), .GPRScanOut,
+     .DebugScanEn, .DebugScanIn(DebugScanReg[1]), .GPRScanIn(DebugScanIn), .DebugScanOut(DebugScanReg[2]), .GPRScanOut,
      .MiscSel, .GPRSel, .DebugCapture, .DebugRegUpdate, .DebugRegAddr(DebugRegAddr[4:0]));
 
   lsu #(P) lsu(
@@ -271,7 +279,7 @@ module wallypipelinedcore import cvw::*; #(parameter cvw_t P) (
     .StoreAmoAccessFaultM,        // connects to privilege
     .InstrUpdateDAF,
     .PCSpillF, .ITLBMissF, .PTE, .PageType, .ITLBWriteF, .SelHPTW,
-    .LSUStallM, .DebugCapture, .DebugScanEn(DebugScanEn & MiscSel), .DebugScanIn(ScanReg[2]), .DebugScanOut);                    
+    .LSUStallM, .DebugCapture, .DebugScanEn(DebugScanEn & MiscSel), .DebugScanIn(DebugScanReg[2]), .DebugScanOut);                    
 
   if(P.BUS_SUPPORTED) begin : ebu
     ebu #(P) ebu(// IFU connections
@@ -300,7 +308,17 @@ module wallypipelinedcore import cvw::*; #(parameter cvw_t P) (
     .wfiM, .IntPendingM, .DebugStall,
     // Stall & flush outputs
     .StallF, .StallD, .StallE, .StallM, .StallW,
-    .FlushD, .FlushE, .FlushM, .FlushW);    
+    .FlushD, .FlushE, .FlushM, .FlushW);
+
+  if (P.DEBUG_SUPPORTED) begin
+    dmc debugcontrol(
+      .clk, .reset,
+      .Step, .HaltReq, .ResumeReq, .HaltOnReset, .AckHaveReset,
+      .ResumeAck, .HaveReset, .DebugMode, .DebugStall
+    );
+  end else begin
+    assign DebugStall = 1'b0;
+  end
 
   // privileged unit
   if (P.ZICSR_SUPPORTED) begin:priv
@@ -324,10 +342,10 @@ module wallypipelinedcore import cvw::*; #(parameter cvw_t P) (
       .PrivilegeModeW, .SATP_REGW,
       .STATUS_MXR, .STATUS_SUM, .STATUS_MPRV, .STATUS_MPP, .STATUS_FS, 
       .PMPCFG_ARRAY_REGW, .PMPADDR_ARRAY_REGW, 
-      .FRM_REGW, .ENVCFG_CBE, .ENVCFG_PBMTE, .ENVCFG_ADUE, .wfiM, .IntPendingM, .BigEndianM,
+      .FRM_REGW, .ENVCFG_CBE, .ENVCFG_PBMTE, .ENVCFG_ADUE, .wfiM, .IntPendingM, .BigEndianM, .Step,
       .DebugSel(CSRSel), .DebugRegAddr, .DebugCapture, .DebugRegUpdate, .DebugScanEn(DebugScanEn & CSRSel), .DebugScanIn, .DebugScanOut(CSRScanOut));
     if (P.DEBUG_SUPPORTED) begin
-      flopenrs #(1) scantrapm (.clk, .reset, .en(DebugCapture), .d(TrapM), .q(), .scan(DebugScanEn), .scanin(DebugScanIn), .scanout(ScanReg[0]));
+      flopenrs #(1) scantrapm (.clk, .reset, .en(DebugCapture), .d(TrapM), .q(), .scan(DebugScanEn), .scanin(DebugScanIn), .scanout(DebugScanReg[0]));
     end
   end else begin
     assign {CSRReadValW, PrivilegeModeW,
@@ -336,7 +354,7 @@ module wallypipelinedcore import cvw::*; #(parameter cvw_t P) (
             ENVCFG_CBE, ENVCFG_PBMTE, ENVCFG_ADUE,
             EPCM, TrapVectorM, RetM, TrapM,
             sfencevmaM, BigEndianM, wfiM, IntPendingM} = '0;
-    assign ScanReg[0] = DebugScanIn;
+    assign DebugScanReg[0] = DebugScanIn;
   end
 
   // multiply/divide unit
diff --git a/src/wally/wallypipelinedsoc.sv b/src/wally/wallypipelinedsoc.sv
index c4f104668..a260b32be 100644
--- a/src/wally/wallypipelinedsoc.sv
+++ b/src/wally/wallypipelinedsoc.sv
@@ -73,9 +73,17 @@ module wallypipelinedsoc import cvw::*; #(parameter cvw_t P)  (
   logic [63:0]                 MTIME_CLINT;       // from CLINT to CSRs
   logic                        MExtInt,SExtInt;   // from PLIC
 
-  // Debug Module signals
+  // Debug Mode control signals
   logic                        NdmReset;
+  logic                        HaltReq;
+  logic                        ResumeReq;
+  logic                        HaltOnReset;
+  logic                        AckHaveReset;
+  logic                        ResumeAck;
+  logic                        HaveReset;
+  logic                        DebugMode;
   logic                        DebugStall;
+  // Debug Module signals
   logic                        DebugScanEn;
   logic                        DebugScanIn;
   logic                        GPRScanIn;
@@ -98,7 +106,8 @@ module wallypipelinedsoc import cvw::*; #(parameter cvw_t P)  (
     .MTimerInt, .MExtInt, .SExtInt, .MSwInt, .MTIME_CLINT,
     .HRDATA, .HREADY, .HRESP, .HCLK, .HRESETn, .HADDR, .HWDATA, .HWSTRB,
     .HWRITE, .HSIZE, .HBURST, .HPROT, .HTRANS, .HMASTLOCK,
-    .DebugStall, .DebugScanEn, .DebugScanOut(DebugScanIn), .GPRScanOut(GPRScanIn), .FPRScanOut(FPRScanIn), .CSRScanOut(CSRScanIn), 
+    .HaltReq, .ResumeReq, .HaltOnReset, .AckHaveReset, .ResumeAck, .HaveReset, .DebugMode,
+    .DebugScanEn, .DebugScanOut(DebugScanIn), .GPRScanOut(GPRScanIn), .FPRScanOut(FPRScanIn), .CSRScanOut(CSRScanIn), 
     .DebugScanIn(DebugScanOut), .MiscSel, .GPRSel, .FPRSel, .CSRSel, .DebugRegAddr, .DebugCapture, .DebugRegUpdate);
 
   // instantiate uncore if a bus interface exists
@@ -115,7 +124,8 @@ module wallypipelinedsoc import cvw::*; #(parameter cvw_t P)  (
 
   // instantiate debug module
   if (P.DEBUG_SUPPORTED) begin : dm
-    dm #(P) dm (.clk, .rst(reset), .tck, .tdi, .tms, .tdo, .NdmReset, .DebugStall,
+    dm #(P) dm (.clk, .rst(reset), .tck, .tdi, .tms, .tdo, .NdmReset,
+      .HaltReq, .ResumeReq, .HaltOnReset, .AckHaveReset, .ResumeAck, .HaveReset, .DebugMode,
       .DebugScanEn, .DebugScanIn, .GPRScanIn, .FPRScanIn, .CSRScanIn, .DebugScanOut,
       .MiscSel, .GPRSel, .FPRSel, .CSRSel, .RegAddr(DebugRegAddr), .DebugCapture, .DebugRegUpdate);
   end