New wishbone-jtag bridge

project.cfg

@@ -13,75 +13,42 @@ package             = tqg144
 speedgrade          = -2
 files_def           = boards/mimas_v1/ip/clk_gen.xco
 
-[target.synth]
+[target.tools]
-toolchain           = ISE
+toolchain           = make
-ise_settings        = /opt/Xilinx/14.7/ISE_DS/settings64.sh
+output_files        = tools/test
-family              = spartan6
+buildroot           = tools
-device              = xc6slx9
+files_makefile      = tools/Makefile
-package             = tqg144
+files_other         = tools/digilent_jtag.cpp
-speedgrade          = -2
+                      tools/digilent_jtag.hpp
-toplevel            = top_generic
+                      tools/argparse.cpp
-xst_opts            = -vlgincdir rtl/util
+                      tools/argparse.hpp
-files_verilog       = rtl/toplevel/top_generic.v
+                      tools/test.cpp
-                      rtl/util/conv.vh
+
-                      rtl/core/nco_q15.v
+# Testbenches
-                      rtl/core/sigmadelta_sampler.v
+# -----------
-                      rtl/core/sigmadelta_rcmodel_q15.v
+
-                      rtl/core/sigmadelta_input_q15.v
+[target.tb_wb_timer]
-                      rtl/core/mul_const.v
+toolchain           = iverilog
-                      rtl/core/lpf_iir_q15_k.v
+runtime             = all
-                      rtl/core/decimate_by_r_q15.v
+toplevel            = tb_wb_timer
-                      rtl/core/mcu_peripherals.v
+files_verilog       = sim/tb/tb_wb_timer.v
-                      rtl/core/mcu.v
-                      rtl/core/mem_jtag_writable.v
-                    # Arch
-                      rtl/arch/spartan-6/lvds_comparator.v
-                      rtl/arch/spartan-6/clk_gen.v
-                      rtl/arch/spartan-6/jtag_if.v
-                    # SERV
-                      rtl/serv/serv_aligner.v
-                      rtl/serv/serv_alu.v
-                      rtl/serv/serv_bufreg.v
-                      rtl/serv/serv_bufreg2.v
-                      rtl/serv/serv_compdec.v
-                      rtl/serv/serv_csr.v
-                      rtl/serv/serv_ctrl.v
-                      rtl/serv/serv_debug.v
-                      rtl/serv/serv_decode.v
-                      rtl/serv/serv_immdec.v
-                      rtl/serv/serv_mem_if.v
-                      rtl/serv/serv_rf_if.v
-                      rtl/serv/serv_rf_ram_if.v
-                      rtl/serv/serv_rf_ram.v
-                      rtl/serv/serv_state.v
-                      rtl/serv/serv_rf_top.v
-                      rtl/serv/serv_synth_wrapper.v
-                      rtl/serv/serv_top.v
-                    # QERV
-                    #   rtl/qerv/serv_rf_top.v
-                    #   rtl/qerv/serv_synth_wrapper.v
-                    #   rtl/qerv/serv_top.v
-                    #   rtl/qerv/qerv_immdec.v
-                    # Servile
-                      rtl/serv/servile_arbiter.v
-                      rtl/serv/servile_mux.v
-                      rtl/serv/servile_rf_mem_if.v
-                      rtl/serv/servile.v
-                    #   rtl/qerv/servile_arbiter.v
-                    #   rtl/qerv/servile_mux.v
-                    #   rtl/qerv/servile_rf_mem_if.v
-                    #   rtl/qerv/servile.v
-                    # WB
-                      rtl/wb/wb_gpio.v
-                      rtl/wb/wb_gpio_banks.v
-                      rtl/wb/wb_mux.v
-                      rtl/wb/jtag_wb_bridge.v
                       rtl/wb/wb_timer.v
 
-files_con           = boards/mimas_v1/constraints.ucf
+[target.tb_cdc_strobe_data]
-files_other         = rtl/util/rc_alpha_q15.vh
+toolchain           = iverilog
-                      rtl/util/clog2.vh
+runtime             = all
-                      sw/sweep/sweep.hex
+toplevel            = tb_cdc_strobe_data
+files_verilog       = sim/tb/tb_cdc_strobe_data.v
+                      rtl/core/cdc_strobe_data.v
+
+[target.tb_jtag_wb_bridge]
+toolchain           = iverilog
+runtime             = all
+toplevel            = tb_jtag_wb_bridge
+files_verilog       = sim/tb/tb_jtag_wb_bridge.v
+                      rtl/wb/jtag_wb_bridge.v
+                      rtl/core/cdc_req_resp.v
+                      rtl/core/cdc_strobe_data.v
 
 [target.synth_sim]
 toolchain           = iverilog
@@ -99,6 +66,8 @@ files_verilog       = rtl/toplevel/top_generic.v
                       rtl/core/decimate_by_r_q15.v
                       rtl/core/mcu_peripherals.v
                       rtl/core/mcu.v
+                      rtl/core/cdc_strobe_data.v
+                      rtl/core/cdc_req_resp.v
                       rtl/core/mem_jtag_writable.v
                     # Arch
                       rtl/core/lvds_comparator.v
@@ -152,6 +121,82 @@ files_other         = rtl/util/rc_alpha_q15.vh
                       rtl/util/conv.vh
                       sw/sweep/sweep.hex
 
+
+# Synth targets
+# -------------
+
+[target.synth]
+toolchain           = ISE
+ise_settings        = /opt/Xilinx/14.7/ISE_DS/settings64.sh
+family              = spartan6
+device              = xc6slx9
+package             = tqg144
+speedgrade          = -2
+toplevel            = top_generic
+xst_opts            = -vlgincdir rtl/util
+files_verilog       = rtl/toplevel/top_generic.v
+                      rtl/util/conv.vh
+                      rtl/core/nco_q15.v
+                      rtl/core/sigmadelta_sampler.v
+                      rtl/core/sigmadelta_rcmodel_q15.v
+                      rtl/core/sigmadelta_input_q15.v
+                      rtl/core/mul_const.v
+                      rtl/core/lpf_iir_q15_k.v
+                      rtl/core/decimate_by_r_q15.v
+                      rtl/core/mcu_peripherals.v
+                      rtl/core/cdc_strobe_data.v
+                      rtl/core/cdc_req_resp.v
+                      rtl/core/mcu.v
+                      rtl/core/mem_jtag_writable.v
+                    # Arch
+                      rtl/arch/spartan-6/lvds_comparator.v
+                      rtl/arch/spartan-6/clk_gen.v
+                      rtl/arch/spartan-6/jtag_if.v
+                    # SERV
+                      rtl/serv/serv_aligner.v
+                      rtl/serv/serv_alu.v
+                      rtl/serv/serv_bufreg.v
+                      rtl/serv/serv_bufreg2.v
+                      rtl/serv/serv_compdec.v
+                      rtl/serv/serv_csr.v
+                      rtl/serv/serv_ctrl.v
+                      rtl/serv/serv_debug.v
+                      rtl/serv/serv_decode.v
+                      rtl/serv/serv_immdec.v
+                      rtl/serv/serv_mem_if.v
+                      rtl/serv/serv_rf_if.v
+                      rtl/serv/serv_rf_ram_if.v
+                      rtl/serv/serv_rf_ram.v
+                      rtl/serv/serv_state.v
+                      rtl/serv/serv_rf_top.v
+                      rtl/serv/serv_synth_wrapper.v
+                      rtl/serv/serv_top.v
+                    # QERV
+                    #   rtl/qerv/serv_rf_top.v
+                    #   rtl/qerv/serv_synth_wrapper.v
+                    #   rtl/qerv/serv_top.v
+                    #   rtl/qerv/qerv_immdec.v
+                    # Servile
+                      rtl/serv/servile_arbiter.v
+                      rtl/serv/servile_mux.v
+                      rtl/serv/servile_rf_mem_if.v
+                      rtl/serv/servile.v
+                    #   rtl/qerv/servile_arbiter.v
+                    #   rtl/qerv/servile_mux.v
+                    #   rtl/qerv/servile_rf_mem_if.v
+                    #   rtl/qerv/servile.v
+                    # WB
+                      rtl/wb/wb_gpio.v
+                      rtl/wb/wb_gpio_banks.v
+                      rtl/wb/wb_mux.v
+                      rtl/wb/jtag_wb_bridge.v
+                      rtl/wb/wb_timer.v
+
+files_con           = boards/mimas_v1/constraints.ucf
+files_other         = rtl/util/rc_alpha_q15.vh
+                      rtl/util/clog2.vh
+                      sw/sweep/sweep.hex
+
 [target.jtag]
 toolchain           = ISE
 ise_settings        = /opt/Xilinx/14.7/ISE_DS/settings64.sh
@@ -165,33 +210,8 @@ files_other         =
 files_con           = boards/mimas_v1/constraints.ucf
 files_verilog       = rtl/arch/spartan-6/jtag_if.v
                       rtl/arch/spartan-6/clk_gen.v
+                      rtl/core/cdc_strobe_data.v
+                      rtl/core/cdc_req_resp.v
                       rtl/wb/jtag_wb_bridge.v
                       rtl/wb/wb_gpio.v
                       rtl/toplevel/top_jtag.v
-
-[target.svftest]
-toolchain           = iverilog
-runtime             = all
-toplevel            = tb_svf
-files_verilog       = sim/tb/tb_svf.v
-                      sim/overrides/jtag_if.v
-                      rtl/core/cdc_strobed.v
-files_other         = sim/other/test.svf
-
-[target.tb_wb_timer]
-toolchain           = iverilog
-runtime             = all
-toplevel            = tb_wb_timer
-files_verilog       = sim/tb/tb_wb_timer.v
-                      rtl/wb/wb_timer.v
-
-[target.tools]
-toolchain           = make
-output_files        = tools/test
-buildroot           = tools
-files_makefile      = tools/Makefile
-files_other         = tools/digilent_jtag.cpp
-                      tools/digilent_jtag.hpp
-                      tools/argparse.cpp
-                      tools/argparse.hpp
-                      tools/test.cpp

rtl/core/cdc_req_resp.v

@@ -0,0 +1,70 @@
+`timescale 1 ns/1 ps
+// =============================================================================
+// cdc_req_resp
+//   Bidirectional channel made from two cdc_strobe_data mailboxes.
+// =============================================================================
+module cdc_req_resp #(
+  parameter integer REQ_W          = 32,
+  parameter integer RESP_W         = 32,
+  parameter integer STABLE_SAMPLES = 2
+)(
+  // Side A (e.g., JTAG/TCK)
+  input  wire               a_clk,
+  input  wire               a_rst,
+
+  input  wire               a_req_pulse,
+  input  wire [REQ_W-1:0]   a_req_data,
+  output wire               a_req_busy,
+  output wire               a_req_accepted,
+
+  output wire               a_resp_pulse,
+  output wire [RESP_W-1:0]  a_resp_data,
+
+  // Side B (e.g., system/i_clk)
+  input  wire               b_clk,
+  input  wire               b_rst,
+
+  output wire               b_req_pulse,
+  output wire [REQ_W-1:0]   b_req_data,
+
+  input  wire               b_resp_pulse,
+  input  wire [RESP_W-1:0]  b_resp_data,
+  output wire               b_resp_busy,
+  output wire               b_resp_accepted
+);
+
+  cdc_strobe_data #(
+    .WIDTH(REQ_W),
+    .STABLE_SAMPLES(STABLE_SAMPLES)
+  ) u_req (
+    .s_clk(a_clk),
+    .s_rst(a_rst),
+    .s_pulse(a_req_pulse),
+    .s_data(a_req_data),
+    .s_busy(a_req_busy),
+    .s_accepted(a_req_accepted),
+
+    .d_clk(b_clk),
+    .d_rst(b_rst),
+    .d_pulse(b_req_pulse),
+    .d_data(b_req_data)
+  );
+
+  cdc_strobe_data #(
+    .WIDTH(RESP_W),
+    .STABLE_SAMPLES(STABLE_SAMPLES)
+  ) u_resp (
+    .s_clk(b_clk),
+    .s_rst(b_rst),
+    .s_pulse(b_resp_pulse),
+    .s_data(b_resp_data),
+    .s_busy(b_resp_busy),
+    .s_accepted(b_resp_accepted),
+
+    .d_clk(a_clk),
+    .d_rst(a_rst),
+    .d_pulse(a_resp_pulse),
+    .d_data(a_resp_data)
+  );
+
+endmodule

rtl/core/cdc_strobe_data.v

@@ -0,0 +1,130 @@
+`timescale 1 ns/1 ps
+// =============================================================================
+// cdc_strobe_data
+//   - One-deep mailbox for (strobe + data) crossing clock domains.
+//   - Uses toggle req/ack with 2FF sync for toggles.
+//   - Wide bus is held stable by source until ack, destination samples-until-stable.
+// =============================================================================
+module cdc_strobe_data #(
+  parameter integer WIDTH = 32,
+  parameter integer STABLE_SAMPLES = 2  // >=2 recommended
+)(
+  // Source domain
+  input  wire             s_clk,
+  input  wire             s_rst,      // async OK (posedge) if used consistently
+  input  wire             s_pulse,    // strobe (1+ cycles). Accepted when not busy.
+  input  wire [WIDTH-1:0] s_data,
+  output wire             s_busy,     // 1 = mailbox full / waiting for ack
+  output wire             s_accepted, // 1-cycle pulse when we accepted s_pulse
+
+  // Destination domain
+  input  wire             d_clk,
+  input  wire             d_rst,
+  output reg              d_pulse,    // 1-cycle pulse on new data
+  output reg  [WIDTH-1:0] d_data      // updated when d_pulse asserted; held otherwise
+);
+
+  // ----------------------------
+  // Source: hold + req toggle
+  // ----------------------------
+  reg [WIDTH-1:0] s_hold;
+  reg             s_req_tog;
+  reg             s_inflight;
+
+  // Ack toggle synchronized into source domain
+  (* ASYNC_REG="TRUE" *) reg s_ack_sync1, s_ack_sync2;
+
+  assign s_busy     = s_inflight;
+  wire   do_accept  = s_pulse && !s_inflight;
+  assign s_accepted = do_accept;
+
+  // d_ack_tog is generated in destination domain (declared below as reg)
+  // and is synced here with 2FF.
+  always @(posedge s_clk or posedge s_rst) begin
+    if (s_rst) begin
+      s_hold      <= {WIDTH{1'b0}};
+      s_req_tog   <= 1'b0;
+      s_inflight  <= 1'b0;
+      s_ack_sync1 <= 1'b0;
+      s_ack_sync2 <= 1'b0;
+    end else begin
+      s_ack_sync1 <= d_ack_tog;
+      s_ack_sync2 <= s_ack_sync1;
+
+      // clear inflight when ack matches current req toggle
+      if (s_inflight && (s_ack_sync2 == s_req_tog))
+        s_inflight <= 1'b0;
+
+      // accept new item
+      if (do_accept) begin
+        s_hold     <= s_data;
+        s_req_tog  <= ~s_req_tog;
+        s_inflight <= 1'b1;
+      end
+    end
+  end
+
+  // ----------------------------
+  // Destination: sync req toggle, sample-until-stable, then ack toggle
+  // ----------------------------
+  (* ASYNC_REG="TRUE" *) reg d_req_sync1, d_req_sync2;
+  reg d_req_seen;
+
+  reg d_ack_tog;
+
+  reg [WIDTH-1:0] samp;
+  reg [WIDTH-1:0] samp_prev;
+  integer stable_cnt;
+  reg capturing;
+
+  wire d_new_req = (d_req_sync2 != d_req_seen);
+
+  always @(posedge d_clk or posedge d_rst) begin
+    if (d_rst) begin
+      d_req_sync1 <= 1'b0;
+      d_req_sync2 <= 1'b0;
+      d_req_seen  <= 1'b0;
+      d_ack_tog   <= 1'b0;
+
+      d_pulse     <= 1'b0;
+      d_data      <= {WIDTH{1'b0}};
+
+      samp        <= {WIDTH{1'b0}};
+      samp_prev   <= {WIDTH{1'b0}};
+      stable_cnt  <= 0;
+      capturing   <= 1'b0;
+    end else begin
+      d_pulse <= 1'b0;
+
+      d_req_sync1 <= s_req_tog;
+      d_req_sync2 <= d_req_sync1;
+
+      if (d_new_req && !capturing) begin
+        capturing  <= 1'b1;
+        stable_cnt <= 0;
+        samp_prev  <= s_hold;
+        samp       <= s_hold;
+      end else if (capturing) begin
+        samp <= s_hold;
+
+        if (samp == samp_prev) begin
+          if (stable_cnt < (STABLE_SAMPLES-1))
+            stable_cnt <= stable_cnt + 1;
+          else begin
+            // accept
+            d_data     <= samp;
+            d_pulse    <= 1'b1;
+            d_req_seen <= d_req_sync2;
+            d_ack_tog  <= ~d_ack_tog;
+            capturing  <= 1'b0;
+          end
+        end else begin
+          stable_cnt <= 0;
+        end
+
+        samp_prev <= samp;
+      end
+    end
+  end
+
+endmodule

rtl/toplevel/top_jtag.v

@@ -48,20 +48,18 @@ module top_jtag(
     );
 
     wire [31:0] gpio;
-    wire [31:0] gpio_in;
-    assign gpio_in = 32'h0;
 
     wb_gpio #(
         .address(32'h00000000)
     ) u_wb_gpio (
         .i_wb_clk(clk_15),
-        .i_wb_rst(i_rst | cmd_reset),
+        .i_wb_rst(i_rst),
         .i_wb_adr(wb_adr),
         .i_wb_dat(wb_dat),
         .i_wb_sel(wb_sel),
         .i_wb_we(wb_we),
         .i_wb_stb(wb_stb & wb_cyc),
-        .i_gpio(gpio_in),
+        .i_gpio(gpio),
         .o_wb_rdt(wb_rdt),
         .o_wb_ack(wb_ack),
         .o_gpio(gpio)
@@ -69,7 +67,7 @@ module top_jtag(
 
     assign LED = gpio[7:0];
     assign r2r = gpio[13:8];
-    assign led_green = gpio[30];
+    assign led_green = cmd_reset;
-    assign led_red = gpio[31];
+    assign led_red = 'b0;
 
 endmodule

rtl/wb/jtag_wb_bridge.v

@@ -1,193 +1,500 @@
-`timescale 1ns/1ps
+`timescale 1 ns/1 ps
 
 module jtag_wb_bridge #(
-    parameter integer chain = 1,
+  parameter integer chain = 1,
-    // 0: Use cmd_addr[1:0] to select byte lane on 32-bit WB data bus.
+  // 0: use addr[1:0] for byte lane on 32-bit WB
-    // 1: Always use lane 0 (LSB), for byte-wide memories that return data in [7:0].
+  // 1: always use lane 0
-    parameter integer byte_aligned = 0
+  parameter integer byte_aligned = 0
 )(
-    input wire i_clk,
+  input  wire        i_clk,
-    input wire i_rst,
+  input  wire        i_rst,
 
-    output wire [31:0] o_wb_adr,
+  output wire [31:0] o_wb_adr,
-    output wire [31:0] o_wb_dat,
+  output wire [31:0] o_wb_dat,
-    output wire [3:0] o_wb_sel,
+  output wire [3:0]  o_wb_sel,
-    output wire o_wb_we,
+  output wire        o_wb_we,
-    output wire o_wb_cyc,
+  output wire        o_wb_cyc,
-    output wire o_wb_stb,
+  output wire        o_wb_stb,
-    input wire [31:0] i_wb_rdt,
+  input  wire [31:0] i_wb_rdt,
-    input wire i_wb_ack,
+  input  wire        i_wb_ack,
 
-    output wire o_cmd_reset
+  output wire        o_cmd_reset
 );
-    // JTAG interface wires
-    wire jtag_tck;
-    wire jtag_tdi;
-    wire jtag_drck;
-    wire jtag_capture;
-    wire jtag_shift;
-    wire jtag_update;
-    wire jtag_runtest;
-    wire jtag_reset;
-    wire jtag_sel;
 
-    reg [41:0] jtag_q;
+  // ===========================================================================
-    wire [41:0] jtag_data_in;
+  // JTAG interface (Spartan-6 BSCAN wrapper)
-    wire jtag_async_reset;
+  // ===========================================================================
+  wire jtag_tck;
+  wire jtag_tdi;
+  wire jtag_drck;
+  wire jtag_capture;
+  wire jtag_shift;
+  wire jtag_update;
+  wire jtag_runtest;
+  wire jtag_reset;
+  wire jtag_sel;
 
-    jtag_if #(
+  // 48-bit DR (symmetrical command/response)
-        .chain(chain)
+  reg [47:0] jtag_shreg;
-    ) jtag (
-        .i_tdo(jtag_q[0]),
-        .o_tck(jtag_tck),
-        .o_tdi(jtag_tdi),
-        .o_drck(jtag_drck),
-        .o_capture(jtag_capture),
-        .o_shift(jtag_shift),
-        .o_update(jtag_update),
-        .o_runtest(jtag_runtest),
-        .o_reset(jtag_reset),
-        .o_sel(jtag_sel)
-    );
 
-    assign jtag_async_reset = jtag_reset || i_rst;
+  jtag_if #(
+    .chain(chain)
+  ) u_jtag (
+    .i_tdo(jtag_shreg[0]),
+    .o_tck(jtag_tck),
+    .o_tdi(jtag_tdi),
+    .o_drck(jtag_drck),
+    .o_capture(jtag_capture),
+    .o_shift(jtag_shift),
+    .o_update(jtag_update),
+    .o_runtest(jtag_runtest),
+    .o_reset(jtag_reset),
+    .o_sel(jtag_sel)
+  );
 
-    // JTAG shift register behavior
+  wire jtag_async_reset = jtag_reset || i_rst;
-    always @(posedge jtag_drck or posedge jtag_async_reset) begin
+
-        if (jtag_async_reset) begin
+  // ===========================================================================
-            jtag_q <= 42'b0;
+  // CDC request/response channel (48/48 symmetric)
-        end else if (jtag_sel && jtag_capture) begin
+  //   Side A: JTAG/TCK domain
-            jtag_q <= jtag_data_in;
+  //   Side B: system/i_clk domain
-        end else if (jtag_sel && jtag_shift) begin
+  // ===========================================================================
-            jtag_q <= {jtag_tdi, jtag_q[41:1]};
+  wire        a_req_busy;
-        end
+  wire        a_req_accepted;
+  wire        a_resp_pulse;
+  wire [47:0] a_resp_data;
+
+  wire        b_req_pulse;
+  wire [47:0] b_req_data;
+
+  reg         b_resp_pulse;
+  reg  [47:0] b_resp_data;
+  wire        b_resp_busy;
+  wire        b_resp_accepted;
+
+  // Accept UPDATE as a request strobe (qualified by SEL and !busy)
+  wire a_req_pulse = jtag_sel && jtag_update && !a_req_busy;
+  wire [47:0] a_req_data = jtag_shreg;
+
+  cdc_req_resp #(
+    .REQ_W(48),
+    .RESP_W(48),
+    .STABLE_SAMPLES(2)
+  ) u_cdc (
+    .a_clk(jtag_tck),
+    .a_rst(jtag_async_reset),
+
+    .a_req_pulse(a_req_pulse),
+    .a_req_data(a_req_data),
+    .a_req_busy(a_req_busy),
+    .a_req_accepted(a_req_accepted),
+
+    .a_resp_pulse(a_resp_pulse),
+    .a_resp_data(a_resp_data),
+
+    .b_clk(i_clk),
+    .b_rst(i_rst),
+
+    .b_req_pulse(b_req_pulse),
+    .b_req_data(b_req_data),
+
+    .b_resp_pulse(b_resp_pulse),
+    .b_resp_data(b_resp_data),
+    .b_resp_busy(b_resp_busy),
+    .b_resp_accepted(b_resp_accepted)
+  );
+
+  // ===========================================================================
+  // JTAG/TCK domain shift/capture
+  // ===========================================================================
+  reg [47:0] resp_hold_tck;
+
+  always @(posedge jtag_tck or posedge jtag_async_reset) begin
+    if (jtag_async_reset) begin
+      jtag_shreg    <= 48'd0;
+      resp_hold_tck <= 48'd0;
+    end else begin
+      // Latch new response word from CDC when it arrives (independent of CAPTURE)
+      if (a_resp_pulse) begin
+        resp_hold_tck <= a_resp_data;
+      end
+
+      if (jtag_sel && jtag_capture) begin
+        // Load response into shift register for host readout
+        jtag_shreg <= resp_hold_tck;
+      end else if (jtag_sel && jtag_shift) begin
+        // Shift: MSB in, LSB out to TDO
+        jtag_shreg <= {jtag_tdi, jtag_shreg[47:1]};
+      end
     end
+  end
 
-    // -----------------------------------------------------------------------------
+  // ===========================================================================
-    // JTAG -> i_clk crossing using toggle request/ack handshake.
+  // System domain: Wishbone master + small command queue + response pending
-    // Command packet format: [41]=we, [40]=reset, [39:8]=addr, [7:0]=wdata
+  // ===========================================================================
-    // -----------------------------------------------------------------------------
+  // Opcodes
-    reg [41:0] j_cmd_hold;
+  localparam [7:0] OP_NOP         = 8'h00;
-    reg j_req_tgl;
+  localparam [7:0] OP_RESET_ON    = 8'h10;
-    reg j_ack_sync_1;
+  localparam [7:0] OP_RESET_OFF   = 8'h11;
-    reg j_ack_sync_2;
+  localparam [7:0] OP_WRITE8      = 8'h20;
+  localparam [7:0] OP_READ8       = 8'h21;
+  localparam [7:0] OP_PING        = 8'h30;
+  localparam [7:0] OP_CLEAR_FLAGS = 8'h40;
 
-    reg s_ack_tgl;
+  // Wishbone regs
-    reg s_req_sync_1;
+  reg        wb_busy;
-    reg s_req_sync_2;
+  reg [31:0] wb_adr_r;
-    reg s_req_sync_3;
+  reg [31:0] wb_dat_r;
-    reg [41:0] s_cmd_sync_1;
+  reg [3:0]  wb_sel_r;
-    reg [41:0] s_cmd_sync_2;
+  reg        wb_we_r;
 
-    always @(posedge jtag_drck or posedge jtag_async_reset) begin
+  assign o_wb_adr = wb_adr_r;
-        if (jtag_async_reset) begin
+  assign o_wb_dat = wb_dat_r;
-            j_ack_sync_1 <= 1'b0;
+  assign o_wb_sel = wb_sel_r;
-            j_ack_sync_2 <= 1'b0;
+  assign o_wb_we  = wb_we_r;
+  assign o_wb_cyc = wb_busy;
+  assign o_wb_stb = wb_busy;
+
+  // Reset control
+  reg cmd_reset_level_r;
+  assign o_cmd_reset = cmd_reset_level_r;
+
+  // For reporting only: sync a_req_busy (TCK domain) into i_clk
+  (* ASYNC_REG="TRUE" *) reg req_busy_sync1, req_busy_sync2;
+  wire req_busy_tck_sync = req_busy_sync2;
+
+  // Sequencing
+  reg [7:0] cmd_seq_r;
+  reg [7:0] resp_seq_r;
+
+  // Sticky flags (cleared by CLEAR_FLAGS or reset)
+  reg flag_cmd_overflow;
+  reg flag_illegal;
+  reg flag_wb_busy_at_req;
+
+  // Snapshot info
+  reg       last_we_r;
+  reg [7:0] last_opcode_r;
+
+  // Active command / queued command
+  reg        act_valid;
+  reg [7:0]  act_opcode;
+  reg [31:0] act_addr;
+  reg [7:0]  act_data;
+  reg [7:0]  act_seq;
+
+  reg        q_valid;
+  reg [7:0]  q_opcode;
+  reg [31:0] q_addr;
+  reg [7:0]  q_data;
+  reg [7:0]  q_seq;
+
+  // Response pending buffer (to avoid dropping if resp mailbox busy)
+  reg        resp_pending;
+  reg [47:0] resp_pending_word;
+
+  // Lane selection
+  wire [1:0] addr_lane = byte_aligned ? 2'b00 : act_addr[1:0];
+
+  // Helpers: form SEL/DAT for byte write
+  function [3:0] sel_from_lane(input [1:0] lane);
+    case (lane)
+      2'b00: sel_from_lane = 4'b0001;
+      2'b01: sel_from_lane = 4'b0010;
+      2'b10: sel_from_lane = 4'b0100;
+      default: sel_from_lane = 4'b1000;
+    endcase
+  endfunction
+
+  function [31:0] dat_from_lane_byte(input [1:0] lane, input [7:0] b);
+    case (lane)
+      2'b00: dat_from_lane_byte = {24'b0, b};
+      2'b01: dat_from_lane_byte = {16'b0, b, 8'b0};
+      2'b10: dat_from_lane_byte = {8'b0,  b, 16'b0};
+      default: dat_from_lane_byte = {b, 24'b0};
+    endcase
+  endfunction
+
+  function [7:0] byte_from_lane(input [1:0] lane, input [31:0] w);
+    case (lane)
+      2'b00: byte_from_lane = w[7:0];
+      2'b01: byte_from_lane = w[15:8];
+      2'b10: byte_from_lane = w[23:16];
+      default: byte_from_lane = w[31:24];
+    endcase
+  endfunction
+
+  // Build response word
+  function [47:0] pack_resp(
+    input [7:0]  resp_seq,
+    input [7:0]  status,
+    input [7:0]  cmd_seq,
+    input [7:0]  data,
+    input [7:0]  flags,
+    input [7:0]  last_op
+  );
+    pack_resp = {resp_seq, status, cmd_seq, data, flags, last_op};
+  endfunction
+
+  // STATUS bits (snapshot)
+  wire [7:0] status_snapshot = {
+    2'b00,                 // [7:6]
+    1'b1,                  // [5] resp_valid
+    last_we_r,             // [4] last_we
+    cmd_reset_level_r,     // [3] reset_level
+    b_resp_busy,           // [2] resp_busy (system domain)
+    req_busy_tck_sync,     // [1] req_busy (synced from TCK just for reporting)
+    wb_busy                // [0] wb_busy
+  };
+
+  // FLAGS bits (sticky)
+  wire [7:0] flags_sticky = {
+    4'b0000,               // [7:4] reserved
+    1'b0,                  // [3] reserved
+    flag_wb_busy_at_req,   // [2]
+    flag_illegal,          // [1]
+    flag_cmd_overflow      // [0]
+  };
+
+  // Queue a command (or set overflow sticky if queue full)
+  task automatic enqueue_cmd(
+    input [7:0]  op,
+    input [31:0] addr,
+    input [7:0]  dat,
+    input [7:0]  seq
+  );
+    begin
+      if (!q_valid) begin
+        q_valid  <= 1'b1;
+        q_opcode <= op;
+        q_addr   <= addr;
+        q_data   <= dat;
+        q_seq    <= seq;
+      end else begin
+        // Already have one queued; mark overflow and drop this command
+        flag_cmd_overflow <= 1'b1;
+      end
+    end
+  endtask
+
+  // Start executing a command (for non-WB ops may immediately create a response)
+  task automatic start_active_cmd(
+    input [7:0]  cmd_opcode,
+    input [31:0] cmd_addr,
+    input [7:0]  cmd_data,
+    input [7:0]  cmd_seq
+  );
+    reg [1:0] cmd_addr_lane;
+    begin
+      cmd_addr_lane = byte_aligned ? 2'b00 : cmd_addr[1:0];
+
+      last_opcode_r <= cmd_opcode;
+      last_we_r     <= (cmd_opcode == OP_WRITE8);
+
+      // If we're already mid-flight or holding a response, note it (diagnostic)
+      if (wb_busy || resp_pending)
+        flag_wb_busy_at_req <= 1'b1;
+
+      case (cmd_opcode)
+        OP_NOP: begin
+          // immediate response
+          resp_pending_word <= pack_resp(resp_seq_r, status_snapshot, cmd_seq, 8'h00, flags_sticky, cmd_opcode);
+          resp_pending      <= 1'b1;
+        end
+
+        OP_PING: begin
+          resp_pending_word <= pack_resp(resp_seq_r, status_snapshot, cmd_seq, 8'hA5, flags_sticky, cmd_opcode);
+          resp_pending      <= 1'b1;
+        end
+
+        OP_CLEAR_FLAGS: begin
+          flag_cmd_overflow    <= 1'b0;
+          flag_illegal         <= 1'b0;
+          flag_wb_busy_at_req  <= 1'b0;
+          resp_pending_word    <= pack_resp(resp_seq_r, status_snapshot, cmd_seq, 8'h00, 8'h00, cmd_opcode);
+          resp_pending         <= 1'b1;
+        end
+
+        OP_RESET_ON: begin
+          cmd_reset_level_r <= 1'b1;
+          resp_pending_word <= pack_resp(resp_seq_r, status_snapshot, cmd_seq, 8'h00, flags_sticky, cmd_opcode);
+          resp_pending      <= 1'b1;
+        end
+
+        OP_RESET_OFF: begin
+          cmd_reset_level_r <= 1'b0;
+          resp_pending_word <= pack_resp(resp_seq_r, status_snapshot, cmd_seq, 8'h00, flags_sticky, cmd_opcode);
+          resp_pending      <= 1'b1;
+        end
+
+        OP_WRITE8: begin
+          // launch WB write (byte)
+          wb_busy  <= 1'b1;
+          wb_we_r  <= 1'b1;
+          wb_adr_r <= cmd_addr;
+          wb_sel_r <= sel_from_lane(cmd_addr_lane);
+          wb_dat_r <= dat_from_lane_byte(cmd_addr_lane, cmd_data);
+        end
+
+        OP_READ8: begin
+          // launch WB read (byte select)
+          wb_busy  <= 1'b1;
+          wb_we_r  <= 1'b0;
+          wb_adr_r <= cmd_addr;
+          wb_sel_r <= sel_from_lane(cmd_addr_lane);
+          wb_dat_r <= 32'b0;
+        end
+
+        default: begin
+          flag_illegal <= 1'b1;
+          resp_pending_word <= pack_resp(resp_seq_r, status_snapshot, cmd_seq, 8'h00, flags_sticky, cmd_opcode);
+          resp_pending      <= 1'b1;
+        end
+      endcase
+    end
+  endtask
+
+  // System main
+  always @(posedge i_clk) begin
+    if (i_rst) begin
+      wb_busy          <= 1'b0;
+      wb_adr_r         <= 32'b0;
+      wb_dat_r         <= 32'b0;
+      wb_sel_r         <= 4'b0000;
+      wb_we_r          <= 1'b0;
+
+      cmd_reset_level_r<= 1'b0;
+
+      req_busy_sync1   <= 1'b0;
+      req_busy_sync2   <= 1'b0;
+
+      cmd_seq_r        <= 8'd0;
+      resp_seq_r       <= 8'd0;
+
+      flag_cmd_overflow<= 1'b0;
+      flag_illegal     <= 1'b0;
+      flag_wb_busy_at_req <= 1'b0;
+
+      last_we_r        <= 1'b0;
+      last_opcode_r    <= 8'h00;
+
+      act_valid        <= 1'b0;
+      act_opcode       <= 8'h00;
+      act_addr         <= 32'h0;
+      act_data         <= 8'h00;
+      act_seq          <= 8'h00;
+
+      q_valid          <= 1'b0;
+      q_opcode         <= 8'h00;
+      q_addr           <= 32'h0;
+      q_data           <= 8'h00;
+      q_seq            <= 8'h00;
+
+      resp_pending     <= 1'b0;
+      resp_pending_word<= 48'h0;
+
+      b_resp_pulse     <= 1'b0;
+      b_resp_data      <= 48'h0;
+    end else begin
+      b_resp_pulse <= 1'b0;
+
+      // Sync req-busy level (reporting only)
+      req_busy_sync1 <= a_req_busy;
+      req_busy_sync2 <= req_busy_sync1;
+
+      // -----------------------------------------------------------------------
+      // Accept incoming command from CDC (always delivered; we buffer internally)
+      // -----------------------------------------------------------------------
+      if (b_req_pulse) begin
+        // assign a sequence number to each received command
+        cmd_seq_r <= cmd_seq_r + 8'd1;
+
+        // If we can start immediately (no active, no wb, no pending response), do so.
+        if (!act_valid && !wb_busy && !resp_pending) begin
+          act_valid  <= 1'b1;
+          act_opcode <= b_req_data[47:40];
+          act_addr   <= b_req_data[39:8];
+          act_data   <= b_req_data[7:0];
+          act_seq    <= cmd_seq_r;
+          // Start it right away
+          start_active_cmd(b_req_data[47:40], b_req_data[39:8], b_req_data[7:0], cmd_seq_r);
         end else begin
-            j_ack_sync_1 <= s_ack_tgl;
+          // Otherwise enqueue one-deep
-            j_ack_sync_2 <= j_ack_sync_1;
+          enqueue_cmd(b_req_data[47:40], b_req_data[39:8], b_req_data[7:0], cmd_seq_r);
         end
-    end
+      end
 
-    always @(posedge jtag_update or posedge jtag_async_reset) begin
+      // -----------------------------------------------------------------------
-        if (jtag_async_reset) begin
+      // Wishbone completion -> create response (but don't drop; buffer pending)
-            j_cmd_hold <= 42'b0;
+      // -----------------------------------------------------------------------
-            j_req_tgl <= 1'b0;
+      if (wb_busy && i_wb_ack) begin
-        end else if (jtag_sel && (j_ack_sync_2 == j_req_tgl)) begin
+        wb_busy <= 1'b0;
-            j_cmd_hold <= jtag_q;
+        wb_we_r <= 1'b0;
-            j_req_tgl <= ~j_req_tgl;
-        end
-    end
 
-    // -----------------------------------------------------------------------------
+        // Determine response byte
-    // Wishbone classic single-request master (1 outstanding transaction max).
+        if (act_opcode == OP_READ8) begin
-    // -----------------------------------------------------------------------------
+          resp_pending_word <= pack_resp(
-    reg wb_busy;
+            resp_seq_r,
-    reg [31:0] wb_adr_r;
+            status_snapshot,
-    reg [31:0] wb_dat_r;
+            act_seq,
-    reg [3:0] wb_sel_r;
+            byte_from_lane(addr_lane, i_wb_rdt),
-    reg wb_we_r;
+            flags_sticky,
-    reg cmd_reset_level_r;
+            act_opcode
-    reg [31:0] resp_addr_r;
+          );
-    reg [7:0] resp_data_r;
-
-    wire req_pulse;
-    wire [7:0] cmd_wdata;
-    wire [31:0] cmd_addr;
-    wire cmd_reset;
-    wire cmd_we;
-    wire [1:0] req_lane;
-    wire [1:0] resp_lane;
-
-    assign req_pulse = s_req_sync_2 ^ s_req_sync_3;
-    assign cmd_wdata = s_cmd_sync_2[7:0];
-    assign cmd_addr = s_cmd_sync_2[39:8];
-    assign cmd_reset = s_cmd_sync_2[40];
-    assign cmd_we = s_cmd_sync_2[41];
-    assign req_lane = byte_aligned ? 2'b00 : cmd_addr[1:0];
-    assign resp_lane = byte_aligned ? 2'b00 : wb_adr_r[1:0];
-
-    assign o_wb_adr = wb_adr_r;
-    assign o_wb_dat = wb_dat_r;
-    assign o_wb_sel = wb_sel_r;
-    assign o_wb_we = wb_we_r;
-    assign o_wb_cyc = wb_busy;
-    assign o_wb_stb = wb_busy;
-    assign o_cmd_reset = cmd_reset_level_r;
-
-    always @(posedge i_clk) begin
-        if (i_rst) begin
-            s_ack_tgl <= 1'b0;
-            s_req_sync_1 <= 1'b0;
-            s_req_sync_2 <= 1'b0;
-            s_req_sync_3 <= 1'b0;
-            s_cmd_sync_1 <= 42'b0;
-            s_cmd_sync_2 <= 42'b0;
-            wb_busy <= 1'b0;
-            wb_adr_r <= 32'b0;
-            wb_dat_r <= 32'b0;
-            wb_sel_r <= 4'b0000;
-            wb_we_r <= 1'b0;
-            cmd_reset_level_r <= 1'b0;
-            resp_addr_r <= 32'b0;
-            resp_data_r <= 8'b0;
         end else begin
-            s_req_sync_1 <= j_req_tgl;
+          // WRITE8: echo written byte (lightweight)
-            s_req_sync_2 <= s_req_sync_1;
+          resp_pending_word <= pack_resp(
-            s_req_sync_3 <= s_req_sync_2;
+            resp_seq_r,
-            s_cmd_sync_1 <= j_cmd_hold;
+            status_snapshot,
-            s_cmd_sync_2 <= s_cmd_sync_1;
+            act_seq,
-            if (req_pulse && !wb_busy) begin
+            act_data,
-                wb_busy <= 1'b1;
+            flags_sticky,
-                wb_we_r <= cmd_we;
+            act_opcode
-                wb_adr_r <= cmd_addr;
+          );
-                cmd_reset_level_r <= cmd_reset;
-
-                case (req_lane)
-                    2'b00: begin wb_sel_r <= 4'b0001; wb_dat_r <= {24'b0, cmd_wdata}; end
-                    2'b01: begin wb_sel_r <= 4'b0010; wb_dat_r <= {16'b0, cmd_wdata, 8'b0}; end
-                    2'b10: begin wb_sel_r <= 4'b0100; wb_dat_r <= {8'b0, cmd_wdata, 16'b0}; end
-                    default: begin wb_sel_r <= 4'b1000; wb_dat_r <= {cmd_wdata, 24'b0}; end
-                endcase
-            end
-
-            if (wb_busy && i_wb_ack) begin
-                wb_busy <= 1'b0;
-                wb_we_r <= 1'b0;
-                resp_addr_r <= wb_adr_r;
-
-                case (resp_lane)
-                    2'b00: resp_data_r <= i_wb_rdt[7:0];
-                    2'b01: resp_data_r <= i_wb_rdt[15:8];
-                    2'b10: resp_data_r <= i_wb_rdt[23:16];
-                    default: resp_data_r <= i_wb_rdt[31:24];
-                endcase
-
-                s_ack_tgl <= s_req_sync_2;
-            end
         end
-    end
+        resp_pending <= 1'b1;
+      end
 
-    assign jtag_data_in = {2'b00, resp_addr_r, resp_data_r};
+      // -----------------------------------------------------------------------
+      // If we have a pending response and response mailbox is free, send it
+      // -----------------------------------------------------------------------
+      if (resp_pending && !b_resp_busy) begin
+        b_resp_data  <= resp_pending_word;
+        b_resp_pulse <= 1'b1;
+        resp_pending <= 1'b0;
+        resp_seq_r   <= resp_seq_r + 8'd1;
+
+        // Mark active command complete
+        act_valid <= 1'b0;
+
+        // If there is a queued command, promote and start it
+        if (q_valid) begin
+          act_valid  <= 1'b1;
+          act_opcode <= q_opcode;
+          act_addr   <= q_addr;
+          act_data   <= q_data;
+          act_seq    <= q_seq;
+          q_valid    <= 1'b0;
+
+          start_active_cmd(q_opcode, q_addr, q_data, q_seq);
+        end
+      end
+
+      // -----------------------------------------------------------------------
+      // If no active command but there is a queued one (and we're not busy), start it
+      // -----------------------------------------------------------------------
+      if (!act_valid && q_valid && !wb_busy && !resp_pending) begin
+        act_valid  <= 1'b1;
+        act_opcode <= q_opcode;
+        act_addr   <= q_addr;
+        act_data   <= q_data;
+        act_seq    <= q_seq;
+        q_valid    <= 1'b0;
+
+        start_active_cmd(q_opcode, q_addr, q_data, q_seq);
+      end
+
+    end
+  end
 
 endmodule

sim/other/test.svf

@@ -1,10 +0,0 @@
-TRST ABSENT;
-ENDIR IDLE;
-ENDDR IDLE;
-STATE RESET;
-STATE IDLE;
-SIR 6 TDI (02);
-
-SDR 42 TDI (3A987654321);
-
-STATE IDLE;

sim/tb/tb_cdc_strobe_data.v

@@ -0,0 +1,124 @@
+`timescale 1ns/1ps
+
+module tb_cdc_strobe_data;
+
+  localparam integer WIDTH = 16;
+  localparam integer NUM_TX = 5;
+
+  // Source domain
+  reg              s_clk;
+  reg              s_rst;
+  reg              s_pulse;
+  reg  [WIDTH-1:0] s_data;
+  wire             s_busy;
+  wire             s_accepted;
+
+  // Destination domain
+  reg              d_clk;
+  reg              d_rst;
+  wire             d_pulse;
+  wire [WIDTH-1:0] d_data;
+
+  reg [WIDTH-1:0] exp_data [0:NUM_TX-1];
+  integer rx_idx;
+  integer tx_idx;
+
+  cdc_strobe_data #(
+    .WIDTH(WIDTH),
+    .STABLE_SAMPLES(2)
+  ) dut (
+    .s_clk(s_clk),
+    .s_rst(s_rst),
+    .s_pulse(s_pulse),
+    .s_data(s_data),
+    .s_busy(s_busy),
+    .s_accepted(s_accepted),
+    .d_clk(d_clk),
+    .d_rst(d_rst),
+    .d_pulse(d_pulse),
+    .d_data(d_data)
+  );
+
+  initial s_clk = 1'b0;
+  always #625 s_clk = ~s_clk;   // 800 kHz
+
+  initial d_clk = 1'b0;
+  always #33.333 d_clk = ~d_clk;   // 15 MHz (asynchronous to s_clk)
+
+  task send_word;
+    input [WIDTH-1:0] value;
+    begin
+      @(posedge s_clk);
+      while (s_busy)
+        @(posedge s_clk);
+
+      s_data  <= value;
+      s_pulse <= 1'b1;
+      @(posedge s_clk);
+
+      if (!s_accepted) begin
+        $display("[%0t] ERROR: expected s_accepted for value 0x%0h", $time, value);
+        $fatal(1);
+      end
+
+      s_pulse <= 1'b0;
+      s_data  <= {WIDTH{1'b0}};
+    end
+  endtask
+
+  always @(posedge d_clk) begin
+    if (!d_rst && d_pulse) begin
+      if (rx_idx >= NUM_TX) begin
+        $display("[%0t] ERROR: unexpected extra d_pulse with data=0x%0h", $time, d_data);
+        $fatal(1);
+      end
+
+      if (d_data !== exp_data[rx_idx]) begin
+        $display("[%0t] ERROR: rx[%0d] expected 0x%0h, got 0x%0h",
+                 $time, rx_idx, exp_data[rx_idx], d_data);
+        $fatal(1);
+      end
+
+      $display("[%0t] INFO: rx[%0d] = 0x%0h", $time, rx_idx, d_data);
+      rx_idx <= rx_idx + 1;
+    end
+  end
+
+  initial begin
+    $dumpfile("out.vcd");
+    $dumpvars(0, tb_cdc_strobe_data);
+
+    s_rst   = 1'b1;
+    d_rst   = 1'b1;
+    s_pulse = 1'b0;
+    s_data  = {WIDTH{1'b0}};
+    rx_idx  = 0;
+    tx_idx  = 0;
+
+    exp_data[0] = 16'h1234;
+    exp_data[1] = 16'h00A5;
+    exp_data[2] = 16'hBEEF;
+    exp_data[3] = 16'h5AA5;
+    exp_data[4] = 16'hCAFE;
+
+    repeat (4) @(posedge s_clk);
+    s_rst = 1'b0;
+    repeat (3) @(posedge d_clk);
+    d_rst = 1'b0;
+
+    for (tx_idx = 0; tx_idx < NUM_TX; tx_idx = tx_idx + 1)
+      send_word(exp_data[tx_idx]);
+
+    wait (rx_idx == NUM_TX);
+    $display("[%0t] PASS: received %0d/%0d transfers correctly", $time, rx_idx, NUM_TX);
+    #20;
+    $finish;
+  end
+
+  initial begin
+    #100000000;
+    $display("[%0t] ERROR: timeout waiting for transfers", $time);
+    $fatal(1);
+  end
+
+endmodule

sim/tb/tb_jtag_wb_bridge.v

@@ -0,0 +1,334 @@
+`timescale 1ns/1ps
+
+module tb_jtag_wb_bridge;
+
+  localparam [7:0] OP_NOP       = 8'h00;
+  localparam [7:0] OP_RESET_ON  = 8'h10;
+  localparam [7:0] OP_RESET_OFF = 8'h11;
+  localparam [7:0] OP_WRITE8    = 8'h20;
+  localparam [7:0] OP_READ8     = 8'h21;
+  localparam [7:0] OP_PING      = 8'h30;
+
+  reg i_clk;
+  reg i_rst;
+
+  wire [31:0] o_wb_adr;
+  wire [31:0] o_wb_dat;
+  wire [3:0]  o_wb_sel;
+  wire        o_wb_we;
+  wire        o_wb_cyc;
+  wire        o_wb_stb;
+  reg  [31:0] i_wb_rdt;
+  reg         i_wb_ack;
+  wire        o_cmd_reset;
+
+  reg [7:0] mem [0:1023];
+  reg wb_req_pending;
+  reg [31:0] wb_addr_latched;
+  reg [31:0] wb_dat_latched;
+  reg [3:0]  wb_sel_latched;
+  reg        wb_we_latched;
+  integer    wb_base;
+
+  integer idx;
+  integer req_accept_count;
+
+  reg saw_write_lane3;
+  reg saw_read_lane3;
+  reg saw_cmd_reset_high;
+  reg [47:0] flush_rx;
+
+  jtag_wb_bridge #(
+    .chain(1),
+    .byte_aligned(0)
+  ) dut (
+    .i_clk(i_clk),
+    .i_rst(i_rst),
+    .o_wb_adr(o_wb_adr),
+    .o_wb_dat(o_wb_dat),
+    .o_wb_sel(o_wb_sel),
+    .o_wb_we(o_wb_we),
+    .o_wb_cyc(o_wb_cyc),
+    .o_wb_stb(o_wb_stb),
+    .i_wb_rdt(i_wb_rdt),
+    .i_wb_ack(i_wb_ack),
+    .o_cmd_reset(o_cmd_reset)
+  );
+
+  function [47:0] make_cmd;
+    input [7:0] op;
+    input [31:0] addr;
+    input [7:0] data;
+    begin
+      make_cmd = {op, addr, data};
+    end
+  endfunction
+
+  task expect_resp;
+    input [47:0] resp;
+    input [7:0] exp_data;
+    input [7:0] exp_last_op;
+    begin
+      if (resp[23:16] !== exp_data) begin
+        $display("[%0t] ERROR: data exp=0x%0h got=0x%0h", $time, exp_data, resp[23:16]);
+        $fatal(1);
+      end
+      if (resp[7:0] !== exp_last_op) begin
+        $display("[%0t] ERROR: last_op exp=0x%0h got=0x%0h", $time, exp_last_op, resp[7:0]);
+        $fatal(1);
+      end
+      if (resp[37] !== 1'b1) begin
+        $display("[%0t] ERROR: status.resp_valid bit not set in response 0x%012h", $time, resp);
+        $fatal(1);
+      end
+    end
+  endtask
+
+  task send_cmd;
+    input [47:0] cmd_word;
+    integer tries;
+    integer start_accepts;
+    reg [47:0] dummy_rx;
+    begin
+      start_accepts = req_accept_count;
+      for (tries = 0; tries < 8; tries = tries + 1) begin
+        do_xfer(cmd_word, dummy_rx);
+        if (req_accept_count > start_accepts)
+          tries = 8;
+      end
+      if (req_accept_count <= start_accepts) begin
+        $display("[%0t] ERROR: command 0x%0h was never accepted", $time, cmd_word[47:40]);
+        $fatal(1);
+      end
+    end
+  endtask
+
+  task expect_eventual_response;
+    input [7:0] exp_data;
+    input [7:0] exp_last_op;
+    integer tries;
+    reg [47:0] rx_word;
+    reg found;
+    begin
+      found = 1'b0;
+      for (tries = 0; tries < 8; tries = tries + 1) begin
+        do_xfer(make_cmd(OP_NOP, 32'h0000_0000, 8'h00), rx_word);
+        if (rx_word[7:0] == exp_last_op) begin
+          expect_resp(rx_word, exp_data, exp_last_op);
+          found = 1'b1;
+          tries = 8;
+        end
+      end
+      if (!found) begin
+        $display("[%0t] ERROR: did not observe response for last_op=0x%0h", $time, exp_last_op);
+        $fatal(1);
+      end
+    end
+  endtask
+
+  task do_xfer;
+    input  [47:0] tx_word;
+    output [47:0] rx_word;
+    begin
+      dut.u_jtag.transceive48(tx_word, rx_word);
+      repeat (400) @(posedge i_clk);
+    end
+  endtask
+
+  initial i_clk = 1'b0;
+  always #10 i_clk = ~i_clk; // 50 MHz
+
+  always @(posedge i_clk) begin
+    if (i_rst) begin
+      i_wb_ack        <= 1'b0;
+      i_wb_rdt        <= 32'h0;
+      wb_req_pending  <= 1'b0;
+      wb_addr_latched <= 32'h0;
+      wb_dat_latched  <= 32'h0;
+      wb_sel_latched  <= 4'h0;
+      wb_we_latched   <= 1'b0;
+    end else begin
+      if (o_cmd_reset)
+        saw_cmd_reset_high <= 1'b1;
+
+      i_wb_ack <= 1'b0;
+
+      if (wb_req_pending) begin
+        wb_base = {wb_addr_latched[31:2], 2'b00};
+
+        if (wb_we_latched) begin
+          if (wb_sel_latched[0]) mem[wb_base + 0] <= wb_dat_latched[7:0];
+          if (wb_sel_latched[1]) mem[wb_base + 1] <= wb_dat_latched[15:8];
+          if (wb_sel_latched[2]) mem[wb_base + 2] <= wb_dat_latched[23:16];
+          if (wb_sel_latched[3]) mem[wb_base + 3] <= wb_dat_latched[31:24];
+        end
+
+        i_wb_rdt <= {mem[wb_base + 3], mem[wb_base + 2], mem[wb_base + 1], mem[wb_base + 0]};
+        i_wb_ack <= 1'b1;
+        wb_req_pending <= 1'b0;
+      end else if (o_wb_cyc && o_wb_stb) begin
+        wb_addr_latched <= o_wb_adr;
+        wb_dat_latched  <= o_wb_dat;
+        wb_sel_latched  <= o_wb_sel;
+        wb_we_latched   <= o_wb_we;
+        wb_req_pending  <= 1'b1;
+
+        if (o_wb_we && (o_wb_adr == 32'h0000_0013) && (o_wb_sel == 4'b1000) && (o_wb_dat == 32'h5A00_0000))
+          saw_write_lane3 <= 1'b1;
+        if (!o_wb_we && (o_wb_adr == 32'h0000_0013) && (o_wb_sel == 4'b1000))
+          saw_read_lane3 <= 1'b1;
+      end
+    end
+  end
+
+  always @(posedge dut.jtag_tck) begin
+    if (i_rst)
+      req_accept_count <= 0;
+    else if (dut.a_req_accepted)
+      req_accept_count <= req_accept_count + 1;
+  end
+
+  initial begin
+    $dumpfile("out.vcd");
+    $dumpvars(0, tb_jtag_wb_bridge);
+
+    i_rst = 1'b1;
+    i_wb_rdt = 32'h0;
+    i_wb_ack = 1'b0;
+    wb_req_pending = 1'b0;
+    wb_addr_latched = 32'h0;
+    wb_dat_latched = 32'h0;
+    wb_sel_latched = 4'h0;
+    wb_we_latched = 1'b0;
+    saw_write_lane3 = 1'b0;
+    saw_read_lane3 = 1'b0;
+    saw_cmd_reset_high = 1'b0;
+    req_accept_count = 0;
+
+    for (idx = 0; idx < 1024; idx = idx + 1)
+      mem[idx] = idx[7:0];
+
+    repeat (10) @(posedge i_clk);
+    i_rst = 1'b0;
+
+    repeat (10) @(posedge i_clk);
+
+    send_cmd(make_cmd(OP_PING,      32'h0000_0000, 8'h00));
+    expect_eventual_response(8'hA5, OP_PING);
+
+    send_cmd(make_cmd(OP_WRITE8,    32'h0000_0013, 8'h5A));
+    send_cmd(make_cmd(OP_READ8,     32'h0000_0013, 8'h00));
+    send_cmd(make_cmd(OP_RESET_ON,  32'h0000_0000, 8'h00));
+    send_cmd(make_cmd(OP_RESET_OFF, 32'h0000_0000, 8'h00));
+
+    repeat (6)
+      do_xfer(make_cmd(OP_NOP, 32'h0000_0000, 8'h00), flush_rx);
+
+    if (!saw_write_lane3) begin
+      $display("[%0t] ERROR: expected WRITE8 lane-3 WB access not observed", $time);
+      $fatal(1);
+    end
+    if (!saw_read_lane3) begin
+      $display("[%0t] ERROR: expected READ8 lane-3 WB access not observed", $time);
+      $fatal(1);
+    end
+    if (!saw_cmd_reset_high) begin
+      $display("[%0t] ERROR: expected o_cmd_reset to go high at least once", $time);
+      $fatal(1);
+    end
+
+    repeat (50) @(posedge i_clk);
+    if (o_cmd_reset !== 1'b0) begin
+      $display("[%0t] ERROR: expected o_cmd_reset low after RESET_OFF, got %0b", $time, o_cmd_reset);
+      $fatal(1);
+    end
+
+    $display("[%0t] PASS: jtag_wb_bridge basic command/response path verified", $time);
+    #100;
+    $finish;
+  end
+
+  initial begin
+    #5_000_000;
+    $display("[%0t] ERROR: timeout", $time);
+    $fatal(1);
+  end
+
+endmodule
+
+module jtag_if #(
+  parameter integer chain = 1
+)(
+  input  wire i_tdo,
+  output reg  o_tck,
+  output reg  o_tdi,
+  output reg  o_drck,
+  output reg  o_capture,
+  output reg  o_shift,
+  output reg  o_update,
+  output reg  o_runtest,
+  output reg  o_reset,
+  output reg  o_sel
+);
+  integer k;
+
+  // Quiet unused parameter warning
+  wire _unused_chain;
+  assign _unused_chain = chain[0];
+
+  task pulse_tck;
+    begin
+      #40 o_tck = 1'b1;
+      #40 o_tck = 1'b0;
+    end
+  endtask
+
+  task transceive48;
+    input [47:0] tx_word;
+    output [47:0] rx_word;
+    begin
+      rx_word = 48'h0;
+
+      // Let CDC response path advance in the TCK domain before CAPTURE.
+      for (k = 0; k < 32; k = k + 1)
+        pulse_tck;
+
+      // CAPTURE-DR
+      o_capture = 1'b1;
+      pulse_tck;
+      o_capture = 1'b0;
+
+      // SHIFT-DR (read previous response while shifting next command in)
+      o_shift = 1'b1;
+      for (k = 0; k < 48; k = k + 1) begin
+        o_tdi = tx_word[k];
+        rx_word[k] = i_tdo;
+        o_drck = 1'b1;
+        pulse_tck;
+        o_drck = 1'b0;
+      end
+      o_shift = 1'b0;
+
+      // UPDATE-DR (request strobe sampled by TCK)
+      o_update = 1'b1;
+      pulse_tck;
+      o_update = 1'b0;
+
+      // Provide a few extra clocks after UPDATE.
+      for (k = 0; k < 8; k = k + 1)
+        pulse_tck;
+    end
+  endtask
+
+  initial begin
+    o_tck = 1'b0;
+    o_tdi = 1'b0;
+    o_drck = 1'b0;
+    o_capture = 1'b0;
+    o_shift = 1'b0;
+    o_update = 1'b0;
+    o_runtest = 1'b0;
+    o_reset = 1'b0;
+    o_sel = 1'b1;
+  end
+endmodule

sim/tb/tb_svf.v

@@ -1,106 +0,0 @@
-`timescale 1ns/1ps
-
-module tb_svf();
-    reg clk;
-    reg resetn;
-    initial clk <= 1'b0;
-    initial resetn <= 1'b0;
-    always #33.33 clk <= !clk;
-    initial #40 resetn <= 1'b1;
-
-    wire [7:0] led_out;
-
-    jtag_byte_sink #(
-        .SVF_FILE("sim/other/test.svf"),
-        .TCK_HALF_PERIOD_NS(500)
-    ) dut (
-        .i_clk(clk),
-        .i_rst(!resetn),
-        .o_led(led_out)
-    );
-
-    initial begin
-        $dumpfile("out.vcd");
-        $dumpvars;
-        #200_000;
-        $finish;
-    end
-endmodule
-
-module jtag_byte_sink #(
-    parameter [8*256-1:0] SVF_FILE = "",
-    parameter integer TCK_HALF_PERIOD_NS = 50
-)(
-    input wire i_clk,
-    input wire i_rst,
-    output reg [7:0] o_led
-);
-
-    initial o_led <= 0;
-
-    // JTAG interface wires
-    wire jtag_tck;
-    wire jtag_tdi;
-    wire jtag_drck;
-    wire jtag_capture;
-    wire jtag_shift;
-    wire jtag_update;
-    wire jtag_runtest;
-    wire jtag_reset;
-    wire jtag_sel;
-
-    reg [41:0] jtag_q;
-    reg [41:0] jtag_data;
-    wire jtag_async_reset;
-
-    jtag_if #(
-        .chain(1),
-        .SVF_FILE(SVF_FILE),
-        .TCK_HALF_PERIOD_NS(TCK_HALF_PERIOD_NS),
-        .USER_IR_OPCODE(32'h0000_0002)
-    ) jtag (
-        .i_tdo(jtag_q[0]),
-        .o_tck(jtag_tck),
-        .o_tdi(jtag_tdi),
-        .o_drck(jtag_drck),
-        .o_capture(jtag_capture),
-        .o_shift(jtag_shift),
-        .o_update(jtag_update),
-        .o_runtest(jtag_runtest),
-        .o_reset(jtag_reset),
-        .o_sel(jtag_sel)
-    );
-
-
-    assign jtag_async_reset = jtag_reset || i_rst;
-
-    always @(posedge jtag_drck or posedge jtag_async_reset) begin
-        if (jtag_async_reset) begin
-            jtag_q <= 0;
-        end else if (jtag_sel && jtag_capture) begin
-            jtag_q <= jtag_data;
-        end else if (jtag_sel && jtag_shift) begin
-            jtag_q <= {jtag_tdi, jtag_q[41:1]};
-        end
-    end
-
-    always @(posedge jtag_update or posedge jtag_async_reset) begin
-        if (jtag_async_reset) begin
-            jtag_data <= 0;
-        end else if (jtag_sel) begin
-            jtag_data <= jtag_q;
-        end
-    end
-
-    wire [41:0] j_data;
-    wire j_data_update;
-    cdc_strobed #(42) j_data_cdc (
-        .i_clk_a(i_clk),
-        .i_clk_b(i_clk),
-        .i_data(jtag_data),
-        .i_strobe(jtag_update),
-        .o_data(j_data),
-        .o_strobe(j_data_update)
-    );
-
-endmodule

sw/sweep/sweep.c

@@ -26,7 +26,7 @@ static inline void irq_init() {
 
 void timer_isr(){
     static int set = 0;
-    *TIMER = 18400;
+    *TIMER = 1840000*4;
     *LEDGR = ~(*LEDGR);
 }
 
@@ -34,12 +34,12 @@ void main(){
     irq_init();
 
     *LEDGR = 3;
-    *TIMER = 18400;
+    *TIMER = 1840000*4;
 
     for(;;){
         for(int i=1000; i<10000; i++){
             *R_FREQ = i;
-            for(int j=0; j<100; j++) asm volatile("nop");
+            for(int j=0; j<80; j++) asm volatile("nop");
         }
     }
 }

tools/argparse.cpp

@@ -12,21 +12,41 @@ ArgParser::ArgParser(std::string program_name)
 void ArgParser::addString(const std::string &name,
                           const std::string &default_value,
                           const std::string &help,
-                          bool required) {
+                          bool required,
+                          const std::string &short_name) {
     order_.push_back(name);
-    meta_[name] = {OptionType::kString, help, required};
+    meta_[name] = {OptionType::kString, help, required, short_name};
     string_values_[name] = default_value;
     provided_[name] = false;
+    if (!short_name.empty()) {
+        short_to_long_[short_name] = name;
+    }
 }
 
 void ArgParser::addInt(const std::string &name,
                        int default_value,
                        const std::string &help,
-                       bool required) {
+                       bool required,
+                       const std::string &short_name) {
     order_.push_back(name);
-    meta_[name] = {OptionType::kInt, help, required};
+    meta_[name] = {OptionType::kInt, help, required, short_name};
     int_values_[name] = default_value;
     provided_[name] = false;
+    if (!short_name.empty()) {
+        short_to_long_[short_name] = name;
+    }
+}
+
+void ArgParser::addFlag(const std::string &name,
+                        const std::string &help,
+                        const std::string &short_name) {
+    order_.push_back(name);
+    meta_[name] = {OptionType::kFlag, help, false, short_name};
+    flag_values_[name] = false;
+    provided_[name] = false;
+    if (!short_name.empty()) {
+        short_to_long_[short_name] = name;
+    }
 }
 
 bool ArgParser::parse(int argc, char **argv, std::string *error) {
@@ -39,6 +59,79 @@ bool ArgParser::parse(int argc, char **argv, std::string *error) {
             return false;
         }
 
+        if (token.rfind("--", 0) != 0 && token.rfind("-", 0) == 0) {
+            std::string short_key = token.substr(1);
+            std::string short_value;
+            size_t short_eq = short_key.find('=');
+            if (short_eq != std::string::npos) {
+                short_value = short_key.substr(short_eq + 1);
+                short_key = short_key.substr(0, short_eq);
+            }
+
+            auto sk = short_to_long_.find(short_key);
+            if (sk == short_to_long_.end()) {
+                if (error) {
+                    *error = "Unknown option: -" + short_key;
+                }
+                return false;
+            }
+
+            auto m = meta_.find(sk->second);
+            if (m == meta_.end()) {
+                if (error) {
+                    *error = "Unknown option: -" + short_key;
+                }
+                return false;
+            }
+
+            if (m->second.type == OptionType::kFlag) {
+                if (short_eq != std::string::npos) {
+                    if (error) {
+                        *error = "Flag does not take a value: -" + short_key;
+                    }
+                    return false;
+                }
+                flag_values_[sk->second] = true;
+                provided_[sk->second] = true;
+            } else if (m->second.type == OptionType::kString) {
+                if (short_eq == std::string::npos) {
+                    if (i + 1 >= argc) {
+                        if (error) {
+                            *error = "Missing value for -" + short_key;
+                        }
+                        return false;
+                    }
+                    short_value = argv[++i];
+                }
+                string_values_[sk->second] = short_value;
+                provided_[sk->second] = true;
+            } else if (m->second.type == OptionType::kInt) {
+                long parsed;
+                if (short_eq == std::string::npos) {
+                    if (i + 1 >= argc) {
+                        if (error) {
+                            *error = "Missing value for -" + short_key;
+                        }
+                        return false;
+                    }
+                    short_value = argv[++i];
+                }
+                errno = 0;
+                char *endp = nullptr;
+                parsed = std::strtol(short_value.c_str(), &endp, 0);
+                if (errno != 0 || endp == short_value.c_str() || *endp != '\0' ||
+                    parsed < INT_MIN || parsed > INT_MAX) {
+                    if (error) {
+                        *error = "Invalid integer for -" + short_key + ": " + short_value;
+                    }
+                    return false;
+                }
+                int_values_[sk->second] = static_cast<int>(parsed);
+                provided_[sk->second] = true;
+            }
+            continue;
+        }
+
         if (token.rfind("--", 0) != 0) {
             if (error) {
                 *error = "Unexpected positional argument: " + token;
@@ -51,13 +144,6 @@ bool ArgParser::parse(int argc, char **argv, std::string *error) {
         size_t eq = token.find('=');
         if (eq == std::string::npos) {
             key = token.substr(2);
-            if (i + 1 >= argc) {
-                if (error) {
-                    *error = "Missing value for --" + key;
-                }
-                return false;
-            }
-            value = argv[++i];
         } else {
             key = token.substr(2, eq - 2);
             value = token.substr(eq + 1);
@@ -71,10 +157,32 @@ bool ArgParser::parse(int argc, char **argv, std::string *error) {
             return false;
         }
 
+        if (m->second.type == OptionType::kFlag) {
+            if (eq != std::string::npos) {
+                if (error) {
+                    *error = "Flag does not take a value: --" + key;
+                }
+                return false;
+            }
+            flag_values_[key] = true;
+            provided_[key] = true;
+            continue;
+        }
+
+        if (eq == std::string::npos) {
+            if (i + 1 >= argc) {
+                if (error) {
+                    *error = "Missing value for --" + key;
+                }
+                return false;
+            }
+            value = argv[++i];
+        }
+
         if (m->second.type == OptionType::kString) {
             string_values_[key] = value;
             provided_[key] = true;
-        } else {
+        } else if (m->second.type == OptionType::kInt) {
             errno = 0;
             char *endp = nullptr;
             long parsed = std::strtol(value.c_str(), &endp, 0);
@@ -124,6 +232,14 @@ int ArgParser::getInt(const std::string &name) const {
     return it->second;
 }
 
+bool ArgParser::getFlag(const std::string &name) const {
+    auto it = flag_values_.find(name);
+    if (it == flag_values_.end()) {
+        return false;
+    }
+    return it->second;
+}
+
 std::string ArgParser::helpText() const {
     std::ostringstream oss;
     oss << "Usage: " << program_name_ << " [options]\n\n";
@@ -135,7 +251,16 @@ std::string ArgParser::helpText() const {
             continue;
         }
 
-        oss << "  --" << key << " <value>";
+        oss << "  ";
+        if (!m->second.short_name.empty()) {
+            oss << "-" << m->second.short_name << ", ";
+        } else {
+            oss << "    ";
+        }
+        oss << "--" << key;
+        if (m->second.type != OptionType::kFlag) {
+            oss << " <value>";
+        }
         if (m->second.required) {
             oss << " (required)";
         }
@@ -147,9 +272,11 @@ std::string ArgParser::helpText() const {
                 oss << " [default: '" << s->second << "']";
             }
         } else {
-            auto iv = int_values_.find(key);
+            if (m->second.type == OptionType::kInt) {
-            if (iv != int_values_.end()) {
+                auto iv = int_values_.find(key);
-                oss << " [default: " << iv->second << "]";
+                if (iv != int_values_.end()) {
+                    oss << " [default: " << iv->second << "]";
+                }
             }
         }
         oss << "\n";

tools/argparse.hpp

@@ -26,31 +26,40 @@ public:
     void addString(const std::string &name,
                    const std::string &default_value,
                    const std::string &help,
-                   bool required = false);
+                   bool required = false,
+                   const std::string &short_name = "");
 
     void addInt(const std::string &name,
                 int default_value,
                 const std::string &help,
-                bool required = false);
+                bool required = false,
+                const std::string &short_name = "");
+
+    void addFlag(const std::string &name,
+                 const std::string &help,
+                 const std::string &short_name = "");
 
     bool parse(int argc, char **argv, std::string *error);
 
     bool has(const std::string &name) const;
     std::string getString(const std::string &name) const;
     int getInt(const std::string &name) const;
+    bool getFlag(const std::string &name) const;
 
     std::string helpText() const;
 
 private:
     enum class OptionType {
         kString,
-        kInt
+        kInt,
+        kFlag
     };
 
     struct OptionMeta {
         OptionType type;
         std::string help;
         bool required;
+        std::string short_name;
     };
 
     std::string program_name_;
@@ -59,5 +68,7 @@ private:
 
     std::unordered_map<std::string, std::string> string_values_;
     std::unordered_map<std::string, int> int_values_;
+    std::unordered_map<std::string, bool> flag_values_;
     std::unordered_map<std::string, bool> provided_;
+    std::unordered_map<std::string, std::string> short_to_long_;
 };

tools/test.cpp

@@ -5,37 +5,55 @@
 #include <cstdio>
 #include <string>
 
-void write(DigilentJtag &jtag, uint32_t addr, uint8_t data, bool reset){
+static constexpr uint8_t OP_NOP         = 0x00;
-    uint8_t d[6], din[6];
+static constexpr uint8_t OP_RESET_ON    = 0x10;
-    d[0] = data;
+static constexpr uint8_t OP_RESET_OFF   = 0x11;
-    d[1] = (uint8_t)addr;
+static constexpr uint8_t OP_WRITE8      = 0x20;
-    d[2] = (uint8_t)(addr>>8);
+static constexpr uint8_t OP_READ8       = 0x21;
-    d[3] = (uint8_t)(addr>>16);
+static constexpr uint8_t OP_PING        = 0x30;
-    d[4] = (uint8_t)(addr>>24);
+static constexpr uint8_t OP_CLEAR_FLAGS = 0x40;
-    d[5] = ((reset) ? 1 : 0) | 0x2; // <we><rst>
 
-    jtag.shiftData(d, din, 42);
+static void shift48(DigilentJtag &jtag, const uint8_t tx[6], uint8_t rx[6]) {
+  jtag.shiftData(tx, rx, 48);
 }
 
-uint8_t read(DigilentJtag &jtag, uint32_t addr, bool reset){
+static void make_cmd(uint8_t out[6], uint8_t opcode, uint32_t addr, uint8_t data) {
-    uint8_t d[6], din[6];
+  out[0] = data;
-    d[0] = 0xff;
+  out[1] = (uint8_t)addr;
-    d[1] = (uint8_t)addr;
+  out[2] = (uint8_t)(addr >> 8);
-    d[2] = (uint8_t)(addr>>8);
+  out[3] = (uint8_t)(addr >> 16);
-    d[3] = (uint8_t)(addr>>16);
+  out[4] = (uint8_t)(addr >> 24);
-    d[4] = (uint8_t)(addr>>24);
+  out[5] = opcode;
-    d[5] = ((reset) ? 1 : 0); // <we><rst>
-
-    jtag.shiftData(d, din, 42);
-    // Read back
-    jtag.shiftData(d, din, 42);
-
-    return din[0];
 }
 
+static uint8_t do_cmd(DigilentJtag& jtag, uint8_t opcode, uint32_t addr, uint8_t data){
+    uint8_t tx[6], rx[6];
+    make_cmd(tx, opcode, addr, data);
+    shift48(jtag, tx, rx);
+    for(int i=0; i<32; i++){
+        make_cmd(tx, OP_NOP, 0, 0);
+        shift48(jtag, tx, rx);
+        if(rx[0] == opcode){
+            return rx[2];
+        }
+    }
+    printf("Could not do command\r\n");
+    return 0;
+}
+
+struct Resp48 {
+  uint8_t last_op;
+  uint8_t flags;
+  uint8_t data;
+  uint8_t cmd_seq;
+  uint8_t status;
+  uint8_t resp_seq;
+};
+
 int main(int argc, char** argv){
     ArgParser parser(argc > 0 ? argv[0] : "test");
-    parser.addString("write", "", "file to write");
+    parser.addString("file", "", "File to write", true, "f");
+    parser.addFlag("verify", "Verify", "v");
 
     std::string parse_error;
     if (!parser.parse(argc, argv, &parse_error)) {
@@ -48,8 +66,6 @@ int main(int argc, char** argv){
         return -1;
     }
 
-    const std::string arg_write = parser.getString("write");
-
     DigilentJtag jtag;
     if(!jtag.open()){
         printf("Could not open programmer\r\n");
@@ -57,34 +73,51 @@ int main(int argc, char** argv){
     }
     jtag.setChain(1);
 
-    // Start reset
-    read(jtag, 0, true);
 
-    if(arg_write!=""){
+    do_cmd(jtag, OP_CLEAR_FLAGS, 0, 0);
-        uint32_t addr = 0;
+    // Check for ping
-        uint8_t buf[32];
+    if(do_cmd(jtag, OP_PING, 0, 0) != 0xa5){
-        int nr;
+        printf("PING response was not right\r\n");
-        FILE* f = fopen(arg_write.c_str(), "rb");
+        jtag.close();
-        if(!f){
+        return -1;
-            goto end;
-        }
-
-        do{
-            nr = fread(buf, 1, 32, f);
-            for(int i=0; i<32; i++){
-                write(jtag, addr, buf[i], true);
-                addr++;
-            }
-        }while(nr>0);
-
-        fclose(f);
     }
 
+    const std::string file = parser.getString("file");
+    FILE* f = fopen(file.c_str(), "rb");
+    if(!f){
+        printf("Could not open file\r\n");
+        jtag.close();
+        return -1;
+    }
 
-end:
+    do_cmd(jtag, OP_RESET_ON, 0, 0);
-    // End reset
-    read(jtag, 0, false);
 
+    int nr = 0;
+    int addr = 0;
+    do{
+        uint8_t buf[64];
+        nr = fread(buf, 1, 64, f);
+        for(int i=0; i<nr; i++){
+            do_cmd(jtag, OP_WRITE8, addr+i, buf[i]);
+            printf(".");
+        }
+        printf("\r\n");
+
+        if(parser.getFlag("verify")){
+            for(int i=0; i<nr; i++){
+                uint8_t r = do_cmd(jtag, OP_READ8, addr+i, 0);
+                if(r!=buf[i]){
+                    printf(" -- Verify failed at %04x : %02x != %02x\r\n", addr+i, r, buf[i]);
+                }
+            }
+        }
+
+        addr += nr;
+    }while(nr > 0);
+    
+    do_cmd(jtag, OP_RESET_OFF, 0, 0);
+
+    fclose(f);
     jtag.close();
     return 0;
 }