diff --git a/src/main/scala/vexriscv/demo/smp/VexRiscvSmpCluster.scala b/src/main/scala/vexriscv/demo/smp/VexRiscvSmpCluster.scala
index 6fffb6a5..abf503b2 100644
--- a/src/main/scala/vexriscv/demo/smp/VexRiscvSmpCluster.scala
+++ b/src/main/scala/vexriscv/demo/smp/VexRiscvSmpCluster.scala
@@ -14,6 +14,7 @@ import spinal.lib.com.jtag.xilinx.Bscane2BmbMasterGenerator
 import spinal.lib.generator._
 import spinal.core.fiber._
 import spinal.idslplugin.PostInitCallback
+import spinal.lib.cpu.riscv.debug.{DebugModule, DebugModuleCpuConfig, DebugModuleParameter, DebugTransportModuleParameter, DebugTransportModuleTunneled}
 import spinal.lib.misc.plic.PlicMapping
 import spinal.lib.system.debugger.SystemDebuggerConfig
 import vexriscv.ip.{DataCacheAck, DataCacheConfig, DataCacheMemBus, InstructionCache, InstructionCacheConfig}
@@ -30,7 +31,8 @@ case class VexRiscvSmpClusterParameter(cpuConfigs : Seq[VexRiscvConfig],
                                        withExclusiveAndInvalidation : Boolean,
                                        forcePeripheralWidth : Boolean = true,
                                        outOfOrderDecoder : Boolean = true,
-                                       fpu : Boolean = false)
+                                       fpu : Boolean = false,
+                                       privilegedDebug : Boolean = false)
 
 class VexRiscvSmpClusterBase(p : VexRiscvSmpClusterParameter) extends Area with PostInitCallback{
   val cpuCount = p.cpuConfigs.size
@@ -52,10 +54,12 @@ class VexRiscvSmpClusterBase(p : VexRiscvSmpClusterParameter) extends Area with
 
   implicit val interconnect = BmbInterconnectGenerator()
 
-  val debugBridge = debugCd.outputClockDomain on JtagInstructionDebuggerGenerator(p.jtagHeaderIgnoreWidth)
-  debugBridge.jtagClockDomain.load(ClockDomain.external("jtag", withReset = false))
+  val customDebug = !p.privilegedDebug generate new Area {
+    val debugBridge = debugCd.outputClockDomain on JtagInstructionDebuggerGenerator(p.jtagHeaderIgnoreWidth)
+    debugBridge.jtagClockDomain.load(ClockDomain.external("jtag", withReset = false))
 
-  val debugPort = Handle(debugBridge.logic.jtagBridge.io.ctrl.toIo)
+    val debugPort = Handle(debugBridge.logic.jtagBridge.io.ctrl.toIo)
+  }
 
   val dBusCoherent = BmbBridgeGenerator()
   val dBusNonCoherent = BmbBridgeGenerator()
@@ -80,12 +84,65 @@ class VexRiscvSmpClusterBase(p : VexRiscvSmpClusterParameter) extends Area with
     interconnect.addConnection(
       cpu.dBus -> List(dBusCoherent.bmb)
     )
-    cpu.enableDebugBmb(
-      debugCd = debugCd.outputClockDomain,
-      resetCd = systemCd,
-      mapping = SizeMapping(cpuId*0x1000, 0x1000)
+
+    if(!p.privilegedDebug) {
+      cpu.enableDebugBmb(
+        debugCd = debugCd.outputClockDomain,
+        resetCd = systemCd,
+        mapping = SizeMapping(cpuId * 0x1000, 0x1000)
+      )
+      interconnect.addConnection(customDebug.debugBridge.bmb, cpu.debugBmb)
+    } else {
+      cpu.enableRiscvDebug(debugCd.outputClockDomain, systemCd)
+    }
+  }
+
+  val privilegedDebug = p.privilegedDebug generate new Area{
+    val jtagCd = ClockDomain.external("jtag", withReset = false)
+
+    val systemReset = Handle(Bool())
+    systemCd.relaxedReset(systemReset, ResetSensitivity.HIGH)
+
+    val p = DebugTransportModuleParameter(
+      addressWidth = 7,
+      version = 1,
+      idle = 7
     )
-    interconnect.addConnection(debugBridge.bmb, cpu.debugBmb)
+
+    val logic = hardFork(debugCd.outputClockDomain on new Area {
+      val XLEN = 32
+
+      val dm = DebugModule(
+        DebugModuleParameter(
+          version = p.version + 1,
+          harts = cpuCount,
+          progBufSize = 2,
+          datacount = XLEN / 32 + cores.exists(_.cpu.config.get.FLEN == 64).toInt,
+          hartsConfig = cores.map(c => DebugModuleCpuConfig(
+            xlen = XLEN,
+            flen = c.cpu.config.get.FLEN,
+            withFpuRegAccess = c.cpu.config.get.FLEN == 64
+          ))
+        )
+      )
+      systemReset := dm.io.ndmreset
+      for ((cpu, i) <- cores.zipWithIndex) {
+        val privBus = cpu.cpu.debugRiscv
+        privBus <> dm.io.harts(i)
+        privBus.dmToHart.removeAssignments() <-< dm.io.harts(i).dmToHart
+      }
+
+      val clintStop =  (cores.map(e => e.cpu.logic.cpu.service(classOf[CsrPlugin]).stoptime).andR)
+
+      val tunnel = DebugTransportModuleTunneled(
+        p = p,
+        jtagCd = jtagCd,
+        debugCd = ClockDomain.current
+      )
+      dm.io.ctrl <> tunnel.io.bus
+
+      val debugPort = Handle(tunnel.io.instruction.toIo)
+    })
   }
 }
 
diff --git a/src/main/scala/vexriscv/demo/smp/VexRiscvSmpLitexCluster.scala b/src/main/scala/vexriscv/demo/smp/VexRiscvSmpLitexCluster.scala
index 3454577b..3d380ca9 100644
--- a/src/main/scala/vexriscv/demo/smp/VexRiscvSmpLitexCluster.scala
+++ b/src/main/scala/vexriscv/demo/smp/VexRiscvSmpLitexCluster.scala
@@ -101,6 +101,7 @@ class VexRiscvLitexSmpCluster(p : VexRiscvLitexSmpClusterParameter) extends VexR
 
 
 object VexRiscvLitexSmpClusterCmdGen extends App {
+  Handle.loadHandleAsync = true
   var cpuCount = 1
   var iBusWidth = 64
   var dBusWidth = 64
@@ -108,6 +109,7 @@ object VexRiscvLitexSmpClusterCmdGen extends App {
   var dCacheSize = 8192
   var iCacheWays = 2
   var dCacheWays = 2
+  var privilegedDebug = false
   var liteDramWidth = 128
   var coherentDma = false
   var wishboneMemory = false
@@ -131,6 +133,7 @@ object VexRiscvLitexSmpClusterCmdGen extends App {
     opt[String]("dcache-size") action { (v, c) => dCacheSize = v.toInt }
     opt[String]("icache-ways") action { (v, c) => iCacheWays = v.toInt }
     opt[String]("dcache-ways") action { (v, c) => dCacheWays = v.toInt }
+    opt[Boolean]("privileged-debug") action { (v, c) => privilegedDebug = v }
     opt[String]("litedram-width") action { (v, c) => liteDramWidth = v.toInt }
     opt[String]("netlist-directory") action { (v, c) => netlistDirectory = v }
     opt[String]("netlist-name") action { (v, c) => netlistName = v }
@@ -160,6 +163,7 @@ object VexRiscvLitexSmpClusterCmdGen extends App {
           iCacheWays = iCacheWays,
           dCacheWays = dCacheWays,
           coherency = coherency,
+          privilegedDebug = privilegedDebug,
           iBusRelax = true,
           earlyBranch = true,
           withFloat = fpu,
@@ -178,7 +182,8 @@ object VexRiscvLitexSmpClusterCmdGen extends App {
       forcePeripheralWidth = !wishboneMemory || wishboneForce32b,
       outOfOrderDecoder = outOfOrderDecoder,
       fpu = fpu,
-      jtagHeaderIgnoreWidth = 0
+      jtagHeaderIgnoreWidth = 0,
+      privilegedDebug = privilegedDebug
     ),
     liteDram = LiteDramNativeParameter(addressWidth = 32, dataWidth = liteDramWidth),
     liteDramMapping = SizeMapping(0x40000000l, 0x40000000l),