diff --git a/.mill-version b/.mill-version
index 85b7c69..bc859cb 100644
--- a/.mill-version
+++ b/.mill-version
@@ -1 +1 @@
-0.9.6
+0.11.2
diff --git a/Makefile b/Makefile
index 816359e..4d7a2ff 100644
--- a/Makefile
+++ b/Makefile
@@ -1,3 +1,5 @@
+RTL_OPT = --full-stacktrace -td build --target systemverilog --module $(MOD)
+
 init:
 	git submodule update --init
 	cd rocket-chip && git submodule update --init hardfloat cde
@@ -5,6 +7,10 @@ init:
 compile:
 	mill -i utility.compile
 
+rtl:
+	@mkdir -p build
+	mill -i xsutils.test.runMain top.TestTop $(RTL_OPT)
+
 idea:
 	mill -i mill.scalalib.GenIdea/idea
 
diff --git a/build.sc b/build.sc
index 9f1e4e6..42d7bc7 100644
--- a/build.sc
+++ b/build.sc
@@ -1,16 +1,19 @@
 import mill._
 import scalalib._
 import scalafmt._
+import $file.`rocket-chip`.common
+import $file.`rocket-chip`.hardfloat.common
+import $file.`rocket-chip`.cde.common
 
 val defaultVersions = Map(
-  "chisel3" -> "3.6.0",
-  "chisel3-plugin" -> "3.6.0",
-  "chiseltest" -> "0.3.2",
+  "chisel" -> "6.1.0",
+  "chisel-plugin" -> "6.1.0",
+  "chiseltest" -> "5.0.0",
   "scala" -> "2.13.10",
   "scalatest" -> "3.2.7"
 )
 
-def getVersion(dep: String, org: String = "edu.berkeley.cs", cross: Boolean = false) = {
+def getVersion(dep: String, org: String = "org.chipsalliance", cross: Boolean = false) = {
   val version = sys.env.getOrElse(dep + "Version", defaultVersions(dep))
   if (cross)
     ivy"$org:::$dep:$version"
@@ -20,50 +23,98 @@ def getVersion(dep: String, org: String = "edu.berkeley.cs", cross: Boolean = fa
 
 trait CommonModule extends ScalaModule {
   override def scalaVersion = defaultVersions("scala")
+
+  override def scalacPluginIvyDeps = Agg(getVersion("chisel-plugin", cross = true))
+
+  override def scalacOptions = super.scalacOptions() ++ Agg("-Ymacro-annotations", "-Ytasty-reader")
+
 }
 
+object rocketchip extends RocketChip
 
-object `rocket-chip` extends SbtModule with CommonModule {
+trait RocketChip
+  extends millbuild.`rocket-chip`.common.RocketChipModule
+    with SbtModule {
+  def scalaVersion: T[String] = T(defaultVersions("scala"))
 
-  override def ivyDeps = super.ivyDeps() ++ Agg(
-    ivy"${scalaOrganization()}:scala-reflect:${scalaVersion()}",
-    ivy"org.json4s::json4s-jackson:3.6.1",
-    getVersion("chisel3"),
-  )
+  override def millSourcePath = os.pwd / "rocket-chip"
+
+  def chiselModule = None
+
+  def chiselPluginJar = None
+
+  def chiselIvy = Some(getVersion("chisel"))
+
+  def chiselPluginIvy = Some(getVersion("chisel-plugin", cross = true))
+
+  def macrosModule = macros
+
+  def hardfloatModule = hardfloat
+
+  def cdeModule = cde
+
+  def mainargsIvy = ivy"com.lihaoyi::mainargs:0.5.0"
+
+  def json4sJacksonIvy = ivy"org.json4s::json4s-jackson:4.0.5"
+
+  object macros extends Macros
 
-  object macros extends SbtModule with CommonModule
+  trait Macros
+    extends millbuild.`rocket-chip`.common.MacrosModule
+      with SbtModule {
 
-  object cde extends CommonModule {
-    override def millSourcePath = super.millSourcePath / "cde" / "cde"
+    def scalaVersion: T[String] = T(defaultVersions("scala"))
+
+    def scalaReflectIvy = ivy"org.scala-lang:scala-reflect:${defaultVersions("scala")}"
   }
 
-  object hardfloat extends SbtModule with CommonModule {
-    override def ivyDeps = super.ivyDeps() ++ Agg(getVersion("chisel3"))
+  object hardfloat extends Hardfloat
+
+  trait Hardfloat
+    extends millbuild.`rocket-chip`.hardfloat.common.HardfloatModule {
+
+    def scalaVersion: T[String] = T(defaultVersions("scala"))
+
+    override def millSourcePath = os.pwd / "rocket-chip" / "hardfloat" / "hardfloat"
+
+    def chiselModule = None
+
+    def chiselPluginJar = None
+
+    def chiselIvy = Some(getVersion("chisel"))
+
+    def chiselPluginIvy = Some(getVersion("chisel-plugin", cross = true))
   }
 
-  override def moduleDeps = super.moduleDeps ++ Seq(
-    cde, macros, hardfloat
-  )
+  object cde extends CDE
+
+  trait CDE
+    extends millbuild.`rocket-chip`.cde.common.CDEModule
+      with ScalaModule {
 
+    def scalaVersion: T[String] = T(defaultVersions("scala"))
+
+    override def millSourcePath = os.pwd / "rocket-chip" / "cde" / "cde"
+  }
 }
 
-object utility extends SbtModule with ScalafmtModule with CommonModule {
 
-  override def millSourcePath = millOuterCtx.millSourcePath
+object xsutils extends SbtModule with ScalafmtModule with CommonModule {
 
+  override def millSourcePath = os.pwd
+
+  override def moduleDeps = super.moduleDeps ++ Seq(rocketchip)
 
   override def ivyDeps = super.ivyDeps() ++ Agg(
-    getVersion("chisel3"),
-    getVersion("chiseltest"),
+    getVersion("chisel"),
+    getVersion("chiseltest", "edu.berkeley.cs"),
   )
 
-  override def moduleDeps = super.moduleDeps ++ Seq(`rocket-chip`)
-
-  object test extends Tests {
+  object test extends SbtModuleTests with TestModule.ScalaTest {
     override def ivyDeps = super.ivyDeps() ++ Agg(
       getVersion("scalatest","org.scalatest")
     )
 
-    def testFrameworks = Seq("org.scalatest.tools.Framework")
+    def testFramework = "org.scalatest.tools.Framework"
   }
-}
+}
\ No newline at end of file
diff --git a/src/main/scala/utility/ClockGate.scala b/src/main/scala/utility/ClockGate.scala
index a298858..102f4e8 100644
--- a/src/main/scala/utility/ClockGate.scala
+++ b/src/main/scala/utility/ClockGate.scala
@@ -16,7 +16,10 @@
 package utility
 
 import chisel3._
+import chisel3.experimental.BaseModule
 import chisel3.util.HasBlackBoxInline
+import chisel3.util.experimental.BoringUtils
+import scala.collection.mutable
 
 class ClockGate extends BlackBox with HasBlackBoxInline {
   val io = IO(new Bundle {
@@ -59,12 +62,37 @@ class ClockGate extends BlackBox with HasBlackBoxInline {
   setInline("ClockGate.v", verilog)
 }
 
+class CgteBundle extends Bundle {
+  val te = Input(Bool())
+}
+
 object ClockGate {
-  def apply(TE: Bool, E: Bool, CK: Clock) : Clock = {
-    val clock_gate = Module(new ClockGate).io
-    clock_gate.TE := TE
-    clock_gate.E  := E
-    clock_gate.CK := CK
-    clock_gate.Q
+  private val teBoringQueue = new mutable.Queue[CgteBundle]
+  private val hashModulesHasCgen = new mutable.Queue[BaseModule]
+
+  def apply(TE: Bool, E: Bool, CK: Clock): Clock = {
+    val module = Module.currentModule.get
+    val cgbd = if (hashModulesHasCgen.contains(module)) {
+      teBoringQueue.last
+    } else {
+      val cg = Wire(new CgteBundle)
+      cg.te := TE
+      dontTouch(cg)
+      teBoringQueue.append(cg)
+      hashModulesHasCgen.append(module)
+      cg
+    }
+    val clockGate = Module(new ClockGate)
+    clockGate.io.E := E
+    clockGate.io.TE := cgbd.te
+    clockGate.io.CK := CK
+    clockGate.io.Q
+  }
+
+  def getTop: CgteBundle = {
+    val cgen = Wire(new CgteBundle)
+    teBoringQueue.toSeq.foreach(BoringUtils.bore(_) := cgen)
+    teBoringQueue.clear()
+    cgen
   }
 }
diff --git a/src/main/scala/utility/FileRegisters.scala b/src/main/scala/utility/FileRegisters.scala
index 87c82c6..aa18d04 100644
--- a/src/main/scala/utility/FileRegisters.scala
+++ b/src/main/scala/utility/FileRegisters.scala
@@ -3,27 +3,36 @@ package utility
 import java.io.{File, FileWriter}
 
 object FileRegisters {
-  var files: Seq[(String, () => String)] = Nil
+  var files: Seq[(String, String, () => String)] = Nil
 
   def add(filename: String, contents: => String): Unit = {
-    files = (filename, () => contents) +: files
+    add("", filename, contents)
+  }
+
+  def add(filedir:String, filename:String, contents: => String) :Unit = {
+    val fn = () => contents
+    files = (filedir, filename, fn) +: files
   }
 
   def contains(filename: String): Boolean = {
-    files.foldLeft(false)((t, s) => {s._1 == filename | t})
+    files.count(_._2 == filename) != 0
   }
 
   def write(fileDir: String = "./build", filePrefix: String = ""): Unit = {
-    files.foreach { case (filename, contents) =>
-      writeOutputFile(fileDir, filePrefix + filename, contents())
+    files.foreach { case (fd, fn, fc) =>
+      writeOutputFile(fileDir, fd, filePrefix + fn, fc())
     }
   }
 
-  def writeOutputFile(targetDir: String, fname: String, contents: String): File = {
-    val f = new File(targetDir, fname)
+  def writeOutputFile(td: String, fd: String, fn:String, fc: String): File = {
+    val dirStr = if(fd == "") td else s"$td/$fd"
+    val dir = new File(dirStr)
+    if(!dir.exists()) require(dir.mkdirs())
+    val fname = s"$dirStr/$fn"
+    val f = new File(fname)
     val fw = new FileWriter(f)
-    fw.write(contents)
-    fw.close
+    fw.write(fc)
+    fw.close()
     f
   }
 }
diff --git a/src/main/scala/utility/ResetGen.scala b/src/main/scala/utility/ResetGen.scala
index a0dc2e5..3679534 100644
--- a/src/main/scala/utility/ResetGen.scala
+++ b/src/main/scala/utility/ResetGen.scala
@@ -1,48 +1,65 @@
 /***************************************************************************************
-* Copyright (c) 2020-2021 Institute of Computing Technology, Chinese Academy of Sciences
-* Copyright (c) 2020-2021 Peng Cheng Laboratory
-*
-* XiangShan is licensed under Mulan PSL v2.
-* You can use this software according to the terms and conditions of the Mulan PSL v2.
-* You may obtain a copy of Mulan PSL v2 at:
-*          http://license.coscl.org.cn/MulanPSL2
-*
-* THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
-* EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
-* MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
-*
-* See the Mulan PSL v2 for more details.
-***************************************************************************************/
+ * Copyright (c) 2020-2022 Institute of Computing Technology, Chinese Academy of Sciences
+ * Copyright (c) 2020-2022 Peng Cheng Laboratory
+ *
+ * XiangShan is licensed under Mulan PSL v2.
+ * You can use this software according to the terms and conditions of the Mulan PSL v2.
+ * You may obtain a copy of Mulan PSL v2 at:
+ *          http://license.coscl.org.cn/MulanPSL2
+ *
+ * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
+ * EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
+ * MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
+ *
+ * See the Mulan PSL v2 for more details.
+ ***************************************************************************************/
 
 package utility
 
 import chisel3._
 import chisel3.util._
+class DFTResetSignals extends Bundle{
+  val lgc_rst_n = AsyncReset()
+  val mode = Bool()
+  val scan_mode = Bool()
+}
 
 class ResetGen(SYNC_NUM: Int = 2) extends Module {
   val o_reset = IO(Output(AsyncReset()))
+  val dft = IO(Input(new DFTResetSignals()))
+  val raw_reset = IO(Output(AsyncReset()))
+
+  val lgc_rst = !dft.lgc_rst_n.asBool
+  val real_reset = Mux(dft.mode, lgc_rst, reset.asBool).asAsyncReset
 
-  val pipe_reset = RegInit(((1L << SYNC_NUM) - 1).U(SYNC_NUM.W))
-  pipe_reset := Cat(pipe_reset(SYNC_NUM - 2, 0), 0.U(1.W))
+  withClockAndReset(clock, real_reset){
+    val pipe_reset = RegInit(((1L << SYNC_NUM) - 1).U(SYNC_NUM.W))
+    pipe_reset := Cat(pipe_reset(SYNC_NUM - 2, 0), 0.U(1.W))
+    raw_reset := pipe_reset(SYNC_NUM - 1).asAsyncReset
+  }
 
   // deassertion of the reset needs to be synchronized.
-  o_reset := pipe_reset(SYNC_NUM - 1).asAsyncReset
+  o_reset := Mux(dft.scan_mode, lgc_rst, raw_reset.asBool).asAsyncReset
 }
 
 trait ResetNode
 
 case class ModuleNode(mod: Module) extends ResetNode
 case class CellNode(reset: Reset) extends ResetNode
-
 case class ResetGenNode(children: Seq[ResetNode]) extends ResetNode
 
 object ResetGen {
-  def apply(SYNC_NUM: Int = 2): AsyncReset = {
+  def apply(SYNC_NUM: Int = 2, dft:Option[DFTResetSignals] = None): AsyncReset = {
     val resetSync = Module(new ResetGen(SYNC_NUM))
+    if(dft.isDefined) {
+      resetSync.dft := dft.get
+    } else {
+      resetSync.dft := 0.U.asTypeOf(new DFTResetSignals)
+    }
     resetSync.o_reset
   }
 
-  def apply(resetTree: ResetNode, reset: Reset, sim: Boolean): Unit = {
+  def apply(resetTree: ResetNode, reset: Reset, dft:Option[DFTResetSignals], sim: Boolean): Unit = {
     if(!sim) {
       resetTree match {
         case ModuleNode(mod) =>
@@ -52,26 +69,39 @@ object ResetGen {
         case ResetGenNode(children) =>
           val next_rst = Wire(Reset())
           withReset(reset){
-            val resetGen = Module(new ResetGen)
-            next_rst := resetGen.o_reset
+            next_rst := ResetGen(2, dft)
           }
-          children.foreach(child => apply(child, next_rst, sim))
+          children.foreach(child => apply(child, next_rst, dft, sim))
       }
     }
   }
 
-  def apply(resetChain: Seq[Seq[Module]], reset: Reset, sim: Boolean): Seq[Reset] = {
+  def apply(resetChain: Seq[Seq[Module]], reset: Reset,  dft:Option[DFTResetSignals], sim: Boolean): Seq[Reset] = {
     val resetReg = Wire(Vec(resetChain.length + 1, Reset()))
     resetReg.foreach(_ := reset)
     for ((resetLevel, i) <- resetChain.zipWithIndex) {
       if (!sim) {
         withReset(resetReg(i)) {
-          val resetGen = Module(new ResetGen)
-          resetReg(i + 1) := resetGen.o_reset
+          resetReg(i + 1) := ResetGen(2, dft)
         }
       }
       resetLevel.foreach(_.reset := resetReg(i + 1))
     }
     resetReg.tail
   }
+
+  def applyOneLevel(resetSigs: Seq[Reset], reset: Reset, sim: Boolean): DFTResetSignals = {
+    val resetReg = Wire(Reset())
+    val dft = Wire(new DFTResetSignals())
+    resetReg := reset
+    if (!sim) {
+      withReset(reset) {
+        val resetGen = Module(new ResetGen)
+        resetReg := resetGen.o_reset
+        resetGen.dft := dft
+      }
+    }
+    resetSigs.foreach(_ := resetReg)
+    dft
+  }
 }
diff --git a/src/main/scala/utility/SRAMTemplate.scala b/src/main/scala/utility/SRAMTemplate.scala
deleted file mode 100644
index 541836b..0000000
--- a/src/main/scala/utility/SRAMTemplate.scala
+++ /dev/null
@@ -1,307 +0,0 @@
-/***************************************************************************************
-* Copyright (c) 2020-2021 Institute of Computing Technology, Chinese Academy of Sciences
-* Copyright (c) 2020-2021 Peng Cheng Laboratory
-*
-* XiangShan is licensed under Mulan PSL v2.
-* You can use this software according to the terms and conditions of the Mulan PSL v2.
-* You may obtain a copy of Mulan PSL v2 at:
-*          http://license.coscl.org.cn/MulanPSL2
-*
-* THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
-* EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
-* MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
-*
-* See the Mulan PSL v2 for more details.
-***************************************************************************************/
-
-/**************************************************************************************
-* Copyright (c) 2020 Institute of Computing Technology, CAS
-* Copyright (c) 2020 University of Chinese Academy of Sciences
-*
-* NutShell is licensed under Mulan PSL v2.
-* You can use this software according to the terms and conditions of the Mulan PSL v2.
-* You may obtain a copy of Mulan PSL v2 at:
-*             http://license.coscl.org.cn/MulanPSL2
-*
-* THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER
-* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR
-* FIT FOR A PARTICULAR PURPOSE.
-*
-* See the Mulan PSL v2 for more details.
-***************************************************************************************/
-
-package utility
-
-import chisel3._
-import chisel3.util._
-
-class SRAMBundleA(val set: Int) extends Bundle {
-  val setIdx = Output(UInt(log2Up(set).W))
-
-  def apply(setIdx: UInt) = {
-    this.setIdx := setIdx
-    this
-  }
-}
-
-class SRAMBundleAW[T <: Data](private val gen: T,
-  set: Int,
-  val way: Int = 1,
-  val useBitmask: Boolean = false
-) extends SRAMBundleA(set) {
-
-  private val dataWidth = gen.getWidth
-
-  val data: Vec[T] = Output(Vec(way, gen))
-  val waymask: Option[UInt] = if (way > 1) Some(Output(UInt(way.W))) else None
-  // flattened_bitmask is the flattened form of [waymask, bitmask], can be use directly to mask memory
-  val flattened_bitmask: Option[UInt] = if (useBitmask) Some(Output(UInt((way * dataWidth).W))) else None
-  // bitmask is the original bitmask passed from parameter
-  val bitmask: Option[UInt] = if (useBitmask) Some(Output(UInt((dataWidth).W))) else None
-
-  def apply(data: Vec[T], setIdx: UInt, waymask: UInt): SRAMBundleAW[T] = {
-    require(waymask.getWidth == way,
-     s"waymask width does not equal nWays, waymask width: ${waymask.getWidth}, nWays: ${way}")
-    super.apply(setIdx)
-    this.data := data
-    this.waymask.foreach(_ := waymask)
-    this
-  }
-
-  def apply(data: Vec[T], setIdx: UInt, waymask: UInt, bitmask: UInt): SRAMBundleAW[T] = {
-    require(useBitmask, "passing bitmask when not using bitmask")
-    require(bitmask.getWidth == dataWidth,
-      s"bitmask width does not equal data width, bitmask width: ${bitmask.getWidth}, data width: ${dataWidth}")
-    apply(data, setIdx, waymask)
-    this.flattened_bitmask.foreach(_ :=
-      VecInit.tabulate(way * dataWidth)(n => waymask(n / dataWidth) && bitmask(n % dataWidth)).asUInt)
-    this.bitmask.foreach(_ := bitmask)
-    this
-  }
-
-  // this could only be used when waymask is onehot or nway is 1
-  def apply(data: T, setIdx: UInt, waymask: UInt): SRAMBundleAW[T] = {
-    apply(VecInit(Seq.fill(way)(data)), setIdx, waymask)
-    this
-  }
-
-  def apply(data: T, setIdx: UInt, waymask: UInt, bitmask: UInt): SRAMBundleAW[T] = {
-    apply(VecInit(Seq.fill(way)(data)), setIdx, waymask, bitmask)
-    this
-  }
-}
-
-class SRAMBundleR[T <: Data](private val gen: T, val way: Int = 1) extends Bundle {
-  val data = Output(Vec(way, gen))
-}
-
-class SRAMReadBus[T <: Data](private val gen: T, val set: Int, val way: Int = 1) extends Bundle {
-  val req = Decoupled(new SRAMBundleA(set))
-  val resp = Flipped(new SRAMBundleR(gen, way))
-
-  def apply(valid: Bool, setIdx: UInt) = {
-    this.req.bits.apply(setIdx)
-    this.req.valid := valid
-    this
-  }
-}
-
-class SRAMWriteBus[T <: Data](
-  private val gen: T, val set: Int,
-  val way: Int = 1, val useBitmask: Boolean = false
-) extends Bundle {
-  val req = Decoupled(new SRAMBundleAW(gen, set, way, useBitmask))
-
-  def apply(valid: Bool, data: Vec[T], setIdx: UInt, waymask: UInt): SRAMWriteBus[T] = {
-    this.req.bits.apply(data = data, setIdx = setIdx, waymask = waymask)
-    this.req.valid := valid
-    this
-  }
-
-  def apply(valid: Bool, data: Vec[T], setIdx: UInt, waymask: UInt, bitmask: UInt): SRAMWriteBus[T] = {
-    this.req.bits.apply(data = data, setIdx = setIdx, waymask = waymask, bitmask = bitmask)
-    this.req.valid := valid
-    this
-  }
-
-  def apply(valid: Bool, data: T, setIdx: UInt, waymask: UInt): SRAMWriteBus[T] = {
-    apply(valid, VecInit(Seq.fill(way)(data)), setIdx, waymask)
-    this
-  }
-
-  def apply(valid: Bool, data: T, setIdx: UInt, waymask: UInt, bitmask: UInt): SRAMWriteBus[T] = {
-    apply(valid, VecInit(Seq.fill(way)(data)), setIdx, waymask, bitmask)
-    this
-  }
-}
-
-class SRAMTemplate[T <: Data](
-  gen: T, set: Int, way: Int = 1, singlePort: Boolean = false,
-  shouldReset: Boolean = false, extraReset: Boolean = false,
-  holdRead: Boolean = false, bypassWrite: Boolean = false,
-  useBitmask: Boolean = false,
-) extends Module {
-  val io = IO(new Bundle {
-    val r = Flipped(new SRAMReadBus(gen, set, way))
-    val w = Flipped(new SRAMWriteBus(gen, set, way, useBitmask))
-  })
-  val extra_reset = if (extraReset) Some(IO(Input(Bool()))) else None
-
-  val wordType = UInt(gen.getWidth.W)
-  val arrayWidth = if (useBitmask) 1 else gen.getWidth
-  val arrayType = UInt(arrayWidth.W)
-  val arrayPortSize = if (useBitmask) way * gen.getWidth else way
-  val array = SyncReadMem(set, Vec(arrayPortSize, arrayType))
-  val (resetState, resetSet) = (WireInit(false.B), WireInit(0.U))
-
-  if (shouldReset) {
-    val _resetState = RegInit(true.B)
-    val (_resetSet, resetFinish) = Counter(_resetState, set)
-    when (resetFinish) { _resetState := false.B }
-    if (extra_reset.isDefined) {
-      when (extra_reset.get) {
-        _resetState := true.B
-      }
-    }
-
-    resetState := _resetState
-    resetSet := _resetSet
-  }
-
-  val (ren, wen) = (io.r.req.valid, io.w.req.valid || resetState)
-  val realRen = (if (singlePort) ren && !wen else ren)
-
-  val setIdx = Mux(resetState, resetSet, io.w.req.bits.setIdx)
-  val wdata = Mux(resetState, 0.U.asTypeOf(Vec(way, gen)), io.w.req.bits.data).
-    asTypeOf(Vec(arrayPortSize, arrayType))
-
-  // Memeory write
-  if (!useBitmask) {
-    val waymask = Mux(resetState, Fill(way, "b1".U), io.w.req.bits.waymask.getOrElse("b1".U))
-    when(wen) {
-      array.write(setIdx, wdata, waymask.asBools)
-    }
-  } else {
-    val bitmask = Mux(resetState, Fill(way * gen.getWidth, "b1".U), io.w.req.bits.flattened_bitmask.getOrElse("b1".U))
-    when(wen) {
-      array.write(setIdx, wdata, bitmask.asBools)
-    }
-  }
-
-  // Memory read
-  val raw_rdata = array.read(io.r.req.bits.setIdx, realRen).asTypeOf(Vec(way, wordType))
-  require(wdata.getWidth == raw_rdata.getWidth)
-
-  // bypass for dual-port SRAMs
-  require(!bypassWrite || bypassWrite && !singlePort)
-  def need_bypass(wen: Bool, waddr: UInt, wmask: UInt, ren: Bool, raddr: UInt) : UInt = {
-    val need_check = ren && wen
-    val need_check_reg = GatedValidRegNext(need_check)
-    val waddr_reg = RegEnable(waddr, need_check)
-    val raddr_reg = RegEnable(raddr, need_check)
-    val wmask_reg = RegEnable(wmask, need_check)
-    require(wmask.getWidth == way)
-    val bypass = Fill(way, need_check_reg && waddr_reg === raddr_reg) & wmask_reg
-    bypass.asTypeOf(UInt(way.W))
-  }
-  val bypass_wdata = if (bypassWrite) VecInit(RegEnable(io.w.req.bits.data, io.w.req.valid && io.r.req.valid).map(_.asTypeOf(wordType)))
-    else VecInit((0 until way).map(_ => LFSR64().asTypeOf(wordType)))
-  val bypass_mask = need_bypass(io.w.req.valid, io.w.req.bits.setIdx, io.w.req.bits.waymask.getOrElse("b1".U), io.r.req.valid, io.r.req.bits.setIdx)
-  val mem_rdata = {
-    if (singlePort) raw_rdata
-    else VecInit(bypass_mask.asBools.zip(raw_rdata).zip(bypass_wdata).map {
-      case ((m, r), w) => Mux(m, w, r)
-    })
-  }
-
-  // hold read data for SRAMs
-  val rdata = (if (holdRead) HoldUnless(mem_rdata, GatedValidRegNext(realRen))
-              else mem_rdata).map(_.asTypeOf(gen))
-
-  io.r.resp.data := VecInit(rdata)
-  io.r.req.ready := !resetState && (if (singlePort) !wen else true.B)
-  io.w.req.ready := true.B
-
-}
-
-class FoldedSRAMTemplate[T <: Data](
-  gen: T, set: Int, width: Int = 4, way: Int = 1,
-  shouldReset: Boolean = false, extraReset: Boolean = false,
-  holdRead: Boolean = false, singlePort: Boolean = false,
-  bypassWrite: Boolean = false, useBitmask: Boolean = false,
-) extends Module {
-  val io = IO(new Bundle {
-    val r = Flipped(new SRAMReadBus(gen, set, way))
-    val w = Flipped(new SRAMWriteBus(gen, set, way, useBitmask))
-  })
-  val extra_reset = if (extraReset) Some(IO(Input(Bool()))) else None
-  //   |<----- setIdx ----->|
-  //   | ridx | width | way |
-
-  require(width > 0 && isPow2(width))
-  require(way > 0 && isPow2(way))
-  require(set % width == 0)
-
-  val nRows = set / width
-
-  val array = Module(new SRAMTemplate(gen, set=nRows, way=width*way,
-    shouldReset=shouldReset, extraReset=extraReset, holdRead=holdRead,
-    singlePort=singlePort, bypassWrite=bypassWrite, useBitmask=useBitmask))
-  if (array.extra_reset.isDefined) {
-    array.extra_reset.get := extra_reset.get
-  }
-
-  io.r.req.ready := array.io.r.req.ready
-  io.w.req.ready := array.io.w.req.ready
-
-  val raddr = io.r.req.bits.setIdx >> log2Ceil(width)
-  val ridx = RegEnable(if (width != 1) io.r.req.bits.setIdx(log2Ceil(width)-1, 0) else 0.U(1.W), io.r.req.valid)
-  val ren  = io.r.req.valid
-
-  array.io.r.req.valid := ren
-  array.io.r.req.bits.setIdx := raddr
-
-  val rdata = array.io.r.resp.data
-  for (w <- 0 until way) {
-    val wayData = VecInit(rdata.indices.filter(_ % way == w).map(rdata(_)))
-    val holdRidx = HoldUnless(ridx, GatedValidRegNext(io.r.req.valid))
-    val realRidx = if (holdRead) holdRidx else ridx
-    io.r.resp.data(w) := Mux1H(UIntToOH(realRidx, width), wayData)
-  }
-
-  val wen = io.w.req.valid
-  val wdata = VecInit(Seq.fill(width)(io.w.req.bits.data).flatten)
-  val waddr = io.w.req.bits.setIdx >> log2Ceil(width)
-  val widthIdx = if (width != 1) io.w.req.bits.setIdx(log2Ceil(width)-1, 0) else 0.U
-  val wmask = (width, way) match {
-    case (1, 1) => 1.U(1.W)
-    case (x, 1) => UIntToOH(widthIdx)
-    case _      => VecInit(Seq.tabulate(width*way)(n => (n / way).U === widthIdx && io.w.req.bits.waymask.get(n % way))).asUInt
-  }
-  require(wmask.getWidth == way*width)
-
-  if (useBitmask) {
-    array.io.w.apply(wen, wdata, waddr, wmask, io.w.req.bits.bitmask.get)
-  } else {
-    array.io.w.apply(wen, wdata, waddr, wmask)
-  }
-}
-class SRAMTemplateWithArbiter[T <: Data](nRead: Int, gen: T, set: Int, way: Int = 1,
-  shouldReset: Boolean = false) extends Module {
-  val io = IO(new Bundle {
-    val r = Flipped(Vec(nRead, new SRAMReadBus(gen, set, way)))
-    val w = Flipped(new SRAMWriteBus(gen, set, way))
-  })
-
-  val ram = Module(new SRAMTemplate(gen, set, way, shouldReset = shouldReset, holdRead = false, singlePort = true))
-  ram.io.w <> io.w
-
-  val readArb = Module(new Arbiter(chiselTypeOf(io.r(0).req.bits), nRead))
-  readArb.io.in <> io.r.map(_.req)
-  ram.io.r.req <> readArb.io.out
-
-  // latch read results
-  io.r.map{ case r => {
-    r.resp.data := HoldUnless(ram.io.r.resp.data, GatedValidRegNext(r.req.fire))
-  }}
-}
diff --git a/src/main/scala/utility/mbist/Mbist.scala b/src/main/scala/utility/mbist/Mbist.scala
new file mode 100644
index 0000000..cfac829
--- /dev/null
+++ b/src/main/scala/utility/mbist/Mbist.scala
@@ -0,0 +1,156 @@
+/** *************************************************************************************
+  * Copyright (c) 2020-2022 Institute of Computing Technology, Chinese Academy of Sciences
+  * Copyright (c) 2020-2022 Peng Cheng Laboratory
+  *
+  * XiangShan is licensed under Mulan PSL v2.
+  * You can use this software according to the terms and conditions of the Mulan PSL v2.
+  * You may obtain a copy of Mulan PSL v2 at:
+  *          http://license.coscl.org.cn/MulanPSL2
+  *
+  * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
+  * EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
+  * MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
+  *
+  * See the Mulan PSL v2 for more details.
+  * *************************************************************************************
+  */
+
+package utility.mbist
+
+import chisel3._
+import chisel3.util.experimental.BoringUtils
+
+object Mbist {
+  val maxMbistDataWidth = 256
+
+  protected[mbist] var globalNodes = Seq[BaseNode]()
+
+  sealed trait BaseNode {
+    val bd:          MbistCommonBundle
+    val level:       Int
+    val array_id:    Seq[Int]
+    val array_depth: Seq[Int]
+  }
+  sealed class RamBaseNode(
+    val bd:  Ram2Mbist,
+    val ids: Seq[Int])
+      extends BaseNode {
+    override val level: Int = 0
+    override val array_id = ids
+    override val array_depth = Seq.fill(ids.length)(0)
+  }
+  sealed class PipelineBaseNode(
+    val bd:          MbistBus,
+    val level:       Int,
+    val array_id:    Seq[Int],
+    val array_depth: Seq[Int])
+      extends BaseNode {
+    var children:              Seq[BaseNode] = Seq()
+    var ramParamsBelongToThis: Seq[Ram2MbistParams] = Seq()
+    require(level > 0)
+  }
+
+  sealed class SramNode(bd: Ram2Mbist, ids: Seq[Int]) extends RamBaseNode(bd, ids)
+
+  sealed class PipelineNodeSram(bd: MbistBus, level: Int, array_id: Seq[Int], array_depth: Seq[Int])
+      extends PipelineBaseNode(bd, level, array_id, array_depth)
+
+  def inferMbistBusParamsFromParams(children: Seq[MbistBusParams]): MbistBusParams =
+    MbistBusParams(
+      children.map(_.array).max,
+      children.map(_.set).max,
+      children.map(_.dataWidth).max,
+      children.map(_.maskWidth).max,
+      children.map(_.hasDualPort).reduce(_ || _)
+    )
+
+  def inferMbistBusParams(children: Seq[BaseNode]): MbistBusParams =
+    MbistBusParams(
+      children.map(_.array_id).reduce(_ ++ _).max,
+      children.map {
+        case ram: RamBaseNode      => ram.bd.params.set
+        case pl:  PipelineBaseNode => pl.bd.params.set
+      }.max,
+      (children.map {
+        case ram: RamBaseNode      => ram.bd.params.dataWidth
+        case pl:  PipelineBaseNode => pl.bd.params.dataWidth
+      }).max,
+      (children.map {
+        case ram: RamBaseNode      => ram.bd.params.maskWidth
+        case pl:  PipelineBaseNode => pl.bd.params.maskWidth
+      }).max,
+      (children.map {
+        case ram: RamBaseNode      => !ram.bd.params.singlePort
+        case pl:  PipelineBaseNode => pl.bd.params.hasDualPort
+      }).reduce(_ || _)
+    )
+
+  def addRamNode(bd: Ram2Mbist, ids: Seq[Int]): RamBaseNode = {
+    val node = new SramNode(bd, ids)
+    globalNodes = globalNodes :+ node
+    node
+  }
+
+  def isMaxLevel(level: Int) = level == Int.MaxValue
+
+  def addController(level: Int): PipelineBaseNode = {
+    require(globalNodes.nonEmpty, "No nodes were created before implementing mbist controller!")
+    val candidateNodes = globalNodes.filter(inst => inst.isInstanceOf[SramNode] || inst.isInstanceOf[PipelineNodeSram])
+    val children = candidateNodes.filter(_.level < level)
+    val remain = globalNodes.filterNot(children.contains(_))
+    require(children.nonEmpty, "Mbist controller level setting is wrong or no children nodes were found!")
+    val params = inferMbistBusParams(children)
+    val bd = Wire(new MbistBus(params))
+    bd := DontCare
+    dontTouch(bd)
+    val ids = children.flatMap(_.array_id)
+    val depth = children.flatMap(_.array_depth.map(_ + 1))
+    val node = new PipelineNodeSram(bd, level, ids, depth)
+    node.children = children.map {
+      case ram: RamBaseNode =>
+        val childBd = Wire(ram.bd.cloneType)
+        childBd := DontCare
+        dontTouch(childBd)
+        val boreChildrenBd = BoringUtils.bore(ram.bd)
+        boreChildrenBd.addr := childBd.addr
+        boreChildrenBd.addr_rd := childBd.addr_rd
+        boreChildrenBd.wdata := childBd.wdata
+        boreChildrenBd.wmask := childBd.wmask
+        boreChildrenBd.re := childBd.re
+        boreChildrenBd.we := childBd.we
+        boreChildrenBd.selectedOH := childBd.selectedOH
+        boreChildrenBd.array := childBd.array
+        boreChildrenBd.ack := childBd.ack
+        boreChildrenBd.ere := childBd.ere
+        boreChildrenBd.ewe := childBd.ewe
+        childBd.rdata := boreChildrenBd.rdata
+        new SramNode(childBd, ram.array_id)
+      case pl: PipelineBaseNode =>
+        val childBd = Wire(pl.bd.cloneType)
+        childBd := DontCare
+        dontTouch(childBd)
+        val boreChildrenBd = BoringUtils.bore(pl.bd)
+        boreChildrenBd.mbist_array := childBd.mbist_array
+        boreChildrenBd.mbist_all := childBd.mbist_all
+        boreChildrenBd.mbist_req := childBd.mbist_req
+        boreChildrenBd.mbist_writeen := childBd.mbist_writeen
+        boreChildrenBd.mbist_be := childBd.mbist_be
+        boreChildrenBd.mbist_addr := childBd.mbist_addr
+        boreChildrenBd.mbist_indata := childBd.mbist_indata
+        boreChildrenBd.mbist_readen := childBd.mbist_readen
+        boreChildrenBd.mbist_addr_rd := childBd.mbist_addr_rd
+        childBd.mbist_ack := boreChildrenBd.mbist_ack
+        childBd.mbist_outdata := boreChildrenBd.mbist_outdata
+        new PipelineNodeSram(childBd, pl.level, pl.array_id, pl.array_depth)
+    }
+    node.ramParamsBelongToThis = children.flatMap({
+      case ram: RamBaseNode =>
+        ram.bd.params.getAllNodesParams()
+      case pl: PipelineBaseNode =>
+        pl.ramParamsBelongToThis
+    })
+    globalNodes = remain :+ node
+
+    node
+  }
+}
diff --git a/src/main/scala/utility/mbist/MbistBus.scala b/src/main/scala/utility/mbist/MbistBus.scala
new file mode 100644
index 0000000..a00a078
--- /dev/null
+++ b/src/main/scala/utility/mbist/MbistBus.scala
@@ -0,0 +1,142 @@
+/** *************************************************************************************
+  * Copyright (c) 2020-2022 Institute of Computing Technology, Chinese Academy of Sciences
+  * Copyright (c) 2020-2022 Peng Cheng Laboratory
+  *
+  * XiangShan is licensed under Mulan PSL v2.
+  * You can use this software according to the terms and conditions of the Mulan PSL v2.
+  * You may obtain a copy of Mulan PSL v2 at:
+  *          http://license.coscl.org.cn/MulanPSL2
+  *
+  * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
+  * EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
+  * MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
+  *
+  * See the Mulan PSL v2 for more details.
+  * *************************************************************************************
+  */
+
+package utility.mbist
+
+import chisel3._
+import chisel3.util._
+import chisel3.util.experimental.BoringUtils
+import utility.SRAMTemplate
+
+trait MbistBundleLike {
+  this: Bundle =>
+  def sink_elms:   Seq[String]
+  def source_elms: Seq[String]
+  def get_sink_data:   Seq[Data] = sink_elms.map(e => elements(e))
+  def get_source_data: Seq[Data] = sink_elms.map(e => elements(e))
+}
+abstract class MbistCommonBundle() extends Bundle with MbistBundleLike {
+  def sink_elms:   Seq[String] = Seq()
+  def source_elms: Seq[String] = Seq()
+}
+
+case class MbistBusParams(
+  array:       Int,
+  set:         Int,
+  dataWidth:   Int,
+  maskWidth:   Int,
+  hasDualPort: Boolean,
+  domainName:  String = "Unknown") {
+  val arrayWidth = log2Up(array + 1)
+  val addrWidth = log2Up(set + 1)
+}
+
+class MbistBus(val params: MbistBusParams) extends MbistCommonBundle() {
+  // control signals
+  val mbist_array = Input(UInt(params.arrayWidth.W))
+  val mbist_all, mbist_req = Input(Bool())
+  val mbist_ack = Output(Bool())
+  // write
+  val mbist_writeen = Input(Bool())
+  val mbist_be = Input(UInt(params.maskWidth.W))
+  val mbist_addr = Input(UInt(params.addrWidth.W))
+  val mbist_indata = Input(UInt(params.dataWidth.W))
+  // read
+  val mbist_readen = Input(Bool())
+  val mbist_addr_rd = Input(UInt(params.addrWidth.W)) // not used for single port srams
+  val mbist_outdata = Output(UInt(params.dataWidth.W))
+
+  override def sink_elms: Seq[String] = super.sink_elms ++ Seq(
+    "mbist_array",
+    "mbist_all",
+    "mbist_req",
+    "mbist_writeen",
+    "mbist_be",
+    "mbist_addr",
+    "mbist_indata",
+    "mbist_readen",
+    "mbist_addr_rd"
+  )
+
+  override def source_elms: Seq[String] = super.source_elms ++ Seq("mbist_ack", "mbist_outdata")
+}
+
+case class Ram2MbistParams(
+  set:        Int,
+  dataWidth:  Int,
+  maskWidth:  Int,
+  singlePort: Boolean,
+  vname:      String,
+  nodeSuffix: String,
+  nodeNum:    Int,
+  maxArrayId: Int,
+  bitWrite:   Boolean,
+  foundry:    String,
+  sramInst:   String,
+  latency:    Int = 0,
+  bankRange:  String = "None",
+  holder:     SRAMTemplate[Data]) {
+  val addrWidth = log2Up(set + 1)
+  val arrayWidth = log2Up(maxArrayId + 1)
+  def getAllNodesParams(): Seq[Ram2MbistParams] = {
+    val res = Seq.tabulate(nodeNum)(idx => {
+      Ram2MbistParams(
+        set,
+        dataWidth,
+        maskWidth,
+        singlePort,
+        vname,
+        nodeSuffix + s"_node$idx",
+        nodeNum,
+        maxArrayId,
+        bitWrite,
+        foundry,
+        sramInst,
+        latency,
+        bankRange,
+        holder
+      )
+    })
+    res
+  }
+}
+
+class Ram2Mbist(val params: Ram2MbistParams) extends MbistCommonBundle() {
+  val addr, addr_rd = Input(UInt(params.addrWidth.W))
+  val wdata = Input(UInt(params.dataWidth.W))
+  val wmask = Input(UInt(params.maskWidth.W))
+  val re, we = Input(Bool())
+  val ere, ewe = Input(Bool())
+  val rdata = Output(UInt(params.dataWidth.W))
+  val ack = Input(Bool())
+  val selectedOH = Input(UInt(params.nodeNum.W))
+  val array = Input(UInt(params.arrayWidth.W))
+  override def sink_elms: Seq[String] = super.sink_elms ++ Seq(
+    "addr",
+    "addr_rd",
+    "wdata",
+    "wmask",
+    "re",
+    "we",
+    "ere",
+    "ewe",
+    "ack",
+    "selectedOH",
+    "array"
+  )
+  override def source_elms: Seq[String] = super.source_elms ++ Seq("rdata")
+}
diff --git a/src/main/scala/utility/mbist/MbistClockGateCell.scala b/src/main/scala/utility/mbist/MbistClockGateCell.scala
new file mode 100644
index 0000000..84828b2
--- /dev/null
+++ b/src/main/scala/utility/mbist/MbistClockGateCell.scala
@@ -0,0 +1,40 @@
+package utility.mbist
+import chisel3._
+import utility.ClockGate
+import utility.sram.SramBroadcastBundle
+
+class CgDftBundle extends Bundle {
+  val ram_mcp_hold = Input(Bool())
+  val ram_aux_clk = Input(Bool())
+  val ram_aux_ckbp = Input(Bool())
+  val cgen = Input(Bool())
+  def fromBroadcast(brc: SramBroadcastBundle): Unit = {
+    ram_aux_clk := brc.ram_aux_clk
+    ram_aux_ckbp := brc.ram_aux_ckbp
+    ram_mcp_hold := brc.ram_mcp_hold
+    cgen := brc.cgen
+  }
+}
+
+class MbistClockGateCell(mcpCtl:Boolean) extends Module {
+  val mbist = IO(new Bundle {
+    val writeen = Input(Bool())
+    val readen = Input(Bool())
+    val req = Input(Bool())
+  })
+  val E = IO(Input(Bool()))
+  val dft = IO(new CgDftBundle)
+  val out_clock = IO(Output(Clock()))
+
+  private val CG = Module(new ClockGate)
+  CG.io.TE := dft.cgen
+  CG.io.CK := clock
+
+  if(mcpCtl) {
+    CG.io.E := Mux(mbist.req, mbist.readen | mbist.writeen, E) && !dft.ram_mcp_hold
+    out_clock := Mux(dft.ram_aux_ckbp, dft.ram_aux_clk.asClock, CG.io.Q)
+  } else {
+    CG.io.E := Mux(mbist.req, mbist.readen | mbist.writeen, E)
+    out_clock := CG.io.Q
+  }
+}
diff --git a/src/main/scala/utility/mbist/MbistCsvGen.scala b/src/main/scala/utility/mbist/MbistCsvGen.scala
new file mode 100644
index 0000000..2b5a26b
--- /dev/null
+++ b/src/main/scala/utility/mbist/MbistCsvGen.scala
@@ -0,0 +1,40 @@
+package utility.mbist
+import java.util.regex.Pattern
+
+abstract class MbistFileGenerator {
+  def generate(): String
+}
+
+class MbistCsvGen(val intf: InterfaceInfo, val pip: MbistPipeline, val csvName: String) extends MbistFileGenerator {
+  def generate(): String = {
+    val fileName = s"$csvName.csv"
+    println(s"Generating $fileName")
+    var contents = "\"INTF Name\", \"INTF Addr\", \"INTF Data\", \"INTF Array\", \"INTF Be\", \"Has TpSRAM\"\n"
+    contents += intf.toString + '\n'
+    contents += "\"SRAM Name\",\"SRAM Type\",\"SRAM array\",\"pipeline depth\",\"bitWrite\",\"bank addr\",\"selectOH width\",\"foundry\",\"SRAM Inst\"\n"
+    val pipPath = pip.pathName.split("\\.").init.map(_ + ".").reduce(_ + _)
+    val pattern = Pattern.compile(pipPath)
+    def removeSubstring(str: String): String = {
+      val matcher0 = pattern.matcher(str)
+      matcher0.replaceAll("").replace(".", "_")
+    }
+    val node = pip.myNode
+    node.ramParamsBelongToThis
+      .zip(node.array_id)
+      .zip(node.array_depth)
+      .foreach({
+        case ((p, id), depth) =>
+          contents += removeSubstring(p.holder.pathName) + p.nodeSuffix + ","
+          contents += p.vname + ".v,"
+          contents += id.toString + ","
+          contents += (depth * 2 + p.latency).toString + ","
+          contents += (if (p.bitWrite) "true," else "false,")
+          contents += p.bankRange + ","
+          contents += p.nodeNum + ","
+          contents += p.foundry + ","
+          contents += p.sramInst
+          contents += "\n"
+      })
+    contents
+  }
+}
diff --git a/src/main/scala/utility/mbist/MbistPipeline.scala b/src/main/scala/utility/mbist/MbistPipeline.scala
new file mode 100644
index 0000000..f8ee01b
--- /dev/null
+++ b/src/main/scala/utility/mbist/MbistPipeline.scala
@@ -0,0 +1,265 @@
+/** *************************************************************************************
+  * Copyright (c) 2020-2022 Institute of Computing Technology, Chinese Academy of Sciences
+  * Copyright (c) 2020-2022 Peng Cheng Laboratory
+  *
+  * XiangShan is licensed under Mulan PSL v2.
+  * You can use this software according to the terms and conditions of the Mulan PSL v2.
+  * You may obtain a copy of Mulan PSL v2 at:
+  * http://license.coscl.org.cn/MulanPSL2
+  *
+  * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
+  * EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
+  * MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
+  *
+  * See the Mulan PSL v2 for more details.
+  * *************************************************************************************
+  */
+
+package utility.mbist
+
+import chisel3._
+import chisel3.util._
+import utility.FileRegisters
+import utility.mbist.Mbist._
+import utility.mbist.MbistPipeline.uniqueId
+import utility.sram.SramHelper
+
+class MbitsStandardInterface(val params: MbistBusParams) extends Bundle {
+  val array = Input(UInt(params.arrayWidth.W))
+  val all, req = Input(Bool())
+  val ack = Output(Bool())
+  // write
+  val writeen = Input(Bool())
+  val be = Input(UInt(params.maskWidth.W))
+  val addr = Input(UInt(params.addrWidth.W))
+  val indata = Input(UInt(params.dataWidth.W))
+  // read
+  val readen = Input(Bool())
+  val addr_rd = Input(UInt(params.addrWidth.W)) // not used for single port srams
+  val outdata = Output(UInt(params.dataWidth.W))
+}
+
+case class InterfaceInfo(
+  name:        String,
+  addrWidth:   Int,
+  dataWidth:   Int,
+  arrayWidth:  Int,
+  beWidth:     Int,
+  hasDualPort: Boolean) {
+  override def toString = s"$name,$addrWidth,$dataWidth,$arrayWidth,$beWidth," + (if (hasDualPort) "true" else "false")
+}
+
+class MbistInterface(params: Seq[MbistBusParams], ids: Seq[Seq[Int]], name: String, pipelineNum: Int) extends Module {
+  require(params.nonEmpty)
+  require(params.length == pipelineNum, s"Error @ ${name}:Params Number and pipelineNum must be the same!")
+  val myMbistBusParams = Mbist.inferMbistBusParamsFromParams(params)
+  override val desiredName = name
+
+  val toPipeline = IO(MixedVec(Seq.tabulate(pipelineNum)(idx => Flipped(new MbistBus(params(idx))))))
+  val mbist = IO(new MbitsStandardInterface(myMbistBusParams))
+
+  val info = InterfaceInfo(
+    name,
+    myMbistBusParams.addrWidth,
+    myMbistBusParams.dataWidth,
+    myMbistBusParams.arrayWidth,
+    myMbistBusParams.maskWidth,
+    myMbistBusParams.hasDualPort
+  )
+
+  private val array = mbist.array
+  private val all = mbist.all
+  private val req = mbist.req
+  private val we = mbist.writeen
+  private val be = mbist.be
+  private val addr = mbist.addr
+  private val inData = mbist.indata
+  private val re = mbist.readen
+  private val addrRd = mbist.addr_rd
+  private val hit = if (params.length > 1) ids.map(item => item.map(_.U === array).reduce(_ | _)) else Seq(true.B)
+  private val outDataVec = toPipeline.map(_.mbist_outdata)
+  mbist.outdata := Mux1H(hit.zip(outDataVec))
+  private val ackVec = toPipeline.map(_.mbist_ack)
+  mbist.ack := Mux1H(hit.zip(ackVec))
+
+  for (pip <- toPipeline) {
+    pip.mbist_array := array
+    pip.mbist_all := all
+    pip.mbist_req := req
+    pip.mbist_writeen := we
+    pip.mbist_be := be
+    pip.mbist_addr := addr
+    pip.mbist_indata := inData
+    pip.mbist_readen := re
+    pip.mbist_addr_rd := addrRd
+  }
+}
+
+object MbistPipeline {
+  private var uniqueId = 0
+
+  def PlaceMbistPipeline(
+    level:      Int,
+    moduleName: String = s"MbistPipeline_${uniqueId}",
+    place:      Boolean = true
+  ): Option[MbistPipeline] = {
+    if (place) {
+      val thisNode = Mbist.addController(level)
+      uniqueId += 1
+      val pipelineNodes =
+        thisNode.children.filter(_.isInstanceOf[PipelineBaseNode]).map(_.asInstanceOf[PipelineBaseNode])
+      val ramNodes = thisNode.children.filter(_.isInstanceOf[RamBaseNode]).map(_.asInstanceOf[RamBaseNode])
+      val res = Module(new MbistPipeline(level, moduleName, thisNode))
+      res.mbist.mbist_array := thisNode.bd.mbist_array
+      res.mbist.mbist_all := thisNode.bd.mbist_all
+      res.mbist.mbist_req := thisNode.bd.mbist_req
+      thisNode.bd.mbist_ack := res.mbist.mbist_ack
+      res.mbist.mbist_writeen := thisNode.bd.mbist_writeen
+      res.mbist.mbist_be := thisNode.bd.mbist_be
+      res.mbist.mbist_addr := thisNode.bd.mbist_addr
+      res.mbist.mbist_indata := thisNode.bd.mbist_indata
+      res.mbist.mbist_readen := thisNode.bd.mbist_readen
+      res.mbist.mbist_addr_rd := thisNode.bd.mbist_addr_rd
+      thisNode.bd.mbist_outdata := res.mbist.mbist_outdata
+
+      res.toNextPipeline
+        .zip(pipelineNodes)
+        .foreach({
+          case (a, b) =>
+            b.bd.mbist_array := a.mbist_array
+            b.bd.mbist_all := a.mbist_all
+            b.bd.mbist_req := a.mbist_req
+            a.mbist_ack := b.bd.mbist_ack
+            b.bd.mbist_writeen := a.mbist_writeen
+            b.bd.mbist_be := a.mbist_be
+            b.bd.mbist_addr := a.mbist_addr
+            b.bd.mbist_indata := a.mbist_indata
+            b.bd.mbist_readen := a.mbist_readen
+            b.bd.mbist_addr_rd := a.mbist_addr_rd
+            a.mbist_outdata := b.bd.mbist_outdata
+        })
+
+      res.toSRAM
+        .zip(ramNodes)
+        .foreach({
+          case (a, b) =>
+            b.bd.addr := a.addr
+            b.bd.addr_rd := a.addr_rd
+            b.bd.wdata := a.wdata
+            b.bd.wmask := a.wmask
+            b.bd.re := a.re
+            b.bd.we := a.we
+            b.bd.ack := a.ack
+            b.bd.selectedOH := a.selectedOH
+            b.bd.array := a.array
+            b.bd.ere := a.ere
+            b.bd.ewe := a.ewe
+            a.rdata := b.bd.rdata
+        })
+      Some(res)
+    } else {
+      None
+    }
+  }
+}
+
+class MbistPipeline(level: Int, moduleName: String = s"MbistPipeline_${uniqueId}", val myNode: PipelineBaseNode)
+    extends Module {
+  override val desiredName = moduleName
+
+  def registerCSV(intf: InterfaceInfo, csvName: String): Unit = {
+    val gen = new MbistCsvGen(intf, this, csvName)
+    FileRegisters.add("mbist", s"$csvName.csv", gen.generate)
+  }
+
+  if (Mbist.isMaxLevel(level)) {
+    //Within every mbist domain, sram arrays are indexed from 0
+    SramHelper.restartIndexing()
+  }
+  val nodeParams = myNode.bd.params
+  val childrenIds = myNode.children.flatMap(_.array_id)
+
+  private val pipelineNodes =
+    myNode.children.filter(_.isInstanceOf[PipelineBaseNode]).map(_.asInstanceOf[PipelineBaseNode])
+  private val ramNodes = myNode.children.filter(_.isInstanceOf[RamBaseNode]).map(_.asInstanceOf[RamBaseNode])
+
+  val mbist = IO(new MbistBus(myNode.bd.params))
+  val toNextPipeline = pipelineNodes.map(_.bd.params).map(new MbistBus(_)).map(b => IO(Flipped(b)))
+  val toSRAM = ramNodes.map(_.bd.params).map(new Ram2Mbist(_)).map(b => IO(Flipped(b)))
+  dontTouch(mbist)
+  toNextPipeline.foreach(b => dontTouch(b))
+  toSRAM.foreach(b => dontTouch(b))
+
+  private val ere = mbist.mbist_readen
+  private val ewe = mbist.mbist_writeen
+  private val arrayHit = myNode.array_id.map(_.U === mbist.mbist_array).reduce(_ | _)
+  private val activated = mbist.mbist_all | (mbist.mbist_req & arrayHit)
+  private val dataValid = activated & (ere | ewe)
+
+  private val pipelineNodesAck =
+    if (pipelineNodes.nonEmpty) toNextPipeline.map(_.mbist_ack).reduce(_ | _) else true.B
+
+  private val arrayReg = RegEnable(mbist.mbist_array, 0.U, activated)
+  private val reqReg = RegNext(mbist.mbist_req, 0.U)
+  private val allReg = RegEnable(mbist.mbist_all, false.B, activated)
+  mbist.mbist_ack := reqReg & pipelineNodesAck
+
+  private val wenReg = RegEnable(mbist.mbist_writeen, 0.U, activated)
+  private val beReg = RegEnable(mbist.mbist_be, 0.U, dataValid)
+  private val addrReg = RegEnable(mbist.mbist_addr, 0.U, dataValid)
+  private val dataInReg = RegEnable(mbist.mbist_indata, 0.U, dataValid)
+
+  private val renReg = RegEnable(mbist.mbist_readen, 0.U, activated)
+  private val addrRdReg = RegEnable(mbist.mbist_addr_rd, 0.U, dataValid)
+
+  private val pipelineDataOut = Wire(Vec(toNextPipeline.length, mbist.mbist_outdata.cloneType))
+  private val sramDataOut = Wire(Vec(toSRAM.length, mbist.mbist_outdata.cloneType))
+  private val pipelineDataOutReg = RegEnable((pipelineDataOut :+ 0.U).reduce(_ | _), activated)
+  private val sramDataOutReg = (sramDataOut :+ 0.U).reduce(_ | _)
+
+  mbist.mbist_outdata := sramDataOutReg | pipelineDataOutReg
+
+  ramNodes
+    .zip(toSRAM)
+    .zip(sramDataOut)
+    .foreach({
+      case ((child, bd), dout) =>
+        val selectedVec = child.array_id.map(_.U === arrayReg)
+        val selected = selectedVec.reduce(_ || _)
+        val doSpread = selected || allReg
+        bd.addr := Mux(doSpread, addrReg(child.bd.params.addrWidth - 1, 0), 0.U)
+        bd.addr_rd := Mux(doSpread, addrRdReg(child.bd.params.addrWidth - 1, 0), 0.U)
+        bd.wdata := dataInReg(child.bd.params.dataWidth - 1, 0)
+        bd.re := Mux(doSpread, renReg, 0.U)
+        bd.we := Mux(doSpread, wenReg, 0.U)
+        bd.wmask := beReg(child.bd.params.maskWidth - 1, 0)
+        bd.ack := reqReg
+        bd.ere := ere
+        bd.ewe := ewe
+        bd.selectedOH := Fill(selectedVec.length, allReg) | Mux(
+          reqReg(0).asBool,
+          Cat(selectedVec.reverse),
+          ~0.U(child.bd.selectedOH.getWidth.W)
+        ).asUInt
+        bd.array := arrayReg
+        dout := Mux(selected, bd.rdata, 0.U)
+    })
+  pipelineNodes
+    .zip(toNextPipeline)
+    .zip(pipelineDataOut)
+    .foreach({
+      case ((child, bd), dout) =>
+        val selected = child.array_id.map(_.U === arrayReg).reduce(_ || _)
+        val doSpread = selected || allReg
+        bd.mbist_array := Mux(doSpread, arrayReg(child.bd.params.arrayWidth - 1, 0), 0.U)
+        bd.mbist_req := reqReg
+        bd.mbist_all := Mux(doSpread, allReg, 0.U)
+        bd.mbist_writeen := Mux(doSpread, wenReg, 0.U)
+        bd.mbist_be := beReg(child.bd.params.maskWidth - 1, 0)
+        bd.mbist_addr := Mux(doSpread, addrReg(child.bd.params.addrWidth - 1, 0), 0.U)
+        bd.mbist_indata := dataInReg(child.bd.params.dataWidth - 1, 0)
+        bd.mbist_readen := Mux(doSpread, renReg, 0.U)
+        bd.mbist_addr_rd := Mux(doSpread, addrRdReg(child.bd.params.addrWidth - 1, 0), 0.U)
+        dout := Mux(selected, bd.mbist_outdata, 0.U)
+    })
+}
diff --git a/src/main/scala/utility/sram/SRAMTemplate.scala b/src/main/scala/utility/sram/SRAMTemplate.scala
new file mode 100644
index 0000000..ca082db
--- /dev/null
+++ b/src/main/scala/utility/sram/SRAMTemplate.scala
@@ -0,0 +1,445 @@
+/** *************************************************************************************
+  * Copyright (c) 2020-2021 Institute of Computing Technology, Chinese Academy of Sciences
+  * Copyright (c) 2020-2021 Peng Cheng Laboratory
+  *
+  * XiangShan is licensed under Mulan PSL v2.
+  * You can use this software according to the terms and conditions of the Mulan PSL v2.
+  * You may obtain a copy of Mulan PSL v2 at:
+  *          http://license.coscl.org.cn/MulanPSL2
+  *
+  * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
+  * EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
+  * MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
+  *
+  * See the Mulan PSL v2 for more details.
+  * *************************************************************************************
+  */
+
+/** ************************************************************************************
+  * Copyright (c) 2020 Institute of Computing Technology, CAS
+  * Copyright (c) 2020 University of Chinese Academy of Sciences
+  *
+  * NutShell is licensed under Mulan PSL v2.
+  * You can use this software according to the terms and conditions of the Mulan PSL v2.
+  * You may obtain a copy of Mulan PSL v2 at:
+  *             http://license.coscl.org.cn/MulanPSL2
+  *
+  * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER
+  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR
+  * FIT FOR A PARTICULAR PURPOSE.
+  *
+  * See the Mulan PSL v2 for more details.
+  * *************************************************************************************
+  */
+
+package utility
+
+import chisel3._
+import chisel3.util._
+import sram._
+import utility.mbist.MbistClockGateCell
+
+class SRAMBundleA(val set: Int) extends Bundle {
+  val setIdx = Output(UInt(log2Up(set).W))
+
+  def apply(setIdx: UInt) = {
+    this.setIdx := setIdx
+    this
+  }
+}
+
+class SRAMBundleAW[T <: Data](private val gen: T, set: Int, val way: Int = 1, val useBitmask: Boolean = false)
+    extends SRAMBundleA(set) {
+
+  private val dataWidth = gen.getWidth
+
+  val data:    Vec[T] = Output(Vec(way, gen))
+  val waymask: Option[UInt] = if (way > 1) Some(Output(UInt(way.W))) else None
+  // flattened_bitmask is the flattened form of [waymask, bitmask], can be use directly to mask memory
+  val flattened_bitmask: Option[UInt] = if (useBitmask) Some(Output(UInt((way * dataWidth).W))) else None
+  // bitmask is the original bitmask passed from parameter
+  val bitmask: Option[UInt] = if (useBitmask) Some(Output(UInt((dataWidth).W))) else None
+
+  def apply(data: Vec[T], setIdx: UInt, waymask: UInt): SRAMBundleAW[T] = {
+    require(
+      waymask.getWidth == way,
+      s"waymask width does not equal nWays, waymask width: ${waymask.getWidth}, nWays: ${way}"
+    )
+    super.apply(setIdx)
+    this.data := data
+    this.waymask.foreach(_ := waymask)
+    this
+  }
+
+  def apply(data: Vec[T], setIdx: UInt, waymask: UInt, bitmask: UInt): SRAMBundleAW[T] = {
+    require(useBitmask, "passing bitmask when not using bitmask")
+    require(
+      bitmask.getWidth == dataWidth,
+      s"bitmask width does not equal data width, bitmask width: ${bitmask.getWidth}, data width: ${dataWidth}"
+    )
+    apply(data, setIdx, waymask)
+    this.flattened_bitmask.foreach(
+      _ :=
+        VecInit.tabulate(way * dataWidth)(n => waymask(n / dataWidth) && bitmask(n % dataWidth)).asUInt
+    )
+    this.bitmask.foreach(_ := bitmask)
+    this
+  }
+
+  // this could only be used when waymask is onehot or nway is 1
+  def apply(data: T, setIdx: UInt, waymask: UInt): SRAMBundleAW[T] = {
+    apply(VecInit(Seq.fill(way)(data)), setIdx, waymask)
+    this
+  }
+
+  def apply(data: T, setIdx: UInt, waymask: UInt, bitmask: UInt): SRAMBundleAW[T] = {
+    apply(VecInit(Seq.fill(way)(data)), setIdx, waymask, bitmask)
+    this
+  }
+}
+
+class SRAMBundleR[T <: Data](private val gen: T, val way: Int = 1) extends Bundle {
+  val data = Output(Vec(way, gen))
+}
+
+class SRAMReadBus[T <: Data](private val gen: T, val set: Int, val way: Int = 1) extends Bundle {
+  val req = Decoupled(new SRAMBundleA(set))
+  val resp = Flipped(new SRAMBundleR(gen, way))
+
+  def apply(valid: Bool, setIdx: UInt) = {
+    this.req.bits.apply(setIdx)
+    this.req.valid := valid
+    this
+  }
+}
+
+class SRAMWriteBus[T <: Data](
+  private val gen: T,
+  val set:         Int,
+  val way:         Int = 1,
+  val useBitmask:  Boolean = false)
+    extends Bundle {
+  val req = Decoupled(new SRAMBundleAW(gen, set, way, useBitmask))
+
+  def apply(valid: Bool, data: Vec[T], setIdx: UInt, waymask: UInt): SRAMWriteBus[T] = {
+    this.req.bits.apply(data = data, setIdx = setIdx, waymask = waymask)
+    this.req.valid := valid
+    this
+  }
+
+  def apply(valid: Bool, data: Vec[T], setIdx: UInt, waymask: UInt, bitmask: UInt): SRAMWriteBus[T] = {
+    this.req.bits.apply(data = data, setIdx = setIdx, waymask = waymask, bitmask = bitmask)
+    this.req.valid := valid
+    this
+  }
+
+  def apply(valid: Bool, data: T, setIdx: UInt, waymask: UInt): SRAMWriteBus[T] = {
+    apply(valid, VecInit(Seq.fill(way)(data)), setIdx, waymask)
+    this
+  }
+
+  def apply(valid: Bool, data: T, setIdx: UInt, waymask: UInt, bitmask: UInt): SRAMWriteBus[T] = {
+    apply(valid, VecInit(Seq.fill(way)(data)), setIdx, waymask, bitmask)
+    this
+  }
+}
+
+class SRAMTemplate[T <: Data](
+  gen:         T,
+  set:         Int,
+  way:         Int = 1,
+  singlePort:  Boolean = false,
+  shouldReset: Boolean = false,
+  extraReset:  Boolean = false,
+  holdRead:    Boolean = false,
+  bypassWrite: Boolean = false,
+  useBitmask:  Boolean = false,
+  multicycle:  Int = 1,
+  holdMcp:     Boolean = false,
+  hasMbist:    Boolean = false,
+  suffix:      String = "",
+  foundry:     String = "Unknown",
+  sramInst:    String = "STANDARD")
+    extends Module {
+  val io = IO(new Bundle {
+    val r = Flipped(new SRAMReadBus(gen, set, way))
+    val earlyRen = if(holdMcp) Some(Input(Bool())) else None
+    val w = Flipped(new SRAMWriteBus(gen, set, way, useBitmask))
+    val earlyWen = if(holdMcp) Some(Input(Bool())) else None
+  })
+  private val mcp = multicycle > 1
+  private val cg = Module(new MbistClockGateCell(mcp))
+  private val actualWay = if (useBitmask) gen.getWidth * way else way
+  private val dataWidth = gen.getWidth * way
+  require(multicycle >= 1)
+  private val elementWidth = if (useBitmask) 1 else gen.getWidth
+  private val (mbistBd, brcBd, array, nodeNum, realMaskBits) = SramHelper.genRam(
+    elementWidth,
+    actualWay,
+    set,
+    !singlePort,
+    mcp,
+    hasMbist,
+    cg.out_clock,
+    None,
+    suffix,
+    foundry,
+    sramInst,
+    this.asInstanceOf[SRAMTemplate[Data]]
+  )
+
+  val extra_reset = if (extraReset) Some(IO(Input(Bool()))) else None
+
+  val (resetState, resetSet) = (WireInit(false.B), WireInit(0.U))
+
+  require(multicycle == 1 & shouldReset || !shouldReset, "MCP rams do not support reset!")
+  if (shouldReset) {
+    withClockAndReset(cg.out_clock, reset) {
+      val _resetState = RegInit(true.B)
+      val (_resetSet, resetFinish) = Counter(_resetState, set)
+      when(resetFinish) {
+        _resetState := false.B
+      }
+      if (extra_reset.isDefined) {
+        when(extra_reset.get) {
+          _resetState := true.B
+        }
+      }
+      resetState := _resetState
+      resetSet := _resetSet
+    }
+  }
+  private val resetReg = RegNext(false.B, true.B)
+  private val wen = io.w.req.fire || resetState && !resetReg
+  private val _wmask =
+    if (useBitmask) io.w.req.bits.flattened_bitmask.getOrElse("b1".U) else io.w.req.bits.waymask.getOrElse("b1".U)
+  private val wmask = Mux(resetState, Fill(actualWay, true.B), _wmask)
+  private val wdata = Mux(resetState, 0.U, io.w.req.bits.data.asUInt)
+  private val waddr = Mux(resetState, resetSet, io.w.req.bits.setIdx)
+  private val ren = io.r.req.fire
+  private val raddr = io.r.req.bits.setIdx
+
+  private val ramWen = if (hasMbist) Mux(mbistBd.ack, mbistBd.we, wen) else wen
+  private val ramWaddr = if (hasMbist & singlePort) {
+    Mux(mbistBd.ack, mbistBd.addr_rd, waddr)
+  } else if (hasMbist & !singlePort) {
+    Mux(mbistBd.ack, mbistBd.addr, waddr)
+  } else {
+    waddr
+  }
+
+  private val nto1 = realMaskBits > way
+  private val fullMbistMask = if(actualWay > 1) Fill(nodeNum, mbistBd.wmask) else Fill(realMaskBits, true.B)
+  private val mbistWmask = if(nto1) {
+    mbistBd.selectedOH & fullMbistMask
+  } else {
+    val n = realMaskBits / nodeNum
+    val selMask = Cat(Seq.tabulate(realMaskBits)(i => mbistBd.selectedOH(i / n)).reverse)
+    selMask & fullMbistMask
+  }
+
+  private val funcWmask = if(nto1) {
+    val n = realMaskBits / way
+    Cat(Seq.tabulate(realMaskBits)(i => wmask(i / n)).reverse)
+  } else {
+    wmask
+  }
+
+  private val mbistWdata = Fill(nodeNum, mbistBd.wdata)
+  private val ramWmask = if (hasMbist) Mux(mbistBd.ack, mbistWmask, funcWmask) else funcWmask
+  private val ramWdata = if (hasMbist) Mux(mbistBd.ack, mbistWdata, wdata) else wdata
+  private val ramRen = if (hasMbist) Mux(mbistBd.ack, mbistBd.re, ren) else ren
+  private val ramRaddr = if (hasMbist) Mux(mbistBd.ack, mbistBd.addr_rd, raddr) else raddr
+
+  private val wenReg = RegInit(0.U(multicycle.W))
+  private val renReg = RegInit(0.U(multicycle.W))
+  wenReg := Cat(ramWen, wenReg) >> 1.U
+  renReg := Cat(ramRen, renReg) >> 1.U
+
+  private val wenStretched = if(holdMcp) {
+    val ewe = if(hasMbist) mbistBd.ewe || io.earlyWen.get else io.earlyWen.get
+    val wens = RegInit(0.U((multicycle - 1).W))
+    when(ewe) {
+      wens := Fill(multicycle - 1, true.B)
+    }.otherwise {
+      wens := Cat(ramWen, wens) >> 1.U
+    }
+    wens(0)
+  } else {
+    ramWen
+  }
+
+  private val renStretched = if(holdMcp) {
+    val ere = if(hasMbist) mbistBd.ere || io.earlyRen.get else io.earlyRen.get
+    val rens = RegInit(0.U((multicycle - 1).W))
+    when(ere) {
+      rens := Fill(multicycle - 1, true.B)
+    }.otherwise {
+      rens := Cat(ramRen, rens) >> 1.U
+    }
+    rens(0)
+  } else {
+    ramRen
+  }
+
+  private val ramRdata = SramProto.read(array, singlePort, ramRaddr, renStretched)
+  when(wenStretched && !brcBd.ram_hold) {
+    SramProto.write(array, singlePort, ramWaddr, ramWdata, ramWmask)
+  }
+
+  cg.dft.fromBroadcast(brcBd)
+  cg.mbist.req := mbistBd.ack
+  cg.mbist.readen := mbistBd.re
+  cg.mbist.writeen := mbistBd.we
+  cg.E := ren || wen
+
+  private val concurrentRW = io.w.req.fire && io.r.req.fire && io.w.req.bits.setIdx === io.r.req.bits.setIdx
+  private val doBypass = if (bypassWrite) concurrentRW else false.B
+  private val doBypassReg = RegEnable(doBypass, false.B, io.r.req.fire)
+  private val wmaskReg = RegEnable(wmask, 0.U, doBypass & io.r.req.fire)
+  private val segment = dataWidth / wmask.getWidth
+  private val bypassMask = Cat(Seq.tabulate(wmask.getWidth)(i => wmaskReg(i / segment).asBool).reverse)
+  private val keepMask = Cat(Seq.tabulate(wmask.getWidth)(i => !wmaskReg(i / segment).asBool).reverse)
+  private val rdataReg = Reg(UInt(dataWidth.W))
+  private val bypassData = bypassMask & rdataReg | keepMask & ramRdata
+  if(bypassWrite) {
+    when(doBypass) {
+      rdataReg := wdata.asUInt
+    }.elsewhen(renReg(0)){
+      rdataReg := Mux(doBypassReg, bypassData, ramRdata)
+    }
+  } else {
+    when(renReg(0)) {
+      rdataReg := ramRdata
+    }
+  }
+
+  if(!bypassWrite && !holdRead) {
+    io.r.resp.data := ramRdata.asTypeOf(io.r.resp.data)
+  } else if(!bypassWrite && holdRead) {
+    io.r.resp.data := Mux(renReg(0), ramRdata, rdataReg).asTypeOf(io.r.resp.data)
+  } else if (bypassWrite && !holdRead) {
+    io.r.resp.data := Mux(doBypassReg, bypassData, ramRdata).asTypeOf(io.r.resp.data)
+  } else {
+    when(renReg(0)){
+      io.r.resp.data := Mux(doBypassReg, bypassData, ramRdata).asTypeOf(io.r.resp.data)
+    }.otherwise {
+      io.r.resp.data := rdataReg.asTypeOf(io.r.resp.data)
+    }
+  }
+  private val mbistDw = dataWidth / nodeNum
+  private val selectOHReg = RegEnable(mbistBd.selectedOH, renReg(0))
+  mbistBd.rdata := Mux1H(selectOHReg, rdataReg.asTypeOf(Vec(nodeNum, UInt(mbistDw.W))))
+
+  private val reqVector = renReg | wenReg
+  private val singleHold = if (singlePort) io.w.req.valid else false.B
+  private val mcpHold = if(mcp) reqVector(multicycle - 1, 1).orR else false.B
+  private val resetHold = if(shouldReset) resetState || resetReg else false.B
+  io.r.req.ready := !mcpHold && !resetHold && !singleHold
+  io.w.req.ready := !mcpHold && !resetHold
+  if(!bypassWrite) {
+    assert(!concurrentRW, "Concurrent reading and writing to the same addr of SRAM is not allowed!")
+  }
+}
+
+class FoldedSRAMTemplate[T <: Data](
+  gen:         T,
+  set:         Int,
+  width:       Int = 4,
+  way:         Int = 1,
+  shouldReset: Boolean = false,
+  extraReset:  Boolean = false,
+  holdRead:    Boolean = false,
+  singlePort:  Boolean = false,
+  bypassWrite: Boolean = false,
+  useBitmask:  Boolean = false,
+  hasMbist : Boolean = false)
+    extends Module {
+  val io = IO(new Bundle {
+    val r = Flipped(new SRAMReadBus(gen, set, way))
+    val w = Flipped(new SRAMWriteBus(gen, set, way, useBitmask))
+  })
+  val extra_reset = if (extraReset) Some(IO(Input(Bool()))) else None
+  //   |<----- setIdx ----->|
+  //   | ridx | width | way |
+
+  require(width > 0 && isPow2(width))
+  require(way > 0 && isPow2(way))
+  require(set % width == 0)
+
+  val nRows = set / width
+
+  val array = Module(
+    new SRAMTemplate(
+      gen,
+      set = nRows,
+      way = width * way,
+      shouldReset = shouldReset,
+      extraReset = extraReset,
+      holdRead = holdRead,
+      singlePort = singlePort,
+      bypassWrite = bypassWrite,
+      useBitmask = useBitmask,
+      hasMbist = hasMbist
+    )
+  )
+  if (array.extra_reset.isDefined) {
+    array.extra_reset.get := extra_reset.get
+  }
+
+  io.r.req.ready := array.io.r.req.ready
+  io.w.req.ready := array.io.w.req.ready
+
+  val raddr = io.r.req.bits.setIdx >> log2Ceil(width)
+  val ridx = RegEnable(if (width != 1) io.r.req.bits.setIdx(log2Ceil(width) - 1, 0) else 0.U(1.W), io.r.req.valid)
+  val ren = io.r.req.valid
+
+  array.io.r.req.valid := ren
+  array.io.r.req.bits.setIdx := raddr
+
+  val rdata = array.io.r.resp.data
+  for (w <- 0 until way) {
+    val wayData = VecInit(rdata.indices.filter(_ % way == w).map(rdata(_)))
+    val holdRidx = HoldUnless(ridx, GatedValidRegNext(io.r.req.valid))
+    val realRidx = if (holdRead) holdRidx else ridx
+    io.r.resp.data(w) := Mux1H(UIntToOH(realRidx, width), wayData)
+  }
+
+  val wen = io.w.req.valid
+  val wdata = VecInit(Seq.fill(width)(io.w.req.bits.data).flatten)
+  val waddr = io.w.req.bits.setIdx >> log2Ceil(width)
+  val widthIdx = if (width != 1) io.w.req.bits.setIdx(log2Ceil(width) - 1, 0) else 0.U
+  val wmask = (width, way) match {
+    case (1, 1) => 1.U(1.W)
+    case (x, 1) => UIntToOH(widthIdx)
+    case _ =>
+      VecInit(Seq.tabulate(width * way)(n => (n / way).U === widthIdx && io.w.req.bits.waymask.get(n % way))).asUInt
+  }
+  require(wmask.getWidth == way * width)
+
+  if (useBitmask) {
+    array.io.w.apply(wen, wdata, waddr, wmask, io.w.req.bits.bitmask.get)
+  } else {
+    array.io.w.apply(wen, wdata, waddr, wmask)
+  }
+}
+
+class SRAMTemplateWithArbiter[T <: Data](nRead: Int, gen: T, set: Int, way: Int = 1, shouldReset: Boolean = false)
+    extends Module {
+  val io = IO(new Bundle {
+    val r = Flipped(Vec(nRead, new SRAMReadBus(gen, set, way)))
+    val w = Flipped(new SRAMWriteBus(gen, set, way))
+  })
+
+  val ram = Module(new SRAMTemplate(gen, set, way, shouldReset = shouldReset, holdRead = false, singlePort = true))
+  ram.io.w <> io.w
+
+  val readArb = Module(new Arbiter(chiselTypeOf(io.r(0).req.bits), nRead))
+  readArb.io.in <> io.r.map(_.req)
+  ram.io.r.req <> readArb.io.out
+
+  // latch read results
+  for (r <- io.r) {
+    r.resp.data := HoldUnless(ram.io.r.resp.data, GatedValidRegNext(r.req.fire))
+  }
+}
diff --git a/src/main/scala/utility/sram/SramHelper.scala b/src/main/scala/utility/sram/SramHelper.scala
new file mode 100644
index 0000000..f3b7c71
--- /dev/null
+++ b/src/main/scala/utility/sram/SramHelper.scala
@@ -0,0 +1,139 @@
+package utility.sram
+import chisel3._
+import chisel3.experimental.hierarchy.Instance
+import chisel3.util._
+import chisel3.util.experimental.BoringUtils
+import utility.SRAMTemplate
+import utility.mbist.Mbist._
+import utility.mbist.{Mbist, Ram2Mbist, Ram2MbistParams}
+
+import scala.collection.mutable
+import scala.math.sqrt
+
+object SramHelper {
+  private var nodeId = 0
+  private var wrapperId = 0
+  private var domainId = 0
+  private val broadCastBdQueue = new mutable.Queue[SramBroadcastBundle]
+
+  private def getWayNumForEachNodeAndNodeNum_1toN(dw: Int, way: Int, mw: Int): (Int, Int) = {
+    val dataNum1toNNode = mw / dw
+    val numVec = (1 to dataNum1toNNode)
+    val validVec = numVec.map(num => (way % num == 0) && (way >= num))
+    val validNum = numVec.zip(validVec).filter(_._2)
+    val res = if (validNum.isEmpty) (1, way) else validNum.last
+    (res._1, way / res._1)
+  }
+
+  private def getDivisor(in: Int): Seq[Int] = {
+    val end = sqrt(in).toInt
+    val divisors =
+      Seq.tabulate(end)(_ + 1).map(idx => (in % idx == 0, Seq(idx, in / idx))).filter(_._1).flatMap(_._2).sorted
+    divisors
+  }
+
+  private def getNodeNumForEachWayAndNodeNum_Nto1(dw: Int, way: Int, mw: Int): (Int, Int) = {
+    val divisors = getDivisor(dw)
+    val validDivisors = divisors.filter(_ <= mw)
+    val goodNodeNumForEachWay = dw / validDivisors.max
+    val defaultNodeNumForEachWay = ((dw + mw - 1) / mw)
+    val finalNodeNumForEachWay =
+      if (goodNodeNumForEachWay > 4 * defaultNodeNumForEachWay) defaultNodeNumForEachWay else goodNodeNumForEachWay
+    (finalNodeNumForEachWay, way * finalNodeNumForEachWay)
+  }
+
+  def restartIndexing(): Unit = domainId = 0
+
+  private def getDomainID(): Int = domainId
+
+  private def increaseDomainID(add: Int): Unit = domainId += add
+
+  def genBroadCastBundleTop(): SramBroadcastBundle = {
+    val res = Wire(new SramBroadcastBundle)
+    broadCastBdQueue.toSeq.foreach(bd => {
+      BoringUtils.bore(bd) := res
+    })
+    broadCastBdQueue.clear()
+    res
+  }
+
+  def genRam(
+    ew:       Int,
+    way:      Int,
+    set:      Int,
+    dp:       Boolean,
+    mcp:      Boolean,
+    bist:     Boolean,
+    rclk:     Clock,
+    wclk:     Option[Clock],
+    suffix:   String,
+    foundry:  String,
+    sramInst: String,
+    template: SRAMTemplate[Data]
+  ): (Ram2Mbist, SramBroadcastBundle, Instance[SramArray], Int, Int) = {
+    val isNto1 = ew > maxMbistDataWidth
+    //** ******implement mbist interface node(multiple nodes for one way)******
+    val (mbistNodeNumForEachWay, mbistNodeNumNto1) = getNodeNumForEachWayAndNodeNum_Nto1(ew, way, maxMbistDataWidth)
+    val maskWidthNto1 = 1
+    val mbistDataWidthNto1 = (ew + mbistNodeNumForEachWay - 1) / mbistNodeNumForEachWay
+    //** *******implement mbist interface node(one node for multiple ways)******
+    val (wayNumForEachNode, mbistNodeNum1toN) = getWayNumForEachNodeAndNodeNum_1toN(ew, way, maxMbistDataWidth)
+    val mbistDataWidth1toN = wayNumForEachNode * ew
+    val maskWidth1toN = wayNumForEachNode
+
+    val mbistNodeNum = if (isNto1) mbistNodeNumNto1 else mbistNodeNum1toN
+    val mbistDataWidth = if (isNto1) mbistDataWidthNto1 else mbistDataWidth1toN
+    val mbistMaskWidth = if (isNto1) maskWidthNto1 else maskWidth1toN
+    val mbistArrayIds = Seq.tabulate(mbistNodeNum)(idx => getDomainID() + idx)
+    val bitWrite = way != 1
+    val sramMaskBits = if(isNto1) mbistNodeNum else way
+
+    val (array, vname) = SramProto(rclk, !dp, set, ew * way, sramMaskBits, mcp, wclk, bist, suffix)
+    val bdParam =
+      Ram2MbistParams(
+        set,
+        mbistDataWidth,
+        mbistMaskWidth,
+        !dp,
+        vname,
+        "",
+        mbistNodeNum,
+        mbistArrayIds.max,
+        bitWrite,
+        foundry,
+        sramInst,
+        0,
+        "None",
+        template
+      )
+    val mbist = if (bist) Some(IO(new Ram2Mbist(bdParam))) else None
+    val mbistBundle = Wire(new Ram2Mbist(bdParam))
+    mbistBundle := DontCare
+    mbistBundle.selectedOH := Fill(mbistBundle.selectedOH.getWidth, 1.U(1.W))
+    mbistBundle.ack := false.B
+    mbistBundle.we := false.B
+    mbistBundle.re := false.B
+    mbistBundle.wmask := Fill(mbistMaskWidth, true.B)
+    val broadCastSignals = Wire(new SramBroadcastBundle)
+    broadCastSignals := DontCare
+    if (bist) {
+      dontTouch(mbist.get)
+      mbist.get := DontCare
+      mbist.get.suggestName("mbist")
+      mbistBundle <> mbist.get
+      Mbist.addRamNode(mbist.get, mbistArrayIds)
+      val addId = if (isNto1) mbistNodeNumNto1 else mbistNodeNum1toN
+      nodeId += addId
+      increaseDomainID(addId)
+      val broadcast = IO(new SramBroadcastBundle)
+      broadcast := DontCare
+      dontTouch(broadcast)
+      broadcast.suggestName("broadcast")
+      broadCastSignals := broadcast
+      broadCastBdQueue.enqueue(broadcast)
+      array.mbist.get.dft_ram_bp_clken := broadcast.ram_bp_clken
+      array.mbist.get.dft_ram_bypass := broadcast.ram_bypass
+    }
+    (mbistBundle, broadCastSignals, array, mbistNodeNum, sramMaskBits)
+  }
+}
diff --git a/src/main/scala/utility/sram/SramProto.scala b/src/main/scala/utility/sram/SramProto.scala
new file mode 100644
index 0000000..c4a44cb
--- /dev/null
+++ b/src/main/scala/utility/sram/SramProto.scala
@@ -0,0 +1,213 @@
+package utility.sram
+import chisel3._
+import chisel3.util._
+import chisel3.experimental.hierarchy.{instantiable, public, Definition, Instance}
+import scala.collection.mutable
+
+class SramMbistIO extends Bundle {
+  val dft_ram_bypass = Input(Bool())
+  val dft_ram_bp_clken = Input(Bool())
+}
+
+class SramBroadcastBundle extends Bundle {
+  val ram_hold = Input(Bool())
+  val ram_bypass = Input(Bool())
+  val ram_bp_clken = Input(Bool())
+  val ram_aux_clk = Input(Bool())
+  val ram_aux_ckbp = Input(Bool())
+  val ram_mcp_hold = Input(Bool())
+  val cgen = Input(Bool())
+}
+
+@instantiable
+abstract class SramArray(
+  depth:        Int,
+  width:        Int,
+  maskSegments: Int,
+  hasMbist:     Boolean,
+  sramName:     Option[String] = None,
+  singlePort:   Boolean)
+    extends RawModule {
+  require(width % maskSegments == 0)
+  @public val mbist = if (hasMbist) Some(IO(new SramMbistIO)) else None
+  if (mbist.isDefined) {
+    dontTouch(mbist.get)
+  }
+
+  @public val RW0 = if (singlePort) {
+    Some(IO(new Bundle() {
+      val clk = Input(Clock())
+      val addr = Input(UInt(log2Ceil(depth).W))
+      val en = Input(Bool())
+      val wmode = Input(Bool())
+      val wmask = if (maskSegments > 1) Input(UInt(maskSegments.W)) else Input(UInt(0.W))
+      val wdata = Input(UInt(width.W))
+      val rdata = Output(UInt(width.W))
+    }))
+  } else {
+    None
+  }
+
+  @public val R0 = if (!singlePort) {
+    Some(IO(new Bundle() {
+      val clk = Input(Clock())
+      val addr = Input(UInt(log2Ceil(depth).W))
+      val en = Input(Bool())
+      val data = Output(UInt(width.W))
+    }))
+  } else {
+    None
+  }
+
+  @public val W0 = if (!singlePort) {
+    Some(IO(new Bundle() {
+      val clk = Input(Clock())
+      val addr = Input(UInt(log2Ceil(depth).W))
+      val en = Input(Bool())
+      val data = Input(UInt(width.W))
+      val mask = if (maskSegments > 1) Input(UInt(maskSegments.W)) else Input(UInt(0.W))
+    }))
+  } else {
+    None
+  }
+
+  override def desiredName: String = sramName.getOrElse(super.desiredName)
+}
+
+@instantiable
+class SramArray1P(
+  depth:        Int,
+  width:        Int,
+  maskSegments: Int,
+  hasMbist:     Boolean,
+  sramName:     Option[String] = None
+) extends SramArray(depth, width, maskSegments, hasMbist, sramName, true) {
+  if (maskSegments > 1) {
+    val dataType = Vec(maskSegments, UInt((width / maskSegments).W))
+    val array = SyncReadMem(depth, dataType)
+    RW0.get.rdata := array
+      .readWrite(
+        RW0.get.addr,
+        RW0.get.wdata.asTypeOf(dataType),
+        RW0.get.wmask.asBools,
+        RW0.get.en,
+        RW0.get.wmode,
+        RW0.get.clk
+      )
+      .asUInt
+  } else {
+    val array = SyncReadMem(depth, UInt(width.W))
+    RW0.get.rdata := array.readWrite(
+      RW0.get.addr,
+      RW0.get.wdata,
+      RW0.get.en,
+      RW0.get.wmode,
+      RW0.get.clk
+    )
+  }
+}
+
+@instantiable
+class SramArray2P(
+  depth:        Int,
+  width:        Int,
+  maskSegments: Int,
+  hasMbist:     Boolean,
+  sramName:     Option[String] = None,
+) extends SramArray(depth, width, maskSegments, hasMbist, sramName, false) {
+  require(width % maskSegments == 0)
+
+  if (maskSegments > 1) {
+    val dataType = Vec(maskSegments, UInt((width / maskSegments).W))
+    val array = SyncReadMem(depth, dataType, SyncReadMem.WriteFirst)
+    when(W0.get.en) {
+      array.write(W0.get.addr, W0.get.data.asTypeOf(dataType), W0.get.mask.asBools, W0.get.clk)
+    }
+    R0.get.data := array.read(R0.get.addr, R0.get.en, R0.get.clk).asUInt
+  } else {
+    val array = SyncReadMem(depth, UInt(width.W))
+    when(W0.get.en) {
+      array.write(W0.get.addr, W0.get.data, W0.get.clk)
+    }
+    R0.get.data := array.read(R0.get.addr, R0.get.en, R0.get.clk)
+  }
+}
+
+object SramProto {
+  private val defMap = mutable.Map[String, Definition[SramArray]]()
+
+  def init(sram: Instance[SramArray], singlePort: Boolean, clock: Clock, writeClock: Option[Clock]): Unit = {
+    if (singlePort) {
+      dontTouch(sram.RW0.get)
+      sram.RW0.get := DontCare
+      sram.RW0.get.clk := clock
+      sram.RW0.get.en := false.B
+    } else {
+      dontTouch(sram.R0.get)
+      dontTouch(sram.W0.get)
+      sram.R0.get := DontCare
+      sram.R0.get.clk := clock
+      sram.R0.get.en := false.B
+      sram.W0.get := DontCare
+      sram.W0.get.clk := writeClock.getOrElse(clock)
+      sram.W0.get.en := false.B
+    }
+  }
+
+  def read(sram: Instance[SramArray], singlePort: Boolean, addr: UInt, enable: Bool): UInt = {
+    if (singlePort) {
+      sram.RW0.get.addr := addr
+      sram.RW0.get.en := enable
+      sram.RW0.get.wmode := false.B
+      sram.RW0.get.rdata
+    } else {
+      sram.R0.get.addr := addr
+      sram.R0.get.en := enable
+      sram.R0.get.data
+    }
+  }
+
+  def write(sram: Instance[SramArray], singlePort: Boolean, addr: UInt, data: UInt, mask: UInt): Unit = {
+    if (singlePort) {
+      sram.RW0.get.addr := addr
+      sram.RW0.get.en := true.B
+      sram.RW0.get.wmode := true.B
+      if (sram.RW0.get.wmask.getWidth > 1) sram.RW0.get.wmask := mask else sram.RW0.get.wmask := true.B
+      sram.RW0.get.wdata := data
+    } else {
+      sram.W0.get.addr := addr
+      sram.W0.get.en := true.B
+      if (sram.W0.get.mask.getWidth > 1) sram.W0.get.mask := mask else sram.W0.get.mask := true.B
+      sram.W0.get.data := data
+    }
+  }
+
+  def apply(
+    clock:        Clock,
+    singlePort:   Boolean,
+    depth:        Int,
+    width:        Int,
+    maskSegments: Int = 1,
+    MCP:          Boolean = false,
+    writeClock:   Option[Clock] = None,
+    hasMbist:     Boolean,
+    suffix:       String = ""
+  ): (Instance[SramArray], String) = {
+    val mbist = if (hasMbist) "_bist" else ""
+    val mcpPrefix = if (MCP) "_multicycle" else ""
+    val numPort = if (singlePort) 1 else 2
+    val maskWidth = width / maskSegments
+    val sramName = Some(s"sram_array_${numPort}p${depth}x${width}m$maskWidth$mbist$mcpPrefix$suffix")
+    if (!defMap.contains(sramName.get)) {
+      val sramDef = if (singlePort) {
+        Definition(new SramArray1P(depth, width, maskSegments, hasMbist, sramName))
+      } else {
+        Definition(new SramArray2P(depth, width, maskSegments, hasMbist, sramName))
+      }
+      defMap(sramName.get) = sramDef
+    }
+    val array = Instance(defMap(sramName.get))
+    SramProto.init(array, singlePort, clock, writeClock)
+    (array, sramName.get)
+  }
+}
diff --git a/src/test/scala/MbistTestTop.scala b/src/test/scala/MbistTestTop.scala
new file mode 100644
index 0000000..682e090
--- /dev/null
+++ b/src/test/scala/MbistTestTop.scala
@@ -0,0 +1,68 @@
+package top
+import chisel3._
+import utility.SRAMTemplate
+import utility.mbist.{MbistInterface, MbistPipeline}
+import utility.sram.{SramBroadcastBundle, SramHelper}
+
+class MBISTSubMod extends Module {
+  private val sram0 = Module(
+    new SRAMTemplate(
+      gen = UInt(24.W),
+      set = 32,
+      way = 1,
+      singlePort = true,
+      shouldReset = false,
+      holdRead = true,
+      extraReset = false,
+      bypassWrite = false,
+      useBitmask = true,
+      multicycle = 2,
+      hasMbist = true,
+      foundry = "TSMC28",
+      sramInst = "ts1n28hpcpuhdlvtb32x24m2sw_170a"
+    )
+  )
+  private val sram1 = Module(
+    new SRAMTemplate(
+      gen = UInt(6.W),
+      set = 32,
+      way = 2,
+      singlePort = false,
+      shouldReset = false,
+      holdRead = true,
+      extraReset = false,
+      bypassWrite = true,
+      useBitmask = false,
+      multicycle = 1,
+      hasMbist = true,
+      foundry = "TSMC28",
+      sramInst = "ts6n28hpcplvta32x6m4sw_130a"
+    )
+  )
+  val io = IO(new Bundle {
+    val intf0 = sram0.io.cloneType
+    val intf1 = sram1.io.cloneType
+  })
+  sram0.io <> io.intf0
+  sram1.io <> io.intf1
+}
+
+class MbistTestTop extends Module {
+  private val sramMod = Module(new MBISTSubMod)
+  private val pipeline = MbistPipeline.PlaceMbistPipeline(Int.MaxValue, "TopMBISTPipeline").get
+  private val intf = Module(new MbistInterface(Seq(pipeline.nodeParams), Seq(pipeline.childrenIds), "TopMBISTIntf", 1))
+  pipeline.mbist <> intf.toPipeline.head
+  pipeline.registerCSV(intf.info, "MbistTestTop")
+  private val broadCasts = SramHelper.genBroadCastBundleTop()
+
+  val io = IO(new Bundle {
+    val broadcast = new SramBroadcastBundle
+    val sram0 = sramMod.io.intf0.cloneType
+    val sram1 = sramMod.io.intf1.cloneType
+    val mbist = intf.mbist.cloneType
+  })
+  broadCasts := io.broadcast
+  sramMod.io.intf0 <> io.sram0
+  sramMod.io.intf1 <> io.sram1
+  intf.mbist <> io.mbist
+}
diff --git a/src/test/scala/TestTop.scala b/src/test/scala/TestTop.scala
new file mode 100644
index 0000000..03cbe4d
--- /dev/null
+++ b/src/test/scala/TestTop.scala
@@ -0,0 +1,101 @@
+package top
+
+import chisel3._
+import circt.stage.{ChiselStage, FirtoolOption}
+import utility._
+import chisel3.stage.ChiselGeneratorAnnotation
+import org.chipsalliance.cde.config.{Config, Field, Parameters}
+import scala.annotation.tailrec
+import scala.reflect.runtime.universe._
+
+case object OptKey extends Field[Opt]
+
+case class Opt(module: String = "", build: String = "build")
+
+class DefaultConfig
+    extends Config((site, here, up) => {
+      case OptKey => Opt()
+    })
+
+object Parser {
+
+  def apply(args: Array[String]): (Parameters, Array[String]) = {
+    val defaultConfig = new DefaultConfig
+    var firrtlOpts = Array[String]()
+    var hasHelp: Boolean = false
+    var hasTd:   Boolean = false
+
+    @tailrec
+    def parse(config: Parameters, args: List[String]): Parameters = {
+      args match {
+        case Nil => config
+
+        case "--help" :: tail =>
+          println("""
+                    |Customized Options
+                    |  --module <full class name>
+          """.stripMargin)
+          hasHelp = true
+          parse(config, tail)
+
+        case "--module" :: confStr :: tail =>
+          parse(
+            config.alter((site, here, up) => {
+              case OptKey => up(OptKey).copy(module = confStr)
+            }),
+            tail
+          )
+
+        case "-td" :: bdStr :: tail =>
+          hasTd = true
+          parse(
+            config.alter((site, here, up) => {
+              case OptKey => up(OptKey).copy(build = bdStr)
+            }),
+            tail
+          )
+
+        case option :: tail =>
+          firrtlOpts :+= option
+          parse(config, tail)
+      }
+    }
+
+    val cfg = parse(defaultConfig, args.toList)
+    if (hasHelp) firrtlOpts :+= "--help"
+    if (hasTd) {
+      firrtlOpts :+= "-td"
+      firrtlOpts :+= cfg(OptKey).build
+    }
+    (cfg, firrtlOpts)
+  }
+}
+
+object TestTop extends App {
+  val (config, firrtlOpts) = Parser(args)
+  val module = config(OptKey).module
+  lazy val m = Class.forName("top." + module).getDeclaredConstructor().newInstance().asInstanceOf[RawModule]
+  (new ChiselStage).execute(
+    firrtlOpts,
+    Seq(
+      FirtoolOption("-O=release"),
+      FirtoolOption("--split-verilog"),
+      FirtoolOption(s"-o=${config(OptKey).build}/rtl"),
+      FirtoolOption("--disable-all-randomization"),
+      FirtoolOption("--disable-annotation-unknown"),
+      FirtoolOption("--strip-debug-info"),
+      FirtoolOption("--lower-memories"),
+      FirtoolOption("--add-vivado-ram-address-conflict-synthesis-bug-workaround"),
+      FirtoolOption(
+        "--lowering-options=noAlwaysComb," +
+          " disallowPortDeclSharing, disallowLocalVariables," +
+          " emittedLineLength=120, explicitBitcast, locationInfoStyle=plain," +
+          " disallowExpressionInliningInPorts, disallowMuxInlining"
+      ),
+      ChiselGeneratorAnnotation(() => m)
+    )
+  )
+  val mod = module.split("\\.").last
+  mbist.MbistFileManager.write("mbist")
+  FileRegisters.write(config(OptKey).build, mod + ".")
+}