From 95c656ceef2f6a9ff12d3131698da2b1949fc0b8 Mon Sep 17 00:00:00 2001
From: Dolu1990 <charles.papon.90@gmail.com>
Date: Fri, 21 Oct 2022 12:28:17 +0200
Subject: [PATCH] riscv debug multiple harts

---
 .../scala/vexriscv/VexRiscvBmbGenerator.scala | 19 +++++++++++++++++--
 .../scala/vexriscv/plugin/CsrPlugin.scala     |  2 +-
 2 files changed, 18 insertions(+), 3 deletions(-)

diff --git a/src/main/scala/vexriscv/VexRiscvBmbGenerator.scala b/src/main/scala/vexriscv/VexRiscvBmbGenerator.scala
index 9b08f68..aa4aada 100644
--- a/src/main/scala/vexriscv/VexRiscvBmbGenerator.scala
+++ b/src/main/scala/vexriscv/VexRiscvBmbGenerator.scala
@@ -8,6 +8,7 @@ import spinal.lib.generator._
 import spinal.lib.{sexport, slave}
 import vexriscv.plugin._
 import spinal.core.fiber._
+import spinal.lib.cpu.riscv.debug.DebugHartBus
 
 object VexRiscvBmbGenerator{
   val DEBUG_NONE = 0
@@ -15,6 +16,7 @@ object VexRiscvBmbGenerator{
   val DEBUG_JTAG_CTRL = 2
   val DEBUG_BUS = 3
   val DEBUG_BMB = 4
+  val DEBUG_RISCV = 5
 }
 
 case class VexRiscvBmbGenerator()(implicit interconnectSmp: BmbInterconnectGenerator = null) extends Area {
@@ -63,6 +65,12 @@ case class VexRiscvBmbGenerator()(implicit interconnectSmp: BmbInterconnectGener
     withDebug.load(DEBUG_BUS)
   }
 
+  def enableRiscvDebug(debugCd :  Handle[ClockDomain], resetCd : ClockDomainResetGenerator) : Unit = debugCd.on{
+    this.debugClockDomain.load(debugCd)
+    debugAskReset.loadNothing()
+    withDebug.load(DEBUG_RISCV)
+  }
+
   val debugBmbAccessSource = Handle[BmbAccessCapabilities]
   val debugBmbAccessRequirements = Handle[BmbAccessParameter]
   def enableDebugBmb(debugCd : Handle[ClockDomain], resetCd : ClockDomainResetGenerator, mapping : AddressMapping)(implicit debugMaster : BmbImplicitDebugDecoder = null) : Unit = debugCd.on{
@@ -85,11 +93,14 @@ case class VexRiscvBmbGenerator()(implicit interconnectSmp: BmbInterconnectGener
   val jtagInstructionCtrl = withDebug.produce(withDebug.get == DEBUG_JTAG_CTRL generate JtagTapInstructionCtrl())
   val debugBus = withDebug.produce(withDebug.get == DEBUG_BUS generate DebugExtensionBus())
   val debugBmb = Handle[Bmb]
+  val debugRiscv = withDebug.produce(withDebug.get == DEBUG_RISCV generate DebugHartBus())
   val jtagClockDomain = Handle[ClockDomain]
 
   val logic = Handle(new Area {
-    withDebug.get != DEBUG_NONE generate new Area {
-      config.add(new DebugPlugin(debugClockDomain, hardwareBreakpointCount))
+    withDebug.get match {
+      case DEBUG_NONE =>
+      case DEBUG_RISCV =>
+      case _ => config.add(new DebugPlugin(debugClockDomain, hardwareBreakpointCount))
     }
 
     val cpu = new VexRiscv(config)
@@ -126,6 +137,10 @@ case class VexRiscvBmbGenerator()(implicit interconnectSmp: BmbInterconnectGener
         timerInterrupt load plugin.timerInterrupt
         softwareInterrupt load plugin.softwareInterrupt
         if (plugin.config.supervisorGen) externalSupervisorInterrupt load plugin.externalInterruptS
+        withDebug.get match {
+          case DEBUG_RISCV => debugRiscv <> plugin.debugBus
+          case _ =>
+        }
       }
       case plugin: DebugPlugin => plugin.debugClockDomain {
         if(debugAskReset.get != null) when(RegNext(plugin.io.resetOut)) {
diff --git a/src/main/scala/vexriscv/plugin/CsrPlugin.scala b/src/main/scala/vexriscv/plugin/CsrPlugin.scala
index 2459061..78a5bd9 100644
--- a/src/main/scala/vexriscv/plugin/CsrPlugin.scala
+++ b/src/main/scala/vexriscv/plugin/CsrPlugin.scala
@@ -805,7 +805,7 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
         }
 
         val wakeService = serviceElse(classOf[IWake], null)
-        if(wakeService != null) when(debugMode || step){
+        if(wakeService != null) when(debugMode || step || bus.haltReq){
           wakeService.askWake()
         }
       }