From 588b944ed433d75b52788c47d794c736b202b91f Mon Sep 17 00:00:00 2001
From: "Wesley W. Terpstra" <wesley@sifive.com>
Date: Thu, 8 Dec 2016 00:06:32 -0800
Subject: [PATCH] ahb: implement and test address decoding

---
 .../scala/uncore/ahb/RegisterRouter.scala     |  5 +-
 src/main/scala/uncore/ahb/Test.scala          |  6 ++-
 src/main/scala/uncore/ahb/Xbar.scala          | 50 +++++++++++++++++++
 3 files changed, 58 insertions(+), 3 deletions(-)
 create mode 100644 src/main/scala/uncore/ahb/Xbar.scala

diff --git a/src/main/scala/uncore/ahb/RegisterRouter.scala b/src/main/scala/uncore/ahb/RegisterRouter.scala
index dcdd30ba..204830f9 100644
--- a/src/main/scala/uncore/ahb/RegisterRouter.scala
+++ b/src/main/scala/uncore/ahb/RegisterRouter.scala
@@ -13,8 +13,8 @@ class AHBRegisterNode(address: AddressSet, concurrency: Int = 0, beatBytes: Int
     Seq(AHBSlaveParameters(
       address       = Seq(address),
       executable    = executable,
-      supportsWrite = TransferSizes(1, beatBytes * AHBParameters.maxTransfer),
-      supportsRead  = TransferSizes(1, beatBytes * AHBParameters.maxTransfer))),
+      supportsWrite = TransferSizes(1, min(address.alignment.toInt, beatBytes * AHBParameters.maxTransfer)),
+      supportsRead  = TransferSizes(1, min(address.alignment.toInt, beatBytes * AHBParameters.maxTransfer)))),
     beatBytes  = beatBytes))
 {
   require (address.contiguous)
@@ -45,6 +45,7 @@ class AHBRegisterNode(address: AddressSet, concurrency: Int = 0, beatBytes: Int
     in.bits.mask  := d_mask
     in.bits.extra := UInt(0)
 
+    when (ahb.hready) { d_phase := Bool(false) }
     ahb.hreadyout := !d_phase || out.valid
     ahb.hresp     := AHBParameters.RESP_OKAY
     ahb.hrdata    := out.bits.data
diff --git a/src/main/scala/uncore/ahb/Test.scala b/src/main/scala/uncore/ahb/Test.scala
index 44df8ec7..1c759fe3 100644
--- a/src/main/scala/uncore/ahb/Test.scala
+++ b/src/main/scala/uncore/ahb/Test.scala
@@ -20,10 +20,14 @@ class AHBFuzzBridge()(implicit p: Parameters) extends LazyModule
 {
   val fuzz  = LazyModule(new TLFuzzer(5000))
   val model = LazyModule(new TLRAMModel("AHBFuzzMaster"))
+  var xbar  = LazyModule(new AHBFanout)
   val ram   = LazyModule(new AHBRAM(AddressSet(0x0, 0xff)))
+  val gpio  = LazyModule(new RRTest0(0x100))
 
   model.node := fuzz.node
-  ram.node := TLToAHB()(model.node)
+  xbar.node := TLToAHB()(model.node)
+  ram.node  := xbar.node
+  gpio.node := xbar.node
 
   lazy val module = new LazyModuleImp(this) with HasUnitTestIO {
     io.finished := fuzz.module.io.finished
diff --git a/src/main/scala/uncore/ahb/Xbar.scala b/src/main/scala/uncore/ahb/Xbar.scala
new file mode 100644
index 00000000..41ccf55f
--- /dev/null
+++ b/src/main/scala/uncore/ahb/Xbar.scala
@@ -0,0 +1,50 @@
+// See LICENSE.SiFive for license details.
+
+package uncore.ahb
+
+import Chisel._
+import config._
+import diplomacy._
+import regmapper._
+import scala.math.{min,max}
+
+class AHBFanout()(implicit p: Parameters) extends LazyModule {
+  val node = AHBAdapterNode(
+    numSlavePorts  = 1 to 1,
+    numMasterPorts = 1 to 32,
+    masterFn = { case Seq(m) => m },
+    slaveFn  = { seq => seq(0).copy(slaves = seq.flatMap(_.slaves)) })
+
+  lazy val module = new LazyModuleImp(this) {
+    val io = new Bundle {
+      val in = node.bundleIn
+      val out = node.bundleOut
+    }
+
+    // Require consistent bus widths
+    val port0 = node.edgesIn(0).slave
+    node.edgesOut.foreach { edge =>
+      val port = edge.slave
+      require (port.beatBytes == port0.beatBytes,
+        s"${port.slaves.map(_.name)} ${port.beatBytes} vs ${port0.slaves.map(_.name)} ${port0.beatBytes}")
+    }
+
+    val port_addrs = node.edgesOut.map(_.slave.slaves.map(_.address).flatten)
+    val routingMask = AddressDecoder(port_addrs)
+    val route_addrs = port_addrs.map(_.map(_.widen(~routingMask)).distinct)
+
+    val in = io.in(0)
+    val a_sel = Vec(route_addrs.map(seq => seq.map(_.contains(in.haddr)).reduce(_ || _)))
+    val d_sel = Reg(a_sel)
+
+    when (in.hready) { d_sel := a_sel }
+    (a_sel zip io.out) foreach { case (sel, out) =>
+      out := in
+      out.hsel := in.hsel && sel
+    }
+
+    in.hreadyout := !Mux1H(d_sel, io.out.map(!_.hreadyout))
+    in.hresp     :=  Mux1H(d_sel, io.out.map(_.hresp))
+    in.hrdata    :=  Mux1H(d_sel, io.out.map(_.hrdata))
+  }
+}