Merge branch 'riscv-debug' into dev

README.md

@@ -45,6 +45,7 @@
     - [MmuPlugin](#mmuplugin)
     - [PmpPlugin](#pmpplugin)
     - [DebugPlugin](#debugplugin)
+    - [EmbeddedRiscvJtag](#embeddedRiscvJtag)
     - [YamlPlugin](#yamlplugin)
     - [FpuPlugin](#fpuplugin)
     - [AesPlugin](#aesplugin)
@@ -773,6 +774,7 @@ This chapter describes the currently implemented plugins.
 - [StaticMemoryTranslatorPlugin](#staticmemorytranslatorplugin)
 - [MemoryTranslatorPlugin](#memorytranslatorplugin)
 - [DebugPlugin](#debugplugin)
+- [EmbeddedRiscvJtag](#embeddedRiscvJtag)
 - [YamlPlugin](#yamlplugin)
 - [FpuPlugin](#fpuplugin)
 
@@ -1180,6 +1182,42 @@ Write Address 0x04 ->
 
 The OpenOCD port is here: <https://github.com/SpinalHDL/openocd_riscv>
 
+#### EmbeddedRiscvJtag
+
+VexRiscv also support the official RISC-V debug specification (Thanks Efinix for the funding !).
+
+To enable it, you need to add the EmbeddedRiscvJtag to the plugin list : 
+
+```scala
+new EmbeddedRiscvJtag(
+  p = DebugTransportModuleParameter(
+    addressWidth = 7,
+    version      = 1,
+    idle         = 7
+  ),
+  withTunneling = false,
+  withTap = true
+)
+```
+
+And turn on the withPrivilegedDebug option in the CsrPlugin config.
+
+Here is an example of openocd tcl script to connect : 
+
+```tcl
+# ADD HERE YOUR JTAG ADAPTER SETTINGS
+
+set _CHIPNAME riscv
+jtag newtap $_CHIPNAME cpu -irlen 5 -expected-id 0x10002FFF
+
+set _TARGETNAME $_CHIPNAME.cpu
+
+target create $_TARGETNAME.0 riscv -chain-position $_TARGETNAME
+
+init
+halt
+```
+
 #### YamlPlugin
 
 This plugin offers a service to other plugins to generate a useful Yaml file describing the CPU configuration. It contains, for instance, the sequence of instructions required

src/main/scala/vexriscv/Riscv.scala

@@ -238,5 +238,13 @@ object Riscv{
     val FFLAGS = 0x1
     val FRM = 0x2
     val FCSR = 0x3
+
+    val DCSR      = 0x7B0
+    val DPC       = 0x7B1
+    val TSELECT   = 0x7A0
+    val TDATA1    = 0x7A1
+    val TDATA2    = 0x7A2
+    val TINFO     = 0x7a4
+    val TCONTROL  = 0x7A5
   }
 }

src/main/scala/vexriscv/TestsWorkspace.scala

@@ -24,6 +24,7 @@ import spinal.core._
 import spinal.lib._
 import vexriscv.ip._
 import spinal.lib.bus.avalon.AvalonMM
+import spinal.lib.cpu.riscv.debug.DebugTransportModuleParameter
 import spinal.lib.eda.altera.{InterruptReceiverTag, ResetEmitterTag}
 import vexriscv.demo.smp.VexRiscvSmpClusterGen
 import vexriscv.ip.fpu.FpuParameter
@@ -141,14 +142,28 @@ object TestsWorkspace {
         withFloat = true,
         withDouble = true,
         externalFpu = false,
-        simHalt = true
+        simHalt = true,
+        privilegedDebug = true
       )
 
+      config.plugins += new EmbeddedRiscvJtag(
+        p = DebugTransportModuleParameter(
+          addressWidth = 7,
+          version      = 1,
+          idle         = 7
+        ),
+        withTunneling = false,
+        withTap = true
+      )
+
+//      l.foreach{
+//        case p : EmbeddedRiscvJtag => p.debugCd.load(ClockDomain.current.copy(reset = Bool().setName("debug_reset")))
+//        case _ =>
+//      }
 
       println("Args :")
       println(config.getRegressionArgs().mkString(" "))
 
-
       val toplevel = new VexRiscv(config)
 //      val toplevel = new VexRiscv(configLight)
 //      val toplevel = new VexRiscv(configTest)

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 {
+    withDebug.get match {
-      config.add(new DebugPlugin(debugClockDomain, hardwareBreakpointCount))
+      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)) {

src/main/scala/vexriscv/demo/WhiteboxPlugin.scala

@@ -0,0 +1,14 @@
+package vexriscv.demo
+
+import spinal.core._
+import vexriscv.plugin.Plugin
+import vexriscv.{DecoderService, Stageable, VexRiscv}
+
+class WhiteboxPlugin extends Plugin[VexRiscv]{
+  override def build(pipeline: VexRiscv): Unit = {
+    Component.current.afterElaboration {
+      def export(name : String): Unit = out(Component.current.reflectBaseType(name))
+      export("IBusCachedPlugin_fetchPc_pc")
+    }
+  }
+}

src/main/scala/vexriscv/demo/smp/VexRiscvSmpCluster.scala

@@ -194,14 +194,16 @@ object VexRiscvSmpClusterGen {
                      withDataCache : Boolean = true,
                      withInstructionCache : Boolean = true,
                      forceMisa : Boolean = false,
-                     forceMscratch : Boolean = false
+                     forceMscratch : Boolean = false,
+                     privilegedDebug : Boolean = false
                     ) = {
     assert(iCacheSize/iCacheWays <= 4096, "Instruction cache ways can't be bigger than 4096 bytes")
     assert(dCacheSize/dCacheWays <= 4096, "Data cache ways can't be bigger than 4096 bytes")
     assert(!(withDouble && !withFloat))
 
+    val misa = Riscv.misaToInt(s"ima${if(withFloat) "f" else ""}${if(withDouble) "d" else ""}${if(rvc) "c" else ""}${if(withSupervisor) "s" else ""}")
     val csrConfig = if(withSupervisor){
-      CsrPluginConfig.openSbi(mhartid = hartId, misa = Riscv.misaToInt(s"ima${if(withFloat) "f" else ""}${if(withDouble) "d" else ""}s")).copy(utimeAccess = CsrAccess.READ_ONLY)
+      CsrPluginConfig.openSbi(mhartid = hartId, misa = misa).copy(utimeAccess = CsrAccess.READ_ONLY, withPrivilegedDebug = privilegedDebug)
     } else {
       CsrPluginConfig(
         catchIllegalAccess = true,
@@ -209,7 +211,7 @@ object VexRiscvSmpClusterGen {
         marchid        = null,
         mimpid         = null,
         mhartid        = hartId,
-        misaExtensionsInit = Riscv.misaToInt(s"ima${if(withFloat) "f" else ""}${if(withDouble) "d" else ""}s"),
+        misaExtensionsInit = misa,
         misaAccess     = if(forceMisa) CsrAccess.WRITE_ONLY else CsrAccess.NONE,
         mtvecAccess    = CsrAccess.READ_WRITE,
         mtvecInit      = null,
@@ -223,7 +225,8 @@ object VexRiscvSmpClusterGen {
         ebreakGen      = true,
         wfiGenAsWait   = false,
         wfiGenAsNop    = true,
-        ucycleAccess   = CsrAccess.NONE
+        ucycleAccess   = CsrAccess.NONE,
+        withPrivilegedDebug = privilegedDebug
       )
     }
     val config = VexRiscvConfig(

src/main/scala/vexriscv/plugin/CsrPlugin.scala

@@ -9,6 +9,8 @@ import vexriscv.plugin.IntAluPlugin.{ALU_BITWISE_CTRL, ALU_CTRL, AluBitwiseCtrlE
 import scala.collection.mutable.ArrayBuffer
 import scala.collection.mutable
 import spinal.core.sim._
+import spinal.lib.cpu.riscv.debug._
+import spinal.lib.fsm.{State, StateMachine}
 
 /**
   * Created by spinalvm on 21.03.17.
@@ -78,12 +80,15 @@ case class CsrPluginConfig(
                             pipelinedInterrupt  : Boolean = true,
                             csrOhDecoder        : Boolean = true,
                             deterministicInteruptionEntry : Boolean = false, //Only used for simulatation purposes
-                            wfiOutput           : Boolean = false
+                            wfiOutput           : Boolean = false,
+                            withPrivilegedDebug : Boolean = false, //For the official RISC-V debug spec implementation
+                            debugTriggers       : Int     = 2
                           ){
   assert(!ucycleAccess.canWrite)
-  def privilegeGen = userGen || supervisorGen
+  def privilegeGen = userGen || supervisorGen || withPrivilegedDebug
   def noException = this.copy(ecallGen = false, ebreakGen = false, catchIllegalAccess = false)
   def noExceptionButEcall = this.copy(ecallGen = true, ebreakGen = false, catchIllegalAccess = false)
+  def withEbreak = ebreakGen || withPrivilegedDebug
 }
 
 object CsrPluginConfig{
@@ -465,6 +470,10 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
   var externalMhartId : UInt = null
   var utime : UInt = null
 
+  var debugBus : DebugHartBus = null
+  var debugMode : Bool = null
+  var injectionPort : Stream[Bits] = null
+
   override def askWake(): Unit = thirdPartyWake := True
 
   override def isContextSwitching = contextSwitching
@@ -473,7 +482,7 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
     val NONE, XRET = newElement()
     val WFI = if(wfiGenAsWait) newElement() else null
     val ECALL = if(ecallGen) newElement() else null
-    val EBREAK = if(ebreakGen) newElement() else null
+    val EBREAK = if(withEbreak) newElement() else null
   }
 
   object ENV_CTRL extends Stageable(EnvCtrlEnum())
@@ -532,7 +541,7 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
   override def setup(pipeline: VexRiscv): Unit = {
     import pipeline.config._
 
-    if(!config.ebreakGen) {
+    if(!config.withEbreak) {
       SpinalWarning("This VexRiscv configuration is set without software ebreak instruction support. Some software may rely on it (ex: Rust). (This isn't related to JTAG ebreak)")
     }
 
@@ -578,7 +587,7 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
     if(wfiGenAsWait)   decoderService.add(WFI,  defaultEnv ++ List(ENV_CTRL -> EnvCtrlEnum.WFI))
     if(wfiGenAsNop)   decoderService.add(WFI, Nil)
     if(ecallGen) decoderService.add(ECALL,  defaultEnv ++ List(ENV_CTRL -> EnvCtrlEnum.ECALL, HAS_SIDE_EFFECT -> True))
-    if(ebreakGen) decoderService.add(EBREAK,  defaultEnv ++ List(ENV_CTRL -> EnvCtrlEnum.EBREAK, HAS_SIDE_EFFECT -> True))
+    if(withEbreak) decoderService.add(EBREAK,  defaultEnv ++ List(ENV_CTRL -> EnvCtrlEnum.EBREAK, HAS_SIDE_EFFECT -> True))
 
     val  pcManagerService = pipeline.service(classOf[JumpService])
     jumpInterface = pcManagerService.createJumpInterface(pipeline.stages.last)
@@ -603,7 +612,7 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
     privilege = UInt(2 bits).setName("CsrPlugin_privilege")
     forceMachineWire = False
 
-    if(catchIllegalAccess || ecallGen || ebreakGen)
+    if(catchIllegalAccess || ecallGen || withEbreak)
       selfException = newExceptionPort(pipeline.execute)
 
     allowInterrupts = True
@@ -622,6 +631,11 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
       decoderService.addDefault(IS_SFENCE_VMA, False)
       decoderService.add(SFENCE_VMA, List(IS_SFENCE_VMA -> True))
     }
+
+
+    injectionPort = withPrivilegedDebug generate pipeline.service(classOf[IBusFetcher]).getInjectionPort()
+    debugMode = withPrivilegedDebug generate Bool().setName("debugMode")
+    debugBus = withPrivilegedDebug generate slave(DebugHartBus()).setName("debugBus")
   }
 
   def inhibateInterrupts() : Unit = allowInterrupts := False
@@ -657,11 +671,234 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
       val base = UInt(xlen-2 bits)
     }
 
+    val trapEvent = False
+
     val privilegeReg = privilegeGen generate RegInit(U"11")
     privilege := (if(privilegeGen) privilegeReg else U"11")
 
     when(forceMachineWire) { privilege := 3 }
 
+    val debug = withPrivilegedDebug generate pipeline.plug(new Area{
+      val iBusFetcher = service(classOf[IBusFetcher])
+      def bus = debugBus
+
+
+      val running = RegInit(True)
+      debugMode := !running
+
+
+      when(!running) {
+        iBusFetcher.haltIt()
+      }
+
+      bus.resume.rsp.valid := False
+
+      bus.running :=  running
+      bus.halted  := !running
+      bus.unavailable := RegNext(ClockDomain.current.isResetActive)
+      val enterHalt = running.getAheadValue().fall(False)
+
+      val doHalt = RegInit(False) setWhen(bus.haltReq && bus.running && !debugMode) clearWhen(enterHalt)
+      val forceResume = False
+      val doResume = forceResume || bus.resume.isPending(1)
+
+      // Pipeline execution timeout used to trigger some redo
+      val timeout = Timeout(7)
+      when(pipeline.stages.map(_.arbitration.isValid).orR){
+        timeout.clear()
+      }
+
+      val inject = new Area{
+        val cmd = bus.dmToHart.translateWith(bus.dmToHart.data).takeWhen(bus.dmToHart.op === DebugDmToHartOp.EXECUTE)
+        injectionPort << cmd.toStream.stage
+
+
+        val pending = RegInit(False) setWhen(cmd.valid) clearWhen(bus.exception || bus.commit || bus.ebreak || bus.redo)
+        when(cmd.valid){ timeout.clear() }
+        bus.redo := pending && timeout.state
+      }
+      val dataCsrr = new Area{
+        bus.hartToDm.valid   := isWriting(DebugModule.CSR_DATA)
+        bus.hartToDm.address := 0
+        bus.hartToDm.data    := execute.input(SRC1)
+      }
+
+      val dataCsrw = new Area{
+        val value = Reg(Bits(32 bits))
+
+        val fromDm = new Area{
+          when(bus.dmToHart.valid && bus.dmToHart.op === DebugDmToHartOp.DATA){
+            value := bus.dmToHart.data
+          }
+        }
+
+        val toHart = new Area{
+          r(DebugModule.CSR_DATA, value)
+        }
+      }
+
+      val dpc = Reg(UInt(32 bits))
+      val dcsr = new Area{
+        rw(CSR.DPC, dpc)
+        val prv = RegInit(U"11")
+        val step = RegInit(False) //TODO
+        val nmip = False
+        val mprven = False
+        val cause = RegInit(U"000")
+        val stoptime = False
+        val stopcount = False
+        val stepie = RegInit(False) //TODO
+        val ebreaku = userGen generate RegInit(False)
+        val ebreaks = supervisorGen generate RegInit(False)
+        val ebreakm = RegInit(False)
+        val xdebugver = U(4, 4 bits)
+
+        val stepLogic = new StateMachine{
+          val IDLE, SINGLE, WAIT = new State()
+          setEntry(IDLE)
+
+          IDLE whenIsActive{
+            when(step && bus.resume.rsp.valid){
+              goto(SINGLE)
+            }
+          }
+          SINGLE whenIsActive{
+            when(decode.arbitration.isFiring) {
+              goto(WAIT)
+            }
+          }
+
+          WAIT whenIsActive{
+            decode.arbitration.haltByOther setWhen(decode.arbitration.isValid)
+            //re resume the execution in case of timeout (ex cache miss)
+            when(!doHalt && timeout.state){
+              forceResume := True
+              goto(SINGLE)
+            } otherwise {
+              when(stages.last.arbitration.isFiring) {
+                doHalt := True
+              }
+            }
+          }
+
+          always{
+            when(enterHalt){
+              goto(IDLE)
+            }
+          }
+          build()
+        }
+
+
+        r(CSR.DCSR, 3 -> nmip, 6 -> cause, 28 -> xdebugver, 4 -> mprven, 9 -> stoptime, 10 -> stopcount)
+        rw(CSR.DCSR, 0 -> prv, 2 -> step, 11 -> stepie, 15 -> ebreakm)
+        if(supervisorGen) rw(CSR.DCSR, 13 -> ebreaks)
+        if(userGen)       rw(CSR.DCSR, 12 -> ebreaku)
+
+
+        when(debugMode) {
+          pipeline.plugins.foreach{
+            case p : PredictionInterface => p.inDebugNoFetch()
+            case _ =>
+          }
+        }
+
+        val wakeService = serviceElse(classOf[IWake], null)
+        if(wakeService != null) when(debugMode || step || bus.haltReq){
+          wakeService.askWake()
+        }
+      }
+
+      //Very limited subset of the trigger spec
+      val trigger = (debugTriggers > 0) generate new Area {
+        val tselect = new Area{
+          val index = Reg(UInt(log2Up(debugTriggers) bits))
+          rw(CSR.TSELECT, index)
+
+          val outOfRange = if(isPow2(debugTriggers)) False else index < debugTriggers
+        }
+
+        val tinfo = new Area{
+          r(CSR.TINFO, 0 -> tselect.outOfRange, 2 -> !tselect.outOfRange)
+        }
+
+        val decodeBreak = new Area {
+          val enabled = False
+          val timeout = Timeout(3).clearWhen(!enabled || stages.tail.map(_.arbitration.isValid).orR)
+          when(enabled) {
+            decode.arbitration.haltByOther := True
+            when(timeout.state) {
+              decode.arbitration.flushIt := True
+              dpc := decode.input(PC)
+              running := False
+              dcsr.cause := 2
+              dcsr.prv := privilege
+              privilegeReg := 3
+            }
+          }
+        }
+
+        val slots = for(slotId <- 0 until debugTriggers) yield new Area {
+          val selected = tselect.index === slotId
+          def csrw(csrId : Int, thats : (Int, Data)*): Unit ={
+            onWrite(csrId){
+              when(selected) {
+                for((offset, data) <- thats){
+                  data.assignFromBits(writeData()(offset, widthOf(data) bits))
+                }
+              }
+            }
+          }
+          def csrr(csrId : Int, read : Bits, thats : (Int, Data)*): Unit ={
+            when(selected) {
+              for((offset, data) <- thats){
+                read(offset, widthOf(data) bits) := data.asBits
+              }
+            }
+          }
+          def csrrw(csrId : Int, read : Bits, thats : (Int, Data)*) : Unit = {
+            csrw(csrId, thats :_*)
+            csrr(csrId, read, thats :_*)
+          }
+
+          val tdata1 = new Area{
+            val read = B(0, 32 bits)
+            val tpe = U(2, 4 bits)
+            val dmode = Reg(Bool()) init(False)
+
+            val execute = RegInit(False)
+            val m, s, u = RegInit(False)
+            val action = RegInit(U"0000")
+            val privilegeHit = !debugMode && privilege.mux(
+              0 -> u,
+              1 -> s,
+              3 -> m,
+              default -> False
+            )
+
+            csrrw(CSR.TDATA1, read, 2 -> execute , 3 -> u, 4-> s, 6 -> m, 32 - 5 -> dmode, 12 -> action)
+            csrr(CSR.TDATA1, read, 32 - 4 -> tpe)
+
+            //TODO action sizelo timing select sizehi maskmax
+          }
+
+          val tdata2 = new Area{
+            val value = Reg(PC)
+            csrw(CSR.TDATA2, 0 -> value)
+
+            val execute = new Area{
+              val enabled = !debugMode && tdata1.action === 1 && tdata1.execute && tdata1.privilegeHit
+              val hit =  enabled && value === decode.input(PC)
+              decodeBreak.enabled.setWhen(hit)
+            }
+          }
+        }
+
+        r(CSR.TDATA1, 0 -> slots.map(_.tdata1.read).read(tselect.index))
+      }
+    })
+
+
     val machineCsr = pipeline plug new Area{
       //Define CSR registers
       // Status => MXR, SUM, TVM, TW, TSE ?
@@ -973,6 +1210,11 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
         }
 
         code.addTag(Verilator.public)
+
+        if(withPrivilegedDebug) {
+          valid clearWhen(!debug.dcsr.stepie)
+          valid setWhen(debug.doHalt)
+        }
       }
 
 
@@ -1012,6 +1254,11 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
       val hadException = RegNext(exception) init(False) addTag(Verilator.public)
       pipelineLiberator.done.clearWhen(hadException)
 
+      if(withPrivilegedDebug) {
+        debugBus.commit := debugMode && pipeline.stages.last.arbitration.isFiring
+        debugBus.exception := debugMode && hadException
+        debugBus.ebreak := False
+      }
 
       val targetPrivilege = CombInit(interrupt.targetPrivilege)
       if(exceptionPortCtrl != null) when(hadException) {
@@ -1019,8 +1266,17 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
       }
 
       val trapCause = CombInit(interrupt.code.resize(trapCodeWidth))
+      val trapCauseEbreakDebug = False
       if(exceptionPortCtrl != null) when( hadException){
         trapCause := exceptionPortCtrl.exceptionContext.code.resized
+        if(withPrivilegedDebug) {
+          when(exceptionPortCtrl.exceptionContext.code === 3){
+            trapCauseEbreakDebug setWhen(debugMode)
+            trapCauseEbreakDebug setWhen(privilege === 3 && debug.dcsr.ebreakm)
+            if (userGen) trapCauseEbreakDebug setWhen (privilege === 0 && debug.dcsr.ebreaku)
+            if (supervisorGen) trapCauseEbreakDebug setWhen (privilege === 1 && debug.dcsr.ebreaks)
+          }
+        }
       }
 
       val xtvec = Xtvec().assignDontCare()
@@ -1029,38 +1285,61 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
         is(3){ xtvec := machineCsr.mtvec }
       }
 
+      val trapEnterDebug = False
+      if(withPrivilegedDebug) trapEnterDebug setWhen(debug.doHalt || trapCauseEbreakDebug || !hadException && debug.doHalt)
       when(hadException || interruptJump){
+        trapEvent := True
         fetcher.haltIt() //Avoid having the fetch confused by the incomming privilege switch
 
         jumpInterface.valid         := True
         jumpInterface.payload       := (if(!xtvecModeGen) xtvec.base @@ U"00" else (xtvec.mode === 0 || hadException) ? (xtvec.base @@ U"00") | ((xtvec.base + trapCause) @@ U"00") )
         lastStage.arbitration.flushNext := True
 
-        if(privilegeGen) privilegeReg := targetPrivilege
+        when(!trapEnterDebug){
+          if(privilegeGen) privilegeReg := targetPrivilege
+          switch(targetPrivilege){
+            if(supervisorGen) is(1) {
+              sstatus.SIE := False
+              sstatus.SPIE := sstatus.SIE
+              sstatus.SPP := privilege(0 downto 0)
+              scause.interrupt := !hadException
+              scause.exceptionCode := trapCause
+              sepc := mepcCaptureStage.input(PC)
+              if (exceptionPortCtrl != null) when(hadException){
+                stval := exceptionPortCtrl.exceptionContext.badAddr
+              }
+            }
 
-        switch(targetPrivilege){
+            is(3){
-          if(supervisorGen) is(1) {
+              mstatus.MIE  := False
-            sstatus.SIE := False
+              mstatus.MPIE := mstatus.MIE
-            sstatus.SPIE := sstatus.SIE
+              mstatus.MPP  := privilege
-            sstatus.SPP := privilege(0 downto 0)
+              mcause.interrupt := !hadException
-            scause.interrupt := !hadException
+              mcause.exceptionCode := trapCause
-            scause.exceptionCode := trapCause
+              mepc := mepcCaptureStage.input(PC)
-            sepc := mepcCaptureStage.input(PC)
+              if(exceptionPortCtrl != null) when(hadException){
-            if (exceptionPortCtrl != null) when(hadException){
+                mtval := exceptionPortCtrl.exceptionContext.badAddr
-              stval := exceptionPortCtrl.exceptionContext.badAddr
+              }
             }
           }
-
+        } otherwise {
-          is(3){
+          if(withPrivilegedDebug) {
-            mstatus.MIE  := False
+            debug.running := False
-            mstatus.MPIE := mstatus.MIE
+            when(!debugMode) {
-            mstatus.MPP  := privilege
+              debug.dpc := mepcCaptureStage.input(PC)
-            mcause.interrupt := !hadException
+              debug.dcsr.cause := 3
-            mcause.exceptionCode := trapCause
+              when(debug.dcsr.step) {
-            mepc := mepcCaptureStage.input(PC)
+                debug.dcsr.cause := 4
-            if(exceptionPortCtrl != null) when(hadException){
+              }
-              mtval := exceptionPortCtrl.exceptionContext.badAddr
+              when(trapCauseEbreakDebug) {
+                debug.dcsr.cause := 1
+              }
+              debug.dcsr.prv := privilege
+            } otherwise {
+              debugBus.exception := !trapCauseEbreakDebug //TODO mask interrupt while in debug mode
+              debugBus.ebreak := trapCauseEbreakDebug
             }
+            privilegeReg := 3
           }
         }
       }
@@ -1097,9 +1376,22 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
         }
       }
 
-
       contextSwitching := jumpInterface.valid
 
+      // Debug resume
+      if(withPrivilegedDebug) {
+        when(debug.doResume) {
+          jumpInterface.valid   := True
+          jumpInterface.payload := debug.dpc
+          lastStage.arbitration.flushIt := True
+
+          privilegeReg := debug.dcsr.prv
+          debug.running := True
+          debug.bus.resume.rsp.valid := True
+        }
+      }
+
+
       //CSR read/write instructions management
       decode plug new Area{
         import decode._
@@ -1164,7 +1456,7 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
         }
 
 
-        if(ebreakGen) when(arbitration.isValid && input(ENV_CTRL) === EnvCtrlEnum.EBREAK && allowEbreakException){
+        if(withEbreak) when(arbitration.isValid && input(ENV_CTRL) === EnvCtrlEnum.EBREAK && allowEbreakException){
           selfException.valid := True
           selfException.code := 3
         }
@@ -1200,12 +1492,6 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
             memory.output(REGFILE_WRITE_DATA) := memory.input(PIPELINED_CSR_READ)
           }
         }
-//
-//        Component.current.rework{
-//          when(arbitration.isFiring && input(IS_CSR)) {
-//            memory.input(REGFILE_WRITE_DATA).getDrivingReg := readData
-//          }
-//        }
 
         //Translation of the csrMapping into real logic
         val csrAddress = input(INSTRUCTION)(csrRange)
@@ -1307,11 +1593,15 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
 
           illegalAccess clearWhen(csrMapping.allowCsrSignal)
 
-          when(privilege < csrAddress(9 downto 8).asUInt){
+          val forceFail = False
+          forceFail setWhen(privilege < csrAddress(9 downto 8).asUInt)
+          if(withPrivilegedDebug) forceFail setWhen(!debugMode && csrAddress >> 4 === 0x7B)
+          when(forceFail){
             illegalAccess := True
             readInstruction := False
             writeInstruction := False
           }
+
           illegalAccess clearWhen(!arbitration.isValid || !input(IS_CSR))
         }
       }

src/main/scala/vexriscv/plugin/DebugPlugin.scala

@@ -179,7 +179,6 @@ case class DebugExtensionIo() extends Bundle with IMasterSlave{
 class DebugPlugin(var debugClockDomain : ClockDomain, hardwareBreakpointCount : Int = 0, BreakpointReadback : Boolean = false) extends Plugin[VexRiscv] {
 
   var io : DebugExtensionIo = null
-  val injectionAsks = ArrayBuffer[(Stage, Bool)]()
   var injectionPort : Stream[Bits] = null
 
 

src/main/scala/vexriscv/plugin/EmbeddedRiscvJtag.scala

@@ -0,0 +1,97 @@
+/**
+  * Thanks Efinix for funding the official RISC-V debug implementation on VexRiscv !
+  */
+
+
+package vexriscv.plugin
+
+import spinal.core._
+import spinal.lib._
+import spinal.lib.com.jtag._
+import spinal.lib.cpu.riscv.debug._
+import vexriscv._
+
+
+class EmbeddedRiscvJtag(var p : DebugTransportModuleParameter,
+                        var withTap : Boolean = true,
+                        var withTunneling : Boolean = false
+                        ) extends Plugin[VexRiscv] with VexRiscvRegressionArg{
+
+
+  override def getVexRiscvRegressionArgs() = List("DEBUG_PLUGIN=RISCV")
+
+  var jtag : Jtag = null
+  var jtagInstruction : JtagTapInstructionCtrl = null
+  var ndmreset : Bool = null
+
+//  val debugCd = Handle[ClockDomain].setName("debugCd")
+//  val noTapCd = Handle[ClockDomain].setName("jtagCd")
+
+  override def setup(pipeline: VexRiscv): Unit = {
+    jtag = withTap generate slave(Jtag()).setName("jtag")
+    jtagInstruction = !withTap generate slave(JtagTapInstructionCtrl()).setName("jtagInstruction")
+    ndmreset = Bool().setName("ndmreset")
+  }
+
+  override def build(pipeline: VexRiscv): Unit = {
+    val XLEN = 32
+    val dm = DebugModule(
+      DebugModuleParameter(
+        version = p.version + 1,
+        harts = 1,
+        progBufSize = 2,
+        datacount   = XLEN/32,
+        xlens = List(XLEN)
+      )
+    )
+
+    ndmreset := dm.io.ndmreset
+
+    val dmiDirect = if(withTap && !withTunneling) new Area {
+      val logic = DebugTransportModuleJtagTap(
+        p.copy(addressWidth = 7),
+        debugCd = ClockDomain.current
+      )
+      dm.io.ctrl <> logic.io.bus
+      logic.io.jtag <> jtag
+    }
+    val dmiTunneled = if(withTap && withTunneling) new Area {
+      val logic = DebugTransportModuleJtagTapWithTunnel(
+        p.copy(addressWidth = 7),
+        debugCd = ClockDomain.current
+      )
+      dm.io.ctrl <> logic.io.bus
+      logic.io.jtag <> jtag
+    }
+
+    val privBus = pipeline.service(classOf[CsrPlugin]).debugBus.setAsDirectionLess()
+    privBus <> dm.io.harts(0)
+    privBus.dmToHart.removeAssignments() <-< dm.io.harts(0).dmToHart
+  }
+}
+
+/*
+make IBUS=CACHED IBUS_DATA_WIDTH=64 COMPRESSED=no DBUS=CACHED DBUS_LOAD_DATA_WIDTH=64 DBUS_STORE_DATA_WIDTH=64 LRSC=yes AMO=yes DBUS_EXCLUSIVE=yes DBUS_INVALIDATE=yes MUL=yes DIV=yes RVF=yes RVD=yes RISCV_JTAG=yes  REDO=1 WITH_RISCV_REF=no DEBUG_PLUGIN_EXTERNAL=yes DEBUG_PLUGIN=no
+src/openocd -f ../VexRiscvOoo/src/main/tcl/openocd/naxriscv_sim.tcl -c "sleep 5000" -c "reg pc 0x80000000" -c "exit" -d3
+
+mdw 0x1000 16
+mww 0x1000 0x12345678
+mdw 0x1000 16
+
+load_image /media/data/open/VexRiscv/src/test/resources/hex/dhrystoneO3.hex 0 ihex
+mdw 0x80000000 16
+reg pc 0x80000000
+bp 0x80000114 4
+resume
+
+
+mdw 0x1000 16
+mww 0x1000 0x12345678
+mdw 0x1000 16
+
+#load_image /media/data/open/VexRiscv/src/test/resources/hex/dhrystoneO3.hex 0 ihex
+mww 0x80000000 0x13
+mww 0x80000004 0x13
+reg pc 0x80000000
+step; reg pc
+*/

src/test/cpp/regression/jtag.h

@@ -0,0 +1,188 @@
+
+#include <stdio.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <string.h>
+#include <arpa/inet.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <netinet/tcp.h>
+
+/** Returns true on success, or false if there was an error */
+bool SetSocketBlockingEnabled(int fd, bool blocking)
+{
+   if (fd < 0) return false;
+
+#ifdef WIN32
+   unsigned long mode = blocking ? 0 : 1;
+   return (ioctlsocket(fd, FIONBIO, &mode) == 0) ? true : false;
+#else
+   int flags = fcntl(fd, F_GETFL, 0);
+   if (flags < 0) return false;
+   flags = blocking ? (flags&~O_NONBLOCK) : (flags|O_NONBLOCK);
+   return (fcntl(fd, F_SETFL, flags) == 0) ? true : false;
+#endif
+}
+
+class Jtag : public SimElement{
+public:
+	CData *tms, *tdi, *tdo, *tck;
+	enum State {reset};
+	uint32_t state;
+
+	int serverSocket, clientHandle;
+	struct sockaddr_in serverAddr;
+	struct sockaddr_storage serverStorage;
+	socklen_t addr_size;
+	uint64_t tooglePeriod;
+//	char buffer[1024];
+
+    uint32_t timer;
+	Jtag(CData *tms, CData *tdi, CData *tdo, CData* tck,uint64_t period){
+		this->tms = tms;
+		this->tdi = tdi;
+		this->tdo = tdo;
+		this->tck = tck;
+		this->tooglePeriod = period-1;
+		*tms = 0;
+		*tdi = 0;
+		*tdo = 0;
+		*tck = 0;
+		state = 0;
+		timer = 0;
+
+
+		//---- Create the socket. The three arguments are: ----//
+		// 1) Internet domain 2) Stream socket 3) Default protocol (TCP in this case) //
+		serverSocket = socket(PF_INET, SOCK_STREAM, 0);
+		assert(serverSocket != -1);
+		int flag = 1;
+		setsockopt(  serverSocket,            /* socket affected */
+					 IPPROTO_TCP,     /* set option at TCP level */
+					 TCP_NODELAY,     /* name of option */
+					 (char *) &flag,  /* the cast is historical
+											 cruft */
+					 sizeof(int));    /* length of option value */
+
+		/*int a = 0xFFF;
+		if (setsockopt(serverSocket, SOL_SOCKET, SO_RCVBUF, &a, sizeof(int)) == -1) {
+		    fprintf(stderr, "Error setting socket opts: %s\n", strerror(errno));
+		}
+		a = 0xFFFFFF;
+		if (setsockopt(serverSocket, SOL_SOCKET, SO_SNDBUF, &a, sizeof(int)) == -1) {
+		    fprintf(stderr, "Error setting socket opts: %s\n", strerror(errno));
+		}*/
+
+		SetSocketBlockingEnabled(serverSocket,0);
+
+
+		//---- Configure settings of the server address struct ----//
+		// Address family = Internet //
+		serverAddr.sin_family = AF_INET;
+		serverAddr.sin_port = htons(7894);
+		serverAddr.sin_addr.s_addr = inet_addr("127.0.0.1");
+		memset(serverAddr.sin_zero, '\0', sizeof serverAddr.sin_zero);
+
+		//---- Bind the address struct to the socket ----//
+		bind(serverSocket, (struct sockaddr *) &serverAddr, sizeof(serverAddr));
+
+		//---- Listen on the socket, with 5 max connection requests queued ----//
+		listen(serverSocket,1);
+
+		//---- Accept call creates a new socket for the incoming connection ----//
+		addr_size = sizeof serverStorage;
+		clientHandle = -1;
+
+	}
+	void connectionReset(){
+		printf("CONNECTION RESET\n");
+		shutdown(clientHandle,SHUT_RDWR);
+		clientHandle = -1;
+	}
+
+
+	virtual ~Jtag(){
+		if(clientHandle != -1) {
+			shutdown(clientHandle,SHUT_RDWR);
+			usleep(100);
+		}
+		if(serverSocket != -1) {
+			close(serverSocket);
+			usleep(100);
+		}
+	}
+
+	uint32_t selfSleep = 0;
+	uint32_t checkNewConnectionsTimer = 0;
+	uint8_t rxBuffer[100];
+	int32_t rxBufferSize = 0;
+	int32_t rxBufferRemaining = 0;
+
+//    	virtual void onReset(){}
+//    	virtual void postReset(){}
+//    	virtual void preCycle(){}
+//    	virtual void postCycle(){}
+	virtual void postCycle(){
+	    if(timer != 0){
+	        timer -= 1;
+	        return;
+	    }
+		checkNewConnectionsTimer++;
+		if(checkNewConnectionsTimer == 5000){
+			checkNewConnectionsTimer = 0;
+			int newclientHandle = accept(serverSocket, (struct sockaddr *) &serverStorage, &addr_size);
+			if(newclientHandle != -1){
+				if(clientHandle != -1){
+					connectionReset();
+				}
+				clientHandle = newclientHandle;
+				printf("CONNECTED\n");
+			}
+			else{
+				if(clientHandle == -1)
+					selfSleep = 1000;
+			}
+		}
+		if(selfSleep)
+			selfSleep--;
+		else{
+			if(clientHandle != -1){
+				uint8_t buffer;
+				int n;
+
+				if(rxBufferRemaining == 0){
+					if(ioctl(clientHandle,FIONREAD,&n) != 0)
+						connectionReset();
+					else if(n >= 1){
+						rxBufferSize = read(clientHandle,&rxBuffer,100);
+						if(rxBufferSize < 0){
+							connectionReset();
+						}else {
+							rxBufferRemaining = rxBufferSize;
+						}
+					}else {
+						selfSleep = 30;
+					}
+				}
+
+				if(rxBufferRemaining != 0){
+					uint8_t buffer = rxBuffer[rxBufferSize - (rxBufferRemaining--)];
+					*tms = (buffer & 1) != 0;
+					*tdi = (buffer & 2) != 0;
+					*tck = (buffer & 8) != 0;
+					if(buffer & 4){
+						buffer = (*tdo != 0);
+						//printf("TDO=%d\n",buffer);
+						if(-1 == send(clientHandle,&buffer,1,0))
+							connectionReset();
+					}else {
+
+					//	printf("\n");
+					}
+				}
+			}
+		}
+		timer = tooglePeriod;
+	}
+
+};

src/test/cpp/regression/main.cpp

@@ -3071,6 +3071,8 @@ public:
 
 #endif
 
+#include "jtag.h"
+
 void Workspace::fillSimELements(){
 	#ifdef IBUS_SIMPLE
 		simElements.push_back(new IBusSimple(this));
@@ -3121,6 +3123,9 @@ void Workspace::fillSimELements(){
 	#ifdef DEBUG_PLUGIN_AVALON
 		simElements.push_back(new DebugPluginAvalon(this));
 	#endif
+	#ifdef RISCV_JTAG
+		simElements.push_back(new Jtag(&top->jtag_tms, &top->jtag_tdi, &top->jtag_tdo, &top->jtag_tck, 4));
+	#endif
 }
 
 mutex Workspace::staticMutex;
@@ -4126,16 +4131,7 @@ int main(int argc, char **argv, char **env) {
 
 
 
-    #ifdef RVF
+
-    for(const string &name : riscvTestFloat){
-        redo(REDO,RiscvTest(name).withRiscvRef()->bootAt(0x80000188u)->writeWord(0x80000184u, 0x00305073)->run();)
-    }
-    #endif
-    #ifdef RVD
-    for(const string &name : riscvTestDouble){
-        redo(REDO,RiscvTest(name).withRiscvRef()->bootAt(0x80000188u)->writeWord(0x80000184u, 0x00305073)->run();)
-    }
-    #endif
     //return 0;
 
 //#ifdef LITEX
@@ -4365,6 +4361,17 @@ int main(int argc, char **argv, char **env) {
 			redo(REDO,WorkspaceRegression("amo").withRiscvRef()->loadHex(string(REGRESSION_PATH) + "../raw/amo/build/amo.hex")->bootAt(0x00000000u)->run(10e3););
 		#endif
 
+        #ifdef RVF
+        for(const string &name : riscvTestFloat){
+            redo(REDO,RiscvTest(name).withRiscvRef()->bootAt(0x80000188u)->writeWord(0x80000184u, 0x00305073)->run();)
+        }
+        #endif
+        #ifdef RVD
+        for(const string &name : riscvTestDouble){
+            redo(REDO,RiscvTest(name).withRiscvRef()->bootAt(0x80000188u)->writeWord(0x80000184u, 0x00305073)->run();)
+        }
+        #endif
+
 		#ifdef DHRYSTONE
 			Dhrystone("dhrystoneO3_Stall","dhrystoneO3",true,true).run(1.5e6);
 			#if defined(COMPRESSED)

src/test/cpp/regression/makefile

@@ -30,6 +30,7 @@ AMO?=no
 NO_STALL?=no
 DEBUG_PLUGIN?=STD
 DEBUG_PLUGIN_EXTERNAL?=no
+RISCV_JTAG?=no
 RUN_HEX=no
 WITH_RISCV_REF=yes
 CUSTOM_SIMD_ADD?=no
@@ -302,6 +303,12 @@ ifeq ($(DEBUG_PLUGIN_EXTERNAL),yes)
 	ADDCFLAGS += -CFLAGS -DDEBUG_PLUGIN_EXTERNAL
 endif
 
+
+ifeq ($(RISCV_JTAG),yes)
+	ADDCFLAGS += -CFLAGS -DRISCV_JTAG
+endif
+
+
 ifeq ($(REF),yes)
 	ADDCFLAGS += -CFLAGS -DREF
 endif