New wishbone-jtag bridge

TODO: think of other way of shifting in data. Bit errors make uploading difficult

.gitignore

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

boards/mimas_v1/constraints.ucf

@@ -2,9 +2,6 @@
 NET "aclk" LOC = P126;
 NET "aclk" TNM_NET = "SYS_CLK_PIN";
 TIMESPEC TS_SYS_CLK_PIN = PERIOD "SYS_CLK_PIN" 10 ns HIGH 50 %;
-# Generated clocks
-NET "clk_15" TNM_NET = "SYS_CLK_15";
-TIMESPEC TS_SYS_CLK_15 = PERIOD "SYS_CLK_15" 13.334 ns HIGH 50%;
 
 # Boards button row
 NET "aresetn" LOC = P120;
@@ -28,3 +25,28 @@ NET "r2r[2]" IOSTANDARD = LVCMOS33;
 NET "r2r[3]" IOSTANDARD = LVCMOS33;
 NET "r2r[4]" 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,14 +4,130 @@ 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
+ise_settings        = /opt/Xilinx/14.7/ISE_DS/settings64.sh
 family              = spartan6
 device              = xc6slx9
 package             = tqg144
@@ -19,42 +135,83 @@ speedgrade          = -2
 toplevel            = top_generic
 xst_opts            = -vlgincdir rtl/util
 files_verilog       = rtl/toplevel/top_generic.v
+                      rtl/util/conv.vh
                       rtl/core/nco_q15.v
                       rtl/core/sigmadelta_sampler.v
                       rtl/core/sigmadelta_rcmodel_q15.v
+                      rtl/core/sigmadelta_input_q15.v
                       rtl/core/mul_const.v
                       rtl/core/lpf_iir_q15_k.v
                       rtl/core/decimate_by_r_q15.v
+                      rtl/core/mcu_peripherals.v
+                      rtl/core/cdc_strobe_data.v
+                      rtl/core/cdc_req_resp.v
+                      rtl/core/mcu.v
+                      rtl/core/mem_jtag_writable.v
+                    # Arch
                       rtl/arch/spartan-6/lvds_comparator.v
                       rtl/arch/spartan-6/clk_gen.v
-files_con           = boards/mimas_v1/constraints.ucf
+                      rtl/arch/spartan-6/jtag_if.v
-files_other         = rtl/util/conv.vh
+                    # SERV
-                      rtl/util/rc_alpha_q15.vh
+                      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
 
-[target.ip]
+files_con           = boards/mimas_v1/constraints.ucf
-toolchain           = ISE_IP
+files_other         = rtl/util/rc_alpha_q15.vh
+                      rtl/util/clog2.vh
+                      sw/sweep/sweep.hex
+
+[target.jtag]
+toolchain           = ISE
+ise_settings        = /opt/Xilinx/14.7/ISE_DS/settings64.sh
 family              = spartan6
 device              = xc6slx9
 package             = tqg144
 speedgrade          = -2
-files_xco           = boards/mimas_v1/ip/clk_gen.xco
+toplevel            = top_jtag
-
+xst_opts            = -vlgincdir rtl/util -keep_hierarchy yes
-
+files_other         =
-[target.sim]
+files_con           = boards/mimas_v1/constraints.ucf
-toolchain           = iverilog
+files_verilog       = rtl/arch/spartan-6/jtag_if.v
-runtime             = all
+                      rtl/arch/spartan-6/clk_gen.v
-toplevel            = tb_sigmadelta
+                      rtl/core/cdc_strobe_data.v
-ivl_opts            = -Irtl/util
+                      rtl/core/cdc_req_resp.v
-files_verilog       = sim/tb/tb_nco_q15.v
+                      rtl/wb/jtag_wb_bridge.v
-                      sim/tb/tb_sigmadelta.v
+                      rtl/wb/wb_gpio.v
-                      sim/tb/tb_mul_const.v
+                      rtl/toplevel/top_jtag.v
-                      rtl/core/nco_q15.v
-                      rtl/core/lvds_comparator.v
-                      rtl/core/sigmadelta_rcmodel_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

rtl/arch/spartan-6/clk_gen.v

@@ -67,9 +67,9 @@
 (* CORE_GENERATION_INFO = "clk_gen,clk_wiz_v3_6,{component_name=clk_gen,use_phase_alignment=true,use_min_o_jitter=false,use_max_i_jitter=false,use_dyn_phase_shift=false,use_inclk_switchover=false,use_dyn_reconfig=false,feedback_source=FDBK_AUTO,primtype_sel=DCM_SP,num_out_clk=1,clkin1_period=10.0,clkin2_period=10.0,use_power_down=false,use_reset=false,use_locked=false,use_inclk_stopped=false,use_status=false,use_freeze=false,use_clk_valid=false,feedback_type=SINGLE,clock_mgr_type=AUTO,manual_override=false}" *)
 module clk_gen
  (// Clock in ports
-  input         clk_in,
+  input  wire   clk_in,
   // Clock out ports
-  output        clk_out_15
+  output wire   clk_out_15
  );
 
   // Input buffering
@@ -78,6 +78,7 @@ module clk_gen
   //  (.O (clkin1),
     // .I (clk_in));
 
+  wire clkin1;
   assign clkin1 = clk_in;
 
   // Clocking primitive
@@ -145,4 +146,3 @@ module clk_gen
 
 
 endmodule
-

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/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,183 @@
+`timescale 1ns/1ps
+`include "../util/clog2.vh"
+
+module mcu #(
+    parameter memfile = "",
+    parameter memsize = 8192,
+    parameter sim = 1'b0
+)(
+    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,
+    output wire o_test
+);
+    localparam WITH_CSR = 1;
+    localparam regs = 32+WITH_CSR*4;
+    localparam rf_width = 8;
+
+    wire rst;
+    wire rst_mem_reason;
+    wire timer_irq;
+    assign rst = i_rst | rst_mem_reason;
+
+    assign o_test = timer_irq;
+
+    // Busses
+    //  CPU->memory
+    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;
+    wire wb_mem_ack;
+    //  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;
+    //  CPU->RF
+    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;
+    //  combined RF and mem bus to actual RAM
+    wire [`CLOG2(memsize)-1:0] sram_waddr;
+    wire [rf_width-1:0] sram_wdata;
+    wire sram_wen;
+    wire [`CLOG2(memsize)-1:0] sram_raddr;
+    wire [rf_width-1:0] sram_rdata;
+    wire sram_ren;
+
+    // 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;
+
+    // SERV core with mux splitting dbus into mem and ext and
+    //  arbiter combining mem and ibus
+    //  separate rst line to let other hardware keep core under reset
+    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(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),
+        .i_wb_mem_ack(wb_mem_ack),
+
+        //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),
+
+        //RF IF
+        .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)
+    );
+
+    // WB arbiter combining RF and mem interfaces into 1
+    //  Last 128 bytes are used for registers
+    servile_rf_mem_if #(
+        .depth(memsize),
+        .rf_regs(regs)
+    ) rf_mem_if (
+        .i_clk (i_clk),
+        .i_rst (rst),
+
+        .i_waddr(rf_waddr),
+        .i_wdata(rf_wdata),
+        .i_wen(rf_wen),
+        .i_raddr(rf_raddr),
+        .o_rdata(rf_rdata),
+        .i_ren(rf_ren),
+
+        .o_sram_waddr(sram_waddr),
+        .o_sram_wdata(sram_wdata),
+        .o_sram_wen(sram_wen),
+        .o_sram_raddr(sram_raddr),
+        .i_sram_rdata(sram_rdata),
+        // .o_sram_ren(sram_ren),
+
+        .i_wb_adr(wb_mem_adr[`CLOG2(memsize)-1:2]),
+        .i_wb_stb(wb_mem_stb),
+        .i_wb_we(wb_mem_we) ,
+        .i_wb_sel(wb_mem_sel),
+        .i_wb_dat(wb_mem_dat),
+        .o_wb_rdt(wb_mem_rdt),
+        .o_wb_ack(wb_mem_ack)
+    );
+
+    memory #(
+        .memfile(memfile),
+        .depth(memsize),
+        .sim(sim)
+    ) mem (
+        .i_clk(i_clk),
+        .i_rst(i_rst),
+        .i_waddr(sram_waddr),
+        .i_wdata(sram_wdata),
+        .i_wen(sram_wen),
+        .i_raddr(sram_raddr),
+        .o_rdata(sram_rdata),
+        .o_core_reset(rst_mem_reason)
+    );
+
+    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)
+    );
+
+endmodule

rtl/core/mcu_peripherals.v

@@ -0,0 +1,123 @@
+`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
+);
+    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;
+
+    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 #(
+	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/core/sigmadelta_input_q15.v

@@ -0,0 +1,58 @@
+`timescale 1ns/1ps
+
+module sigmadelta_input #(
+    parameter integer R_OHM = 3300,
+    parameter integer C_PF = 220
+)(
+    input wire clk_15,
+    input wire resetn,
+    
+    input wire adc_a,
+    input wire adc_b,
+    output wire adc_o,
+
+    output wire signed [15:0] signal_q15,
+    output wire signal_valid
+);
+    `include "rc_alpha_q15.vh"
+
+    wire sd_signal;
+    wire signed [15:0] raw_sample_q15;
+    wire signed [15:0] lpf_sample_q15;
+
+    sigmadelta_sampler sd_sampler(
+        .clk(clk_15),
+        .a(adc_a), .b(adc_b),
+        .o(sd_signal)
+    );
+    assign adc_o = sd_signal;
+
+    localparam integer alpha_q15_int = alpha_q15_from_rc(R_OHM, C_PF, 15000000);
+    localparam signed [15:0] alpha_q15 = alpha_q15_int[15:0];
+    localparam signed [15:0] alpha_q15_top = alpha_q15 & 16'hff00;
+    sigmadelta_rcmodel_q15 #(
+        .alpha_q15(alpha_q15_top)
+    ) rc_model (
+        .clk(clk_15), .resetn(resetn),
+        .sd_sample(sd_signal),
+        .sample_q15(raw_sample_q15)
+    );
+
+    lpf_iir_q15_k #(
+        .K(10)
+    ) lpf (
+        .clk(clk_15), .rst_n(resetn),
+        .x_q15(raw_sample_q15),
+        .y_q15(lpf_sample_q15)
+    );
+
+    decimate_by_r_q15 #(
+        .R(375), // 15MHz/375 = 40KHz
+        .CNT_W(10)
+    ) decimate (
+        .clk(clk_15), .rst_n(resetn),
+        .in_valid(1'b1), .in_q15(lpf_sample_q15),
+        .out_valid(signal_valid), .out_q15(signal_q15)
+    );
+
+endmodule

rtl/core/sigmadelta_sampler.v

@@ -17,7 +17,7 @@ module sigmadelta_sampler(
     );
 
     reg registered_comp_out;
-    always @(posedge clk) registered_comp_out <= o;
+    always @(posedge clk) registered_comp_out <= comp_out;
     assign o = registered_comp_out;
 
 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/serv/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/serv/serv_aligner.v

@@ -0,0 +1,73 @@
+/*
+ * serv_aligner.v : Realign a misaligned 32-bit word fetched from memory
+ *
+ * SPDX-FileCopyrightText: 2022 Olof Kindgren <olof@award-winning.me>
+ * SPDX-License-Identifier: ISC
+ */
+module serv_aligner
+   (
+    input wire clk,
+    input wire rst,
+    // serv_top
+    input  wire [31:0]  i_ibus_adr,
+    input  wire         i_ibus_cyc,
+    output wire [31:0]  o_ibus_rdt,
+    output wire         o_ibus_ack,
+    // serv_rf_top
+    output wire [31:0]  o_wb_ibus_adr,
+    output wire         o_wb_ibus_cyc,
+    input  wire [31:0]  i_wb_ibus_rdt,
+    input  wire         i_wb_ibus_ack);
+
+    wire [31:0] ibus_rdt_concat;
+    wire        ack_en;
+
+    reg  [15:0] lower_hw;
+    reg         ctrl_misal ;
+
+    /* From SERV core to Memory
+
+    o_wb_ibus_adr: Carries address of instruction to memory. In case of misaligned access,
+    which is caused by pc+2 due to compressed instruction, next instruction is fetched
+    by pc+4 and concatenation is done to make the instruction aligned.
+
+    o_wb_ibus_cyc: Simply forwarded from SERV to Memory and is only altered by memory or SERV core.
+    */
+    assign o_wb_ibus_adr = ctrl_misal ? (i_ibus_adr+32'b100) : i_ibus_adr;
+    assign o_wb_ibus_cyc = i_ibus_cyc;
+
+    /* From Memory to SERV core
+
+        o_ibus_ack: Instruction bus acknowledge is send to SERV only when the aligned instruction,
+        either compressed or un-compressed, is ready to dispatch.
+
+        o_ibus_rdt: Carries the instruction from memory to SERV core. It can be either aligned
+        instruction coming from memory or made aligned by two bus transactions and concatenation.
+    */
+    assign o_ibus_ack = i_wb_ibus_ack & ack_en;
+    assign o_ibus_rdt = ctrl_misal ? ibus_rdt_concat : i_wb_ibus_rdt;
+
+    /* 16-bit register used to hold the upper half word of the current instruction in-case
+       concatenation will be required with the upper half word of upcoming instruction
+    */
+    always @(posedge clk) begin
+        if(i_wb_ibus_ack)begin
+            lower_hw <= i_wb_ibus_rdt[31:16];
+        end
+    end
+
+    assign ibus_rdt_concat = {i_wb_ibus_rdt[15:0],lower_hw};
+
+    /* Two control signals: ack_en, ctrl_misal are set to control the bus transactions between
+    SERV core and the memory
+    */
+    assign ack_en   = !(i_ibus_adr[1] & !ctrl_misal);
+
+    always @(posedge clk ) begin
+        if(rst)
+            ctrl_misal <= 0;
+        else if(i_wb_ibus_ack & i_ibus_adr[1])
+            ctrl_misal <= !ctrl_misal;
+    end
+
+endmodule

rtl/serv/serv_alu.v

@@ -0,0 +1,87 @@
+/*
+ * serv_alu.v : SERV Arithmetic Logic Unit
+ *
+ * SPDX-FileCopyrightText: 2018 Olof Kindgren <olof@award-winning.me>
+ * SPDX-License-Identifier: ISC
+ */
+`default_nettype none
+module serv_alu
+  #(
+   parameter W = 1,
+   parameter B = W-1
+  )
+  (
+   input wire 	    clk,
+   //State
+   input wire 	    i_en,
+   input wire 	    i_cnt0,
+   output wire 	    o_cmp,
+   //Control
+   input wire 	    i_sub,
+   input wire [1:0] i_bool_op,
+   input wire 	    i_cmp_eq,
+   input wire 	    i_cmp_sig,
+   input wire [2:0] i_rd_sel,
+   //Data
+   input wire  [B:0] i_rs1,
+   input wire  [B:0] i_op_b,
+   input wire  [B:0] i_buf,
+   output wire [B:0] o_rd);
+
+   wire [B:0]  result_add;
+   wire [B:0]  result_slt;
+
+   reg 	       cmp_r;
+
+   wire        add_cy;
+   reg [B:0]   add_cy_r;
+
+   //Sign-extended operands
+   wire rs1_sx  = i_rs1[B] & i_cmp_sig;
+   wire op_b_sx = i_op_b[B]  & i_cmp_sig;
+
+   wire [B:0] add_b = i_op_b^{W{i_sub}};
+
+   assign {add_cy,result_add}   = i_rs1+add_b+add_cy_r;
+
+   wire result_lt = rs1_sx + ~op_b_sx + add_cy;
+
+   wire result_eq = !(|result_add) & (cmp_r | i_cnt0);
+
+   assign o_cmp = i_cmp_eq ? result_eq : result_lt;
+
+   /*
+    The result_bool expression implements the following operations between
+    i_rs1 and i_op_b depending on the value of i_bool_op
+
+    00 xor
+    01 0
+    10 or
+    11 and
+
+    i_bool_op will be 01 during shift operations, so by outputting zero under
+    this condition we can safely or result_bool with i_buf
+    */
+   wire [B:0] result_bool = ((i_rs1 ^ i_op_b) & ~{W{i_bool_op[0]}}) | ({W{i_bool_op[1]}} & i_op_b & i_rs1);
+
+   assign result_slt[0] = cmp_r & i_cnt0;
+   generate
+      if (W>1) begin : gen_w_gt_1
+	 assign result_slt[B:1] = {B{1'b0}};
+      end
+   endgenerate
+
+   assign o_rd = i_buf |
+                 ({W{i_rd_sel[0]}} & result_add) |
+                 ({W{i_rd_sel[1]}} & result_slt) |
+                 ({W{i_rd_sel[2]}} & result_bool);
+
+   always @(posedge clk) begin
+      add_cy_r    <= {W{1'b0}};
+      add_cy_r[0] <= i_en ? add_cy : i_sub;
+
+      if (i_en)
+	cmp_r <= o_cmp;
+   end
+
+endmodule

rtl/serv/serv_bufreg.v

@@ -0,0 +1,101 @@
+/*
+ * serv_bufreg.v : SERV buffer register for load/store address and shift data
+ *
+ * SPDX-FileCopyrightText: 2019 Olof Kindgren <olof@award-winning.me>
+ * SPDX-License-Identifier: ISC
+ */
+module serv_bufreg #(
+      parameter [0:0] MDU = 0,
+      parameter W = 1,
+      parameter B = W-1
+)(
+   input wire 	      i_clk,
+   //State
+   input wire 	      i_cnt0,
+   input wire 	      i_cnt1,
+   input wire 	      i_cnt_done,
+   input wire 	      i_en,
+   input wire 	      i_init,
+   input wire           i_mdu_op,
+   output wire [1:0]    o_lsb,
+   //Control
+   input wire 	      i_rs1_en,
+   input wire 	      i_imm_en,
+   input wire 	      i_clr_lsb,
+   input wire 	      i_shift_op,
+   input wire 	      i_right_shift_op,
+   input wire [2:0]   i_shamt,
+   input wire 	      i_sh_signed,
+   //Data
+   input wire [B:0] i_rs1,
+   input wire [B:0] i_imm,
+   output wire [B:0] o_q,
+   //External
+   output wire [31:0] o_dbus_adr,
+   //Extension
+   output wire [31:0] o_ext_rs1);
+
+   wire		      c;
+   wire [B:0]	      q;
+   reg [B:0]	      c_r;
+   reg [31:0]	      data;
+   wire [B:0]	      clr_lsb;
+
+   assign clr_lsb[0] = i_cnt0 & i_clr_lsb;
+
+   generate
+      if (W > 1) begin : gen_clr_lsb_w_gt_1
+         assign  clr_lsb[B:1] = {B{1'b0}};
+      end
+   endgenerate
+
+   assign {c,q} = {1'b0,(i_rs1 & {W{i_rs1_en}})} + {1'b0,(i_imm & {W{i_imm_en}} & ~clr_lsb)} + c_r;
+
+   always @(posedge i_clk) begin
+      //Make sure carry is cleared before loading new data
+      c_r    <= {W{1'b0}};
+      c_r[0] <= c & i_en;
+   end
+
+   generate
+      if (W == 1) begin : gen_w_eq_1
+	 always @(posedge i_clk) begin
+	    if (i_en)
+	      data[31:2] <= {i_init ? q : {W{data[31] & i_sh_signed}}, data[31:3]};
+
+	    if (i_init ? (i_cnt0 | i_cnt1) : i_en)
+	      data[1:0] <= {i_init ? q : data[2], data[1]};
+	 end
+	 assign o_lsb = (MDU & i_mdu_op) ? 2'b00 : data[1:0];
+	 assign o_q = data[0] & {W{i_en}};
+      end else if (W == 4) begin : gen_lsb_w_4
+	 reg [1:0] lsb;
+	 reg [W-2:0] data_tail;
+
+	 wire [2:0] shift_amount
+	   = !i_shift_op ? 3'd3 :
+	     i_right_shift_op ? (3'd3+{1'b0,i_shamt[1:0]}) :
+	     ({1'b0,~i_shamt[1:0]});
+
+	 always @(posedge i_clk) begin
+            if (i_en)
+              if (i_cnt0) lsb <= q[1:0];
+	    if (i_en)
+              data <= {i_init ? q : {W{i_sh_signed & data[31]}}, data[31:W]};
+	    if (i_en)
+	      data_tail <= data[B:1] & {B{~i_cnt_done}};
+	 end
+
+	 wire [2*W+B-2:0] muxdata = {data[W+B-1:0],data_tail};
+	 wire [B:0]	  muxout = muxdata[{1'b0,shift_amount}+:W];
+
+	 assign o_lsb = (MDU & i_mdu_op) ? 2'b00 : lsb;
+	 assign o_q = i_en ? muxout : {W{1'b0}};
+      end
+   endgenerate
+
+
+   assign o_dbus_adr = {data[31:2], 2'b00};
+   assign o_ext_rs1  = data;
+
+endmodule

rtl/serv/serv_bufreg2.v

@@ -0,0 +1,105 @@
+/*
+ * serv_bufreg2.v : SERV buffer register for load/store data and shift amount
+ *
+ * SPDX-FileCopyrightText: 2022 Olof Kindgren <olof@award-winning.me>
+ * SPDX-License-Identifier: ISC
+ */
+module serv_bufreg2
+  #(parameter W = 1,
+    //Internally calculated. Do not touch
+    parameter B=W-1)
+  (
+   input wire	      i_clk,
+   //State
+   input wire	      i_en,
+   input wire	      i_init,
+   input wire	      i_cnt7,
+   input wire	      i_cnt_done,
+   input wire	      i_sh_right,
+   input wire [1:0]   i_lsb,
+   input wire [1:0]   i_bytecnt,
+   output wire	      o_sh_done,
+   //Control
+   input wire	      i_op_b_sel,
+   input wire	      i_shift_op,
+   //Data
+   input wire [B:0]   i_rs2,
+   input wire [B:0]   i_imm,
+   output wire [B:0]  o_op_b,
+   output wire [B:0]  o_q,
+   //External
+   output wire [31:0] o_dat,
+   input wire	      i_load,
+   input wire [31:0]  i_dat);
+
+   // High and low data words form a 32-bit word
+   reg [7:0] 	 dhi;
+   reg [23:0] 	 dlo;
+
+   /*
+    Before a store operation, the data to be written needs to be shifted into
+    place. Depending on the address alignment, we need to shift different
+    amounts. One formula for calculating this is to say that we shift when
+    i_lsb + i_bytecnt < 4. Unfortunately, the synthesis tools don't seem to be
+    clever enough so the hideous expression below is used to achieve the same
+    thing in a more optimal way.
+    */
+   wire byte_valid
+     = (!i_lsb[0] & !i_lsb[1])         |
+       (!i_bytecnt[0] & !i_bytecnt[1]) |
+       (!i_bytecnt[1] & !i_lsb[1])     |
+       (!i_bytecnt[1] & !i_lsb[0])     |
+       (!i_bytecnt[0] & !i_lsb[1]);
+
+   assign o_op_b = i_op_b_sel ? i_rs2 : i_imm;
+
+   wire 	 shift_en = i_shift_op ? (i_en & i_init & (i_bytecnt == 2'b00)) : (i_en & byte_valid);
+
+   wire		 cnt_en = (i_shift_op & (!i_init | (i_cnt_done & i_sh_right)));
+
+   /* The dat register has three different use cases for store, load and
+    shift operations.
+    store : Data to be written is shifted to the correct position in dat during
+            init by shift_en and is presented on the data bus as o_wb_dat
+    load  : Data from the bus gets latched into dat during i_wb_ack and is then
+            shifted out at the appropriate time to end up in the correct
+            position in rd
+    shift : Data is shifted in during init. After that, the six LSB are used as
+            a downcounter (with bit 5 initially set to 0) that trigger
+            o_sh_done when they wrap around to indicate that
+            the requested number of shifts have been performed
+    */
+
+   wire [7:0]	 cnt_next;
+   generate
+      if (W == 1) begin : gen_cnt_w_eq_1
+	 assign cnt_next = {o_op_b, dhi[7], dhi[5:0]-6'd1};
+      end else if (W == 4) begin : gen_cnt_w_eq_4
+	 assign cnt_next = {o_op_b[3:2], dhi[5:0]-6'd4};
+      end
+   endgenerate
+
+   wire [7:0] dat_shamt = cnt_en ?
+	      //Down counter mode
+	      cnt_next :
+	      //Shift reg mode
+	      {o_op_b, dhi[7:W]};
+
+   assign o_sh_done = dat_shamt[5];
+
+   assign o_q =
+	       ({W{(i_lsb == 2'd3)}} & o_dat[W+23:24]) |
+	       ({W{(i_lsb == 2'd2)}} & o_dat[W+15:16]) |
+	       ({W{(i_lsb == 2'd1)}} & o_dat[W+7:8])   |
+	       ({W{(i_lsb == 2'd0)}} & o_dat[W-1:0]);
+
+   assign o_dat = {dhi,dlo};
+
+   always @(posedge i_clk) begin
+      if (shift_en | cnt_en | i_load)
+	dhi <= i_load ? i_dat[31:24] : dat_shamt & {2'b11, !(i_shift_op & i_cnt7 & !cnt_en), 5'b11111};
+      if (shift_en | i_load)
+	dlo <= i_load ? i_dat[23:0] : {dhi[B:0], dlo[23:W]};
+   end
+
+endmodule

rtl/serv/serv_compdec.v

@@ -0,0 +1,232 @@
+/* Copyright lowRISC contributors.
+Copyright 2018 ETH Zurich and University of Bologna, see also CREDITS.md.
+Licensed under the Apache License, Version 2.0, see LICENSE for details.
+SPDX-License-Identifier: Apache-2.0
+
+* Adapted to SERV by @Abdulwadoodd as part of the project under spring '22 LFX Mentorship program */
+
+/* Decodes RISC-V compressed instructions into their RV32i equivalent. */
+
+module serv_compdec
+  (
+   input wire i_clk,
+   input  wire [31:0] i_instr,
+   input  wire i_ack,
+   output wire [31:0] o_instr,
+   output reg o_iscomp);
+
+  localparam OPCODE_LOAD     = 7'h03;
+  localparam OPCODE_OP_IMM   = 7'h13;
+  localparam OPCODE_STORE    = 7'h23;
+  localparam OPCODE_OP       = 7'h33;
+  localparam OPCODE_LUI      = 7'h37;
+  localparam OPCODE_BRANCH   = 7'h63;
+  localparam OPCODE_JALR     = 7'h67;
+  localparam OPCODE_JAL      = 7'h6f;
+
+  reg  [31:0] comp_instr;
+  reg  illegal_instr;
+
+  assign o_instr = illegal_instr ? i_instr : comp_instr;
+
+  always @(posedge i_clk) begin
+    if(i_ack)
+      o_iscomp <= !illegal_instr;
+  end
+
+  always @ (*) begin
+    // By default, forward incoming instruction, mark it as legal.
+    comp_instr    = i_instr;
+    illegal_instr = 1'b0;
+
+    // Check if incoming instruction is compressed.
+    case (i_instr[1:0])
+      // C0
+      2'b00: begin
+        case (i_instr[15:14])
+          2'b00: begin
+            // c.addi4spn -> addi rd', x2, imm
+            comp_instr = {2'b0, i_instr[10:7], i_instr[12:11], i_instr[5],
+                      i_instr[6], 2'b00, 5'h02, 3'b000, 2'b01, i_instr[4:2], {OPCODE_OP_IMM}};
+          end
+
+          2'b01: begin
+            // c.lw -> lw rd', imm(rs1')
+            comp_instr = {5'b0, i_instr[5], i_instr[12:10], i_instr[6],
+                      2'b00, 2'b01, i_instr[9:7], 3'b010, 2'b01, i_instr[4:2], {OPCODE_LOAD}};
+          end
+
+          2'b11: begin
+            // c.sw -> sw rs2', imm(rs1')
+            comp_instr = {5'b0, i_instr[5], i_instr[12], 2'b01, i_instr[4:2],
+                      2'b01, i_instr[9:7], 3'b010, i_instr[11:10], i_instr[6],
+                      2'b00, {OPCODE_STORE}};
+          end
+
+          2'b10: begin
+            illegal_instr = 1'b1;
+          end
+
+        endcase
+      end
+
+      // C1
+
+      // Register address checks for RV32E are performed in the regular instruction decoder.
+      // If this check fails, an illegal instruction exception is triggered and the controller
+      // writes the actual faulting instruction to mtval.
+      2'b01: begin
+        case (i_instr[15:13])
+          3'b000: begin
+            // c.addi -> addi rd, rd, nzimm
+            // c.nop
+            comp_instr = {{6 {i_instr[12]}}, i_instr[12], i_instr[6:2],
+                      i_instr[11:7], 3'b0, i_instr[11:7], {OPCODE_OP_IMM}};
+          end
+
+          3'b001, 3'b101: begin
+            // 001: c.jal -> jal x1, imm
+            // 101: c.j   -> jal x0, imm
+            comp_instr = {i_instr[12], i_instr[8], i_instr[10:9], i_instr[6],
+                      i_instr[7], i_instr[2], i_instr[11], i_instr[5:3],
+                      {9 {i_instr[12]}}, 4'b0, ~i_instr[15], {OPCODE_JAL}};
+          end
+
+          3'b010: begin
+            // c.li -> addi rd, x0, nzimm
+            // (c.li hints are translated into an addi hint)
+            comp_instr = {{6 {i_instr[12]}}, i_instr[12], i_instr[6:2], 5'b0,
+                      3'b0, i_instr[11:7], {OPCODE_OP_IMM}};
+          end
+
+          3'b011: begin
+            // c.lui -> lui rd, imm
+            // (c.lui hints are translated into a lui hint)
+            comp_instr = {{15 {i_instr[12]}}, i_instr[6:2], i_instr[11:7], {OPCODE_LUI}};
+
+            if (i_instr[11:7] == 5'h02) begin
+              // c.addi16sp -> addi x2, x2, nzimm
+              comp_instr = {{3 {i_instr[12]}}, i_instr[4:3], i_instr[5], i_instr[2],
+                        i_instr[6], 4'b0, 5'h02, 3'b000, 5'h02, {OPCODE_OP_IMM}};
+            end
+
+          end
+
+          3'b100: begin
+            case (i_instr[11:10])
+              2'b00,
+              2'b01: begin
+                // 00: c.srli -> srli rd, rd, shamt
+                // 01: c.srai -> srai rd, rd, shamt
+                // (c.srli/c.srai hints are translated into a srli/srai hint)
+                comp_instr = {1'b0, i_instr[10], 5'b0, i_instr[6:2], 2'b01, i_instr[9:7],
+                          3'b101, 2'b01, i_instr[9:7], {OPCODE_OP_IMM}};
+              end
+
+              2'b10: begin
+                // c.andi -> andi rd, rd, imm
+                comp_instr = {{6 {i_instr[12]}}, i_instr[12], i_instr[6:2], 2'b01, i_instr[9:7],
+                          3'b111, 2'b01, i_instr[9:7], {OPCODE_OP_IMM}};
+              end
+
+              2'b11: begin
+                case (i_instr[6:5])
+                  2'b00: begin
+                    // c.sub -> sub rd', rd', rs2'
+                    comp_instr = {2'b01, 5'b0, 2'b01, i_instr[4:2], 2'b01, i_instr[9:7],
+                                  3'b000, 2'b01, i_instr[9:7], {OPCODE_OP}};
+                  end
+
+                  2'b01: begin
+                    // c.xor -> xor rd', rd', rs2'
+                    comp_instr = {7'b0, 2'b01, i_instr[4:2], 2'b01, i_instr[9:7], 3'b100,
+                              2'b01, i_instr[9:7], {OPCODE_OP}};
+                  end
+
+                  2'b10: begin
+                    // c.or  -> or  rd', rd', rs2'
+                    comp_instr = {7'b0, 2'b01, i_instr[4:2], 2'b01, i_instr[9:7], 3'b110,
+                              2'b01, i_instr[9:7], {OPCODE_OP}};
+                  end
+
+                  2'b11: begin
+                    // c.and -> and rd', rd', rs2'
+                    comp_instr = {7'b0, 2'b01, i_instr[4:2], 2'b01, i_instr[9:7], 3'b111,
+                              2'b01, i_instr[9:7], {OPCODE_OP}};
+                  end
+                endcase
+              end
+            endcase
+          end
+
+          3'b110, 3'b111: begin
+            // 0: c.beqz -> beq rs1', x0, imm
+            // 1: c.bnez -> bne rs1', x0, imm
+            comp_instr = {{4 {i_instr[12]}}, i_instr[6:5], i_instr[2], 5'b0, 2'b01,
+                      i_instr[9:7], 2'b00, i_instr[13], i_instr[11:10], i_instr[4:3],
+                      i_instr[12], {OPCODE_BRANCH}};
+          end
+        endcase
+      end
+
+      // C2
+
+      // Register address checks for RV32E are performed in the regular instruction decoder.
+      // If this check fails, an illegal instruction exception is triggered and the controller
+      // writes the actual faulting instruction to mtval.
+      2'b10: begin
+        case (i_instr[15:14])
+          2'b00: begin
+            // c.slli -> slli rd, rd, shamt
+            // (c.slli hints are translated into a slli hint)
+            comp_instr = {7'b0, i_instr[6:2], i_instr[11:7], 3'b001, i_instr[11:7], {OPCODE_OP_IMM}};
+          end
+
+          2'b01: begin
+            // c.lwsp -> lw rd, imm(x2)
+            comp_instr = {4'b0, i_instr[3:2], i_instr[12], i_instr[6:4], 2'b00, 5'h02,
+                      3'b010, i_instr[11:7], OPCODE_LOAD};
+          end
+
+          2'b10: begin
+            if (i_instr[12] == 1'b0) begin
+              if (i_instr[6:2] != 5'b0) begin
+                // c.mv -> add rd/rs1, x0, rs2
+                // (c.mv hints are translated into an add hint)
+                comp_instr = {7'b0, i_instr[6:2], 5'b0, 3'b0, i_instr[11:7], {OPCODE_OP}};
+              end else begin
+                // c.jr -> jalr x0, rd/rs1, 0
+                comp_instr = {12'b0, i_instr[11:7], 3'b0, 5'b0, {OPCODE_JALR}};
+              end
+            end else begin
+              if (i_instr[6:2] != 5'b0) begin
+                // c.add -> add rd, rd, rs2
+                // (c.add hints are translated into an add hint)
+                comp_instr = {7'b0, i_instr[6:2], i_instr[11:7], 3'b0, i_instr[11:7], {OPCODE_OP}};
+              end else begin
+                if (i_instr[11:7] == 5'b0) begin
+                  // c.ebreak -> ebreak
+                  comp_instr = {32'h00_10_00_73};
+                end else begin
+                  // c.jalr -> jalr x1, rs1, 0
+                  comp_instr = {12'b0, i_instr[11:7], 3'b000, 5'b00001, {OPCODE_JALR}};
+                end
+              end
+            end
+          end
+
+          2'b11: begin
+            // c.swsp -> sw rs2, imm(x2)
+            comp_instr = {4'b0, i_instr[8:7], i_instr[12], i_instr[6:2], 5'h02, 3'b010,
+                      i_instr[11:9], 2'b00, {OPCODE_STORE}};
+          end
+        endcase
+      end
+
+      // Incoming instruction is not compressed.
+      2'b11: illegal_instr = 1'b1;
+
+    endcase
+  end
+
+  endmodule

rtl/serv/serv_csr.v

@@ -0,0 +1,164 @@
+/*
+ * serv_csr.v : SERV module for handling CSR registers
+ *
+ * SPDX-FileCopyrightText: 2018 Olof Kindgren <olof@award-winning.me>
+ * SPDX-License-Identifier: ISC
+ */
+`default_nettype none
+module serv_csr
+  #(
+    parameter RESET_STRATEGY = "MINI",
+    parameter W = 1,
+    parameter B = W-1
+  )
+  (
+   input wire 	    i_clk,
+   input wire 	    i_rst,
+   //State
+   input wire 	    i_trig_irq,
+   input wire 	    i_en,
+   input wire 	    i_cnt0to3,
+   input wire 	    i_cnt3,
+   input wire 	    i_cnt7,
+   input wire 	    i_cnt11,
+   input wire 	    i_cnt12,
+   input wire 	    i_cnt_done,
+   input wire 	    i_mem_op,
+   input wire 	    i_mtip,
+   input wire 	    i_trap,
+   output reg 	    o_new_irq,
+   //Control
+   input wire 	    i_e_op,
+   input wire 	    i_ebreak,
+   input wire 	    i_mem_cmd,
+   input wire 	    i_mstatus_en,
+   input wire 	    i_mie_en,
+   input wire 	    i_mcause_en,
+   input wire [1:0] i_csr_source,
+   input wire 	    i_mret,
+   input wire 	    i_csr_d_sel,
+   //Data
+   input wire 	[B:0]    i_rf_csr_out,
+   output wire 	[B:0]    o_csr_in,
+   input wire 	[B:0]    i_csr_imm,
+   input wire 	[B:0]    i_rs1,
+   output wire 	[B:0]    o_q);
+
+   localparam [1:0]
+     CSR_SOURCE_CSR = 2'b00,
+     CSR_SOURCE_EXT = 2'b01,
+     CSR_SOURCE_SET = 2'b10,
+     CSR_SOURCE_CLR = 2'b11;
+
+   reg 		    mstatus_mie;
+   reg 		    mstatus_mpie;
+   reg 		    mie_mtie;
+
+   reg 		mcause31;
+   reg [3:0] 	mcause3_0;
+   wire [B:0]	mcause;
+
+   wire [B:0]	csr_in;
+   wire [B:0]	csr_out;
+
+   reg 		timer_irq_r;
+
+   wire [B:0]	d = i_csr_d_sel ? i_csr_imm : i_rs1;
+
+   assign csr_in = (i_csr_source == CSR_SOURCE_EXT) ? d :
+		   (i_csr_source == CSR_SOURCE_SET) ? csr_out | d :
+		   (i_csr_source == CSR_SOURCE_CLR) ? csr_out & ~d :
+		   (i_csr_source == CSR_SOURCE_CSR) ? csr_out :
+		   {W{1'bx}};
+
+   wire [B:0]	mstatus;
+
+   generate
+      if (W==1) begin : gen_mstatus_w1
+	 assign mstatus = ((mstatus_mie & i_cnt3) | (i_cnt11 | i_cnt12));
+      end else if (W==4) begin : gen_mstatus_w4
+	 assign mstatus = {i_cnt11 | (mstatus_mie & i_cnt3), 2'b00, i_cnt12};
+      end
+   endgenerate
+
+   assign csr_out = ({W{i_mstatus_en & i_en}} & mstatus) |
+		    i_rf_csr_out |
+		    ({W{i_mcause_en & i_en}} & mcause);
+
+   assign o_q = csr_out;
+
+   wire 	timer_irq = i_mtip & mstatus_mie & mie_mtie;
+
+   assign mcause = i_cnt0to3 ? mcause3_0[B:0] : //[3:0]
+		   i_cnt_done ? {mcause31,{B{1'b0}}} //[31]
+		   : {W{1'b0}};
+
+   assign o_csr_in = csr_in;
+
+   always @(posedge i_clk) begin
+      if (i_trig_irq) begin
+	 timer_irq_r <= timer_irq;
+	 o_new_irq   <= timer_irq & !timer_irq_r;
+      end
+
+      if (i_mie_en & i_cnt7)
+	mie_mtie <= csr_in[B];
+
+      /*
+       The mie bit in mstatus gets updated under three conditions
+
+       When a trap is taken, the bit is cleared
+       During an mret instruction, the bit is restored from mpie
+       During a mstatus CSR access instruction it's assigned when
+        bit 3 gets updated
+
+       These conditions are all mutually exclusive
+       */
+      if ((i_trap & i_cnt_done) | i_mstatus_en & i_cnt3 & i_en | i_mret)
+	mstatus_mie <= !i_trap & (i_mret ?  mstatus_mpie : csr_in[B]);
+
+      /*
+       Note: To save resources mstatus_mpie (mstatus bit 7) is not
+       readable or writable from sw
+       */
+      if (i_trap & i_cnt_done)
+	mstatus_mpie <= mstatus_mie;
+
+      /*
+       The four lowest bits in mcause hold the exception code
+
+       These bits get updated under three conditions
+
+       During an mcause CSR access function, they are assigned when
+       bits 0 to 3 gets updated
+
+       During an external interrupt the exception code is set to
+       7, since SERV only support timer interrupts
+
+       During an exception, the exception code is assigned to indicate
+       if it was caused by an ebreak instruction (3),
+       ecall instruction (11), misaligned load (4), misaligned store (6)
+       or misaligned jump (0)
+
+       The expressions below are derived from the following truth table
+       irq  => 0111 (timer=7)
+       e_op => x011 (ebreak=3, ecall=11)
+       mem  => 01x0 (store=6, load=4)
+       ctrl => 0000 (jump=0)
+       */
+      if (i_mcause_en & i_en & i_cnt0to3 | (i_trap & i_cnt_done)) begin
+	 mcause3_0[3] <= (i_e_op & !i_ebreak) | (!i_trap & csr_in[B]);
+	 mcause3_0[2] <= o_new_irq | i_mem_op | (!i_trap & ((W == 1) ? mcause3_0[3] : csr_in[(W == 1) ? 0 : 2]));
+	 mcause3_0[1] <= o_new_irq | i_e_op | (i_mem_op & i_mem_cmd) | (!i_trap & ((W == 1) ? mcause3_0[2] : csr_in[(W == 1) ? 0 : 1]));
+	 mcause3_0[0] <= o_new_irq | i_e_op | (!i_trap & ((W == 1) ? mcause3_0[1] : csr_in[0]));
+      end
+      if (i_mcause_en & i_cnt_done | i_trap)
+	mcause31 <= i_trap ? o_new_irq : csr_in[B];
+      if (i_rst)
+	if (RESET_STRATEGY != "NONE") begin
+	   o_new_irq <= 1'b0;
+	   mie_mtie <= 1'b0;
+	end
+   end
+
+endmodule

rtl/serv/serv_ctrl.v

@@ -0,0 +1,115 @@
+/*
+ * serv_ctrl.v : SERV module for updating program counter
+ *
+ * SPDX-FileCopyrightText: 2018 Olof Kindgren <olof@award-winning.me>
+ * SPDX-License-Identifier: ISC
+ */
+`default_nettype none
+module serv_ctrl
+  #(parameter RESET_STRATEGY = "MINI",
+    parameter RESET_PC = 32'd0,
+    parameter WITH_CSR = 1,
+    parameter W = 1,
+    parameter B = W-1
+  )
+  (
+   input wire 	     clk,
+   input wire 	     i_rst,
+   //State
+   input wire 	     i_pc_en,
+   input wire 	     i_cnt12to31,
+   input wire 	     i_cnt0,
+   input wire        i_cnt1,
+   input wire 	     i_cnt2,
+   //Control
+   input wire 	     i_jump,
+   input wire 	     i_jal_or_jalr,
+   input wire 	     i_utype,
+   input wire 	     i_pc_rel,
+   input wire 	     i_trap,
+   input wire        i_iscomp,
+   //Data
+   input wire [B:0] i_imm,
+   input wire [B:0] i_buf,
+   input wire [B:0] i_csr_pc,
+   output wire [B:0] o_rd,
+   output wire [B:0] o_bad_pc,
+   //External
+   output reg [31:0] o_ibus_adr);
+
+   wire [B:0] pc_plus_4;
+   wire       pc_plus_4_cy;
+   reg 	      pc_plus_4_cy_r;
+   wire [B:0] pc_plus_4_cy_r_w;
+   wire [B:0] pc_plus_offset;
+   wire       pc_plus_offset_cy;
+   reg	      pc_plus_offset_cy_r;
+   wire [B:0] pc_plus_offset_cy_r_w;
+   wire [B:0] pc_plus_offset_aligned;
+   wire [B:0] plus_4;
+
+   wire [B:0] pc = o_ibus_adr[B:0];
+
+   wire [B:0] new_pc;
+
+   wire [B:0] offset_a;
+   wire [B:0] offset_b;
+
+  /*  If i_iscomp=1: increment pc by 2 else increment pc by 4  */
+
+   generate
+      if (W == 1) begin : gen_plus_4_w_eq_1
+	 assign plus_4 = i_iscomp ? i_cnt1 : i_cnt2;
+      end else if (W == 4) begin : gen_plus_4_w_eq_4
+	 assign plus_4 = (i_cnt0 | i_cnt1) ? (i_iscomp ? 2 : 4) : 0;
+      end
+   endgenerate
+
+   assign o_bad_pc = pc_plus_offset_aligned;
+
+   assign {pc_plus_4_cy,pc_plus_4} = pc+plus_4+pc_plus_4_cy_r_w;
+
+   generate
+      if (|WITH_CSR) begin : gen_csr
+	 if (W == 1) begin : gen_new_pc_w_eq_1
+	    assign new_pc = i_trap ? (i_csr_pc & !(i_cnt0 || i_cnt1)) : i_jump ? pc_plus_offset_aligned : pc_plus_4;
+         end else if (W == 4) begin : gen_new_pc_w_eq_4
+	    assign new_pc = i_trap ? (i_csr_pc & ((i_cnt0 || i_cnt1) ? 4'b1100 : 4'b1111)) : i_jump ? pc_plus_offset_aligned : pc_plus_4;
+	 end
+      end else begin : gen_no_csr
+	 assign new_pc = i_jump ? pc_plus_offset_aligned : pc_plus_4;
+      end
+   endgenerate
+   assign o_rd  = ({W{i_utype}} & pc_plus_offset_aligned) | (pc_plus_4 & {W{i_jal_or_jalr}});
+
+   assign offset_a = {W{i_pc_rel}} & pc;
+   assign offset_b = i_utype ? (i_imm & {W{i_cnt12to31}}) : i_buf;
+   assign {pc_plus_offset_cy,pc_plus_offset} = offset_a+offset_b+pc_plus_offset_cy_r_w;
+
+   generate
+   if (W>1) begin : gen_w_gt_1
+	 assign pc_plus_offset_aligned[B:1] = pc_plus_offset[B:1];
+	 assign pc_plus_offset_cy_r_w[B:1] = {B{1'b0}};
+	 assign pc_plus_4_cy_r_w[B:1] = {B{1'b0}};
+   end
+   endgenerate
+
+   assign pc_plus_offset_aligned[0] = pc_plus_offset[0] & !i_cnt0;
+   assign pc_plus_offset_cy_r_w[0] = pc_plus_offset_cy_r;
+   assign pc_plus_4_cy_r_w[0] = pc_plus_4_cy_r;
+
+   initial if (RESET_STRATEGY == "NONE") o_ibus_adr = RESET_PC;
+
+   always @(posedge clk) begin
+      pc_plus_4_cy_r <= i_pc_en & pc_plus_4_cy;
+      pc_plus_offset_cy_r <= i_pc_en & pc_plus_offset_cy;
+
+      if (RESET_STRATEGY == "NONE") begin
+	 if (i_pc_en)
+	   o_ibus_adr <= {new_pc, o_ibus_adr[31:W]};
+      end else begin
+	 if (i_pc_en | i_rst)
+	   o_ibus_adr <= i_rst ? RESET_PC : {new_pc, o_ibus_adr[31:W]};
+      end
+   end
+endmodule

rtl/serv/serv_debug.v

@@ -0,0 +1,351 @@
+/*
+ * serv_debug.v : SERV module for introspecting CPU/RF state during simulations
+ *
+ * SPDX-FileCopyrightText: 2024 Olof Kindgren <olof@award-winning.me>
+ * SPDX-License-Identifier: ISC
+ */
+module serv_debug
+  #(parameter W = 1,
+    parameter RESET_PC = 0,
+    //Internally calculated. Do not touch
+    parameter B=W-1)
+   (
+`ifdef RISCV_FORMAL
+    output reg	      rvfi_valid = 1'b0,
+    output reg [63:0]  rvfi_order = 64'd0,
+    output reg [31:0]  rvfi_insn = 32'd0,
+    output reg	      rvfi_trap = 1'b0,
+    output reg	      rvfi_halt = 1'b0,  // Not used
+    output reg	      rvfi_intr = 1'b0,  // Not used
+    output reg [1:0]   rvfi_mode = 2'b11, // Not used
+    output reg [1:0]   rvfi_ixl = 2'b01,  // Not used
+    output reg [4:0]   rvfi_rs1_addr,
+    output reg [4:0]   rvfi_rs2_addr,
+    output reg [31:0]  rvfi_rs1_rdata,
+    output reg [31:0]  rvfi_rs2_rdata,
+    output reg [4:0]   rvfi_rd_addr,
+    output wire [31:0] rvfi_rd_wdata,
+    output reg [31:0]  rvfi_pc_rdata,
+    output wire [31:0]  rvfi_pc_wdata,
+    output reg [31:0]  rvfi_mem_addr,
+    output reg [3:0]   rvfi_mem_rmask,
+    output reg [3:0]   rvfi_mem_wmask,
+    output reg [31:0]  rvfi_mem_rdata,
+    output reg [31:0]  rvfi_mem_wdata,
+    input wire [31:0]  i_dbus_adr,
+    input wire [31:0]  i_dbus_dat,
+    input wire [3:0]   i_dbus_sel,
+    input wire	      i_dbus_we,
+    input wire [31:0]  i_dbus_rdt,
+    input wire	      i_dbus_ack,
+    input wire	      i_ctrl_pc_en,
+    input wire	[B:0]      rs1,
+    input wire [B:0]	      rs2,
+    input wire [4:0]   rs1_addr,
+    input wire [4:0]   rs2_addr,
+    input wire [3:0]   immdec_en,
+    input wire	      rd_en,
+    input wire	      trap,
+    input wire	      i_rf_ready,
+    input wire	      i_ibus_cyc,
+    input wire	      two_stage_op,
+    input wire	      init,
+    input wire [31:0]  i_ibus_adr,
+`endif
+    input wire	      i_clk,
+    input wire	      i_rst,
+    input wire [31:0] i_ibus_rdt,
+    input wire	      i_ibus_ack,
+    input wire [4:0]  i_rd_addr,
+    input wire	      i_cnt_en,
+    input wire [B:0]  i_csr_in,
+    input wire	      i_csr_mstatus_en,
+    input wire	      i_csr_mie_en,
+    input wire	      i_csr_mcause_en,
+    input wire	      i_csr_en,
+    input wire [1:0]  i_csr_addr,
+    input wire	      i_wen0,
+    input wire [B:0]  i_wdata0,
+    input wire	      i_cnt_done);
+
+   reg		      update_rd = 1'b0;
+   reg		      update_mscratch;
+   reg		      update_mtvec;
+   reg		      update_mepc;
+   reg		      update_mtval;
+   reg		      update_mstatus;
+   reg		      update_mie;
+   reg		      update_mcause;
+
+   reg [31:0]	      dbg_rd = 32'hxxxxxxxx;
+   reg [31:0]	      dbg_csr = 32'hxxxxxxxx;
+   reg [31:0]	      dbg_mstatus  = 32'hxxxxxxxx;
+   reg [31:0]	      dbg_mie      = 32'hxxxxxxxx;
+   reg [31:0]	      dbg_mcause   = 32'hxxxxxxxx;
+   reg [31:0]	      dbg_mscratch = 32'hxxxxxxxx;
+   reg [31:0]	      dbg_mtvec    = 32'hxxxxxxxx;
+   reg [31:0]	      dbg_mepc     = 32'hxxxxxxxx;
+   reg [31:0]	      dbg_mtval    = 32'hxxxxxxxx;
+   reg [31:0]	      x1  = 32'hxxxxxxxx;
+   reg [31:0]	      x2  = 32'hxxxxxxxx;
+   reg [31:0]	      x3  = 32'hxxxxxxxx;
+   reg [31:0]	      x4  = 32'hxxxxxxxx;
+   reg [31:0]	      x5  = 32'hxxxxxxxx;
+   reg [31:0]	      x6  = 32'hxxxxxxxx;
+   reg [31:0]	      x7  = 32'hxxxxxxxx;
+   reg [31:0]	      x8  = 32'hxxxxxxxx;
+   reg [31:0]	      x9  = 32'hxxxxxxxx;
+   reg [31:0]	      x10 = 32'hxxxxxxxx;
+   reg [31:0]	      x11 = 32'hxxxxxxxx;
+   reg [31:0]	      x12 = 32'hxxxxxxxx;
+   reg [31:0]	      x13 = 32'hxxxxxxxx;
+   reg [31:0]	      x14 = 32'hxxxxxxxx;
+   reg [31:0]	      x15 = 32'hxxxxxxxx;
+   reg [31:0]	      x16 = 32'hxxxxxxxx;
+   reg [31:0]	      x17 = 32'hxxxxxxxx;
+   reg [31:0]	      x18 = 32'hxxxxxxxx;
+   reg [31:0]	      x19 = 32'hxxxxxxxx;
+   reg [31:0]	      x20 = 32'hxxxxxxxx;
+   reg [31:0]	      x21 = 32'hxxxxxxxx;
+   reg [31:0]	      x22 = 32'hxxxxxxxx;
+   reg [31:0]	      x23 = 32'hxxxxxxxx;
+   reg [31:0]	      x24 = 32'hxxxxxxxx;
+   reg [31:0]	      x25 = 32'hxxxxxxxx;
+   reg [31:0]	      x26 = 32'hxxxxxxxx;
+   reg [31:0]	      x27 = 32'hxxxxxxxx;
+   reg [31:0]	      x28 = 32'hxxxxxxxx;
+   reg [31:0]	      x29 = 32'hxxxxxxxx;
+   reg [31:0]	      x30 = 32'hxxxxxxxx;
+   reg [31:0]	      x31 = 32'hxxxxxxxx;
+
+   always @(posedge i_clk) begin
+      update_rd <= i_cnt_done & i_wen0;
+
+      if (i_wen0)
+        dbg_rd <= {i_wdata0,dbg_rd[31:W]};
+
+      //End of instruction that writes to RF
+      if (update_rd) begin
+	 case (i_rd_addr)
+	   5'd1  : x1  <= dbg_rd;
+	   5'd2  : x2  <= dbg_rd;
+	   5'd3  : x3  <= dbg_rd;
+	   5'd4  : x4  <= dbg_rd;
+	   5'd5  : x5  <= dbg_rd;
+	   5'd6  : x6  <= dbg_rd;
+	   5'd7  : x7  <= dbg_rd;
+	   5'd8  : x8  <= dbg_rd;
+	   5'd9  : x9  <= dbg_rd;
+	   5'd10 : x10 <= dbg_rd;
+	   5'd11 : x11 <= dbg_rd;
+	   5'd12 : x12 <= dbg_rd;
+	   5'd13 : x13 <= dbg_rd;
+	   5'd14 : x14 <= dbg_rd;
+	   5'd15 : x15 <= dbg_rd;
+	   5'd16 : x16 <= dbg_rd;
+	   5'd17 : x17 <= dbg_rd;
+	   5'd18 : x18 <= dbg_rd;
+	   5'd19 : x19 <= dbg_rd;
+	   5'd20 : x20 <= dbg_rd;
+	   5'd21 : x21 <= dbg_rd;
+	   5'd22 : x22 <= dbg_rd;
+	   5'd23 : x23 <= dbg_rd;
+	   5'd24 : x24 <= dbg_rd;
+	   5'd25 : x25 <= dbg_rd;
+	   5'd26 : x26 <= dbg_rd;
+	   5'd27 : x27 <= dbg_rd;
+	   5'd28 : x28 <= dbg_rd;
+	   5'd29 : x29 <= dbg_rd;
+	   5'd30 : x30 <= dbg_rd;
+	   5'd31 : x31 <= dbg_rd;
+	   default : ;
+	 endcase
+      end
+
+      update_mscratch <= i_cnt_done & i_csr_en & (i_csr_addr == 2'b00);
+      update_mtvec    <= i_cnt_done & i_csr_en & (i_csr_addr == 2'b01);
+      update_mepc     <= i_cnt_done & i_csr_en & (i_csr_addr == 2'b10);
+      update_mtval    <= i_cnt_done & i_csr_en & (i_csr_addr == 2'b11);
+      update_mstatus  <= i_cnt_done & i_csr_mstatus_en;
+      update_mie      <= i_cnt_done & i_csr_mie_en;
+      update_mcause   <= i_cnt_done & i_csr_mcause_en;
+
+      if (i_cnt_en)
+	dbg_csr <= {i_csr_in, dbg_csr[31:W]};
+
+      if (update_mscratch) dbg_mscratch <= dbg_csr;
+      if (update_mtvec)    dbg_mtvec    <= dbg_csr;
+      if (update_mepc )    dbg_mepc     <= dbg_csr;
+      if (update_mtval)    dbg_mtval    <= dbg_csr;
+      if (update_mstatus)  dbg_mstatus  <= dbg_csr;
+      if (update_mie)      dbg_mie      <= dbg_csr;
+      if (update_mcause)   dbg_mcause   <= dbg_csr;
+   end
+
+   reg LUI, AUIPC, JAL, JALR, BEQ, BNE, BLT, BGE, BLTU, BGEU, LB, LH, LW, LBU, LHU, SB, SH, SW, ADDI, SLTI, SLTIU, XORI, ORI, ANDI,SLLI, SRLI, SRAI, ADD, SUB, SLL, SLT, SLTU, XOR, SRL, SRA, OR, AND, FENCE, ECALL, EBREAK;
+   reg CSRRW, CSRRS, CSRRC, CSRRWI, CSRRSI, CSRRCI;
+   reg OTHER;
+
+   always @(posedge i_clk) begin
+      if (i_ibus_ack) begin
+	 LUI    <= 1'b0;
+	 AUIPC  <= 1'b0;
+	 JAL    <= 1'b0;
+	 JALR   <= 1'b0;
+	 BEQ    <= 1'b0;
+	 BNE    <= 1'b0;
+	 BLT    <= 1'b0;
+	 BGE    <= 1'b0;
+	 BLTU   <= 1'b0;
+	 BGEU   <= 1'b0;
+	 LB     <= 1'b0;
+	 LH     <= 1'b0;
+	 LW     <= 1'b0;
+	 LBU    <= 1'b0;
+	 LHU    <= 1'b0;
+	 SB     <= 1'b0;
+	 SH     <= 1'b0;
+	 SW     <= 1'b0;
+	 ADDI   <= 1'b0;
+	 SLTI   <= 1'b0;
+	 SLTIU  <= 1'b0;
+	 XORI   <= 1'b0;
+	 ORI    <= 1'b0;
+	 ANDI   <= 1'b0;
+	 SLLI   <= 1'b0;
+	 SRLI   <= 1'b0;
+	 SRAI   <= 1'b0;
+	 ADD    <= 1'b0;
+	 SUB    <= 1'b0;
+	 SLL    <= 1'b0;
+	 SLT    <= 1'b0;
+	 SLTU   <= 1'b0;
+	 XOR    <= 1'b0;
+	 SRL    <= 1'b0;
+	 SRA    <= 1'b0;
+	 OR     <= 1'b0;
+	 AND    <= 1'b0;
+	 FENCE  <= 1'b0;
+	 ECALL  <= 1'b0;
+	 EBREAK <= 1'b0;
+	 CSRRW  <= 1'b0;
+	 CSRRS  <= 1'b0;
+	 CSRRC  <= 1'b0;
+	 CSRRWI <= 1'b0;
+	 CSRRSI <= 1'b0;
+	 CSRRCI <= 1'b0;
+	 OTHER  <= 1'b0;
+
+	 casez(i_ibus_rdt)
+	   //  3322222_22222 11111_111 11
+	   //  1098765_43210 98765_432 10987_65432_10
+	   32'b???????_?????_?????_???_?????_01101_11 : LUI    <= 1'b1;
+	   32'b???????_?????_?????_???_?????_00101_11 : AUIPC  <= 1'b1;
+	   32'b???????_?????_?????_???_?????_11011_11 : JAL    <= 1'b1;
+	   32'b???????_?????_?????_000_?????_11001_11 : JALR   <= 1'b1;
+	   32'b???????_?????_?????_000_?????_11000_11 : BEQ    <= 1'b1;
+	   32'b???????_?????_?????_001_?????_11000_11 : BNE    <= 1'b1;
+	   32'b???????_?????_?????_100_?????_11000_11 : BLT    <= 1'b1;
+	   32'b???????_?????_?????_101_?????_11000_11 : BGE    <= 1'b1;
+	   32'b???????_?????_?????_110_?????_11000_11 : BLTU   <= 1'b1;
+	   32'b???????_?????_?????_111_?????_11000_11 : BGEU   <= 1'b1;
+	   32'b???????_?????_?????_000_?????_00000_11 : LB     <= 1'b1;
+	   32'b???????_?????_?????_001_?????_00000_11 : LH     <= 1'b1;
+	   32'b???????_?????_?????_010_?????_00000_11 : LW     <= 1'b1;
+	   32'b???????_?????_?????_100_?????_00000_11 : LBU    <= 1'b1;
+	   32'b???????_?????_?????_101_?????_00000_11 : LHU    <= 1'b1;
+	   32'b???????_?????_?????_000_?????_01000_11 : SB     <= 1'b1;
+	   32'b???????_?????_?????_001_?????_01000_11 : SH     <= 1'b1;
+	   32'b???????_?????_?????_010_?????_01000_11 : SW     <= 1'b1;
+	   32'b???????_?????_?????_000_?????_00100_11 : ADDI   <= 1'b1;
+	   32'b???????_?????_?????_010_?????_00100_11 : SLTI   <= 1'b1;
+	   32'b???????_?????_?????_011_?????_00100_11 : SLTIU  <= 1'b1;
+	   32'b???????_?????_?????_100_?????_00100_11 : XORI   <= 1'b1;
+	   32'b???????_?????_?????_110_?????_00100_11 : ORI    <= 1'b1;
+	   32'b???????_?????_?????_111_?????_00100_11 : ANDI   <= 1'b1;
+	   32'b0000000_?????_?????_001_?????_00100_11 : SLLI   <= 1'b1;
+	   32'b0000000_?????_?????_101_?????_00100_11 : SRLI   <= 1'b1;
+	   32'b0100000_?????_?????_101_?????_00100_11 : SRAI   <= 1'b1;
+	   32'b0000000_?????_?????_000_?????_01100_11 : ADD    <= 1'b1;
+	   32'b0100000_?????_?????_000_?????_01100_11 : SUB    <= 1'b1;
+	   32'b0000000_?????_?????_001_?????_01100_11 : SLL    <= 1'b1;
+	   32'b0000000_?????_?????_010_?????_01100_11 : SLT    <= 1'b1;
+	   32'b0000000_?????_?????_011_?????_01100_11 : SLTU   <= 1'b1;
+	   32'b???????_?????_?????_100_?????_01100_11 : XOR    <= 1'b1;
+	   32'b0000000_?????_?????_101_?????_01100_11 : SRL    <= 1'b1;
+	   32'b0100000_?????_?????_101_?????_01100_11 : SRA    <= 1'b1;
+	   32'b???????_?????_?????_110_?????_01100_11 : OR     <= 1'b1;
+	   32'b???????_?????_?????_111_?????_01100_11 : AND    <= 1'b1;
+	   32'b???????_?????_?????_000_?????_00011_11 : FENCE  <= 1'b1;
+	   32'b0000000_00000_00000_000_00000_11100_11 : ECALL  <= 1'b1;
+	   32'b0000000_00001_00000_000_00000_11100_11 : EBREAK <= 1'b1;
+	   32'b???????_?????_?????_001_?????_11100_11 : CSRRW  <= 1'b1;
+	   32'b???????_?????_?????_010_?????_11100_11 : CSRRS  <= 1'b1;
+	   32'b???????_?????_?????_011_?????_11100_11 : CSRRC  <= 1'b1;
+	   32'b???????_?????_?????_101_?????_11100_11 : CSRRWI <= 1'b1;
+	   32'b???????_?????_?????_110_?????_11100_11 : CSRRSI <= 1'b1;
+	   32'b???????_?????_?????_111_?????_11100_11 : CSRRCI <= 1'b1;
+ 	   default : OTHER <= 1'b1;
+	 endcase
+      end
+   end
+
+`ifdef RISCV_FORMAL
+   reg [31:0] 	 pc = RESET_PC;
+
+   wire rs_en = two_stage_op ? init : i_ctrl_pc_en;
+
+   assign rvfi_rd_wdata = update_rd ? dbg_rd : 32'd0;
+
+   always @(posedge i_clk) begin
+      /* End of instruction */
+      rvfi_valid <= i_cnt_done & i_ctrl_pc_en & !i_rst;
+      rvfi_order <= rvfi_order + {63'd0,rvfi_valid};
+
+      /* Get instruction word when it's fetched from ibus */
+      if (i_ibus_cyc & i_ibus_ack)
+	rvfi_insn <= i_ibus_rdt;
+
+
+      if (i_cnt_done & i_ctrl_pc_en) begin
+         rvfi_pc_rdata <= pc;
+	 if (!(rd_en & (|i_rd_addr))) begin
+	   rvfi_rd_addr <= 5'd0;
+	 end
+      end
+      rvfi_trap <= trap;
+      if (rvfi_valid) begin
+         rvfi_trap <= 1'b0;
+         pc <= rvfi_pc_wdata;
+      end
+
+      /* RS1 not valid during J, U instructions (immdec_en[1]) */
+      /* RS2 not valid during I, J, U instructions (immdec_en[2]) */
+      if (i_rf_ready) begin
+	 rvfi_rs1_addr <= !immdec_en[1] ? rs1_addr : 5'd0;
+         rvfi_rs2_addr <= !immdec_en[2] /*rs2_valid*/ ? rs2_addr : 5'd0;
+	 rvfi_rd_addr  <= i_rd_addr;
+      end
+      if (rs_en) begin
+         rvfi_rs1_rdata <= {(!immdec_en[1] ? rs1 : {W{1'b0}}),rvfi_rs1_rdata[31:W]};
+         rvfi_rs2_rdata <= {(!immdec_en[2] ? rs2 : {W{1'b0}}),rvfi_rs2_rdata[31:W]};
+      end
+
+      if (i_dbus_ack) begin
+         rvfi_mem_addr  <= i_dbus_adr;
+         rvfi_mem_rmask <= i_dbus_we ? 4'b0000 : i_dbus_sel;
+         rvfi_mem_wmask <= i_dbus_we ? i_dbus_sel : 4'b0000;
+         rvfi_mem_rdata <= i_dbus_rdt;
+         rvfi_mem_wdata <= i_dbus_dat;
+      end
+      if (i_ibus_ack) begin
+         rvfi_mem_rmask <= 4'b0000;
+         rvfi_mem_wmask <= 4'b0000;
+      end
+   end
+
+   assign rvfi_pc_wdata = i_ibus_adr;
+
+`endif
+
+endmodule

rtl/serv/serv_decode.v

@@ -0,0 +1,367 @@
+/*
+ * serv_decode.v : SERV module decoding instruction word into control signals
+ *
+ * SPDX-FileCopyrightText: 2018 Olof Kindgren <olof@award-winning.me>
+ * SPDX-License-Identifier: ISC
+ */
+`default_nettype none
+module serv_decode
+  #(parameter [0:0] PRE_REGISTER = 1,
+    parameter [0:0] MDU = 0)
+  (
+   input wire        clk,
+   //Input
+   input wire [31:2] i_wb_rdt,
+   input wire        i_wb_en,
+   //To state
+   output reg       o_sh_right,
+   output reg       o_bne_or_bge,
+   output reg       o_cond_branch,
+   output reg       o_e_op,
+   output reg       o_ebreak,
+   output reg       o_branch_op,
+   output reg       o_shift_op,
+   output reg       o_rd_op,
+   output reg       o_two_stage_op,
+   output reg       o_dbus_en,
+   //MDU
+   output reg       o_mdu_op,
+   //Extension
+   output reg [2:0] o_ext_funct3,
+   //To bufreg
+   output reg       o_bufreg_rs1_en,
+   output reg       o_bufreg_imm_en,
+   output reg       o_bufreg_clr_lsb,
+   output reg       o_bufreg_sh_signed,
+   //To ctrl
+   output reg       o_ctrl_jal_or_jalr,
+   output reg       o_ctrl_utype,
+   output reg       o_ctrl_pc_rel,
+   output reg       o_ctrl_mret,
+   //To alu
+   output reg       o_alu_sub,
+   output reg [1:0] o_alu_bool_op,
+   output reg       o_alu_cmp_eq,
+   output reg       o_alu_cmp_sig,
+   output reg [2:0] o_alu_rd_sel,
+   //To mem IF
+   output reg       o_mem_signed,
+   output reg       o_mem_word,
+   output reg       o_mem_half,
+   output reg       o_mem_cmd,
+   //To CSR
+   output reg       o_csr_en,
+   output reg [1:0] o_csr_addr,
+   output reg       o_csr_mstatus_en,
+   output reg       o_csr_mie_en,
+   output reg       o_csr_mcause_en,
+   output reg [1:0] o_csr_source,
+   output reg       o_csr_d_sel,
+   output reg       o_csr_imm_en,
+   output reg       o_mtval_pc,
+   //To top
+   output reg [3:0] o_immdec_ctrl,
+   output reg [3:0] o_immdec_en,
+   output reg       o_op_b_source,
+   //To RF IF
+   output reg       o_rd_mem_en,
+   output reg       o_rd_csr_en,
+   output reg       o_rd_alu_en);
+
+   reg [4:0] opcode;
+   reg [2:0] funct3;
+   reg        op20;
+   reg        op21;
+   reg        op22;
+   reg        op26;
+
+   reg       imm25;
+   reg       imm30;
+
+   wire co_mdu_op     = MDU & (opcode == 5'b01100) & imm25;
+
+   wire co_two_stage_op =
+	~opcode[2] | (funct3[0] & ~funct3[1] & ~opcode[0] & ~opcode[4]) |
+	(funct3[1] & ~funct3[2] & ~opcode[0] & ~opcode[4]) | co_mdu_op;
+   wire co_shift_op = (opcode[2] & ~funct3[1]) & !co_mdu_op;
+   wire co_branch_op = opcode[4];
+   wire co_dbus_en    = ~opcode[2] & ~opcode[4];
+   wire co_mtval_pc   = opcode[4];
+   wire co_mem_word   = funct3[1];
+   wire co_rd_alu_en  = !opcode[0] & opcode[2] & !opcode[4] & !co_mdu_op;
+   wire co_rd_mem_en  = (!opcode[2] & !opcode[0]) | co_mdu_op;
+   wire [2:0] co_ext_funct3 = funct3;
+
+   //jal,branch =     imm
+   //jalr       = rs1+imm
+   //mem        = rs1+imm
+   //shift      = rs1
+   wire co_bufreg_rs1_en = !opcode[4] | (!opcode[1] & opcode[0]);
+   wire co_bufreg_imm_en = !opcode[2];
+
+   //Clear LSB of immediate for BRANCH and JAL ops
+   //True for BRANCH and JAL
+   //False for JALR/LOAD/STORE/OP/OPIMM?
+   wire co_bufreg_clr_lsb = opcode[4] & ((opcode[1:0] == 2'b00) | (opcode[1:0] == 2'b11));
+
+   //Conditional branch
+   //True for BRANCH
+   //False for JAL/JALR
+   wire co_cond_branch = !opcode[0];
+
+   wire co_ctrl_utype       = !opcode[4] & opcode[2] & opcode[0];
+   wire co_ctrl_jal_or_jalr = opcode[4] & opcode[0];
+
+   //PC-relative operations
+   //True for jal, b* auipc, ebreak
+   //False for jalr, lui
+   wire co_ctrl_pc_rel = (opcode[2:0] == 3'b000)  |
+                          (opcode[1:0] == 2'b11)  |
+                          (opcode[4] & opcode[2]) & op20|
+                          (opcode[4:3] == 2'b00);
+   //Write to RD
+   //True for OP-IMM, AUIPC, OP, LUI, SYSTEM, JALR, JAL, LOAD
+   //False for STORE, BRANCH, MISC-MEM
+   wire co_rd_op = (opcode[2] |
+                     (!opcode[2] & opcode[4] & opcode[0]) |
+                     (!opcode[2] & !opcode[3] & !opcode[0]));
+
+   //
+   //funct3
+   //
+
+   wire co_sh_right   = funct3[2];
+   wire co_bne_or_bge = funct3[0];
+
+   //Matches system ops except ecall/ebreak/mret
+   wire csr_op = opcode[4] & opcode[2] & (|funct3);
+
+
+   //op20
+   wire co_ebreak = op20;
+
+
+   //opcode & funct3 & op21
+
+   wire co_ctrl_mret = opcode[4] & opcode[2] & op21 & !(|funct3);
+   //Matches system opcodes except CSR accesses (funct3 == 0)
+   //and mret (!op21)
+   wire co_e_op = opcode[4] & opcode[2] & !op21 & !(|funct3);
+
+   //opcode & funct3 & imm30
+
+   wire co_bufreg_sh_signed = imm30;
+
+   /*
+    True for sub, b*, slt*
+    False for add*
+    op    opcode f3  i30
+    b*    11000  xxx x   t
+    addi  00100  000 x   f
+    slt*  0x100  01x x   t
+    add   01100  000 0   f
+    sub   01100  000 1   t
+    */
+   wire co_alu_sub = funct3[1] | funct3[0] | (opcode[3] & imm30) | opcode[4];
+
+   /*
+    Bits 26, 22, 21 and 20 are enough to uniquely identify the eight supported CSR regs
+    mtvec, mscratch, mepc and mtval are stored externally (normally in the RF) and are
+    treated differently from mstatus, mie and mcause which are stored in serv_csr.
+
+    The former get a 2-bit address as seen below while the latter get a
+    one-hot enable signal each.
+
+    Hex|2 222|Reg     |csr
+    adr|6 210|name    |addr
+    ---|-----|--------|----
+    300|0_000|mstatus | xx
+    304|0_100|mie     | xx
+    305|0_101|mtvec   | 01
+    340|1_000|mscratch| 00
+    341|1_001|mepc    | 10
+    342|1_010|mcause  | xx
+    343|1_011|mtval   | 11
+
+    */
+
+   //true  for mtvec,mscratch,mepc and mtval
+   //false for mstatus, mie, mcause
+   wire csr_valid = op20 | (op26 & !op21);
+
+   wire co_rd_csr_en = csr_op;
+
+   wire co_csr_en         = csr_op & csr_valid;
+   wire co_csr_mstatus_en = csr_op & !op26 & !op22 & !op20;
+   wire co_csr_mie_en     = csr_op & !op26 &  op22 & !op20;
+   wire co_csr_mcause_en  = csr_op         &  op21 & !op20;
+
+   wire [1:0] co_csr_source = funct3[1:0];
+   wire co_csr_d_sel = funct3[2];
+   wire co_csr_imm_en = opcode[4] & opcode[2] & funct3[2];
+   wire [1:0] co_csr_addr = {op26 & op20, !op26 | op21};
+
+   wire co_alu_cmp_eq = funct3[2:1] == 2'b00;
+
+   wire co_alu_cmp_sig = ~((funct3[0] & funct3[1]) | (funct3[1] & funct3[2]));
+
+   wire co_mem_cmd  = opcode[3];
+   wire co_mem_signed = ~funct3[2];
+   wire co_mem_half   = funct3[0];
+
+   wire [1:0] co_alu_bool_op = funct3[1:0];
+
+   wire [3:0] co_immdec_ctrl;
+   //True for S (STORE) or B (BRANCH) type instructions
+   //False for J type instructions
+   assign co_immdec_ctrl[0] = opcode[3:0] == 4'b1000;
+   //True for OP-IMM, LOAD, STORE, JALR  (I S)
+   //False for LUI, AUIPC, JAL           (U J)
+   assign co_immdec_ctrl[1] = (opcode[1:0] == 2'b00) | (opcode[2:1] == 2'b00);
+   assign co_immdec_ctrl[2] = opcode[4] & !opcode[0];
+   assign co_immdec_ctrl[3] = opcode[4];
+
+   wire [3:0] co_immdec_en;
+   assign co_immdec_en[3] = opcode[4] | opcode[3] | opcode[2] | !opcode[0];                 //B I J S U
+   assign co_immdec_en[2] = (opcode[4] & opcode[2]) | !opcode[3] | opcode[0];               //  I J   U
+   assign co_immdec_en[1] = (opcode[2:1] == 2'b01) | (opcode[2] & opcode[0]) | co_csr_imm_en;//    J   U
+   assign co_immdec_en[0] = ~co_rd_op;                                                       //B     S
+
+   wire [2:0] co_alu_rd_sel;
+   assign co_alu_rd_sel[0] = (funct3 == 3'b000); // Add/sub
+   assign co_alu_rd_sel[1] = (funct3[2:1] == 2'b01); //SLT*
+   assign co_alu_rd_sel[2] = funct3[2]; //Bool
+
+   //0 (OP_B_SOURCE_IMM) when OPIMM
+   //1 (OP_B_SOURCE_RS2) when BRANCH or OP
+   wire co_op_b_source = opcode[3];
+
+   generate
+      if (PRE_REGISTER) begin : gen_pre_register
+
+         always @(posedge clk) begin
+            if (i_wb_en) begin
+               funct3 <= i_wb_rdt[14:12];
+               imm30  <= i_wb_rdt[30];
+               imm25  <= i_wb_rdt[25];
+               opcode <= i_wb_rdt[6:2];
+               op20   <= i_wb_rdt[20];
+               op21   <= i_wb_rdt[21];
+               op22   <= i_wb_rdt[22];
+               op26   <= i_wb_rdt[26];
+            end
+         end
+
+         always @(*) begin
+            o_sh_right         = co_sh_right;
+            o_bne_or_bge       = co_bne_or_bge;
+            o_cond_branch      = co_cond_branch;
+            o_dbus_en          = co_dbus_en;
+            o_mtval_pc         = co_mtval_pc;
+	    o_two_stage_op     = co_two_stage_op;
+            o_e_op             = co_e_op;
+            o_ebreak           = co_ebreak;
+            o_branch_op        = co_branch_op;
+            o_shift_op         = co_shift_op;
+            o_rd_op            = co_rd_op;
+            o_mdu_op           = co_mdu_op;
+            o_ext_funct3       = co_ext_funct3;
+            o_bufreg_rs1_en    = co_bufreg_rs1_en;
+            o_bufreg_imm_en    = co_bufreg_imm_en;
+            o_bufreg_clr_lsb   = co_bufreg_clr_lsb;
+            o_bufreg_sh_signed = co_bufreg_sh_signed;
+            o_ctrl_jal_or_jalr = co_ctrl_jal_or_jalr;
+            o_ctrl_utype       = co_ctrl_utype;
+            o_ctrl_pc_rel      = co_ctrl_pc_rel;
+            o_ctrl_mret        = co_ctrl_mret;
+            o_alu_sub          = co_alu_sub;
+            o_alu_bool_op      = co_alu_bool_op;
+            o_alu_cmp_eq       = co_alu_cmp_eq;
+            o_alu_cmp_sig      = co_alu_cmp_sig;
+            o_alu_rd_sel       = co_alu_rd_sel;
+            o_mem_signed       = co_mem_signed;
+            o_mem_word         = co_mem_word;
+            o_mem_half         = co_mem_half;
+            o_mem_cmd          = co_mem_cmd;
+            o_csr_en           = co_csr_en;
+            o_csr_addr         = co_csr_addr;
+            o_csr_mstatus_en   = co_csr_mstatus_en;
+            o_csr_mie_en       = co_csr_mie_en;
+            o_csr_mcause_en    = co_csr_mcause_en;
+            o_csr_source       = co_csr_source;
+            o_csr_d_sel        = co_csr_d_sel;
+            o_csr_imm_en       = co_csr_imm_en;
+            o_immdec_ctrl      = co_immdec_ctrl;
+            o_immdec_en        = co_immdec_en;
+            o_op_b_source      = co_op_b_source;
+            o_rd_csr_en        = co_rd_csr_en;
+            o_rd_alu_en        = co_rd_alu_en;
+            o_rd_mem_en        = co_rd_mem_en;
+         end
+
+      end else begin : gen_post_register
+
+         always @(*) begin
+            funct3  = i_wb_rdt[14:12];
+            imm30   = i_wb_rdt[30];
+            imm25   = i_wb_rdt[25];
+            opcode  = i_wb_rdt[6:2];
+            op20    = i_wb_rdt[20];
+            op21    = i_wb_rdt[21];
+            op22    = i_wb_rdt[22];
+            op26    = i_wb_rdt[26];
+         end
+
+         always @(posedge clk) begin
+            if (i_wb_en) begin
+               o_sh_right         <= co_sh_right;
+               o_bne_or_bge       <= co_bne_or_bge;
+               o_cond_branch      <= co_cond_branch;
+               o_e_op             <= co_e_op;
+               o_ebreak           <= co_ebreak;
+               o_two_stage_op     <= co_two_stage_op;
+               o_dbus_en          <= co_dbus_en;
+               o_mtval_pc         <= co_mtval_pc;
+               o_branch_op        <= co_branch_op;
+               o_shift_op         <= co_shift_op;
+               o_rd_op            <= co_rd_op;
+               o_mdu_op           <= co_mdu_op;
+               o_ext_funct3       <= co_ext_funct3;
+               o_bufreg_rs1_en    <= co_bufreg_rs1_en;
+               o_bufreg_imm_en    <= co_bufreg_imm_en;
+               o_bufreg_clr_lsb   <= co_bufreg_clr_lsb;
+               o_bufreg_sh_signed <= co_bufreg_sh_signed;
+               o_ctrl_jal_or_jalr <= co_ctrl_jal_or_jalr;
+               o_ctrl_utype       <= co_ctrl_utype;
+               o_ctrl_pc_rel      <= co_ctrl_pc_rel;
+               o_ctrl_mret        <= co_ctrl_mret;
+               o_alu_sub          <= co_alu_sub;
+               o_alu_bool_op      <= co_alu_bool_op;
+               o_alu_cmp_eq       <= co_alu_cmp_eq;
+               o_alu_cmp_sig      <= co_alu_cmp_sig;
+               o_alu_rd_sel       <= co_alu_rd_sel;
+               o_mem_signed       <= co_mem_signed;
+               o_mem_word         <= co_mem_word;
+               o_mem_half         <= co_mem_half;
+               o_mem_cmd          <= co_mem_cmd;
+               o_csr_en           <= co_csr_en;
+               o_csr_addr         <= co_csr_addr;
+               o_csr_mstatus_en   <= co_csr_mstatus_en;
+               o_csr_mie_en       <= co_csr_mie_en;
+               o_csr_mcause_en    <= co_csr_mcause_en;
+               o_csr_source       <= co_csr_source;
+               o_csr_d_sel        <= co_csr_d_sel;
+               o_csr_imm_en       <= co_csr_imm_en;
+               o_immdec_ctrl      <= co_immdec_ctrl;
+               o_immdec_en        <= co_immdec_en;
+               o_op_b_source      <= co_op_b_source;
+               o_rd_csr_en        <= co_rd_csr_en;
+               o_rd_alu_en        <= co_rd_alu_en;
+               o_rd_mem_en        <= co_rd_mem_en;
+            end
+         end
+
+      end
+   endgenerate
+
+endmodule

rtl/serv/serv_immdec.v

@@ -0,0 +1,234 @@
+/*
+ * serv_immdec.v : SERV module for decoding immediates from instruction words
+ *
+ * SPDX-FileCopyrightText: 2020 Olof Kindgren <olof@award-winning.me>
+ * SPDX-License-Identifier: ISC
+ */
+`default_nettype none
+module serv_immdec
+  #(parameter SHARED_RFADDR_IMM_REGS = 1,
+    parameter W = 1)
+  (
+   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 [W-1:0] o_csr_imm,
+   output wire [W-1:0] o_imm,
+   //External
+   input wire 	     i_wb_en,
+   input wire [31:7] i_wb_rdt);
+
+generate
+   if (W == 1) begin : gen_immdec_w_eq_1
+   reg 		     imm31;
+
+   reg [8:0]  imm19_12_20;
+   reg 	      imm7;
+   reg [5:0]  imm30_25;
+   reg [4:0]  imm24_20;
+   reg [4:0]  imm11_7;
+
+   assign o_csr_imm = imm19_12_20[4];
+
+   wire       signbit = imm31 & !i_csr_imm_en;
+
+      if (SHARED_RFADDR_IMM_REGS) begin : gen_shared_imm_regs
+	 assign o_rs1_addr = imm19_12_20[8:4];
+	 assign o_rs2_addr = imm24_20;
+	 assign o_rd_addr  = imm11_7;
+
+	 always @(posedge i_clk) begin
+	    if (i_wb_en) begin
+	       /* CSR immediates are always zero-extended, hence clear the signbit */
+	       imm31     <= i_wb_rdt[31];
+	    end
+	    if (i_wb_en | (i_cnt_en & i_immdec_en[1]))
+	      imm19_12_20 <= i_wb_en ? {i_wb_rdt[19:12],i_wb_rdt[20]} : {i_ctrl[3] ? signbit : imm24_20[0], imm19_12_20[8:1]};
+	    if (i_wb_en | (i_cnt_en))
+	      imm7        <= i_wb_en ? i_wb_rdt[7]                    : signbit;
+
+	    if (i_wb_en | (i_cnt_en & i_immdec_en[3]))
+	      imm30_25    <= i_wb_en ? i_wb_rdt[30:25] : {i_ctrl[2] ? imm7 : i_ctrl[1] ? signbit : imm19_12_20[0], imm30_25[5:1]};
+
+	    if (i_wb_en | (i_cnt_en & i_immdec_en[2]))
+	      imm24_20    <= i_wb_en ? i_wb_rdt[24:20] : {imm30_25[0], imm24_20[4:1]};
+
+	    if (i_wb_en | (i_cnt_en & i_immdec_en[0]))
+	      imm11_7     <= i_wb_en ? i_wb_rdt[11:7] : {imm30_25[0], imm11_7[4:1]};
+	 end
+      end else begin : gen_separate_imm_regs
+	 reg [4:0]  rd_addr;
+	 reg [4:0]  rs1_addr;
+	 reg [4:0]  rs2_addr;
+
+	 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
+	       /* CSR immediates are always zero-extended, hence clear the signbit */
+	       imm31       <= i_wb_rdt[31];
+	       imm19_12_20 <= {i_wb_rdt[19:12],i_wb_rdt[20]};
+	       imm7        <= i_wb_rdt[7];
+	       imm30_25    <= i_wb_rdt[30:25];
+	       imm24_20    <= i_wb_rdt[24:20];
+	       imm11_7     <= i_wb_rdt[11:7];
+
+               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
+	       imm19_12_20 <= {i_ctrl[3] ? signbit : imm24_20[0], imm19_12_20[8:1]};
+	       imm7        <= signbit;
+	       imm30_25    <= {i_ctrl[2] ? imm7 : i_ctrl[1] ? signbit : imm19_12_20[0], imm30_25[5:1]};
+	       imm24_20    <= {imm30_25[0], imm24_20[4:1]};
+	       imm11_7     <= {imm30_25[0], imm11_7[4:1]};
+	    end
+	 end
+      end
+
+	 assign o_imm = i_cnt_done ? signbit : i_ctrl[0] ? imm11_7[0] : imm24_20[0];
+   end else begin : gen_immdec_w_eq_4
+   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;
+
+   end
+endgenerate
+
+endmodule

rtl/serv/serv_mem_if.v

@@ -0,0 +1,59 @@
+/*
+ * serv_mem_if.v : SERV memory interface
+ *
+ * SPDX-FileCopyrightText: 2018 Olof Kindgren <olof@award-winning.me>
+ * SPDX-License-Identifier: ISC
+ */
+`default_nettype none
+module serv_mem_if
+  #(
+    parameter [0:0] WITH_CSR = 1,
+    parameter	    W = 1,
+    parameter	    B = W-1
+  )
+  (
+   input wire 	     i_clk,
+   //State
+   input wire [1:0]  i_bytecnt,
+   input wire [1:0]  i_lsb,
+   output wire 	     o_misalign,
+   //Control
+   input wire 	     i_signed,
+   input wire 	     i_word,
+   input wire 	     i_half,
+   //MDU
+   input wire 	     i_mdu_op,
+   //Data
+   input wire [B:0] i_bufreg2_q,
+   output wire [B:0] o_rd,
+   //External interface
+   output wire [3:0] o_wb_sel);
+
+   reg signbit;
+
+   wire dat_valid =
+	i_mdu_op |
+	i_word |
+	(i_bytecnt == 2'b00) |
+	(i_half & !i_bytecnt[1]);
+
+   assign o_rd = dat_valid ? i_bufreg2_q : {W{i_signed & signbit}};
+
+   assign o_wb_sel[3] = (i_lsb == 2'b11) | i_word | (i_half & i_lsb[1]);
+   assign o_wb_sel[2] = (i_lsb == 2'b10) | i_word;
+   assign o_wb_sel[1] = (i_lsb == 2'b01) | i_word | (i_half & !i_lsb[1]);
+   assign o_wb_sel[0] = (i_lsb == 2'b00);
+
+   always @(posedge i_clk) begin
+      if (dat_valid)
+        signbit <= i_bufreg2_q[B];
+   end
+
+   /*
+    mem_misalign is checked after the init stage to decide whether to do a data
+    bus transaction or go to the trap state. It is only guaranteed to be correct
+    at this time
+    */
+   assign o_misalign = WITH_CSR & ((i_lsb[0] & (i_word | i_half)) | (i_lsb[1] & i_word));
+
+endmodule

rtl/serv/serv_rf_if.v

@@ -0,0 +1,159 @@
+/*
+ * serv_rf_if.v : SERV register file interface
+ *
+ * SPDX-FileCopyrightText: 2019 Olof Kindgren <olof@award-winning.me>
+ * SPDX-License-Identifier: ISC
+ */
+`default_nettype none
+module serv_rf_if
+  #(parameter WITH_CSR = 1,
+    parameter W = 1,
+    parameter B = W-1
+  )
+  (//RF Interface
+   input wire 		      i_cnt_en,
+   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,
+
+   //Trap interface
+   input wire 		      i_trap,
+   input wire 		      i_mret,
+   input wire [B:0] i_mepc,
+   input wire                      i_mtval_pc,
+   input wire [B:0] i_bufreg_q,
+   input wire [B:0] i_bad_pc,
+   output wire [B:0] o_csr_pc,
+   //CSR interface
+   input wire 		      i_csr_en,
+   input wire [1:0] 	      i_csr_addr,
+   input wire [B:0] i_csr,
+   output wire [B:0] o_csr,
+   //RD write port
+   input wire 		      i_rd_wen,
+   input wire [4:0] 	      i_rd_waddr,
+   input wire [B:0] i_ctrl_rd,
+   input wire [B:0] i_alu_rd,
+   input wire 		      i_rd_alu_en,
+   input wire [B:0] i_csr_rd,
+   input wire 		      i_rd_csr_en,
+   input wire [B:0] i_mem_rd,
+   input wire 		      i_rd_mem_en,
+
+   //RS1 read port
+   input wire [4:0] 	      i_rs1_raddr,
+   output wire [B:0] o_rs1,
+   //RS2 read port
+   input wire [4:0] 	      i_rs2_raddr,
+   output wire [B:0] o_rs2);
+
+
+   /*
+    ********** Write side ***********
+    */
+
+   wire 	     rd_wen = i_rd_wen & (|i_rd_waddr);
+
+   generate
+   if (|WITH_CSR) begin : gen_csr
+   wire [B:0] rd =
+       {W{i_rd_alu_en}} & i_alu_rd |
+       {W{i_rd_csr_en}} & i_csr_rd |
+       {W{i_rd_mem_en}} & i_mem_rd |
+                       i_ctrl_rd;
+
+   wire [B:0]  mtval = i_mtval_pc ? i_bad_pc : i_bufreg_q;
+
+   assign 	     o_wdata0 = i_trap ? mtval  : rd;
+   assign	     o_wdata1 = i_trap ? i_mepc : i_csr;
+
+   /* Port 0 handles writes to mtval during traps and rd otherwise
+    * Port 1 handles writes to mepc during traps and csr accesses otherwise
+    *
+    * GPR registers are mapped to address 0-31 (bits 0xxxxx).
+    * Following that are four CSR registers
+    * mscratch 100000
+    * mtvec    100001
+    * mepc     100010
+    * mtval    100011
+    */
+
+   assign o_wreg0 = i_trap ? {6'b100011} : {1'b0,i_rd_waddr};
+   assign o_wreg1 = i_trap ? {6'b100010} : {4'b1000,i_csr_addr};
+
+   assign       o_wen0 = i_cnt_en & (i_trap | rd_wen);
+   assign       o_wen1 = i_cnt_en & (i_trap | i_csr_en);
+
+   /*
+    ********** Read side ***********
+    */
+
+   //0 : RS1
+   //1 : RS2 / CSR
+
+   assign o_rreg0 = {1'b0, i_rs1_raddr};
+
+   /*
+    The address of the second read port (o_rreg1) can get assigned from four
+    different sources
+
+    Normal operations : i_rs2_raddr
+    CSR access        : i_csr_addr
+    trap              : MTVEC
+    mret              : MEPC
+
+    Address 0-31 in the RF are assigned to the GPRs. After that follows the four
+    CSRs on addresses 32-35
+
+    32 MSCRATCH
+    33 MTVEC
+    34 MEPC
+    35 MTVAL
+
+    The expression below is an optimized version of this logic
+    */
+   wire sel_rs2 = !(i_trap | i_mret | i_csr_en);
+   assign o_rreg1 = {~sel_rs2,
+		     i_rs2_raddr[4:2] & {3{sel_rs2}},
+		     {1'b0,i_trap} | {i_mret,1'b0} | ({2{i_csr_en}} & i_csr_addr) | ({2{sel_rs2}} & i_rs2_raddr[1:0])};
+
+   assign o_rs1 = i_rdata0;
+   assign o_rs2 = i_rdata1;
+   assign o_csr = i_rdata1 & {W{i_csr_en}};
+   assign o_csr_pc = i_rdata1;
+
+   end else begin : gen_no_csr
+      wire [B:0] rd = (i_ctrl_rd) |
+          i_alu_rd  & {W{i_rd_alu_en}} |
+          i_mem_rd  & {W{i_rd_mem_en}};
+
+      assign 	     o_wdata0 = rd;
+      assign	     o_wdata1 = {W{1'b0}};
+
+      assign o_wreg0 = i_rd_waddr;
+      assign o_wreg1 = 5'd0;
+
+      assign       o_wen0 = i_cnt_en & rd_wen;
+      assign       o_wen1 = 1'b0;
+
+   /*
+    ********** Read side ***********
+    */
+
+      assign o_rreg0 = i_rs1_raddr;
+      assign o_rreg1 = i_rs2_raddr;
+
+      assign o_rs1 = i_rdata0;
+      assign o_rs2 = i_rdata1;
+      assign o_csr = {W{1'b0}};
+      assign o_csr_pc = {W{1'b0}};
+   end // else: !if(WITH_CSR)
+   endgenerate
+endmodule

rtl/serv/serv_rf_ram.v

@@ -0,0 +1,52 @@
+/*
+ * serv_rf_ram.v : SRAM-based RF storage for SERV
+ *
+ * SPDX-FileCopyrightText: 2019 Olof Kindgren <olof@award-winning.me>
+ * SPDX-License-Identifier: ISC
+ */
+`include "../util/clog2.vh"
+module serv_rf_ram
+  #(parameter width=0,
+    parameter csr_regs=4,
+    parameter depth=32*(32+csr_regs)/width)
+   (input wire i_clk,
+    input wire [`CLOG2(depth)-1:0] i_waddr,
+    input wire [width-1:0] 	   i_wdata,
+    input wire 			   i_wen,
+    input wire [`CLOG2(depth)-1:0] i_raddr,
+    input wire			   i_ren,
+    output wire [width-1:0] 	   o_rdata);
+
+   reg [width-1:0] 		   memory [0:depth-1];
+   reg [width-1:0] 		   rdata ;
+
+   always @(posedge i_clk) begin
+      if (i_wen)
+	memory[i_waddr] <= i_wdata;
+      rdata <= i_ren ? memory[i_raddr] : {width{1'bx}};
+   end
+
+   /* Reads from reg x0 needs to return 0
+    Check that the part of the read address corresponding to the register
+    is zero and gate the output
+    width LSB of reg index `CLOG2(width)
+    2     4                1
+    4     3                2
+    8     2                3
+    16    1                4
+    32    0                5
+    */
+   reg regzero;
+
+   always @(posedge i_clk)
+     regzero <= !(|i_raddr[`CLOG2(depth)-1:5-`CLOG2(width)]);
+
+   assign o_rdata = rdata & ~{width{regzero}};
+
+`ifdef SERV_CLEAR_RAM
+   integer i;
+   initial
+     for (i=0;i<depth;i=i+1)
+       memory[i] = {width{1'd0}};
+`endif
+endmodule

rtl/serv/serv_rf_ram_if.v

@@ -0,0 +1,187 @@
+/*
+ * serv_rf_ram_if.v : Interface between SERV and SRAM-based RF storage
+ *
+ * SPDX-FileCopyrightText: 2019 Olof Kindgren <olof@award-winning.me>
+ * SPDX-License-Identifier: ISC
+ */
+`default_nettype none
+`include "../util/clog2.vh"
+module serv_rf_ram_if
+  #(//Data width. Adjust to preferred width of SRAM data interface
+    parameter width=8,
+
+    parameter W = 1,
+    //Select reset strategy.
+    // "MINI" for resetting minimally required FFs
+    // "NONE" for relying on FFs having a defined value on startup
+    parameter reset_strategy="MINI",
+
+    //Number of CSR registers. These are allocated after the normal
+    // GPR registers in the RAM.
+    parameter csr_regs=4,
+
+    //Internal parameters calculated from above values. Do not change
+    parameter B=W-1,
+    parameter raw=`CLOG2(32+csr_regs), //Register address width
+    parameter l2w=`CLOG2(width), //log2 of width
+    parameter aw=5+raw-l2w) //Address width
+  (
+   //SERV side
+   input wire		   i_clk,
+   input wire		   i_rst,
+   input wire		   i_wreq,
+   input wire		   i_rreq,
+   output wire		   o_ready,
+   input wire [raw-1:0]	   i_wreg0,
+   input wire [raw-1:0]	   i_wreg1,
+   input wire		   i_wen0,
+   input wire		   i_wen1,
+   input wire [B:0]	   i_wdata0,
+   input wire [B:0]	   i_wdata1,
+   input wire [raw-1:0]	   i_rreg0,
+   input wire [raw-1:0]	   i_rreg1,
+   output wire [B:0]	   o_rdata0,
+   output wire [B:0]	   o_rdata1,
+   //RAM side
+   output wire [aw-1:0]	   o_waddr,
+   output wire [width-1:0] o_wdata,
+   output wire		   o_wen,
+   output wire [aw-1:0]	   o_raddr,
+   output wire		   o_ren,
+   input wire [width-1:0]  i_rdata);
+
+   localparam ratio = width/W;
+   localparam CMSB = 4-`CLOG2(W); //Counter MSB
+   localparam l2r  = `CLOG2(ratio);
+
+   reg 				   rgnt;
+   assign o_ready = rgnt | i_wreq;
+   reg [CMSB:0] 	  rcnt;
+
+   reg 		  rtrig1;
+   /*
+    ********** Write side ***********
+    */
+
+   wire [CMSB:0] 	     wcnt;
+
+   reg [width-1:0]   wdata0_r;
+   reg [width+W-1:0]   wdata1_r;
+
+   reg 		     wen0_r;
+   reg 		     wen1_r;
+   wire 	     wtrig0;
+   wire 	     wtrig1;
+
+   assign wtrig0 = rtrig1;
+
+   generate if (ratio == 2) begin : gen_wtrig_ratio_eq_2
+      assign wtrig1 =  wcnt[0];
+   end else begin : gen_wtrig_ratio_neq_2
+      reg wtrig0_r;
+      always @(posedge i_clk) wtrig0_r <= wtrig0;
+      assign wtrig1 = wtrig0_r;
+   end
+   endgenerate
+
+   assign 	     o_wdata = wtrig1 ?
+			       wdata1_r[width-1:0] :
+			       wdata0_r;
+
+   wire [raw-1:0] wreg  = wtrig1 ? i_wreg1 : i_wreg0;
+   generate if (width == 32) begin : gen_w_eq_32
+      assign o_waddr = wreg;
+   end else begin : gen_w_neq_32
+      assign o_waddr = {wreg, wcnt[CMSB:l2r]};
+   end
+   endgenerate
+
+   assign o_wen = (wtrig0 & wen0_r) | (wtrig1 & wen1_r);
+
+   assign wcnt = rcnt-4;
+
+   always @(posedge i_clk) begin
+      if (wcnt[0]) begin
+	 wen0_r    <= i_wen0;
+	 wen1_r    <= i_wen1;
+      end
+
+      wdata0_r  <= {i_wdata0,wdata0_r[width-1:W]};
+      wdata1_r  <= {i_wdata1,wdata1_r[width+W-1:W]};
+
+   end
+
+   /*
+    ********** Read side ***********
+    */
+
+
+   wire 	  rtrig0;
+
+   wire [raw-1:0] rreg = rtrig0 ? i_rreg1 : i_rreg0;
+   generate if (width == 32) begin : gen_rreg_eq_32
+      assign o_raddr = rreg;
+   end else begin : gen_rreg_neq_32
+      assign o_raddr = {rreg, rcnt[CMSB:l2r]};
+   end
+   endgenerate
+
+   reg [width-1:0]  rdata0;
+   reg [width-1-W:0]  rdata1;
+
+   reg 		    rgate;
+
+   assign o_rdata0 = rdata0[B:0];
+   assign o_rdata1 = rtrig1 ? i_rdata[B:0] : rdata1[B:0];
+
+   assign rtrig0 = (rcnt[l2r-1:0] == 1);
+
+   generate if (ratio == 2) begin : gen_ren_w_eq_2
+      assign o_ren = rgate;
+   end else begin : gen_ren_w_neq_2
+      assign o_ren = rgate & (rcnt[l2r-1:1] == 0);
+   end
+   endgenerate
+
+   reg 	      rreq_r;
+
+   generate if (ratio > 2) begin : gen_rdata1_w_neq_2
+      always @(posedge i_clk) begin
+	 rdata1 <= {{W{1'b0}},rdata1[width-W-1:W]};
+	 if (rtrig1)
+	   rdata1[width-W-1:0] <= i_rdata[width-1:W];
+      end
+   end else begin : gen_rdata1_w_eq_2
+      always @(posedge i_clk) if (rtrig1) rdata1 <= i_rdata[W*2-1:W];
+   end
+   endgenerate
+
+   always @(posedge i_clk) begin
+      if (&rcnt | i_rreq)
+	rgate <= i_rreq;
+
+      rtrig1 <= rtrig0;
+      rcnt <= rcnt+{{CMSB{1'b0}},1'b1};
+      if (i_rreq | i_wreq)
+	 rcnt <= {{CMSB-1{1'b0}},i_wreq,1'b0};
+
+      rreq_r <= i_rreq;
+      rgnt <= rreq_r;
+
+      rdata0 <= {{W{1'b0}}, rdata0[width-1:W]};
+      if (rtrig0)
+	rdata0 <= i_rdata;
+
+      if (i_rst) begin
+	 if (reset_strategy != "NONE") begin
+	    rgate <= 1'b0;
+	    rgnt <= 1'b0;
+	    rreq_r <= 1'b0;
+	    rcnt <= {CMSB+1{1'b0}};
+	 end
+      end
+   end
+
+
+
+endmodule

rtl/serv/serv_rf_top.v

@@ -0,0 +1,228 @@
+/*
+ * serv_rf_top.v : Toplevel including SERV and SRAM-based RF storage
+ *
+ * SPDX-FileCopyrightText: 2019 Olof Kindgren <olof@award-winning.me>
+ * SPDX-License-Identifier: ISC
+ */
+`default_nettype none
+`include "../util/clog2.vh"
+
+module serv_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));
+
+   serv_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/serv/serv_state.v

@@ -0,0 +1,239 @@
+/*
+ * serv_state.v : SERV module for handling internal state during instructions
+ *
+ * SPDX-FileCopyrightText: 2019 Olof Kindgren <olof@award-winning.me>
+ * SPDX-License-Identifier: ISC
+ */
+module serv_state
+  #(parameter RESET_STRATEGY = "MINI",
+    parameter [0:0] WITH_CSR = 1,
+    parameter [0:0] ALIGN =0,
+    parameter [0:0] MDU = 0,
+    parameter       W = 1
+  )
+  (
+   input wire 	     i_clk,
+   input wire 	     i_rst,
+   //State
+   input wire 	     i_new_irq,
+   input wire 	     i_alu_cmp,
+   output wire 	     o_init,
+   output wire	     o_cnt_en,
+   output wire 	     o_cnt0to3,
+   output wire 	     o_cnt12to31,
+   output wire 	     o_cnt0,
+   output wire 	     o_cnt1,
+   output wire 	     o_cnt2,
+   output wire 	     o_cnt3,
+   output wire 	     o_cnt7,
+   output wire 	     o_cnt11,
+   output wire 	     o_cnt12,
+   output wire 	     o_cnt_done,
+   output wire 	     o_bufreg_en,
+   output wire 	     o_ctrl_pc_en,
+   output reg 	     o_ctrl_jump,
+   output wire 	     o_ctrl_trap,
+   input wire 	     i_ctrl_misalign,
+   input wire 	     i_sh_done,
+   output wire [1:0] o_mem_bytecnt,
+   input wire 	     i_mem_misalign,
+   //Control
+   input wire 	     i_bne_or_bge,
+   input wire 	     i_cond_branch,
+   input wire 	     i_dbus_en,
+   input wire 	     i_two_stage_op,
+   input wire 	     i_branch_op,
+   input wire 	     i_shift_op,
+   input wire 	     i_sh_right,
+   input wire 	     i_alu_rd_sel1,
+   input wire 	     i_rd_alu_en,
+   input wire 	     i_e_op,
+   input wire 	     i_rd_op,
+   //MDU
+   input wire 	     i_mdu_op,
+   output wire 	     o_mdu_valid,
+   //Extension
+   input wire 	     i_mdu_ready,
+   //External
+   output wire 	     o_dbus_cyc,
+   input wire 	     i_dbus_ack,
+   output wire 	     o_ibus_cyc,
+   input wire 	     i_ibus_ack,
+   //RF Interface
+   output wire 	     o_rf_rreq,
+   output wire 	     o_rf_wreq,
+   input wire 	     i_rf_ready,
+   output wire 	     o_rf_rd_en);
+
+   reg 	init_done;
+   wire misalign_trap_sync;
+
+   reg [4:2] o_cnt;
+   wire [3:0] cnt_r;
+
+   reg 	     ibus_cyc;
+   //Update PC in RUN or TRAP states
+   assign o_ctrl_pc_en  = o_cnt_en & !o_init;
+
+   assign o_mem_bytecnt = o_cnt[4:3];
+
+   assign o_cnt0to3   = (o_cnt[4:2] == 3'd0);
+   assign o_cnt12to31 = (o_cnt[4] | (o_cnt[3:2] == 2'b11));
+   assign o_cnt0 = (o_cnt[4:2] == 3'd0) & cnt_r[0];
+   assign o_cnt1 = (o_cnt[4:2] == 3'd0) & cnt_r[1];
+   assign o_cnt2 = (o_cnt[4:2] == 3'd0) & cnt_r[2];
+   assign o_cnt3 = (o_cnt[4:2] == 3'd0) & cnt_r[3];
+   assign o_cnt7 = (o_cnt[4:2] == 3'd1) & cnt_r[3];
+   assign o_cnt11 = (o_cnt[4:2] == 3'd2) & cnt_r[3];
+   assign o_cnt12 = (o_cnt[4:2] == 3'd3) & cnt_r[0];
+
+   //Take branch for jump or branch instructions (opcode == 1x0xx) if
+   //a) It's an unconditional branch (opcode[0] == 1)
+   //b) It's a conditional branch (opcode[0] == 0) of type beq,blt,bltu (funct3[0] == 0) and ALU compare is true
+   //c) It's a conditional branch (opcode[0] == 0) of type bne,bge,bgeu (funct3[0] == 1) and ALU compare is false
+   //Only valid during the last cycle of INIT, when the branch condition has
+   //been calculated.
+   wire      take_branch = i_branch_op & (!i_cond_branch | (i_alu_cmp^i_bne_or_bge));
+
+   wire last_init = o_cnt_done & o_init;
+
+   //valid signal for mdu
+   assign o_mdu_valid = MDU & !o_cnt_en & init_done & i_mdu_op;
+
+   //trap_pending is only guaranteed to have correct value during the
+   // last cycle of the init stage
+   wire	trap_pending = WITH_CSR & ((take_branch & i_ctrl_misalign & !ALIGN) |
+					 (i_dbus_en   & i_mem_misalign));
+
+   //Prepare RF for writes when everything is ready to enter stage two
+   // and the first stage didn't cause a misalign exception
+   //Left shifts, SLT & Branch ops. First cycle after init
+   //Right shift. o_sh_done
+   //Mem ops. i_dbus_ack
+   //MDU ops. i_mdu_ready
+   assign o_rf_wreq = (i_shift_op & (i_sh_right ? (i_sh_done & (last_init | !o_cnt_en & init_done)) : last_init)) |
+	   	       i_dbus_ack | (MDU & i_mdu_ready) |
+	   	      (i_branch_op & (last_init & !trap_pending)) |
+	   	      (i_rd_alu_en & i_alu_rd_sel1 & last_init);
+
+   assign o_dbus_cyc = !o_cnt_en & init_done & i_dbus_en & !i_mem_misalign;
+
+   //Prepare RF for reads when a new instruction is fetched
+   // or when stage one caused an exception (rreq implies a write request too)
+   assign o_rf_rreq = i_ibus_ack | (trap_pending & last_init);
+
+   assign o_rf_rd_en = i_rd_op & !o_init;
+
+   /*
+    bufreg is used during mem, branch, and shift operations
+
+    mem : bufreg is used for dbus address. Shift in data during phase 1.
+          Shift out during phase 2 if there was a misalignment exception.
+
+    branch : Shift in during phase 1. Shift out during phase 2
+
+    shift : Shift in during phase 1. Continue shifting between phases (except
+            for the first cycle after init). Shift out during phase 2
+    */
+   
+   assign o_bufreg_en = (o_cnt_en & (o_init | ((o_ctrl_trap | i_branch_op) & i_two_stage_op))) | (i_shift_op & init_done & (i_sh_right | i_sh_done));
+
+   assign o_ibus_cyc = ibus_cyc & !i_rst;
+
+   assign o_init = i_two_stage_op & !i_new_irq & !init_done;
+
+   assign o_cnt_done = (o_cnt[4:2] == 3'b111) & cnt_r[3];
+
+   always @(posedge i_clk) begin
+      //ibus_cyc changes on three conditions.
+      //1. i_rst is asserted. Together with the async gating above, o_ibus_cyc
+      //   will be asserted as soon as the reset is released. This is how the
+      //   first instruction is fetched
+      //2. o_cnt_done and o_ctrl_pc_en are asserted. This means that SERV just
+      //   finished updating the PC, is done with the current instruction and
+      //   o_ibus_cyc gets asserted to fetch a new instruction
+      //3. When i_ibus_ack, a new instruction is fetched and o_ibus_cyc gets
+      //   deasserted to finish the transaction
+      if (i_ibus_ack | o_cnt_done | i_rst)
+	ibus_cyc <= o_ctrl_pc_en | i_rst;
+
+      if (o_cnt_done) begin
+	 init_done <= o_init & !init_done;
+	 o_ctrl_jump <= o_init & take_branch;
+      end
+
+      if (i_rst) begin
+	 if (RESET_STRATEGY != "NONE") begin
+	    init_done <= 1'b0;
+	    o_ctrl_jump <= 1'b0;
+	 end
+      end
+   end
+
+   generate
+      /*
+       Because SERV is 32-bit bit-serial we need a counter than can count 0-31
+       to keep track of which bit we are currently processing. o_cnt and cnt_r
+       are used together to create such a counter.
+       The top three bits (o_cnt) are implemented as a normal counter, but
+       instead of the two LSB, cnt_r is a 4-bit shift register which loops 0-3
+       When cnt_r[3] is 1, o_cnt will be increased.
+
+       The counting starts when the core is idle and the i_rf_ready signal
+       comes in from the RF module by shifting in the i_rf_ready bit as LSB of
+       the shift register. Counting is stopped by using o_cnt_done to block the
+       bit that was supposed to be shifted into bit 0 of cnt_r.
+
+       There are two benefit of doing the counter this way
+       1. We only need to check four bits instead of five when we want to check
+       if the counter is at a certain value. For 4-LUT architectures this means
+       we only need one LUT instead of two for each comparison.
+       2. We don't need a separate enable signal to turn on and off the counter
+       between stages, which saves an extra FF and a unique control signal. We
+       just need to check if cnt_r is not zero to see if the counter is
+       currently running
+       */
+      if (W == 1) begin : gen_cnt_w_eq_1
+	 reg [3:0] cnt_lsb;
+	 always @(posedge i_clk) begin
+            o_cnt <= o_cnt + {2'd0,cnt_r[3]};
+            cnt_lsb <= {cnt_lsb[2:0],(cnt_lsb[3] & !o_cnt_done) | i_rf_ready};
+	    if (i_rst & (RESET_STRATEGY != "NONE")) begin
+	       o_cnt   <= 3'd0;
+	       cnt_lsb <= 4'b0000;
+	    end
+	 end
+	 assign cnt_r = cnt_lsb;
+	 assign o_cnt_en = |cnt_lsb;
+      end else if (W == 4) begin : gen_cnt_w_eq_4
+	 reg cnt_en;
+	 always @(posedge i_clk) begin
+            if (i_rf_ready) cnt_en <= 1; else
+            if (o_cnt_done) cnt_en <= 0;
+            o_cnt <= o_cnt + { 2'd0, cnt_en };
+	    if (i_rst & (RESET_STRATEGY != "NONE")) begin
+	       o_cnt   <= 3'd0;
+	       cnt_en <= 1'b0;
+	    end
+	 end
+	 assign cnt_r = 4'b1111;
+	 assign o_cnt_en = cnt_en;
+      end
+   endgenerate
+
+   assign o_ctrl_trap = WITH_CSR & (i_e_op | i_new_irq | misalign_trap_sync);
+
+   generate
+      if (WITH_CSR) begin : gen_csr
+	 reg 	misalign_trap_sync_r;
+
+	 always @(posedge i_clk) begin
+	    if (i_ibus_ack | o_cnt_done | i_rst)
+	      misalign_trap_sync_r <= !(i_ibus_ack | i_rst) & ((trap_pending & o_init) | misalign_trap_sync_r);
+	 end
+	 assign misalign_trap_sync = misalign_trap_sync_r;
+      end else begin : gen_no_csr
+	 assign misalign_trap_sync = 1'b0;
+      end
+   endgenerate
+endmodule

rtl/serv/serv_synth_wrapper.v

@@ -0,0 +1,139 @@
+/*
+ * serv_synth_wrapper.v : SERV wrapper for synthesis
+ *
+ * SPDX-FileCopyrightText: 2021 Olof Kindgren <olof@award-winning.me>
+ * SPDX-License-Identifier: ISC
+ */
+`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 = 2,
+	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;
+
+   wire 	      rf_wreq;
+   wire 	      rf_rreq;
+   wire [4+WITH_CSR:0] wreg0;
+   wire [4+WITH_CSR:0] wreg1;
+   wire 	      wen0;
+   wire 	      wen1;
+   wire 	      wdata0;
+   wire 	      wdata1;
+   wire [4+WITH_CSR:0] rreg0;
+   wire [4+WITH_CSR:0] rreg1;
+   wire 	      rf_ready;
+   wire 	      rdata0;
+   wire 	      rdata1;
+
+   serv_rf_ram_if
+     #(.width    (RF_WIDTH),
+       .reset_strategy (RESET_STRATEGY),
+       .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));
+
+   serv_top
+     #(.RESET_PC (32'd0),
+       .PRE_REGISTER (PRE_REGISTER),
+       .RESET_STRATEGY (RESET_STRATEGY),
+       .WITH_CSR (WITH_CSR),
+       .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/serv/serv_top.v

@@ -0,0 +1,674 @@
+/*
+ * serv_top.v : SERV toplevel
+ *
+ * SPDX-FileCopyrightText: 2018 Olof Kindgren <olof@award-winning.me>
+ * SPDX-License-Identifier: ISC
+ */
+`default_nettype none
+
+module serv_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;
+
+   wire [31:0] i_wb_rdt;
+
+   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));
+
+   serv_immdec #(.W (W)) 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]));
+
+   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/serv/servile.v

@@ -0,0 +1,280 @@
+/*
+ * 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
+`include "../util/clog2.vh"
+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
+
+   serv_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/serv/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/serv/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/serv/servile_rf_mem_if.v

@@ -0,0 +1,78 @@
+/*
+ * 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
+`include "../util/clog2.vh"
+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/serv/serving.v

@@ -0,0 +1,149 @@
+/* serving.v : Top-level for the serving SoC
+ *
+ * ISC License
+ *
+ * Copyright (C) 2020 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.
+ */
+
+`default_nettype none
+`include "../util/clog2.vh"
+module serving
+  (
+   input wire 	      i_clk,
+   input wire 	      i_rst,
+   input wire 	      i_timer_irq,
+
+   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_stb,
+   input wire [31:0]  i_wb_rdt,
+   input wire 	      i_wb_ack);
+
+   parameter memfile = "";
+   parameter memsize = 8192;
+   parameter sim = 1'b0;
+   parameter RESET_STRATEGY = "NONE";
+   parameter WITH_CSR = 1;
+   localparam regs = 32+WITH_CSR*4;
+
+   localparam rf_width = 8;
+
+   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;
+   wire 	wb_mem_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;
+
+   wire [`CLOG2(memsize)-1:0] sram_waddr;
+   wire [rf_width-1:0] sram_wdata;
+   wire 	       sram_wen;
+   wire [`CLOG2(memsize)-1:0] sram_raddr;
+   wire [rf_width-1:0] sram_rdata;
+   wire		       sram_ren;
+
+   serving_ram
+     #(.memfile (memfile),
+       .depth   (memsize),
+       .sim     (sim))
+   ram
+     (// Wishbone interface
+      .i_clk (i_clk),
+      .i_waddr  (sram_waddr),
+      .i_wdata  (sram_wdata),
+      .i_wen    (sram_wen),
+      .i_raddr  (sram_raddr),
+      .o_rdata  (sram_rdata)/*,
+      .i_ren    (rf_ren)*/);
+
+   servile_rf_mem_if
+     #(.depth   (memsize),
+       .rf_regs (regs))
+   rf_mem_if
+     (// Wishbone interface
+      .i_clk (i_clk),
+      .i_rst (i_rst),
+
+      .i_waddr  (rf_waddr),
+      .i_wdata  (rf_wdata),
+      .i_wen    (rf_wen),
+      .i_raddr  (rf_raddr),
+      .o_rdata  (rf_rdata),
+      .i_ren    (rf_ren),
+
+      .o_sram_waddr (sram_waddr),
+      .o_sram_wdata (sram_wdata),
+      .o_sram_wen   (sram_wen),
+      .o_sram_raddr (sram_raddr),
+      .i_sram_rdata (sram_rdata),
+      .o_sram_ren   (sram_ren),
+
+      .i_wb_adr (wb_mem_adr[`CLOG2(memsize)-1:2]),
+      .i_wb_stb (wb_mem_stb),
+      .i_wb_we  (wb_mem_we) ,
+      .i_wb_sel (wb_mem_sel),
+      .i_wb_dat (wb_mem_dat),
+      .o_wb_rdt (wb_mem_rdt),
+      .o_wb_ack (wb_mem_ack));
+
+   servile
+     #(.reset_pc (32'h0000_0000),
+       .reset_strategy (RESET_STRATEGY),
+       .rf_width (rf_width),
+       .sim (sim),
+       .with_csr (WITH_CSR))
+   servile
+     (
+      .i_clk       (i_clk),
+      .i_rst       (i_rst),
+      .i_timer_irq (i_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),
+      .i_wb_mem_ack   (wb_mem_ack),
+
+      //Extension interface
+      .o_wb_ext_adr   (o_wb_adr),
+      .o_wb_ext_dat   (o_wb_dat),
+      .o_wb_ext_sel   (o_wb_sel),
+      .o_wb_ext_we    (o_wb_we),
+      .o_wb_ext_stb   (o_wb_stb),
+      .i_wb_ext_rdt   (i_wb_rdt),
+      .i_wb_ext_ack   (i_wb_ack),
+
+      //RF IF
+      .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));
+
+
+endmodule

rtl/serv/serving_ram.v

@@ -0,0 +1,54 @@
+/* serving_ram.v : I/D SRAM for the serving SoC
+ *
+ * ISC License
+ *
+ * Copyright (C) 2020 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.
+ */
+
+`default_nettype none
+`include "../util/clog2.vh"
+module serving_ram
+  #(//Memory parameters
+	parameter depth = 256,
+	parameter aw    = `CLOG2(depth),
+	parameter memfile = "",
+	parameter sim = 1'b0)
+   (input wire		i_clk,
+	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);
+
+   	reg [7:0]		mem [0:depth-1] /* verilator public */;
+
+	always @(posedge i_clk) begin
+		if (i_wen) mem[i_waddr]   <= i_wdata;
+		o_rdata <= mem[i_raddr];
+	end
+
+	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/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,46 +23,72 @@ 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 aclk) 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)
-                    count <= 0;               // wrap at 4000
             else
-                    count <= count + 1'b1;    // increment every 1 ms
+                rst_cnt <= rst_cnt + 1'b1;
-            end else begin
-                div_counter <= div_counter + 1'b1;
         end
     end
-        freq <= count;
+    wire sys_reset = sys_reset_r;
-    end
+    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)
+    ) 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),
+        .o_test(test)
+    );
 
 
     wire [15:0] sin_q15;
     wire clk_en;
     nco_q15 #(
-        .CLK_HZ(100_000_000),
+        .CLK_HZ(15_000_000),
-        .FS_HZ(40_000)
+        .FS_HZ(80_000)
     ) nco (
-        .clk    (aclk),
+        .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)
     );
 
     reg [5:0] dac_code;
-    always @(posedge aclk) begin
+    always @(posedge clk_15) begin
         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 = test;
 endmodule

rtl/toplevel/top_jtag.v

@@ -0,0 +1,73 @@
+module top_jtag(
+    input wire aclk,
+    input wire aresetn,
+
+    output wire led_green,
+    output wire led_red,
+    output wire [7:0] LED,
+
+    output wire [5:0] r2r
+);
+    // Clocking
+    wire clk_100;
+    wire clk_15;
+    assign clk_100 = aclk;
+    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;
+
+    jtag_wb_bridge #(
+        .chain(1)
+    ) jtag_wb (
+        .i_clk(clk_15),
+        .i_rst(!aresetn),
+
+        .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)
+    );
+
+    wire [31:0] gpio;
+
+    wb_gpio #(
+        .address(32'h00000000)
+    ) u_wb_gpio (
+        .i_wb_clk(clk_15),
+        .i_wb_rst(i_rst),
+        .i_wb_adr(wb_adr),
+        .i_wb_dat(wb_dat),
+        .i_wb_sel(wb_sel),
+        .i_wb_we(wb_we),
+        .i_wb_stb(wb_stb & wb_cyc),
+        .i_gpio(gpio),
+        .o_wb_rdt(wb_rdt),
+        .o_wb_ack(wb_ack),
+        .o_gpio(gpio)
+    );
+
+    assign LED = gpio[7:0];
+    assign r2r = gpio[13:8];
+    assign led_green = cmd_reset;
+    assign led_red = 'b0;
+
+endmodule

rtl/util/clog2.vh

@@ -0,0 +1,39 @@
+`ifndef CLOG2_VH
+`define CLOG2_VH
+
+// Verilog-2001 compatible ceil(log2(x)) macro (matches $clog2 semantics).
+`define CLOG2(x) \
+  (((x) <= 1) ? 0 : \
+   ((x) <= 2) ? 1 : \
+   ((x) <= 4) ? 2 : \
+   ((x) <= 8) ? 3 : \
+   ((x) <= 16) ? 4 : \
+   ((x) <= 32) ? 5 : \
+   ((x) <= 64) ? 6 : \
+   ((x) <= 128) ? 7 : \
+   ((x) <= 256) ? 8 : \
+   ((x) <= 512) ? 9 : \
+   ((x) <= 1024) ? 10 : \
+   ((x) <= 2048) ? 11 : \
+   ((x) <= 4096) ? 12 : \
+   ((x) <= 8192) ? 13 : \
+   ((x) <= 16384) ? 14 : \
+   ((x) <= 32768) ? 15 : \
+   ((x) <= 65536) ? 16 : \
+   ((x) <= 131072) ? 17 : \
+   ((x) <= 262144) ? 18 : \
+   ((x) <= 524288) ? 19 : \
+   ((x) <= 1048576) ? 20 : \
+   ((x) <= 2097152) ? 21 : \
+   ((x) <= 4194304) ? 22 : \
+   ((x) <= 8388608) ? 23 : \
+   ((x) <= 16777216) ? 24 : \
+   ((x) <= 33554432) ? 25 : \
+   ((x) <= 67108864) ? 26 : \
+   ((x) <= 134217728) ? 27 : \
+   ((x) <= 268435456) ? 28 : \
+   ((x) <= 536870912) ? 29 : \
+   ((x) <= 1073741824) ? 30 : \
+   ((x) <= 2147483648) ? 31 : 32)
+
+`endif

rtl/util/conv.vh

@@ -1,3 +1,6 @@
+`ifndef CONV_VH
+`define CONV_VH
+
 // =============================================================================
 // Convert Q1.15 to a biased UQ0.16 signal
 // =============================================================================
@@ -9,3 +12,5 @@ begin
     q15_to_uq16 = biased[15:0];
 end
 endfunction
+
+`endif

rtl/util/rc_alpha_q15.vh

@@ -7,58 +7,79 @@
 `define RC_ALPHA_Q15_VH
 
 function integer alpha_q15_from_rc;
-    input integer R_OHM; // resistance in ohms
+    input integer R_OHM; // ohms
-    input integer C_PF;  // capacitance in picofarads
+    input integer C_PF;  // picofarads
-    input integer FS_HZ; // sampling frequency in Hz
+    input integer FS_HZ; // Hz
 
-    integer N; // fractional bits for x (QN)
-    reg  [127:0] num_1e12_sllN;
-    reg  [127:0] denom_u;
-    reg  [127:0] x_qN;        // x in QN
-    reg  [255:0] x2;          // x^2 in Q(2N)
-    reg  [383:0] x3;          // x^3 in Q(3N)
-
-    integer term1_q15;        // x -> Q1.15
-    integer term2_q15;        // x^2/2 -> Q1.15
-    integer term3_q15;        // x^3/6 -> Q1.15
-    integer acc;              // accumulator for result
-begin
     // Choose QN for x. N=24 is a good balance for accuracy/width.
-    N = 24;
+    integer N;
 
-    // x = 1 / (Fs * R * C) with C in pF  ==>  x = 1e12 / (Fs * R * C_PF)
+    // We'll keep everything as unsigned vectors; inputs copied into vectors first.
+    reg [63:0] R_u, C_u, FS_u;
+
+    // x = 1 / (Fs * R * C) with C in pF -> x = 1e12 / (Fs*R*C_pf)
     // x_qN = round( x * 2^N ) = round( (1e12 << N) / denom )
-    num_1e12_sllN = 128'd1000000000000 << N;
+    reg  [127:0] NUM_1E12_SLLN;  // big enough for 1e12 << N
-
+    reg  [127:0] DENOM;          // Fs*R*C
-    // denom = Fs * R * C_PF  (fits in 64..96 bits for typical values)
+    reg  [127:0] X_qN;           // x in QN
-    denom_u = 0;
-    denom_u = denom_u + FS_HZ[127:0];
-    denom_u = denom_u * R_OHM[127:0];
-    denom_u = denom_u * C_PF[127:0];
-
-    // rounded divide for x_qN
-    x_qN = (num_1e12_sllN + (denom_u >> 1)) / denom_u;
 
     // Powers
-    x2 = x_qN * x_qN;       // 128x128 -> 256
+    reg  [255:0] X2;             // x^2 in Q(2N)
-    x3 = x2 * x_qN;         // 256x128 -> 384
+    reg  [383:0] X3;             // x^3 in Q(3N)
 
+    integer term1_q15;
+    integer term2_q15;
+    integer term3_q15;
+    integer acc;
+
+begin
+    N = 24;
+
+    // Copy integer inputs into 64-bit vectors (no bit-slicing of integers)
+    R_u  = R_OHM[31:0];
+    C_u  = C_PF[31:0];
+    FS_u = FS_HZ[31:0];
+
+    // Denominator = Fs * R * C_pf  (fits in < 2^64 for typical values)
+    DENOM = 128'd0;
+    DENOM = FS_u;
+    DENOM = DENOM * R_u;
+    DENOM = DENOM * C_u;
+
+    // // Guard: avoid divide by zero
+    // if (DENOM == 0) begin
+    //     alpha_q15_from_rc = 0;
+    //     disable alpha_q15_from_rc;
+    // end
+
+    // Numerator = (1e12 << N). 1e12 * 2^24 ≈ 1.6777e19 (fits in 2^64..2^65),
+    // so use 128 bits to be safe.
+    NUM_1E12_SLLN = 128'd1000000000000 << N;
+
+    // x_qN = rounded division
+    X_qN = (NUM_1E12_SLLN + (DENOM >> 1)) / DENOM;
+
+    // Powers
+    X2 = X_qN * X_qN;
+    X3 = X2 * X_qN;
+
+    // Convert terms to Q1.15:
     // term1 = x            -> shift from QN to Q15
-    term1_q15 = (x_qN >> (N - 15)) & 16'hFFFF;
+    term1_q15 = (X_qN >> (N - 15)) & 16'hFFFF;
 
-    // term2 = x^2 / 2  -> shift from Q(2N) to Q15 and divide by 2
+    // term2 = x^2 / 2      -> Q(2N) to Q15 and /2
-    term2_q15 = (x2 >> (2*N - 15 + 1)) & 16'hFFFF;
+    term2_q15 = (X2 >> (2*N - 15 + 1)) & 16'hFFFF;
 
-    // term3 = x^3 / 6  -> shift from Q(3N) to Q15, then divide by 6 (rounded)
+    // term3 = x^3 / 6      -> Q(3N) to Q15, then /6 with rounding
-    begin : gen_term3
+    begin : gen_t3
         reg [383:0] tmp_q15_wide;
         reg [383:0] tmp_div6;
-        tmp_q15_wide = (x3 >> (3*N - 15));
+        tmp_q15_wide = (X3 >> (3*N - 15));
-        tmp_div6     = (tmp_q15_wide + 6'd3) / 6; // +3 for rounding
+        tmp_div6     = (tmp_q15_wide + 6'd3) / 6;
         term3_q15    = tmp_div6[15:0];
     end
 
-    // Combine: alpha_q15 = x - x^2/2 + x^3/6 ; clamp to [0, 0x7FFF]
+    // Combine and clamp
     acc = term1_q15 - term2_q15 + term3_q15;
     if (acc < 0)            acc = 0;
     else if (acc > 16'h7FFF) acc = 16'h7FFF;

rtl/wb/jtag_wb_bridge.v

@@ -0,0 +1,500 @@
+`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;
+
+  // 48-bit DR (symmetrical command/response)
+  reg [47: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 (48/48 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 [47:0] a_resp_data;
+
+  wire        b_req_pulse;
+  wire [47:0] b_req_data;
+
+  reg         b_resp_pulse;
+  reg  [47:0] b_resp_data;
+  wire        b_resp_busy;
+  wire        b_resp_accepted;
+
+  // Accept UPDATE as a request strobe (qualified by SEL and !busy)
+  wire a_req_pulse = jtag_sel && jtag_update && !a_req_busy;
+  wire [47:0] a_req_data = jtag_shreg;
+
+  cdc_req_resp #(
+    .REQ_W(48),
+    .RESP_W(48),
+    .STABLE_SAMPLES(2)
+  ) u_cdc (
+    .a_clk(jtag_tck),
+    .a_rst(jtag_async_reset),
+
+    .a_req_pulse(a_req_pulse),
+    .a_req_data(a_req_data),
+    .a_req_busy(a_req_busy),
+    .a_req_accepted(a_req_accepted),
+
+    .a_resp_pulse(a_resp_pulse),
+    .a_resp_data(a_resp_data),
+
+    .b_clk(i_clk),
+    .b_rst(i_rst),
+
+    .b_req_pulse(b_req_pulse),
+    .b_req_data(b_req_data),
+
+    .b_resp_pulse(b_resp_pulse),
+    .b_resp_data(b_resp_data),
+    .b_resp_busy(b_resp_busy),
+    .b_resp_accepted(b_resp_accepted)
+  );
+
+  // ===========================================================================
+  // JTAG/TCK domain shift/capture
+  // ===========================================================================
+  reg [47:0] resp_hold_tck;
+
+  always @(posedge jtag_tck or posedge jtag_async_reset) begin
+    if (jtag_async_reset) begin
+      jtag_shreg    <= 48'd0;
+      resp_hold_tck <= 48'd0;
+    end else begin
+      // Latch new response word from CDC when it arrives (independent of CAPTURE)
+      if (a_resp_pulse) begin
+        resp_hold_tck <= a_resp_data;
+      end
+
+      if (jtag_sel && jtag_capture) begin
+        // Load response into shift register for host readout
+        jtag_shreg <= resp_hold_tck;
+      end else if (jtag_sel && jtag_shift) begin
+        // Shift: MSB in, LSB out to TDO
+        jtag_shreg <= {jtag_tdi, jtag_shreg[47:1]};
+      end
+    end
+  end
+
+  // ===========================================================================
+  // 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_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 [7:0]  act_data;
+  reg [7:0]  act_seq;
+
+  reg        q_valid;
+  reg [7:0]  q_opcode;
+  reg [31:0] q_addr;
+  reg [7:0]  q_data;
+  reg [7:0]  q_seq;
+
+  // Response pending buffer (to avoid dropping if resp mailbox busy)
+  reg        resp_pending;
+  reg [47:0] resp_pending_word;
+
+  // Lane selection
+  wire [1:0] addr_lane = byte_aligned ? 2'b00 : act_addr[1:0];
+
+  // Helpers: form SEL/DAT for byte write
+  function [3:0] sel_from_lane(input [1:0] lane);
+    case (lane)
+      2'b00: sel_from_lane = 4'b0001;
+      2'b01: sel_from_lane = 4'b0010;
+      2'b10: sel_from_lane = 4'b0100;
+      default: sel_from_lane = 4'b1000;
+    endcase
+  endfunction
+
+  function [31:0] dat_from_lane_byte(input [1:0] lane, input [7:0] b);
+    case (lane)
+      2'b00: dat_from_lane_byte = {24'b0, b};
+      2'b01: dat_from_lane_byte = {16'b0, b, 8'b0};
+      2'b10: dat_from_lane_byte = {8'b0,  b, 16'b0};
+      default: dat_from_lane_byte = {b, 24'b0};
+    endcase
+  endfunction
+
+  function [7:0] byte_from_lane(input [1:0] lane, input [31:0] w);
+    case (lane)
+      2'b00: byte_from_lane = w[7:0];
+      2'b01: byte_from_lane = w[15:8];
+      2'b10: byte_from_lane = w[23:16];
+      default: byte_from_lane = w[31:24];
+    endcase
+  endfunction
+
+  // Build response word
+  function [47:0] pack_resp(
+    input [7:0]  resp_seq,
+    input [7:0]  status,
+    input [7:0]  cmd_seq,
+    input [7:0]  data,
+    input [7:0]  flags,
+    input [7:0]  last_op
+  );
+    pack_resp = {resp_seq, status, cmd_seq, data, flags, last_op};
+  endfunction
+
+  // STATUS bits (snapshot)
+  wire [7:0] status_snapshot = {
+    2'b00,                 // [7:6]
+    1'b1,                  // [5] resp_valid
+    last_we_r,             // [4] last_we
+    cmd_reset_level_r,     // [3] reset_level
+    b_resp_busy,           // [2] resp_busy (system domain)
+    req_busy_tck_sync,     // [1] req_busy (synced from TCK just for reporting)
+    wb_busy                // [0] wb_busy
+  };
+
+  // FLAGS bits (sticky)
+  wire [7:0] flags_sticky = {
+    4'b0000,               // [7:4] reserved
+    1'b0,                  // [3] reserved
+    flag_wb_busy_at_req,   // [2]
+    flag_illegal,          // [1]
+    flag_cmd_overflow      // [0]
+  };
+
+  // Queue a command (or set overflow sticky if queue full)
+  task automatic enqueue_cmd(
+    input [7:0]  op,
+    input [31:0] addr,
+    input [7:0]  dat,
+    input [7:0]  seq
+  );
+    begin
+      if (!q_valid) begin
+        q_valid  <= 1'b1;
+        q_opcode <= op;
+        q_addr   <= addr;
+        q_data   <= dat;
+        q_seq    <= seq;
+      end else begin
+        // Already have one queued; mark overflow and drop this command
+        flag_cmd_overflow <= 1'b1;
+      end
+    end
+  endtask
+
+  // Start executing a command (for non-WB ops may immediately create a response)
+  task automatic start_active_cmd(
+    input [7:0]  cmd_opcode,
+    input [31:0] cmd_addr,
+    input [7:0]  cmd_data,
+    input [7:0]  cmd_seq
+  );
+    reg [1:0] cmd_addr_lane;
+    begin
+      cmd_addr_lane = byte_aligned ? 2'b00 : cmd_addr[1:0];
+
+      last_opcode_r <= cmd_opcode;
+      last_we_r     <= (cmd_opcode == OP_WRITE8);
+
+      // If we're already mid-flight or holding a response, note it (diagnostic)
+      if (wb_busy || resp_pending)
+        flag_wb_busy_at_req <= 1'b1;
+
+      case (cmd_opcode)
+        OP_NOP: begin
+          // immediate response
+          resp_pending_word <= pack_resp(resp_seq_r, status_snapshot, cmd_seq, 8'h00, flags_sticky, cmd_opcode);
+          resp_pending      <= 1'b1;
+        end
+
+        OP_PING: begin
+          resp_pending_word <= pack_resp(resp_seq_r, status_snapshot, cmd_seq, 8'hA5, flags_sticky, cmd_opcode);
+          resp_pending      <= 1'b1;
+        end
+
+        OP_CLEAR_FLAGS: begin
+          flag_cmd_overflow    <= 1'b0;
+          flag_illegal         <= 1'b0;
+          flag_wb_busy_at_req  <= 1'b0;
+          resp_pending_word    <= pack_resp(resp_seq_r, status_snapshot, cmd_seq, 8'h00, 8'h00, cmd_opcode);
+          resp_pending         <= 1'b1;
+        end
+
+        OP_RESET_ON: begin
+          cmd_reset_level_r <= 1'b1;
+          resp_pending_word <= pack_resp(resp_seq_r, status_snapshot, cmd_seq, 8'h00, flags_sticky, cmd_opcode);
+          resp_pending      <= 1'b1;
+        end
+
+        OP_RESET_OFF: begin
+          cmd_reset_level_r <= 1'b0;
+          resp_pending_word <= pack_resp(resp_seq_r, status_snapshot, cmd_seq, 8'h00, flags_sticky, cmd_opcode);
+          resp_pending      <= 1'b1;
+        end
+
+        OP_WRITE8: begin
+          // launch WB write (byte)
+          wb_busy  <= 1'b1;
+          wb_we_r  <= 1'b1;
+          wb_adr_r <= cmd_addr;
+          wb_sel_r <= sel_from_lane(cmd_addr_lane);
+          wb_dat_r <= dat_from_lane_byte(cmd_addr_lane, cmd_data);
+        end
+
+        OP_READ8: begin
+          // launch WB read (byte select)
+          wb_busy  <= 1'b1;
+          wb_we_r  <= 1'b0;
+          wb_adr_r <= cmd_addr;
+          wb_sel_r <= sel_from_lane(cmd_addr_lane);
+          wb_dat_r <= 32'b0;
+        end
+
+        default: begin
+          flag_illegal <= 1'b1;
+          resp_pending_word <= pack_resp(resp_seq_r, status_snapshot, cmd_seq, 8'h00, flags_sticky, cmd_opcode);
+          resp_pending      <= 1'b1;
+        end
+      endcase
+    end
+  endtask
+
+  // System main
+  always @(posedge i_clk) begin
+    if (i_rst) begin
+      wb_busy          <= 1'b0;
+      wb_adr_r         <= 32'b0;
+      wb_dat_r         <= 32'b0;
+      wb_sel_r         <= 4'b0000;
+      wb_we_r          <= 1'b0;
+
+      cmd_reset_level_r<= 1'b0;
+
+      req_busy_sync1   <= 1'b0;
+      req_busy_sync2   <= 1'b0;
+
+      cmd_seq_r        <= 8'd0;
+      resp_seq_r       <= 8'd0;
+
+      flag_cmd_overflow<= 1'b0;
+      flag_illegal     <= 1'b0;
+      flag_wb_busy_at_req <= 1'b0;
+
+      last_we_r        <= 1'b0;
+      last_opcode_r    <= 8'h00;
+
+      act_valid        <= 1'b0;
+      act_opcode       <= 8'h00;
+      act_addr         <= 32'h0;
+      act_data         <= 8'h00;
+      act_seq          <= 8'h00;
+
+      q_valid          <= 1'b0;
+      q_opcode         <= 8'h00;
+      q_addr           <= 32'h0;
+      q_data           <= 8'h00;
+      q_seq            <= 8'h00;
+
+      resp_pending     <= 1'b0;
+      resp_pending_word<= 48'h0;
+
+      b_resp_pulse     <= 1'b0;
+      b_resp_data      <= 48'h0;
+    end else begin
+      b_resp_pulse <= 1'b0;
+
+      // Sync req-busy level (reporting only)
+      req_busy_sync1 <= a_req_busy;
+      req_busy_sync2 <= req_busy_sync1;
+
+      // -----------------------------------------------------------------------
+      // Accept incoming command from CDC (always delivered; we buffer internally)
+      // -----------------------------------------------------------------------
+      if (b_req_pulse) begin
+        // assign a sequence number to each received command
+        cmd_seq_r <= cmd_seq_r + 8'd1;
+
+        // If we can start immediately (no active, no wb, no pending response), do so.
+        if (!act_valid && !wb_busy && !resp_pending) begin
+          act_valid  <= 1'b1;
+          act_opcode <= b_req_data[47:40];
+          act_addr   <= b_req_data[39:8];
+          act_data   <= b_req_data[7:0];
+          act_seq    <= cmd_seq_r;
+          // Start it right away
+          start_active_cmd(b_req_data[47:40], b_req_data[39:8], b_req_data[7:0], cmd_seq_r);
+        end else begin
+          // Otherwise enqueue one-deep
+          enqueue_cmd(b_req_data[47:40], b_req_data[39:8], b_req_data[7: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 byte
+        if (act_opcode == OP_READ8) begin
+          resp_pending_word <= pack_resp(
+            resp_seq_r,
+            status_snapshot,
+            act_seq,
+            byte_from_lane(addr_lane, i_wb_rdt),
+            flags_sticky,
+            act_opcode
+          );
+        end else begin
+          // WRITE8: echo written byte (lightweight)
+          resp_pending_word <= pack_resp(
+            resp_seq_r,
+            status_snapshot,
+            act_seq,
+            act_data,
+            flags_sticky,
+            act_opcode
+          );
+        end
+        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_gpio.v

@@ -0,0 +1,55 @@
+module wb_gpio #(
+  parameter address = 32'h00000000
+)(
+  input  wire        i_wb_clk,
+  input  wire        i_wb_rst,   // optional; tie low if unused
+  input  wire [31:0] i_wb_adr,   // optional; can ignore for single-reg
+  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 [31:0] i_gpio,
+
+  output reg  [31:0] o_wb_rdt,
+  output reg         o_wb_ack,
+  output reg  [31:0] o_gpio
+);
+
+  initial o_gpio <= 32'h00000000;
+  initial o_wb_rdt <= 32'h00000000;
+
+  wire addr_check;
+  assign addr_check = (i_wb_adr == address);
+
+  // One-cycle ACK pulse per request (works even if stb stays high)
+  always @(posedge i_wb_clk) begin
+    if (i_wb_rst) begin
+      o_wb_ack <= 1'b0;
+    end else begin
+      o_wb_ack <= i_wb_stb & ~o_wb_ack; // pulse while stb asserted
+    end
+  end
+
+  // Read data (combinational or registered; registered here)
+  always @(posedge i_wb_clk) begin
+    if (i_wb_rst) begin
+      o_wb_rdt <= 32'h0;
+    end else if (i_wb_stb && !i_wb_we) begin
+      o_wb_rdt <= i_gpio;
+    end
+  end
+
+  // Write latch (update on the acknowledged cycle)
+  always @(posedge i_wb_clk) begin
+    if (i_wb_rst) begin
+      o_gpio <= 32'h0;
+    end else if (i_wb_stb && i_wb_we && addr_check && (i_wb_stb & ~o_wb_ack)) begin
+      // Apply byte enables (so sb works if the master uses sel)
+      if (i_wb_sel[0]) o_gpio[7:0]   <= i_wb_dat[7:0];
+      if (i_wb_sel[1]) o_gpio[15:8]  <= i_wb_dat[15:8];
+      if (i_wb_sel[2]) o_gpio[23:16] <= i_wb_dat[23:16];
+      if (i_wb_sel[3]) o_gpio[31:24] <= i_wb_dat[31:24];
+    end
+  end
+
+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_coe.py

@@ -0,0 +1,136 @@
+#!/usr/bin/env python3
+"""Convert a simple .hex image to Xilinx .coe format.
+
+Supported input:
+- One or more hex tokens per line (e.g. "37" or "0x37")
+- Optional comments after '#' or '//'
+
+By default, each token is written as one .coe entry (8-bit style memory init).
+Use --word-bytes > 1 to pack byte tokens into wider words.
+"""
+
+from __future__ import annotations
+
+import argparse
+from pathlib import Path
+
+
+def parse_tokens(path: Path) -> list[int]:
+    values: list[int] = []
+    for raw in path.read_text().splitlines():
+        line = raw.split("//", 1)[0].split("#", 1)[0].strip()
+        if not line:
+            continue
+        for token in line.replace(",", " ").split():
+            token = token.strip()
+            if not token:
+                continue
+            if token.lower().startswith("0x"):
+                token = token[2:]
+            values.append(int(token, 16))
+    return values
+
+
+def pack_words(
+    byte_values: list[int], word_bytes: int, little_endian: bool
+) -> tuple[list[int], int]:
+    if word_bytes <= 0:
+        raise ValueError("word_bytes must be >= 1")
+    if word_bytes == 1:
+        return byte_values[:], 2
+
+    words: list[int] = []
+    width = word_bytes * 2
+    for i in range(0, len(byte_values), word_bytes):
+        chunk = byte_values[i : i + word_bytes]
+        if len(chunk) < word_bytes:
+            chunk = chunk + [0] * (word_bytes - len(chunk))
+
+        word = 0
+        if little_endian:
+            for b_idx, b in enumerate(chunk):
+                word |= (b & 0xFF) << (8 * b_idx)
+        else:
+            for b in chunk:
+                word = (word << 8) | (b & 0xFF)
+        words.append(word)
+
+    return words, width
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description="Convert .hex to Xilinx .coe")
+    parser.add_argument("input_hex", type=Path, help="Input .hex file")
+    parser.add_argument("output_coe", type=Path, help="Output .coe file")
+    parser.add_argument(
+        "--word-bytes",
+        type=int,
+        default=1,
+        help="Bytes per output word (default: 1)",
+    )
+    parser.add_argument(
+        "--little-endian",
+        action="store_true",
+        help="Pack bytes little-endian when --word-bytes > 1 (default)",
+    )
+    parser.add_argument(
+        "--big-endian",
+        action="store_true",
+        help="Pack bytes big-endian when --word-bytes > 1",
+    )
+    parser.add_argument(
+        "--radix",
+        type=int,
+        default=16,
+        choices=[2, 10, 16],
+        help="COE radix (default: 16)",
+    )
+    parser.add_argument(
+        "--depth",
+        type=int,
+        default=0,
+        help="Optional output depth (pads with zeros up to this many words)",
+    )
+    args = parser.parse_args()
+
+    if args.little_endian and args.big_endian:
+        raise SystemExit("Choose only one of --little-endian or --big-endian")
+
+    little_endian = True
+    if args.big_endian:
+        little_endian = False
+
+    byte_values = parse_tokens(args.input_hex)
+    words, hex_digits = pack_words(byte_values, args.word_bytes, little_endian)
+
+    if args.depth > 0 and args.depth < len(words):
+        raise SystemExit(
+            f"Requested --depth={args.depth} but image has {len(words)} words"
+        )
+    if args.depth > len(words):
+        words.extend([0] * (args.depth - len(words)))
+
+    if args.radix == 16:
+        data = [f"{w:0{hex_digits}X}" for w in words]
+    elif args.radix == 10:
+        data = [str(w) for w in words]
+    else:
+        width_bits = args.word_bytes * 8
+        data = [f"{w:0{width_bits}b}" for w in words]
+
+    out_lines = [
+        f"memory_initialization_radix={args.radix};",
+        "memory_initialization_vector=",
+    ]
+    if data:
+        out_lines.extend(
+            [f"{v}," for v in data[:-1]] + [f"{data[-1]};"]
+        )
+    else:
+        out_lines.append("0;")
+
+    args.output_coe.write_text("\n".join(out_lines) + "\n")
+
+
+if __name__ == "__main__":
+    main()

scripts/hex_to_mif.py

@@ -0,0 +1,104 @@
+#!/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:
+- one binary word per line
+- no header
+"""
+
+from __future__ import annotations
+
+import argparse
+from pathlib import Path
+
+
+def parse_tokens(path: Path) -> list[int]:
+    values: list[int] = []
+    for raw in path.read_text().splitlines():
+        line = raw.split("//", 1)[0].split("#", 1)[0].strip()
+        if not line:
+            continue
+        for token in line.replace(",", " ").split():
+            if token.lower().startswith("0x"):
+                token = token[2:]
+            values.append(int(token, 16))
+    return values
+
+
+def pack_words(
+    byte_values: list[int], word_bytes: int, little_endian: bool
+) -> list[int]:
+    if word_bytes <= 0:
+        raise ValueError("word_bytes must be >= 1")
+    if word_bytes == 1:
+        return byte_values[:]
+
+    words: list[int] = []
+    for i in range(0, len(byte_values), word_bytes):
+        chunk = byte_values[i : i + word_bytes]
+        if len(chunk) < word_bytes:
+            chunk = chunk + [0] * (word_bytes - len(chunk))
+
+        word = 0
+        if little_endian:
+            for b_idx, b in enumerate(chunk):
+                word |= (b & 0xFF) << (8 * b_idx)
+        else:
+            for b in chunk:
+                word = (word << 8) | (b & 0xFF)
+        words.append(word)
+    return words
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description="Convert .hex to plain .mif")
+    parser.add_argument("input_hex", type=Path, help="Input .hex file")
+    parser.add_argument("output_mif", type=Path, help="Output .mif file")
+    parser.add_argument(
+        "--word-bytes",
+        type=int,
+        default=1,
+        help="Bytes per output word (default: 1)",
+    )
+    parser.add_argument(
+        "--little-endian",
+        action="store_true",
+        help="Pack bytes little-endian when --word-bytes > 1 (default)",
+    )
+    parser.add_argument(
+        "--big-endian",
+        action="store_true",
+        help="Pack bytes big-endian when --word-bytes > 1",
+    )
+    parser.add_argument(
+        "--depth",
+        type=int,
+        default=0,
+        help="Optional output depth (pads with zeros up to this many words)",
+    )
+    args = parser.parse_args()
+
+    if args.little_endian and args.big_endian:
+        raise SystemExit("Choose only one of --little-endian or --big-endian")
+
+    little_endian = True
+    if args.big_endian:
+        little_endian = False
+
+    words = pack_words(parse_tokens(args.input_hex), args.word_bytes, little_endian)
+    width_bits = args.word_bytes * 8
+    max_word = (1 << width_bits) - 1
+
+    if args.depth > 0 and args.depth < len(words):
+        raise SystemExit(
+            f"Requested --depth={args.depth} but image has {len(words)} words"
+        )
+    if args.depth > len(words):
+        words.extend([0] * (args.depth - len(words)))
+
+    lines = [f"{(w & max_word):0{width_bits}b}" for w in words]
+    args.output_mif.write_text("\n".join(lines) + ("\n" if lines else ""))
+
+
+if __name__ == "__main__":
+    main()

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

@@ -5,3 +5,4 @@ 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
+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

@@ -0,0 +1,67 @@
+`timescale 1ns/1ps
+
+module tb_serving();
+    // Clock and reset generation
+    reg clk;
+    reg resetn;
+    initial clk <= 1'b0;
+    always #4.17 clk <= !clk;
+    initial begin 
+        resetn <= 1'b1;
+        #(4.17*40) resetn <= 1'b0;
+        #(4.17*40) resetn <= 1'b1;
+    end;
+
+    // Default run
+    initial begin
+        $dumpfile("out.vcd");
+        $dumpvars;
+        #50_000
+        $finish;
+    end;
+
+    wire [31:0] wb_adr;
+    wire [31:0] wb_dat;
+    wire [3:0] wb_sel;
+    wire wb_we;
+    wire wb_stb;
+    wire [31:0] wb_rdt;
+    wire wb_ack;
+
+    wire [31:0] GPIO;
+
+    serving #(
+        .memfile("../sw/blinky/blinky.hex"),
+        .memsize(8192),
+        .sim(1'b1),
+        .RESET_STRATEGY("MINI"),
+        .WITH_CSR(1)
+    ) serv (
+        .i_clk(clk),
+        .i_rst(!resetn),
+        .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)
+    );
+
+    wb_gpio #(
+      .address(32'h40000000)
+    ) gpio (
+        .i_wb_clk(clk),
+        .i_wb_dat(wb_dat),
+        .i_wb_adr(wb_adr),
+        .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

sim/tb/tb_sigmadelta.v

@@ -13,39 +13,24 @@ module tb_sigmadelta();
     initial begin
         $dumpfile("out.vcd");
         $dumpvars;
-        #1_000_000
+        #2_000_000
         $finish;
     end;
 
     wire sd_a;
     wire sd_b;
     wire sd_o;
-    // 3K3R 220PC 15MHZT
-    sigmadelta_sampler sd_sampler(
-        .clk(clk),
-        .a(sd_a), .b(sd_b),
-        .o(sd_o)
-    );
-
-    wire signed [15:0] sample_q15;
-    sigmadelta_rcmodel_q15 rc_model(
-        .clk(clk), .resetn(resetn),
-        .sd_sample(sd_o),
-        .sample_q15(sample_q15)
-    );
-
-    wire signed [15:0] y_q15;
-    lpf_iir_q15_k #(9) lpf(
-        .clk(clk), .rst_n(resetn),
-        .x_q15(sample_q15),
-        .y_q15(y_q15)
-    );
-
     wire signed [15:0] decimated_q15;
-    decimate_by_r_q15 #(400, 10) decimate(
+    wire decimated_valid;
-        .clk(clk), .rst_n(resetn),
+
-        .in_valid(1'b1), .in_q15(y_q15),
+    sigmadelta_input #(
-        .out_valid(), .out_q15(decimated_q15)
+        .R_OHM(3300),
+        .C_PF(220)
+    ) dut(
+        .clk_15(clk), .resetn(resetn),
+        .adc_a(sd_a), .adc_b(sd_b), .adc_o(sd_o),
+        .signal_q15(decimated_q15),
+        .signal_valid(decimated_valid)
     );
 
 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/.gitignore

@@ -0,0 +1,8 @@
+*.o
+*.hex
+*.bin
+*.map
+*.elf.asm
+*.elf
+*.coe
+*.mif

sw/blinky/Makefile

@@ -0,0 +1,56 @@
+TOOLCHAIN_PREFIX ?= riscv64-elf-
+
+CC      := $(TOOLCHAIN_PREFIX)gcc
+OBJCOPY := $(TOOLCHAIN_PREFIX)objcopy
+OBJDUMP := $(TOOLCHAIN_PREFIX)objdump
+SIZE    := $(TOOLCHAIN_PREFIX)size
+
+TARGET  := blinky
+SRCS_C  := blinky.c
+SRCS_S  := start.s
+OBJS    := $(SRCS_C:.c=.o) $(SRCS_S:.s=.o)
+
+ARCH_FLAGS := -march=rv32i_zicsr -mabi=ilp32
+CFLAGS     := $(ARCH_FLAGS) -Os -ffreestanding -fno-builtin -Wall -Wextra
+ASFLAGS    := $(ARCH_FLAGS)
+LDFLAGS    := $(ARCH_FLAGS) -nostdlib -nostartfiles -Wl,-Bstatic,-Tlink.ld,--gc-sections,-Map,$(TARGET).map
+
+HEX_TO_COE := ../../scripts/hex_to_coe.py
+HEX_TO_MIF := ../../scripts/hex_to_mif.py
+
+.PHONY: all clean disasm size
+
+all: $(TARGET).elf $(TARGET).bin $(TARGET).hex $(TARGET).coe $(TARGET).mif $(TARGET).elf.asm
+
+$(TARGET).elf: $(OBJS) link.ld
+	$(CC) $(LDFLAGS) -o $@ $(OBJS)
+
+%.o: %.c
+	$(CC) $(CFLAGS) -c -o $@ $<
+
+%.o: %.s
+	$(CC) $(ASFLAGS) -c -o $@ $<
+
+$(TARGET).bin: $(TARGET).elf
+	$(OBJCOPY) -O binary $< $@
+
+$(TARGET).hex: $(TARGET).bin
+	hexdump -v -e '1/1 "%02x\n"' $< > $@
+
+$(TARGET).coe: $(TARGET).hex
+	$(HEX_TO_COE) $< $@
+
+$(TARGET).mif: $(TARGET).hex
+	$(HEX_TO_MIF) $< $@
+
+$(TARGET).elf.asm: $(TARGET).elf
+	$(OBJDUMP) -d -S $< > $@
+
+disasm: $(TARGET).elf.asm
+
+size: $(TARGET).elf
+	$(SIZE) $<
+
+clean:
+	rm -f $(TARGET).elf $(TARGET).bin $(TARGET).hex $(TARGET).coe $(TARGET).mif \
+		$(TARGET).elf.asm $(TARGET).map $(OBJS)

sw/blinky/blinky.c

@@ -0,0 +1,27 @@
+#include <stdint.h>
+
+#define GPIO_BASE 0x40000000u
+#define VOUT_BASE 0x40000004u
+
+static volatile uint32_t * const gpio = (volatile uint32_t *)GPIO_BASE;
+static volatile uint32_t * const vout = (volatile uint32_t *)VOUT_BASE;
+
+static void delay(volatile uint32_t ticks){
+    while (ticks--) {
+        __asm__ volatile ("nop");
+    }
+}
+
+int main(void)
+{
+    uint32_t v = 0;
+
+    for (;;) {
+        for(int i=0; i<1000; i++){
+            *vout = v;
+            v++;
+            delay(5u);
+        }
+        *gpio ^= 0xffffffff;
+    }
+}

sw/blinky/link.ld

@@ -0,0 +1,33 @@
+OUTPUT_ARCH("riscv")
+ENTRY(_start)
+
+MEMORY
+{
+  RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 8064
+}
+
+SECTIONS
+{
+  .text :
+  {
+    KEEP(*(.text.init))
+    *(.text .text.*)
+    *(.rodata .rodata.*)
+  } > RAM
+
+  .data :
+  {
+    *(.data .data.*)
+  } > RAM
+
+  .bss (NOLOAD) :
+  {
+    __bss_start = .;
+    *(.bss .bss.*)
+    *(COMMON)
+    __bss_end = .;
+  } > RAM
+
+  . = ALIGN(4);
+  __stack_top = ORIGIN(RAM) + LENGTH(RAM);
+}

sw/blinky/start.s

@@ -0,0 +1,23 @@
+.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

@@ -0,0 +1,56 @@
+TOOLCHAIN_PREFIX ?= riscv64-elf-
+
+CC      := $(TOOLCHAIN_PREFIX)gcc
+OBJCOPY := $(TOOLCHAIN_PREFIX)objcopy
+OBJDUMP := $(TOOLCHAIN_PREFIX)objdump
+SIZE    := $(TOOLCHAIN_PREFIX)size
+
+TARGET  := sweep
+SRCS_C  := sweep.c
+SRCS_S  := start.s
+OBJS    := $(SRCS_C:.c=.o) $(SRCS_S:.s=.o)
+
+ARCH_FLAGS := -march=rv32i_zicsr -mabi=ilp32
+CFLAGS     := $(ARCH_FLAGS) -Os -ffreestanding -fno-builtin -Wall -Wextra
+ASFLAGS    := $(ARCH_FLAGS)
+LDFLAGS    := $(ARCH_FLAGS) -nostdlib -nostartfiles -Wl,-Bstatic,-Tlink.ld,--gc-sections,-Map,$(TARGET).map
+
+HEX_TO_COE := ../../scripts/hex_to_coe.py
+HEX_TO_MIF := ../../scripts/hex_to_mif.py
+
+.PHONY: all clean disasm size
+
+all: $(TARGET).elf $(TARGET).bin $(TARGET).hex $(TARGET).coe $(TARGET).mif $(TARGET).elf.asm
+
+$(TARGET).elf: $(OBJS) link.ld
+	$(CC) $(LDFLAGS) -o $@ $(OBJS)
+
+%.o: %.c
+	$(CC) $(CFLAGS) -c -o $@ $<
+
+%.o: %.s
+	$(CC) $(ASFLAGS) -c -o $@ $<
+
+$(TARGET).bin: $(TARGET).elf
+	$(OBJCOPY) -O binary $< $@
+
+$(TARGET).hex: $(TARGET).bin
+	hexdump -v -e '1/1 "%02x\n"' $< > $@
+
+$(TARGET).coe: $(TARGET).hex
+	$(HEX_TO_COE) $< $@
+
+$(TARGET).mif: $(TARGET).hex
+	$(HEX_TO_MIF) $< $@
+
+$(TARGET).elf.asm: $(TARGET).elf
+	$(OBJDUMP) -d -S $< > $@
+
+disasm: $(TARGET).elf.asm
+
+size: $(TARGET).elf
+	$(SIZE) $<
+
+clean:
+	rm -f $(TARGET).elf $(TARGET).bin $(TARGET).hex $(TARGET).coe $(TARGET).mif \
+		$(TARGET).elf.asm $(TARGET).map $(OBJS)

sw/sweep/link.ld

@@ -0,0 +1,35 @@
+OUTPUT_ARCH("riscv")
+ENTRY(_start)
+
+MEMORY
+{
+  RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 8192 
+}
+
+SECTIONS
+{
+  .text :
+  {
+    KEEP(*(.text.init))
+    *(.text .text.*)
+    *(.rodata .rodata.*)
+  } > RAM
+
+  .data :
+  {
+    *(.data .data.*)
+  } > RAM
+
+  .bss (NOLOAD) :
+  {
+    __bss_start = .;
+    *(.bss .bss.*)
+    *(.sbss .sbss.*)
+    *(.scommon)
+    *(COMMON)
+    __bss_end = .;
+  } > RAM
+
+  . = ALIGN(4);
+  __stack_top = ORIGIN(RAM) + LENGTH(RAM) - 256;
+}

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*4;
+    *LEDGR = ~(*LEDGR);
+}
+
+void main(){
+    irq_init();
+
+    *LEDGR = 3;
+    *TIMER = 1840000*4;
+
+    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,123 @@
+#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_PING        = 0x30;
+static constexpr uint8_t OP_CLEAR_FLAGS = 0x40;
+
+static void shift48(DigilentJtag &jtag, const uint8_t tx[6], uint8_t rx[6]) {
+  jtag.shiftData(tx, rx, 48);
+}
+
+static void make_cmd(uint8_t out[6], uint8_t opcode, uint32_t addr, uint8_t data) {
+  out[0] = data;
+  out[1] = (uint8_t)addr;
+  out[2] = (uint8_t)(addr >> 8);
+  out[3] = (uint8_t)(addr >> 16);
+  out[4] = (uint8_t)(addr >> 24);
+  out[5] = opcode;
+}
+
+static uint8_t do_cmd(DigilentJtag& jtag, uint8_t opcode, uint32_t addr, uint8_t data){
+    uint8_t tx[6], rx[6];
+    make_cmd(tx, opcode, addr, data);
+    shift48(jtag, tx, rx);
+    for(int i=0; i<32; i++){
+        make_cmd(tx, OP_NOP, 0, 0);
+        shift48(jtag, tx, rx);
+        if(rx[0] == opcode){
+            return rx[2];
+        }
+    }
+    printf("Could not do command\r\n");
+    return 0;
+}
+
+struct Resp48 {
+  uint8_t last_op;
+  uint8_t flags;
+  uint8_t data;
+  uint8_t cmd_seq;
+  uint8_t status;
+  uint8_t resp_seq;
+};
+
+int main(int argc, char** argv){
+    ArgParser parser(argc > 0 ? argv[0] : "test");
+    parser.addString("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_cmd(jtag, OP_CLEAR_FLAGS, 0, 0);
+    // Check for ping
+    if(do_cmd(jtag, OP_PING, 0, 0) != 0xa5){
+        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_cmd(jtag, OP_RESET_ON, 0, 0);
+
+    int nr = 0;
+    int addr = 0;
+    do{
+        uint8_t buf[64];
+        nr = fread(buf, 1, 64, f);
+        for(int i=0; i<nr; i++){
+            do_cmd(jtag, OP_WRITE8, addr+i, buf[i]);
+            printf(".");
+        }
+        printf("\r\n");
+
+        if(parser.getFlag("verify")){
+            for(int i=0; i<nr; i++){
+                uint8_t r = do_cmd(jtag, OP_READ8, addr+i, 0);
+                if(r!=buf[i]){
+                    printf(" -- Verify failed at %04x : %02x != %02x\r\n", addr+i, r, buf[i]);
+                }
+            }
+        }
+
+        addr += nr;
+    }while(nr > 0);
+    
+    do_cmd(jtag, OP_RESET_OFF, 0, 0);
+
+    fclose(f);
+    jtag.close();
+    return 0;
+}