core: add basic debug mode features (#918)

Basic features of debug mode are implemented.

* Rewrite CSR for debug mode
* Peripheral work for implementing debug module
* Added single step support
* Use difftest with JTAG support

difftest @ 1878daec

-Subproject commit 526dfba2b15ea73ab65d8e7ce77255d62e440b50
+Subproject commit 1878daec7a3a506fc75230b29c4bda4a37e6e75e

src/main/scala/device/RocketDebugWrapper.scala

+/***************************************************************************************
+* Copyright (c) 2020-2021 Institute of Computing Technology, Chinese Academy of Sciences
+* Copyright (c) 2020-2021 Peng Cheng Laboratory
+*
+* XiangShan is licensed under Mulan PSL v2.
+* You can use this software according to the terms and conditions of the Mulan PSL v2.
+* You may obtain a copy of Mulan PSL v2 at:
+*          http://license.coscl.org.cn/MulanPSL2
+*
+* THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
+* EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
+* MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
+*
+* See the Mulan PSL v2 for more details.
+***************************************************************************************/
+
+package device
+
+import chisel3._
+import xiangshan._
+import chisel3.experimental.{IntParam, noPrefix}
+import chisel3.util._
+import chisel3.util.HasBlackBoxResource
+import freechips.rocketchip.config.{Field, Parameters}
+import freechips.rocketchip.subsystem._
+import freechips.rocketchip.amba.apb._
+import freechips.rocketchip.diplomacy._
+import freechips.rocketchip.diplomaticobjectmodel.logicaltree.LogicalModuleTree
+import freechips.rocketchip.jtag._
+import freechips.rocketchip.util._
+import freechips.rocketchip.prci.{ClockSinkParameters, ClockSinkNode}
+import freechips.rocketchip.tilelink._
+import freechips.rocketchip.devices.debug.{JtagDTMConfig, TLDebugModule, DebugCustomXbar, ResetCtrlIO, DebugIO, SystemJTAGIO, DebugTransportModuleJTAG, PSDIO}
+import freechips.rocketchip.diplomaticobjectmodel.logicaltree.GenericLogicalTreeNode
+import freechips.rocketchip.devices.debug._
+
+// this file uses code from rocketchip Periphery.scala
+// to simplify the code we remove options for apb, cjtag and dmi
+// this module creates wrapped dm and dtm 
+
+class DebugModule(numCores: Int)(implicit p: Parameters) extends LazyModule {
+
+  val debug = LazyModule(new TLDebugModule(8)(p))
+
+//  debug.node := TLFragmenter() := peripheralXbar
+  val debugCustomXbarOpt = p(DebugModuleKey).map(params => LazyModule( new DebugCustomXbar(outputRequiresInput = false)))
+  debug.dmInner.dmInner.customNode := debugCustomXbarOpt.get.node
+
+//  debug.dmInner.dmInner.sb2tlOpt.foreach { sb2tl  =>
+//    l2xbar := TLBuffer() := TLWidthWidget(1) := sb2tl.node
+//  }
+  val fakeTreeNode = new GenericLogicalTreeNode
+  LogicalModuleTree.add(fakeTreeNode, debug.logicalTreeNode)
+
+  lazy val module = new LazyRawModuleImp(this) {
+    val io = IO(new Bundle{
+      val resetCtrl = new ResetCtrlIO(numCores)(p)
+      val debugIO = new DebugIO()(p)
+      val clock = Input(Bool())
+      val reset = Input(Bool())
+    })
+    debug.module.io.tl_reset := io.reset // this should be TL reset
+    debug.module.io.tl_clock := io.clock.asClock // this should be TL clock
+    debug.module.io.hartIsInReset := io.resetCtrl.hartIsInReset
+    io.resetCtrl.hartResetReq.foreach { rcio => debug.module.io.hartResetReq.foreach { rcdm => rcio := rcdm }}
+
+    io.debugIO.clockeddmi.foreach { dbg => debug.module.io.dmi.get <> dbg } // not connected in current case since we use dtm
+    debug.module.io.debug_reset := io.debugIO.reset
+    debug.module.io.debug_clock := io.debugIO.clock
+    io.debugIO.ndreset := debug.module.io.ctrl.ndreset
+    io.debugIO.dmactive := debug.module.io.ctrl.dmactive
+    debug.module.io.ctrl.dmactiveAck := io.debugIO.dmactiveAck
+    io.debugIO.extTrigger.foreach { x => debug.module.io.extTrigger.foreach {y => x <> y}}
+    debug.module.io.ctrl.debugUnavail.foreach { _ := false.B }
+
+    val dtm = io.debugIO.systemjtag.map(instantiateJtagDTM(_))
+
+    def instantiateJtagDTM(sj: SystemJTAGIO): DebugTransportModuleJTAG = {
+      val c = new JtagDTMKeyDefault
+      val dtm = Module(new DebugTransportModuleJTAG(p(DebugModuleKey).get.nDMIAddrSize, c))
+      dtm.io.jtag <> sj.jtag
+
+      io.debugIO.disableDebug.foreach { x => dtm.io.jtag.TMS := sj.jtag.TMS | x }  // force TMS high when debug is disabled
+
+      dtm.io.jtag_clock  := sj.jtag.TCK
+      dtm.io.jtag_reset  := sj.reset
+      dtm.io.jtag_mfr_id := sj.mfr_id
+      dtm.io.jtag_part_number := sj.part_number
+      dtm.io.jtag_version := sj.version
+      dtm.rf_reset := sj.reset
+      debug.module.io.dmi.get.dmi <> dtm.io.dmi
+      debug.module.io.dmi.get.dmiClock := sj.jtag.TCK
+      debug.module.io.dmi.get.dmiReset := sj.reset
+      dtm
+    }
+  }
+}
+
+object XSDebugModuleParams {
+
+  def apply(xlen:Int /*TODO , val configStringAddr: Int*/): DebugModuleParams = {
+    new DebugModuleParams().copy(
+      nAbstractDataWords   = (if (xlen == 32) 1 else if (xlen == 64) 2 else 4),
+      maxSupportedSBAccess = xlen,
+      hasBusMaster = true,
+      baseAddress = BigInt(0x38020000),
+      nScratch = 2
+    )
+  }
+}
+
+case object EnableJtag extends Field[Bool]
+
+class SimJTAG(tickDelay: Int = 50)(implicit val p: Parameters) extends BlackBox(Map("TICK_DELAY" -> IntParam(tickDelay)))
+  with HasBlackBoxResource {
+  val io = IO(new Bundle { 
+    val clock = Input(Clock())
+    val reset = Input(Bool())
+    val jtag = new JTAGIO(hasTRSTn = true)
+    val enable = Input(Bool())
+    val init_done = Input(Bool())
+    val exit = Output(UInt(32.W))
+  })
+
+  def connect(dutio: JTAGIO, tbclock: Clock, tbreset: Bool, init_done: Bool, tbsuccess: Bool) = {
+    dutio.TCK := io.jtag.TCK
+    dutio.TMS := io.jtag.TMS
+    dutio.TDI := io.jtag.TDI
+    io.jtag.TDO := dutio.TDO
+
+    io.clock := tbclock
+    io.reset := tbreset
+
+    io.enable    := p(EnableJtag)
+    io.init_done := init_done
+
+    // Success is determined by the gdbserver
+    // which is controlling this simulation.
+    tbsuccess := io.exit === 1.U
+    when (io.exit >= 2.U) {
+      printf("*** FAILED *** (exit code = %d)\n", io.exit >> 1.U)
+      stop(1)
+    }
+  }
+}

src/main/scala/top/Configs.scala

@@ -23,11 +23,14 @@ import utils._
 import system._
 import chipsalliance.rocketchip.config._
 import freechips.rocketchip.tile.{BusErrorUnit, BusErrorUnitParams, XLen}
-import sifive.blocks.inclusivecache.{CacheParameters, InclusiveCache, InclusiveCacheMicroParameters}
+import freechips.rocketchip.devices.debug._
+import freechips.rocketchip.tile.MaxHartIdBits
+import sifive.blocks.inclusivecache.{InclusiveCache, InclusiveCacheMicroParameters, CacheParameters}
 import xiangshan.backend.dispatch.DispatchParameters
 import xiangshan.backend.exu.ExuParameters
 import xiangshan.cache.{DCacheParameters, ICacheParameters, L1plusCacheParameters}
 import xiangshan.cache.prefetch.{BOPParameters, L1plusPrefetcherParameters, L2PrefetcherParameters, StreamPrefetchParameters}
+import device.{XSDebugModuleParams, EnableJtag}
 
 class DefaultConfig(n: Int) extends Config((site, here, up) => {
   case XLen => 64
@@ -35,6 +38,11 @@ class DefaultConfig(n: Int) extends Config((site, here, up) => {
   case SoCParamsKey => SoCParameters(
     cores = List.tabulate(n){ i => XSCoreParameters(HartId = i) }
   )
+  case ExportDebug => DebugAttachParams(protocols = Set(JTAG))
+  case DebugModuleKey => Some(XSDebugModuleParams(site(XLen)))
+  case JtagDTMKey => JtagDTMKey
+  case MaxHartIdBits => 2
+  case EnableJtag => false.B
 })
 
 // Synthesizable minimal XiangShan

src/main/scala/top/Top.scala

@@ -21,9 +21,10 @@ import chisel3.util._
 import xiangshan._
 import utils._
 import system._
+import device._
 import chisel3.stage.ChiselGeneratorAnnotation
 import chipsalliance.rocketchip.config._
-import device.{AXI4Plic, TLTimer}
+import device.{AXI4Plic, TLTimer, DebugModule}
 import firrtl.stage.RunFirrtlTransformAnnotation
 import freechips.rocketchip.diplomacy._
 import freechips.rocketchip.tilelink._
@@ -32,12 +33,15 @@ import freechips.rocketchip.devices.tilelink._
 import freechips.rocketchip.diplomaticobjectmodel.logicaltree.GenericLogicalTreeNode
 import freechips.rocketchip.interrupts._
 import freechips.rocketchip.stage.phases.GenerateArtefacts
+import freechips.rocketchip.jtag.JTAGIO
 import freechips.rocketchip.tile.{BusErrorUnit, BusErrorUnitParams, XLen}
 import freechips.rocketchip.util.{ElaborationArtefacts, HasRocketChipStageUtils}
+import freechips.rocketchip.devices.debug.{DebugIO, ResetCtrlIO}
 import sifive.blocks.inclusivecache.{CacheParameters, InclusiveCache, InclusiveCacheMicroParameters}
 import xiangshan.cache.prefetch.L2Prefetcher
 
 
+
 class XSCoreWithL2()(implicit p: Parameters) extends LazyModule
   with HasXSParameter with HasSoCParameter {
   private val core = LazyModule(new XSCore)
@@ -208,7 +212,7 @@ trait HaveAXI4MemPort {
 
 
 trait HaveAXI4PeripheralPort { this: BaseXSSoc =>
-  // on-chip devices: 0x3800_000 - 0x3fff_ffff
+  // on-chip devices: 0x3800_0000 - 0x3fff_ffff 0x0000_0000 - 0x0000_0fff
   val onChipPeripheralRange = AddressSet(0x38000000L, 0x07ffffffL)
   val uartRange = AddressSet(0x40600000, 0xf)
   val uartDevice = new SimpleDevice("serial", Seq("xilinx,uartlite"))
@@ -356,6 +360,14 @@ class XSTopWithoutDMA()(implicit p: Parameters) extends BaseXSSoc()
     bankedNode :*= l3cache.node :*= TLBuffer() :*= l3_xbar
   }
 
+  val debugModule = LazyModule(new DebugModule(NumCores)(p))
+  debugModule.debug.node := peripheralXbar
+  val debugIntSink = LazyModule(new IntSinkNodeToModule(NumCores))
+  debugIntSink.sinkNode := debugModule.debug.dmOuter.dmOuter.intnode
+  debugModule.debug.dmInner.dmInner.sb2tlOpt.foreach { sb2tl  =>
+    l3_xbar := TLBuffer() := TLWidthWidget(1) := sb2tl.node
+  }
+
   lazy val module = new LazyRawModuleImp(this) {
     ElaborationArtefacts.add("dts", dts)
     ElaborationArtefacts.add("graphml", graphML)
@@ -368,13 +380,21 @@ class XSTopWithoutDMA()(implicit p: Parameters) extends BaseXSSoc()
       val extIntrs = Input(UInt(NrExtIntr.W))
       // val meip = Input(Vec(NumCores, Bool()))
       val ila = if(debugOpts.FPGAPlatform && EnableILA) Some(Output(new ILABundle)) else None
+      val systemjtag = new Bundle {
+        val jtag = Flipped(new JTAGIO(hasTRSTn = false))
+        val reset = Input(Bool()) // No reset allowed on top
+        val mfr_id = Input(UInt(11.W))
+        val part_number = Input(UInt(16.W))
+        val version = Input(UInt(4.W))
+      }
+      // val resetCtrl = new ResetCtrlIO(NumCores)(p)
     })
     childClock := io.clock.asClock()
 
     withClockAndReset(childClock, io.reset) {
       val resetGen = Module(new ResetGen(1, !debugOpts.FPGAPlatform))
       resetGen.suggestName("top_reset_gen")
-      childReset := resetGen.io.out
+      childReset := resetGen.io.out | debugModule.module.io.debugIO.ndreset
     }
 
     withClockAndReset(childClock, childReset) {
@@ -388,6 +408,7 @@ class XSTopWithoutDMA()(implicit p: Parameters) extends BaseXSSoc()
         core_with_l2(i).module.io.externalInterrupt.msip := clintIntSinks(i).module.out(0)
         core_with_l2(i).module.io.externalInterrupt.mtip := clintIntSinks(i).module.out(1)
         core_with_l2(i).module.io.externalInterrupt.meip := plicIntSinks(i).module.out(0)
+        core_with_l2(i).module.io.externalInterrupt.debug := debugIntSink.module.out(i)
         beu.module.io.errors.l1plus(i) := core_with_l2(i).module.io.l1plus_error
         beu.module.io.errors.icache(i) := core_with_l2(i).module.io.icache_error
         beu.module.io.errors.dcache(i) := core_with_l2(i).module.io.dcache_error
@@ -404,6 +425,22 @@ class XSTopWithoutDMA()(implicit p: Parameters) extends BaseXSSoc()
       val tick = cnt === 0.U
       cnt := Mux(tick, freq.U, cnt - 1.U)
       clint.module.io.rtcTick := tick
+
+      debugModule.module.io.resetCtrl.hartIsInReset.foreach {x => x := childReset.asBool() }
+      debugModule.module.io.clock := io.clock
+      debugModule.module.io.reset := io.reset
+
+      debugModule.module.io.debugIO.reset := io.systemjtag.reset // TODO: use synchronizer?
+      debugModule.module.io.debugIO.clock := childClock
+      debugModule.module.io.debugIO.dmactiveAck  := debugModule.module.io.debugIO.dmactive // TODO: delay 3 cycles?
+      // jtag connector
+      debugModule.module.io.debugIO.systemjtag.foreach { x =>
+        x.jtag <> io.systemjtag.jtag
+        x.reset  := io.systemjtag.reset
+        x.mfr_id := io.systemjtag.mfr_id
+        x.part_number := io.systemjtag.part_number
+        x.version := io.systemjtag.version
+      }
     }
   }
 }

src/main/scala/xiangshan/Bundle.scala

@@ -286,6 +286,7 @@ class CtrlSignals(implicit p: Parameters) extends XSBundle {
   val commitType = CommitType()
   val fpu = new FPUCtrlSignals
   val isMove = Bool()
+  val singleStep = Bool()
 
   def decode(inst: UInt, table: Iterable[(BitPat, List[BitPat])]) = {
     val decoder = freechips.rocketchip.rocket.DecodeLogic(inst, XDecode.decodeDefault, table)
@@ -406,6 +407,7 @@ class ExternalInterruptIO(implicit p: Parameters) extends XSBundle {
   val mtip = Input(Bool())
   val msip = Input(Bool())
   val meip = Input(Bool())
+  val debug = Input(Bool())
 }
 
 class CSRSpecialIO(implicit p: Parameters) extends XSBundle {

src/main/scala/xiangshan/PMA.scala

@@ -48,7 +48,8 @@ object AddressSpace {
     MemMap("h00_3112_0000", "h00_37FF_FFFF",   "h0", "Reserved",    "RW"),
     MemMap("h00_3800_0000", "h00_3800_FFFF",   "h0", "CLINT",       "RW"),
     MemMap("h00_3801_0000", "h00_3801_FFFF",   "h0", "BEU",         "RW"),
-    MemMap("h00_3802_0000", "h00_3BFF_FFFF",   "h0", "Reserved",    ""),
+    MemMap("h00_3802_0000", "h00_3802_0FFF",   "h0", "DebugModule",    "RWX"),
+    MemMap("h00_3802_1000", "h00_3BFF_FFFF",   "h0", "Reserved",    ""),
     MemMap("h00_3C00_0000", "h00_3FFF_FFFF",   "h0", "PLIC",        "RW"),
     MemMap("h00_4000_0000", "h00_7FFF_FFFF",   "h0", "PCIe",        "RW"),
     MemMap("h00_8000_0000", "h1F_FFFF_FFFF",   "h0", "DDR",         "RWXIDSA"),
@@ -56,7 +57,8 @@ object AddressSpace {
 
   def FullMemMapList = List(
     //     Base address      Top address       Width  Description    Mode (RWXIDSAC)
-    MemMap("h00_0000_0000", "h00_0FFF_FFFF",   "h0", "Reserved",    ""),
+    MemMap("h00_0000_0000", "h00_0000_0FFF",   "h0", "DebugModule",    "RWX"),
+    MemMap("h00_0000_1000", "h00_0FFF_FFFF",   "h0", "Reserved",    ""),
     MemMap("h00_1000_0000", "h00_1FFF_FFFF",   "h0", "QSPI_Flash",  "RX"),
     MemMap("h00_2000_0000", "h00_2FFF_FFFF",   "h0", "Reserved",    ""),
     MemMap("h00_3000_0000", "h00_3000_FFFF",   "h0", "DMA",         "RW"),

src/main/scala/xiangshan/Parameters.scala

@@ -25,6 +25,7 @@ import xiangshan.backend.fu.fpu._
 import xiangshan.backend.dispatch.DispatchParameters
 import xiangshan.cache.{DCacheParameters, ICacheParameters, L1plusCacheParameters}
 import xiangshan.cache.prefetch.{BOPParameters, L1plusPrefetcherParameters, L2PrefetcherParameters, StreamPrefetchParameters}
+import freechips.rocketchip.diplomacy.AddressSet
 
 case object XSCoreParamsKey extends Field[XSCoreParameters]
 

src/main/scala/xiangshan/backend/CtrlBlock.scala

@@ -337,6 +337,7 @@ class CtrlBlock(implicit p: Parameters) extends XSModule
   dispatch.io.flush := flushReg
   dispatch.io.enqRoq <> roq.io.enq
   dispatch.io.enqLsq <> io.enqLsq
+  dispatch.io.singleStep := false.B
   dispatch.io.allocPregs.zipWithIndex.foreach { case (preg, i) =>
     intBusyTable.io.allocPregs(i).valid := preg.isInt
     fpBusyTable.io.allocPregs(i).valid := preg.isFp

src/main/scala/xiangshan/backend/decode/DecodeUnit.scala

@@ -166,9 +166,11 @@ object XDecode extends DecodeConstants {
     CSRRCI  -> List(SrcType.reg, SrcType.imm, SrcType.DC, FuType.csr, CSROpType.clri, Y, N, N, Y, Y, N, N, SelImm.IMM_Z),
 
     SFENCE_VMA->List(SrcType.reg, SrcType.imm, SrcType.DC, FuType.fence, FenceOpType.sfence, N, N, N, Y, Y, Y, N, SelImm.IMM_X),
+    EBREAK  -> List(SrcType.reg, SrcType.imm, SrcType.DC, FuType.csr, CSROpType.jmp, Y, N, N, Y, Y, N, N, SelImm.IMM_I),
     ECALL   -> List(SrcType.reg, SrcType.imm, SrcType.DC, FuType.csr, CSROpType.jmp, Y, N, N, Y, Y, N, N, SelImm.IMM_I),
     SRET    -> List(SrcType.reg, SrcType.imm, SrcType.DC, FuType.csr, CSROpType.jmp, Y, N, N, Y, Y, N, N, SelImm.IMM_I),
     MRET    -> List(SrcType.reg, SrcType.imm, SrcType.DC, FuType.csr, CSROpType.jmp, Y, N, N, Y, Y, N, N, SelImm.IMM_I),
+    DRET    -> List(SrcType.reg, SrcType.imm, SrcType.DC, FuType.csr, CSROpType.jmp, Y, N, N, Y, Y, N, N, SelImm.IMM_I),
 
     WFI     -> List(SrcType.pc, SrcType.imm, SrcType.DC, FuType.alu, ALUOpType.sll, Y, N, N, N, N, N, N, SelImm.IMM_X),
 
@@ -495,6 +497,7 @@ class DecodeUnit(implicit p: Parameters) extends XSModule with DecodeUnitConstan
   // output
   cf_ctrl.cf := ctrl_flow
   val cs = Wire(new CtrlSignals()).decode(ctrl_flow.instr, decode_table)
+  cs.singleStep := false.B
 
   val fpDecoder = Module(new FPDecoder)
   fpDecoder.io.instr := ctrl_flow.instr

src/main/scala/xiangshan/backend/dispatch/Dispatch.scala

@@ -69,6 +69,8 @@ class Dispatch(implicit p: Parameters) extends XSModule {
       val fpdqFull  = Output(Bool())
       val lsdqFull  = Output(Bool())
     }
+    // From CSR: to control single step execution
+    val singleStep = Input(Bool())
   })
 
   val dispatch1 = Module(new Dispatch1)
@@ -97,6 +99,7 @@ class Dispatch(implicit p: Parameters) extends XSModule {
   dispatch1.io.storeIssue <> io.storeIssue
   dispatch1.io.redirect <> io.redirect
   dispatch1.io.flush <> io.flush
+  dispatch1.io.singleStep := io.singleStep
 
   // dispatch queue: queue uops and dispatch them to different reservation stations or issue queues
   // it may cancel the uops

src/main/scala/xiangshan/backend/dispatch/Dispatch1.scala

@@ -70,6 +70,8 @@ class Dispatch1(implicit p: Parameters) extends XSModule with HasExceptionNO {
     val csrCtrl = Input(new CustomCSRCtrlIO)
     // LFST state sync
     val storeIssue = Vec(StorePipelineWidth, Flipped(Valid(new ExuInput)))
+    // singleStep
+    val singleStep = Input(Bool())
   })
 
 
@@ -104,8 +106,15 @@ class Dispatch1(implicit p: Parameters) extends XSModule with HasExceptionNO {
 
   /**
     * Part 2:
-    *   Update commitType, psrc(0), psrc(1), psrc(2), old_pdest, roqIdx, lqIdx, sqIdx for the uops
+    *   Update commitType, psrc(0), psrc(1), psrc(2), old_pdest, roqIdx, lqIdx, sqIdx and singlestep for the uops
     */
+
+  val singleStepStatus = RegInit(false.B)
+  when (io.flush) {
+    singleStepStatus := false.B
+  }.elsewhen (io.singleStep && io.fromRename(0).fire()) {
+    singleStepStatus := true.B
+  }
   val updatedUop = Wire(Vec(RenameWidth, new MicroOp))
   val updatedCommitType = Wire(Vec(RenameWidth, CommitType()))
   val updatedPsrc1 = Wire(Vec(RenameWidth, UInt(PhyRegIdxWidth.W)))
@@ -182,6 +191,9 @@ class Dispatch1(implicit p: Parameters) extends XSModule with HasExceptionNO {
     io.lfst(i).bits.roqIdx := updatedUop(i).roqIdx
     io.lfst(i).bits.sqIdx := updatedUop(i).sqIdx
     io.lfst(i).bits.ssid := updatedUop(i).cf.ssid
+
+    // update singleStep
+    updatedUop(i).ctrl.singleStep := io.singleStep && (if (i == 0) singleStepStatus else true.B)
   }
 
   // store set perf count

src/main/scala/xiangshan/backend/fu/CSR.scala

@@ -42,8 +42,11 @@ trait HasExceptionNO {
   def loadPageFault       = 13
   def storePageFault      = 15
 
+  def singleStep          = 14
+
   val ExcPriority = Seq(
     breakPoint, // TODO: different BP has different priority
+    singleStep,
     instrPageFault,
     instrAccessFault,
     illegalInstr,
@@ -176,6 +179,9 @@ class CSRFileIO(implicit p: Parameters) extends XSBundle {
   val externalInterrupt = new ExternalInterruptIO
   // TLB
   val tlb = Output(new TlbCsrBundle)
+  // Debug Mode
+  val singleStep = Output(Bool())
+  val debugMode = Output(Bool())
   // Custom microarchiture ctrl signal
   val customCtrl = Output(new CustomCSRCtrlIO)
   // to Fence to disable sfence
@@ -209,6 +215,23 @@ class CSR(implicit p: Parameters) extends FunctionUnit with HasCSRConst
 
   val csrNotImplemented = RegInit(UInt(XLEN.W), 0.U)
 
+  class DcsrStruct extends Bundle {
+    val xdebugver = Output(UInt(2.W))
+    val zero4 = Output(UInt(2.W))
+    val zero3 = Output(UInt(12.W))
+    val ebreakm = Output(Bool())
+    val ebreakh = Output(Bool())
+    val ebreaks = Output(Bool())
+    val ebreaku = Output(Bool())
+    val zero2 = Output(Bool())
+    val stopcycle = Output(Bool())
+    val stoptime = Output(Bool())
+    val cause = Output(UInt(3.W))
+    val zero1 = Output(UInt(3.W))
+    val step = Output(Bool())
+    val prv = Output(UInt(2.W))
+  }
+
   class MstatusStruct extends Bundle {
     val sd = Output(UInt(1.W))
 
@@ -240,11 +263,52 @@ class CSR(implicit p: Parameters) extends FunctionUnit with HasCSRConst
   }
 
   class Interrupt extends Bundle {
+//  val d = Output(Bool())    // Debug
     val e = new Priv
     val t = new Priv
     val s = new Priv
   }
 
+  // Debug CSRs
+  val dcsr = RegInit(UInt(32.W), 0x4000b010.U)
+  val dpc = Reg(UInt(64.W))
+  val dscratch = Reg(UInt(64.W))
+  val dscratch1 = Reg(UInt(64.W))
+  val debugMode = RegInit(false.B)
+  val debugIntrEnable = RegInit(true.B)
+  csrio.debugMode := debugMode
+
+  val dpcPrev = RegNext(dpc)
+  XSDebug(dpcPrev =/= dpc, "Debug Mode: dpc is altered! Current is %x, previous is %x.", dpc, dpcPrev)
+
+// dcsr value table
+// | debugver | 0100
+// | zero     | 10 bits of 0
+// | ebreakvs | 0
+// | ebreakvu | 0
+// | ebreakm  | 1 if ebreak enters debug
+// | zero     | 0
+// | ebreaks  |
+// | ebreaku  |
+// | stepie   | 0 disable interrupts in singlestep
+// | stopcount| stop counter, 0
+// | stoptime | stop time, 0
+// | cause    | 3 bits read only
+// | v        | 0
+// | mprven   | 1
+// | nmip     | read only
+// | step     |
+// | prv      | 2 bits
+  val dcsrData = Wire(new DcsrStruct)
+  dcsrData := dcsr.asTypeOf(new DcsrStruct)
+  val dcsrMask = GenMask(15) | GenMask(13, 11) | GenMask(2, 0)// Dcsr write mask
+  def dcsrUpdateSideEffect(dcsr: UInt): UInt = {
+    val dcsrOld = WireInit(dcsr.asTypeOf(new DcsrStruct))
+    val dcsrNew = dcsr | (dcsrOld.prv(0) | dcsrOld.prv(1)).asUInt // turn 10 priv into 11
+    dcsrNew
+  }
+  csrio.singleStep := dcsrData.step
+
   // Machine-Level CSRs
 
   val mtvec = RegInit(UInt(XLEN.W), 0.U)
@@ -560,6 +624,12 @@ class CSR(implicit p: Parameters) extends FunctionUnit with HasCSRConst
     MaskedRegMap(Mcause, mcause),
     MaskedRegMap(Mtval, mtval),
     MaskedRegMap(Mip, mip.asUInt, 0.U, MaskedRegMap.Unwritable),
+
+    //--- Debug Mode ---
+    MaskedRegMap(Dcsr, dcsr, dcsrMask, dcsrUpdateSideEffect),
+    MaskedRegMap(Dpc, dpc),
+    MaskedRegMap(Dscratch, dscratch),
+    MaskedRegMap(Dscratch1, dscratch1)
   )
 
   // PMP is unimplemented yet
@@ -675,6 +745,7 @@ class CSR(implicit p: Parameters) extends FunctionUnit with HasCSRConst
   val isMret   = addr === privMret   && func === CSROpType.jmp
   val isSret   = addr === privSret   && func === CSROpType.jmp
   val isUret   = addr === privUret   && func === CSROpType.jmp
+  val isDret   = addr === privDret   && func === CSROpType.jmp
 
   XSDebug(wen, "csr write: pc %x addr %x rdata %x wdata %x func %x\n", cfIn.pc, addr, rdata, wdata, func)
   XSDebug(wen, "pc %x mstatus %x mideleg %x medeleg %x mode %x\n", cfIn.pc, mstatus, mideleg , medeleg, priviledgeMode)
@@ -724,6 +795,21 @@ class CSR(implicit p: Parameters) extends FunctionUnit with HasCSRConst
   retTarget := DontCare
   // val illegalEret = TODO
 
+  when (valid && isDret) {
+    val mstatusOld = WireInit(mstatus.asTypeOf(new MstatusStruct))
+    val mstatusNew = WireInit(mstatus.asTypeOf(new MstatusStruct))
+    val dcsrNew = WireInit(dcsr.asTypeOf(new DcsrStruct))
+    val debugModeNew = WireInit(debugMode)
+    when (dcsr.asTypeOf(new DcsrStruct).prv =/= ModeM) {mstatusNew.mprv := 0.U} //If the new privilege mode is less privileged than M-mode, MPRV in mstatus is cleared.
+    mstatus := mstatusNew.asUInt
+    priviledgeMode := dcsrNew.prv
+    retTarget := dpc(VAddrBits-1, 0)
+    debugModeNew := false.B
+    debugIntrEnable := true.B
+    debugMode := debugModeNew
+    XSDebug("Debug Mode: Dret executed, returning to %x.", retTarget)
+  }
+
   when (valid && isMret) {
     val mstatusOld = WireInit(mstatus.asTypeOf(new MstatusStruct))
     val mstatusNew = WireInit(mstatus.asTypeOf(new MstatusStruct))
@@ -764,8 +850,10 @@ class CSR(implicit p: Parameters) extends FunctionUnit with HasCSRConst
   io.in.ready := true.B
   io.out.valid := valid
 
+  val ebreakCauseException = (priviledgeMode === ModeM && dcsrData.ebreakm) || (priviledgeMode === ModeS && dcsrData.ebreaks) || (priviledgeMode === ModeU && dcsrData.ebreaku)
+
   val csrExceptionVec = WireInit(cfIn.exceptionVec)
-  csrExceptionVec(breakPoint) := io.in.valid && isEbreak
+  csrExceptionVec(breakPoint) := io.in.valid && isEbreak && ebreakCauseException
   csrExceptionVec(ecallM) := priviledgeMode === ModeM && io.in.valid && isEcall
   csrExceptionVec(ecallS) := priviledgeMode === ModeS && io.in.valid && isEcall
   csrExceptionVec(ecallU) := priviledgeMode === ModeU && io.in.valid && isEcall
@@ -782,10 +870,12 @@ class CSR(implicit p: Parameters) extends FunctionUnit with HasCSRConst
   def priviledgedEnableDetect(x: Bool): Bool = Mux(x, ((priviledgeMode === ModeS) && mstatusStruct.ie.s) || (priviledgeMode < ModeS),
     ((priviledgeMode === ModeM) && mstatusStruct.ie.m) || (priviledgeMode < ModeM))
 
+  val debugIntr = csrio.externalInterrupt.debug & debugIntrEnable
+  XSDebug(debugIntr, "Debug Mode: debug interrupt is asserted and valid!")
   // send interrupt information to ROQ
   val intrVecEnable = Wire(Vec(12, Bool()))
   intrVecEnable.zip(ideleg.asBools).map{case(x,y) => x := priviledgedEnableDetect(y)}
-  val intrVec = mie(11,0) & mip.asUInt & intrVecEnable.asUInt
+  val intrVec = Cat(debugIntr, (mie(11,0) & mip.asUInt & intrVecEnable.asUInt))
   val intrBitSet = intrVec.orR()
   csrio.interrupt := intrBitSet
   mipWire.t.m := csrio.externalInterrupt.mtip
@@ -797,6 +887,7 @@ class CSR(implicit p: Parameters) extends FunctionUnit with HasCSRConst
   val intrNO = IntPriority.foldRight(0.U)((i: Int, sum: UInt) => Mux(intrVec(i), i.U, sum))
   val raiseIntr = csrio.exception.valid && csrio.exception.bits.isInterrupt
   XSDebug(raiseIntr, "interrupt: pc=0x%x, %d\n", csrio.exception.bits.uop.cf.pc, intrNO)
+  val raiseDebugIntr = intrNO === IRQ_DEBUG.U && raiseIntr
 
   // exceptions
   val raiseException = csrio.exception.valid && !csrio.exception.bits.isInterrupt
@@ -808,12 +899,18 @@ class CSR(implicit p: Parameters) extends FunctionUnit with HasCSRConst
   val hasInstrAccessFault = csrio.exception.bits.uop.cf.exceptionVec(instrAccessFault) && raiseException
   val hasLoadAccessFault = csrio.exception.bits.uop.cf.exceptionVec(loadAccessFault) && raiseException
   val hasStoreAccessFault = csrio.exception.bits.uop.cf.exceptionVec(storeAccessFault) && raiseException
+  val hasbreakPoint = csrio.exception.bits.uop.cf.exceptionVec(breakPoint) && raiseException
+  val hasSingleStep = csrio.exception.bits.uop.cf.exceptionVec(singleStep) && raiseException
 
   val raiseExceptionVec = csrio.exception.bits.uop.cf.exceptionVec
   val exceptionNO = ExcPriority.foldRight(0.U)((i: Int, sum: UInt) => Mux(raiseExceptionVec(i), i.U, sum))
   val causeNO = (raiseIntr << (XLEN-1)).asUInt() | Mux(raiseIntr, intrNO, exceptionNO)
 
   val raiseExceptionIntr = csrio.exception.valid
+
+  val raiseDebugExceptionIntr = !debugMode && hasbreakPoint || raiseDebugIntr || hasSingleStep
+  val ebreakEnterParkLoop = debugMode && raiseExceptionIntr // exception in debug mode (except ebrk) changes cmderr. how ???
+
   XSDebug(raiseExceptionIntr, "int/exc: pc %x int (%d):%x exc: (%d):%x\n",
     csrio.exception.bits.uop.cf.pc, intrNO, intrVec, exceptionNO, raiseExceptionVec.asUInt
   )
@@ -849,11 +946,12 @@ class CSR(implicit p: Parameters) extends FunctionUnit with HasCSRConst
     mtval := memExceptionAddr
   }
 
+  val debugTrapTarget = Mux(!isEbreak && debugMode, 0x38020808.U, 0x38020800.U) // 0x808 is when an exception occurs in debug mode prog buf exec
   val deleg = Mux(raiseIntr, mideleg , medeleg)
   // val delegS = ((deleg & (1 << (causeNO & 0xf))) != 0) && (priviledgeMode < ModeM);
   val delegS = deleg(causeNO(3,0)) && (priviledgeMode < ModeM)
   val tvalWen = !(hasInstrPageFault || hasLoadPageFault || hasStorePageFault || hasLoadAddrMisaligned || hasStoreAddrMisaligned) || raiseIntr // TODO: need check
-  val isXRet = io.in.valid && func === CSROpType.jmp && !isEcall
+  val isXRet = io.in.valid && func === CSROpType.jmp && !isEcall && !isEbreak
 
   // ctrl block will use theses later for flush
   val isXRetFlag = RegInit(false.B)
@@ -867,14 +965,37 @@ class CSR(implicit p: Parameters) extends FunctionUnit with HasCSRConst
   csrio.isXRet := isXRetFlag
   csrio.trapTarget := Mux(isXRetFlag,
     retTargetReg,
-    Mux(delegS, stvec, mtvec)(VAddrBits-1, 0)
+    Mux(raiseDebugExceptionIntr || ebreakEnterParkLoop, debugTrapTarget,
+      Mux(delegS, stvec, mtvec))(VAddrBits-1, 0)
   )
 
   when (raiseExceptionIntr) {
     val mstatusOld = WireInit(mstatus.asTypeOf(new MstatusStruct))
     val mstatusNew = WireInit(mstatus.asTypeOf(new MstatusStruct))
-
-    when (delegS) {
+    val dcsrNew = WireInit(dcsr.asTypeOf(new DcsrStruct))
+    val debugModeNew = WireInit(debugMode)
+
+    when (raiseDebugExceptionIntr) {
+      when (raiseDebugIntr) {
+        debugModeNew := true.B 
+        mstatusNew.mprv := false.B
+        dpc := SignExt(csrio.exception.bits.uop.cf.pc, XLEN)
+        dcsrNew.cause := 1.U
+        dcsrNew.prv := priviledgeMode
+        priviledgeMode := ModeM
+        XSDebug(raiseDebugIntr, "Debug Mode: Trap to %x at pc %x\n", debugTrapTarget, dpc)
+      }.elsewhen ((hasbreakPoint || hasSingleStep) && !debugMode) {
+        // ebreak or ss in running hart
+        debugModeNew := true.B
+        dpc := SignExt(csrio.exception.bits.uop.cf.pc, XLEN)
+        dcsrNew.cause := Mux(hasbreakPoint, 3.U, 0.U)
+        dcsrNew.prv := priviledgeMode // TODO
+        priviledgeMode := ModeM
+        mstatusNew.mprv := false.B
+      }
+      dcsr := dcsrNew.asUInt
+      debugIntrEnable := false.B
+    }.elsewhen (delegS) {
       scause := causeNO
       sepc := SignExt(csrio.exception.bits.uop.cf.pc, XLEN)
       mstatusNew.spp := priviledgeMode
@@ -891,8 +1012,8 @@ class CSR(implicit p: Parameters) extends FunctionUnit with HasCSRConst
       priviledgeMode := ModeM
       when (tvalWen) { mtval := 0.U }
     }
-
     mstatus := mstatusNew.asUInt
+    debugMode := debugModeNew
   }
 
   XSDebug(raiseExceptionIntr && delegS, "sepc is writen!!! pc:%x\n", cfIn.pc)

src/main/scala/xiangshan/backend/fu/util/CSRConst.scala

@@ -173,13 +173,18 @@ trait HasCSRConst {
   val Mhpmevent31   = 0x33F
 
   // Debug/Trace Registers (shared with Debug Mode) (not implemented)
-  // Debug Mode Registers (not implemented)
+  // Debug Mode Registers
+  val Dcsr          = 0x7B0
+  val Dpc           = 0x7B1
+  val Dscratch      = 0x7B2
+  val Dscratch1     = 0x7B3
 
   def privEcall  = 0x000.U
   def privEbreak = 0x001.U
   def privMret   = 0x302.U
   def privSret   = 0x102.U
   def privUret   = 0x002.U
+  def privDret   = 0x7b2.U
 
   def ModeM     = 0x3.U
   def ModeH     = 0x2.U
@@ -198,7 +203,10 @@ trait HasCSRConst {
   def IRQ_SSIP  = 9
   def IRQ_MSIP  = 11
 
+  def IRQ_DEBUG = 12
+
   val IntPriority = Seq(
+    IRQ_DEBUG, 
     IRQ_MEIP, IRQ_MSIP, IRQ_MTIP,
     IRQ_SEIP, IRQ_SSIP, IRQ_STIP,
     IRQ_UEIP, IRQ_USIP, IRQ_UTIP

src/main/scala/xiangshan/backend/roq/Roq.scala

@@ -179,10 +179,11 @@ class RoqExceptionInfo(implicit p: Parameters) extends XSBundle {
   val roqIdx = new RoqPtr
   val exceptionVec = ExceptionVec()
   val flushPipe = Bool()
+  val singleStep = Bool()
 
-  def has_exception = exceptionVec.asUInt.orR || flushPipe
+  def has_exception = exceptionVec.asUInt.orR || flushPipe || singleStep
   // only exceptions are allowed to writeback when enqueue
-  def can_writeback = exceptionVec.asUInt.orR
+  def can_writeback = exceptionVec.asUInt.orR || singleStep
 }
 
 class ExceptionGen(implicit p: Parameters) extends XSModule with HasCircularQueuePtrHelper {
@@ -240,6 +241,7 @@ class ExceptionGen(implicit p: Parameters) extends XSModule with HasCircularQueu
       }.elsewhen (current.bits.roqIdx === s1_out_bits.roqIdx) {
         current.bits.exceptionVec := (s1_out_bits.exceptionVec.asUInt | current.bits.exceptionVec.asUInt).asTypeOf(ExceptionVec())
         current.bits.flushPipe := s1_out_bits.flushPipe || current.bits.flushPipe
+        current.bits.singleStep := s1_out_bits.singleStep || current.bits.singleStep
       }
     }
   }.elsewhen (s1_out_valid && !s1_flush) {
@@ -426,6 +428,7 @@ class Roq(numWbPorts: Int)(implicit p: Parameters) extends XSModule with HasCirc
   io.exception.bits.uop := RegEnable(debug_deqUop, exceptionHappen)
   io.exception.bits.uop.ctrl.commitType := RegEnable(deqDispatchData.commitType, exceptionHappen)
   io.exception.bits.uop.cf.exceptionVec := RegEnable(exceptionDataRead.bits.exceptionVec, exceptionHappen)
+  io.exception.bits.uop.ctrl.singleStep := RegEnable(exceptionDataRead.bits.singleStep, exceptionHappen)
   io.exception.bits.uop.cf.crossPageIPFFix := RegEnable(deqDispatchData.crossPageIPFFix, exceptionHappen)
   io.exception.bits.isInterrupt := RegEnable(intrEnable, exceptionHappen)
 
@@ -741,6 +744,7 @@ class Roq(numWbPorts: Int)(implicit p: Parameters) extends XSModule with HasCirc
     exceptionGen.io.enq(i).bits.roqIdx := io.enq.req(i).bits.roqIdx
     exceptionGen.io.enq(i).bits.exceptionVec := selectFrontend(io.enq.req(i).bits.cf.exceptionVec, false, true)
     exceptionGen.io.enq(i).bits.flushPipe := io.enq.req(i).bits.ctrl.flushPipe
+    exceptionGen.io.enq(i).bits.singleStep := io.enq.req(i).bits.ctrl.singleStep
   }
 
   // TODO: don't hard code these idxes
@@ -763,6 +767,7 @@ class Roq(numWbPorts: Int)(implicit p: Parameters) extends XSModule with HasCirc
     }
     exceptionGen.io.wb(index).bits.exceptionVec := selectFunc(io.exeWbResults(wb_index).bits.uop.cf.exceptionVec, false, true)
     exceptionGen.io.wb(index).bits.flushPipe    := io.exeWbResults(wb_index).bits.uop.ctrl.flushPipe
+    exceptionGen.io.wb(index).bits.singleStep   := false.B
   }
 
   // 4 fmac + 2 fmisc + 1 i2f

src/test/scala/top/SimTop.scala

@@ -19,10 +19,13 @@ package top
 import chipsalliance.rocketchip.config.{Config, Parameters}
 import chisel3.stage.ChiselGeneratorAnnotation
 import chisel3._
-import device.AXI4RAMWrapper
+
+import device.{AXI4RAMWrapper, SimJTAG}
 import freechips.rocketchip.diplomacy.{LazyModule, LazyModuleImp}
 import utils.GTimer
 import xiangshan.{DebugOptions, DebugOptionsKey}
+import chipsalliance.rocketchip.config._
+import freechips.rocketchip.devices.debug._
 import difftest._
 
 class SimTop(implicit p: Parameters) extends Module {
@@ -48,6 +51,13 @@ class SimTop(implicit p: Parameters) extends Module {
   soc.io.reset := reset.asBool()
   soc.io.extIntrs := simMMIO.io.interrupt.intrVec
 
+  val success = Wire(Bool())
+  val jtag = Module(new SimJTAG(tickDelay=3)(p)).connect(soc.io.systemjtag.jtag, clock, reset.asBool, ~reset.asBool, success)
+  soc.io.systemjtag.reset := reset
+  soc.io.systemjtag.mfr_id := 0.U(11.W)
+  soc.io.systemjtag.part_number := 0.U(16.W)
+  soc.io.systemjtag.version := 0.U(4.W)
+
   val io = IO(new Bundle(){
     val logCtrl = new LogCtrlIO
     val perfInfo = new PerfInfoIO