diff --git a/src/main/scala/uk/ac/soton/ecs/can/CanConfiguration.scala b/src/main/scala/uk/ac/soton/ecs/can/config/CanConfiguration.scala
similarity index 73%
rename from src/main/scala/uk/ac/soton/ecs/can/CanConfiguration.scala
rename to src/main/scala/uk/ac/soton/ecs/can/config/CanConfiguration.scala
index d5008b8264ecb710adf5c184a9452598864f847c..38d33aec60040abfa26a61432cf30b8815289a5c 100644
--- a/src/main/scala/uk/ac/soton/ecs/can/CanConfiguration.scala
+++ b/src/main/scala/uk/ac/soton/ecs/can/config/CanConfiguration.scala
@@ -1,9 +1,7 @@
 // SPDX-FileCopyrightText: 2021 Minyong Li <ml10g20@soton.ac.uk>
 // SPDX-License-Identifier: CERN-OHL-W-2.0
 
-package uk.ac.soton.ecs.can
-
-import core.CanCoreConfiguration
+package uk.ac.soton.ecs.can.config
 
 case class CanConfiguration(
     core: CanCoreConfiguration
diff --git a/src/main/scala/uk/ac/soton/ecs/can/core/CanCoreConfiguration.scala b/src/main/scala/uk/ac/soton/ecs/can/config/CanCoreConfiguration.scala
similarity index 84%
rename from src/main/scala/uk/ac/soton/ecs/can/core/CanCoreConfiguration.scala
rename to src/main/scala/uk/ac/soton/ecs/can/config/CanCoreConfiguration.scala
index 725a3ab4e37cd923abbd9dad3569585407dfa76d..e17b7319c99fe62d59c5ae6523c869449ed9081c 100644
--- a/src/main/scala/uk/ac/soton/ecs/can/core/CanCoreConfiguration.scala
+++ b/src/main/scala/uk/ac/soton/ecs/can/config/CanCoreConfiguration.scala
@@ -1,9 +1,10 @@
 // SPDX-FileCopyrightText: 2021 Minyong Li <ml10g20@soton.ac.uk>
 // SPDX-License-Identifier: CERN-OHL-W-2.0
 
-package uk.ac.soton.ecs.can.core
+package uk.ac.soton.ecs.can.config
 
 case class CanCoreConfiguration(
+    immediateWidth: Int,
     programMemoryWords: Int,
     dataMemoryWords: Int,
     syncReadMemory: Boolean,
diff --git a/src/main/scala/uk/ac/soton/ecs/can/core/Adder.scala b/src/main/scala/uk/ac/soton/ecs/can/core/Adder.scala
index 87049e8463588fbe57f23c967d228c1cb1f364ce..7d470511dac5d16212b242a78d6adb0ba3f248d6 100644
--- a/src/main/scala/uk/ac/soton/ecs/can/core/Adder.scala
+++ b/src/main/scala/uk/ac/soton/ecs/can/core/Adder.scala
@@ -6,9 +6,9 @@ package uk.ac.soton.ecs.can.core
 import chisel3._
 
 class Adder extends MultiIOModule {
-  val lhs = IO(Input(Vec(16, UInt(32.W))))
-  val rhs = IO(Input(Vec(16, UInt(32.W))))
-  val out = IO(Output(Vec(16, UInt(32.W))))
+  val lhs = IO(Input(UInt(512.W)))
+  val rhs = IO(Input(UInt(512.W)))
+  val out = IO(Output(UInt(512.W)))
 
-  out := lhs.zip(rhs).map { case (lhs, rhs) => lhs + rhs }
+  out := lhs + rhs
 }
diff --git a/src/main/scala/uk/ac/soton/ecs/can/core/BaseQuarterRound.scala b/src/main/scala/uk/ac/soton/ecs/can/core/BaseQuarterRound.scala
new file mode 100644
index 0000000000000000000000000000000000000000..e855983a893a15b3fbc966edc813facac204d8e3
--- /dev/null
+++ b/src/main/scala/uk/ac/soton/ecs/can/core/BaseQuarterRound.scala
@@ -0,0 +1,14 @@
+// SPDX-FileCopyrightText: 2021 Minyong Li <ml10g20@soton.ac.uk>
+// SPDX-License-Identifier: CERN-OHL-W-2.0
+
+package uk.ac.soton.ecs.can.core
+
+import chisel3._
+
+abstract class BaseQuarterRound extends MultiIOModule {
+  val in = IO(Input(Vec(4, UInt(32.W))))
+  val out = IO(Output(Vec(4, UInt(32.W))))
+
+  protected def rotateLeft(v: UInt, b: Int): UInt =
+    v(31 - b, 0) ## v(31, 32 - b)
+}
diff --git a/src/main/scala/uk/ac/soton/ecs/can/core/BaseRound.scala b/src/main/scala/uk/ac/soton/ecs/can/core/BaseRound.scala
new file mode 100644
index 0000000000000000000000000000000000000000..e05e6ed3aa3f8d5023bde73c703b704f552ed9fb
--- /dev/null
+++ b/src/main/scala/uk/ac/soton/ecs/can/core/BaseRound.scala
@@ -0,0 +1,25 @@
+// SPDX-FileCopyrightText: 2021 Minyong Li <ml10g20@soton.ac.uk>
+// SPDX-License-Identifier: CERN-OHL-W-2.0
+
+package uk.ac.soton.ecs.can.core
+
+import chisel3._
+
+abstract class BaseRound extends MultiIOModule {
+  val in = IO(Input(UInt(512.W)))
+  val out = IO(Output(UInt(512.W)))
+
+  protected val _in = in.asTypeOf(Vec(16, UInt(32.W)))
+  protected val _out = Wire(Vec(16, UInt(32.W)))
+  out := _out.asUInt()
+
+  protected def wire(wireBox: Seq[Seq[Int]]): Unit = wireBox.foreach {
+    wireSeq =>
+      val quarterRound = Module(new CombinationalQuarterRound)
+      quarterRound.in.zip(quarterRound.out).zip(wireSeq).foreach {
+        case ((i, o), w) =>
+          i := _in(w)
+          _out(w) := o
+      }
+  }
+}
diff --git a/src/main/scala/uk/ac/soton/ecs/can/core/BlockInitializer.scala b/src/main/scala/uk/ac/soton/ecs/can/core/BlockInitializer.scala
index f69ebcfb09075890bbce23eaa2549c674f71abde..d555f4abb36f284779d79e2782f3a38d14c92311 100644
--- a/src/main/scala/uk/ac/soton/ecs/can/core/BlockInitializer.scala
+++ b/src/main/scala/uk/ac/soton/ecs/can/core/BlockInitializer.scala
@@ -4,32 +4,26 @@
 package uk.ac.soton.ecs.can.core
 
 import chisel3._
+import uk.ac.soton.ecs.can.types.ChaCha20IETFBlock
 
 class BlockInitializer extends MultiIOModule {
   val fillConstants = IO(Input(Bool()))
   val incrementBlockCount = IO(Input(Bool()))
-  val in = IO(Input(Vec(16, UInt(32.W))))
-  val out = IO(Output(Vec(16, UInt(32.W))))
+  val in = IO(Input(UInt(512.W)))
+  val out = IO(Output(UInt(512.W)))
 
-  private val constants = VecInit(
-    "h61707865".U(32.W),
-    "h3320646e".U(32.W),
-    "h79622d32".U(32.W),
-    "h6b206574".U(32.W)
-  )
-  private val incrementedBlockCount = in(12) + 1.U(32.W)
-
-  private val io = in.zip(out)
-
-  io.take(4).zip(constants).foreach { case ((i, o), c) =>
-    o := Mux(fillConstants, c, i)
-  }
+  private val _in = in.asTypeOf(new ChaCha20IETFBlock)
+  private val _out = Wire(new ChaCha20IETFBlock)
+  out := _out.asUInt()
 
-  io.slice(4, 12).foreach { case (i, o) => o := i }
+  private val constant = "h617078653320646e79622d326b206574".U(128.W)
 
-  io(12) match {
-    case (i, o) => o := Mux(incrementBlockCount, incrementedBlockCount, i)
-  }
-
-  io.takeRight(3).foreach { case (i, o) => o := i }
+  _out.constant := Mux(fillConstants, constant, _in.constant)
+  _out.key := _in.key
+  _out.blockCount := Mux(
+    incrementBlockCount,
+    _in.blockCount + 1.U,
+    _in.blockCount
+  )
+  _out.nonce := _in.nonce
 }
diff --git a/src/main/scala/uk/ac/soton/ecs/can/core/CanCore.scala b/src/main/scala/uk/ac/soton/ecs/can/core/CanCore.scala
index 2ea297cdcf7cfd7a0e7f70c5e7a2a7e713222bc0..231700cf3d5a6508744b4c0508888774f86647fc 100644
--- a/src/main/scala/uk/ac/soton/ecs/can/core/CanCore.scala
+++ b/src/main/scala/uk/ac/soton/ecs/can/core/CanCore.scala
@@ -5,13 +5,15 @@ package uk.ac.soton.ecs.can.core
 
 import chisel3._
 import chisel3.util.log2Ceil
+import uk.ac.soton.ecs.can.config.CanCoreConfiguration
+import uk.ac.soton.ecs.can.types.CanCoreControlWord
 
 class CanCore(implicit cfg: CanCoreConfiguration) extends MultiIOModule {
 
   // ========== Calculated Parameters ========== //
 
   private val programMemoryAddressWidth = log2Ceil(cfg.programMemoryWords)
-  private val controlWordWidth = ControlWord(programMemoryAddressWidth).getWidth
+  private val controlWordWidth = (new CanCoreControlWord).getWidth
   private val dataMemoryAddressWidth = log2Ceil(cfg.dataMemoryWords)
   private val blockWidth = 512
 
@@ -45,25 +47,11 @@ class CanCore(implicit cfg: CanCoreConfiguration) extends MultiIOModule {
 
   // ========== Modules ========== //
 
-  private val programMemory = Module(
-    new ProgramMemory(
-      programMemoryAddressWidth,
-      controlWordWidth,
-      cfg.programMemoryWords,
-      cfg.syncReadMemory
-    )
-  )
-  private val dataMemory = Module(
-    new DataMemory(
-      dataMemoryAddressWidth,
-      blockWidth,
-      cfg.dataMemoryWords,
-      cfg.syncReadMemory
-    )
-  )
+  private val programMemory = Module(new ProgramMemory)
+  private val dataMemory = Module(new DataMemory)
   private val blockInitializer = Module(new BlockInitializer)
-  private val columnarRound = Module(ChaChaRound.columnar)
-  private val diagonalRound = Module(ChaChaRound.diagonal)
+  private val columnarRound = Module(new ColumnarRound)
+  private val diagonalRound = Module(new DiagonalRound)
   private val adder = Module(new Adder)
   private val xorer = Module(new Xorer)
 
@@ -71,28 +59,26 @@ class CanCore(implicit cfg: CanCoreConfiguration) extends MultiIOModule {
 
   private val afterBlockInitializer =
     if (cfg.regAfterBlockInitializer)
-      Reg(Vec(16, UInt(32.W)))
+      Reg(UInt(512.W))
     else
-      Wire(Vec(16, UInt(32.W)))
+      Wire(UInt(512.W))
   private val betweenRounds =
     if (cfg.regBetweenRounds)
-      Reg(Vec(16, UInt(32.W)))
+      Reg(UInt(512.W))
     else
-      Wire(Vec(16, UInt(32.W)))
-  private val afterRounds = Reg(Vec(16, UInt(32.W)))
+      Wire(UInt(512.W))
+  private val afterRounds = Reg(UInt(512.W))
   private val afterAdder =
     if (cfg.regAfterAdder)
-      Reg(Vec(16, UInt(32.W)))
+      Reg(UInt(512.W))
     else
-      Wire(Vec(16, UInt(32.W)))
-  private val afterXorer = Wire(Vec(16, UInt(32.W)))
+      Wire(UInt(512.W))
+  private val afterXorer = Wire(UInt(512.W))
 
   // ========== Buses (Port Aliases) ========== //
 
-  private val ctrl =
-    programMemory.cw.asTypeOf(ControlWord(programMemoryAddressWidth))
-  private val data =
-    dataMemory.read.map(_.data.asTypeOf(Vec(16, UInt(32.W))))
+  private val ctrl = programMemory.cw.asTypeOf(new CanCoreControlWord)
+  private val data = dataMemory.read.map(_.data)
 
   // ========== Multiplexers ========== //
 
@@ -113,22 +99,22 @@ class CanCore(implicit cfg: CanCoreConfiguration) extends MultiIOModule {
   programMemory.read <> io.programMemory.read
   programMemory.write <> io.programMemory.write
 
-  dataMemory.read(0).addr := ctrl.ramReadAddress(0)
+  dataMemory.read(0).addr := ctrl.dataMemoryReadAddress(0)
   dataMemory.read(1).addr := Mux(
     io.dataMemory.take,
     io.dataMemory.read.addr,
-    ctrl.ramReadAddress(1)
+    ctrl.dataMemoryReadAddress(1)
   )
   io.dataMemory.read.data := dataMemory.read(1).data
   dataMemory.write.en := Mux(
     io.dataMemory.take,
     io.dataMemory.write.en,
-    ctrl.ramWriteEnable
+    ctrl.dataMemoryWriteEnable
   )
   dataMemory.write.addr := Mux(
     io.dataMemory.take,
     io.dataMemory.write.addr,
-    ctrl.ramWriteAddress
+    ctrl.dataMemoryWriteAddress
   )
   dataMemory.write.data := Mux(
     io.dataMemory.take,
diff --git a/src/main/scala/uk/ac/soton/ecs/can/core/ChaChaRound.scala b/src/main/scala/uk/ac/soton/ecs/can/core/ChaChaRound.scala
deleted file mode 100644
index 8fa39ba5843081dd7bf9d7744e01541e9f20e875..0000000000000000000000000000000000000000
--- a/src/main/scala/uk/ac/soton/ecs/can/core/ChaChaRound.scala
+++ /dev/null
@@ -1,40 +0,0 @@
-// SPDX-FileCopyrightText: 2021 Minyong Li <ml10g20@soton.ac.uk>
-// SPDX-License-Identifier: CERN-OHL-W-2.0
-
-package uk.ac.soton.ecs.can.core
-
-import chisel3._
-
-class ChaChaRound(wires: Seq[Seq[Int]]) extends MultiIOModule {
-  val in = IO(Input(Vec(16, UInt(32.W))))
-  val out = IO(Output(Vec(16, UInt(32.W))))
-
-  wires.foreach { qRWires =>
-    val quarterRound = Module(new QuarterRound)
-
-    qRWires.zipWithIndex.foreach { case (qRWire, index) =>
-      quarterRound.in(index) := in(qRWire)
-      out(qRWire) := quarterRound.out(index)
-    }
-  }
-}
-
-object ChaChaRound {
-  def columnar = new ChaChaRound(
-    Seq(
-      Seq(0, 4, 8, 12),
-      Seq(1, 5, 9, 13),
-      Seq(2, 6, 10, 14),
-      Seq(3, 7, 11, 15)
-    )
-  )
-
-  def diagonal = new ChaChaRound(
-    Seq(
-      Seq(0, 5, 10, 15),
-      Seq(1, 6, 11, 12),
-      Seq(2, 7, 8, 13),
-      Seq(3, 4, 9, 14)
-    )
-  )
-}
diff --git a/src/main/scala/uk/ac/soton/ecs/can/core/ColumnarRound.scala b/src/main/scala/uk/ac/soton/ecs/can/core/ColumnarRound.scala
new file mode 100644
index 0000000000000000000000000000000000000000..d44203d2aed9d4a02176ff52934360942f95e2b8
--- /dev/null
+++ b/src/main/scala/uk/ac/soton/ecs/can/core/ColumnarRound.scala
@@ -0,0 +1,15 @@
+// SPDX-FileCopyrightText: 2021 Minyong Li <ml10g20@soton.ac.uk>
+// SPDX-License-Identifier: CERN-OHL-W-2.0
+
+package uk.ac.soton.ecs.can.core
+
+class ColumnarRound extends BaseRound {
+  wire(
+    Seq(
+      Seq(0, 4, 8, 12),
+      Seq(1, 5, 9, 13),
+      Seq(2, 6, 10, 14),
+      Seq(3, 7, 11, 15)
+    )
+  )
+}
diff --git a/src/main/scala/uk/ac/soton/ecs/can/core/QuarterRound.scala b/src/main/scala/uk/ac/soton/ecs/can/core/CombinationalQuarterRound.scala
similarity index 71%
rename from src/main/scala/uk/ac/soton/ecs/can/core/QuarterRound.scala
rename to src/main/scala/uk/ac/soton/ecs/can/core/CombinationalQuarterRound.scala
index b84740baa9d1e52321e6fcaff1da1cc4439406f3..7f6e3a49a9cd68eb35cb9871afdc08de1cd32661 100644
--- a/src/main/scala/uk/ac/soton/ecs/can/core/QuarterRound.scala
+++ b/src/main/scala/uk/ac/soton/ecs/can/core/CombinationalQuarterRound.scala
@@ -3,15 +3,7 @@
 
 package uk.ac.soton.ecs.can.core
 
-import chisel3._
-
-class QuarterRound extends MultiIOModule {
-  val in = IO(Input(Vec(4, UInt(32.W))))
-  val out = IO(Output(Vec(4, UInt(32.W))))
-
-  private def rotateLeft(v: UInt, b: Int): UInt =
-    v(31 - b, 0) ## v(31, 32 - b)
-
+class CombinationalQuarterRound extends BaseQuarterRound {
   private val a0 = in(0)
   private val b0 = in(1)
   private val c0 = in(2)
diff --git a/src/main/scala/uk/ac/soton/ecs/can/core/ControlWord.scala b/src/main/scala/uk/ac/soton/ecs/can/core/ControlWord.scala
deleted file mode 100644
index 32838e9eee49c7250280371c0d34f147b076b4d5..0000000000000000000000000000000000000000
--- a/src/main/scala/uk/ac/soton/ecs/can/core/ControlWord.scala
+++ /dev/null
@@ -1,27 +0,0 @@
-// SPDX-FileCopyrightText: 2021 Minyong Li <ml10g20@soton.ac.uk>
-// SPDX-License-Identifier: CERN-OHL-W-2.0
-
-package uk.ac.soton.ecs.can.core
-
-import chisel3._
-
-class ControlWord(addrWidth: Int, immWidth: Int = 8) extends Bundle {
-  val immediate = UInt(immWidth.W)
-  val absoluteBranch = Bool()
-  val relativeBranch = Bool()
-  val ramReadAddress = Vec(2, UInt(addrWidth.W))
-  val ramWriteEnable = Bool()
-  val ramWriteAddress = UInt(addrWidth.W)
-  val fillConstants = Bool()
-  val incrementBlockCount = Bool()
-  val roundLoop = Bool()
-  val addFrom = Bool()
-  val xorFrom = Bool()
-  val writeBackFromInit = Bool()
-  val writeBackFromRound = Bool()
-}
-
-object ControlWord {
-  def apply(addrWidth: Int, immWidth: Int = 8) =
-    new ControlWord(addrWidth, immWidth)
-}
diff --git a/src/main/scala/uk/ac/soton/ecs/can/core/DataMemory.scala b/src/main/scala/uk/ac/soton/ecs/can/core/DataMemory.scala
index 9eaf108bf44c3335cf777fa3664bba9d515a921f..cd5cade25b95fc2da72652ecbef194a93f5c16e9 100644
--- a/src/main/scala/uk/ac/soton/ecs/can/core/DataMemory.scala
+++ b/src/main/scala/uk/ac/soton/ecs/can/core/DataMemory.scala
@@ -4,31 +4,32 @@
 package uk.ac.soton.ecs.can.core
 
 import chisel3._
+import chisel3.util.log2Ceil
+import uk.ac.soton.ecs.can.config.CanCoreConfiguration
+
+class DataMemory(implicit cfg: CanCoreConfiguration) extends MultiIOModule {
+  private val addrWidth = log2Ceil(cfg.dataMemoryWords)
 
-class DataMemory(
-    addrWidth: Int,
-    dataWidth: Int,
-    size: Int,
-    syncMem: Boolean
-) extends MultiIOModule {
   val read = IO(
     Vec(
       2,
       new Bundle {
         val addr = Input(UInt(addrWidth.W))
-        val data = Output(UInt(dataWidth.W))
+        val data = Output(UInt(512.W))
       }
     )
   )
   val write = IO(new Bundle {
     val en = Input(Bool())
     val addr = Input(UInt(addrWidth.W))
-    val data = Input(UInt(dataWidth.W))
+    val data = Input(UInt(512.W))
   })
 
   private val mem =
-    if (syncMem) SyncReadMem(size, UInt(dataWidth.W))
-    else Mem(size, UInt(dataWidth.W))
+    if (cfg.syncReadMemory)
+      SyncReadMem(cfg.dataMemoryWords, UInt(512.W))
+    else
+      Mem(cfg.dataMemoryWords, UInt(512.W))
 
   read.foreach(p => p.data := mem(p.addr))
 
diff --git a/src/main/scala/uk/ac/soton/ecs/can/core/DiagonalRound.scala b/src/main/scala/uk/ac/soton/ecs/can/core/DiagonalRound.scala
new file mode 100644
index 0000000000000000000000000000000000000000..a95747372f0ee087dc44837bc9568253b05e3608
--- /dev/null
+++ b/src/main/scala/uk/ac/soton/ecs/can/core/DiagonalRound.scala
@@ -0,0 +1,15 @@
+// SPDX-FileCopyrightText: 2021 Minyong Li <ml10g20@soton.ac.uk>
+// SPDX-License-Identifier: CERN-OHL-W-2.0
+
+package uk.ac.soton.ecs.can.core
+
+class DiagonalRound extends BaseRound {
+  wire(
+    Seq(
+      Seq(0, 5, 10, 15),
+      Seq(1, 6, 11, 12),
+      Seq(2, 7, 8, 13),
+      Seq(3, 4, 9, 14)
+    )
+  )
+}
diff --git a/src/main/scala/uk/ac/soton/ecs/can/core/ProgramMemory.scala b/src/main/scala/uk/ac/soton/ecs/can/core/ProgramMemory.scala
index 5074434bb331df563483f5fc5a784e8700fd4f38..5d2f87e13fa9b68497f837a220c50516ec30cba3 100644
--- a/src/main/scala/uk/ac/soton/ecs/can/core/ProgramMemory.scala
+++ b/src/main/scala/uk/ac/soton/ecs/can/core/ProgramMemory.scala
@@ -4,13 +4,14 @@
 package uk.ac.soton.ecs.can.core
 
 import chisel3._
+import chisel3.util.log2Ceil
+import uk.ac.soton.ecs.can.types.CanCoreControlWord
+import uk.ac.soton.ecs.can.config.CanCoreConfiguration
+
+class ProgramMemory(implicit cfg: CanCoreConfiguration) extends MultiIOModule {
+  private val addrWidth = log2Ceil(cfg.programMemoryWords)
+  private val cwWidth = (new CanCoreControlWord).getWidth
 
-class ProgramMemory(
-    addrWidth: Int,
-    cwWidth: Int,
-    nWords: Int,
-    syncMem: Boolean
-) extends MultiIOModule {
   val br = IO(new Bundle {
     val abs = Input(Bool())
     val rel = Input(Bool())
@@ -29,8 +30,11 @@ class ProgramMemory(
   })
 
   private val mem =
-    if (syncMem) SyncReadMem(nWords, UInt(cwWidth.W))
-    else Mem(nWords, UInt(cwWidth.W))
+    if (cfg.syncReadMemory)
+      SyncReadMem(cfg.programMemoryWords, UInt(cwWidth.W))
+    else
+      Mem(cfg.programMemoryWords, UInt(cwWidth.W))
+
   private val pc = RegInit(0.U(addrWidth.W))
 
   when(br.abs) {
diff --git a/src/main/scala/uk/ac/soton/ecs/can/core/Xorer.scala b/src/main/scala/uk/ac/soton/ecs/can/core/Xorer.scala
index d0824752a7157031d631bc7c6870dcaa428e7c86..e009f2c2d7a7bd74f48cea0fe7aeaa2a0daaeb3b 100644
--- a/src/main/scala/uk/ac/soton/ecs/can/core/Xorer.scala
+++ b/src/main/scala/uk/ac/soton/ecs/can/core/Xorer.scala
@@ -6,9 +6,9 @@ package uk.ac.soton.ecs.can.core
 import chisel3._
 
 class Xorer extends MultiIOModule {
-  val lhs = IO(Input(Vec(16, UInt(32.W))))
-  val rhs = IO(Input(Vec(16, UInt(32.W))))
-  val out = IO(Output(Vec(16, UInt(32.W))))
+  val lhs = IO(Input(UInt(512.W)))
+  val rhs = IO(Input(UInt(512.W)))
+  val out = IO(Output(UInt(512.W)))
 
-  out := lhs.zip(rhs).map { case (lhs, rhs) => lhs ^ rhs }
+  out := lhs ^ rhs
 }
diff --git a/src/main/scala/uk/ac/soton/ecs/can/types/CanCoreControlWord.scala b/src/main/scala/uk/ac/soton/ecs/can/types/CanCoreControlWord.scala
new file mode 100644
index 0000000000000000000000000000000000000000..19ee395fc3540c6e13e174ae02b369db9ec6de47
--- /dev/null
+++ b/src/main/scala/uk/ac/soton/ecs/can/types/CanCoreControlWord.scala
@@ -0,0 +1,27 @@
+// SPDX-FileCopyrightText: 2021 Minyong Li <ml10g20@soton.ac.uk>
+// SPDX-License-Identifier: CERN-OHL-W-2.0
+
+package uk.ac.soton.ecs.can.types
+
+import chisel3._
+import chisel3.util.log2Ceil
+import uk.ac.soton.ecs.can.config.CanCoreConfiguration
+
+class CanCoreControlWord(implicit val cfg: CanCoreConfiguration)
+    extends Bundle {
+  private val dataMemoryAddrWidth = log2Ceil(cfg.dataMemoryWords)
+
+  val immediate = UInt(cfg.immediateWidth.W)
+  val absoluteBranch = Bool()
+  val relativeBranch = Bool()
+  val dataMemoryReadAddress = Vec(2, UInt(dataMemoryAddrWidth.W))
+  val dataMemoryWriteEnable = Bool()
+  val dataMemoryWriteAddress = UInt(dataMemoryAddrWidth.W)
+  val fillConstants = Bool()
+  val incrementBlockCount = Bool()
+  val roundLoop = Bool()
+  val addFrom = Bool()
+  val xorFrom = Bool()
+  val writeBackFromInit = Bool()
+  val writeBackFromRound = Bool()
+}
diff --git a/src/main/scala/uk/ac/soton/ecs/can/types/ChaCha20IETFBlock.scala b/src/main/scala/uk/ac/soton/ecs/can/types/ChaCha20IETFBlock.scala
new file mode 100644
index 0000000000000000000000000000000000000000..62bfa799472d1113770abc0e344efdb2d3f50d8e
--- /dev/null
+++ b/src/main/scala/uk/ac/soton/ecs/can/types/ChaCha20IETFBlock.scala
@@ -0,0 +1,13 @@
+// SPDX-FileCopyrightText: 2021 Minyong Li <ml10g20@soton.ac.uk>
+// SPDX-License-Identifier: CERN-OHL-W-2.0
+
+package uk.ac.soton.ecs.can.types
+
+import chisel3._
+
+class ChaCha20IETFBlock extends Bundle {
+  val constant = UInt(128.W)
+  val key = UInt(256.W)
+  val blockCount = UInt(32.W)
+  val nonce = UInt(96.W)
+}
diff --git a/src/test/scala/uk/ac/soton/ecs/can/core/AdderTest.scala b/src/test/scala/uk/ac/soton/ecs/can/core/AdderTest.scala
deleted file mode 100644
index 6fe7840c7bbaef5a4d76ac07d33ee8a2fc8964db..0000000000000000000000000000000000000000
--- a/src/test/scala/uk/ac/soton/ecs/can/core/AdderTest.scala
+++ /dev/null
@@ -1,30 +0,0 @@
-// SPDX-FileCopyrightText: 2021 Minyong Li <ml10g20@soton.ac.uk>
-// SPDX-License-Identifier: GPL-3.0-or-later
-
-package uk.ac.soton.ecs.can.core
-
-import org.scalatest._
-import chiseltest._
-import chisel3._
-import scala.util.Random
-import scala.math.abs
-
-class AdderTest extends FlatSpec with ChiselScalatestTester {
-  private val maxUInt = (Int.MaxValue.toLong << 1) | 1
-
-  behavior of "The Adder"
-
-  it should "sum the 16 32b unsigned integers" in {
-    test(new Adder) { c =>
-      val randomLhs = c.lhs.map(_ => abs(Random.nextInt))
-      val randomRhs = c.rhs.map(_ => abs(Random.nextInt))
-      val randomRes = randomLhs.zip(randomRhs).map { case (l, r) =>
-        (l.toLong + r.toLong) % maxUInt
-      }
-
-      c.lhs.zip(randomLhs).foreach { case (p, r) => p.poke(r.U) }
-      c.rhs.zip(randomRhs).foreach { case (p, r) => p.poke(r.U) }
-      c.out.zip(randomRes).foreach { case (p, r) => p.expect(r.U) }
-    }
-  }
-}
diff --git a/src/test/scala/uk/ac/soton/ecs/can/core/BlockInitializerTest.scala b/src/test/scala/uk/ac/soton/ecs/can/core/BlockInitializerTest.scala
deleted file mode 100644
index f5f53245111ab4585c3c04657329b2f2e0bf7b0a..0000000000000000000000000000000000000000
--- a/src/test/scala/uk/ac/soton/ecs/can/core/BlockInitializerTest.scala
+++ /dev/null
@@ -1,48 +0,0 @@
-// SPDX-FileCopyrightText: 2021 Minyong Li <ml10g20@soton.ac.uk>
-// SPDX-License-Identifier: GPL-3.0-or-later
-
-package uk.ac.soton.ecs.can.core
-
-import org.scalatest._
-import chiseltest._
-import chisel3._
-
-class BlockInitializerTest extends FlatSpec with ChiselScalatestTester {
-  private val input = Seq.fill(16)(0.U(32.W))
-
-  behavior of "The Block Initializer"
-
-  it should "fill the ChaCha constant when requested" in {
-    test(new BlockInitializer) { c =>
-      c.in.zip(input).foreach { case (p, n) => p.poke(n) }
-
-      c.fillConstants.poke(true.B)
-      c.out(0).expect("h61707865".U(32.W))
-      c.out(1).expect("h3320646e".U(32.W))
-      c.out(2).expect("h79622d32".U(32.W))
-      c.out(3).expect("h6b206574".U(32.W))
-      c.out.takeRight(12).foreach(_.expect(0.U(32.W)))
-
-      c.fillConstants.poke(false.B)
-      c.out.foreach(_.expect(0.U(32.W)))
-    }
-  }
-
-  it should "increment the block count when requested" in {
-    test(new BlockInitializer) { c =>
-      c.in.zip(input).foreach { case (p, n) => p.poke(n) }
-
-      c.incrementBlockCount.poke(true.B)
-      (0 until 10).foreach { i =>
-        c.in(12).poke(i.U(32.W))
-        c.out(12).expect((i + 1).U(32.W))
-      }
-
-      c.in(12).poke(0.U(32.W))
-
-      c.incrementBlockCount.poke(false.B)
-      c.out.foreach(_.expect(0.U(32.W)))
-    }
-  }
-
-}
diff --git a/src/test/scala/uk/ac/soton/ecs/can/core/DataMemoryTest.scala b/src/test/scala/uk/ac/soton/ecs/can/core/DataMemoryTest.scala
deleted file mode 100644
index e7200b089bc2add699636ae6f6d154378b84fefe..0000000000000000000000000000000000000000
--- a/src/test/scala/uk/ac/soton/ecs/can/core/DataMemoryTest.scala
+++ /dev/null
@@ -1,55 +0,0 @@
-// SPDX-FileCopyrightText: 2021 Minyong Li <ml10g20@soton.ac.uk>
-// SPDX-License-Identifier: GPL-3.0-or-later
-
-package uk.ac.soton.ecs.can.core
-
-import org.scalatest._
-import chiseltest._
-import chisel3._
-
-class DataMemoryTest extends FlatSpec with ChiselScalatestTester {
-  private val addrWidth = 8
-  private val dataWidth = 16
-  private val size = 32
-
-  behavior of "The Data Memory"
-
-  it should "store some values" in {
-    test(new DataMemory(addrWidth, dataWidth, size)) { c =>
-      c.write.addr.poke("h01".U(addrWidth.W))
-      c.write.data.poke("h1234".U(dataWidth.W))
-      c.write.en.poke(true.B)
-      c.clock.step()
-      c.write.en.poke(false.B)
-      c.read.foreach(_.addr.poke("h01".U(addrWidth.W)))
-      c.clock.step()
-      c.read.foreach(_.data.expect("h1234".U(dataWidth.W)))
-
-      c.write.addr.poke("h0a".U(addrWidth.W))
-      c.write.data.poke("hfefe".U(dataWidth.W))
-      c.write.en.poke(true.B)
-      c.clock.step()
-      c.write.en.poke(false.B)
-      c.read.foreach(_.addr.poke("h0a".U(addrWidth.W)))
-      c.clock.step()
-      c.read.foreach(_.data.expect("hfefe".U(dataWidth.W)))
-    }
-  }
-
-  it should "not write without write enable" in {
-    test(new DataMemory(addrWidth, dataWidth, size)) { c =>
-      c.write.addr.poke("h06".U(addrWidth.W))
-      c.write.data.poke("hcafe".U(dataWidth.W))
-      c.write.en.poke(true.B)
-      c.clock.step()
-      c.write.en.poke(false.B)
-      c.read.foreach(_.addr.poke("h06".U(addrWidth.W)))
-      c.clock.step()
-      c.read.foreach(_.data.expect("hcafe".U(dataWidth.W)))
-
-      c.write.data.poke("hefac".U(dataWidth.W))
-      c.clock.step()
-      c.read.foreach(_.data.expect("hcafe".U(dataWidth.W)))
-    }
-  }
-}
diff --git a/src/test/scala/uk/ac/soton/ecs/can/core/ProgramMemoryTest.scala b/src/test/scala/uk/ac/soton/ecs/can/core/ProgramMemoryTest.scala
deleted file mode 100644
index 96ffa8961ac3f5d5cb3168384766e486d7b3365a..0000000000000000000000000000000000000000
--- a/src/test/scala/uk/ac/soton/ecs/can/core/ProgramMemoryTest.scala
+++ /dev/null
@@ -1,153 +0,0 @@
-// SPDX-FileCopyrightText: 2021 Minyong Li <ml10g20@soton.ac.uk>
-// SPDX-License-Identifier: GPL-3.0-or-later
-
-package uk.ac.soton.ecs.can.core
-
-import org.scalatest._
-import chiseltest._
-import chisel3._
-
-import scala.util.Random
-import scala.math.pow
-
-class ProgramMemoryTest extends FlatSpec with ChiselScalatestTester {
-  private val addrWidth = 8
-  private val immWidth = 8
-  private val cwWidth = ControlWord(addrWidth, immWidth).getWidth
-  private val memMap = Seq.fill(8)(
-    Random
-      .nextInt(pow(2, cwWidth).toInt - 1)
-      .asUInt(cwWidth.W)
-  )
-  private val nWords = memMap.length
-
-  private def initMemory(pm: ProgramMemory) {
-    pm.reset.poke(true.B)
-    pm.write.en.poke(true.B)
-
-    memMap.zipWithIndex.foreach { case (data, addr) =>
-      pm.write.addr.poke(addr.U(addrWidth.W))
-      pm.write.data.poke(data)
-      pm.clock.step()
-    }
-
-    pm.write.en.poke(false.B)
-    pm.reset.poke(false.B)
-  }
-
-  behavior of "The Program Memory"
-
-  it should "be writable and readable as PC increments" in {
-    test(new ProgramMemory(addrWidth, cwWidth, nWords)) { c =>
-      c.br.abs.poke(false.B)
-      c.br.rel.poke(false.B)
-      c.br.addr.poke(0.U(immWidth.W))
-
-      initMemory(c)
-
-      c.cw.expect(memMap.head)
-      c.clock.step()
-
-      // NOTE: FPGA block RAM is synchronous-read. At this moment a new value
-      // has been fetched, but this 1-cycle delay exists because it hasn't been
-      // stored into the read register yet.
-      c.cw.expect(memMap.head)
-      c.clock.step()
-
-      memMap.tail.foreach { data =>
-        c.cw.expect(data)
-        c.clock.step()
-      }
-    }
-
-    test(new ProgramMemory(addrWidth, cwWidth, nWords, false)) { c =>
-      c.br.abs.poke(false.B)
-      c.br.rel.poke(false.B)
-      c.br.addr.poke(0.U(immWidth.W))
-
-      initMemory(c)
-
-      memMap.foreach { data =>
-        c.cw.expect(data)
-        c.clock.step()
-      }
-    }
-  }
-
-  it should "do relative branching correctly" in {
-    test(new ProgramMemory(addrWidth, cwWidth, nWords)) { c =>
-      c.br.abs.poke(false.B)
-      c.br.rel.poke(false.B)
-      c.br.addr.poke(0.U(immWidth.W))
-
-      initMemory(c)
-
-      c.cw.expect(memMap.head)
-      c.clock.step()
-
-      memMap.take(3).foreach { data =>
-        c.cw.expect(data)
-        c.clock.step()
-      }
-
-      // @ 0x03
-      c.cw.expect(memMap(3))
-
-      // > 0x06
-      // NOTE: Because of the synchronous BRAM, a 1-cycle delay slot is
-      // introduced. At this moment 0x04 has been fetched, so the offset should
-      // be calculated based on 0x04 rather than 0x03. Here 4 + 2 = 6.
-      c.br.addr.poke(2.U(immWidth.W))
-      c.br.rel.poke(true.B)
-      c.clock.step()
-      c.br.rel.poke(false.B)
-
-      // 0x04 is now present, but 0x06 has been fetched
-      c.cw.expect(memMap(4))
-      c.clock.step()
-
-      // @ 0x06 now
-      memMap.takeRight(2).foreach { data =>
-        c.cw.expect(data)
-        c.clock.step()
-      }
-    }
-  }
-
-  it should "do absolute branching correctly" in {
-    test(new ProgramMemory(addrWidth, cwWidth, nWords)) { c =>
-      c.br.abs.poke(false.B)
-      c.br.rel.poke(false.B)
-      c.br.addr.poke(0.U(immWidth.W))
-
-      initMemory(c)
-
-      c.cw.expect(memMap.head)
-      c.clock.step()
-
-      memMap.take(5).foreach { data =>
-        c.cw.expect(data)
-        c.clock.step()
-      }
-
-      // @ 0x05
-      c.cw.expect(memMap(5))
-
-      // > 0x01
-      c.br.addr.poke("h01".U(immWidth.W))
-      c.br.abs.poke(true.B)
-      c.clock.step()
-      c.br.abs.poke(false.B)
-
-      // Delay slot: 0x06 will present no matter what
-      c.cw.expect(memMap(6))
-      c.clock.step()
-
-      // @ 0x01 now
-      memMap.tail.foreach { data =>
-        c.cw.expect(data)
-        c.clock.step()
-      }
-    }
-  }
-}
diff --git a/src/test/scala/uk/ac/soton/ecs/can/core/QuarterRoundTest.scala b/src/test/scala/uk/ac/soton/ecs/can/core/QuarterRoundTest.scala
deleted file mode 100644
index d67f1a26f7eff139df72b7993516b78961e16927..0000000000000000000000000000000000000000
--- a/src/test/scala/uk/ac/soton/ecs/can/core/QuarterRoundTest.scala
+++ /dev/null
@@ -1,40 +0,0 @@
-// SPDX-FileCopyrightText: 2021 Minyong Li <ml10g20@soton.ac.uk>
-// SPDX-License-Identifier: GPL-3.0-or-later
-
-package uk.ac.soton.ecs.can.core
-
-import org.scalatest._
-import chiseltest._
-import chisel3._
-
-class QuarterRoundTest extends FlatSpec with ChiselScalatestTester {
-  behavior of "The ChaCha Quarter Round Function"
-
-  it should "compute RFC8439 2.1.1 test vector correctly" in {
-    test(new QuarterRound) { c =>
-      c.in(0).poke("h11111111".U(32.W))
-      c.in(1).poke("h01020304".U(32.W))
-      c.in(2).poke("h9b8d6f43".U(32.W))
-      c.in(3).poke("h01234567".U(32.W))
-
-      c.out(0).expect("hea2a92f4".U(32.W))
-      c.out(1).expect("hcb1cf8ce".U(32.W))
-      c.out(2).expect("h4581472e".U(32.W))
-      c.out(3).expect("h5881c4bb".U(32.W))
-    }
-  }
-
-  it should "compute RFC8439 2.2.1 test vector correctly" in {
-    test(new QuarterRound) { c =>
-      c.in(0).poke("h516461b1".U(32.W))
-      c.in(1).poke("h2a5f714c".U(32.W))
-      c.in(2).poke("h53372767".U(32.W))
-      c.in(3).poke("h3d631689".U(32.W))
-
-      c.out(0).expect("hbdb886dc".U(32.W))
-      c.out(1).expect("hcfacafd2".U(32.W))
-      c.out(2).expect("he46bea80".U(32.W))
-      c.out(3).expect("hccc07c79".U(32.W))
-    }
-  }
-}
diff --git a/src/test/scala/uk/ac/soton/ecs/can/core/XorerTest.scala b/src/test/scala/uk/ac/soton/ecs/can/core/XorerTest.scala
deleted file mode 100644
index e0e36e6c019b49fa2b574b9df3d60f14516b8e6e..0000000000000000000000000000000000000000
--- a/src/test/scala/uk/ac/soton/ecs/can/core/XorerTest.scala
+++ /dev/null
@@ -1,26 +0,0 @@
-// SPDX-FileCopyrightText: 2021 Minyong Li <ml10g20@soton.ac.uk>
-// SPDX-License-Identifier: GPL-3.0-or-later
-
-package uk.ac.soton.ecs.can.core
-
-import org.scalatest._
-import chiseltest._
-import chisel3._
-import scala.util.Random
-import scala.math.abs
-
-class XorerTest extends FlatSpec with ChiselScalatestTester {
-  behavior of "The Xorer"
-
-  it should "exclusive-or the 16 32b unsigned integers" in {
-    test(new Xorer) { c =>
-      val randomLhs = c.lhs.map(_ => abs(Random.nextInt))
-      val randomRhs = c.rhs.map(_ => abs(Random.nextInt))
-      val randomRes = randomLhs.zip(randomRhs).map { case (l, r) => l ^ r }
-
-      c.lhs.zip(randomLhs).foreach { case (p, r) => p.poke(r.U) }
-      c.rhs.zip(randomRhs).foreach { case (p, r) => p.poke(r.U) }
-      c.out.zip(randomRes).foreach { case (p, r) => p.expect(r.U) }
-    }
-  }
-}