package spinal.lib.bus.tilelink.coherent

import spinal.core._
import spinal.lib._
import spinal.lib.bus.misc.SizeMapping
import spinal.lib.bus.tilelink._
import spinal.lib.misc.pipeline._

import scala.collection.mutable.ArrayBuffer

object Hub{
  def downM2s(name : Nameable,
              addressWidth : Int,
              dataWidth : Int,
              blockSize : Int,
              downPendingMax : Int) = M2sParameters(
    addressWidth = addressWidth,
    dataWidth = dataWidth,
    masters = List(M2sAgent(
      name = name,
      mapping = List(M2sSource(
        id = SizeMapping(0, 1 << log2Up(downPendingMax)),
        emits = M2sTransfers(
          putPartial = SizeRange.upTo(blockSize),
          putFull = SizeRange.upTo(blockSize),
          get = SizeRange.upTo(blockSize)
        )
      )
      ))
    )
  )

  def upS2m(name : Nameable,
            blockSize : Int,
            setCount : Int) = S2mParameters(List(
    S2mAgent(
      name = name,
      emits = S2mTransfers(
        probe = SizeRange(blockSize)
      ),
      sinkId = SizeMapping(0, setCount*2)
    )
  ))
}

case class HubParameters(var unp : NodeParameters,
                         var downPendingMax : Int,
                         var sets: Int,
                         var wayCount: Int,
                         var blockSize : Int,
                         var probeCount : Int = 8,
                         var aBufferCount: Int = 4,
                         var probeRegion : UInt => Bool) {
  def cacheSize = sets*wayCount*blockSize
  def addressWidth = unp.m.addressWidth
  def dataWidth = unp.m.dataWidth
  def dataBytes = dataWidth/8
  def waySize = cacheSize/wayCount
  def linePerWay = waySize/lineSize
  def tagRange = addressWidth-1 downto log2Up(linePerWay*lineSize)
  def lineRange = tagRange.low-1 downto log2Up(lineSize)
  def wordRange = log2Up(lineSize)-1 downto log2Up(dataBytes)
  def refillRange = tagRange.high downto lineRange.low
  def blockRange = addressWidth-1 downto log2Up(lineSize)
  def lineSize = blockSize
  def setsRange = lineRange
}

case class OrderingCmd(bytesMax : Int) extends Bundle{
  val debugId = DebugId()
  val bytes = UInt(log2Up(bytesMax+1) bits)
}

/*
access main memory :
- read without ownership
- writes without ownership
- refill
- writeback
- release miss

Read cache :
- read without ownership
- read allocate
- writeback

write cache
- writes without ownership on hit
- downstream D (on refill)
- release hit

drive upstream D
- cache hit read
- downstream D (include cache miss refill)


A transaction will always :
- Wait no slot conflicts
- [Fetch tags]
- [Probe stuff]
- [wait probe completion]
- acquire:
  - [writeback/fill cache cmd]
  - [Read data from $]/[GrandData down D to up D]/[grant] + [update tags]
  - wait up.E grant ack [and writeback] completion
- get / put
  - [writeback/fill cache cmd]/[down A]
  - [Read data from $]/[down D to up D]

C : release data
- [Fetch tags]
- on hit [writeback cache] else write write to main mem
- wait completion


down.D may trigger:
- up.D redirection (on up get/put/acquireBlock with data/releaseData), need source context
- conflict release (on up get/put), need sets context
- probe bypass (on ProbeData), need probeId context

up.E will do a conflict release (acquireAck)


$ =>
- [dma no allocate]
- [release allocate]
- [victim buffer]
- [inclusive]

Inclusive tricky things :
- Allocation require making room
- Allocation while a release is also on the way (but easily solved by not allowing same set in the pipeline)

non inclusive :
- Probe filtering via a Set counter memory

 */


/*
Currently when a io region access is done, the HUB will still lock the memory block (but that's not necessary)
 */
class Hub(p : HubParameters) extends Component{
  import p._
  val ubp = p.unp.toBusParameter()
  val dbp = NodeParameters(
    m = Hub.downM2s(
      name           = this,
      addressWidth   = addressWidth,
      dataWidth      = dataWidth,
      blockSize      = blockSize,
      downPendingMax = downPendingMax
    ),
    s = S2mParameters(slaves = Nil)
  ).toBusParameter()

  val io = new Bundle{
    val up = slave(Bus(ubp))
    val down = master(Bus(dbp))
    val backendOrdering = master(Flow(OrderingCmd(up.p.sizeBytes)))
    val probeOrdering = master(Flow(OrderingCmd(up.p.sizeBytes)))
  }

  this.addTag(OrderingTag(io.backendOrdering))
  this.addTag(OrderingTag(io.probeOrdering))


  case class DataPayload() extends Bundle {
    val mask = ubp.mask()
    val data = ubp.data()
  }

  val FROM_C = Payload(Bool())
  val ADDRESS  = Payload(ubp.address)
//  val CAP  = Payload(UInt(2 bits))
  val SET_ID  = Payload(UInt(setsRange.size bits))
  val PROBE_ID  = Payload(UInt(log2Up(probeCount) bits))
  val SLOT_ALLOCATE = Payload(Bool())
  val SIZE  = Payload(ubp.size)
  val SOURCE  = Payload(ubp.source)
  val BUFFER_ID = Payload(UInt(log2Up(aBufferCount) bits))
  val PARTIAL_DATA = Payload(Bool())
  val WRITE_DATA = Payload(Bool())
  val READ_DATA = Payload(Bool())
  val LAST, FIRST = Payload(Bool())
  val PAYLOAD_C, PAYLOAD = Payload(DataPayload())
  val SLOT_ID = Payload(UInt(log2Up(downPendingMax) bits))
  val TO_DOWN = Payload(Bool())
  val FROM_NONE = Payload(Bool())
  val UNIQUE = Payload(Bool())
  val CONFLICT_CTX = Payload(Bool())

  val initializer = new Area{
    val initCycles = p.sets
    val counter = Reg(UInt(log2Up(initCycles) + 1 bits)) init(0)
    val done = counter.msb
    when(!done) {
      counter := counter + 1
    }
  }

  val slots = new Area{
    val valids = Reg(Bits(downPendingMax bits)) init(0)
    val freeOh = OHMasking.firstV2(~valids)
    val freeId = OHToUInt(freeOh)
    val full = valids.andR
    val allocate = Bool()
    val release = Flow(UInt(log2Up(downPendingMax) bits))
    valids := (valids | freeOh.andMask(allocate)) & ~(UIntToOh(release.payload).andMask(release.valid))
    val ctx = Mem.fill(downPendingMax)(Bits((widthOf(CtxA()) max widthOf(CtxC())) + 1 bits))
    val ctxWrite = ctx.writePort()
  }

  val setsBusy = new Area{
    class MemArea extends Area {
      val mem = Mem.fill(sets)(Bool())
      val write = mem.writePort()
      write.valid := !initializer.done
      write.address := initializer.counter.resized
      write.data := False
    }
    val target = new MemArea
    val hit = new MemArea {
      val upE = Stream(write.payload)
      val downD = Stream(write.payload)
      val arbiter = StreamArbiterFactory().noLock.lowerFirst.onArgs(upE, downD).toFlow
      when(initializer.done) {
        write << arbiter
      }
    }
  }

  val frontendA = new StageCtrlPipeline{
    val PAYLOAD = Payload(DataPayload())
    val BUFFER_ID = Payload(UInt(log2Up(aBufferCount) bits))

    val buffer =  ubp.withDataA generate new Area{
      val ram = Mem.fill(aBufferCount*p.blockSize/p.unp.m.dataBytes)(DataPayload())
      val allocated = Reg(Bits(aBufferCount bits)) init(0)
      val set, clear = B(0, aBufferCount bits)
      val firstFree = OHToUInt(OHMasking.firstV2(~allocated))
      val full = allocated.andR
      val source = Vec.fill(aBufferCount)(Reg(ubp.source))
      val write = ram.writePort()
      allocated := (allocated | set) & ~clear
    }

    val spawnNode = ctrl(0)
    val spawn = new spawnNode.Area{
      up.arbitrateFrom(io.up.a)
      haltWhen(!initializer.done)

      val CMD = insert(io.up.a.payload.asNoData())
      for(i <- 0 until ubp.sizeMax) CMD.address(i) clearWhen(CMD.size > i)
      val LAST = io.up.a.isLast()
      if(ubp.withDataA) {
        PAYLOAD.data := io.up.a.data
        PAYLOAD.mask := io.up.a.mask
      }
      val dataSplit = ubp.withDataA generate new Area{
        val withBeats = io.up.a.payload.withBeats
        val hazard = withBeats && buffer.full
        haltWhen(hazard)
        throwWhen(!hazard && !LAST)

        val locked = RegInit(False) setWhen(buffer.write.valid)
        val lockedValue = RegNextWhen(buffer.firstFree, !locked)
        BUFFER_ID := locked ? lockedValue | buffer.firstFree
        buffer.write.valid := up.valid && withBeats && !hazard
        buffer.write.address := BUFFER_ID @@ io.up.a.beatAddress(wordRange)
        buffer.write.data := PAYLOAD
        when(up.isFiring && LAST && withBeats){
          buffer.set(BUFFER_ID) := True
          buffer.source.onSel(BUFFER_ID)(_ := io.up.a.payload.source)
          locked := False
        }
      }
    }

    val readConflictsNode = ctrl(0)
    val readConflicts = new readConflictsNode.Area{
      val readAddress = spawn.CMD.address(setsRange)
      val JUST_BUSY = insert(setsBusy.target.write.valid && setsBusy.target.write.address === readAddress)
      val JUST_FREE = insert(setsBusy.hit.write.valid && setsBusy.hit.write.address === readAddress)
    }

    val checkConflictsNode = ctrl(1)
    val checkConflicts = new checkConflictsNode.Area{
      import readConflicts._

      val hitPort = setsBusy.hit.mem.readSyncPort()
      hitPort.cmd.valid := !up.isValid || up.isMoving
      hitPort.cmd.payload := readAddress
      hitPort.writeFirstAndUpdate(setsBusy.hit.write)

      val targetPort = setsBusy.target.mem.readSyncPort()
      targetPort.cmd.valid := !up.isValid || up.isMoving
      targetPort.cmd.payload := readAddress
      targetPort.writeFirstAndUpdate(setsBusy.target.write)

      val hit    = hitPort.rsp
      val target = targetPort.rsp
      val hazard = hit =/= target
      haltWhen(hazard)

      when(initializer.done) {
        setsBusy.target.write.valid := up.isFiring
        setsBusy.target.write.address := spawn.CMD.address(setsRange)
        setsBusy.target.write.data := !target
      }

      CONFLICT_CTX := !target
    }
  }

  val coherentMasters = unp.m.masters.filter(_.emits.withBCE)
  val coherentMasterCount = coherentMasters.size
  val coherentMasterToSource = coherentMasters.map(_.bSourceId)
  def MasterId() = UInt(log2Up(coherentMasterCount) bits)
  class ProbeCtx extends Bundle{
    val opcode  = Opcode.A()
    val address = ubp.address()
    val size    = ubp.size()
    val toTrunk = Bool()
    val source  = ubp.source()
    val debugId  = DebugId()
    val bufferId = ubp.withDataA generate UInt(log2Up(aBufferCount) bits)
    val conflictCtx = CONFLICT_CTX()
  }
  class ProbeCtxFull extends ProbeCtx{
    val probeId = UInt(log2Up(probeCount) bits)
  }

  case class ProbeCmd() extends Bundle {
    val ctx = new ProbeCtx()
    val fromNone = Bool()
    val selfMask = Bits(coherentMasterCount bits)
    val mask = Bits(coherentMasterCount bits)
  }

  case class CtxA() extends Bundle{
    val source = ubp.source()
    val set = SET_ID()
    val getPut = Bool()
    val toTrunk = Bool()
    val conflictCtx = CONFLICT_CTX()
  }
  case class CtxC() extends Bundle{
    val source = ubp.source()
    val isProbeData = Bool()
    def isReleaseData = !isProbeData
    val probeId = PROBE_ID()
  }

  val probe = new Area{
    val isl = frontendA.last
    val push = Stream(ProbeCmd())
    val pushCmd = isl(frontendA.spawn.CMD)
    val isAcquire = Opcode.A.isAcquire(pushCmd.opcode)
    push.valid := isl.isValid && !isl.requests.halts.orR
    isl.haltWhen(!push.ready)
    push.ctx.opcode     := pushCmd.opcode
    push.ctx.address    := pushCmd.address
    push.ctx.toTrunk    := pushCmd.param =/= Param.Grow.NtoB
    push.ctx.debugId    := pushCmd.debugId
    if(ubp.withDataA) push.ctx.bufferId := isl(frontendA.BUFFER_ID)
    push.ctx.conflictCtx := isl(CONFLICT_CTX)
    push.ctx.size       := pushCmd.size
    push.ctx.source     := pushCmd.source
    push.fromNone := pushCmd.param =/= Param.Grow.BtoT && isAcquire
    push.selfMask   := B(coherentMasters.map(_.sourceHit(pushCmd.source))).andMask(isAcquire)
    push.mask := ~push.selfMask
    when(!push.fromNone){
      push.mask.setAll()
    }
    val isProbeRegion = p.probeRegion(pushCmd.address)
    when(!isProbeRegion){
      push.mask.clearAll()
    }


    val slots = for(i <- 0 until probeCount) yield new Area{
      val fire = False
      val valid = RegInit(False) clearWhen(fire)
      val set = Reg(UInt(setsRange.size bits))
      val pending = Reg(UInt(log2Up(coherentMasterCount+1) bits))
      val fromNone = Reg(Bool())
      val unique = Reg(Bool())
      val probeAckDataCompleted = Reg(Bool())
      val selfId = Reg(UInt(log2Up(coherentMasterCount) bits))
      val done = valid && (pending & ~U(probeAckDataCompleted, widthOf(pending) bits)) === 0
    }

    val ctxMem = Mem.fill(probeCount)(new ProbeCtx())
    val ctxWrite = ctxMem.writePort()

    val cmd = new Area{
      val slotted = RegInit(False)
      val full = slots.map(_.valid).andR
      val freeMask = OHMasking.firstV2(B(slots.map(!_.valid)))
      val freeId = OHToUInt(freeMask)
      val pending = CountOne(push.mask)
      val selfId = OHToUInt(push.selfMask)

      ctxWrite.valid := False
      ctxWrite.address := freeId
      ctxWrite.data := push.ctx
      when(push.valid && !slotted && !full){
        slots.onMask(freeMask){s =>
          s.valid := True
          s.set := push.ctx.address(setsRange)
          s.pending := pending
          s.fromNone := push.fromNone
          s.unique := True
          s.probeAckDataCompleted := False
          s.selfId := selfId
        }
        ctxWrite.valid := True
        slotted := True
      }

      val bus = io.up.b
      val halted = push.haltWhen(!slotted && full)
      val fired = Reg(Bits(coherentMasterCount bits)) init(0)
      val isPut = Opcode.A.isPut(halted.ctx.opcode)
      val isAcquire = Opcode.A.isAcquire(halted.ctx.opcode)
      val requests = push.mask & ~fired
      val masterOh = OHMasking.firstV2(requests)
      val selfProbe = (masterOh & halted.selfMask).orR
      val removeBranches = isPut || isAcquire && halted.ctx.toTrunk && !selfProbe
      bus.valid   := halted.valid && requests.orR
      bus.opcode  := Opcode.B.PROBE_BLOCK
      bus.param   := removeBranches ? B(Param.Cap.toN, 3 bits) | B(Param.Cap.toB, 3 bits)
      bus.source  := OhMux(masterOh, coherentMasterToSource.map(id => U(id, ubp.sourceWidth bits)).toList)
      bus.address := halted.ctx.address(blockRange) @@ U(0, blockRange.low bits)
      bus.size    := log2Up(p.blockSize)
      halted.ready := requests === 0

      when(bus.fire) {
        fired.asBools.onMask(masterOh)(_ := True)
      }

      when(halted.ready) {
        fired := 0
        slotted := False
      }
    }

    val rsp = new Area{
      val bypass = Flow(UInt(log2Up(probeCount) bits))
      val input = io.up.c.haltWhen(bypass.valid)
      val ack = input.fire && input.opcode === Opcode.C.PROBE_ACK
      val hitOh = slots.map(s => s.valid && s.set === input.address(setsRange))
      val hitId = OHToUInt(hitOh)
      val pending = slots.map(_.pending).read(hitId)
      val selfId = slots.map(_.selfId).read(hitId)
      val pendingNext = pending - 1
      val masterOh = coherentMasterToSource.map(input.source === _).asBits
      val masterId = OHToUInt(masterOh)
      val isSelf = selfId === masterId
      val lostIt = isSelf && input.param === Param.Report.NtoN

      when(bypass.valid){
        hitId := bypass.payload
        lostIt := False
      }

      when(ack || bypass.valid) {
        slots.onSel(hitId) { s =>
          s.pending := pendingNext
          s.fromNone setWhen (lostIt)
        }
      }

      when(input.fire && (input.opcode === Opcode.C.PROBE_ACK || input.opcode === Opcode.C.PROBE_ACK_DATA)){
        when(!isSelf && Param.reportPruneKeepCopy(input.param)){
          slots.onSel(hitId) { s =>
            s.unique := False
          }
        }
      }

      val toBackend = cloneOf(input)
      val toBackendProbeId = hitId
      val hitBackend = !Opcode.C.withoutData(input.opcode)
      toBackend.valid := input.valid && hitBackend
      toBackend.payload := input.payload

      val hitUpD = input.opcode === Opcode.C.RELEASE
      val toUpD = Stream(io.up.d.payloadType)
      toUpD.valid := input.valid && hitUpD
      toUpD.opcode  := Opcode.D.RELEASE_ACK
      toUpD.param   := 0
      toUpD.source  := input.source
      toUpD.size    := input.size
      toUpD.denied  := False
      toUpD.corrupt := False
      toUpD.data.assignDontCare()
      toUpD.sink.assignDontCare()

      input.ready := !(hitBackend && !toBackend.ready) && !(hitUpD && !toUpD.ready)
    }

    val wake = new StageCtrlPipeline{
      val CTX = Payload(new ProbeCtx())
      val toBackend = Stream(new ProbeCtxFull())
      val toUpD = cloneOf(io.up.d)

      val insertionNode = ctrl(0)
      val insertion = new insertionNode.Area {
        val hits = slots.map(_.done).asBits
        up.valid := hits.orR
        val hitOh = OHMasking.roundRobinNext(hits, up.isFiring)
        val hitId = OHToUInt(hitOh)
        PROBE_ID := hitId
        when(up.isFiring){
          slots.onMask(hitOh){ s =>
            s.fire := True
          }
        }
        FROM_NONE := OhMux.or(hitOh, slots.map(_.fromNone))
        UNIQUE := OhMux.or(hitOh, slots.map(_.unique))
      }

      val ctxFetcherNode = ctrl(1)
      val ctxFetcher = new ctxFetcherNode.Area {
        CTX := ctxMem.readSync(insertion.hitId, !up.isValid || up.isMoving)
        val hitUpD = CTX.opcode === Opcode.A.ACQUIRE_PERM ||
                     CTX.opcode === Opcode.A.ACQUIRE_BLOCK && !FROM_NONE

        toBackend.valid := isValid && !hitUpD
        toBackend.payload.assignSomeByName(CTX)
        toBackend.probeId := PROBE_ID
        when(UNIQUE){
          toBackend.toTrunk := True
        }

        toUpD.valid := isValid && hitUpD
        toUpD.opcode  := Opcode.D.GRANT
        toUpD.param   := 0
        toUpD.source  := CTX.source
        toUpD.size    := CTX.size
        toUpD.sink    := U(CTX.conflictCtx ## CTX.address(setsRange))
        toUpD.denied  := False
        toUpD.corrupt := False
        toUpD.data.assignDontCare()

        io.probeOrdering.valid := toUpD.fire
        io.probeOrdering.debugId := CTX.debugId
        io.probeOrdering.bytes := (U(1) << toUpD.size).resized

        haltWhen(hitUpD ? !toUpD.ready | !toBackend.ready)
      }
    }
  }



  val backend = new StageCtrlPipeline {
    val insertionNode = ctrl(0)
    val insertion = new insertionNode.Area{
      val c = probe.rsp.toBackend
      val cProbeId = probe.rsp.toBackendProbeId
      val a = probe.wake.toBackend

      val lockValid = RegInit(False) setWhen(up.valid) clearWhen(up.isFiring && LAST)
      val lockSel = Reg(Bool())
      val selC = lockValid ? lockSel | c.valid
      lockSel := selC


      val counter = Reg(UInt(wordRange.size bits)) init(0)
      when(up.isFiring){
        counter := counter + 1
        when(LAST){
          counter := 0
        }
      }

      up.valid := selC ? c.valid | a.valid
      FROM_C :=  selC
      when(selC){
        WRITE_DATA := c.withBeats
        READ_DATA := False
        PARTIAL_DATA := False
        ADDRESS := c.address
        SIZE := log2Up(blockSize)
        SOURCE := c.source
        PROBE_ID := cProbeId
      } otherwise {
        WRITE_DATA := Opcode.A.isPut(a.opcode)
        READ_DATA  := Opcode.A.isGet(a.opcode) || a.opcode === Opcode.A.ACQUIRE_BLOCK
        PARTIAL_DATA := a.opcode === Opcode.A.PUT_PARTIAL_DATA
        ADDRESS := a.address
        ADDRESS(wordRange) := a.address(wordRange) + counter
        SIZE := a.size
        SOURCE := a.source
        PROBE_ID := a.probeId
      }
      LAST := READ_DATA || WRITE_DATA && counter === sizeToBeatMinusOne(ubp, SIZE)
      if(ubp.withDataA) BUFFER_ID := a.bufferId
      CONFLICT_CTX := a.conflictCtx

      c.ready := up.isReady &&  selC
      a.ready := up.isReady && !selC && LAST

      PAYLOAD_C.data := c.data
      PAYLOAD_C.mask.setAll()

      val first = RegNextWhen[Bool](LAST, up.isFiring) init(True)
      FIRST := first

      val A_GET_PUT  = insert(Opcode.A.isGetPut(a.opcode))
      val A_TO_TRUNK = insert(a.toTrunk)
      val C_IS_PROBE_DATA = insert(c.opcode === Opcode.C.PROBE_ACK_DATA)

      //need update when $
      TO_DOWN := WRITE_DATA | READ_DATA
      SLOT_ALLOCATE := TO_DOWN

      io.backendOrdering.valid := up.isFiring && !FROM_C
      io.backendOrdering.debugId := a.debugId
      io.backendOrdering.bytes := (U(1) << a.size).resized
    }

    val aPayloadSyncRead = new Area{
      val s0 = ctrl(0)
      val s1 = ctrl(1)
      val wordAddress = s0(BUFFER_ID) @@ s0(ADDRESS)(wordRange)

      s1(PAYLOAD) := s1(PAYLOAD_C)
      if(ubp.withDataA) {
        when(!s1(FROM_C)){
          s1(PAYLOAD) := frontendA.buffer.ram.readSync(wordAddress, !s1.up.isValid || s1.up.isMoving)
        }
        when(s1.up.isFiring && s1(LAST) && !s1(FROM_C) && s1(WRITE_DATA)){
          frontendA.buffer.clear(s1(BUFFER_ID)) := True
        }
      }
    }

    val allocateNode = ctrl(0)
    val allocateSlot = new allocateNode.Area{
      val slotLock = RegNextWhen(!LAST, up.isFiring) init(False)
      val slotReg = RegNextWhen(slots.freeId, !slotLock)
      SLOT_ID := slotLock ? slotReg | slots.freeId
      haltWhen(SLOT_ALLOCATE && slots.full)

      val a = CtxA()
      a.source := SOURCE
      a.set    := ADDRESS(setsRange)
      a.getPut  := insertion.A_GET_PUT
      a.toTrunk := insertion.A_TO_TRUNK
      a.conflictCtx := CONFLICT_CTX

      val c = CtxC()
      c.source      := SOURCE
      c.isProbeData := insertion.C_IS_PROBE_DATA
      c.probeId     := PROBE_ID

      slots.allocate := up.isFiring && SLOT_ALLOCATE && FIRST
      slots.ctxWrite.valid := up.isFiring && SLOT_ALLOCATE
      slots.ctxWrite.address := SLOT_ID
      slots.ctxWrite.data.assignDontCare()
      slots.ctxWrite.data(0) := FROM_C
      when(FROM_C){
        slots.ctxWrite.data(1, widthOf(c) bits) := B(c)
      } otherwise {
        slots.ctxWrite.data(1, widthOf(a) bits) := B(a)
      }
      assert(widthOf(a) < widthOf(slots.ctxWrite.data))
      assert(widthOf(c) < widthOf(slots.ctxWrite.data))
    }

    /* The origianl pipeline is 3-stage, so this should be stage 2 instead of the use pipeline last stage */
    val downNode = ctrl(2)
    val toDown = new downNode.Area{
      haltWhen(TO_DOWN && !io.down.a.ready)
      io.down.a.valid := up.valid && TO_DOWN
      io.down.a.opcode := WRITE_DATA ? (PARTIAL_DATA ? Opcode.A.PUT_PARTIAL_DATA | Opcode.A.PUT_FULL_DATA) | Opcode.A.GET
      io.down.a.param := 0
      io.down.a.source := SLOT_ID
      io.down.a.address := ADDRESS
      io.down.a.size := SIZE
      io.down.a.data := PAYLOAD.data
      io.down.a.mask := PAYLOAD.mask
      io.down.a.corrupt := False
      io.down.a.debugId := DebugId.withPostfix(io.down.a.source)

      val first = RegNextWhen[Bool](LAST, up.isFiring) init(True)
      val base = RegNextWhen(ADDRESS(wordRange), up.isFiring && first)
      when(!first){
        io.down.a.address(wordRange) := base
      }
    }
  }

  val downD = new StageCtrlPipeline{
    val D_PAYLOAD = Payload(io.down.d.payloadType)
    val CTX = Payload(slots.ctx.wordType)

    val insertNode = ctrl(0)
    val insert = new insertNode.Area{
      up.arbitrateFrom(io.down.d)
      D_PAYLOAD := io.down.d
      LAST := io.down.d.isLast()
    }

    val ctxReadSync = new Area{
      val s0 = ctrl(0)
      val s1 = ctrl(1)
      val wordAddress = s0(D_PAYLOAD).source
      s1(CTX) := slots.ctx.readSync(wordAddress, !s1.up.isValid || s1.up.isMoving)
    }

    def decodeCtx(ctx : Bits) = new Area{
      val fromC = ctx(0)
      val a = (ctx >> 1).resized.as(CtxA())
      val c = (ctx >> 1).resized.as(CtxC())
      val hit = fromC ? (!c.isProbeData) | True
    }

    val slotNode = ctrl(1)
    val slotRelease = new slotNode.Area{
      val ctx = decodeCtx(CTX)

      val fired = RegInit(False) setWhen(isValid) clearWhen(up.isReady)

      slots.release.valid := up.valid && !fired && LAST
      slots.release.payload := D_PAYLOAD.source
    }

    val busyNode = ctrl(1)
    val toSetsBusy = new busyNode.Area{
      val ctx = decodeCtx(CTX)
      val fired = RegInit(False) setWhen(setsBusy.hit.downD.fire) clearWhen(up.isReady)
      val hit = !ctx.fromC && ctx.a.getPut && LAST && !fired

      setsBusy.hit.downD.valid := up.valid && hit
      setsBusy.hit.downD.address := ctx.a.set
      setsBusy.hit.downD.data := ctx.a.conflictCtx

      haltWhen(hit && !setsBusy.hit.downD.ready)
    }

    val probeNode = ctrl(1)
    val toProbe = new probeNode.Area{
      val ctx = decodeCtx(CTX)
      val hit = ctx.fromC && ctx.c.isProbeData
      val fired = RegInit(False) setWhen(probe.rsp.bypass.fire) clearWhen(up.isReady)
      probe.rsp.bypass.valid := up.valid && hit && ! fired
      probe.rsp.bypass.payload := ctx.c.probeId
    }

    val upNode = ctrl(1)
    val toUp = new upNode.Area{
      val upD = cloneOf(io.up.d)

      val ctx = decodeCtx(CTX)
      val hit = ctx.fromC ? (!ctx.c.isProbeData) | True
      val fired = RegInit(False) setWhen(upD.fire) clearWhen(up.isReady)
      upD.valid := isValid && hit && !fired
      upD.payload := D_PAYLOAD
      upD.param.removeAssignments() := 0
      upD.source.removeAssignments()
      upD.sink.removeAssignments() := U(ctx.a.conflictCtx ## ctx.a.set)
      when(ctx.fromC){
        upD.opcode := Opcode.D.RELEASE_ACK
        upD.source := ctx.c.source
      } otherwise {
        upD.source := ctx.a.source
        when(!ctx.a.getPut) {
          upD.opcode := Opcode.D.GRANT_DATA
          upD.param := ctx.a.toTrunk ? B(Param.Cap.toT, 3 bits) | B(Param.Cap.toB, 3 bits)
        }
      }
      haltWhen(!fired && hit && !upD.ready)
    }
  }

  val upD = new Area{
    val arbitered = StreamArbiterFactory().lowerFirst.lambdaLock[ChannelD](_.isLast()).onArgs(
      downD.toUp.upD,
      probe.rsp.toUpD.stage(),
      probe.wake.toUpD.stage()
    )
    io.up.d << arbitered
  }

  val upE = new Area{
    setsBusy.hit.upE.arbitrationFrom(io.up.e)
    setsBusy.hit.upE.address := io.up.e.sink.resized
    setsBusy.hit.upE.data := io.up.e.sink.msb
  }

  frontendA.build()
  probe.wake.build()
  backend.build()
  downD.build()
}



object HubGen extends App{
  def basicConfig(probeCount : Int = 8, downPendingMax : Int = 16, masterPerChannel : Int = 4, dataWidth : Int = 64, addressWidth : Int = 13, setCount : Int = 256, wayCount : Int = 2, lineSize : Int = 64) = {
    val blockSize = 64
    HubParameters(
      unp = NodeParameters(
        m = M2sParameters(
          addressWidth = addressWidth,
          dataWidth = dataWidth,
          masters = List.tabulate(masterPerChannel)(mId =>
            M2sAgent(
              name = null,
              mapping = List.fill(1)(M2sSource(
                emits = M2sTransfers(
                  get = SizeRange(64),
                  putFull = SizeRange(64),
                  putPartial = SizeRange(64),
                  acquireT = SizeRange(64),
                  acquireB = SizeRange(64)
                ),
                id = SizeMapping(mId*4, 4)
              ))
            ))
        ),
        s = S2mParameters(List(
          S2mAgent(
            name = null,
            emits = S2mTransfers(
              probe = SizeRange(64)
            ),
            sinkId = SizeMapping(0, setCount*2)
          )
        ))
      ),
      downPendingMax = downPendingMax,
      probeCount = probeCount,
      sets = setCount,
      wayCount  = wayCount,
      blockSize = blockSize,
      probeRegion = _ => True
    )
  }
  def gen = new Hub(HubGen.basicConfig(8, masterPerChannel = 4, dataWidth = 16, addressWidth = 32))
  SpinalVerilog(gen)

}

object HubSynt extends App{
  import spinal.lib.eda.bench._
  val rtls = ArrayBuffer[Rtl]()
  for(probeCount <- List(2, 8, 16)) { //Rtl.ffIo
    rtls += Rtl(SpinalVerilog((new Hub(HubGen.basicConfig(probeCount = probeCount, masterPerChannel = 4, dataWidth = 16, addressWidth = 32)).setDefinitionName(s"Hub$probeCount"))))
  }
  val targets = XilinxStdTargets().take(2) ++ AlteraStdTargets(quartusCycloneIIPath = null)

  Bench(rtls, targets)
}


object HubSyntLight extends App{
  import spinal.lib.eda.bench._
  val rtls = ArrayBuffer[Rtl]()
  rtls += Rtl(SpinalVerilog((new Hub(HubGen.basicConfig(
    probeCount = 2,
    downPendingMax = 4,
    masterPerChannel = 2,
    dataWidth = 16,
    addressWidth = 32
  )).setDefinitionName(s"Hub"))))

  val targets = XilinxStdTargets().take(2) ++ AlteraStdTargets(quartusCycloneIIPath = null)

  Bench(rtls, targets)
  /*

Artix 7 -> 180 MHz 336 LUT 381 FF
Artix 7 -> 315 MHz 404 LUT 381 FF
Cyclone V -> FAILED
Cyclone IV -> 152 MHz 488 LUT 524 FF

   */
}

/*
Hub16 ->
Artix 7 -> 163 MHz 164 LUT 215 FF
Artix 7 -> 268 MHz 180 LUT 215 FF
Cyclone V -> FAILED
Cyclone IV -> 181 MHz 217 LUT 214 FF

Hub16 ->
Artix 7 -> 165 MHz 160 LUT 215 FF
Artix 7 -> 269 MHz 174 LUT 215 FF
Cyclone V -> FAILED
Cyclone IV -> 181 MHz 217 LUT 214 FF

Hub16 ->
Artix 7 -> 198 MHz 218 LUT 182 FF
Artix 7 -> 280 MHz 250 LUT 182 FF
Cyclone V -> 192 MHz 155 ALMs
Cyclone IV -> 169 MHz 300 LUT 215 FF

Artix 7 -> 145 MHz 251 LUT 206 FF
Artix 7 -> 253 MHz 281 LUT 206 FF
Cyclone V -> 183 MHz 180 ALMs
Cyclone IV -> 162 MHz 341 LUT 239 FF




Hub2 ->
Artix 7 -> 212 MHz 167 LUT 237 FF
Artix 7 -> 357 MHz 179 LUT 237 FF
Cyclone V -> 214 MHz 109 ALMs
Cyclone IV -> 180 MHz 224 LUT 307 FF
Hub8 ->
Artix 7 -> 151 MHz 296 LUT 333 FF
Artix 7 -> 234 MHz 326 LUT 333 FF
Cyclone V -> 193 MHz 201 ALMs
Cyclone IV -> 147 MHz 550 LUT 625 FF
Hub16 ->
Artix 7 -> 117 MHz 476 LUT 461 FF
Artix 7 -> 205 MHz 540 LUT 461 FF
Cyclone V -> FAILED
Cyclone IV -> 135 MHz 569 LUT 420 FF


Hub2 ->
Artix 7 -> 178 MHz 225 LUT 327 FF
Artix 7 -> 327 MHz 247 LUT 327 FF
Cyclone V -> FAILED
Cyclone IV -> 176 MHz 319 LUT 351 FF
Hub8 ->
Artix 7 -> 162 MHz 351 LUT 423 FF
Artix 7 -> 241 MHz 385 LUT 423 FF
Cyclone V -> FAILED
Cyclone IV -> 152 MHz 641 LUT 681 FF
Hub16 ->
Artix 7 -> 103 MHz 526 LUT 551 FF
Artix 7 -> 191 MHz 607 LUT 551 FF
Cyclone V -> FAILED
Cyclone IV -> 138 MHz 654 LUT 458 FF

Hub2 ->
Artix 7 -> 216 MHz 238 LUT 327 FF
Artix 7 -> 363 MHz 255 LUT 327 FF
Cyclone V -> FAILED
Cyclone IV -> 184 MHz 315 LUT 349 FF
Hub8 ->
Artix 7 -> 144 MHz 345 LUT 423 FF
Artix 7 -> 250 MHz 386 LUT 423 FF
Cyclone V -> FAILED
Cyclone IV -> 151 MHz 650 LUT 673 FF
Hub16 ->
Artix 7 -> 124 MHz 598 LUT 551 FF
Artix 7 -> 188 MHz 698 LUT 551 FF
Cyclone V -> FAILED
Cyclone IV -> 133 MHz 674 LUT 459 FF


Hub2 ->
Artix 7 -> 153 MHz 276 LUT 338 FF
Artix 7 -> 339 MHz 313 LUT 338 FF
Cyclone V -> FAILED
Cyclone IV -> 147 MHz 351 LUT 389 FF
Hub8 ->
Artix 7 -> 103 MHz 392 LUT 436 FF
Artix 7 -> 228 MHz 448 LUT 436 FF
Cyclone V -> FAILED
Cyclone IV -> 123 MHz 689 LUT 721 FF
Hub16 ->
Artix 7 -> 119 MHz 635 LUT 565 FF
Artix 7 -> 188 MHz 738 LUT 565 FF
Cyclone V -> FAILED
Cyclone IV -> 115 MHz 707 LUT 499 FF

Process finished with exit code 0

Hub2 ->
Artix 7 -> 116 MHz 284 LUT 359 FF
Artix 7 -> 309 MHz 324 LUT 359 FF
Cyclone V -> FAILED
Cyclone IV -> 134 MHz 372 LUT 412 FF
Hub8 ->
Artix 7 -> 153 MHz 395 LUT 457 FF
Artix 7 -> 243 MHz 444 LUT 457 FF
Cyclone V -> FAILED
Cyclone IV -> 117 MHz 726 LUT 768 FF
Hub16 ->
Artix 7 -> 118 MHz 572 LUT 586 FF
Artix 7 -> 194 MHz 661 LUT 586 FF
Cyclone V -> FAILED
Cyclone IV -> 115 MHz 726 LUT 510 FF

Process finished with exit code 0

INFO: [Common 17-206] Exiting Vivado at Thu Jun 15 08:31:51 2023...
Hub2 ->
Artix 7 -> 101 MHz 319 LUT 371 FF
Artix 7 -> 305 MHz 381 LUT 371 FF
Cyclone V -> FAILED
Cyclone IV -> 148 MHz 396 LUT 427 FF
Hub8 ->
Artix 7 -> 97 MHz 428 LUT 468 FF
Artix 7 -> 233 MHz 493 LUT 468 FF
Cyclone V -> FAILED
Cyclone IV -> 121 MHz 754 LUT 795 FF
Hub16 ->
Artix 7 -> 122 MHz 656 LUT 597 FF
Artix 7 -> 189 MHz 792 LUT 597 FF
Cyclone V -> FAILED
Cyclone IV -> 117 MHz 745 LUT 519 FF

Process finished with exit code 0

INFO: [Common 17-206] Exiting Vivado at Thu Jun 15 09:27:01 2023...
Hub2 ->
Artix 7 -> 160 MHz 316 LUT 379 FF
Artix 7 -> 312 MHz 382 LUT 379 FF
Cyclone V -> FAILED
Cyclone IV -> 132 MHz 413 LUT 435 FF
Hub8 ->
Artix 7 -> 154 MHz 433 LUT 477 FF
Artix 7 -> 248 MHz 498 LUT 477 FF
Cyclone V -> FAILED
Cyclone IV -> 125 MHz 773 LUT 803 FF
Hub16 ->
Artix 7 -> 118 MHz 667 LUT 606 FF
Artix 7 -> 184 MHz 820 LUT 606 FF
Cyclone V -> FAILED
Cyclone IV -> 118 MHz 765 LUT 527 FF

Process finished with exit code 0

INFO: [Common 17-206] Exiting Vivado at Thu Jun 15 12:24:32 2023...
Hub2 ->
Artix 7 -> 180 MHz 377 LUT 406 FF
Artix 7 -> 311 MHz 446 LUT 406 FF
Cyclone V -> FAILED
Cyclone IV -> 121 MHz 512 LUT 475 FF
Hub8 ->
Artix 7 -> 136 MHz 508 LUT 510 FF
Artix 7 -> 238 MHz 588 LUT 510 FF
Cyclone V -> FAILED
Cyclone IV -> 115 MHz 893 LUT 855 FF
Hub16 ->
Artix 7 -> 119 MHz 734 LUT 647 FF
Artix 7 -> 199 MHz 835 LUT 647 FF
Cyclone V -> FAILED
Cyclone IV -> 119 MHz 879 LUT 578 FF

Process finished with exit code 0


Hub2 ->
Artix 7 -> 126 MHz 387 LUT 412 FF
Artix 7 -> 292 MHz 461 LUT 412 FF
Cyclone V -> FAILED
Cyclone IV -> 123 MHz 529 LUT 481 FF
Hub8 ->
Artix 7 -> 160 MHz 515 LUT 514 FF
Artix 7 -> 230 MHz 598 LUT 514 FF
Cyclone V -> FAILED
Cyclone IV -> 113 MHz 907 LUT 859 FF
Hub16 ->
Artix 7 -> 116 MHz 737 LUT 650 FF
Artix 7 -> 193 MHz 852 LUT 650 FF
Cyclone V -> FAILED
Cyclone IV -> 114 MHz 898 LUT 581 FF

Process finished with exit code 0

 */
