Added back in the jtag bridge

Now talking over the bus instead of using dpram

.gitignore

@@ -1,3 +1,5 @@
 out
 build
-env
+env
+__pycache*
+_impactbatch.log

boards/mimas_v1/constraints.ucf

@@ -24,4 +24,29 @@ NET "r2r[1]" IOSTANDARD = LVCMOS33;
 NET "r2r[2]" IOSTANDARD = LVCMOS33;
 NET "r2r[3]" IOSTANDARD = LVCMOS33;
 NET "r2r[4]" IOSTANDARD = LVCMOS33;
-NET "r2r[5]" IOSTANDARD = LVCMOS33;
+NET "r2r[5]" IOSTANDARD = LVCMOS33;
+
+NET "LED[0]" LOC = P119;
+NET "LED[0]" IOSTANDARD = LVCMOS33;
+NET "LED[0]" DRIVE = 8;
+NET "LED[1]" LOC = P118;
+NET "LED[1]" IOSTANDARD = LVCMOS33;
+NET "LED[1]" DRIVE = 8;
+NET "LED[2]" LOC = P117;
+NET "LED[2]" IOSTANDARD = LVCMOS33;
+NET "LED[2]" DRIVE = 8;
+NET "LED[3]" LOC = P116;
+NET "LED[3]" IOSTANDARD = LVCMOS33;
+NET "LED[3]" DRIVE = 8;
+NET "LED[4]" LOC = P115;
+NET "LED[4]" IOSTANDARD = LVCMOS33;
+NET "LED[4]" DRIVE = 8;
+NET "LED[5]" LOC = P114;
+NET "LED[5]" IOSTANDARD = LVCMOS33;
+NET "LED[5]" DRIVE = 8;
+NET "LED[6]" LOC = P112;
+NET "LED[6]" IOSTANDARD = LVCMOS33;
+NET "LED[6]" DRIVE = 8;
+NET "LED[7]" LOC = P111;
+NET "LED[7]" IOSTANDARD = LVCMOS33;
+NET "LED[7]" DRIVE = 8;

project.cfg

@@ -4,11 +4,126 @@ version             = 0.1
 out_dir             = out
 build_dir           = build
 
-[server]
+[target.ip]
-hostname            = localhost
+toolchain           = ISE_IP
-port                = 2020
+ise_settings        = /opt/Xilinx/14.7/ISE_DS/settings64.sh
-privkey             = /home/joppe/.ssh/id_rsa
+family              = spartan6
-pubkey              = /home/joppe/.ssh/id_rsa.pub
+device              = xc6slx9
+package             = tqg144
+speedgrade          = -2
+files_def           = boards/mimas_v1/ip/clk_gen.xco
+
+[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
+
+# Testbenches
+# -----------
+
+[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.tb_cdc_strobe_data]
+toolchain           = iverilog
+runtime             = all
+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
+runtime             = all
+toplevel            = tb_top_generic
+ivl_opts            = -Irtl/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/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
+                      sim/overrides/clk_gen.v
+                      rtl/core/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
+
+                      sim/tb/tb_top_generic.v
+
+files_con           = boards/mimas_v1/constraints.ucf
+files_other         = rtl/util/rc_alpha_q15.vh
+                      rtl/util/clog2.vh
+                      rtl/util/conv.vh
+                      sw/sweep/sweep.hex
+
+
+# Synth targets
+# -------------
 
 [target.synth]
 toolchain           = ISE
@@ -18,9 +133,9 @@ device              = xc6slx9
 package             = tqg144
 speedgrade          = -2
 toplevel            = top_generic
-xst_opts            = -vlgincdir rtl/util
+xst_opts            = -vlgincdir rtl/util -keep_hierarchy yes
-files_verilog       = rtl/util/conv.vh
+files_verilog       = rtl/toplevel/top_generic.v
-                      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
@@ -28,113 +143,78 @@ files_verilog       = rtl/util/conv.vh
                       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/arbiter.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
+                      rtl/serv/serving_ram.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/wb_arbiter.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.ip]
+[target.jtag]
-toolchain           = ISE_IP
-ise_settings        = /opt/Xilinx/14.7/ISE_DS/settings64.sh
-family              = spartan6
-device              = xc6slx9
-package             = tqg144
-speedgrade          = -2
-files_def           = boards/mimas_v1/ip/mem_8kx8b.xco
-                      #boards/mimas_v1/ip/clk_gen.xco
-
-[target.serv]
 toolchain           = ISE
 ise_settings        = /opt/Xilinx/14.7/ISE_DS/settings64.sh
 family              = spartan6
 device              = xc6slx9
 package             = tqg144
 speedgrade          = -2
-toplevel            = top_generic
+toplevel            = top_jtag
-xst_opts            = -vlgincdir rtl
+xst_opts            = -vlgincdir rtl/util -keep_hierarchy yes
+files_other         =
 files_con           = boards/mimas_v1/constraints.ucf
-files_other         = sw/blinky/blinky.hex
+files_verilog       = rtl/arch/spartan-6/jtag_if.v
-                      rtl/util/clog2.vh
-files_verilog       = 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_rf_top.v
-                      rtl/serv/serv_state.v
-                      rtl/serv/serv_synth_wrapper.v
-                      rtl/serv/serv_top.v
-                      rtl/serv/servile_arbiter.v
-                      rtl/serv/servile_mux.v
-                      rtl/serv/servile_rf_mem_if.v
-                      rtl/serv/servile.v
-                      rtl/serv/serving_ram.v
-                      rtl/serv/serving.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_serv.v
+                      rtl/toplevel/top_jtag.v
-
-[target.sim]
-toolchain           = iverilog
-runtime             = all
-toplevel            = tb_sigmadelta
-ivl_opts            = -Irtl/util
-files_verilog       = sim/tb/tb_nco_q15.v
-                      sim/tb/tb_sigmadelta.v
-                      sim/tb/tb_mul_const.v
-                      rtl/core/nco_q15.v
-                      rtl/core/lvds_comparator.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
-                      sim/overrides/sigmadelta_sampler.v
-                      sim/overrides/clk_gen.v
-files_other         = rtl/util/conv.vh
-                      rtl/util/rc_alpha_q15.vh
-
-[target.servsim]
-toolchain           = iverilog
-runtime             = all
-toplevel            = tb_serving
-ivl_opts            = -Irtl/util
-files_other         = sw/blinky/blinky.hex
-                      rtl/util/clog2.vh
-files_verilog       = 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_rf_top.v
-                      rtl/serv/serv_state.v
-                      rtl/serv/serv_synth_wrapper.v
-                      rtl/serv/serv_top.v
-                      rtl/serv/servile_arbiter.v
-                      rtl/serv/servile_mux.v
-                      rtl/serv/servile_rf_mem_if.v
-                      rtl/serv/servile.v
-                      rtl/serv/serving_ram.v
-                      rtl/serv/serving.v
-                      rtl/arch/spartan-6/clk_gen.v
-                      rtl/wb/wb_gpio.v
-                      rtl/toplevel/top_serv.v
-                      sim/tb/tb_serving.v

rtl/arch/spartan-6/jtag_if.v

@@ -0,0 +1,35 @@
+`timescale 1ns/1ps
+
+// =============================================================================
+// JTAG interface
+// Spartan-6 BSCAN primitive wrapper (USER1 chain).
+// =============================================================================
+module jtag_if #(
+    parameter chain = 1
+)(
+    input wire i_tdo,
+    output wire o_tck,
+    output wire o_tdi,
+    output wire o_drck,
+    output wire o_capture,
+    output wire o_shift,
+    output wire o_update,
+    output wire o_runtest,
+    output wire o_reset,
+    output wire o_sel
+);
+    BSCAN_SPARTAN6 #(
+        .JTAG_CHAIN(chain)
+    ) bscan_i (
+        .CAPTURE(o_capture),
+        .DRCK(o_drck),
+        .RESET(o_reset),
+        .RUNTEST(o_runtest),
+        .SEL(o_sel),
+        .SHIFT(o_shift),
+        .TCK(o_tck),
+        .TDI(o_tdi),
+        .TDO(i_tdo),
+        .UPDATE(o_update)
+    );
+endmodule

rtl/core/arbiter.v

@@ -0,0 +1,138 @@
+/**
+ * Module: arbiter
+ *
+ * Description:
+ *  A look ahead, round-robing parameterized arbiter.
+ *
+ * <> request
+ *  each bit is controlled by an actor and each actor can 'request' ownership
+ *  of the shared resource by bring high its request bit.
+ *
+ * <> grant
+ *  when an actor has been given ownership of shared resource its 'grant' bit
+ *  is driven high
+ *
+ * <> select
+ *  binary representation of the grant signal (optional use)
+ *
+ * <> active
+ *  is brought high by the arbiter when (any) actor has been given ownership
+ *  of shared resource.
+ *
+ *
+ * Created: Sat Jun  1 20:26:44 EDT 2013
+ *
+ * Author:  Berin Martini // berin.martini@gmail.com
+ */
+ `ifndef _arbiter_ `define _arbiter_
+ `include "../util/clog2.vh"
+
+module arbiter
+  #(parameter
+    NUM_PORTS = 6,
+    SEL_WIDTH = ((NUM_PORTS > 1) ? `CLOG2(NUM_PORTS) : 1))
+   (input wire                  clk,
+    input wire                  rst,
+    input wire [NUM_PORTS-1:0]  request,
+    output reg [NUM_PORTS-1:0]  grant,
+    output reg [SEL_WIDTH-1:0]  select,
+    output reg                  active
+);
+
+    /**
+     * Local parameters
+     */
+
+    localparam WRAP_LENGTH = 2*NUM_PORTS;
+
+
+    // Find First 1 - Start from MSB and count downwards, returns 0 when no
+    // bit set
+    function [SEL_WIDTH-1:0] ff1 (
+        input [NUM_PORTS-1:0] in
+    );
+        reg     set;
+        integer i;
+
+        begin
+            set = 1'b0;
+            ff1 = 'b0;
+
+            for (i = 0; i < NUM_PORTS; i = i + 1) begin
+                if (in[i] & ~set) begin
+                    set = 1'b1;
+                    ff1 = i[0 +: SEL_WIDTH];
+                end
+            end
+        end
+    endfunction
+
+
+`ifdef VERBOSE
+    initial $display("Bus arbiter with %d units", NUM_PORTS);
+`endif
+
+
+    /**
+     * Internal signals
+     */
+
+    integer                 yy;
+
+    wire                    next;
+    wire [NUM_PORTS-1:0]    order;
+
+    reg  [NUM_PORTS-1:0]    token;
+    wire [NUM_PORTS-1:0]    token_lookahead [NUM_PORTS-1:0];
+    wire [WRAP_LENGTH-1:0]  token_wrap;
+
+
+    /**
+     * Implementation
+     */
+
+    assign token_wrap   = {token, token};
+
+    assign next         = ~|(token & request);
+
+
+    always @(posedge clk)
+        grant <= token & request;
+
+
+    always @(posedge clk)
+        select <= ff1(token & request);
+
+
+    always @(posedge clk)
+        active <= |(token & request);
+
+
+    always @(posedge clk)
+        if (rst) token <= 'b1;
+        else if (next) begin
+
+            for (yy = 0; yy < NUM_PORTS; yy = yy + 1) begin : TOKEN_
+
+                if (order[yy]) begin
+                    token <= token_lookahead[yy];
+                end
+            end
+        end
+
+
+    genvar xx;
+    generate
+        for (xx = 0; xx < NUM_PORTS; xx = xx + 1) begin : ORDER_
+
+            assign token_lookahead[xx]  = token_wrap[xx +: NUM_PORTS];
+
+            assign order[xx]            = |(token_lookahead[xx] & request);
+
+        end
+    endgenerate
+
+
+endmodule
+
+`endif //  `ifndef _arbiter_

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/core/jtag_if.v

@@ -0,0 +1,34 @@
+`timescale 1ns/1ps
+
+// =============================================================================
+// JTAG interface
+// Generic stub model with inactive/tied-off outputs.
+// =============================================================================
+module jtag_if #(
+    parameter chain = 1
+)(
+    input wire i_tdo,
+    output wire o_tck,
+    output wire o_tdi,
+    output wire o_drck,
+    output wire o_capture,
+    output wire o_shift,
+    output wire o_update,
+    output wire o_runtest,
+    output wire o_reset,
+    output wire o_sel
+);
+    assign o_tck = 1'b0;
+    assign o_tdi = 1'b0;
+    assign o_drck = 1'b0;
+    assign o_capture = 1'b0;
+    assign o_shift = 1'b0;
+    assign o_update = 1'b0;
+    assign o_runtest = 1'b0;
+    assign o_reset = 1'b0;
+    assign o_sel = 1'b0;
+
+    // Keep lint tools quiet in generic builds where TDO is unused.
+    wire _unused_tdo;
+    assign _unused_tdo = i_tdo;
+endmodule

rtl/core/mcu.v

@@ -0,0 +1,382 @@
+`timescale 1ns/1ps
+`include "../util/clog2.vh"
+
+module mcu #(
+    parameter memfile = "",
+    parameter memsize = 8192,
+    parameter sim = 1'b0,
+    parameter jtag = 1
+)(
+    input wire i_clk,
+    input wire i_rst,
+
+    input wire [31:0] i_GPI_A,
+    input wire [31:0] i_GPI_B,
+    input wire [31:0] i_GPI_C,
+    input wire [31:0] i_GPI_D,
+    output wire [31:0] o_GPO_A,
+    output wire [31:0] o_GPO_B,
+    output wire [31:0] o_GPO_C,
+    output wire [31:0] o_GPO_D
+);
+    localparam WITH_CSR = 1;
+    localparam rf_width = 8;
+
+    wire rst;
+    wire rst_wb;
+    wire rst_mem_peripherals;
+    wire rst_cmd_jtag;
+    wire timer_irq;
+    assign rst = i_rst | rst_mem_peripherals | rst_cmd_jtag;
+    // Keep the Wishbone path alive during JTAG "core reset" so memory can be programmed.
+    assign rst_wb = i_rst;
+
+    // Busses
+    // CPU<->memory interconnect (CPU is a WB master)
+    wire [31:0] wb_mem_adr;
+    wire [31:0] wb_mem_dat;
+    wire [3:0] wb_mem_sel;
+    wire wb_mem_we;
+    wire wb_mem_stb;
+    wire [31:0] wb_mem_rdt_cpu;
+    wire wb_mem_ack_cpu;
+
+    // Interconnect->memory (shared WB slave side)
+    wire [31:0] wb_mem_adr_s;
+    wire [31:0] wb_mem_dat_s;
+    wire [3:0] wb_mem_sel_s;
+    wire wb_mem_we_s;
+    wire wb_mem_stb_s;
+    wire [31:0] wb_mem_rdt_s;
+    wire wb_mem_ack_s;
+
+    //  CPU->peripherals
+    wire [31:0] wb_ext_adr;
+    wire [31:0] wb_ext_dat;
+    wire [3:0] wb_ext_sel;
+    wire wb_ext_we;
+    wire wb_ext_stb;
+    wire [31:0] wb_ext_rdt;
+    wire wb_ext_ack;
+    
+    // GPIO
+    wire [4*32-1:0] GPO;
+    wire [4*32-1:0] GPI;
+    assign o_GPO_A = GPO[32*1-1:32*0];
+    assign o_GPO_B = GPO[32*2-1:32*1];
+    assign o_GPO_C = GPO[32*3-1:32*2];
+    assign o_GPO_D = GPO[32*4-1:32*3];
+    assign GPI[32*1-1:32*0] = i_GPI_A;
+    assign GPI[32*2-1:32*1] = i_GPI_B;
+    assign GPI[32*3-1:32*2] = i_GPI_C;
+    assign GPI[32*4-1:32*3] = i_GPI_D;
+
+    cpu #(
+        .sim(sim),
+        .WITH_CSR(WITH_CSR),
+        .rf_width(rf_width)
+    ) cpu (
+        .i_clk(i_clk),
+        .i_rst(rst),
+        .i_timer_irq(timer_irq),
+
+        //Memory interface
+        .o_wb_mem_adr(wb_mem_adr),
+        .o_wb_mem_dat(wb_mem_dat),
+        .o_wb_mem_sel(wb_mem_sel),
+        .o_wb_mem_we(wb_mem_we),
+        .o_wb_mem_stb(wb_mem_stb),
+        .i_wb_mem_rdt(wb_mem_rdt_cpu),
+        .i_wb_mem_ack(wb_mem_ack_cpu),
+
+        //Extension interface
+        .o_wb_ext_adr(wb_ext_adr),
+        .o_wb_ext_dat(wb_ext_dat),
+        .o_wb_ext_sel(wb_ext_sel),
+        .o_wb_ext_we(wb_ext_we),
+        .o_wb_ext_stb(wb_ext_stb),
+        .i_wb_ext_rdt(wb_ext_rdt),
+        .i_wb_ext_ack(wb_ext_ack)
+    );
+
+    generate
+        if (jtag) begin : gen_jtag_wb
+            wire [31:0] wb_jtag_adr;
+            wire [31:0] wb_jtag_dat;
+            wire [3:0] wb_jtag_sel;
+            wire wb_jtag_we;
+            wire wb_jtag_cyc;
+            wire wb_jtag_stb;
+            wire [31:0] wb_jtag_rdt;
+            wire wb_jtag_ack;
+
+            wire [2*32-1:0] wbm_adr_i;
+            wire [2*32-1:0] wbm_dat_i;
+            wire [2*4-1:0] wbm_sel_i;
+            wire [1:0] wbm_we_i;
+            wire [1:0] wbm_cyc_i;
+            wire [1:0] wbm_stb_i;
+            wire [2*3-1:0] wbm_cti_i;
+            wire [2*2-1:0] wbm_bte_i;
+            wire [2*32-1:0] wbm_dat_o;
+            wire [1:0] wbm_ack_o;
+            wire [1:0] wbm_err_o;
+            wire [1:0] wbm_rty_o;
+
+            assign wbm_adr_i = {wb_jtag_adr, wb_mem_adr};
+            assign wbm_dat_i = {wb_jtag_dat, wb_mem_dat};
+            assign wbm_sel_i = {wb_jtag_sel, wb_mem_sel};
+            assign wbm_we_i = {wb_jtag_we, wb_mem_we};
+            assign wbm_cyc_i = {wb_jtag_cyc, wb_mem_stb};
+            assign wbm_stb_i = {wb_jtag_stb, wb_mem_stb};
+            assign wbm_cti_i = 6'b0;
+            assign wbm_bte_i = 4'b0;
+
+            assign wb_mem_rdt_cpu = wbm_dat_o[31:0];
+            assign wb_mem_ack_cpu = wbm_ack_o[0];
+            assign wb_jtag_rdt = wbm_dat_o[63:32];
+            assign wb_jtag_ack = wbm_ack_o[1];
+
+            wb_arbiter #(
+                .dw(32),
+                .aw(32),
+                .num_masters(2)
+            ) wb_mem_arbiter (
+                .wb_clk_i(i_clk),
+                .wb_rst_i(rst_wb),
+                .wbm_adr_i(wbm_adr_i),
+                .wbm_dat_i(wbm_dat_i),
+                .wbm_sel_i(wbm_sel_i),
+                .wbm_we_i(wbm_we_i),
+                .wbm_cyc_i(wbm_cyc_i),
+                .wbm_stb_i(wbm_stb_i),
+                .wbm_cti_i(wbm_cti_i),
+                .wbm_bte_i(wbm_bte_i),
+                .wbm_dat_o(wbm_dat_o),
+                .wbm_ack_o(wbm_ack_o),
+                .wbm_err_o(wbm_err_o),
+                .wbm_rty_o(wbm_rty_o),
+                .wbs_adr_o(wb_mem_adr_s),
+                .wbs_dat_o(wb_mem_dat_s),
+                .wbs_sel_o(wb_mem_sel_s),
+                .wbs_we_o(wb_mem_we_s),
+                .wbs_cyc_o(),
+                .wbs_stb_o(wb_mem_stb_s),
+                .wbs_cti_o(),
+                .wbs_bte_o(),
+                .wbs_dat_i(wb_mem_rdt_s),
+                .wbs_ack_i(wb_mem_ack_s),
+                .wbs_err_i(1'b0),
+                .wbs_rty_i(1'b0)
+            );
+
+            jtag_wb_bridge #(
+                .chain(1)
+            ) jtag_wb (
+                .i_clk(i_clk),
+                .i_rst(i_rst),
+                .o_wb_adr(wb_jtag_adr),
+                .o_wb_dat(wb_jtag_dat),
+                .o_wb_sel(wb_jtag_sel),
+                .o_wb_we(wb_jtag_we),
+                .o_wb_cyc(wb_jtag_cyc),
+                .o_wb_stb(wb_jtag_stb),
+                .i_wb_rdt(wb_jtag_rdt),
+                .i_wb_ack(wb_jtag_ack),
+                .o_cmd_reset(rst_cmd_jtag)
+            );
+        end else begin : gen_no_jtag_wb
+            assign wb_mem_adr_s = wb_mem_adr;
+            assign wb_mem_dat_s = wb_mem_dat;
+            assign wb_mem_sel_s = wb_mem_sel;
+            assign wb_mem_we_s = wb_mem_we;
+            assign wb_mem_stb_s = wb_mem_stb;
+            assign wb_mem_rdt_cpu = wb_mem_rdt_s;
+            assign wb_mem_ack_cpu = wb_mem_ack_s;
+            assign rst_cmd_jtag = 1'b0;
+        end
+    endgenerate
+
+    memory #(
+        .memfile(memfile),
+        .memsize(memsize),
+        .sim(sim)
+    ) memory (
+        .i_clk(i_clk),
+        .i_rst(i_rst),
+        .i_wb_rst(rst_wb),
+        .i_wb_adr(wb_mem_adr_s),
+        .i_wb_dat(wb_mem_dat_s),
+        .i_wb_sel(wb_mem_sel_s),
+        .i_wb_we(wb_mem_we_s),
+        .i_wb_stb(wb_mem_stb_s),
+        .o_wb_rdt(wb_mem_rdt_s),
+        .o_wb_ack(wb_mem_ack_s)
+    );
+
+    mcu_peripherals peripherals (
+        .i_clk(i_clk),
+        .i_rst(rst),
+        .i_wb_adr(wb_ext_adr),
+        .i_wb_dat(wb_ext_dat),
+        .i_wb_sel(wb_ext_sel),
+        .i_wb_we(wb_ext_we),
+        .i_wb_stb(wb_ext_stb),
+        .o_wb_rdt(wb_ext_rdt),
+        .o_wb_ack(wb_ext_ack),
+        // Peripheral IO
+        .i_gpio(GPI),
+        .o_gpio(GPO),
+        .o_timer_irq(timer_irq),
+        .o_core_reset(rst_mem_peripherals)
+    );
+
+endmodule
+
+module cpu #(
+    parameter sim = 1'b0,
+    parameter WITH_CSR = 1,
+    parameter rf_width = 8
+)(
+    input wire i_clk,
+    input wire i_rst,
+    input wire i_timer_irq,
+    // CPU->memory
+    output wire [31:0] o_wb_mem_adr,
+    output wire [31:0] o_wb_mem_dat,
+    output wire [3:0] o_wb_mem_sel,
+    output wire o_wb_mem_we,
+    output wire o_wb_mem_stb,
+    input wire [31:0] i_wb_mem_rdt,
+    input wire i_wb_mem_ack,
+    // CPU->peripherals
+    output wire [31:0] o_wb_ext_adr,
+    output wire [31:0] o_wb_ext_dat,
+    output wire [3:0] o_wb_ext_sel,
+    output wire o_wb_ext_we,
+    output wire o_wb_ext_stb,
+    input wire [31:0] i_wb_ext_rdt,
+    input wire i_wb_ext_ack
+);
+    wire [6+WITH_CSR:0] rf_waddr;
+    wire [rf_width-1:0] rf_wdata;
+    wire rf_wen;
+    wire [6+WITH_CSR:0] rf_raddr;
+    wire [rf_width-1:0] rf_rdata;
+    wire rf_ren;
+
+    // SERV core with mux splitting dbus into mem and ext and
+    // arbiter combining mem and ibus.
+    servile #(
+        .reset_pc(32'h0000_0000),
+        .reset_strategy("MINI"),
+        .rf_width(rf_width),
+        .sim(sim),
+        .with_csr(WITH_CSR),
+        .with_c(0),
+        .with_mdu(0)
+    ) servile (
+        .i_clk(i_clk),
+        .i_rst(i_rst),
+        .i_timer_irq(i_timer_irq),
+
+        .o_wb_mem_adr(o_wb_mem_adr),
+        .o_wb_mem_dat(o_wb_mem_dat),
+        .o_wb_mem_sel(o_wb_mem_sel),
+        .o_wb_mem_we(o_wb_mem_we),
+        .o_wb_mem_stb(o_wb_mem_stb),
+        .i_wb_mem_rdt(i_wb_mem_rdt),
+        .i_wb_mem_ack(i_wb_mem_ack),
+
+        .o_wb_ext_adr(o_wb_ext_adr),
+        .o_wb_ext_dat(o_wb_ext_dat),
+        .o_wb_ext_sel(o_wb_ext_sel),
+        .o_wb_ext_we(o_wb_ext_we),
+        .o_wb_ext_stb(o_wb_ext_stb),
+        .i_wb_ext_rdt(i_wb_ext_rdt),
+        .i_wb_ext_ack(i_wb_ext_ack),
+
+        .o_rf_waddr(rf_waddr),
+        .o_rf_wdata(rf_wdata),
+        .o_rf_wen(rf_wen),
+        .o_rf_raddr(rf_raddr),
+        .o_rf_ren(rf_ren),
+        .i_rf_rdata(rf_rdata)
+    );
+
+    serv_rf_ram #(
+        .width(rf_width),
+        .csr_regs(WITH_CSR*4)
+    ) rf_ram (
+        .i_clk(i_clk),
+        .i_waddr(rf_waddr),
+        .i_wdata(rf_wdata),
+        .i_wen(rf_wen),
+        .i_raddr(rf_raddr),
+        .i_ren(rf_ren),
+        .o_rdata(rf_rdata)
+    );
+endmodule
+
+module memory #(
+    parameter memfile = "",
+    parameter memsize = 8192,
+    parameter sim = 1'b0
+)(
+    input wire i_clk,
+    input wire i_rst,
+    input wire i_wb_rst,
+    input wire [31:0] i_wb_adr,
+    input wire [31:0] i_wb_dat,
+    input wire [3:0] i_wb_sel,
+    input wire i_wb_we,
+    input wire i_wb_stb,
+    output wire [31:0] o_wb_rdt,
+    output wire o_wb_ack
+);
+    localparam mem_depth = memsize/4;
+    localparam mem_aw = `CLOG2(mem_depth);
+
+    reg [31:0] mem [0:mem_depth-1] /* verilator public */;
+    reg [31:0] wb_rdt_r;
+    reg wb_ack_r;
+    wire [mem_aw-1:0] wb_word_adr = i_wb_adr[mem_aw+1:2];
+
+    assign o_wb_rdt = wb_rdt_r;
+    assign o_wb_ack = wb_ack_r;
+
+    always @(posedge i_clk) begin
+        if (i_rst || i_wb_rst) begin
+            wb_ack_r <= 1'b0;
+            wb_rdt_r <= 32'b0;
+        end else begin
+            wb_ack_r <= i_wb_stb & ~wb_ack_r;
+
+            if (i_wb_stb & ~wb_ack_r) begin
+                wb_rdt_r <= mem[wb_word_adr];
+
+                if (i_wb_we) begin
+                    if (i_wb_sel[0]) mem[wb_word_adr][7:0] <= i_wb_dat[7:0];
+                    if (i_wb_sel[1]) mem[wb_word_adr][15:8] <= i_wb_dat[15:8];
+                    if (i_wb_sel[2]) mem[wb_word_adr][23:16] <= i_wb_dat[23:16];
+                    if (i_wb_sel[3]) mem[wb_word_adr][31:24] <= i_wb_dat[31:24];
+                end
+            end
+        end
+    end
+
+    integer i;
+    initial begin
+        if (sim == 1'b1) begin
+            for (i = 0; i < mem_depth; i = i + 1)
+                mem[i] = 32'h00000000;
+        end
+        if (|memfile) begin
+            $display("Preloading %m from %s", memfile);
+            $readmemh(memfile, mem);
+        end
+        wb_rdt_r = 32'b0;
+        wb_ack_r = 1'b0;
+    end
+
+endmodule

rtl/core/mcu_peripherals.v

@@ -0,0 +1,126 @@
+`timescale 1ns/1ps
+
+module mcu_peripherals (
+    input wire i_clk,
+    input wire i_rst,
+
+    input wire [31:0] i_wb_adr,
+    input wire [31:0] i_wb_dat,
+    input wire [3:0] i_wb_sel,
+    input wire i_wb_we,
+    input wire i_wb_stb,
+    output wire [31:0] o_wb_rdt,
+    output wire o_wb_ack,
+
+    input wire [4*32-1:0] i_gpio,
+    output wire [4*32-1:0] o_gpio,
+    output wire o_timer_irq,
+    output wire o_core_reset
+);
+    localparam [31:0] GPIO_BASE_ADDR = 32'h4000_0000;
+    localparam [31:0] GPIO_ADDR_MASK = 32'hFFFF_0000;
+    localparam [31:0] TIMER_BASE_ADDR = 32'h4001_0000;
+    localparam [31:0] TIMER_ADDR_MASK = 32'hFFFF_0000;
+
+    assign o_core_reset = 1'b0;
+
+    wire [2*32-1:0] wbs_adr;
+    wire [2*32-1:0] wbs_dat_w;
+    wire [2*4-1:0] wbs_sel;
+    wire [1:0] wbs_we;
+    wire [1:0] wbs_cyc;
+    wire [1:0] wbs_stb;
+    wire [2*3-1:0] wbs_cti;
+    wire [2*2-1:0] wbs_bte;
+    wire [2*32-1:0] wbs_dat_r;
+    wire [1:0] wbs_ack;
+
+    wire [31:0] gpio_wbs_adr = wbs_adr[0*32 +: 32];
+    wire [31:0] gpio_wbs_dat_w = wbs_dat_w[0*32 +: 32];
+    wire [3:0] gpio_wbs_sel = wbs_sel[0*4 +: 4];
+    wire gpio_wbs_we = wbs_we[0];
+    wire gpio_wbs_cyc = wbs_cyc[0];
+    wire gpio_wbs_stb = wbs_stb[0];
+    wire [31:0] gpio_wbs_dat_r;
+    wire gpio_wbs_ack;
+
+    wire [31:0] timer_wbs_dat_w = wbs_dat_w[1*32 +: 32];
+    wire timer_wbs_we = wbs_we[1];
+    wire timer_wbs_cyc = wbs_cyc[1];
+    wire timer_wbs_stb = wbs_stb[1];
+    wire [31:0] timer_wbs_dat_r;
+    wire timer_wbs_ack;
+
+    wb_mux #(
+        .dw(32),
+        .aw(32),
+        .num_slaves(2),
+        .MATCH_ADDR({TIMER_BASE_ADDR, GPIO_BASE_ADDR}),
+        .MATCH_MASK({TIMER_ADDR_MASK, GPIO_ADDR_MASK})
+    ) ext_mux (
+        .wb_clk_i(i_clk),
+        .wb_rst_i(i_rst),
+
+        .wbm_adr_i(i_wb_adr),
+        .wbm_dat_i(i_wb_dat),
+        .wbm_sel_i(i_wb_sel),
+        .wbm_we_i(i_wb_we),
+        .wbm_cyc_i(i_wb_stb),
+        .wbm_stb_i(i_wb_stb),
+        .wbm_cti_i(3'b000),
+        .wbm_bte_i(2'b00),
+        .wbm_dat_o(o_wb_rdt),
+        .wbm_ack_o(o_wb_ack),
+        .wbm_err_o(),
+        .wbm_rty_o(),
+        
+        .wbs_adr_o(wbs_adr),
+        .wbs_dat_o(wbs_dat_w),
+        .wbs_sel_o(wbs_sel),
+        .wbs_we_o(wbs_we),
+        .wbs_cyc_o(wbs_cyc),
+        .wbs_stb_o(wbs_stb),
+        .wbs_cti_o(wbs_cti),
+        .wbs_bte_o(wbs_bte),
+        .wbs_dat_i(wbs_dat_r),
+        .wbs_ack_i(wbs_ack),
+        .wbs_err_i(2'b00),
+        .wbs_rty_i(2'b00)
+    );
+
+    wb_gpio_banks #(
+        .BASE_ADDR(GPIO_BASE_ADDR),
+        .NUM_BANKS(4)
+    ) gpio (
+        .i_wb_clk(i_clk),
+        .i_wb_rst(i_rst),
+        .i_wb_dat(gpio_wbs_dat_w),
+        .i_wb_adr(gpio_wbs_adr),
+        .i_wb_we(gpio_wbs_we),
+        .i_wb_stb(gpio_wbs_stb & gpio_wbs_cyc),
+        .i_wb_sel(gpio_wbs_sel),
+        .o_wb_rdt(gpio_wbs_dat_r),
+        .o_wb_ack(gpio_wbs_ack),
+        .i_gpio(i_gpio),
+        .o_gpio(o_gpio)
+    );
+
+    assign wbs_dat_r[0*32 +: 32] = gpio_wbs_dat_r;
+    assign wbs_ack[0] = gpio_wbs_ack;
+
+    wb_countdown_timer timer (
+        .i_clk(i_clk),
+        .i_rst(i_rst),
+        .o_irq(o_timer_irq),
+        .i_wb_dat(timer_wbs_dat_w),
+        .o_wb_dat(timer_wbs_dat_r),
+        .i_wb_we(timer_wbs_we),
+        .i_wb_cyc(timer_wbs_cyc),
+        .i_wb_stb(timer_wbs_stb),
+        .o_wb_ack(timer_wbs_ack)
+    );
+
+
+    assign wbs_dat_r[1*32 +: 32] = timer_wbs_dat_r;
+    assign wbs_ack[1] = timer_wbs_ack;
+endmodule

rtl/core/mem_jtag_writable.v

@@ -0,0 +1,91 @@
+`timescale 1ns/1ps
+
+module memory_jtag #(
+	parameter memfile = "",
+    parameter depth = 256,
+	parameter sim = 1'b0,
+    parameter aw = `CLOG2(depth)
+    )(
+        input wire i_clk,
+        input wire i_rst,
+	input wire [aw-1:0]	i_waddr,
+	input wire [7:0] i_wdata,
+	input wire i_wen,
+	input wire [aw-1:0]	i_raddr,
+	output reg [7:0] o_rdata,
+    output wire o_core_reset
+);
+    // The actual memory
+   	reg [7:0] mem [0:depth-1];
+    wire [aw-1:0] mem_adr;
+    assign mem_adr = (i_wen==1'b1) ? i_waddr :
+                     i_raddr;
+    
+    // Second port wishbone
+    wire [31:0] wb_adr;
+    wire [31:0] wb_dat;
+    reg [31:0] wb_rdt;
+    wire [3:0] wb_sel;
+    wire wb_cyc;
+    wire wb_we;
+    wire wb_stb;
+    reg wb_ack;
+    reg wb_req_d;
+    wire cmd_reset;
+    // Driven by JTAG
+    jtag_wb_bridge #(
+        .chain(1),
+        .byte_aligned(1)
+    ) jtag_wb (
+        .i_clk(i_clk),
+        .i_rst(i_rst),
+
+        .o_wb_adr(wb_adr),
+        .o_wb_dat(wb_dat),
+        .o_wb_sel(wb_sel),
+        .o_wb_we(wb_we),
+        .o_wb_cyc(wb_cyc),
+        .o_wb_stb(wb_stb),
+        .i_wb_rdt(wb_rdt),
+        .i_wb_ack(wb_ack),
+        .o_cmd_reset(cmd_reset)
+    );
+
+    assign o_core_reset = cmd_reset;
+
+    // Read/Write
+	always @(posedge i_clk) begin
+        if (i_rst) begin
+            wb_req_d <= 1'b0;
+            wb_ack <= 1'b0;
+            wb_rdt <= 32'h00000000;
+            o_rdata <= 32'h00000000;
+        end else begin
+            if (i_wen) 
+                mem[mem_adr] <= i_wdata;
+            o_rdata <= mem[mem_adr];
+
+            wb_req_d <= wb_stb && wb_cyc;
+            wb_ack <= wb_req_d;
+            if (wb_we && wb_stb && wb_cyc)
+                mem[wb_adr[aw-1:0]] <= wb_dat[7:0];
+            wb_rdt <= {24'h000000, mem[wb_adr[aw-1:0]]};
+        end
+	end
+
+    // Preload memory
+	integer i;
+   	initial begin
+		if(sim==1'b1) begin
+			for (i = 0; i < depth; i = i + 1)
+				mem[i] = 8'h00;
+		end
+		if(|memfile) begin
+			$display("Preloading %m from %s", memfile);
+			$readmemh(memfile, mem);
+		end
+   end
+
+
+
+endmodule

rtl/qerv/LICENSE

@@ -0,0 +1,7 @@
+ISC License
+
+Copyright 2019, Olof Kindgren
+
+Permission to use, copy, modify, and/or distribute this software for any purpose with or without fee is hereby granted, provided that the above copyright notice and this permission notice appear in all copies.
+
+THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

rtl/qerv/qerv_immdec.v

@@ -0,0 +1,152 @@
+// SPDX-License-Identifier: ISC
+`default_nettype none
+module qerv_immdec
+  (
+   input wire 	     i_clk,
+   //State
+   input wire 	     i_cnt_en,
+   input wire 	     i_cnt_done,
+   //Control
+   input wire [3:0]  i_immdec_en,
+   input wire 	     i_csr_imm_en,
+   input wire [3:0]  i_ctrl,
+   output wire [4:0] o_rd_addr,
+   output wire [4:0] o_rs1_addr,
+   output wire [4:0] o_rs2_addr,
+   //Data
+   output wire [3:0] o_csr_imm,
+   output wire [3:0] o_imm,
+   //External
+   input wire	     i_wb_en,
+   input wire [31:7] i_wb_rdt);
+
+   reg [4:0]	     rd_addr;
+   reg [4:0]	     rs1_addr;
+   reg [4:0]	     rs2_addr;
+
+   reg		     i31;
+   reg		     i30;
+   reg		     i29;
+   reg		     i28;
+   reg		     i27;
+   reg		     i26;
+   reg		     i25;
+   reg		     i24;
+   reg		     i23;
+   reg		     i22;
+   reg		     i21;
+   reg		     i20;
+   reg		     i19;
+   reg		     i18;
+   reg		     i17;
+   reg		     i16;
+   reg		     i15;
+   reg		     i14;
+   reg		     i13;
+   reg		     i12;
+   reg		     i11;
+   reg		     i10;
+   reg		     i9;
+   reg		     i8;
+   reg		     i7;
+
+   reg		     i7_2;
+   reg		     i20_2;
+
+   wire		     signbit = i31 & !i_csr_imm_en;
+
+   assign o_csr_imm[3] = i18;
+   assign o_csr_imm[2] = i17;
+   assign o_csr_imm[1] = i16;
+   assign o_csr_imm[0] = i15;
+
+   assign o_rd_addr  = rd_addr;
+   assign o_rs1_addr = rs1_addr;
+   assign o_rs2_addr = rs2_addr;
+   always @(posedge i_clk) begin
+      if (i_wb_en) begin
+	 //Common
+	 i31 <= i_wb_rdt[31];
+
+	 //Bit lane 3
+	 i19 <= i_wb_rdt[19];
+	 i15 <= i_wb_rdt[15];
+	 i20 <= i_wb_rdt[20];
+	 i7  <= i_wb_rdt[7];
+	 i27 <= i_wb_rdt[27];
+	 i23 <= i_wb_rdt[23];
+	 i10 <= i_wb_rdt[10];
+
+	 //Bit lane 2
+	 i22 <= i_wb_rdt[22];
+	 i9  <= i_wb_rdt[ 9];
+	 i26 <= i_wb_rdt[26];
+	 i30 <= i_wb_rdt[30];
+	 i14 <= i_wb_rdt[14];
+	 i18 <= i_wb_rdt[18];
+
+	 //Bit lane 1
+	 i21 <= i_wb_rdt[21];
+	 i8  <= i_wb_rdt[ 8];
+	 i25 <= i_wb_rdt[25];
+	 i29 <= i_wb_rdt[29];
+	 i13 <= i_wb_rdt[13];
+	 i17 <= i_wb_rdt[17];
+
+	 //Bit lane 0
+	 i11 <= i_wb_rdt[11];
+	 i7_2  <= i_wb_rdt[7 ];
+	 i20_2   <= i_wb_rdt[20];
+	 i24   <= i_wb_rdt[24];
+	 i28   <= i_wb_rdt[28];
+	 i12   <= i_wb_rdt[12];
+	 i16   <= i_wb_rdt[16];
+
+         rd_addr  <= i_wb_rdt[11:7];
+         rs1_addr <= i_wb_rdt[19:15];
+         rs2_addr <= i_wb_rdt[24:20];
+      end
+      if (i_cnt_en) begin
+	 //Bit lane 3
+	 i10 <= i27;
+	 i23 <= i27;
+	 i27 <= i_ctrl[2] ? i7 : i_ctrl[1] ? signbit : i20;
+	 i7  <= signbit;
+	 i20 <= i15;
+	 i15 <= i19;
+	 i19 <= i_ctrl[3] ? signbit : i23;
+
+	 //Bit lane 2
+	 i22 <= i26;
+	 i9  <= i26;
+	 i26 <= i30;
+	 i30 <= (i_ctrl[1] | i_ctrl[2]) ? signbit : i14;
+	 i14 <= i18;
+	 i18 <= i_ctrl[3] ? signbit : i22;
+
+	 //Bit lane 1
+	 i21 <= i25;
+	 i8  <= i25;
+	 i25 <= i29;
+	 i29 <= (i_ctrl[1] | i_ctrl[2]) ? signbit : i13;
+	 i13 <= i17;
+	 i17 <= i_ctrl[3] ? signbit : i21;
+
+	 //Bit lane 0
+	 i7_2  <= i11;
+	 i11   <= i28;
+	 i20_2   <= i24;
+	 i24   <= i28;
+	 i28   <= (i_ctrl[1] | i_ctrl[2]) ? signbit : i12;
+	 i12   <= i16;
+	 i16   <= i_ctrl[3] ? signbit : i20_2;
+
+      end
+   end
+
+   assign o_imm[3] = (i_cnt_done ? signbit : (i_ctrl[0] ? i10 : i23));
+   assign o_imm[2] = i_ctrl[0] ? i9 : i22;
+   assign o_imm[1] = i_ctrl[0] ? i8 : i21;
+   assign o_imm[0] = i_ctrl[0] ? i7_2 : i20_2;
+
+endmodule

rtl/qerv/serv_rf_top.v

@@ -0,0 +1,222 @@
+`default_nettype none
+`include "../util/clog2.vh"
+
+module qerv_rf_top
+  #(parameter RESET_PC = 32'd0,
+    /*  COMPRESSED=1: Enable the compressed decoder and allowed misaligned jump of pc
+        COMPRESSED=0: Disable the compressed decoder and does not allow the misaligned jump of pc
+    */
+    parameter [0:0] COMPRESSED = 0,
+    /*
+      ALIGN = 1: Fetch the aligned instruction by making two bus transactions if the misaligned address
+      is given to the instruction bus.
+    */
+    parameter [0:0] ALIGN = COMPRESSED,
+    /* Multiplication and Division Unit
+       This parameter enables the interface for connecting SERV and MDU
+    */
+    parameter [0:0] MDU = 0,
+    /* Register signals before or after the decoder
+       0 : Register after the decoder. Faster but uses more resources
+       1 : (default) Register before the decoder. Slower but uses less resources
+     */
+    parameter PRE_REGISTER = 1,
+    /* Amount of reset applied to design
+       "NONE" : No reset at all. Relies on a POR to set correct initialization
+                 values and that core isn't reset during runtime
+       "MINI" : Standard setting. Resets the minimal amount of FFs needed to
+                 restart execution from the instruction at RESET_PC
+     */
+    parameter RESET_STRATEGY = "MINI",
+    parameter [0:0] DEBUG = 1'b0,
+    parameter WITH_CSR = 1,
+    parameter W        = 1,
+    parameter RF_WIDTH = W * 2,
+	parameter RF_L2D   = `CLOG2((32+(WITH_CSR*4))*32/RF_WIDTH))
+  (
+   input wire 	      clk,
+   input wire 	      i_rst,
+   input wire 	      i_timer_irq,
+`ifdef RISCV_FORMAL
+   output wire 	      rvfi_valid,
+   output wire [63:0] rvfi_order,
+   output wire [31:0] rvfi_insn,
+   output wire 	      rvfi_trap,
+   output wire 	      rvfi_halt,
+   output wire 	      rvfi_intr,
+   output wire [1:0]  rvfi_mode,
+   output wire [1:0]  rvfi_ixl,
+   output wire [4:0]  rvfi_rs1_addr,
+   output wire [4:0]  rvfi_rs2_addr,
+   output wire [31:0] rvfi_rs1_rdata,
+   output wire [31:0] rvfi_rs2_rdata,
+   output wire [4:0]  rvfi_rd_addr,
+   output wire [31:0] rvfi_rd_wdata,
+   output wire [31:0] rvfi_pc_rdata,
+   output wire [31:0] rvfi_pc_wdata,
+   output wire [31:0] rvfi_mem_addr,
+   output wire [3:0]  rvfi_mem_rmask,
+   output wire [3:0]  rvfi_mem_wmask,
+   output wire [31:0] rvfi_mem_rdata,
+   output wire [31:0] rvfi_mem_wdata,
+`endif
+   output wire [31:0] o_ibus_adr,
+   output wire 	      o_ibus_cyc,
+   input wire [31:0]  i_ibus_rdt,
+   input wire 	      i_ibus_ack,
+   output wire [31:0] o_dbus_adr,
+   output wire [31:0] o_dbus_dat,
+   output wire [3:0]  o_dbus_sel,
+   output wire 	      o_dbus_we ,
+   output wire 	      o_dbus_cyc,
+   input wire [31:0]  i_dbus_rdt,
+   input wire 	      i_dbus_ack,
+
+   // Extension
+   output wire [31:0] o_ext_rs1,
+   output wire [31:0] o_ext_rs2,
+   output wire [ 2:0] o_ext_funct3,
+   input  wire [31:0] i_ext_rd,
+   input  wire        i_ext_ready,
+   // MDU
+   output wire        o_mdu_valid);
+
+   localparam CSR_REGS = WITH_CSR*4;
+
+   wire 	      rf_wreq;
+   wire 	      rf_rreq;
+   wire [4+WITH_CSR:0] wreg0;
+   wire [4+WITH_CSR:0] wreg1;
+   wire 	      wen0;
+   wire 	      wen1;
+   wire [W-1:0]	      wdata0;
+   wire [W-1:0]	      wdata1;
+   wire [4+WITH_CSR:0] rreg0;
+   wire [4+WITH_CSR:0] rreg1;
+   wire 	      rf_ready;
+   wire [W-1:0]	      rdata0;
+   wire [W-1:0]	      rdata1;
+
+   wire [RF_L2D-1:0]   waddr;
+   wire [RF_WIDTH-1:0] wdata;
+   wire 	       wen;
+   wire [RF_L2D-1:0]   raddr;
+   wire 	       ren;
+   wire [RF_WIDTH-1:0] rdata;
+
+   serv_rf_ram_if
+     #(.width    (RF_WIDTH),
+       .reset_strategy (RESET_STRATEGY),
+       .csr_regs (CSR_REGS),
+       .W(W))
+   rf_ram_if
+     (.i_clk    (clk),
+      .i_rst    (i_rst),
+      .i_wreq   (rf_wreq),
+      .i_rreq   (rf_rreq),
+      .o_ready  (rf_ready),
+      .i_wreg0  (wreg0),
+      .i_wreg1  (wreg1),
+      .i_wen0   (wen0),
+      .i_wen1   (wen1),
+      .i_wdata0 (wdata0),
+      .i_wdata1 (wdata1),
+      .i_rreg0  (rreg0),
+      .i_rreg1  (rreg1),
+      .o_rdata0 (rdata0),
+      .o_rdata1 (rdata1),
+      .o_waddr  (waddr),
+      .o_wdata  (wdata),
+      .o_wen    (wen),
+      .o_raddr  (raddr),
+      .o_ren    (ren),
+      .i_rdata  (rdata));
+
+   serv_rf_ram
+     #(.width (RF_WIDTH),
+       .csr_regs (CSR_REGS))
+   rf_ram
+     (.i_clk    (clk),
+      .i_waddr (waddr),
+      .i_wdata (wdata),
+      .i_wen   (wen),
+      .i_raddr (raddr),
+      .i_ren    (ren),
+      .o_rdata (rdata));
+
+   qerv_top
+     #(.RESET_PC (RESET_PC),
+       .PRE_REGISTER (PRE_REGISTER),
+       .RESET_STRATEGY (RESET_STRATEGY),
+       .WITH_CSR (WITH_CSR),
+       .DEBUG (DEBUG),
+       .MDU(MDU),
+       .COMPRESSED(COMPRESSED),
+       .ALIGN(ALIGN),
+       .W(W))
+   cpu
+     (
+      .clk      (clk),
+      .i_rst    (i_rst),
+      .i_timer_irq  (i_timer_irq),
+`ifdef RISCV_FORMAL
+      .rvfi_valid     (rvfi_valid    ),
+      .rvfi_order     (rvfi_order    ),
+      .rvfi_insn      (rvfi_insn     ),
+      .rvfi_trap      (rvfi_trap     ),
+      .rvfi_halt      (rvfi_halt     ),
+      .rvfi_intr      (rvfi_intr     ),
+      .rvfi_mode      (rvfi_mode     ),
+      .rvfi_ixl       (rvfi_ixl      ),
+      .rvfi_rs1_addr  (rvfi_rs1_addr ),
+      .rvfi_rs2_addr  (rvfi_rs2_addr ),
+      .rvfi_rs1_rdata (rvfi_rs1_rdata),
+      .rvfi_rs2_rdata (rvfi_rs2_rdata),
+      .rvfi_rd_addr   (rvfi_rd_addr  ),
+      .rvfi_rd_wdata  (rvfi_rd_wdata ),
+      .rvfi_pc_rdata  (rvfi_pc_rdata ),
+      .rvfi_pc_wdata  (rvfi_pc_wdata ),
+      .rvfi_mem_addr  (rvfi_mem_addr ),
+      .rvfi_mem_rmask (rvfi_mem_rmask),
+      .rvfi_mem_wmask (rvfi_mem_wmask),
+      .rvfi_mem_rdata (rvfi_mem_rdata),
+      .rvfi_mem_wdata (rvfi_mem_wdata),
+`endif
+      .o_rf_rreq   (rf_rreq),
+      .o_rf_wreq   (rf_wreq),
+      .i_rf_ready  (rf_ready),
+      .o_wreg0     (wreg0),
+      .o_wreg1     (wreg1),
+      .o_wen0      (wen0),
+      .o_wen1      (wen1),
+      .o_wdata0    (wdata0),
+      .o_wdata1    (wdata1),
+      .o_rreg0     (rreg0),
+      .o_rreg1     (rreg1),
+      .i_rdata0    (rdata0),
+      .i_rdata1    (rdata1),
+
+      .o_ibus_adr   (o_ibus_adr),
+      .o_ibus_cyc   (o_ibus_cyc),
+      .i_ibus_rdt   (i_ibus_rdt),
+      .i_ibus_ack   (i_ibus_ack),
+
+      .o_dbus_adr   (o_dbus_adr),
+      .o_dbus_dat   (o_dbus_dat),
+      .o_dbus_sel   (o_dbus_sel),
+      .o_dbus_we    (o_dbus_we),
+      .o_dbus_cyc   (o_dbus_cyc),
+      .i_dbus_rdt   (i_dbus_rdt),
+      .i_dbus_ack   (i_dbus_ack),
+
+      //Extension
+      .o_ext_funct3 (o_ext_funct3),
+      .i_ext_ready  (i_ext_ready),
+      .i_ext_rd     (i_ext_rd),
+      .o_ext_rs1    (o_ext_rs1),
+      .o_ext_rs2    (o_ext_rs2),
+      //MDU
+      .o_mdu_valid  (o_mdu_valid));
+
+endmodule
+`default_nettype wire

rtl/qerv/serv_synth_wrapper.v

@@ -0,0 +1,136 @@
+`default_nettype none
+`include "../util/clog2.vh"
+
+module serv_synth_wrapper
+  #(
+    /* Register signals before or after the decoder
+     0 : Register after the decoder. Faster but uses more resources
+     1 : (default) Register before the decoder. Slower but uses less resources
+     */
+    parameter PRE_REGISTER = 1,
+    /* Amount of reset applied to design
+       "NONE" : No reset at all. Relies on a POR to set correct initialization
+                 values and that core isn't reset during runtime
+       "MINI" : Standard setting. Resets the minimal amount of FFs needed to
+                 restart execution from the instruction at RESET_PC
+     */
+    parameter RESET_STRATEGY = "MINI",
+    parameter WITH_CSR = 1,
+    parameter RF_WIDTH = 8,
+	parameter RF_L2D   = `CLOG2((32+(WITH_CSR*4))*32/RF_WIDTH))
+  (
+   input wire 		      clk,
+   input wire 		      i_rst,
+   input wire 		      i_timer_irq,
+   output wire [31:0] 	      o_ibus_adr,
+   output wire 		      o_ibus_cyc,
+   input wire [31:0] 	      i_ibus_rdt,
+   input wire 		      i_ibus_ack,
+   output wire [31:0] 	      o_dbus_adr,
+   output wire [31:0] 	      o_dbus_dat,
+   output wire [3:0] 	      o_dbus_sel,
+   output wire 		      o_dbus_we ,
+   output wire 		      o_dbus_cyc,
+   input wire [31:0] 	      i_dbus_rdt,
+   input wire 		      i_dbus_ack,
+
+   output wire [RF_L2D-1:0]   o_waddr,
+   output wire [RF_WIDTH-1:0] o_wdata,
+   output wire 		      o_wen,
+   output wire [RF_L2D-1:0]   o_raddr,
+   input wire [RF_WIDTH-1:0]  i_rdata);
+
+   localparam CSR_REGS = WITH_CSR*4;
+   localparam W = 4;
+
+   wire 	      rf_wreq;
+   wire 	      rf_rreq;
+   wire [4+WITH_CSR:0] wreg0;
+   wire [4+WITH_CSR:0] wreg1;
+   wire 	      wen0;
+   wire 	      wen1;
+   wire [W-1:0]	      wdata0;
+   wire [W-1:0]	      wdata1;
+   wire [4+WITH_CSR:0] rreg0;
+   wire [4+WITH_CSR:0] rreg1;
+   wire 	      rf_ready;
+   wire [W-1:0]	      rdata0;
+   wire [W-1:0]	      rdata1;
+
+   serv_rf_ram_if
+     #(.width    (RF_WIDTH),
+       .reset_strategy (RESET_STRATEGY),
+       .W (W),
+       .csr_regs (CSR_REGS))
+   rf_ram_if
+     (.i_clk    (clk),
+      .i_rst    (i_rst),
+      .i_wreq   (rf_wreq),
+      .i_rreq   (rf_rreq),
+      .o_ready  (rf_ready),
+      .i_wreg0  (wreg0),
+      .i_wreg1  (wreg1),
+      .i_wen0   (wen0),
+      .i_wen1   (wen1),
+      .i_wdata0 (wdata0),
+      .i_wdata1 (wdata1),
+      .i_rreg0  (rreg0),
+      .i_rreg1  (rreg1),
+      .o_rdata0 (rdata0),
+      .o_rdata1 (rdata1),
+      .o_waddr  (o_waddr),
+      .o_wdata  (o_wdata),
+      .o_wen    (o_wen),
+      .o_raddr  (o_raddr),
+      .i_rdata  (i_rdata));
+
+   qerv_top
+     #(.RESET_PC (32'd0),
+       .PRE_REGISTER (PRE_REGISTER),
+       .RESET_STRATEGY (RESET_STRATEGY),
+       .WITH_CSR (WITH_CSR),
+       .W (W),
+       .MDU(1'b0))
+   cpu
+     (
+      .clk      (clk),
+      .i_rst    (i_rst),
+      .i_timer_irq  (i_timer_irq),
+      .o_rf_rreq   (rf_rreq),
+      .o_rf_wreq   (rf_wreq),
+      .i_rf_ready  (rf_ready),
+      .o_wreg0     (wreg0),
+      .o_wreg1     (wreg1),
+      .o_wen0      (wen0),
+      .o_wen1      (wen1),
+      .o_wdata0    (wdata0),
+      .o_wdata1    (wdata1),
+      .o_rreg0     (rreg0),
+      .o_rreg1     (rreg1),
+      .i_rdata0    (rdata0),
+      .i_rdata1    (rdata1),
+
+      .o_ibus_adr   (o_ibus_adr),
+      .o_ibus_cyc   (o_ibus_cyc),
+      .i_ibus_rdt   (i_ibus_rdt),
+      .i_ibus_ack   (i_ibus_ack),
+
+      .o_dbus_adr   (o_dbus_adr),
+      .o_dbus_dat   (o_dbus_dat),
+      .o_dbus_sel   (o_dbus_sel),
+      .o_dbus_we    (o_dbus_we),
+      .o_dbus_cyc   (o_dbus_cyc),
+      .i_dbus_rdt   (i_dbus_rdt),
+      .i_dbus_ack   (i_dbus_ack),
+
+      //Extension
+      .o_ext_funct3 (),
+      .i_ext_ready  (1'b0),
+      .i_ext_rd     (32'd0),
+      .o_ext_rs1    (),
+      .o_ext_rs2    (),
+      //MDU
+      .o_mdu_valid  ());
+
+endmodule
+`default_nettype wire

rtl/qerv/serv_top.v

@@ -0,0 +1,694 @@
+`default_nettype none
+
+module qerv_top
+  #(parameter	    WITH_CSR = 1,
+    parameter	    W = 1,
+    parameter	    B = W-1,
+    parameter	    PRE_REGISTER = 1,
+    parameter	    RESET_STRATEGY = "MINI",
+    parameter	    RESET_PC = 32'd0,
+    parameter [0:0] DEBUG = 1'b0,
+    parameter [0:0] MDU = 1'b0,
+    parameter [0:0] COMPRESSED=0,
+    parameter [0:0] ALIGN = COMPRESSED)
+   (
+   input wire 		      clk,
+   input wire 		      i_rst,
+   input wire 		      i_timer_irq,
+`ifdef RISCV_FORMAL
+   output wire 		      rvfi_valid,
+   output wire [63:0] 	      rvfi_order,
+   output wire [31:0] 	      rvfi_insn,
+   output wire 		      rvfi_trap,
+   output wire 		      rvfi_halt,
+   output wire 		      rvfi_intr,
+   output wire [1:0] 	      rvfi_mode,
+   output wire [1:0] 	      rvfi_ixl,
+   output wire [4:0] 	      rvfi_rs1_addr,
+   output wire [4:0] 	      rvfi_rs2_addr,
+   output wire [31:0] 	      rvfi_rs1_rdata,
+   output wire [31:0] 	      rvfi_rs2_rdata,
+   output wire [4:0] 	      rvfi_rd_addr,
+   output wire [31:0] 	      rvfi_rd_wdata,
+   output wire [31:0] 	      rvfi_pc_rdata,
+   output wire [31:0] 	      rvfi_pc_wdata,
+   output wire [31:0] 	      rvfi_mem_addr,
+   output wire [3:0] 	      rvfi_mem_rmask,
+   output wire [3:0] 	      rvfi_mem_wmask,
+   output wire [31:0] 	      rvfi_mem_rdata,
+   output wire [31:0] 	      rvfi_mem_wdata,
+`endif
+   //RF Interface
+   output wire 		      o_rf_rreq,
+   output wire 		      o_rf_wreq,
+   input wire 		      i_rf_ready,
+   output wire [4+WITH_CSR:0] o_wreg0,
+   output wire [4+WITH_CSR:0] o_wreg1,
+   output wire 		      o_wen0,
+   output wire 		      o_wen1,
+   output wire [B:0] o_wdata0,
+   output wire [B:0] o_wdata1,
+   output wire [4+WITH_CSR:0] o_rreg0,
+   output wire [4+WITH_CSR:0] o_rreg1,
+   input wire  [B:0] i_rdata0,
+   input wire  [B:0] i_rdata1,
+
+   output wire [31:0] 	      o_ibus_adr,
+   output wire 		      o_ibus_cyc,
+   input wire [31:0] 	      i_ibus_rdt,
+   input wire 		      i_ibus_ack,
+   output wire [31:0] 	      o_dbus_adr,
+   output wire [31:0] 	      o_dbus_dat,
+   output wire [3:0] 	      o_dbus_sel,
+   output wire 		      o_dbus_we ,
+   output wire 		      o_dbus_cyc,
+   input wire [31:0] 	      i_dbus_rdt,
+   input wire 		      i_dbus_ack,
+   //Extension
+   output wire [ 2:0] o_ext_funct3,
+   input  wire        i_ext_ready,
+   input wire  [31:0] i_ext_rd,
+   output wire [31:0] o_ext_rs1,
+   output wire [31:0] o_ext_rs2,
+   //MDU
+   output wire        o_mdu_valid);
+
+   wire [4:0]    rd_addr;
+   wire [4:0]    rs1_addr;
+   wire [4:0]    rs2_addr;
+
+   wire [3:0] 	 immdec_ctrl;
+   wire [3:0] 	immdec_en;
+
+   wire          sh_right;
+   wire 	 bne_or_bge;
+   wire 	 cond_branch;
+   wire 	 two_stage_op;
+   wire 	 e_op;
+   wire 	 ebreak;
+   wire 	 branch_op;
+   wire 	 shift_op;
+   wire 	 rd_op;
+   wire   mdu_op;
+
+   wire 	 rd_alu_en;
+   wire 	 rd_csr_en;
+   wire 	 rd_mem_en;
+   wire [B:0]    ctrl_rd;
+   wire [B:0]    alu_rd;
+   wire [B:0]    mem_rd;
+   wire [B:0]    csr_rd;
+   wire 	 mtval_pc;
+
+   wire          ctrl_pc_en;
+   wire          jump;
+   wire          jal_or_jalr;
+   wire          utype;
+   wire 	 mret;
+   wire [B:0]    imm;
+   wire 	 trap;
+   wire 	 pc_rel;
+   wire          iscomp;
+
+   wire          init;
+   wire          cnt_en;
+   wire 	 cnt0to3;
+   wire 	 cnt12to31;
+   wire          cnt0;
+   wire          cnt1;
+   wire          cnt2;
+   wire          cnt3;
+   wire          cnt7;
+   wire          cnt11;
+   wire          cnt12;
+
+   wire 	 cnt_done;
+
+   wire 	 bufreg_en;
+   wire          bufreg_sh_signed;
+   wire 	 bufreg_rs1_en;
+   wire 	 bufreg_imm_en;
+   wire 	 bufreg_clr_lsb;
+   wire [B:0]    bufreg_q;
+   wire [B:0]    bufreg2_q;
+   wire [31:0] dbus_rdt;
+   wire        dbus_ack;
+
+   wire          alu_sub;
+   wire [1:0] 	 alu_bool_op;
+   wire          alu_cmp_eq;
+   wire          alu_cmp_sig;
+   wire          alu_cmp;
+   wire [2:0]    alu_rd_sel;
+
+   wire [B:0]    rs1;
+   wire [B:0]    rs2;
+   wire          rd_en;
+
+   wire [B:0]    op_b;
+   wire          op_b_sel;
+
+   wire          mem_signed;
+   wire          mem_word;
+   wire          mem_half;
+   wire [1:0] 	 mem_bytecnt;
+   wire 	 sh_done;
+
+   wire 	 mem_misalign;
+
+   wire [B:0]    bad_pc;
+
+   wire 	 csr_mstatus_en;
+   wire 	 csr_mie_en;
+   wire 	 csr_mcause_en;
+   wire [1:0]	 csr_source;
+   wire	[B:0]	 csr_imm;
+   wire 	 csr_d_sel;
+   wire 	 csr_en;
+   wire [1:0] 	 csr_addr;
+   wire [B:0]    csr_pc;
+   wire 	 csr_imm_en;
+   wire [B:0]    csr_in;
+   wire [B:0]    rf_csr_out;
+   wire 	 dbus_en;
+
+   wire 	 new_irq;
+
+   wire [1:0]   lsb;
+
+   //verilator lint_off UNUSED
+   wire [31:0] i_wb_rdt;
+   //verilator lint_on UNUSED
+
+   wire [31:0] wb_ibus_adr;
+   wire        wb_ibus_cyc;
+   wire [31:0] wb_ibus_rdt;
+   wire        wb_ibus_ack;
+
+   generate
+      if (ALIGN) begin : gen_align
+         serv_aligner  align
+           (
+            .clk(clk),
+            .rst(i_rst),
+            // serv_rf_top
+            .i_ibus_adr(wb_ibus_adr),
+            .i_ibus_cyc(wb_ibus_cyc),
+            .o_ibus_rdt(wb_ibus_rdt),
+            .o_ibus_ack(wb_ibus_ack),
+            // servant_arbiter
+            .o_wb_ibus_adr(o_ibus_adr),
+            .o_wb_ibus_cyc(o_ibus_cyc),
+            .i_wb_ibus_rdt(i_ibus_rdt),
+            .i_wb_ibus_ack(i_ibus_ack));
+      end else begin : gen_no_align
+         assign  o_ibus_adr  = wb_ibus_adr;
+         assign  o_ibus_cyc  = wb_ibus_cyc;
+         assign  wb_ibus_rdt = i_ibus_rdt;
+         assign  wb_ibus_ack = i_ibus_ack;
+        end
+   endgenerate
+
+   generate
+      if (COMPRESSED) begin : gen_compressed
+         serv_compdec compdec
+           (
+            .i_clk(clk),
+            .i_instr(wb_ibus_rdt),
+            .i_ack(wb_ibus_ack),
+            .o_instr(i_wb_rdt),
+            .o_iscomp(iscomp));
+      end else begin : gen_no_compressed
+         assign i_wb_rdt =  wb_ibus_rdt;
+         assign iscomp   =  1'b0;
+      end
+   endgenerate
+
+   serv_state
+     #(.RESET_STRATEGY (RESET_STRATEGY),
+       .WITH_CSR (WITH_CSR[0:0]),
+       .MDU(MDU),
+       .ALIGN(ALIGN),
+       .W(W))
+   state
+     (
+      .i_clk (clk),
+      .i_rst          (i_rst),
+      //State
+      .i_new_irq      (new_irq),
+      .i_alu_cmp      (alu_cmp),
+      .o_init         (init),
+      .o_cnt_en       (cnt_en),
+      .o_cnt0to3      (cnt0to3),
+      .o_cnt12to31    (cnt12to31),
+      .o_cnt0         (cnt0),
+      .o_cnt1         (cnt1),
+      .o_cnt2         (cnt2),
+      .o_cnt3         (cnt3),
+      .o_cnt7         (cnt7),
+      .o_cnt11        (cnt11),
+      .o_cnt12        (cnt12),
+      .o_cnt_done     (cnt_done),
+      .o_bufreg_en    (bufreg_en),
+      .o_ctrl_pc_en   (ctrl_pc_en),
+      .o_ctrl_jump    (jump),
+      .o_ctrl_trap    (trap),
+      .i_ctrl_misalign(lsb[1]),
+      .i_sh_done      (sh_done),
+      .o_mem_bytecnt  (mem_bytecnt),
+      .i_mem_misalign (mem_misalign),
+      //Control
+      .i_bne_or_bge   (bne_or_bge),
+      .i_cond_branch  (cond_branch),
+      .i_dbus_en      (dbus_en),
+      .i_two_stage_op (two_stage_op),
+      .i_branch_op    (branch_op),
+      .i_shift_op     (shift_op),
+      .i_sh_right     (sh_right),
+      .i_alu_rd_sel1  (alu_rd_sel[1]),
+      .i_rd_alu_en    (rd_alu_en),
+      .i_e_op         (e_op),
+      .i_rd_op        (rd_op),
+      //MDU
+      .i_mdu_op       (mdu_op),
+      .o_mdu_valid    (o_mdu_valid),
+      //Extension
+      .i_mdu_ready    (i_ext_ready),
+      //External
+      .o_dbus_cyc     (o_dbus_cyc),
+      .i_dbus_ack     (i_dbus_ack),
+      .o_ibus_cyc     (wb_ibus_cyc),
+      .i_ibus_ack     (wb_ibus_ack),
+      //RF Interface
+      .o_rf_rreq      (o_rf_rreq),
+      .o_rf_wreq      (o_rf_wreq),
+      .i_rf_ready     (i_rf_ready),
+      .o_rf_rd_en     (rd_en));
+
+   serv_decode
+     #(.PRE_REGISTER (PRE_REGISTER),
+       .MDU(MDU))
+   decode
+     (
+      .clk (clk),
+      //Input
+      .i_wb_rdt           (i_wb_rdt[31:2]),
+      .i_wb_en            (wb_ibus_ack),
+      //To state
+      .o_bne_or_bge       (bne_or_bge),
+      .o_cond_branch      (cond_branch),
+      .o_dbus_en          (dbus_en),
+      .o_e_op             (e_op),
+      .o_ebreak           (ebreak),
+      .o_branch_op        (branch_op),
+      .o_shift_op         (shift_op),
+      .o_rd_op            (rd_op),
+      .o_sh_right         (sh_right),
+      .o_mdu_op           (mdu_op),
+      .o_two_stage_op     (two_stage_op),
+      //Extension
+      .o_ext_funct3       (o_ext_funct3),
+
+      //To bufreg
+      .o_bufreg_rs1_en    (bufreg_rs1_en),
+      .o_bufreg_imm_en    (bufreg_imm_en),
+      .o_bufreg_clr_lsb   (bufreg_clr_lsb),
+      .o_bufreg_sh_signed (bufreg_sh_signed),
+      //To bufreg2
+      .o_op_b_source      (op_b_sel),
+      //To ctrl
+      .o_ctrl_jal_or_jalr (jal_or_jalr),
+      .o_ctrl_utype       (utype),
+      .o_ctrl_pc_rel      (pc_rel),
+      .o_ctrl_mret        (mret),
+      //To alu
+      .o_alu_sub          (alu_sub),
+      .o_alu_bool_op      (alu_bool_op),
+      .o_alu_cmp_eq       (alu_cmp_eq),
+      .o_alu_cmp_sig      (alu_cmp_sig),
+      .o_alu_rd_sel       (alu_rd_sel),
+      //To mem IF
+      .o_mem_cmd          (o_dbus_we),
+      .o_mem_signed       (mem_signed),
+      .o_mem_word         (mem_word),
+      .o_mem_half         (mem_half),
+      //To CSR
+      .o_csr_en           (csr_en),
+      .o_csr_addr         (csr_addr),
+      .o_csr_mstatus_en   (csr_mstatus_en),
+      .o_csr_mie_en       (csr_mie_en),
+      .o_csr_mcause_en    (csr_mcause_en),
+      .o_csr_source       (csr_source),
+      .o_csr_d_sel        (csr_d_sel),
+      .o_csr_imm_en       (csr_imm_en),
+      .o_mtval_pc         (mtval_pc      ),
+      //To top
+      .o_immdec_ctrl      (immdec_ctrl),
+      .o_immdec_en        (immdec_en),
+      //To RF IF
+      .o_rd_mem_en        (rd_mem_en),
+      .o_rd_csr_en        (rd_csr_en),
+      .o_rd_alu_en        (rd_alu_en));
+
+   generate
+   if (W == 1) begin : gen_serv_immdec
+   serv_immdec immdec
+     (
+      .i_clk        (clk),
+      //State
+      .i_cnt_en     (cnt_en),
+      .i_cnt_done   (cnt_done),
+      //Control
+      .i_immdec_en        (immdec_en),
+      .i_csr_imm_en (csr_imm_en),
+      .i_ctrl       (immdec_ctrl),
+      .o_rd_addr    (rd_addr),
+      .o_rs1_addr   (rs1_addr),
+      .o_rs2_addr   (rs2_addr),
+      //Data
+      .o_csr_imm    (csr_imm),
+      .o_imm        (imm),
+      //External
+      .i_wb_en      (wb_ibus_ack),
+      .i_wb_rdt     (i_wb_rdt[31:7]));
+   end else if (W == 4) begin : gen_qerv_immdec
+   qerv_immdec immdec
+     (
+      .i_clk        (clk),
+      //State
+      .i_cnt_en     (cnt_en),
+      .i_cnt_done   (cnt_done),
+      //Control
+      .i_immdec_en        (immdec_en),
+      .i_csr_imm_en (csr_imm_en),
+      .i_ctrl       (immdec_ctrl),
+      .o_rd_addr    (rd_addr),
+      .o_rs1_addr   (rs1_addr),
+      .o_rs2_addr   (rs2_addr),
+      //Data
+      .o_csr_imm    (csr_imm),
+      .o_imm        (imm),
+      //External
+      .i_wb_en      (wb_ibus_ack),
+      .i_wb_rdt     (i_wb_rdt[31:7]));
+   end
+   endgenerate
+
+   serv_bufreg
+      #(.MDU(MDU),
+	.W(W))
+   bufreg
+     (
+      .i_clk    (clk),
+      //State
+      .i_cnt0   (cnt0),
+      .i_cnt1   (cnt1),
+      .i_cnt_done (cnt_done),
+      .i_en     (bufreg_en),
+      .i_init   (init),
+      .i_mdu_op (mdu_op),
+      .o_lsb    (lsb),
+      //Control
+      .i_sh_signed (bufreg_sh_signed),
+      .i_rs1_en    (bufreg_rs1_en),
+      .i_imm_en    (bufreg_imm_en),
+      .i_clr_lsb   (bufreg_clr_lsb),
+      .i_shift_op   (shift_op),
+      .i_right_shift_op (sh_right),
+      .i_shamt (o_dbus_dat[26:24]),
+      //Data
+      .i_rs1    (rs1),
+      .i_imm    (imm),
+      .o_q      (bufreg_q),
+      //External
+      .o_dbus_adr (o_dbus_adr),
+      .o_ext_rs1  (o_ext_rs1));
+
+   serv_bufreg2 #(.W(W)) bufreg2
+     (
+      .i_clk        (clk),
+      //State
+      .i_en         (cnt_en),
+      .i_init       (init),
+      .i_cnt7       (cnt7),
+      .i_cnt_done   (cnt_done),
+      .i_sh_right   (sh_right),
+      .i_lsb        (lsb),
+      .i_bytecnt    (mem_bytecnt),
+      .o_sh_done    (sh_done),
+      //Control
+      .i_op_b_sel   (op_b_sel),
+      .i_shift_op   (shift_op),
+      //Data
+      .i_rs2        (rs2),
+      .i_imm        (imm),
+      .o_op_b       (op_b),
+      .o_q          (bufreg2_q),
+      //External
+      .o_dat        (o_dbus_dat),
+      .i_load       (dbus_ack),
+      .i_dat        (dbus_rdt));
+
+   serv_ctrl
+     #(.RESET_PC (RESET_PC),
+       .RESET_STRATEGY (RESET_STRATEGY),
+       .WITH_CSR (WITH_CSR),
+       .W (W))
+   ctrl
+     (
+      .clk        (clk),
+      .i_rst      (i_rst),
+      //State
+      .i_pc_en    (ctrl_pc_en),
+      .i_cnt12to31 (cnt12to31),
+      .i_cnt0     (cnt0),
+      .i_cnt1     (cnt1),
+      .i_cnt2     (cnt2),
+      //Control
+      .i_jump     (jump),
+      .i_jal_or_jalr (jal_or_jalr),
+      .i_utype    (utype),
+      .i_pc_rel   (pc_rel),
+      .i_trap     (trap | mret),
+      .i_iscomp    (iscomp),
+      //Data
+      .i_imm      (imm),
+      .i_buf      (bufreg_q),
+      .i_csr_pc   (csr_pc),
+      .o_rd       (ctrl_rd),
+      .o_bad_pc   (bad_pc),
+      //External
+      .o_ibus_adr (wb_ibus_adr));
+
+   serv_alu #(.W (W)) alu
+     (
+      .clk        (clk),
+      //State
+      .i_en       (cnt_en),
+      .i_cnt0     (cnt0),
+      .o_cmp      (alu_cmp),
+      //Control
+      .i_sub      (alu_sub),
+      .i_bool_op  (alu_bool_op),
+      .i_cmp_eq   (alu_cmp_eq),
+      .i_cmp_sig  (alu_cmp_sig),
+      .i_rd_sel   (alu_rd_sel),
+      //Data
+      .i_rs1      (rs1),
+      .i_op_b     (op_b),
+      .i_buf      (bufreg_q),
+      .o_rd       (alu_rd));
+
+   serv_rf_if
+     #(.WITH_CSR (WITH_CSR), .W(W))
+   rf_if
+     (//RF interface
+      .i_cnt_en    (cnt_en),
+      .o_wreg0     (o_wreg0),
+      .o_wreg1     (o_wreg1),
+      .o_wen0      (o_wen0),
+      .o_wen1      (o_wen1),
+      .o_wdata0    (o_wdata0),
+      .o_wdata1    (o_wdata1),
+      .o_rreg0     (o_rreg0),
+      .o_rreg1     (o_rreg1),
+      .i_rdata0    (i_rdata0),
+      .i_rdata1    (i_rdata1),
+
+      //Trap interface
+      .i_trap      (trap),
+      .i_mret      (mret),
+      .i_mepc      (wb_ibus_adr[B:0]),
+      .i_mtval_pc  (mtval_pc),
+      .i_bufreg_q  (bufreg_q),
+      .i_bad_pc    (bad_pc),
+      .o_csr_pc    (csr_pc),
+      //CSR write port
+      .i_csr_en    (csr_en),
+      .i_csr_addr  (csr_addr),
+      .i_csr       (csr_in),
+      //RD write port
+      .i_rd_wen    (rd_en),
+      .i_rd_waddr  (rd_addr),
+      .i_ctrl_rd   (ctrl_rd),
+      .i_alu_rd    (alu_rd),
+      .i_rd_alu_en (rd_alu_en),
+      .i_csr_rd    (csr_rd),
+      .i_rd_csr_en (rd_csr_en),
+      .i_mem_rd    (mem_rd),
+      .i_rd_mem_en (rd_mem_en),
+
+      //RS1 read port
+      .i_rs1_raddr (rs1_addr),
+      .o_rs1       (rs1),
+      //RS2 read port
+      .i_rs2_raddr (rs2_addr),
+      .o_rs2       (rs2),
+
+      //CSR read port
+      .o_csr       (rf_csr_out));
+
+   serv_mem_if
+     #(.WITH_CSR (WITH_CSR[0:0]),
+       .W (W))
+   mem_if
+     (
+      .i_clk        (clk),
+      //State
+      .i_bytecnt    (mem_bytecnt),
+      .i_lsb        (lsb),
+      .o_misalign   (mem_misalign),
+      //Control
+      .i_mdu_op     (mdu_op),
+      .i_signed     (mem_signed),
+      .i_word       (mem_word),
+      .i_half       (mem_half),
+      //Data
+      .i_bufreg2_q  (bufreg2_q),
+      .o_rd         (mem_rd),
+      //External interface
+      .o_wb_sel     (o_dbus_sel));
+
+   generate
+      if (|WITH_CSR) begin : gen_csr
+	 serv_csr
+	   #(.RESET_STRATEGY (RESET_STRATEGY),
+	     .W(W))
+	 csr
+	   (
+	    .i_clk        (clk),
+	    .i_rst        (i_rst),
+	    //State
+	    .i_trig_irq   (wb_ibus_ack),
+	    .i_en         (cnt_en),
+	    .i_cnt0to3    (cnt0to3),
+	    .i_cnt3       (cnt3),
+	    .i_cnt7       (cnt7),
+	    .i_cnt11      (cnt11),
+	    .i_cnt12      (cnt12),
+	    .i_cnt_done   (cnt_done),
+	    .i_mem_op     (!mtval_pc),
+	    .i_mtip       (i_timer_irq),
+	    .i_trap       (trap),
+	    .o_new_irq    (new_irq),
+	    //Control
+	    .i_e_op       (e_op),
+	    .i_ebreak     (ebreak),
+	    .i_mem_cmd    (o_dbus_we),
+	    .i_mstatus_en (csr_mstatus_en),
+	    .i_mie_en     (csr_mie_en    ),
+	    .i_mcause_en  (csr_mcause_en ),
+	    .i_csr_source (csr_source),
+	    .i_mret       (mret),
+	    .i_csr_d_sel  (csr_d_sel),
+	    //Data
+	    .i_rf_csr_out (rf_csr_out),
+	    .o_csr_in     (csr_in),
+	    .i_csr_imm    (csr_imm),
+	    .i_rs1        (rs1),
+	    .o_q          (csr_rd));
+      end else begin : gen_no_csr
+	 assign csr_in = {W{1'b0}};
+	 assign csr_rd = {W{1'b0}};
+	 assign new_irq = 1'b0;
+      end
+   endgenerate
+
+   generate
+      if (DEBUG) begin : gen_debug
+	 serv_debug #(.W (W), .RESET_PC (RESET_PC)) debug
+	   (
+`ifdef RISCV_FORMAL
+	    .rvfi_valid     (rvfi_valid    ),
+	    .rvfi_order     (rvfi_order    ),
+	    .rvfi_insn      (rvfi_insn     ),
+	    .rvfi_trap      (rvfi_trap     ),
+	    .rvfi_halt      (rvfi_halt     ),
+	    .rvfi_intr      (rvfi_intr     ),
+	    .rvfi_mode      (rvfi_mode     ),
+	    .rvfi_ixl       (rvfi_ixl      ),
+	    .rvfi_rs1_addr  (rvfi_rs1_addr ),
+	    .rvfi_rs2_addr  (rvfi_rs2_addr ),
+	    .rvfi_rs1_rdata (rvfi_rs1_rdata),
+	    .rvfi_rs2_rdata (rvfi_rs2_rdata),
+	    .rvfi_rd_addr   (rvfi_rd_addr  ),
+	    .rvfi_rd_wdata  (rvfi_rd_wdata ),
+	    .rvfi_pc_rdata  (rvfi_pc_rdata ),
+	    .rvfi_pc_wdata  (rvfi_pc_wdata ),
+	    .rvfi_mem_addr  (rvfi_mem_addr ),
+	    .rvfi_mem_rmask (rvfi_mem_rmask),
+	    .rvfi_mem_wmask (rvfi_mem_wmask),
+	    .rvfi_mem_rdata (rvfi_mem_rdata),
+	    .rvfi_mem_wdata (rvfi_mem_wdata),
+	    .i_dbus_adr     (o_dbus_adr),
+	    .i_dbus_dat     (o_dbus_dat),
+	    .i_dbus_sel     (o_dbus_sel),
+	    .i_dbus_we      (o_dbus_we ),
+	    .i_dbus_rdt     (i_dbus_rdt),
+	    .i_dbus_ack     (i_dbus_ack),
+	    .i_ctrl_pc_en   (ctrl_pc_en),
+	    .rs1            (rs1),
+	    .rs2            (rs2),
+	    .rs1_addr       (rs1_addr),
+	    .rs2_addr       (rs2_addr),
+	    .immdec_en      (immdec_en),
+	    .rd_en          (rd_en),
+	    .trap           (trap),
+	    .i_rf_ready     (i_rf_ready),
+	    .i_ibus_cyc     (o_ibus_cyc),
+	    .two_stage_op   (two_stage_op),
+	    .init           (init),
+	    .i_ibus_adr     (o_ibus_adr),
+`endif
+	    .i_clk            (clk),
+	    .i_rst            (i_rst),
+	    .i_ibus_rdt       (i_ibus_rdt),
+	    .i_ibus_ack       (i_ibus_ack),
+	    .i_rd_addr        (rd_addr       ),
+	    .i_cnt_en         (cnt_en        ),
+	    .i_csr_in         (csr_in        ),
+	    .i_csr_mstatus_en (csr_mstatus_en),
+	    .i_csr_mie_en     (csr_mie_en    ),
+	    .i_csr_mcause_en  (csr_mcause_en ),
+	    .i_csr_en         (csr_en        ),
+	    .i_csr_addr       (csr_addr),
+	    .i_wen0           (o_wen0),
+	    .i_wdata0         (o_wdata0),
+	    .i_cnt_done       (cnt_done));
+      end
+   endgenerate
+
+
+generate
+  if (MDU) begin: gen_mdu
+    assign dbus_rdt = i_ext_ready ? i_ext_rd:i_dbus_rdt;
+    assign dbus_ack = i_dbus_ack | i_ext_ready;
+  end else begin : gen_no_mdu
+    assign dbus_rdt = i_dbus_rdt;
+    assign dbus_ack = i_dbus_ack;
+  end
+  assign o_ext_rs2 = o_dbus_dat;
+endgenerate
+
+endmodule
+`default_nettype wire

rtl/qerv/servile.v

@@ -0,0 +1,279 @@
+/*
+ * servile.v : Top-level for Servile, the SERV convenience wrapper
+ *
+ * SPDX-FileCopyrightText: 2024 Olof Kindgren <olof.kindgren@gmail.com>
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+`default_nettype none
+module servile
+  #(
+    parameter	    width = 1,
+    parameter	    reset_pc = 32'h00000000,
+    parameter	    reset_strategy = "MINI",
+    parameter	    rf_width = 2*width,
+    parameter [0:0] sim = 1'b0,
+    parameter [0:0] debug = 1'b0,
+    parameter [0:0] with_c = 1'b0,
+    parameter [0:0] with_csr = 1'b0,
+    parameter [0:0] with_mdu = 1'b0,
+    //Internally calculated. Do not touch
+    parameter	    B = width-1,
+    parameter	    regs = 32+with_csr*4,
+    parameter	    rf_l2d = $clog2(regs*32/rf_width))
+  (
+   input wire		      i_clk,
+   input wire		      i_rst,
+   input wire		      i_timer_irq,
+
+   //Memory (WB) interface
+   output wire [31:0]	      o_wb_mem_adr,
+   output wire [31:0]	      o_wb_mem_dat,
+   output wire [3:0]	      o_wb_mem_sel,
+   output wire		      o_wb_mem_we ,
+   output wire		      o_wb_mem_stb,
+   input wire [31:0]	      i_wb_mem_rdt,
+   input wire		      i_wb_mem_ack,
+
+   //Extension (WB) interface
+   output wire [31:0]	      o_wb_ext_adr,
+   output wire [31:0]	      o_wb_ext_dat,
+   output wire [3:0]	      o_wb_ext_sel,
+   output wire		      o_wb_ext_we ,
+   output wire		      o_wb_ext_stb,
+   input wire [31:0]	      i_wb_ext_rdt,
+   input wire		      i_wb_ext_ack,
+
+   //RF (SRAM) interface
+   output wire [rf_l2d-1:0]   o_rf_waddr,
+   output wire [rf_width-1:0] o_rf_wdata,
+   output wire		      o_rf_wen,
+   output wire [rf_l2d-1:0]   o_rf_raddr,
+   input wire [rf_width-1:0]  i_rf_rdata,
+   output wire		      o_rf_ren);
+
+
+
+   wire [31:0] 	wb_ibus_adr;
+   wire 	wb_ibus_stb;
+   wire [31:0] 	wb_ibus_rdt;
+   wire 	wb_ibus_ack;
+
+   wire [31:0] 	wb_dbus_adr;
+   wire [31:0] 	wb_dbus_dat;
+   wire [3:0] 	wb_dbus_sel;
+   wire 	wb_dbus_we;
+   wire 	wb_dbus_stb;
+   wire [31:0] 	wb_dbus_rdt;
+   wire 	wb_dbus_ack;
+
+   wire [31:0] 	wb_dmem_adr;
+   wire [31:0] 	wb_dmem_dat;
+   wire [3:0] 	wb_dmem_sel;
+   wire 	wb_dmem_we;
+   wire 	wb_dmem_stb;
+   wire [31:0] 	wb_dmem_rdt;
+   wire 	wb_dmem_ack;
+
+   wire 		   rf_wreq;
+   wire 		   rf_rreq;
+   wire [$clog2(regs)-1:0] wreg0;
+   wire [$clog2(regs)-1:0] wreg1;
+   wire 		   wen0;
+   wire 		   wen1;
+   wire [B:0]		   wdata0;
+   wire [B:0]		   wdata1;
+   wire [$clog2(regs)-1:0] rreg0;
+   wire [$clog2(regs)-1:0] rreg1;
+   wire 		   rf_ready;
+   wire [B:0]		   rdata0;
+   wire [B:0]		   rdata1;
+
+   wire [31:0]		   mdu_rs1;
+   wire [31:0]		   mdu_rs2;
+   wire [ 2:0]		   mdu_op;
+   wire			   mdu_valid;
+   wire [31:0]		   mdu_rd;
+   wire			   mdu_ready;
+
+   servile_mux
+     #(.sim (sim))
+   mux
+     (.i_clk        (i_clk),
+      .i_rst        (i_rst & (reset_strategy != "NONE")),
+
+      .i_wb_cpu_adr (wb_dbus_adr),
+      .i_wb_cpu_dat (wb_dbus_dat),
+      .i_wb_cpu_sel (wb_dbus_sel),
+      .i_wb_cpu_we  (wb_dbus_we),
+      .i_wb_cpu_stb (wb_dbus_stb),
+      .o_wb_cpu_rdt (wb_dbus_rdt),
+      .o_wb_cpu_ack (wb_dbus_ack),
+
+      .o_wb_mem_adr (wb_dmem_adr),
+      .o_wb_mem_dat (wb_dmem_dat),
+      .o_wb_mem_sel (wb_dmem_sel),
+      .o_wb_mem_we  (wb_dmem_we),
+      .o_wb_mem_stb (wb_dmem_stb),
+      .i_wb_mem_rdt (wb_dmem_rdt),
+      .i_wb_mem_ack (wb_dmem_ack),
+
+      .o_wb_ext_adr (o_wb_ext_adr),
+      .o_wb_ext_dat (o_wb_ext_dat),
+      .o_wb_ext_sel (o_wb_ext_sel),
+      .o_wb_ext_we  (o_wb_ext_we),
+      .o_wb_ext_stb (o_wb_ext_stb),
+      .i_wb_ext_rdt (i_wb_ext_rdt),
+      .i_wb_ext_ack (i_wb_ext_ack));
+
+   servile_arbiter arbiter
+     (.i_wb_cpu_dbus_adr (wb_dmem_adr),
+      .i_wb_cpu_dbus_dat (wb_dmem_dat),
+      .i_wb_cpu_dbus_sel (wb_dmem_sel),
+      .i_wb_cpu_dbus_we  (wb_dmem_we ),
+      .i_wb_cpu_dbus_stb (wb_dmem_stb),
+      .o_wb_cpu_dbus_rdt (wb_dmem_rdt),
+      .o_wb_cpu_dbus_ack (wb_dmem_ack),
+
+      .i_wb_cpu_ibus_adr (wb_ibus_adr),
+      .i_wb_cpu_ibus_stb (wb_ibus_stb),
+      .o_wb_cpu_ibus_rdt (wb_ibus_rdt),
+      .o_wb_cpu_ibus_ack (wb_ibus_ack),
+
+      .o_wb_mem_adr (o_wb_mem_adr),
+      .o_wb_mem_dat (o_wb_mem_dat),
+      .o_wb_mem_sel (o_wb_mem_sel),
+      .o_wb_mem_we  (o_wb_mem_we ),
+      .o_wb_mem_stb (o_wb_mem_stb),
+      .i_wb_mem_rdt (i_wb_mem_rdt),
+      .i_wb_mem_ack (i_wb_mem_ack));
+
+
+
+   serv_rf_ram_if
+     #(.width    (rf_width),
+       .W        (width),
+       .reset_strategy (reset_strategy),
+       .csr_regs (with_csr*4))
+   rf_ram_if
+     (.i_clk    (i_clk),
+      .i_rst    (i_rst),
+      //RF IF
+      .i_wreq   (rf_wreq),
+      .i_rreq   (rf_rreq),
+      .o_ready  (rf_ready),
+      .i_wreg0  (wreg0),
+      .i_wreg1  (wreg1),
+      .i_wen0   (wen0),
+      .i_wen1   (wen1),
+      .i_wdata0 (wdata0),
+      .i_wdata1 (wdata1),
+      .i_rreg0  (rreg0),
+      .i_rreg1  (rreg1),
+      .o_rdata0 (rdata0),
+      .o_rdata1 (rdata1),
+      //SRAM IF
+      .o_waddr  (o_rf_waddr),
+      .o_wdata  (o_rf_wdata),
+      .o_wen    (o_rf_wen),
+      .o_raddr  (o_rf_raddr),
+      .o_ren    (o_rf_ren),
+      .i_rdata  (i_rf_rdata));
+
+   generate
+      if (with_mdu) begin : gen_mdu
+	 mdu_top mdu_serv
+	   (.i_clk       (i_clk),
+	    .i_rst       (i_rst),
+	    .i_mdu_rs1   (mdu_rs1),
+	    .i_mdu_rs2   (mdu_rs2),
+	    .i_mdu_op    (mdu_op),
+	    .i_mdu_valid (mdu_valid),
+	    .o_mdu_ready (mdu_ready),
+	    .o_mdu_rd    (mdu_rd));
+      end else begin
+	 assign mdu_ready = 1'b0;
+	 assign mdu_rd = 32'd0;
+      end
+   endgenerate
+
+   qerv_top
+     #(
+       .WITH_CSR       (with_csr?1:0),
+       .W              (width),
+       .PRE_REGISTER   (1'b1),
+       .RESET_STRATEGY (reset_strategy),
+       .RESET_PC       (reset_pc),
+       .DEBUG          (debug),
+       .MDU            (with_mdu),
+       .COMPRESSED     (with_c))
+   cpu
+     (
+      .clk         (i_clk),
+      .i_rst       (i_rst),
+      .i_timer_irq (i_timer_irq),
+
+`ifdef RISCV_FORMAL
+      .rvfi_valid     (),
+      .rvfi_order     (),
+      .rvfi_insn      (),
+      .rvfi_trap      (),
+      .rvfi_halt      (),
+      .rvfi_intr      (),
+      .rvfi_mode      (),
+      .rvfi_ixl       (),
+      .rvfi_rs1_addr  (),
+      .rvfi_rs2_addr  (),
+      .rvfi_rs1_rdata (),
+      .rvfi_rs2_rdata (),
+      .rvfi_rd_addr   (),
+      .rvfi_rd_wdata  (),
+      .rvfi_pc_rdata  (),
+      .rvfi_pc_wdata  (),
+      .rvfi_mem_addr  (),
+      .rvfi_mem_rmask (),
+      .rvfi_mem_wmask (),
+      .rvfi_mem_rdata (),
+      .rvfi_mem_wdata (),
+`endif
+      //RF IF
+      .o_rf_rreq   (rf_rreq),
+      .o_rf_wreq   (rf_wreq),
+      .i_rf_ready  (rf_ready),
+      .o_wreg0     (wreg0),
+      .o_wreg1     (wreg1),
+      .o_wen0      (wen0),
+      .o_wen1      (wen1),
+      .o_wdata0    (wdata0),
+      .o_wdata1    (wdata1),
+      .o_rreg0     (rreg0),
+      .o_rreg1     (rreg1),
+      .i_rdata0    (rdata0),
+      .i_rdata1    (rdata1),
+
+      //Instruction bus
+      .o_ibus_adr  (wb_ibus_adr),
+      .o_ibus_cyc  (wb_ibus_stb),
+      .i_ibus_rdt  (wb_ibus_rdt),
+      .i_ibus_ack  (wb_ibus_ack),
+
+      //Data bus
+      .o_dbus_adr  (wb_dbus_adr),
+      .o_dbus_dat  (wb_dbus_dat),
+      .o_dbus_sel  (wb_dbus_sel),
+      .o_dbus_we   (wb_dbus_we),
+      .o_dbus_cyc  (wb_dbus_stb),
+      .i_dbus_rdt  (wb_dbus_rdt),
+      .i_dbus_ack  (wb_dbus_ack),
+
+      //Extension IF
+      .o_ext_rs1    (mdu_rs1),
+      .o_ext_rs2    (mdu_rs2),
+      .o_ext_funct3 (mdu_op),
+      .i_ext_rd     (mdu_rd),
+      .i_ext_ready  (mdu_ready),
+      //MDU
+      .o_mdu_valid  (mdu_valid));
+
+endmodule
+`default_nettype wire

rtl/qerv/servile_arbiter.v

@@ -0,0 +1,45 @@
+/*
+ * servile_arbiter.v : I/D arbiter for the servile convenience wrapper.
+ *  Relies on the fact that not ibus and dbus are active at the same time.
+ *
+ * SPDX-FileCopyrightText: 2024 Olof Kindgren <olof.kindgren@gmail.com>
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+module servile_arbiter
+  (
+   input wire [31:0]  i_wb_cpu_dbus_adr,
+   input wire [31:0]  i_wb_cpu_dbus_dat,
+   input wire [3:0]   i_wb_cpu_dbus_sel,
+   input wire 	      i_wb_cpu_dbus_we,
+   input wire 	      i_wb_cpu_dbus_stb,
+   output wire [31:0] o_wb_cpu_dbus_rdt,
+   output wire 	      o_wb_cpu_dbus_ack,
+
+   input wire [31:0]  i_wb_cpu_ibus_adr,
+   input wire 	      i_wb_cpu_ibus_stb,
+   output wire [31:0] o_wb_cpu_ibus_rdt,
+   output wire 	      o_wb_cpu_ibus_ack,
+
+   output wire [31:0] o_wb_mem_adr,
+   output wire [31:0] o_wb_mem_dat,
+   output wire [3:0]  o_wb_mem_sel,
+   output wire 	      o_wb_mem_we,
+   output wire 	      o_wb_mem_stb,
+   input wire [31:0]  i_wb_mem_rdt,
+   input wire 	      i_wb_mem_ack);
+
+   assign o_wb_cpu_dbus_rdt = i_wb_mem_rdt;
+   assign o_wb_cpu_dbus_ack = i_wb_mem_ack & !i_wb_cpu_ibus_stb;
+
+   assign o_wb_cpu_ibus_rdt = i_wb_mem_rdt;
+   assign o_wb_cpu_ibus_ack = i_wb_mem_ack & i_wb_cpu_ibus_stb;
+
+   assign o_wb_mem_adr = i_wb_cpu_ibus_stb ? i_wb_cpu_ibus_adr : i_wb_cpu_dbus_adr;
+   assign o_wb_mem_dat = i_wb_cpu_dbus_dat;
+   assign o_wb_mem_sel = i_wb_cpu_dbus_sel;
+   assign o_wb_mem_we  = i_wb_cpu_dbus_we & !i_wb_cpu_ibus_stb;
+   assign o_wb_mem_stb = i_wb_cpu_ibus_stb | i_wb_cpu_dbus_stb;
+
+
+endmodule

rtl/qerv/servile_mux.v

@@ -0,0 +1,100 @@
+/*
+ * servile_mux.v : Simple Wishbone mux for the servile convenience wrapper.
+ *
+ * SPDX-FileCopyrightText: 2024 Olof Kindgren <olof.kindgren@gmail.com>
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+module servile_mux
+  #(parameter [0:0]  sim = 1'b0, //Enable simulation features
+    parameter [31:0] sim_sig_adr = 32'h80000000,
+    parameter [31:0] sim_halt_adr = 32'h90000000)
+   (
+    input wire	       i_clk,
+    input wire	       i_rst,
+
+    input wire [31:0]  i_wb_cpu_adr,
+    input wire [31:0]  i_wb_cpu_dat,
+    input wire [3:0]   i_wb_cpu_sel,
+    input wire	       i_wb_cpu_we,
+    input wire	       i_wb_cpu_stb,
+    output wire [31:0] o_wb_cpu_rdt,
+    output wire	       o_wb_cpu_ack,
+
+    output wire [31:0] o_wb_mem_adr,
+    output wire [31:0] o_wb_mem_dat,
+    output wire [3:0]  o_wb_mem_sel,
+    output wire	       o_wb_mem_we,
+    output wire	       o_wb_mem_stb,
+    input wire [31:0]  i_wb_mem_rdt,
+    input wire	       i_wb_mem_ack,
+
+    output wire [31:0] o_wb_ext_adr,
+    output wire [31:0] o_wb_ext_dat,
+    output wire [3:0]  o_wb_ext_sel,
+    output wire	       o_wb_ext_we,
+    output wire	       o_wb_ext_stb,
+    input wire [31:0]  i_wb_ext_rdt,
+    input wire	       i_wb_ext_ack);
+
+   wire		       sig_en;
+   wire		       halt_en;
+   reg		       sim_ack;
+
+   wire		       ext = (i_wb_cpu_adr[31:30] != 2'b00);
+
+   assign o_wb_cpu_rdt = ext ? i_wb_ext_rdt : i_wb_mem_rdt;
+   assign o_wb_cpu_ack = i_wb_ext_ack | i_wb_mem_ack | sim_ack;
+
+   assign o_wb_mem_adr = i_wb_cpu_adr;
+   assign o_wb_mem_dat = i_wb_cpu_dat;
+   assign o_wb_mem_sel = i_wb_cpu_sel;
+   assign o_wb_mem_we  = i_wb_cpu_we;
+   assign o_wb_mem_stb = i_wb_cpu_stb & !ext & !(sig_en|halt_en);
+
+   assign o_wb_ext_adr = i_wb_cpu_adr;
+   assign o_wb_ext_dat = i_wb_cpu_dat;
+   assign o_wb_ext_sel = i_wb_cpu_sel;
+   assign o_wb_ext_we  = i_wb_cpu_we;
+   assign o_wb_ext_stb = i_wb_cpu_stb & ext & !(sig_en|halt_en);
+
+   generate
+      if (sim) begin
+
+	 integer      f = 0;
+
+	 assign sig_en  = |f & i_wb_cpu_we & (i_wb_cpu_adr == sim_sig_adr);
+	 assign halt_en = i_wb_cpu_we & (i_wb_cpu_adr == sim_halt_adr);
+
+	 reg [1023:0] signature_file;
+
+	 initial
+	   /* verilator lint_off WIDTH */
+	   if ($value$plusargs("signature=%s", signature_file)) begin
+	      $display("Writing signature to %0s", signature_file);
+	      f = $fopen(signature_file, "w");
+	   end
+	 /* verilator lint_on WIDTH */
+
+	 always @(posedge i_clk) begin
+	    sim_ack <= 1'b0;
+	    if (i_wb_cpu_stb & !sim_ack) begin
+	       sim_ack <= sig_en|halt_en;
+	       if (sig_en & (f != 0))
+		 $fwrite(f, "%c", i_wb_cpu_dat[7:0]);
+	       else if(halt_en) begin
+		  $display("Test complete");
+		  $finish;
+	       end
+	    end
+	    if (i_rst)
+	      sim_ack <= 1'b0;
+	 end
+      end else begin
+	 assign sig_en = 1'b0;
+	 assign halt_en = 1'b0;
+	 initial sim_ack = 1'b0;
+      end
+   endgenerate
+
+endmodule

rtl/qerv/servile_rf_mem_if.v

@@ -0,0 +1,77 @@
+/*
+ * servile_rf_mem_if.v : Arbiter to allow a shared SRAM for RF and memory accesses. RF is mapped to the highest 128 bytes of the memory. Requires 8-bit RF accesses.
+ *
+ * SPDX-FileCopyrightText: 2024 Olof Kindgren <olof.kindgren@gmail.com>
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+`default_nettype none
+module servile_rf_mem_if
+  #(//Memory parameters
+    parameter depth = 256,
+    //RF parameters
+    parameter rf_regs = 32,
+    //Internally calculated. Do not touch
+    parameter rf_depth = $clog2(rf_regs*4),
+    parameter aw = $clog2(depth))
+   (
+    input wire		      i_clk,
+    input wire		      i_rst,
+    input wire [rf_depth-1:0] i_waddr,
+    input wire [7:0]	      i_wdata,
+    input wire		      i_wen,
+    input wire [rf_depth-1:0] i_raddr,
+    output wire [7:0]	      o_rdata,
+    input wire		      i_ren,
+
+    output wire [aw-1:0]      o_sram_waddr,
+    output wire [7:0]	      o_sram_wdata,
+    output wire		      o_sram_wen,
+    output wire [aw-1:0]      o_sram_raddr,
+    input wire [7:0]	      i_sram_rdata,
+    output wire		      o_sram_ren,
+
+    input wire [aw-1:2]	      i_wb_adr,
+    input wire [31:0]	      i_wb_dat,
+    input wire [3:0]	      i_wb_sel,
+    input wire		      i_wb_we,
+    input wire		      i_wb_stb,
+    output wire [31:0]	      o_wb_rdt,
+    output reg		      o_wb_ack);
+
+   reg [1:0] 		bsel;
+
+   wire 		wb_en = i_wb_stb & !i_wen & !o_wb_ack;
+
+   wire 		wb_we = i_wb_we & i_wb_sel[bsel];
+
+   wire [aw-1:0] rf_waddr = ~{{aw-rf_depth{1'b0}},i_waddr};
+   wire [aw-1:0] rf_raddr = ~{{aw-rf_depth{1'b0}},i_raddr};
+
+   assign o_sram_waddr = wb_en ? {i_wb_adr[aw-1:2],bsel} : rf_waddr;
+   assign o_sram_wdata = wb_en ? i_wb_dat[bsel*8+:8]     : i_wdata;
+   assign o_sram_wen   = wb_en ? wb_we : i_wen;
+   assign o_sram_raddr = wb_en ? {i_wb_adr[aw-1:2],bsel} : rf_raddr;
+   assign o_sram_ren   = wb_en ? !i_wb_we : i_ren;
+
+   reg [23:0] 		wb_rdt;
+   assign o_wb_rdt = {i_sram_rdata, wb_rdt};
+
+   reg 			regzero;
+   always @(posedge i_clk) begin
+
+      if (wb_en) bsel <= bsel + 2'd1;
+      o_wb_ack <= wb_en & &bsel;
+      if (bsel == 2'b01) wb_rdt[7:0]   <= i_sram_rdata;
+      if (bsel == 2'b10) wb_rdt[15:8]  <= i_sram_rdata;
+      if (bsel == 2'b11) wb_rdt[23:16] <= i_sram_rdata;
+      if (i_rst) begin
+	 bsel <= 2'd0;
+	 o_wb_ack <= 1'b0;
+      end
+      regzero <= &i_raddr[rf_depth-1:2];
+   end
+
+   assign o_rdata = regzero ? 8'd0 : i_sram_rdata;
+
+endmodule

rtl/toplevel/top_generic.v

@@ -1,17 +1,18 @@
 `timescale 1ns/1ps
 
-module top_generic(
+module top_generic #(
+    parameter sim = 0
+)(
     input wire aclk,
     input wire aresetn,
 
     output wire led_green,
     output wire led_red,
 
-    output wire[5:0] r2r
+    output wire[5:0] r2r,
+    output wire[7:0] LED
 );
     `include "conv.vh"
-    assign led_green = 1'b0;
-    assign led_red = 1'b0;
 
     // Clocking
     wire clk_100;
@@ -22,38 +23,60 @@ module top_generic(
         .clk_out_15(clk_15)
     );
 
-    reg [11:0] count;
+    // Reset conditioning for button input:
-    localparam integer DIV_MAX = 100_000 - 1; // 1 ms tick at 100 MHz
+    // - asynchronous assert when button is pressed (aresetn=0)
-    reg [16:0] div_counter = 0;               // enough bits for 100k (2^17=131072)
+    // - synchronous, debounced deassert in clk_15 domain
-    reg [31:0] freq;
+    localparam [17:0] RESET_RELEASE_CYCLES = sim ? 18'd16 : 18'd150000; // ~10 ms @ 15 MHz on hardware
-    always @(posedge clk_15) begin
+    reg [17:0] rst_cnt = 18'd0;
+    reg        sys_reset_r = 1'b1;
+    always @(posedge clk_15 or negedge aresetn) begin
         if (!aresetn) begin
-            div_counter <= 0;
+            rst_cnt <= 18'd0;
-            count <= 0;
+            sys_reset_r <= 1'b1;
-        end else begin
+        end else if (sys_reset_r) begin
-            if (div_counter == DIV_MAX) begin
+            if (rst_cnt == RESET_RELEASE_CYCLES - 1'b1)
-                div_counter <= 0;
+                sys_reset_r <= 1'b0;
-                if (count == 12'd3999)
+            else
-                    count <= 0;               // wrap at 4000
+                rst_cnt <= rst_cnt + 1'b1;
-                else
-                    count <= count + 1'b1;    // increment every 1 ms
-            end else begin
-                div_counter <= div_counter + 1'b1;
-            end
         end
-        freq <= count;
     end
+    wire sys_reset = sys_reset_r;
+    wire sys_resetn = !sys_reset_r;
+
+    wire [31:0] GPIO_A;
+    wire [31:0] GPIO_B;
+    wire [31:0] GPIO_C;
+    wire [31:0] GPIO_D;
+
+    wire test;
+
+    mcu #(
+        .memfile("../sw/sweep/sweep.hex"),
+        .sim(sim),
+        .jtag(1)
+    ) mcu (
+        .i_clk(clk_15),
+        .i_rst(sys_reset),
+        .i_GPI_A(GPIO_A),
+        .i_GPI_B(GPIO_B),
+        .i_GPI_C(GPIO_C),
+        .i_GPI_D(GPIO_D),
+        .o_GPO_A(GPIO_A),
+        .o_GPO_B(GPIO_B),
+        .o_GPO_C(GPIO_C),
+        .o_GPO_D(GPIO_D)
+    );
 
 
     wire [15:0] sin_q15;
     wire clk_en;
     nco_q15 #(
         .CLK_HZ(15_000_000),
-        .FS_HZ(40_000)
+        .FS_HZ(80_000)
     ) nco (
         .clk    (clk_15),
-        .rst_n  (aresetn),
+        .rst_n  (sys_resetn),
-        .freq_hz(freq),
+        .freq_hz(GPIO_A),
         .sin_q15(sin_q15),
         .cos_q15(),
         .clk_en (clk_en)
@@ -64,4 +87,8 @@ module top_generic(
         dac_code <= q15_to_uq16(sin_q15) >> 10;
     end
     assign r2r = dac_code;
+
+    assign LED = GPIO_B[7:0];
+    assign led_green = GPIO_C[0];
+    assign led_red = GPIO_C[1];
 endmodule

rtl/toplevel/top_jtag.v

@@ -1,70 +1,73 @@
-`timescale 1ns/1ps
+module top_jtag(
-
-module top_generic(
     input wire aclk,
     input wire aresetn,
 
     output wire led_green,
     output wire led_red,
+    output wire [7:0] LED,
 
-    output wire[5:0] r2r
+    output wire [5:0] r2r
 );
-
     // Clocking
     wire clk_100;
     wire clk_15;
     assign clk_100 = aclk;
-    clk_gen clocking(
+    clk_gen clk(
         .clk_in(clk_100),
         .clk_out_15(clk_15)
     );
 
+    wire i_rst;
+    assign i_rst = !aresetn;
+
     wire [31:0] wb_adr;
     wire [31:0] wb_dat;
     wire [31:0] wb_rdt;
     wire [3:0] wb_sel;
+    wire wb_cyc;
     wire wb_we;
     wire wb_stb;
     wire wb_ack;
+    wire cmd_reset;
 
-    wire [31:0] GPIO;
+    jtag_wb_bridge #(
-
+        .chain(1)
-    assign led_green = GPIO[0];
+    ) jtag_wb (
-    assign led_red = GPIO[1];
-    assign r2r = GPIO[8:2];
-
-    serving #(
-        .memfile("../sw/blinky/blinky.hex"),
-        .memsize(8192),
-        .sim(1'b0),
-        .RESET_STRATEGY("MINI"),
-        .WITH_CSR(1)
-    ) serv (
         .i_clk(clk_15),
         .i_rst(!aresetn),
-        .i_timer_irq(1'b0),
+
-        .i_wb_rdt(wb_rdt),
-        .i_wb_ack(wb_ack),
         .o_wb_adr(wb_adr),
         .o_wb_dat(wb_dat),
         .o_wb_sel(wb_sel),
         .o_wb_we(wb_we),
-        .o_wb_stb(wb_stb)
+        .o_wb_cyc(wb_cyc),
+        .o_wb_stb(wb_stb),
+        .i_wb_rdt(wb_rdt),
+        .i_wb_ack(wb_ack),
+        .o_cmd_reset(cmd_reset)
     );
 
+    wire [31:0] gpio;
+
     wb_gpio #(
-      .address(32'h40000000)
+        .address(32'h00000000)
-    ) gpio (
+    ) u_wb_gpio (
         .i_wb_clk(clk_15),
-        .i_wb_rst(!aresetn),
+        .i_wb_rst(i_rst),
-        .i_wb_dat(wb_dat),
         .i_wb_adr(wb_adr),
-        .i_wb_we(wb_we),
+        .i_wb_dat(wb_dat),
-        .i_wb_stb(wb_stb),
         .i_wb_sel(wb_sel),
+        .i_wb_we(wb_we),
+        .i_wb_stb(wb_stb & wb_cyc),
+        .i_gpio(gpio),
         .o_wb_rdt(wb_rdt),
         .o_wb_ack(wb_ack),
-        .o_gpio(GPIO)
+        .o_gpio(gpio)
     );
 
-endmodule
+    assign LED = gpio[7:0];
+    assign r2r = gpio[13:8];
+    assign led_green = cmd_reset;
+    assign led_red = 'b0;
+
+endmodule

rtl/wb/jtag_wb_bridge.v

@@ -0,0 +1,538 @@
+`timescale 1 ns/1 ps
+
+module jtag_wb_bridge #(
+  parameter integer chain = 1,
+  // 0: use addr[1:0] for byte lane on 32-bit WB
+  // 1: always use lane 0
+  parameter integer byte_aligned = 0
+)(
+  input  wire        i_clk,
+  input  wire        i_rst,
+
+  output wire [31:0] o_wb_adr,
+  output wire [31:0] o_wb_dat,
+  output wire [3:0]  o_wb_sel,
+  output wire        o_wb_we,
+  output wire        o_wb_cyc,
+  output wire        o_wb_stb,
+  input  wire [31:0] i_wb_rdt,
+  input  wire        i_wb_ack,
+
+  output wire        o_cmd_reset
+);
+
+  // ===========================================================================
+  // JTAG interface (Spartan-6 BSCAN wrapper)
+  // ===========================================================================
+  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;
+
+  localparam integer JTAG_DR_W = 72;
+
+  // 72-bit DR (symmetrical command/response)
+  // Command layout:  [71:64] opcode, [63:32] addr, [31:0] data
+  // Response layout: [71:64] resp_seq, [63:56] status, [55:48] cmd_seq,
+  //                  [47:16] data, [15:8] flags, [7:0] last_op
+  reg [JTAG_DR_W-1:0] jtag_shreg;
+
+  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)
+  );
+
+  wire jtag_async_reset = jtag_reset || i_rst;
+
+  // ===========================================================================
+  // CDC request/response channel (72/72 symmetric)
+  //   Side A: JTAG/TCK domain
+  //   Side B: system/i_clk domain
+  // ===========================================================================
+  wire        a_req_busy;
+  wire        a_req_accepted;
+  wire        a_resp_pulse;
+  wire [JTAG_DR_W-1:0] a_resp_data;
+
+  wire        b_req_pulse;
+  wire [JTAG_DR_W-1:0] b_req_data;
+
+  reg         b_resp_pulse;
+  reg  [JTAG_DR_W-1: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 [JTAG_DR_W-1:0] a_req_data = jtag_shreg;
+
+  cdc_req_resp #(
+    .REQ_W(JTAG_DR_W),
+    .RESP_W(JTAG_DR_W),
+    .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 [JTAG_DR_W-1:0] resp_hold_tck;
+
+  always @(posedge jtag_tck or posedge jtag_async_reset) begin
+    if (jtag_async_reset) begin
+      jtag_shreg    <= {JTAG_DR_W{1'b0}};
+      resp_hold_tck <= {JTAG_DR_W{1'b0}};
+    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[JTAG_DR_W-1:1]};
+      end
+    end
+  end
+
+  // ===========================================================================
+  // System domain: Wishbone master + small command queue + response pending
+  // ===========================================================================
+  // Opcodes
+  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_WRITE32     = 8'h22;
+  localparam [7:0] OP_READ32      = 8'h23;
+  localparam [7:0] OP_PING        = 8'h30;
+  localparam [7:0] OP_CLEAR_FLAGS = 8'h40;
+
+  // Wishbone regs
+  reg        wb_busy;
+  reg [31:0] wb_adr_r;
+  reg [31:0] wb_dat_r;
+  reg [3:0]  wb_sel_r;
+  reg        wb_we_r;
+
+  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;
+
+  // 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 [31:0] act_data;
+  reg [7:0]  act_seq;
+
+  reg        q_valid;
+  reg [7:0]  q_opcode;
+  reg [31:0] q_addr;
+  reg [31:0] q_data;
+  reg [7:0]  q_seq;
+
+  // Response pending buffer (to avoid dropping if resp mailbox busy)
+  reg        resp_pending;
+  reg [JTAG_DR_W-1: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 [JTAG_DR_W-1:0] pack_resp(
+    input [7:0]  resp_seq,
+    input [7:0]  status,
+    input [7:0]  cmd_seq,
+    input [31: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 [31: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 [31: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) || (cmd_opcode == OP_WRITE32);
+
+      // 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[7:0]);
+        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
+
+        OP_WRITE32: begin
+          // launch WB write (full word)
+          wb_busy  <= 1'b1;
+          wb_we_r  <= 1'b1;
+          wb_adr_r <= cmd_addr;
+          wb_sel_r <= 4'b1111;
+          wb_dat_r <= cmd_data;
+        end
+
+        OP_READ32: begin
+          // launch WB read (full word)
+          wb_busy  <= 1'b1;
+          wb_we_r  <= 1'b0;
+          wb_adr_r <= cmd_addr;
+          wb_sel_r <= 4'b1111;
+          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         <= 32'h0000_0000;
+      act_seq          <= 8'h00;
+
+      q_valid          <= 1'b0;
+      q_opcode         <= 8'h00;
+      q_addr           <= 32'h0;
+      q_data           <= 32'h0000_0000;
+      q_seq            <= 8'h00;
+
+      resp_pending     <= 1'b0;
+      resp_pending_word<= {JTAG_DR_W{1'b0}};
+
+      b_resp_pulse     <= 1'b0;
+      b_resp_data      <= {JTAG_DR_W{1'b0}};
+    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[71:64];
+          act_addr   <= b_req_data[63:32];
+          act_data   <= b_req_data[31:0];
+          act_seq    <= cmd_seq_r;
+          // Start it right away
+          start_active_cmd(b_req_data[71:64], b_req_data[63:32], b_req_data[31:0], cmd_seq_r);
+        end else begin
+          // Otherwise enqueue one-deep
+          enqueue_cmd(b_req_data[71:64], b_req_data[63:32], b_req_data[31:0], cmd_seq_r);
+        end
+      end
+
+      // -----------------------------------------------------------------------
+      // Wishbone completion -> create response (but don't drop; buffer pending)
+      // -----------------------------------------------------------------------
+      if (wb_busy && i_wb_ack) begin
+        wb_busy <= 1'b0;
+        wb_we_r <= 1'b0;
+
+        // Determine response data
+        case (act_opcode)
+          OP_READ8: begin
+            resp_pending_word <= pack_resp(
+              resp_seq_r,
+              status_snapshot,
+              act_seq,
+              {24'b0, byte_from_lane(addr_lane, i_wb_rdt)},
+              flags_sticky,
+              act_opcode
+            );
+          end
+          OP_READ32: begin
+            resp_pending_word <= pack_resp(
+              resp_seq_r,
+              status_snapshot,
+              act_seq,
+              i_wb_rdt,
+              flags_sticky,
+              act_opcode
+            );
+          end
+          default: begin
+            // WRITE8/WRITE32: echo written data
+            resp_pending_word <= pack_resp(
+              resp_seq_r,
+              status_snapshot,
+              act_seq,
+              act_data,
+              flags_sticky,
+              act_opcode
+            );
+          end
+        endcase
+        resp_pending <= 1'b1;
+      end
+
+      // -----------------------------------------------------------------------
+      // 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

rtl/wb/wb_arbiter.v

@@ -0,0 +1,101 @@
+/* wb_arbiter. Part of wb_intercon
+ *
+ * ISC License
+ *
+ * Copyright (C) 2013-2019  Olof Kindgren <olof.kindgren@gmail.com>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+/*
+ Wishbone arbiter, burst-compatible
+ Simple round-robin arbiter for multiple Wishbone masters
+ */
+`include "../util/clog2.vh"
+
+module wb_arbiter
+ #(parameter dw = 32,
+   parameter aw = 32,
+   parameter num_hosts = 0,
+   parameter num_masters = num_hosts)
+  (
+   input wire			    wb_clk_i,
+   input wire			    wb_rst_i,
+
+   // Wishbone Master Interface
+   input wire [num_masters*aw-1:0]  wbm_adr_i,
+   input wire [num_masters*dw-1:0]  wbm_dat_i,
+   input wire [num_masters*4-1:0]   wbm_sel_i,
+   input wire [num_masters-1:0]	    wbm_we_i,
+   input wire [num_masters-1:0]	    wbm_cyc_i,
+   input wire [num_masters-1:0]	    wbm_stb_i,
+   input wire [num_masters*3-1:0]   wbm_cti_i,
+   input wire [num_masters*2-1:0]   wbm_bte_i,
+   output wire [num_masters*dw-1:0] wbm_dat_o,
+   output wire [num_masters-1:0]    wbm_ack_o,
+   output wire [num_masters-1:0]    wbm_err_o,
+   output wire [num_masters-1:0]    wbm_rty_o,
+
+   // Wishbone Slave interface
+   output wire [aw-1:0]		    wbs_adr_o,
+   output wire [dw-1:0]		    wbs_dat_o,
+   output wire [3:0]		    wbs_sel_o,
+   output wire			    wbs_we_o,
+   output wire			    wbs_cyc_o,
+   output wire			    wbs_stb_o,
+   output wire [2:0]		    wbs_cti_o,
+   output wire [1:0]		    wbs_bte_o,
+   input wire [dw-1:0]		    wbs_dat_i,
+   input wire			    wbs_ack_i,
+   input wire			    wbs_err_i,
+   input wire			    wbs_rty_i);
+
+
+///////////////////////////////////////////////////////////////////////////////
+// Parameters
+///////////////////////////////////////////////////////////////////////////////
+
+   //Use parameter instead of localparam to work around a bug in Xilinx ISE
+   parameter master_sel_bits = num_masters > 1 ? `CLOG2(num_masters) : 1;
+
+   wire [num_masters-1:0]     grant;
+   wire [master_sel_bits-1:0] master_sel;
+   wire 		      active;
+
+   arbiter
+     #(.NUM_PORTS (num_masters))
+   arbiter0
+     (.clk (wb_clk_i),
+      .rst (wb_rst_i),
+      .request (wbm_cyc_i),
+      .grant (grant),
+      .select (master_sel),
+      .active (active));
+/* verilator lint_off WIDTH */
+   //Mux active master
+   assign wbs_adr_o = wbm_adr_i[master_sel*aw+:aw];
+   assign wbs_dat_o = wbm_dat_i[master_sel*dw+:dw];
+   assign wbs_sel_o = wbm_sel_i[master_sel*4+:4];
+   assign wbs_we_o  = wbm_we_i [master_sel];
+   assign wbs_cyc_o = wbm_cyc_i[master_sel] & active;
+   assign wbs_stb_o = wbm_stb_i[master_sel];
+   assign wbs_cti_o = wbm_cti_i[master_sel*3+:3];
+   assign wbs_bte_o = wbm_bte_i[master_sel*2+:2];
+
+   assign wbm_dat_o = {num_masters{wbs_dat_i}};
+   assign wbm_ack_o = ((wbs_ack_i & active) << master_sel);
+   assign wbm_err_o = ((wbs_err_i & active) << master_sel);
+   assign wbm_rty_o = ((wbs_rty_i & active) << master_sel);
+/* verilator lint_on WIDTH */
+
+endmodule // wb_arbiter

rtl/wb/wb_gpio.v

@@ -8,6 +8,7 @@ module wb_gpio #(
   input  wire [3:0]  i_wb_sel,
   input  wire        i_wb_we,
   input  wire        i_wb_stb,
+  input  wire [31:0] i_gpio,
 
   output reg  [31:0] o_wb_rdt,
   output reg         o_wb_ack,
@@ -34,7 +35,7 @@ module wb_gpio #(
     if (i_wb_rst) begin
       o_wb_rdt <= 32'h0;
     end else if (i_wb_stb && !i_wb_we) begin
-      o_wb_rdt <= o_gpio;
+      o_wb_rdt <= i_gpio;
     end
   end
 
@@ -51,4 +52,4 @@ module wb_gpio #(
     end
   end
 
-endmodule
+endmodule

rtl/wb/wb_gpio_banks.v

@@ -0,0 +1,63 @@
+`default_nettype none
+
+module wb_gpio_banks #(
+  parameter integer NUM_BANKS = 4,
+  parameter [31:0] BASE_ADDR = 32'h8000_0000
+) (
+  input  wire                     i_wb_clk,
+  input  wire                     i_wb_rst,
+  input  wire [31:0]              i_wb_adr,
+  input  wire [31:0]              i_wb_dat,
+  input  wire [3:0]               i_wb_sel,
+  input  wire                     i_wb_we,
+  input  wire                     i_wb_stb,
+  input  wire [NUM_BANKS*32-1:0]  i_gpio,
+  output reg  [31:0]              o_wb_rdt,
+  output reg                      o_wb_ack,
+  output wire [NUM_BANKS*32-1:0]  o_gpio
+);
+
+  wire [NUM_BANKS-1:0]           bank_sel;
+  wire [NUM_BANKS-1:0]           bank_stb;
+  wire [NUM_BANKS*32-1:0]        bank_rdt;
+  wire [NUM_BANKS-1:0]           bank_ack;
+
+  genvar gi;
+  generate
+    for (gi = 0; gi < NUM_BANKS; gi = gi + 1) begin : gen_gpio
+      localparam [31:0] BANK_ADDR = BASE_ADDR + (gi * 4);
+
+      assign bank_sel[gi] = (i_wb_adr == BANK_ADDR);
+      assign bank_stb[gi] = i_wb_stb & bank_sel[gi];
+
+      wb_gpio #(
+        .address(BANK_ADDR)
+      ) u_gpio (
+        .i_wb_clk(i_wb_clk),
+        .i_wb_rst(i_wb_rst),
+        .i_wb_adr(i_wb_adr),
+        .i_wb_dat(i_wb_dat),
+        .i_wb_sel(i_wb_sel),
+        .i_wb_we(i_wb_we),
+        .i_wb_stb(bank_stb[gi]),
+        .i_gpio(i_gpio[gi*32 +: 32]),
+        .o_wb_rdt(bank_rdt[gi*32 +: 32]),
+        .o_wb_ack(bank_ack[gi]),
+        .o_gpio(o_gpio[gi*32 +: 32])
+      );
+    end
+  endgenerate
+
+  integer bi;
+  always @* begin
+    o_wb_rdt = 32'h0000_0000;
+    o_wb_ack = 1'b0;
+    for (bi = 0; bi < NUM_BANKS; bi = bi + 1) begin
+      if (bank_sel[bi]) begin
+        o_wb_rdt = bank_rdt[bi*32 +: 32];
+        o_wb_ack = bank_ack[bi];
+      end
+    end
+  end
+
+endmodule

rtl/wb/wb_mux.v

@@ -0,0 +1,145 @@
+/* wb_mux. Part of wb_intercon
+ *
+ * ISC License
+ *
+ * Copyright (C) 2013-2019  Olof Kindgren <olof.kindgren@gmail.com>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+/*
+ Wishbone multiplexer, burst-compatible
+
+ Simple mux with an arbitrary number of slaves.
+
+ The parameters MATCH_ADDR and MATCH_MASK are flattened arrays
+ aw*NUM_SLAVES sized arrays that are used to calculate the
+ active slave. slave i is selected when
+ (wb_adr_i & MATCH_MASK[(i+1)*aw-1:i*aw] is equal to
+ MATCH_ADDR[(i+1)*aw-1:i*aw]
+ If several regions are overlapping, the slave with the lowest
+ index is selected. This can be used to have fallback
+ functionality in the last slave, in case no other slave was
+ selected.
+
+ If no match is found, the wishbone transaction will stall and
+ an external watchdog is required to abort the transaction
+
+ Todo:
+ Registered master/slave connections
+ Rewrite with System Verilog 2D arrays when tools support them
+*/
+`include "../util/clog2.vh"
+
+module wb_mux
+  #(parameter dw = 32,        // Data width
+    parameter aw = 32,        // Address width
+    parameter num_devices = 2, // Number of devices
+    parameter num_slaves = num_devices, // Number of devices (deprecated)
+    parameter [num_slaves*aw-1:0] MATCH_ADDR = 0,
+    parameter [num_slaves*aw-1:0] MATCH_MASK = 0)
+
+   (
+    input wire			    wb_clk_i,
+    input wire			    wb_rst_i,
+
+    // Master Interface
+    input wire [aw-1:0]		    wbm_adr_i,
+    input wire [dw-1:0]		    wbm_dat_i,
+    input wire [3:0]		    wbm_sel_i,
+    input wire			    wbm_we_i,
+    input wire			    wbm_cyc_i,
+    input wire			    wbm_stb_i,
+    input wire [2:0]		    wbm_cti_i,
+    input wire [1:0]		    wbm_bte_i,
+    output wire [dw-1:0]	    wbm_dat_o,
+    output wire			    wbm_ack_o,
+    output wire			    wbm_err_o,
+    output wire			    wbm_rty_o,
+    // Wishbone Slave interface
+    output wire [num_slaves*aw-1:0] wbs_adr_o,
+    output wire [num_slaves*dw-1:0] wbs_dat_o,
+    output wire [num_slaves*4-1:0]  wbs_sel_o,
+    output wire [num_slaves-1:0]    wbs_we_o,
+    output wire [num_slaves-1:0]    wbs_cyc_o,
+    output wire [num_slaves-1:0]    wbs_stb_o,
+    output wire [num_slaves*3-1:0]  wbs_cti_o,
+    output wire [num_slaves*2-1:0]  wbs_bte_o,
+    input wire [num_slaves*dw-1:0]  wbs_dat_i,
+    input wire [num_slaves-1:0]	    wbs_ack_i,
+    input wire [num_slaves-1:0]	    wbs_err_i,
+    input wire [num_slaves-1:0]	    wbs_rty_i);
+
+///////////////////////////////////////////////////////////////////////////////
+// Master/slave connection
+///////////////////////////////////////////////////////////////////////////////
+
+   //Use parameter instead of localparam to work around a bug in Xilinx ISE
+   parameter slave_sel_bits = num_slaves > 1 ? `CLOG2(num_slaves) : 1;
+
+   reg  			 wbm_err;
+   wire [slave_sel_bits-1:0] 	 slave_sel;
+   wire [num_slaves-1:0] 	 match;
+
+   genvar 			 idx;
+
+   generate
+      for(idx=0; idx<num_slaves ; idx=idx+1) begin : addr_match
+	 assign match[idx] = (wbm_adr_i & MATCH_MASK[idx*aw+:aw]) == MATCH_ADDR[idx*aw+:aw];
+      end
+   endgenerate
+
+//
+// Find First 1 - Start from MSB and count downwards, returns 0 when no bit set
+//
+   function [slave_sel_bits-1:0] ff1;
+      input [num_slaves-1:0] in;
+      integer 		     i;
+
+      begin
+	 ff1 = 0;
+	 for (i = num_slaves-1; i >= 0; i=i-1) begin
+	    if (in[i])
+/* verilator lint_off WIDTH */
+	      ff1 = i;
+/* verilator lint_on WIDTH */
+	 end
+      end
+   endfunction
+
+   assign slave_sel = ff1(match);
+
+   always @(posedge wb_clk_i)
+     wbm_err <= wbm_cyc_i & !(|match);
+
+   assign wbs_adr_o = {num_slaves{wbm_adr_i}};
+   assign wbs_dat_o = {num_slaves{wbm_dat_i}};
+   assign wbs_sel_o = {num_slaves{wbm_sel_i}};
+   assign wbs_we_o  = {num_slaves{wbm_we_i}};
+/* verilator lint_off WIDTH */
+
+   // Expand master CYC to slave bus width before shifting to one-hot select.
+   // Shifting a 1-bit signal would otherwise zero out all but slave 0.
+   assign wbs_cyc_o = match & ({num_slaves{wbm_cyc_i}} << slave_sel);
+/* verilator lint_on WIDTH */
+   assign wbs_stb_o = {num_slaves{wbm_stb_i}};
+
+   assign wbs_cti_o = {num_slaves{wbm_cti_i}};
+   assign wbs_bte_o = {num_slaves{wbm_bte_i}};
+
+   assign wbm_dat_o = wbs_dat_i[slave_sel*dw+:dw];
+   assign wbm_ack_o = wbs_ack_i[slave_sel];
+   assign wbm_err_o = wbs_err_i[slave_sel] | wbm_err;
+   assign wbm_rty_o = wbs_rty_i[slave_sel];
+
+endmodule

rtl/wb/wb_timer.v

@@ -0,0 +1,74 @@
+`timescale 1ns/1ps
+
+module wb_countdown_timer #(
+    parameter WIDTH   = 32,  // counter width (<=32 makes bus mapping easy)
+    parameter DIVIDER = 0    // optional prescaler: tick every 2^DIVIDER cycles
+)(
+    input  wire         i_clk,
+    input  wire         i_rst,
+    output reg          o_irq,
+
+    input  wire [31:0]  i_wb_dat,
+    output reg  [31:0]  o_wb_dat,
+    input  wire         i_wb_we,
+    input  wire         i_wb_cyc,
+    input  wire         i_wb_stb,
+    output wire         o_wb_ack
+);
+
+    // One-cycle acknowledge on any valid WB access
+    // (classic, zero-wait-state peripheral)
+    assign o_wb_ack = i_wb_cyc & i_wb_stb;
+
+    // Internal countdown and prescaler
+    reg [WIDTH-1:0] counter;
+    reg [DIVIDER:0] presc;   // enough bits to count up to 2^DIVIDER-1
+    wire tick = (DIVIDER == 0) ? 1'b1 : (presc[DIVIDER] == 1'b1);
+
+    // Readback: expose the current counter value
+    always @(*) begin
+        o_wb_dat = 32'd0;
+        o_wb_dat[WIDTH-1:0] = counter;
+    end
+
+    // Main logic
+    always @(posedge i_clk) begin
+        if (i_rst) begin
+            counter <= {WIDTH{1'b0}};
+            presc   <= { (DIVIDER+1){1'b0} };
+            o_irq   <= 1'b0;
+        end else begin
+            // Default prescaler behavior
+            if (DIVIDER != 0) begin
+                if (counter != 0 && !o_irq)
+                    presc <= presc + 1'b1;
+                else
+                    presc <= { (DIVIDER+1){1'b0} };
+            end
+
+            // Wishbone write: load counter and clear IRQ
+            if (o_wb_ack && i_wb_we) begin
+                counter <= i_wb_dat[WIDTH-1:0];
+                o_irq   <= 1'b0;
+
+                // reset prescaler on (re)start or stop
+                presc   <= { (DIVIDER+1){1'b0} };
+
+            end else begin
+                // Countdown when running (counter>0), not already IRQ'd
+                if (!o_irq && counter != 0) begin
+                    if (tick) begin
+                        if (counter == 1) begin
+                            counter <= {WIDTH{1'b0}};
+                            o_irq   <= 1'b1; // sticky until next write
+                            presc   <= { (DIVIDER+1){1'b0} };
+                        end else begin
+                            counter <= counter - 1'b1;
+                        end
+                    end
+                end
+            end
+        end
+    end
+
+endmodule

scripts/hex_to_mif.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 """Convert a simple .hex image to a plain .mif-style binary file.
 
-Default output format matches build/mem_8kx8b.mif in this repo:
+Default output format is 32-bit words:
 - one binary word per line
 - no header
 """
@@ -57,8 +57,8 @@ def main() -> None:
     parser.add_argument(
         "--word-bytes",
         type=int,
-        default=1,
+        default=4,
-        help="Bytes per output word (default: 1)",
+        help="Bytes per output word (default: 4)",
     )
     parser.add_argument(
         "--little-endian",

scripts/planahead.sh

@@ -1,4 +1,5 @@
 #!/bin/bash
 cd build
-. /opt/packages/xilinx/ISE/14.7/ISE_DS/settings64.sh
+. /opt/Xilinx/14.7/ISE_DS/settings64.sh
-planAhead -mode gui -source ../scripts/planahead.tcl 
+Xephyr :1 -screen 1600x900 &
+DISPLAY=:1 planAhead -mode batch -source ../scripts/planahead.tcl 

scripts/planahead.tcl

@@ -4,4 +4,5 @@ add_files -norecurse ../out/synth/synth.ngc
 import_files -force -norecurse
 import_files -fileset constrs_1 -force -norecurse ../boards/mimas_v1/constraints.ucf
 import_as_run -run impl_1 -twx ../out/synth/timing.twx ../out/synth/synth.ncd
-open_run impl_1
+open_run impl_1
+start_gui

sim/overrides/jtag_if.v

@@ -0,0 +1,196 @@
+`timescale 1ns/1ps
+
+// =============================================================================
+// JTAG interface (simulation override)
+// Behavioral SVF player for simple USER-chain simulation.
+//
+// Supported SVF commands (line-oriented, uppercase recommended):
+//   - SIR <n> TDI (<hex>);
+//   - SDR <n> TDI (<hex>);
+//   - RUNTEST <n> TCK;
+//   - STATE RESET;
+//   - STATE IDLE;
+// Other lines are ignored.
+// =============================================================================
+module jtag_if #(
+    parameter integer chain = 1,
+    parameter [8*256-1:0] SVF_FILE = "write_jtag.svf",
+    parameter integer TCK_HALF_PERIOD_NS = 50,
+    parameter [31:0] USER_IR_OPCODE = 32'h00000002
+)(
+    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 fd;
+    integer got;
+    integer nbits;
+    integer cycles;
+    integer i;
+    integer line_no;
+    integer scan_bits;
+
+    reg [8*1024-1:0] line;
+    reg [8*32-1:0] cmd;
+    reg [8*32-1:0] arg1;
+    reg [8*256-1:0] svf_file_path;
+    reg [4095:0] scan_data;
+    reg [31:0] current_ir;
+    reg selected;
+
+    // Keep linters quiet: TDO is not consumed by this simple player.
+    wire _unused_tdo;
+    assign _unused_tdo = i_tdo;
+
+    task pulse_tck;
+    begin
+        #TCK_HALF_PERIOD_NS o_tck = 1'b1;
+        #TCK_HALF_PERIOD_NS o_tck = 1'b0;
+    end
+    endtask
+
+    task pulse_drck_shift;
+        input bit_in;
+    begin
+        o_tdi = bit_in;
+        o_shift = 1'b1;
+        #TCK_HALF_PERIOD_NS begin
+            o_tck = 1'b1;
+            o_drck = 1'b1;
+        end
+        #TCK_HALF_PERIOD_NS begin
+            o_tck = 1'b0;
+            o_drck = 1'b0;
+        end
+    end
+    endtask
+
+    task pulse_capture;
+    begin
+        o_capture = 1'b1;
+        #TCK_HALF_PERIOD_NS begin
+            o_tck = 1'b1;
+            o_drck = 1'b1;
+        end
+        #TCK_HALF_PERIOD_NS begin
+            o_tck = 1'b0;
+            o_drck = 1'b0;
+            o_capture = 1'b0;
+        end
+    end
+    endtask
+
+    task pulse_update;
+    begin
+        o_shift = 1'b0;
+        #TCK_HALF_PERIOD_NS o_update = 1'b1;
+        #TCK_HALF_PERIOD_NS o_update = 1'b0;
+    end
+    endtask
+
+    initial begin
+        // Default output levels
+        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'b0;
+        selected = 1'b0;
+        current_ir = 32'h0;
+        line_no = 0;
+        svf_file_path = SVF_FILE;
+
+        // Deterministic startup reset pulse before consuming SVF.
+        o_reset = 1'b1;
+        #TCK_HALF_PERIOD_NS o_tck = 1'b1;
+        #TCK_HALF_PERIOD_NS o_tck = 1'b0;
+        #TCK_HALF_PERIOD_NS o_tck = 1'b1;
+        #TCK_HALF_PERIOD_NS o_tck = 1'b0;
+        o_reset = 1'b0;
+
+        fd = $fopen(svf_file_path, "r");
+        if (fd == 0) begin
+            $display("jtag_if(sim): could not open SVF file '%0s'", svf_file_path);
+        end else begin
+            while (!$feof(fd)) begin
+                line = {8*1024{1'b0}};
+                got = $fgets(line, fd);
+                line_no = line_no + 1;
+                if (got > 0) begin
+                cmd = {8*32{1'b0}};
+                arg1 = {8*32{1'b0}};
+                scan_data = {4096{1'b0}};
+                nbits = 0;
+                cycles = 0;
+
+                got = $sscanf(line, "%s", cmd);
+                if (got < 1) begin
+                    // Empty line
+                end else if (cmd == "!") begin
+                    // Comment line
+                end else if (cmd == "SIR") begin
+                    got = $sscanf(line, "SIR %d TDI (%h);", nbits, scan_data);
+                    if (got == 2) begin
+                        current_ir = scan_data[31:0];
+                        selected = (current_ir == USER_IR_OPCODE);
+                        o_sel = selected;
+
+                        // Emit TCK pulses for IR shift activity.
+                        scan_bits = nbits;
+                        if (scan_bits > 4096) scan_bits = 4096;
+                        for (i = 0; i < scan_bits; i = i + 1) begin
+                            o_tdi = scan_data[i];
+                            pulse_tck;
+                        end
+                    end
+                end else if (cmd == "SDR") begin
+                    got = $sscanf(line, "SDR %d TDI (%h);", nbits, scan_data);
+                    if (got == 2) begin
+                        if (selected) begin
+                            pulse_capture;
+                        end
+                        scan_bits = nbits;
+                        if (scan_bits > 4096) scan_bits = 4096;
+                        for (i = 0; i < scan_bits; i = i + 1) begin
+                            pulse_drck_shift(scan_data[i]);
+                        end
+                        if (selected) begin
+                            pulse_update;
+                        end
+                    end
+                end else if (cmd == "RUNTEST") begin
+                    got = $sscanf(line, "RUNTEST %d TCK;", cycles);
+                    if (got == 1) begin
+                        o_runtest = 1'b1;
+                        for (i = 0; i < cycles; i = i + 1)
+                            pulse_tck;
+                        o_runtest = 1'b0;
+                    end
+                end else if (cmd == "STATE") begin
+                    got = $sscanf(line, "STATE %s", arg1);
+                    if (got == 1) begin
+                        if (arg1 == "RESET;") begin
+                            o_reset = 1'b1;
+                            #TCK_HALF_PERIOD_NS o_reset = 1'b0;
+                            selected = 1'b0;
+                            o_sel = 1'b0;
+                        end
+                    end
+                end
+                end
+            end
+            $fclose(fd);
+        end
+    end
+endmodule

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_serving.v

@@ -58,9 +58,10 @@ module tb_serving();
         .i_wb_we(wb_we),
         .i_wb_stb(wb_stb),
         .i_wb_sel(wb_sel),
+        .i_gpio(GPIO),
         .o_wb_rdt(wb_rdt),
         .o_wb_ack(wb_ack),
         .o_gpio(GPIO)
     );
 
-endmodule
+endmodule

sim/tb/tb_top_generic.v

@@ -0,0 +1,45 @@
+`timescale 1ns/1ps
+
+module tb_top_generic();
+    reg aclk;
+    reg aresetn;
+
+    wire led_green;
+    wire led_red;
+    wire [5:0] r2r;
+    wire [7:0] LED;
+
+    // 100 MHz board input clock
+    initial aclk = 1'b0;
+    always #33.33 aclk = ~aclk;
+
+    // Hold reset low, then release
+    initial begin
+        aresetn = 1'b0;
+        #200;
+        aresetn = 1'b1;
+    end
+
+    top_generic #(
+        .sim(1)
+    ) dut (
+        .aclk(aclk),
+        .aresetn(aresetn),
+        .led_green(led_green),
+        .led_red(led_red),
+        .r2r(r2r),
+        .LED(LED)
+    );
+
+    // Ensure firmware path resolves from repository root when simulating.
+    defparam dut.mcu.memfile = "sw/sweep/sweep.hex";
+
+    initial begin
+        $dumpfile("out.vcd");
+        $dumpvars(0, tb_top_generic);
+
+        // Let firmware run for a while.
+        #2_000_000;
+        $finish;
+    end
+endmodule

sim/tb/tb_wb_timer.v

@@ -0,0 +1,157 @@
+`timescale 1ns/1ps
+
+module tb_wb_timer();
+    localparam integer CLK_HALF_NS = 5;
+
+    reg clk;
+    reg rst;
+
+    reg  [31:0] i_wb_dat;
+    wire [31:0] o_wb_dat;
+    reg         i_wb_we;
+    reg         i_wb_cyc;
+    reg         i_wb_stb;
+    wire        o_wb_ack;
+    wire        o_irq;
+
+    integer errors;
+
+    wb_countdown_timer #(
+        .WIDTH(8),
+        .DIVIDER(0)
+    ) dut (
+        .i_clk   (clk),
+        .i_rst   (rst),
+        .o_irq   (o_irq),
+        .i_wb_dat(i_wb_dat),
+        .o_wb_dat(o_wb_dat),
+        .i_wb_we (i_wb_we),
+        .i_wb_cyc(i_wb_cyc),
+        .i_wb_stb(i_wb_stb),
+        .o_wb_ack(o_wb_ack)
+    );
+
+    always #CLK_HALF_NS clk = ~clk;
+
+    task wb_write;
+        input [31:0] data;
+        begin
+            @(negedge clk);
+            i_wb_dat = data;
+            i_wb_we  = 1'b1;
+            i_wb_cyc = 1'b1;
+            i_wb_stb = 1'b1;
+
+            #1;
+            if (!o_wb_ack) begin
+                $display("FAIL: WB write ACK not asserted when cyc/stb high");
+                errors = errors + 1;
+            end
+
+            @(negedge clk);
+            i_wb_we  = 1'b0;
+            i_wb_cyc = 1'b0;
+            i_wb_stb = 1'b0;
+        end
+    endtask
+
+    task wb_read_check;
+        input [31:0] expected;
+        begin
+            @(negedge clk);
+            i_wb_we  = 1'b0;
+            i_wb_cyc = 1'b1;
+            i_wb_stb = 1'b1;
+
+            #1;
+            if (!o_wb_ack) begin
+                $display("FAIL: WB read ACK not asserted when cyc/stb high");
+                errors = errors + 1;
+            end
+
+            if (o_wb_dat !== expected) begin
+                $display("FAIL: WB read mismatch. got=0x%08h expected=0x%08h", o_wb_dat, expected);
+                errors = errors + 1;
+            end
+
+            @(negedge clk);
+            i_wb_cyc = 1'b0;
+            i_wb_stb = 1'b0;
+        end
+    endtask
+
+    task wait_irq_with_timeout;
+        input integer max_cycles;
+        integer k;
+        begin
+            k = 0;
+            while ((o_irq !== 1'b1) && (k < max_cycles)) begin
+                @(posedge clk);
+                k = k + 1;
+            end
+
+            if (o_irq !== 1'b1) begin
+                $display("FAIL: IRQ did not assert within %0d cycles", max_cycles);
+                errors = errors + 1;
+            end
+        end
+    endtask
+
+    initial begin
+        $dumpfile("out.vcd");
+        $dumpvars;
+        
+        clk      = 1'b0;
+        rst      = 1'b1;
+        i_wb_dat = 32'd0;
+        i_wb_we  = 1'b0;
+        i_wb_cyc = 1'b0;
+        i_wb_stb = 1'b0;
+        errors   = 0;
+
+        $display("---------------------------------------");
+        $display("Testbench: wb_countdown_timer");
+        $display("---------------------------------------");
+
+        repeat (3) @(posedge clk);
+        rst = 1'b0;
+        repeat (1) @(posedge clk);
+
+        if (o_irq !== 1'b0) begin
+            $display("FAIL: IRQ should be low after reset");
+            errors = errors + 1;
+        end
+        wb_read_check(32'd0);
+
+        // Load timer and check readback at a deterministic point
+        wb_write(32'd5);
+        @(posedge clk);
+        wb_read_check(32'd4);
+
+        // Wait until expiration, then verify sticky IRQ behavior
+        wait_irq_with_timeout(6);
+        wb_read_check(32'd0);
+
+        repeat (2) @(posedge clk);
+        if (o_irq !== 1'b1) begin
+            $display("FAIL: IRQ should remain sticky until next write");
+            errors = errors + 1;
+        end
+
+        // A new write must clear IRQ and restart the counter
+        wb_write(32'd2);
+        if (o_irq !== 1'b0) begin
+            $display("FAIL: IRQ should clear on write");
+            errors = errors + 1;
+        end
+
+        wait_irq_with_timeout(3);
+
+        if (errors == 0)
+            $display("PASS: wb_timer basic functionality verified");
+        else
+            $display("FAIL: wb_timer testbench found %0d error(s)", errors);
+
+        $finish;
+    end
+endmodule

sw/blinky/blinky.c

@@ -1,22 +0,0 @@
-#include <stdint.h>
-
-#define GPIO_BASE 0x40000000u
-
-static volatile uint32_t * const gpio = (volatile uint32_t *)GPIO_BASE;
-
-static void delay(volatile uint32_t ticks){
-    while (ticks--) {
-        __asm__ volatile ("nop");
-    }
-}
-
-int main(void)
-{
-    uint32_t v = 0;
-
-    for (;;) {
-        *gpio = v;
-        v++;
-        delay(20000u);
-    }
-}

sw/blinky/start.s

@@ -1,23 +0,0 @@
-.section .text.init
-.globl _start
-.type _start, @function
-
-_start:
-    la sp, __stack_top
-
-    # Zero .bss
-    la t0, __bss_start
-    la t1, __bss_end
-1:
-    bgeu t0, t1, 2f
-    sw zero, 0(t0)
-    addi t0, t0, 4
-    j 1b
-
-2:
-    call main
-
-3:
-    j 3b
-
-.size _start, .-_start

sw/sweep/Makefile

@@ -5,8 +5,8 @@ OBJCOPY := $(TOOLCHAIN_PREFIX)objcopy
 OBJDUMP := $(TOOLCHAIN_PREFIX)objdump
 SIZE    := $(TOOLCHAIN_PREFIX)size
 
-TARGET  := blinky
+TARGET  := sweep
-SRCS_C  := blinky.c
+SRCS_C  := sweep.c
 SRCS_S  := start.s
 OBJS    := $(SRCS_C:.c=.o) $(SRCS_S:.s=.o)
 
@@ -35,7 +35,7 @@ $(TARGET).bin: $(TARGET).elf
 	$(OBJCOPY) -O binary $< $@
 
 $(TARGET).hex: $(TARGET).bin
-	hexdump -v -e '1/1 "%02x\n"' $< > $@
+	hexdump -v -e '1/4 "%08x\n"' $< > $@
 
 $(TARGET).coe: $(TARGET).hex
 	$(HEX_TO_COE) $< $@

sw/sweep/link.ld

@@ -3,7 +3,7 @@ ENTRY(_start)
 
 MEMORY
 {
-  RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 8K
+  RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 8192 
 }
 
 SECTIONS
@@ -24,6 +24,8 @@ SECTIONS
   {
     __bss_start = .;
     *(.bss .bss.*)
+    *(.sbss .sbss.*)
+    *(.scommon)
     *(COMMON)
     __bss_end = .;
   } > RAM

sw/sweep/start.s

@@ -0,0 +1,99 @@
+.section .text.init
+.globl _start
+.type _start, @function
+_start:
+    la sp, __stack_top
+
+    # Zero .bss
+    la t0, __bss_start
+    la t1, __bss_end
+1:
+    bgeu t0, t1, 2f
+    sw zero, 0(t0)
+    addi t0, t0, 4
+    j 1b
+
+2:
+    call main
+
+3:
+    j 3b
+
+.size _start, .-_start
+
+
+.section .text
+.globl trap_entry
+.type trap_entry, @function
+trap_entry:
+    # Save full integer context (except x0/x2) because an interrupt can
+    # preempt code with live values in any register, not just caller-saved.
+    addi sp, sp, -128
+    sw   ra, 124(sp)
+    sw   gp, 120(sp)
+    sw   tp, 116(sp)
+    sw   t0, 112(sp)
+    sw   t1, 108(sp)
+    sw   t2, 104(sp)
+    sw   s0, 100(sp)
+    sw   s1,  96(sp)
+    sw   a0,  92(sp)
+    sw   a1,  88(sp)
+    sw   a2,  84(sp)
+    sw   a3,  80(sp)
+    sw   a4,  76(sp)
+    sw   a5,  72(sp)
+    sw   a6,  68(sp)
+    sw   a7,  64(sp)
+    sw   s2,  60(sp)
+    sw   s3,  56(sp)
+    sw   s4,  52(sp)
+    sw   s5,  48(sp)
+    sw   s6,  44(sp)
+    sw   s7,  40(sp)
+    sw   s8,  36(sp)
+    sw   s9,  32(sp)
+    sw   s10, 28(sp)
+    sw   s11, 24(sp)
+    sw   t3,  20(sp)
+    sw   t4,  16(sp)
+    sw   t5,  12(sp)
+    sw   t6,   8(sp)
+
+    csrr t0, mcause
+    li   t1, 0x80000007      # machine timer interrupt (RV32)
+    bne  t0, t1, 1f
+    call timer_isr           # C function that ACKs/clears the timer so i_timer_irq goes low
+1:
+    lw   t6,   8(sp)
+    lw   t5,  12(sp)
+    lw   t4,  16(sp)
+    lw   t3,  20(sp)
+    lw   s11, 24(sp)
+    lw   s10, 28(sp)
+    lw   s9,  32(sp)
+    lw   s8,  36(sp)
+    lw   s7,  40(sp)
+    lw   s6,  44(sp)
+    lw   s5,  48(sp)
+    lw   s4,  52(sp)
+    lw   s3,  56(sp)
+    lw   s2,  60(sp)
+    lw   a7,  64(sp)
+    lw   a6,  68(sp)
+    lw   a5,  72(sp)
+    lw   a4,  76(sp)
+    lw   a3,  80(sp)
+    lw   a2,  84(sp)
+    lw   a1,  88(sp)
+    lw   a0,  92(sp)
+    lw   s1,  96(sp)
+    lw   s0, 100(sp)
+    lw   t2, 104(sp)
+    lw   t1, 108(sp)
+    lw   t0, 112(sp)
+    lw   tp, 116(sp)
+    lw   gp, 120(sp)
+    lw   ra, 124(sp)
+    addi sp, sp, 128
+    mret

sw/sweep/sweep.c

@@ -0,0 +1,45 @@
+#include <stdint.h>
+
+#define GPIO_BASE   0x40000000u
+static volatile uint32_t * const R_FREQ     = (volatile uint32_t *)(GPIO_BASE+0);
+static volatile uint32_t * const LEDS       = (volatile uint32_t *)(GPIO_BASE+4);
+static volatile uint32_t * const LEDGR      = (volatile uint32_t *)(GPIO_BASE+8);
+
+#define TIMER_BASE  0x40010000u
+static volatile uint32_t * const TIMER      = (volatile uint32_t *)(TIMER_BASE+0);
+
+#define MSTATUS_MIE (1u << 3)
+#define MIE_MTIE    (1u << 7)
+
+extern void trap_entry();
+
+static inline void irq_init() {
+    /* mtvec first */
+    asm volatile ("csrw mtvec, %0" :: "r"(trap_entry));
+
+    /* enable machine timer interrupt */
+    asm volatile ("csrs mie, %0" :: "r"(MIE_MTIE));
+
+    /* global enable last */
+    asm volatile ("csrs mstatus, %0" :: "r"(MSTATUS_MIE));
+}
+
+void timer_isr(){
+    static int set = 0;
+    *TIMER = 1840000*2;
+    *LEDGR = ~(*LEDGR);
+}
+
+void main(){
+    irq_init();
+
+    *LEDGR = 3;
+    *TIMER = 1840000*2;
+
+    for(;;){
+        for(int i=1000; i<10000; i++){
+            *R_FREQ = i;
+            for(int j=0; j<80; j++) asm volatile("nop");
+        }
+    }
+}

tools/.gitignore

@@ -0,0 +1,2 @@
+*.o
+test

tools/Makefile

@@ -0,0 +1,32 @@
+TOOLCHAIN_PREFIX ?= 
+
+CC      := $(TOOLCHAIN_PREFIX)g++
+
+TARGET  := test
+SRCS_C  := 
+SRCS_CPP:= test.cpp digilent_jtag.cpp argparse.cpp
+OBJS    := $(SRCS_C:.c=.o) $(SRCS_CPP:.cpp=.o)
+
+ADEPT_LIBDIR := /opt/packages/digilent.adept.runtime_2.27.9-x86_64/lib64
+
+CFLAGS     := 
+ASFLAGS    :=
+LDFLAGS    := -L$(ADEPT_LIBDIR) -Wl,--disable-new-dtags -Wl,-rpath,$(ADEPT_LIBDIR)
+LIBS 	   := -ldjtg -ldmgr -ldpcomm -ldabs -ldftd2xx
+
+
+.PHONY: all clean size
+
+all: $(TARGET)
+
+$(TARGET): $(OBJS)
+	$(CC) $(LDFLAGS) -o $@ $(OBJS) $(LIBS)
+
+%.o: %.c
+	$(CC) $(CFLAGS) -c -o $@ $<
+
+%.o: %.cpp
+	$(CC) $(CFLAGS) -c -o $@ $<
+
+clean:
+	rm -f $(TARGET) $(OBJS)

tools/argparse.cpp

@@ -0,0 +1,285 @@
+#include "argparse.hpp"
+
+#include <cerrno>
+#include <climits>
+#include <cstdio>
+#include <cstdlib>
+#include <sstream>
+
+ArgParser::ArgParser(std::string program_name)
+    : program_name_(std::move(program_name)) {}
+
+void ArgParser::addString(const std::string &name,
+                          const std::string &default_value,
+                          const std::string &help,
+                          bool required,
+                          const std::string &short_name) {
+    order_.push_back(name);
+    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,
+                       const std::string &short_name) {
+    order_.push_back(name);
+    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) {
+    for (int i = 1; i < argc; ++i) {
+        std::string token(argv[i]);
+        if (token == "--help" || token == "-h") {
+            if (error) {
+                *error = "help";
+            }
+            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;
+            }
+            return false;
+        }
+
+        std::string key;
+        std::string value;
+        size_t eq = token.find('=');
+        if (eq == std::string::npos) {
+            key = token.substr(2);
+        } else {
+            key = token.substr(2, eq - 2);
+            value = token.substr(eq + 1);
+        }
+
+        auto m = meta_.find(key);
+        if (m == meta_.end()) {
+            if (error) {
+                *error = "Unknown option: --" + key;
+            }
+            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 if (m->second.type == OptionType::kInt) {
+            errno = 0;
+            char *endp = nullptr;
+            long parsed = std::strtol(value.c_str(), &endp, 0);
+            if (errno != 0 || endp == value.c_str() || *endp != '\0' ||
+                parsed < INT_MIN || parsed > INT_MAX) {
+                if (error) {
+                    *error = "Invalid integer for --" + key + ": " + value;
+                }
+                return false;
+            }
+            int_values_[key] = static_cast<int>(parsed);
+            provided_[key] = true;
+        }
+    }
+
+    for (const auto &key : order_) {
+        auto m = meta_.find(key);
+        if (m != meta_.end() && m->second.required && !has(key)) {
+            if (error) {
+                *error = "Missing required option: --" + key;
+            }
+            return false;
+        }
+    }
+
+    return true;
+}
+
+bool ArgParser::has(const std::string &name) const {
+    auto p = provided_.find(name);
+    return p != provided_.end() && p->second;
+}
+
+std::string ArgParser::getString(const std::string &name) const {
+    auto it = string_values_.find(name);
+    if (it == string_values_.end()) {
+        return std::string();
+    }
+    return it->second;
+}
+
+int ArgParser::getInt(const std::string &name) const {
+    auto it = int_values_.find(name);
+    if (it == int_values_.end()) {
+        return 0;
+    }
+    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";
+    oss << "Options:\n";
+    oss << "  -h, --help                 Show this help\n";
+    for (const auto &key : order_) {
+        auto m = meta_.find(key);
+        if (m == meta_.end()) {
+            continue;
+        }
+
+        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)";
+        }
+        oss << "\n";
+        oss << "      " << m->second.help;
+        if (m->second.type == OptionType::kString) {
+            auto s = string_values_.find(key);
+            if (s != string_values_.end()) {
+                oss << " [default: '" << s->second << "']";
+            }
+        } else {
+            if (m->second.type == OptionType::kInt) {
+                auto iv = int_values_.find(key);
+                if (iv != int_values_.end()) {
+                    oss << " [default: " << iv->second << "]";
+                }
+            }
+        }
+        oss << "\n";
+    }
+    return oss.str();
+}

tools/argparse.hpp

@@ -0,0 +1,74 @@
+#pragma once
+
+#include <cstdint>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+class ArgParser {
+public:
+    struct StringOption {
+        std::string name;
+        std::string default_value;
+        std::string help;
+        bool required;
+    };
+
+    struct IntOption {
+        std::string name;
+        int default_value;
+        std::string help;
+        bool required;
+    };
+
+    explicit ArgParser(std::string program_name);
+
+    void addString(const std::string &name,
+                   const std::string &default_value,
+                   const std::string &help,
+                   bool required = false,
+                   const std::string &short_name = "");
+
+    void addInt(const std::string &name,
+                int default_value,
+                const std::string &help,
+                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,
+        kFlag
+    };
+
+    struct OptionMeta {
+        OptionType type;
+        std::string help;
+        bool required;
+        std::string short_name;
+    };
+
+    std::string program_name_;
+    std::vector<std::string> order_;
+    std::unordered_map<std::string, OptionMeta> meta_;
+
+    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/digilent_jtag.cpp

@@ -0,0 +1,276 @@
+#include "digilent_jtag.hpp"
+
+#include <algorithm>
+#include <cstring>
+
+#include <digilent/adept/dmgr.h>
+#include <digilent/adept/djtg.h>
+
+namespace {
+
+constexpr int kDefaultIrBits = 6;
+
+std::string ercToString(ERC erc) {
+    char code[cchErcMax] = {0};
+    char msg[cchErcMsgMax] = {0};
+    if (DmgrSzFromErc(erc, code, msg)) {
+        return std::string(code) + ": " + msg;
+    }
+    return "ERC=" + std::to_string(erc);
+}
+
+inline uint8_t getBit(const uint8_t* packed_bits, int bit_idx) {
+    return static_cast<uint8_t>((packed_bits[bit_idx / 8] >> (bit_idx % 8)) & 0x1u);
+}
+
+inline void setBit(uint8_t* packed_bits, int bit_idx, uint8_t bit) {
+    const uint8_t mask = static_cast<uint8_t>(1u << (bit_idx % 8));
+    if (bit & 0x1u) {
+        packed_bits[bit_idx / 8] |= mask;
+    } else {
+        packed_bits[bit_idx / 8] &= static_cast<uint8_t>(~mask);
+    }
+}
+
+} // namespace
+
+DigilentJtag::DigilentJtag() : hif_(hifInvalid), enabled_port_(-1), last_error_() {}
+
+DigilentJtag::~DigilentJtag() { close(); }
+
+bool DigilentJtag::open(int port) {
+    close();
+
+    int count = 0;
+    if (!DmgrEnumDevices(&count)) {
+        return setErrorFromDmgr("DmgrEnumDevices");
+    }
+    if (count <= 0) {
+        return setError("open: no Digilent devices found");
+    }
+
+    DVC dvc{};
+    if (!DmgrGetDvc(0, &dvc)) {
+        return setErrorFromDmgr("DmgrGetDvc");
+    }
+
+    return open(std::string(dvc.szConn), port);
+}
+
+bool DigilentJtag::open(const std::string& selector, int port) {
+    close();
+
+    if (selector.empty()) {
+        return setError("open: selector is empty");
+    }
+
+    std::vector<char> sel(selector.begin(), selector.end());
+    sel.push_back('\0');
+
+    if (!DmgrOpen(&hif_, sel.data())) {
+        hif_ = hifInvalid;
+        return setErrorFromDmgr("DmgrOpen");
+    }
+
+    if (!DjtgEnableEx(hif_, static_cast<INT32>(port))) {
+        if (!DjtgEnable(hif_)) {
+            DmgrClose(hif_);
+            hif_ = hifInvalid;
+            return setErrorFromDmgr("DjtgEnableEx/DjtgEnable");
+        }
+        enabled_port_ = 0;
+    } else {
+        enabled_port_ = port;
+    }
+
+    last_error_.clear();
+    return true;
+}
+
+void DigilentJtag::close() {
+    if (hif_ != hifInvalid) {
+        (void)DjtgDisable(hif_);
+        (void)DmgrClose(hif_);
+    }
+    hif_ = hifInvalid;
+    enabled_port_ = -1;
+}
+
+bool DigilentJtag::isOpen() const { return hif_ != hifInvalid; }
+
+HIF DigilentJtag::handle() const { return hif_; }
+
+bool DigilentJtag::setSpeed(uint32_t requested_hz, uint32_t* actual_hz) {
+    if (!isOpen()) {
+        return setError("setSpeed: device not open");
+    }
+
+    DWORD actual = 0;
+    if (!DjtgSetSpeed(hif_, static_cast<DWORD>(requested_hz), &actual)) {
+        return setErrorFromDmgr("DjtgSetSpeed");
+    }
+    if (actual_hz) {
+        *actual_hz = static_cast<uint32_t>(actual);
+    }
+    last_error_.clear();
+    return true;
+}
+
+bool DigilentJtag::setChain(int chain, int ir_bits) {
+    uint32_t opcode = 0;
+    if (chain == 1) {
+        opcode = 0x02; // USER1 on Spartan-6
+    } else if (chain == 2) {
+        opcode = 0x03; // USER2 on Spartan-6
+    } else {
+        return setError("setChain: unsupported chain index (expected 1 or 2)");
+    }
+    return setInstruction(opcode, ir_bits);
+}
+
+bool DigilentJtag::setInstruction(uint32_t instruction, int ir_bits) {
+    if (!isOpen()) {
+        return setError("setInstruction: device not open");
+    }
+    if (ir_bits <= 0 || ir_bits > 64) {
+        return setError("setInstruction: ir_bits out of range");
+    }
+
+    // Force Test-Logic-Reset, then RTI.
+    uint8_t tlr = 0x3f; // 6 ones
+    if (!putTmsBits(&tlr, 6)) return false;
+    uint8_t rti = 0x00; // one zero
+    if (!putTmsBits(&rti, 1)) return false;
+
+    // RTI -> Shift-IR : 1,1,0,0 (LSB-first 0b0011)
+    uint8_t to_shift_ir = 0x03;
+    if (!putTmsBits(&to_shift_ir, 4)) return false;
+
+    std::vector<uint8_t> tx(static_cast<size_t>((ir_bits + 7) / 8), 0);
+    for (int i = 0; i < ir_bits && i < 64; ++i) {
+        if ((instruction >> i) & 0x1u) {
+            tx[static_cast<size_t>(i / 8)] |= static_cast<uint8_t>(1u << (i % 8));
+        }
+    }
+    std::vector<uint8_t> rx(tx.size(), 0);
+
+    if (ir_bits > 1) {
+        if (!putTdiBits(false, tx.data(), rx.data(), ir_bits - 1)) return false;
+    }
+    const uint8_t last_tx = getBit(tx.data(), ir_bits - 1);
+    uint8_t last_rx = 0;
+    if (!putTdiBits(true, &last_tx, &last_rx, 1)) return false;
+
+    // Exit1-IR -> Update-IR -> Idle: 1,0
+    uint8_t to_idle = 0x01;
+    if (!putTmsBits(&to_idle, 2)) return false;
+
+    last_error_.clear();
+    return true;
+}
+
+bool DigilentJtag::shiftData(const uint8_t* tx_bits, uint8_t* rx_bits, int bit_count) {
+    if (!isOpen()) {
+        return setError("shiftData: device not open");
+    }
+    if (bit_count <= 0) {
+        return setError("shiftData: bit_count must be > 0");
+    }
+
+    const size_t nbytes = static_cast<size_t>((bit_count + 7) / 8);
+    std::vector<uint8_t> tx_zeros;
+    if (!tx_bits) {
+        tx_zeros.assign(nbytes, 0);
+        tx_bits = tx_zeros.data();
+    }
+
+    std::vector<uint8_t> rx_tmp;
+    if (!rx_bits) {
+        rx_tmp.assign(nbytes, 0);
+        rx_bits = rx_tmp.data();
+    } else {
+        std::memset(rx_bits, 0, nbytes);
+    }
+
+    if (!enterShiftDR()) return false;
+
+    if (bit_count > 1) {
+        if (!putTdiBits(false, tx_bits, rx_bits, bit_count - 1)) return false;
+    }
+
+    const uint8_t tx_last = getBit(tx_bits, bit_count - 1);
+    uint8_t rx_last = 0;
+    if (!putTdiBits(true, &tx_last, &rx_last, 1)) return false;
+    setBit(rx_bits, bit_count - 1, rx_last & 0x1u);
+
+    if (!leaveShiftToIdle()) return false;
+
+    last_error_.clear();
+    return true;
+}
+
+bool DigilentJtag::shiftData(const std::vector<uint8_t>& tx_bits, std::vector<uint8_t>* rx_bits, int bit_count) {
+    if (bit_count <= 0) {
+        return setError("shiftData(vector): bit_count must be > 0");
+    }
+    const size_t nbytes = static_cast<size_t>((bit_count + 7) / 8);
+    if (tx_bits.size() < nbytes) {
+        return setError("shiftData(vector): tx_bits is smaller than required bit_count");
+    }
+
+    std::vector<uint8_t> local_rx;
+    local_rx.assign(nbytes, 0);
+
+    if (!shiftData(tx_bits.data(), local_rx.data(), bit_count)) {
+        return false;
+    }
+
+    if (rx_bits) {
+        *rx_bits = std::move(local_rx);
+    }
+    return true;
+}
+
+const std::string& DigilentJtag::lastError() const { return last_error_; }
+
+bool DigilentJtag::enterShiftDR() {
+    // Idle -> Select-DR -> Capture-DR -> Shift-DR : 1,0,0
+    uint8_t to_shift_dr = 0x01;
+    return putTmsBits(&to_shift_dr, 3);
+}
+
+bool DigilentJtag::leaveShiftToIdle() {
+    // Exit1-DR -> Update-DR -> Idle : 1,0
+    uint8_t to_idle = 0x01;
+    return putTmsBits(&to_idle, 2);
+}
+
+bool DigilentJtag::putTmsBits(const uint8_t* tms_bits, int bit_count) {
+    if (!DjtgPutTmsBits(hif_, fFalse, const_cast<uint8_t*>(tms_bits), nullptr, static_cast<DWORD>(bit_count), fFalse)) {
+        return setErrorFromDmgr("DjtgPutTmsBits");
+    }
+    return true;
+}
+
+bool DigilentJtag::putTdiBits(bool tms, const uint8_t* tx_bits, uint8_t* rx_bits, int bit_count) {
+    if (!DjtgPutTdiBits(
+            hif_,
+            tms ? fTrue : fFalse,
+            const_cast<uint8_t*>(tx_bits),
+            rx_bits,
+            static_cast<DWORD>(bit_count),
+            fFalse)) {
+        return setErrorFromDmgr("DjtgPutTdiBits");
+    }
+    return true;
+}
+
+bool DigilentJtag::setError(const std::string& msg) {
+    last_error_ = msg;
+    return false;
+}
+
+bool DigilentJtag::setErrorFromDmgr(const std::string& where) {
+    last_error_ = where + " failed: " + ercToString(DmgrGetLastError());
+    return false;
+}

tools/digilent_jtag.hpp

@@ -0,0 +1,52 @@
+#pragma once
+
+#include <cstdint>
+#include <string>
+#include <vector>
+
+#include <digilent/adept/dpcdecl.h>
+
+class DigilentJtag {
+public:
+    DigilentJtag();
+    ~DigilentJtag();
+
+    DigilentJtag(const DigilentJtag&) = delete;
+    DigilentJtag& operator=(const DigilentJtag&) = delete;
+
+    bool open(int port = 0);
+    bool open(const std::string& selector, int port = 0);
+    void close();
+
+    bool isOpen() const;
+    HIF handle() const;
+
+    bool setSpeed(uint32_t requested_hz, uint32_t* actual_hz = nullptr);
+
+    // For Spartan-6 style USER chains:
+    // chain=1 -> USER1 opcode 0x02, chain=2 -> USER2 opcode 0x03
+    bool setChain(int chain, int ir_bits = 6);
+
+    bool setInstruction(uint32_t instruction, int ir_bits);
+
+    // Shifts one DR transaction from Idle -> ShiftDR -> UpdateDR -> Idle.
+    // tx_bits is LSB-first in packed byte form.
+    // rx_bits, when non-null, receives captured TDO bits in the same packing.
+    bool shiftData(const uint8_t* tx_bits, uint8_t* rx_bits, int bit_count);
+
+    bool shiftData(const std::vector<uint8_t>& tx_bits, std::vector<uint8_t>* rx_bits, int bit_count);
+
+    const std::string& lastError() const;
+
+private:
+    bool enterShiftDR();
+    bool leaveShiftToIdle();
+    bool putTmsBits(const uint8_t* tms_bits, int bit_count);
+    bool putTdiBits(bool tms, const uint8_t* tx_bits, uint8_t* rx_bits, int bit_count);
+    bool setError(const std::string& msg);
+    bool setErrorFromDmgr(const std::string& where);
+
+    HIF hif_;
+    int enabled_port_;
+    std::string last_error_;
+};

tools/test.cpp

@@ -0,0 +1,142 @@
+#include "digilent_jtag.hpp"
+#include "argparse.hpp"
+
+#include <algorithm>
+#include <cstdio>
+#include <string>
+
+static constexpr uint8_t OP_NOP         = 0x00;
+static constexpr uint8_t OP_RESET_ON    = 0x10;
+static constexpr uint8_t OP_RESET_OFF   = 0x11;
+static constexpr uint8_t OP_WRITE8      = 0x20;
+static constexpr uint8_t OP_READ8       = 0x21;
+static constexpr uint8_t OP_WRITE32     = 0x22;
+static constexpr uint8_t OP_READ32      = 0x23;
+static constexpr uint8_t OP_PING        = 0x30;
+static constexpr uint8_t OP_CLEAR_FLAGS = 0x40;
+
+static void shift72(DigilentJtag &jtag, const uint8_t tx[9], uint8_t rx[9]) {
+  jtag.shiftData(tx, rx, 72);
+}
+
+static void make_cmd(uint8_t out[9], uint8_t opcode, uint32_t addr, uint32_t data) {
+  out[0] = (uint8_t)data;
+  out[1] = (uint8_t)(data >> 8);
+  out[2] = (uint8_t)(data >> 16);
+  out[3] = (uint8_t)(data >> 24);
+  out[4] = (uint8_t)addr;
+  out[5] = (uint8_t)(addr >> 8);
+  out[6] = (uint8_t)(addr >> 16);
+  out[7] = (uint8_t)(addr >> 24);
+  out[8] = opcode;
+}
+
+static uint32_t get_data32(const uint8_t rx[9]) {
+    return ((uint32_t)rx[2]) |
+           ((uint32_t)rx[3] << 8) |
+           ((uint32_t)rx[4] << 16) |
+           ((uint32_t)rx[5] << 24);
+}
+
+static uint32_t do_cmd32(DigilentJtag& jtag, uint8_t opcode, uint32_t addr, uint32_t data){
+    uint8_t tx[9], rx[9];
+    make_cmd(tx, opcode, addr, data);
+    shift72(jtag, tx, rx);
+    for(int i=0; i<32; i++){
+        make_cmd(tx, OP_NOP, 0, 0);
+        shift72(jtag, tx, rx);
+        if(rx[0] == opcode){
+            return get_data32(rx);
+        }
+    }
+    printf("Could not do command\r\n");
+    return 0;
+}
+
+static inline void write8(DigilentJtag& jtag, uint32_t addr, uint8_t value) {
+    (void)do_cmd32(jtag, OP_WRITE8, addr, value);
+}
+
+static inline uint8_t read8(DigilentJtag& jtag, uint32_t addr) {
+    return (uint8_t)do_cmd32(jtag, OP_READ8, addr, 0);
+}
+
+static inline void write32(DigilentJtag& jtag, uint32_t addr, uint32_t value) {
+    (void)do_cmd32(jtag, OP_WRITE32, addr, value);
+}
+
+static inline uint32_t read32(DigilentJtag& jtag, uint32_t addr) {
+    return do_cmd32(jtag, OP_READ32, addr, 0);
+}
+
+int main(int argc, char** argv){
+    ArgParser parser(argc > 0 ? argv[0] : "test");
+    parser.addString("file", "", "File to write", true, "f");
+    parser.addFlag("verify", "Verify", "v");
+
+    std::string parse_error;
+    if (!parser.parse(argc, argv, &parse_error)) {
+        if (parse_error == "help") {
+            std::printf("%s", parser.helpText().c_str());
+            return 0;
+        }
+        std::printf("Argument error: %s\n\n", parse_error.c_str());
+        std::printf("%s", parser.helpText().c_str());
+        return -1;
+    }
+
+    DigilentJtag jtag;
+    if(!jtag.open()){
+        printf("Could not open programmer\r\n");
+        return -1;
+    }
+    jtag.setChain(1);
+
+
+    do_cmd32(jtag, OP_CLEAR_FLAGS, 0, 0);
+    // Check for ping
+    if((do_cmd32(jtag, OP_PING, 0, 0) & 0xffu) != 0xa5u){
+        printf("PING response was not right\r\n");
+        jtag.close();
+        return -1;
+    }
+
+    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;
+    }
+
+    do_cmd32(jtag, OP_RESET_ON, 0, 0);
+
+    int nr = 0;
+    int addr = 0;
+    do{
+        uint32_t buf[32];
+        nr = fread(buf, sizeof(uint32_t), 32, f);
+        for(int i=0; i<nr; i++){
+            write32(jtag, addr+(i*4), buf[i]);
+            printf(".");
+        }
+        printf("\r\n");
+
+        if(parser.getFlag("verify")){
+            for(int i=0; i<nr; i++){
+                uint32_t r = read32(jtag, addr+(i*4));
+                if(r!=buf[i]){
+                    printf(" -- Verify failed at %04x : %08x != %08x\r\n", addr+(i*4), r, buf[i]);
+                }
+            }
+        }
+
+        addr += nr*4;
+    }while(nr > 0);
+    
+    do_cmd32(jtag, OP_RESET_OFF, 0, 0);
+
+    fclose(f);
+    jtag.close();
+    return 0;
+}