diff --git a/library/i3c_controller/i3c_controller_core/i3c_controller_bit_mod.v b/library/i3c_controller/i3c_controller_core/i3c_controller_bit_mod.v
index 3e467615f2..0e89fb2293 100644
--- a/library/i3c_controller/i3c_controller_core/i3c_controller_bit_mod.v
+++ b/library/i3c_controller/i3c_controller_core/i3c_controller_bit_mod.v
@@ -34,7 +34,7 @@
 // ***************************************************************************
 /**
  * Translate simple commands into SCL/SDA transitions
- * Each command has 4 states and shall jump to a new command
+ * Each command has 8 states and shall jump to a new command
  * without returning to idle, A/B/C/D/*, where * is a next command
  * first state A or idle.
  *
@@ -75,13 +75,23 @@
 
 module i3c_controller_bit_mod (
   input wire reset_n,
-  input wire clk, // 200MHz
-  input wire clk_bus, // 50Mhz, 25MHz
+  input wire clk, // 100MHz
+  input wire clk_bus, // 100Mhz, 50MHz
 
   // Bit Modulation Command
-  // "cmd_valid" is embed in known enum values
+
   input wire [`MOD_BIT_CMD_WIDTH:0] cmd,
-  output wire cmd_ready, // indicate sample window
+  output wire cmd_ready,
+
+  // RX and ACK
+
+  output wire rx,
+  output wire rx_valid,
+  output wire rx_stop,
+  output wire rx_nack,
+
+
+  // Bus drive signals
 
   output reg  scl,
   output reg  sdi,
@@ -89,200 +99,147 @@ module i3c_controller_bit_mod (
   output wire t
 );
 
-  wire clk_scl;
-  wire [2:0] key;
+  reg reset_n_ctl;
+  reg reset_n_clr;
+
+  reg pp;
+  reg [2:0] i ;
+  reg [2:0] i_;
+  reg [7:0] scl_;
+  reg [7:0] sdi_;
+
+  reg [1:0] rx_valid_reg;
+  reg [1:0] rx_stop_reg;
+  reg [1:0] rx_nack_reg;
+  reg cmd_ready_ctrl;
+
+  reg  [1:0] st;
+  reg  [`MOD_BIT_CMD_WIDTH:2] sm;
 
   reg sdo_reg;
-  reg clk_scl_reg;
-  reg clk_scl_posedge;
-  reg reset_n_reg;
-  reg reset_n_clr;
-  reg pp_en;
-  reg [1:0] clk_counter;
+  reg scl_reg;
 
-  reg [4:0] sm;
-  localparam [4:0]
-    idle     = 0,
-    start_b  = 1,
-    start_c  = 2,
-    start_d  = 3,
-    stop_b   = 4,
-    stop_c   = 5,
-    stop_d   = 6,
-    rd_b     = 7,
-    rd_c     = 8,
-    rd_d     = 9,
-    wr_b     = 10,
-    wr_c     = 11,
-    wr_d     = 12,
-    t_rd_b   = 13,
-    t_rd_c   = 14,
-    t_rd_d   = 15;
+  always @(sm) begin
+    case (sm)
+      `MOD_BIT_CMD_NOP_ : i_ = 0;
+      default           : i_ = 7;
+    endcase
+
+    case (sm)
+      `MOD_BIT_CMD_STOP_     : scl_ = 8'b00011111;
+      default                : scl_ = 8'b00011100;
+    endcase
+
+    case (sm)
+      `MOD_BIT_CMD_START_    : sdi_ = 8'b11111000;
+      `MOD_BIT_CMD_STOP_     : sdi_ = 8'b00000111;
+      `MOD_BIT_CMD_ACK_IBI_  : sdi_ = 8'b00001111;
+      default                : sdi_ = 8'b00000000;
+    endcase
+  end
 
   always @(posedge clk) begin
     if (!reset_n) begin
-      reset_n_reg <= 1'b0;
-      clk_scl_posedge <= 1'b1;
-    end else begin
-      if (reset_n_clr) begin
-        reset_n_reg <= 1'b1;
-      end
-      clk_scl_posedge <= clk_scl;
+      reset_n_ctl <= 1'b0;
+    end else if (reset_n_clr) begin
+      reset_n_ctl <= 1'b1;
     end
+    rx_valid_reg [1] <= rx_valid_reg [0];
+    rx_stop_reg  [1] <= rx_stop_reg  [0];
+    rx_nack_reg  [1] <= rx_nack_reg  [0];
+    cmd_ready_ctrl <= i == i_ & cmd[`MOD_BIT_CMD_WIDTH:2] != `MOD_BIT_CMD_NOP_;
   end
 
   always @(posedge clk_bus) begin
-    if (!reset_n_reg) begin
+    rx_nack_reg[0]  <= 1'b0;
+    rx_valid_reg[0] <= 1'b0;
+    rx_stop_reg[0]  <= 1'b0;
+    if (!reset_n_ctl) begin
       reset_n_clr <= 1'b1;
+      sm  <= `MOD_BIT_CMD_NOP_;
       scl <= 1'b1;
       sdi <= 1'b1;
-      sm <= idle;
-      clk_counter <= 2'b00;
+      pp  <= 1'b0;
+      i   <= 0;
     end else begin
       reset_n_clr <= 1'b0;
-      clk_counter <= clk_counter + 1;
-      case (sm)
-        idle:
-        begin
-          if (~clk_scl_reg && clk_scl) begin
-            case (key)
-              `MOD_BIT_CMD_START_:
-              begin
-                sm <= start_b;
-                pp_en <= cmd[1];
-                scl <= scl;
-                sdi <= 1'b1;
-              end
-              `MOD_BIT_CMD_STOP_:
-              begin
-                sm <= stop_b;
-                pp_en <= 1'b0;
-                scl <= 1'b0;
-                sdi <= 1'b0;
-              end
-              `MOD_BIT_CMD_READ_:
-              begin
-                sm <= rd_b;
-                pp_en <= 1'b0;
-                scl <= 1'b0;
-                sdi <= 1'b1;
-              end
-              `MOD_BIT_CMD_WRITE_:
-              begin
-                sm <= wr_b;
-                pp_en <= cmd[1];
-                scl <= 1'b0;
-                sdi <= cmd[0];
-              end
-              `MOD_BIT_CMD_T_READ_:
-              begin
-                sm <= t_rd_b;
-                pp_en <= 1'b0;
-                scl <= 1'b0;
-                sdi <= 1'b1;
-              end
-              default: begin
-                sm <= idle;
-                pp_en <= 1'b0;
-                sdi <= sdi;
-                scl <= scl;
-              end
-            endcase
-          end
-        end
-        start_b, start_c,
-        stop_b, stop_c,
-        rd_b, rd_c,
-        wr_b, wr_c,
-        t_rd_b, t_rd_c:
-        begin
-          sm <= sm + 1;
-        end
-        start_d, stop_d,
-        rd_d, wr_d,
-        t_rd_d:
-        begin
-          sm <= idle;
-        end
-        default:
-        begin
-          sm <= idle;
-        end
-      endcase
 
+      if (i == i_) begin
+        sm <= cmd[`MOD_BIT_CMD_WIDTH:2];
+        st <= cmd[1:0];
+        i  <= 0;
+      end else begin
+        sm <= sm;
+        st <= st;
+        i  <= i + 1;
+      end
+
+      scl <= scl_[7-i];
+      sdi <= sdi_[7-i];
+      pp  <= st[1];
       case (sm)
-        idle: begin
-        end
-        start_b: begin
-          scl <= 1'b1;
+        `MOD_BIT_CMD_NOP_: begin
           sdi <= 1'b1;
-        end
-        start_c: begin
-          scl <= 1'b1;
-          sdi <= 1'b0;
-        end
-        start_d: begin
-          scl <= 1'b0;
-          sdi <= 1'b0;
-        end
-        stop_b: begin
-          scl <= 1'b1;
-          sdi <= 1'b0;
-        end
-        stop_c: begin
+          pp  <= 1'b0;
           scl <= 1'b1;
-          sdi <= 1'b0;
-        end
-        stop_d: begin
-          scl <= 1'b1;
-          sdi <= 1'b1;
-        end
-        rd_b: begin
-          scl <= 1'b1;
-          sdi <= 1'b1;
         end
-        rd_c: begin
-          scl <= 1'b1;
-          sdi <= 1'b1;
+        `MOD_BIT_CMD_WRITE_: begin
+          sdi <= st[0];
+          if (i == 5) begin
+            rx_valid_reg[0] <= 1'b1;
+          end
         end
-        rd_d: begin
-          scl <= 1'b0;
+        `MOD_BIT_CMD_READ_: begin
           sdi <= 1'b1;
+          pp  <= 1'b0;
         end
-        wr_b: begin
-          scl <= 1'b1;
-          sdi <= sdi;
-        end
-        wr_c: begin
-          scl <= 1'b1;
-          sdi <= sdi;
+        `MOD_BIT_CMD_START_: begin
+          // For Sr
+          if (i <= 2) begin
+            scl <= scl_reg;
+          end
         end
-        wr_d: begin
-          scl <= 1'b0;
-          sdi <= sdi;
+        `MOD_BIT_CMD_STOP_: begin
+        end
+        `MOD_BIT_CMD_ACK_SDR_: begin
+          if (i == 0 || i == 1) begin
+            sdi <= 1'b1;
+            pp  <= 1'b0;
+          end else if (i == 2) begin
+            sdi <=  sdo_reg;
+            rx_nack_reg[0] <= sdo_reg;
+          end else begin
+            sdi <= sdi;
+          end
         end
-        t_rd_b: begin
-          scl <= 1'b1;
-          sdi <= sdo_reg;
+        `MOD_BIT_CMD_T_READ_: begin
+          if (i == 0 || i == 1) begin
+            sdi <= 1'b1;
+            pp  <= 1'b0;
+          end else if (i == 2) begin
+            sdi <= st[0] ? 1'b0 : sdo_reg;
+            rx_stop_reg[0] <= ~sdo_reg;
+          end else begin
+            sdi <= sdi;
+          end
         end
-        t_rd_c: begin
-          scl <= 1'b1;
-          sdi <= sdi;
+        `MOD_BIT_CMD_ACK_IBI_: begin
+          pp  <= 1'b0;
         end
-        t_rd_d: begin
-          scl <= 1'b0;
-          sdi <= sdi;
+        default: begin
+          sm <= `MOD_BIT_CMD_NOP_;
         end
-        default:
-          sm <= idle;
       endcase
     end
-    clk_scl_reg <= clk_scl;
-    sdo_reg <= sdo;
+    sdo_reg <= sdo === 1'b0 ? 1'b0 : 1'b1;
+    scl_reg <= scl;
   end
 
-  assign key = cmd[`MOD_BIT_CMD_WIDTH:2];
-  assign t = ~pp_en & sdi ? 1'b1 : 1'b0;
-  assign clk_scl = clk_counter[1];
-  assign cmd_ready = clk_scl & !clk_scl_posedge;
+  assign rx_valid = rx_valid_reg[0] & ~rx_valid_reg[1];
+  assign rx_stop  = rx_stop_reg [0] & ~rx_stop_reg [1];
+  assign rx_nack  = rx_nack_reg [0] & ~rx_nack_reg [1];
+  assign rx = sdo_reg;
+
+  assign cmd_ready = i == i_ & ~cmd_ready_ctrl & reset_n;
+  assign t = ~pp & sdi ? 1'b1 : 1'b0;
 endmodule
diff --git a/library/i3c_controller/i3c_controller_core/i3c_controller_bit_mod_cmd.v b/library/i3c_controller/i3c_controller_core/i3c_controller_bit_mod_cmd.v
index 83d2609e54..31e11687ed 100644
--- a/library/i3c_controller/i3c_controller_core/i3c_controller_bit_mod_cmd.v
+++ b/library/i3c_controller/i3c_controller_core/i3c_controller_bit_mod_cmd.v
@@ -5,28 +5,33 @@
 
 // Modulation bit commands
 
-`define MOD_BIT_CMD_NOP_    3'b000
-`define MOD_BIT_CMD_START_  3'b001
-`define MOD_BIT_CMD_STOP_   3'b010
-`define MOD_BIT_CMD_WRITE_  3'b011
-`define MOD_BIT_CMD_READ_   3'b100
-`define MOD_BIT_CMD_T_READ_ 3'b101
+`define MOD_BIT_CMD_NOP_     3'b000
+`define MOD_BIT_CMD_START_   3'b001
+`define MOD_BIT_CMD_STOP_    3'b010
+`define MOD_BIT_CMD_WRITE_   3'b011
+`define MOD_BIT_CMD_READ_    3'b100
+`define MOD_BIT_CMD_T_READ_  3'b101
+`define MOD_BIT_CMD_ACK_SDR_ 3'b110
+`define MOD_BIT_CMD_ACK_IBI_ 3'b111
 
 // Read and ACK are modulated identically
 // The mod_byte shall send a CMD_READ to assert ACK
-// Reserved: 11000 11100
 // T-bit LOW is a low write
 // T-bit HIGH is either a open-drain high write or repeated start
 
-`define MOD_BIT_CMD_NOP        {`MOD_BIT_CMD_NOP_,   2'b00}
-`define MOD_BIT_CMD_START_OD   {`MOD_BIT_CMD_START_, 2'b00}
-`define MOD_BIT_CMD_START_PP   {`MOD_BIT_CMD_START_, 2'b10}
-`define MOD_BIT_CMD_STOP       {`MOD_BIT_CMD_STOP_,  2'b00}
-`define MOD_BIT_CMD_WRITE_OD_0 {`MOD_BIT_CMD_WRITE_, 2'b00}
-`define MOD_BIT_CMD_WRITE_OD_1 {`MOD_BIT_CMD_WRITE_, 2'b01}
-`define MOD_BIT_CMD_WRITE_PP_0 {`MOD_BIT_CMD_WRITE_, 2'b10}
-`define MOD_BIT_CMD_WRITE_PP_1 {`MOD_BIT_CMD_WRITE_, 2'b11}
-`define MOD_BIT_CMD_READ       {`MOD_BIT_CMD_READ_,  2'b00}
-`define MOD_BIT_CMD_T_READ     {`MOD_BIT_CMD_T_READ_,2'b00}
+`define MOD_BIT_CMD_NOP         {`MOD_BIT_CMD_NOP_,    2'b00}
+`define MOD_BIT_CMD_START_OD    {`MOD_BIT_CMD_START_,  2'b00}
+`define MOD_BIT_CMD_START_PP    {`MOD_BIT_CMD_START_,  2'b10}
+`define MOD_BIT_CMD_STOP_OD     {`MOD_BIT_CMD_STOP_,   2'b00}
+`define MOD_BIT_CMD_STOP_PP     {`MOD_BIT_CMD_STOP_,   2'b10}
+`define MOD_BIT_CMD_WRITE_OD_0  {`MOD_BIT_CMD_WRITE_,  2'b00}
+`define MOD_BIT_CMD_WRITE_OD_1  {`MOD_BIT_CMD_WRITE_,  2'b01}
+`define MOD_BIT_CMD_WRITE_PP_0  {`MOD_BIT_CMD_WRITE_,  2'b10}
+`define MOD_BIT_CMD_WRITE_PP_1  {`MOD_BIT_CMD_WRITE_,  2'b11}
+`define MOD_BIT_CMD_READ        {`MOD_BIT_CMD_READ_,   2'b00}
+`define MOD_BIT_CMD_T_READ_CONT {`MOD_BIT_CMD_T_READ_ ,2'b00}
+`define MOD_BIT_CMD_T_READ_STOP {`MOD_BIT_CMD_T_READ_ ,2'b01}
+`define MOD_BIT_CMD_ACK_SDR     {`MOD_BIT_CMD_ACK_SDR_,2'b00}
+`define MOD_BIT_CMD_ACK_IBI     {`MOD_BIT_CMD_ACK_IBI_,2'b00}
 
 `endif
diff --git a/library/i3c_controller/i3c_controller_core/i3c_controller_clk_div.v b/library/i3c_controller/i3c_controller_core/i3c_controller_clk_div.v
index 09c2a5c542..146508eb32 100644
--- a/library/i3c_controller/i3c_controller_core/i3c_controller_clk_div.v
+++ b/library/i3c_controller/i3c_controller_core/i3c_controller_clk_div.v
@@ -34,7 +34,7 @@
 // ***************************************************************************
 /**
  * Async I3C: Divides the input clock by 1 (sel=1) or 2 (sel=0).
- * Sync I3C: Divides the input clock by 16 (sel=1) or 8 (sel=0).
+ * Sync I3C: Divides the input clock by 8 (sel=1) or 4 (sel=0).
  * Multipath constraints should be tuned.
  */
 
@@ -46,46 +46,43 @@ module i3c_controller_clk_div #(
 ) (
   input  wire reset_n,
   input  wire sel,
+  input  wire clr,
   input  wire cmd_ready,
   input  wire clk,
   input  wire clk_bus,
-  output wire  clk_out
+  output wire clk_out
   );
 
-  reg sel_reg;
+  reg [1:0] sel_reg;
   always @(posedge clk) begin
-    if (!reset_n) begin
-      sel_reg <= 1'b0;
-    end else begin
-      if (cmd_ready) begin
-        sel_reg <= sel;
-      end
+    if (clr) begin
+      sel_reg <= 2'b00;
+    end else if (cmd_ready) begin
+      sel_reg <= {sel_reg[0], sel};
     end
   end
 
   generate if (ASYNC_CLK) begin
-    reg clk_reg;
-    always @(posedge clk_bus) begin
-      if (!reset_n) begin
-        clk_reg <= 1'b0;
-      end else begin
-        clk_reg <= ~clk_reg;
-      end
-    end
-    assign clk_out = sel_reg ? clk_bus : clk_reg;
+    BUFGMUX #(
+    ) i_BUFGMUX (
+       .O(clk_out),
+       .I0(clk_bus),
+       .I1(clk),
+       .S(sel_reg)
+    );
   end
   endgenerate
 
   generate if (!ASYNC_CLK) begin
-    reg [3:0] clk_reg;
-    always @(posedge clk) begin
+    reg [2:0] clk_reg;
+      always @(posedge clk) begin
       if (!reset_n) begin
         clk_reg <= 5'd0;
       end else begin
         clk_reg <= clk_reg + 1;
       end
     end
-    assign clk_out = sel_reg ? clk_reg[2] : clk_reg[3];
+    assign clk_out = sel_reg[1] ? clk_reg[1] : clk_reg[2];
   end
   endgenerate
 
diff --git a/library/i3c_controller/i3c_controller_core/i3c_controller_core.v b/library/i3c_controller/i3c_controller_core/i3c_controller_core.v
index 158bb77ba5..17ce9c4032 100644
--- a/library/i3c_controller/i3c_controller_core/i3c_controller_core.v
+++ b/library/i3c_controller/i3c_controller_core/i3c_controller_core.v
@@ -83,6 +83,11 @@ module i3c_controller_core #(
   wire sdo_bit;
   wire sdi_bit;
 
+  wire rx;
+  wire rx_valid;
+  wire rx_stop;
+  wire rx_nack;
+
   //wire [63:0] o_pid_bcr_dcr;
   //wire [6:0] o_da;
   //wire o_pid_da_valid;
@@ -155,7 +160,10 @@ module i3c_controller_core #(
     .cmdw_rx(cmdw_rx),
     .cmd(cmd),
     .cmd_ready(cmd_ready),
-    .sdo(sdo_bit),
+    .rx(rx),
+    .rx_valid(rx_valid),
+    .rx_stop(rx_stop),
+    .rx_nack(rx_nack),
     .clk_sel(clk_sel)
   );
 
@@ -177,6 +185,10 @@ module i3c_controller_core #(
     .clk_bus(clk_bus_2),
     .cmd(cmd),
     .cmd_ready(cmd_ready),
+    .rx(rx),
+    .rx_valid(rx_valid),
+    .rx_stop(rx_stop),
+    .rx_nack(rx_nack),
     .scl(scl),
     .sdi(sdi_bit),
     .sdo(sdo_bit),
diff --git a/library/i3c_controller/i3c_controller_core/i3c_controller_word.v b/library/i3c_controller/i3c_controller_core/i3c_controller_word.v
index d576a4976d..6c3f8eead1 100644
--- a/library/i3c_controller/i3c_controller_core/i3c_controller_word.v
+++ b/library/i3c_controller/i3c_controller_core/i3c_controller_word.v
@@ -42,8 +42,6 @@
 `include "i3c_controller_word_cmd.v"
 `include "i3c_controller_bit_mod_cmd.v"
 
-`define DO_ACK do_ack <= 1'b0; // TODO: Change to 1
-
 module i3c_controller_word #(
 ) (
   input  wire clk,
@@ -68,18 +66,23 @@ module i3c_controller_word #(
   output reg [`MOD_BIT_CMD_WIDTH:0] cmd,
   input  wire cmd_ready,
 
-  // I3C Bus SDA, used for ACK and T (read) check
+  // RX and ACK
 
-  input wire sdo,
+  input wire rx,
+  input wire rx_valid,
+  input wire rx_stop,
+  input wire rx_nack,
 
   // Modulation clock selection
 
-  output reg clk_sel
+  output reg clk_sel,
+  output reg clk_clr
 );
 
   wire [`CMDW_HEADER_WIDTH:0] cmdw_header;
   wire [`CMDW_HEADER_WIDTH+8:0] cmdw;
   reg cmd_ready_reg;
+  reg cmd_lock;
 
   reg [7:0] cmdw_body;
   reg [`CMDW_HEADER_WIDTH:0] sm;
@@ -91,7 +94,7 @@ module i3c_controller_word #(
 
   reg do_ack; // Peripheral did NACK?
   reg do_rx_t; // Peripheral end Message at T in Read Data?
-  reg sdo_reg;
+  reg rx_sampled;
 
   reg [5:0] i;
   reg [5:0] i_;
@@ -112,9 +115,7 @@ module i3c_controller_word #(
       `CMDW_DYN_ADDR        : i_ =  8; // DA+T+ACK
       default               : i_ =  0;
     endcase
-  end
 
-  always @(sm) begin
     case (sm)
       `CMDW_NOP             : clk_sel = 0;
       `CMDW_START           : clk_sel = 0;
@@ -129,118 +130,116 @@ module i3c_controller_word #(
       `CMDW_PROV_ID_BCR_DCR : clk_sel = 0;
       `CMDW_DYN_ADDR        : clk_sel = 0;
       default               : clk_sel = 0;
-    endcase
+      endcase
   end
 
   always @(posedge clk) begin
+    cmd_lock <= 1'b0;
+    do_ack <= 1'b0;
+    do_rx_t <= 1'b0;
+    cmdw_nack <= 1'b0;
+    cmdw_rx_valid_reg <= 1'b0;
+    clk_clr <= 1'b0;
     if (!reset_n) begin
-      do_ack <= 1'b0;
-      do_rx_t <= 1'b0;
       cmd <= `MOD_BIT_CMD_NOP;
-      cmdw_nack <= 1'b0;
-      cmdw_rx_valid_reg <= 1'b0;
       sm <= `CMDW_NOP;
       i <= 0;
-    end else if (cmd_ready_reg) begin
+    end if ((do_ack & rx_nack) | (do_rx_t & rx_stop)) begin
+      sm <= `CMDW_NOP;
+      cmd <= `MOD_BIT_CMD_STOP_OD;
+      cmdw_nack <= 1'b1;
+      clk_clr <= 1'b1;
+    end else if (cmd_ready & ~cmd_lock) begin
       sm <= i == i_ ? cmdw_header : sm;
       cmdw_body <= i == i_ ? cmdw[7:0] : cmdw_body;
       i <= sm == `CMDW_NOP ? 0 : (i == i_ ? 0 : i + 1);
-      do_ack <= 1'b0;
-      do_rx_t <= 1'b0;
-      cmdw_rx_valid_reg <= 1'b0;
-      if ((do_ack & !(sdo_reg === 1'b0)) | (do_rx_t & (sdo_reg === 1'b0))) begin
-        cmd <= `MOD_BIT_CMD_STOP;
-        sm <= `CMDW_NOP;
-        cmdw_nack <= 1'b1;
-      end else begin
-        cmdw_nack <= 1'b0;
-        case (sm)
-          `CMDW_NOP: begin
-            cmd <= `MOD_BIT_CMD_NOP;
+      case (sm)
+        `CMDW_NOP: begin
+          cmd <= `MOD_BIT_CMD_NOP;
+        end
+        `CMDW_START: begin
+          cmd <= `MOD_BIT_CMD_START_OD;
+          cmd_lock <= 1'b1;
+        end
+        `CMDW_BCAST_7E_W0: begin
+          if (i == 7) begin
+            // RnW=0
+            cmd <= {`MOD_BIT_CMD_WRITE_,1'b1,1'b0};
+          end else if (i == 8) begin
+            // ACK
+            cmd <= `MOD_BIT_CMD_ACK_SDR;
+            do_ack <= 1'b1;
+          end else begin
+            // 7'h7e
+            cmd <= {`MOD_BIT_CMD_WRITE_,1'b0,I3C_RESERVED[6 - i[2:0]]};
           end
-          `CMDW_START: begin
-            cmd <= `MOD_BIT_CMD_START_OD;
+        end
+        `CMDW_BCAST_7E_W1: begin
+          if (i == 7) begin
+            // RnW=1
+            cmd <= {`MOD_BIT_CMD_WRITE_,1'b1,1'b1};
+          end else if (i == 8) begin
+            // ACK
+            cmd <= `MOD_BIT_CMD_ACK_SDR;
+            do_ack <= 1'b1;
+          end else begin
+            // 7'h7e
+            cmd <= {`MOD_BIT_CMD_WRITE_,1'b1,I3C_RESERVED[6 - i[2:0]]};
           end
-          `CMDW_BCAST_7E_W0: begin
-            if (i == 7) begin
-              // RnW=0
-              cmd <= {`MOD_BIT_CMD_WRITE_,1'b1,1'b0};
-            end else if (i == 8) begin
-              // ACK
-              cmd <= {`MOD_BIT_CMD_READ};
-              `DO_ACK
-            end else begin
-              // 7'h7e
-              cmd <= {`MOD_BIT_CMD_WRITE_,1'b0,I3C_RESERVED[6 - i[2:0]]};
-            end
-          end
-          `CMDW_BCAST_7E_W1: begin
-            if (i == 7) begin
-              // RnW=1
-              cmd <= {`MOD_BIT_CMD_WRITE_,1'b1,1'b1};
-            end else if (i == 8) begin
-              // ACK
-              cmd <= {`MOD_BIT_CMD_READ};
-              `DO_ACK
-            end else begin
-              // 7'h7e
-              cmd <= {`MOD_BIT_CMD_WRITE_,1'b1,I3C_RESERVED[6 - i[2:0]]};
-            end
+        end
+        `CMDW_DYN_ADDR,
+        `CMDW_TARGET_ADDR: begin
+          if (i == 8) begin
+            // ACK
+            cmd <= `MOD_BIT_CMD_ACK_SDR;
+            do_ack <= 1'b1;
+          end else begin
+            // DA+RnW/DA+T
+            cmd <= {`MOD_BIT_CMD_WRITE_,1'b1,cmdw_body[7 - i[2:0]]};
           end
-          `CMDW_DYN_ADDR,
-          `CMDW_TARGET_ADDR: begin
-            if (i == 8) begin
-              // ACK
-              cmd <= {`MOD_BIT_CMD_READ};
-              `DO_ACK
+        end
+        `CMDW_MSG_SR: begin
+            cmd <= `MOD_BIT_CMD_START_PP;
+        end
+        `CMDW_MSG_RX: begin
+          if (i == 8) begin
+            cmdw_rx_valid_reg <= 1'b1;
+            // T
+            if (cmdw_rx_ready) begin
+              do_rx_t <= 1'b1;
+              cmd <= `MOD_BIT_CMD_T_READ_CONT; // continue, if peripheral wishes to do so
             end else begin
-              // DA+RnW/DA+T
-              cmd <= {`MOD_BIT_CMD_WRITE_,1'b1,cmdw_body[7 - i[2:0]]};
+              cmd <= `MOD_BIT_CMD_T_READ_STOP; // stop
             end
-          end
-          `CMDW_MSG_SR: begin
-              cmd <= `MOD_BIT_CMD_START_PP;
-          end
-          `CMDW_MSG_RX: begin
-            if (i == 8) begin
-              cmdw_rx_valid_reg <= 1'b1;
-              // T
-              if (cmdw_rx_ready) begin
-                cmd <= `MOD_BIT_CMD_T_READ; // continue, if peripheral wishes to do so
-                do_rx_t <= 1'b1;
-              end else begin
-                cmd <= `MOD_BIT_CMD_START_OD; // stop
-              end
-            end else begin
-              // SDI
-              cmd <= `MOD_BIT_CMD_READ;
-            end
-            cmdw_rx_reg[8-i] <= sdo_reg;
-          end
-          `CMDW_CCC,
-          `CMDW_MSG_TX: begin
-            if (i == 8) begin
-              // T
-              cmd <= {`MOD_BIT_CMD_WRITE_,1'b1,~^cmdw_body};
-            end else begin
-              // SDO/BCAST+CCC
-              cmd <= {`MOD_BIT_CMD_WRITE_,1'b1,cmdw_body[7 - i[2:0]]};
-            end
-          end
-          `CMDW_STOP: begin
-            cmd <= `MOD_BIT_CMD_STOP;
-          end
-          `CMDW_PROV_ID_BCR_DCR: begin
+          end else begin
+            // SDI
             cmd <= `MOD_BIT_CMD_READ;
-            // TODO: Figure out what to do with PID,BCR,DCR
           end
-          default: begin
-            sm <= `CMDW_NOP;
+          cmdw_rx_reg[8-i] <= rx_sampled;
+        end
+        `CMDW_CCC,
+        `CMDW_MSG_TX: begin
+          if (i == 8) begin
+            // T
+            cmd <= {`MOD_BIT_CMD_WRITE_,1'b1,~^cmdw_body};
+          end else begin
+            // SDO/BCAST+CCC
+            cmd <= {`MOD_BIT_CMD_WRITE_,1'b1,cmdw_body[7 - i[2:0]]};
           end
-        endcase
-      end
+        end
+        `CMDW_STOP: begin
+          cmd <= `MOD_BIT_CMD_STOP_PP;
+        end
+        `CMDW_PROV_ID_BCR_DCR: begin
+          cmd <= `MOD_BIT_CMD_READ;
+          // TODO: Figure out what to do with PID,BCR,DCR
+        end
+        default: begin
+          sm <= `CMDW_NOP;
+        end
+      endcase
     end
-    sdo_reg <= sdo;
+    rx_sampled <= rx_valid ? rx : rx_sampled;
     cmd_ready_reg <= cmd_ready;
   end
 
diff --git a/projects/__i3c_ardz/coraz7s/system_constr.xdc b/projects/__i3c_ardz/coraz7s/system_constr.xdc
index 4ef21aa699..db42ed1ebe 100644
--- a/projects/__i3c_ardz/coraz7s/system_constr.xdc
+++ b/projects/__i3c_ardz/coraz7s/system_constr.xdc
@@ -1,7 +1,5 @@
 # __i3c_ardz I3C interface
 
-set i3c_clock [get_clocks  "i3c_clk"]
-set i3c_clock_bus [get_clocks  "i3c_clk_bus"]
 # Ultrascale devices could use IOB_TRI_REG.
 # Since IOB_TRI_REG is not available in the zynq7000 the tristate flip-flop is placed in the device fabric.
 # Valid IOSTANDARD for the I3C bus are: LVCMOS12, LVCMOS18, and LVCMOS33
@@ -12,25 +10,35 @@ set_property -dict {PACKAGE_PIN Y18 IOSTANDARD LVCMOS33 PULLTYPE PULLUP IOB FALS
 # NOTE: clk_fpga_0 is the first PL fabric clock, also called $sys_cpu_clk
 create_generated_clock -name i3c_clk -source [get_pins -filter name=~*CLKIN1 -of [get_cells -hier -filter name=~*i3c_clkgen*i_mmcm]] -master_clock clk_fpga_0 [get_pins -filter name=~*CLKOUT0 -of [get_cells -hier -filter name=~*i3c_clkgen*i_mmcm]]
 create_generated_clock -name i3c_clk_bus -source [get_pins -filter name=~*CLKIN1 -of [get_cells -hier -filter name=~*i3c_clkgen*i_mmcm]] -master_clock clk_fpga_0 [get_pins -filter name=~*CLKOUT1 -of [get_cells -hier -filter name=~*i3c_clkgen*i_mmcm]]
-set_period -name i3c_clk -source [get_pins -filter name=~*CLKIN1 -of [get_cells -hier -filter name=~*i3c_clkgen*i_mmcm]] -master_clock clk_fpga_0 [get_pins -filter name=~*CLKOUT0 -of [get_cells -hier -filter name=~*i3c_clkgen*i_mmcm]]
+#set_period -name i3c_clk -source [get_pins -filter name=~*CLKIN1 -of [get_cells -hier -filter name=~*i3c_clkgen*i_mmcm]] -master_clock clk_fpga_0 [get_pins -filter name=~*CLKOUT0 -of [get_cells -hier -filter name=~*i3c_clkgen*i_mmcm]]
+set i3c_clock [get_clocks  "i3c_clk"]
+set i3c_clock_bus [get_clocks  "i3c_clk_bus"]
+
+# Input data driven the peripherals toggles every 4 cycles max of the capture clock i3c_clk_bus (PP)
+set_multicycle_path -from [get_ports i3c_controller_0_sda] -to i_system_wrapper/system_i/i3c_controller_0/core/inst/i_i3c_controller_bit_mod/sdo_reg_reg/D -setup 4
+set_multicycle_path -from [get_ports i3c_controller_0_sda] -to i_system_wrapper/system_i/i3c_controller_0/core/inst/i_i3c_controller_bit_mod/sdo_reg_reg/D -hold 3
 
-# Input data driven the peripherals toggles every 4 cycles of the capture clock i3c_clk_bus (PP)
-set_multicycle_path -from [get_ports i3c_controller_0_sda] -to i_system_wrapper/system_i/i3c_controller_0/core/inst/i_i3c_controller_word/sdo_reg_reg/D -setup 4
-set_multicycle_path -from [get_ports i3c_controller_0_sda] -to i_system_wrapper/system_i/i3c_controller_0/core/inst/i_i3c_controller_word/sdo_reg_reg/D -hold 1
+# Output data toggles every 2 cycles max of the capture clock i3c_clk_bus (PP)
+set_multicycle_path -to [get_ports i3c_controller_0_sda] -from i_system_wrapper/system_i/i3c_controller_0/core/inst/i_i3c_controller_bit_mod/sdi_reg/C -setup 2
+set_multicycle_path -to [get_ports i3c_controller_0_sda] -from i_system_wrapper/system_i/i3c_controller_0/core/inst/i_i3c_controller_bit_mod/sdi_reg/C -hold 2
+set_multicycle_path -to [get_ports i3c_controller_0_sda] -from i_system_wrapper/system_i/i3c_controller_0/core/inst/i_i3c_controller_bit_mod/pp_reg/C -setup 2
+set_multicycle_path -to [get_ports i3c_controller_0_sda] -from i_system_wrapper/system_i/i3c_controller_0/core/inst/i_i3c_controller_bit_mod/pp_reg/C -hold 2
+set_multicycle_path -to [get_ports i3c_controller_0_scl] -from i_system_wrapper/system_i/i3c_controller_0/core/inst/i_i3c_controller_bit_mod/scl_reg/C -setup 2
+set_multicycle_path -to [get_ports i3c_controller_0_scl] -from i_system_wrapper/system_i/i3c_controller_0/core/inst/i_i3c_controller_bit_mod/scl_reg/C -hold 2
 
 # Notes
-# rising/fall time for SCL is 150e06 * 1 / fSCL, worse at fSCL = 12.5 MHz => 12ns
+# tcr/tcf rising/fall time for SCL is 150e06 * 1 / fSCL, at fSCL = 12.5 MHz => 12ns, at fSCL = 6.25 MHz, 24ns.
 # and t_SCO has a minimum/default value of 8ns and max of 12 ns
 # Taking those into consideration, the input_delay and output_delay are selected for the worst case scenario.
 set tsco_max   12;
 set tsco_min    8;
 set trc_dly_max 1;
 set trc_dly_min 0;
-set_input_delay  -clock i3c_clock_bus -max [expr $tsco_max + $trc_dly_max] [get_ports i3c_controller_0_sda]
-set_input_delay  -clock i3c_clock_bus -min [expr $tsco_min + $trc_dly_min] [get_ports i3c_controller_0_sda]
+set_input_delay  -clock $i3c_clock_bus -max [expr $tsco_max + $trc_dly_max] [get_ports i3c_controller_0_sda]
+set_input_delay  -clock $i3c_clock_bus -min [expr $tsco_min + $trc_dly_min] [get_ports i3c_controller_0_sda]
 set tsu         2;
 set thd         0;
-set_output_delay  -clock i3c_clock_bus -max [expr $trc_dly_max + $tsu] [get_ports i3c_controller_0_sda]
-set_output_delay  -clock i3c_clock_bus -min [expr $trc_dly_min - $thd] [get_ports i3c_controller_0_sda]
-set_output_delay  -clock i3c_clock_bus -max [expr $trc_dly_max + $tsu] [get_ports i3c_controller_0_scl]
-set_output_delay  -clock i3c_clock_bus -min [expr $trc_dly_min - $thd] [get_ports i3c_controller_0_scl]
+set_output_delay  -clock $i3c_clock_bus -max [expr $trc_dly_max + $tsu] [get_ports i3c_controller_0_sda]
+set_output_delay  -clock $i3c_clock_bus -min [expr $trc_dly_min - $thd] [get_ports i3c_controller_0_sda]
+set_output_delay  -clock $i3c_clock_bus -max [expr $trc_dly_max + $tsu] [get_ports i3c_controller_0_scl]
+set_output_delay  -clock $i3c_clock_bus -min [expr $trc_dly_min - $thd] [get_ports i3c_controller_0_scl]