diff --git a/.gitignore b/.gitignore
index bdf1e1b..a63d836 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,8 @@
 .bender
 scripts/compile.tcl
-models/s27ks0641
\ No newline at end of file
+axi_log/
+work/
+transcript
+modelsim.ini
+vsim.wlf
+models/s27ks0641
diff --git a/Bender.yml b/Bender.yml
index 905e54d..e5a6717 100644
--- a/Bender.yml
+++ b/Bender.yml
@@ -47,5 +47,6 @@ sources:
         - test/fixture_hyperbus.sv
         - test/hyperbus_tb.sv
         - test/dut_if.sv
+        - test/hyperbus_tb_pkg.sv
         - test/axi_hyper_tb.sv
     - src/hyperbus.sv
diff --git a/src/hyperbus.sv b/src/hyperbus.sv
index cf7212d..7b4a005 100644
--- a/src/hyperbus.sv
+++ b/src/hyperbus.sv
@@ -28,13 +28,12 @@ module hyperbus #(
     parameter type          reg_rsp_t       = logic,
     parameter type          axi_rule_t      = logic,
     // The below have sensible defaults, but should be set on integration!
-    parameter int unsigned  RxFifoLogDepth  = 2,
-    parameter int unsigned  TxFifoLogDepth  = 2,
+    parameter int unsigned  RxFifoLogDepth  = 3,
+    parameter int unsigned  TxFifoLogDepth  = 3,
     parameter logic [RegDataWidth-1:0]  RstChipBase  = 'h0,      // Base address for all chips
     parameter logic [RegDataWidth-1:0]  RstChipSpace = 'h1_0000, // 64 KiB: Current maximum HyperBus device size
     parameter hyperbus_pkg::hyper_cfg_t RstCfg       = hyperbus_pkg::gen_RstCfg(NumPhys,MinFreqMHz),
     parameter int unsigned  PhyStartupCycles = 300 * 200, /* us*MHz */ // Conservative maximum frequency estimate
-    parameter int unsigned  AxiLogDepth = 3,
     parameter int unsigned  SyncStages  = 2
 ) (
     input  logic                        clk_phy_i,
diff --git a/src/hyperbus_phy_if.sv b/src/hyperbus_phy_if.sv
index 3203b63..28bff22 100644
--- a/src/hyperbus_phy_if.sv
+++ b/src/hyperbus_phy_if.sv
@@ -8,10 +8,8 @@ module hyperbus_phy_if import hyperbus_pkg::*; #(
     parameter int unsigned IsClockODelayed = 1,
     parameter int unsigned NumChips = 2,
     parameter int unsigned NumPhys = 2,
-    parameter int unsigned TimerWidth = 16,
-    parameter int unsigned RxFifoLogDepth = 3,
     parameter int unsigned StartupCycles = 60000, /*MHz*/ // Conservative maximum frequency estimate
-    parameter int unsigned  SyncStages  = 2,
+    parameter int unsigned SyncStages  = 2,
     parameter type hyper_tx_t = logic,
     parameter type hyper_rx_t = logic
 )(
@@ -61,11 +59,11 @@ module hyperbus_phy_if import hyperbus_pkg::*; #(
 
       logic [NumPhys-1:0][1:0]     fifo_axi_usage;
 
-      logic                        tx_both_ready, ts_both_ready;
-      logic                        rx_both_valid, b_both_valid;
+    logic                        tx_both_ready, ts_both_ready;
+    logic                        rx_both_valid, b_both_valid;
 
-      logic [NumPhys-1:0]          phy_tx_ready;
-      logic                        phy_tx_valid;
+    logic [NumPhys-1:0]          phy_tx_ready;
+    logic                        phy_tx_valid;
 
       logic [NumPhys-1:0]          phy_trans_ready;
       logic [NumPhys-1:0]          phy_trans_valid;
@@ -77,7 +75,7 @@ module hyperbus_phy_if import hyperbus_pkg::*; #(
       genvar                          i;
       generate
 
-         if (NumPhys==2) begin : phy_wrap
+    if (NumPhys==2) begin : phy_wrap
 
             logic [NumPhys-1:0] phy_enable;
             logic [NumPhys-1:0] phy_busy;
@@ -156,41 +154,41 @@ module hyperbus_phy_if import hyperbus_pkg::*; #(
 
                    .busy_o         ( phy_busy[i]       ),
 
-                   .rx_data_o      ( phy_fifo_rx[i].data  ),
-                   .rx_last_o      ( phy_fifo_rx[i].last  ),
-                   .rx_error_o     ( phy_fifo_rx[i].error ),
-                   .rx_valid_o     ( phy_fifo_valid[i]    ),
-                   .rx_ready_i     ( phy_fifo_ready[i]    ),
+                .rx_data_o      ( phy_fifo_rx[i].data  ),
+                .rx_last_o      ( phy_fifo_rx[i].last  ),
+                .rx_error_o     ( phy_fifo_rx[i].error ),
+                .rx_valid_o     ( phy_fifo_valid[i]    ),
+                .rx_ready_i     ( phy_fifo_ready[i]    ),
 
-                   .tx_data_i      ( tx_i.data[16*i +:16] ),
-                   .tx_strb_i      ( tx_i.strb[2*i   +:2] ),
-                   .tx_last_i      ( tx_i.last            ),
-                   .tx_valid_i     ( phy_tx_valid         ),
-                   .tx_ready_o     ( phy_tx_ready[i]      ),
+                .tx_data_i      ( tx_i.data[16*i +:16] ),
+                .tx_strb_i      ( tx_i.strb[2*i   +:2] ),
+                .tx_last_i      ( tx_i.last            ),
+                .tx_valid_i     ( phy_tx_valid         ),
+                .tx_ready_o     ( phy_tx_ready[i]      ),
 
-                   .b_error_o      ( phy_b_error[i]       ),
-                   .b_valid_o      ( phy_b_valid[i]       ),
-                   .b_ready_i      ( phy_b_ready          ),
+                .b_error_o      ( phy_b_error[i]       ),
+                .b_valid_o      ( phy_b_valid[i]       ),
+                .b_ready_i      ( phy_b_ready          ),
 
                    .trans_i        ( trans_i              ),
                    .trans_cs_i     ( trans_cs_i           ),
                    .trans_valid_i  ( phy_trans_valid[i]   ),
                    .trans_ready_o  ( phy_trans_ready[i]   ),
 
-                   .hyper_cs_no    ( hyper_cs_no[i]       ),
-                   .hyper_ck_o     ( hyper_ck_o[i]        ),
-                   .hyper_ck_no    ( hyper_ck_no[i]       ),
-                   .hyper_rwds_o   ( hyper_rwds_o[i]      ),
-                   .hyper_rwds_i   ( hyper_rwds_i[i]      ),
-                   .hyper_rwds_oe_o( hyper_rwds_oe_o[i]   ),
-                   .hyper_dq_i     ( hyper_dq_i[i]        ),
-                   .hyper_dq_o     ( hyper_dq_o[i]        ),
-                   .hyper_dq_oe_o  ( hyper_dq_oe_o[i]     ),
-                   .hyper_reset_no ( hyper_reset_no[i]    )
-               );
+                .hyper_cs_no    ( hyper_cs_no[i]       ),
+                .hyper_ck_o     ( hyper_ck_o[i]        ),
+                .hyper_ck_no    ( hyper_ck_no[i]       ),
+                .hyper_rwds_o   ( hyper_rwds_o[i]      ),
+                .hyper_rwds_i   ( hyper_rwds_i[i]      ),
+                .hyper_rwds_oe_o( hyper_rwds_oe_o[i]   ),
+                .hyper_dq_i     ( hyper_dq_i[i]        ),
+                .hyper_dq_o     ( hyper_dq_o[i]        ),
+                .hyper_dq_oe_o  ( hyper_dq_oe_o[i]     ),
+                .hyper_reset_no ( hyper_reset_no[i]    )
+            );
 
-            end // for ( i=0; i<NumPhys;i++)
-         end else begin // if (NumPhys==2)
+        end // for ( i=0; i<NumPhys;i++)
+    end else begin // if (NumPhys==2)
 
             hyperbus_phy #(
                  .IsClockODelayed( IsClockODelayed   ),
@@ -208,26 +206,26 @@ module hyperbus_phy_if import hyperbus_pkg::*; #(
 
                  .busy_o         (                 ),
 
-                 .rx_data_o      ( rx_o.data       ),
-                 .rx_last_o      ( rx_o.last       ),
-                 .rx_error_o     ( rx_o.error      ),
-                 .rx_valid_o     ( rx_valid_o      ),
-                 .rx_ready_i     ( rx_ready_i      ),
-
-                 .tx_data_i      ( tx_i.data       ),
-                 .tx_strb_i      ( tx_i.strb       ),
-                 .tx_last_i      ( tx_i.last       ),
-                 .tx_valid_i     ( tx_valid_i      ),
-                 .tx_ready_o     ( tx_ready_o      ),
-
-                 .b_error_o      ( b_error_o       ),
-                 .b_valid_o      ( b_valid_o       ),
-                 .b_ready_i      ( b_ready_i       ),
-
-                 .trans_i        ( trans_i         ),
-                 .trans_cs_i     ( trans_cs_i      ),
-                 .trans_valid_i  ( trans_valid_i   ),
-                 .trans_ready_o  ( trans_ready_o   ),
+            .rx_data_o      ( rx_o.data       ),
+            .rx_last_o      ( rx_o.last       ),
+            .rx_error_o     ( rx_o.error      ),
+            .rx_valid_o     ( rx_valid_o      ),
+            .rx_ready_i     ( rx_ready_i      ),
+
+            .tx_data_i      ( tx_i.data       ),
+            .tx_strb_i      ( tx_i.strb       ),
+            .tx_last_i      ( tx_i.last       ),
+            .tx_valid_i     ( tx_valid_i      ),
+            .tx_ready_o     ( tx_ready_o      ),
+
+            .b_error_o      ( b_error_o       ),
+            .b_valid_o      ( b_valid_o       ),
+            .b_ready_i      ( b_ready_i       ),
+
+            .trans_i        ( trans_i         ),
+            .trans_cs_i     ( trans_cs_i      ),
+            .trans_valid_i  ( trans_valid_i   ),
+            .trans_ready_o  ( trans_ready_o   ),
 
                  .hyper_cs_no    ( hyper_cs_no     ),
                  .hyper_ck_o     ( hyper_ck_o      ),
diff --git a/test/axi_hyper_tb.sv b/test/axi_hyper_tb.sv
index d3d4bb3..aef556f 100644
--- a/test/axi_hyper_tb.sv
+++ b/test/axi_hyper_tb.sv
@@ -28,12 +28,14 @@ module axi_hyper_tb
   /// Test time of the DUT
   parameter time         TbTestTime =  4ns
 );
+  import hyperbus_tb_pkg::*;
   /////////////////////////////
   // Axi channel definitions //
   /////////////////////////////
   `include "axi/typedef.svh"
   `include "axi/assign.svh"
 
+
   /////////////////////////
   // Clock and Reset gen //
   /////////////////////////
@@ -59,9 +61,11 @@ module axi_hyper_tb
 
   logic                  end_of_sim;
 
-  ////////////////////////////////
-  // Stimuli generator typedefs //
-  ////////////////////////////////
+
+  ///////////////////////
+  // AXI Random Master //
+  ///////////////////////
+  // AXI master for random data transactions
   typedef axi_test::axi_rand_master #(
     .AW                   ( TbAxiAddrWidthFull ),
     .DW                   ( TbAxiDataWidthFull ),
@@ -92,20 +96,12 @@ module axi_hyper_tb
     .TT( TbTestTime         )
   ) axi_scoreboard_mst_t;
 
-  typedef reg_test::reg_driver #(
-    .AW ( RegBusAW   ),
-    .DW ( RegBusDW   ),
-    .TT ( TbTestTime )
-  ) reg_bus_master_t;   
-
-  logic s_reg_error;
-   
   AXI_BUS_DV #(
     .AXI_ADDR_WIDTH ( TbAxiAddrWidthFull ),
     .AXI_DATA_WIDTH ( TbAxiDataWidthFull ),
     .AXI_ID_WIDTH   ( TbAxiIdWidthFull   ),
     .AXI_USER_WIDTH ( TbAxiUserWidthFull )
-  ) axi_mst_intf_dv (
+  ) axi_rand_intf_dv (
     .clk_i ( clk )
   );
 
@@ -118,12 +114,125 @@ module axi_hyper_tb
     .clk_i ( clk )
   );
 
-  `AXI_ASSIGN_MONITOR(score_mst_intf_dv, axi_mst_intf_dv)
+  AXI_BUS #(
+    .AXI_ADDR_WIDTH ( TbAxiAddrWidthFull ),
+    .AXI_DATA_WIDTH ( TbAxiDataWidthFull ),
+    .AXI_ID_WIDTH   ( TbAxiIdWidthFull   ),
+    .AXI_USER_WIDTH ( TbAxiUserWidthFull )
+  ) axi_rand_intf ();
+
+  `AXI_ASSIGN_MONITOR(score_mst_intf_dv, axi_rand_intf_dv)
+  `AXI_ASSIGN(axi_rand_intf, axi_rand_intf_dv)
+
+
+
+  ////////////////////////
+  // AXI Control Master //
+  ////////////////////////
+
+  AXI_BUS_DV #(
+    .AXI_ADDR_WIDTH ( TbAxiAddrWidthFull ),
+    .AXI_DATA_WIDTH ( TbAxiDataWidthFull ),
+    .AXI_ID_WIDTH   ( TbAxiIdWidthFull   ),
+    .AXI_USER_WIDTH ( TbAxiUserWidthFull )
+  ) axi_ctrl_intf_dv (
+    .clk_i ( clk )
+  );
+
+  AXI_BUS #(
+    .AXI_ADDR_WIDTH ( TbAxiAddrWidthFull ),
+    .AXI_DATA_WIDTH ( TbAxiDataWidthFull ),
+    .AXI_ID_WIDTH   ( TbAxiIdWidthFull   ),
+    .AXI_USER_WIDTH ( TbAxiUserWidthFull )
+  ) axi_ctrl_intf ();
+
+  typedef axi_test::axi_driver #(
+    .AW ( TbAxiAddrWidthFull ),
+    .DW ( TbAxiDataWidthFull ),
+    .IW ( TbAxiIdWidthFull   ),
+    .UW ( TbAxiUserWidthFull ),
+    .TA ( TbApplTime         ),
+    .TT ( TbTestTime         )
+  ) axi_ctrl_master_t;
+  axi_ctrl_master_t axi_ctrl_mst = new( axi_ctrl_intf_dv );
+
+  `AXI_ASSIGN(axi_ctrl_intf, axi_ctrl_intf_dv)
+
+  logic s_axi_error;
+
+
+  //////////////////////////////
+  // AXI Control Master Tasks //
+  //////////////////////////////
+  task automatic axi_write_32(
+    input axi_addr_t  addr,
+    input bit [31:0] data
+  );
+    axi_ctrl_master_t::ax_beat_t ax = new();
+    axi_ctrl_master_t::w_beat_t w = new();
+    axi_ctrl_master_t::b_beat_t b;
+    
+    @(posedge clk);
+    ax.ax_addr  = addr;
+    ax.ax_id    = 0;
+    ax.ax_len   = 0;
+    ax.ax_size  = 2;
+    ax.ax_burst = axi_pkg::BURST_INCR;
+    axi_ctrl_mst.send_aw(ax);
+    w.w_strb = 'h0F;
+    w.w_data = data;
+    w.w_last = 1;
+    axi_ctrl_mst.send_w(w);
+    axi_ctrl_mst.recv_b(b);
+    if (b.b_resp != axi_pkg::RESP_OKAY)
+      $error("[AXI-CTRL] - Write error response: %d!", b.b_resp);
+  endtask
+
+
+  ///////////////////
+  // Regbus Master //
+  ///////////////////
+  typedef reg_test::reg_driver #(
+    .AW ( RegBusAW   ),
+    .DW ( RegBusDW   ),
+    .TT ( TbTestTime )
+  ) reg_bus_master_t;   
+
+  logic s_reg_error;
 
   REG_BUS #(
     .ADDR_WIDTH(RegBusAW),
     .DATA_WIDTH(RegBusDW)
   )  reg_bus_mst (.clk_i (clk));
+
+
+
+  ////////////////////
+  // AXI Master MUX //
+  ////////////////////
+
+  AXI_BUS #(
+    .AXI_ADDR_WIDTH ( TbAxiAddrWidthFull  ),
+    .AXI_DATA_WIDTH ( TbAxiDataWidthFull  ),
+    .AXI_ID_WIDTH   ( TbAxiIdWidthFull +1 ),
+    .AXI_USER_WIDTH ( TbAxiUserWidthFull  )
+  ) axi_dut_intf ();
+
+  axi_mux_intf #(
+    .SLV_AXI_ID_WIDTH ( TbAxiIdWidthFull    ),
+    .MST_AXI_ID_WIDTH ( TbAxiIdWidthFull +1 ),
+    .AXI_ADDR_WIDTH   ( TbAxiAddrWidthFull  ),
+    .AXI_DATA_WIDTH   ( TbAxiDataWidthFull  ),
+    .AXI_USER_WIDTH   ( TbAxiUserWidthFull  ),
+    .NO_SLV_PORTS     ( 2 )
+  ) i_axi_mst_mux (
+      .clk_i  ( clk   ),
+      .rst_ni ( rst_n ),
+      .test_i ( 1'b0  ),
+      .slv    ( { axi_ctrl_intf, axi_rand_intf } ),
+      .mst    ( axi_dut_intf )
+  );
+
    
   ////////////////////
   // Address Ranges //
@@ -136,18 +245,21 @@ module axi_hyper_tb
      
   initial begin : proc_sim_crtl
 
-    automatic axi_scoreboard_mst_t   mst_scoreboard  = new( score_mst_intf_dv );
-    automatic axi_rand_master_t      axi_master      = new( axi_mst_intf_dv   );
-    automatic reg_bus_master_t       reg_master      = new( reg_bus_mst       );
+    automatic axi_scoreboard_mst_t mst_scoreboard = new( score_mst_intf_dv );
+    automatic axi_rand_master_t    axi_rand_mst   = new( axi_rand_intf_dv  );
+    automatic reg_bus_master_t     reg_master     = new( reg_bus_mst       );
+    
+    automatic s27ks_cfg0_reg_t s27ks_cfg0 = hyperbus_tb_pkg::s27ks_cfg0_default;
 
     // Reset the AXI drivers and scoreboards
     end_of_sim = 1'b0;
     mst_scoreboard.reset();
-    axi_master.reset();
+    axi_rand_mst.reset();
+    axi_ctrl_mst.reset_master();
     reg_master.reset_master();
 
     // Set some mem regions for rand axi master
-    axi_master.add_memory_region(32'h8000_0000, 32'h8000_0000 + ( TbDramDataWidth * TbDramLenWidth ), axi_pkg::NORMAL_NONCACHEABLE_BUFFERABLE);
+    axi_rand_mst.add_memory_region(32'h8000_0000, 32'h8000_0000 + ( TbDramDataWidth * TbDramLenWidth ), axi_pkg::NORMAL_NONCACHEABLE_BUFFERABLE);
      
     mst_scoreboard.enable_all_checks();
 
@@ -156,11 +268,24 @@ module axi_hyper_tb
 
     #600350ns;
 
+    // switch memory address space to register space
+    reg_master.send_write(32'h7<<2, 1'b1, '1, s_reg_error);
+    if (s_reg_error != 1'b0) $error("unexpected error");
+
+    // enable variable latency so we can test RWDS sampling
+    s27ks_cfg0.fixed_latency_enable = 1'b0;
+    $display("t3est");
+    axi_write_32(32'h8000_0000 + S27KS_CFG0_REG_OFFSET, (s27ks_cfg0 | s27ks_cfg0 << 16));
+
+    // switch back to memory address space
+    reg_master.send_write(32'h7<<2, 1'b0, '1, s_reg_error);
+    if (s_axi_error != 1'b0) $error("unexpected error");
+
     $display("===========================");
     $display("= Random AXI transactions =");
     $display("===========================");
      
-    axi_master.run(TbNumReads, TbNumWrites);
+    axi_rand_mst.run(TbNumReads, TbNumWrites);
 
     $display("===========================");
     $display("=      Test finished      =");
@@ -180,13 +305,13 @@ module axi_hyper_tb
        reg_master.send_write(32'h24,1'b0,'1,s_reg_error);
        if (s_reg_error != 1'b0) $error("unexpected error");
 
-       axi_master.reset();
+       axi_rand_mst.reset();
 
        $display("===========================");
        $display("= Random AXI transactions =");
        $display("===========================");
 
-       axi_master.run(TbNumReads, TbNumWrites);
+       axi_rand_mst.run(TbNumReads, TbNumWrites);
 
        $display("===========================");
        $display("=      Test finished      =");
@@ -201,13 +326,13 @@ module axi_hyper_tb
        reg_master.send_write(32'h24,1'b1,'1,s_reg_error);
        if (s_reg_error != 1'b0) $error("unexpected error");
 
-       axi_master.reset();
+       axi_rand_mst.reset();
 
        $display("===========================");
        $display("= Random AXI transactions =");
        $display("===========================");
 
-       axi_master.run(TbNumReads, TbNumWrites);
+       axi_rand_mst.run(TbNumReads, TbNumWrites);
 
        $display("===========================");
        $display("=      Test finished      =");
@@ -226,7 +351,7 @@ module axi_hyper_tb
     .TbTestTime      ( TbTestTime         ),
     .AxiDataWidth    ( TbAxiDataWidthFull ),
     .AxiAddrWidth    ( TbAxiAddrWidthFull ),
-    .AxiIdWidth      ( TbAxiIdWidthFull   ),
+    .AxiIdWidth      ( TbAxiIdWidthFull+1 ),
     .AxiUserWidth    ( TbAxiUserWidthFull ),
 
     .RegAw           ( RegBusAW           ),
@@ -238,11 +363,11 @@ module axi_hyper_tb
     .axi_rule_t      ( rule_t             )
   ) i_dut_if (
     // clk and rst signal
-    .clk_i      ( clk             ),
-    .rst_ni     ( rst_n           ),
-    .end_sim_i  ( end_of_sim      ),
-    .axi_slv_if ( axi_mst_intf_dv ),
-    .reg_slv_if ( reg_bus_mst     )
+    .clk_i      ( clk          ),
+    .rst_ni     ( rst_n        ),
+    .end_sim_i  ( end_of_sim   ),
+    .axi_slv_if ( axi_dut_intf ),
+    .reg_slv_if ( reg_bus_mst  )
   );
 
 endmodule
diff --git a/test/dut_if.sv b/test/dut_if.sv
index da49ec6..af257eb 100644
--- a/test/dut_if.sv
+++ b/test/dut_if.sv
@@ -31,8 +31,8 @@ module dut_if
  input logic rst_ni,
  input logic end_sim_i,
 
- AXI_BUS_DV.Slave axi_slv_if,
- REG_BUS.in       reg_slv_if
+ AXI_BUS.Slave axi_slv_if,
+ REG_BUS.in    reg_slv_if
 );
   localparam int unsigned DRAM_DB_WIDTH = 16;
    
diff --git a/test/hyperbus_tb_pkg.sv b/test/hyperbus_tb_pkg.sv
new file mode 100644
index 0000000..a1bc827
--- /dev/null
+++ b/test/hyperbus_tb_pkg.sv
@@ -0,0 +1,32 @@
+// Copyright 2024 ETH Zurich and University of Bologna.
+// Solderpad Hardware License, Version 0.51, see LICENSE for details.
+// SPDX-License-Identifier: SHL-0.51
+
+package hyperbus_tb_pkg;
+
+    parameter int unsigned S27KS_ID0_REG_OFFSET  = 32'h0000_0000;
+    parameter int unsigned S27KS_ID1_REG_OFFSET  = 32'h0000_0002;
+    parameter int unsigned S27KS_CFG0_REG_OFFSET = 32'h0000_2000;
+    parameter int unsigned S27KS_CFG1_REG_OFFSET = 32'h0000_2002;
+
+    typedef struct packed {
+        bit       deep_power_done;
+        bit [2:0] drive_strength;
+        bit [3:0] reserved;
+        bit [3:0] initial_latency;
+        bit       fixed_latency_enable;
+        bit       hybrid_burst_enable;
+        bit [1:0] burst_length;
+    } s27ks_cfg0_reg_t;
+
+    parameter s27ks_cfg0_reg_t s27ks_cfg0_default = s27ks_cfg0_reg_t'{  
+        deep_power_done:      1'h1,
+        drive_strength:       3'h0,
+        reserved:             4'hF,
+        initial_latency:      4'h1,
+        fixed_latency_enable: 1'b1,
+        hybrid_burst_enable:  1'b1,
+        burst_length:         2'h3 
+    };
+
+endpackage