diff --git a/bench/serv_top_tb.v b/bench/serv_top_tb.v
index 0280254..c5218d0 100644
--- a/bench/serv_top_tb.v
+++ b/bench/serv_top_tb.v
@@ -1,111 +1,73 @@
 `default_nettype none
 module serv_top_tb;
+
    reg clk = 1'b1;
-
-   wire [31:0] i_ca_adr;
-   wire        i_ca_vld;
-   wire        i_ca_rdy;
-   wire [31:0] i_rd_dat;
-   wire        i_rd_vld;
-   wire        i_rd_rdy;
-
-   wire        d_ca_cmd;
-   wire [31:0] d_ca_adr;
-   wire        d_ca_vld;
-   wire        d_ca_rdy;
-   wire [31:0] d_dm_dat;
-   wire [3:0]  d_dm_msk;
-   wire        d_dm_vld;
-   wire        d_dm_rdy;
-   wire [31:0] d_rd_dat;
-   wire        d_rd_vld;
-   wire        d_rd_rdy;
+   reg wb_rst = 1'b1;
 
    reg [1023:0] firmware_file;
 
-   reg [31:0] pc;
-   reg [31:0] instruction;
+   wire         wb_clk = clk;
 
    always #5 clk <= !clk;
+   initial #100 wb_rst = 1'b0;
 
-   camd_ram
+`include "wb_intercon.vh"
+
+   localparam MEMORY_SIZE = 16384*4;
+
+   wb_ram
      #(.memfile ("firmware.hex"),
-       .depth (16384*4))
-   i_mem
-     (.clk_i    (clk),
-      .rst_i    (1'b0),
-      .ca_adr_i (i_ca_adr),
-      .ca_cmd_i (1'b0/*i_ca_cmd*/),
-      .ca_vld_i (i_ca_vld),
-      .ca_rdy_o (i_ca_rdy),
-      .dm_dat_i (/*i_dm_dat*/),
-      .dm_msk_i (/*i_dm_msk*/),
-      .dm_vld_i (/*i_dm_vld*/),
-      .dm_rdy_o (/*i_dm_rdy*/),
-      .rd_dat_o (i_rd_dat),
-      .rd_vld_o (i_rd_vld),
-      .rd_rdy_i (i_rd_rdy));
-
-   camd_ram
-     #(.memfile ("firmware.hex"),
-       .depth (16384*4))
-   d_mem
-     (.clk_i    (clk),
-      .rst_i    (1'b0),
-      .ca_adr_i (d_ca_adr),
-      .ca_cmd_i (d_ca_cmd),
-      .ca_vld_i (d_ca_vld),
-      .ca_rdy_o (d_ca_rdy),
-      .dm_dat_i (d_dm_dat),
-      .dm_msk_i (d_dm_msk),
-      .dm_vld_i (d_dm_vld),
-      .dm_rdy_o (d_dm_rdy),
-      .rd_dat_o (d_rd_dat),
-      .rd_vld_o (d_rd_vld),
-      .rd_rdy_i (d_rd_rdy));
-
-   reg        catch_write = 1'b0;
-
-   reg        dbg = 1'b0;
-
-   wire       d_ca_en = d_ca_vld & d_ca_rdy;
-   wire       d_dm_en = d_dm_vld & d_dm_rdy;
+       .depth (MEMORY_SIZE))
+   ram
+     (// Wishbone interface
+      .wb_clk_i (clk),
+      .wb_rst_i (wb_rst),
+      .wb_adr_i (wb_m2s_mem_adr[$clog2(MEMORY_SIZE)-1:0]),
+      .wb_stb_i (wb_m2s_mem_stb),
+      .wb_cyc_i (wb_m2s_mem_cyc),
+      .wb_cti_i (wb_m2s_mem_cti),
+      .wb_bte_i (wb_m2s_mem_bte),
+      .wb_we_i  (wb_m2s_mem_we) ,
+      .wb_sel_i (wb_m2s_mem_sel),
+      .wb_dat_i (wb_m2s_mem_dat),
+      .wb_dat_o (wb_s2m_mem_dat),
+      .wb_ack_o (wb_s2m_mem_ack),
+      .wb_err_o ());
    
-   always @(posedge clk) begin
-      dbg <= 1'b0;
-      
-      if (d_ca_en & d_ca_cmd & (d_ca_adr == 32'h10000000))
-        catch_write <= 1'b1;
-      if (((d_ca_en & d_ca_cmd & (d_ca_adr == 32'h10000000)) |catch_write) & d_dm_en & d_dm_msk[0]) begin
-         dbg <= 1'b1;
-         $write("%c", d_dm_dat[7:0]);
-         $fflush();
-         catch_write <= 1'b0;
-      end
-   end
    vlog_tb_utils vtu();
 
+   testprint testprint
+     (
+      .i_wb_clk   (clk),
+      .i_wb_dat   (wb_m2s_testprint_dat),
+      .i_wb_we    (wb_m2s_testprint_we),
+      .i_wb_cyc   (wb_m2s_testprint_cyc),
+      .i_wb_stb   (wb_m2s_testprint_stb),
+      .o_wb_ack   (wb_s2m_testprint_ack));
+
    serv_top
      #(.RESET_PC (32'd8))
    dut
      (
       .clk      (clk),
-      .o_i_ca_adr (i_ca_adr),
-      .o_i_ca_vld (i_ca_vld),
-      .i_i_ca_rdy (i_ca_rdy),
-      .i_i_rd_dat (i_rd_dat),
-      .i_i_rd_vld (i_rd_vld),
-      .o_i_rd_rdy (i_rd_rdy),
-      .o_d_ca_cmd (d_ca_cmd),
-      .o_d_ca_adr (d_ca_adr),
-      .o_d_ca_vld (d_ca_vld),
-      .i_d_ca_rdy (d_ca_rdy),
-      .o_d_dm_dat (d_dm_dat),
-      .o_d_dm_msk (d_dm_msk),
-      .o_d_dm_vld (d_dm_vld),
-      .i_d_dm_rdy (d_dm_rdy),
-      .i_d_rd_dat (d_rd_dat),
-      .i_d_rd_vld (d_rd_vld),
-      .o_d_rd_rdy (d_rd_rdy));
+      .o_ibus_adr   (wb_m2s_cpu_ibus_adr),
+      .o_ibus_cyc   (wb_m2s_cpu_ibus_cyc),
+      .o_ibus_stb   (wb_m2s_cpu_ibus_stb),
+      .i_ibus_rdt   (wb_s2m_cpu_ibus_dat),
+      .i_ibus_ack   (wb_s2m_cpu_ibus_ack),
+      .o_dbus_adr   (wb_m2s_cpu_dbus_adr),
+      .o_dbus_dat   (wb_m2s_cpu_dbus_dat),
+      .o_dbus_sel   (wb_m2s_cpu_dbus_sel),
+      .o_dbus_we    (wb_m2s_cpu_dbus_we),
+      .o_dbus_cyc   (wb_m2s_cpu_dbus_cyc),
+      .o_dbus_stb   (wb_m2s_cpu_dbus_stb),
+      .i_dbus_rdt   (wb_s2m_cpu_dbus_dat),
+      .i_dbus_ack   (wb_s2m_cpu_dbus_ack));
 
+   assign wb_m2s_cpu_ibus_dat = 32'd0;   
+   assign wb_m2s_cpu_ibus_we = 1'b0;
+   assign wb_m2s_cpu_ibus_sel = 4'b1111;
+   assign wb_m2s_cpu_ibus_cti = 3'b000;
+   assign wb_m2s_cpu_ibus_bte = 2'b00;
+   
 endmodule
diff --git a/firmware/README b/picorv32_firmware/firmware/README
similarity index 100%
rename from firmware/README
rename to picorv32_firmware/firmware/README
diff --git a/firmware/custom_ops.S b/picorv32_firmware/firmware/custom_ops.S
similarity index 100%
rename from firmware/custom_ops.S
rename to picorv32_firmware/firmware/custom_ops.S
diff --git a/firmware/firmware.bin b/picorv32_firmware/firmware/firmware.bin
similarity index 100%
rename from firmware/firmware.bin
rename to picorv32_firmware/firmware/firmware.bin
diff --git a/firmware/firmware.elf b/picorv32_firmware/firmware/firmware.elf
similarity index 100%
rename from firmware/firmware.elf
rename to picorv32_firmware/firmware/firmware.elf
diff --git a/firmware/firmware.h b/picorv32_firmware/firmware/firmware.h
similarity index 100%
rename from firmware/firmware.h
rename to picorv32_firmware/firmware/firmware.h
diff --git a/firmware/firmware.hex b/picorv32_firmware/firmware/firmware.hex
similarity index 100%
rename from firmware/firmware.hex
rename to picorv32_firmware/firmware/firmware.hex
diff --git a/firmware/firmware.map b/picorv32_firmware/firmware/firmware.map
similarity index 100%
rename from firmware/firmware.map
rename to picorv32_firmware/firmware/firmware.map
diff --git a/firmware/irq.c b/picorv32_firmware/firmware/irq.c
similarity index 100%
rename from firmware/irq.c
rename to picorv32_firmware/firmware/irq.c
diff --git a/firmware/irq.o b/picorv32_firmware/firmware/irq.o
similarity index 100%
rename from firmware/irq.o
rename to picorv32_firmware/firmware/irq.o
diff --git a/firmware/makehex.py b/picorv32_firmware/firmware/makehex.py
similarity index 100%
rename from firmware/makehex.py
rename to picorv32_firmware/firmware/makehex.py
diff --git a/firmware/multest.c b/picorv32_firmware/firmware/multest.c
similarity index 100%
rename from firmware/multest.c
rename to picorv32_firmware/firmware/multest.c
diff --git a/firmware/multest.o b/picorv32_firmware/firmware/multest.o
similarity index 100%
rename from firmware/multest.o
rename to picorv32_firmware/firmware/multest.o
diff --git a/firmware/print.c b/picorv32_firmware/firmware/print.c
similarity index 100%
rename from firmware/print.c
rename to picorv32_firmware/firmware/print.c
diff --git a/firmware/print.o b/picorv32_firmware/firmware/print.o
similarity index 100%
rename from firmware/print.o
rename to picorv32_firmware/firmware/print.o
diff --git a/firmware/riscv.ld b/picorv32_firmware/firmware/riscv.ld
similarity index 100%
rename from firmware/riscv.ld
rename to picorv32_firmware/firmware/riscv.ld
diff --git a/firmware/riscv.ld.orig b/picorv32_firmware/firmware/riscv.ld.orig
similarity index 100%
rename from firmware/riscv.ld.orig
rename to picorv32_firmware/firmware/riscv.ld.orig
diff --git a/firmware/sections.lds b/picorv32_firmware/firmware/sections.lds
similarity index 100%
rename from firmware/sections.lds
rename to picorv32_firmware/firmware/sections.lds
diff --git a/firmware/sieve.c b/picorv32_firmware/firmware/sieve.c
similarity index 100%
rename from firmware/sieve.c
rename to picorv32_firmware/firmware/sieve.c
diff --git a/firmware/sieve.o b/picorv32_firmware/firmware/sieve.o
similarity index 100%
rename from firmware/sieve.o
rename to picorv32_firmware/firmware/sieve.o
diff --git a/firmware/start.S b/picorv32_firmware/firmware/start.S
similarity index 100%
rename from firmware/start.S
rename to picorv32_firmware/firmware/start.S
diff --git a/firmware/start.o b/picorv32_firmware/firmware/start.o
similarity index 100%
rename from firmware/start.o
rename to picorv32_firmware/firmware/start.o
diff --git a/firmware/stats.c b/picorv32_firmware/firmware/stats.c
similarity index 100%
rename from firmware/stats.c
rename to picorv32_firmware/firmware/stats.c
diff --git a/firmware/stats.o b/picorv32_firmware/firmware/stats.o
similarity index 100%
rename from firmware/stats.o
rename to picorv32_firmware/firmware/stats.o
diff --git a/rtl/serv_ctrl.v b/rtl/serv_ctrl.v
index 0de4af1..5d9b284 100644
--- a/rtl/serv_ctrl.v
+++ b/rtl/serv_ctrl.v
@@ -9,9 +9,9 @@ module serv_ctrl
    input         i_jalr,
    input         i_auipc,
    output        o_rd,
-   output [31:0] o_i_ca_adr,
-   output reg    o_i_ca_vld = 1'b0,
-   input         i_i_ca_rdy);
+   output [31:0] o_ibus_adr,
+   output reg    o_ibus_cyc = 1'b0,
+   input         i_ibus_ack);
 
    parameter RESET_PC = 32'd8;
    
@@ -28,7 +28,7 @@ module serv_ctrl
    
    assign plus_4        = en_2r & !en_3r;
 
-   assign o_i_ca_adr[0] = pc;
+   assign o_ibus_adr[0] = pc;
    
    ser_add ser_add_pc_plus_4
      (
@@ -49,7 +49,7 @@ module serv_ctrl
       .i_en (i_en),
       .i_d  (new_pc),
       .o_q  (pc),
-      .o_par (o_i_ca_adr[31:1])
+      .o_par (o_ibus_adr[31:1])
       );
 
    assign new_pc = i_jump ? pc_plus_offset : pc_plus_4;
@@ -76,9 +76,9 @@ module serv_ctrl
       en_3r <= en_2r;
 
       if (en_r & !i_en)
-        o_i_ca_vld <= 1'b1;
-      else if (o_i_ca_vld & i_i_ca_rdy)
-        o_i_ca_vld <= 1'b0;
+        o_ibus_cyc <= 1'b1;
+      else if (o_ibus_cyc & i_ibus_ack)
+        o_ibus_cyc <= 1'b0;
    end
    
 endmodule
diff --git a/rtl/serv_decode.v b/rtl/serv_decode.v
index b865415..f23c446 100644
--- a/rtl/serv_decode.v
+++ b/rtl/serv_decode.v
@@ -1,18 +1,18 @@
 module serv_decode
   (
    input            clk,
-   input [31:0]     i_i_rd_dat,
-   input            i_i_rd_vld,
-   output reg       o_i_rd_rdy = 1'b1,
+   input [31:0]     i_wb_rdt,
+   input            i_wb_en,
+   output           o_ibus_active,
    output           o_ctrl_en,
    output           o_ctrl_jump,
    output           o_ctrl_jalr,
    output           o_ctrl_auipc, 
    output           o_rf_rd_en,
-   output [4:0]     o_rf_rd_addr,
+   output reg [4:0] o_rf_rd_addr,
    output           o_rf_rs_en,
-   output [4:0]     o_rf_rs1_addr,
-   output [4:0]     o_rf_rs2_addr,
+   output reg [4:0] o_rf_rs1_addr,
+   output reg [4:0] o_rf_rs2_addr,
    output           o_alu_en,
    output           o_alu_init,
    output           o_alu_sub,
@@ -29,7 +29,7 @@ module serv_decode
    output           o_mem_init,
    output reg       o_mem_dat_valid, 
    input            i_mem_busy,
-   output [2:0]     o_funct3,
+   output reg [2:0] o_funct3,
    output reg       o_imm,
    output           o_offset_source,
    output           o_op_b_source,
@@ -56,13 +56,17 @@ module serv_decode
      OP_JALR   = 5'b11001,
      OP_JAL    = 5'b11011;
 
-   reg [2:0]    state = 3'd0;
+   reg [2:0]    state = IDLE;
 
    reg [4:0] cnt     = 5'd0;
 
    wire      running;
    wire      mem_op;
    wire      shift_op;
+
+   reg       signbit;
+   
+   assign o_ibus_active = (state == IDLE);
    
    assign mem_op = (opcode == OP_LOAD) | (opcode == OP_STORE);
    assign shift_op = ((opcode == OP_OPIMM) & (o_funct3[1:0] == 2'b01)) |
@@ -91,7 +95,7 @@ module serv_decode
    assign o_alu_init = (state == COMPARE) |
                        (state == SH_INIT);
 
-   assign o_alu_sub = (opcode == OP_OP) ? i_i_rd_dat[30] /*    ? 1'b1*/ :
+   assign o_alu_sub = (opcode == OP_OP) ? signbit /*    ? 1'b1*/ :
                       ((opcode == OP_BRANCH) & (o_funct3 == 3'b100)) ? 1'b1 :
                       ((opcode == OP_BRANCH) & (o_funct3 == 3'b101)) ? 1'b1 :
                       ((opcode == OP_BRANCH) & (o_funct3 == 3'b110)) ? 1'b1 :
@@ -129,7 +133,7 @@ module serv_decode
       endcase
    end
    assign o_alu_shamt_en = (state == SH_INIT) & (cnt < 5);
-   assign o_alu_sh_signed = i_i_rd_dat[30];
+   assign o_alu_sh_signed = signbit;
    assign o_alu_sh_right = o_funct3[2];
    
    assign o_mem_en   = mem_op & cnt_en;
@@ -137,10 +141,22 @@ module serv_decode
 
    assign o_mem_init = (state == MEM_INIT);
 
-   assign o_rf_rd_addr  = i_i_rd_dat[11:7];
-   assign o_funct3  = i_i_rd_dat[14:12];
-   assign o_rf_rs1_addr = i_i_rd_dat[19:15];
-   assign o_rf_rs2_addr = i_i_rd_dat[24:20];
+   reg [4:0] opcode;
+   reg [31:0] imm;
+   
+   always @(posedge clk) begin
+      if (i_wb_en) begin
+         o_rf_rd_addr  <= i_wb_rdt[11:7];
+         o_rf_rs1_addr <= i_wb_rdt[19:15];
+         o_rf_rs2_addr <= i_wb_rdt[24:20];
+         o_funct3      <= i_wb_rdt[14:12];
+         signbit       <= i_wb_rdt[30];
+         opcode        <= i_wb_rdt[6:2];
+         imm           <= i_wb_rdt;
+         
+      end
+   end
+
    assign o_offset_source = (opcode == OP_JAL)    ? OFFSET_SOURCE_IMM :
                             (opcode == OP_AUIPC)  ? OFFSET_SOURCE_IMM :
                             (opcode == OP_BRANCH) ? OFFSET_SOURCE_IMM :
@@ -152,7 +168,7 @@ module serv_decode
                           (opcode == OP_OP)     ? OP_B_SOURCE_RS2 :
                           1'bx;
 
-   always @(o_funct3, cnt)
+   always @(o_funct3, cnt, o_mem_init)
      if (o_mem_init)
        o_mem_dat_valid = 1'bx;
      else
@@ -163,7 +179,6 @@ module serv_decode
          default: o_mem_dat_valid = 1'bx;
        endcase
 
-   wire [4:0] opcode = i_i_rd_dat[6:2];
 
 
    assign o_rd_source = (opcode == OP_JAL)   ? RD_SOURCE_CTRL :
@@ -174,36 +189,38 @@ module serv_decode
                         (opcode == OP_JALR)  ? RD_SOURCE_CTRL :
                         (opcode == OP_LOAD)  ? RD_SOURCE_MEM  : 2'bxx;
    
-   always @(cnt, opcode, i_i_rd_dat) begin
+   always @(cnt, opcode, imm) begin
       o_imm = 1'bx;
       if (opcode == OP_JAL)
-        if      (cnt > 19) o_imm = i_i_rd_dat[31];
-        else if (cnt > 11) o_imm = i_i_rd_dat[cnt];
-        else if (cnt > 10) o_imm = i_i_rd_dat[20];
-        else if (cnt > 0)  o_imm = i_i_rd_dat[cnt+20];
+        if      (cnt > 19) o_imm = imm[31];
+        else if (cnt > 11) o_imm = imm[cnt];
+        else if (cnt > 10) o_imm = imm[20];
+        else if (cnt > 0)  o_imm = imm[cnt+20];
         else               o_imm = 1'b0;
       else if ((opcode == OP_OPIMM) | (opcode == OP_JALR))
-        if      (cnt > 10) o_imm = i_i_rd_dat[31];
-        else               o_imm = i_i_rd_dat[cnt+20];
+        if      (cnt > 10) o_imm = imm[31];
+        else               o_imm = imm[cnt+20];
       else if ((opcode == OP_LUI) | (opcode == OP_AUIPC))
-        if      (cnt > 11) o_imm = i_i_rd_dat[cnt];
+        if      (cnt > 11) o_imm = imm[cnt];
         else               o_imm = 1'b0;
       else if (opcode == OP_LOAD)
-        if (cnt > 10)      o_imm = i_i_rd_dat[31];
-        else               o_imm = i_i_rd_dat[cnt+20];
+        if (cnt > 10)      o_imm = imm[31];
+        else               o_imm = imm[cnt+20];
       else if (opcode == OP_BRANCH)
-        if      (cnt > 11) o_imm = i_i_rd_dat[31];
-        else if (cnt > 10) o_imm = i_i_rd_dat[7];
-        else if (cnt > 4) o_imm = i_i_rd_dat[cnt+20];
-        else if (cnt > 0) o_imm = i_i_rd_dat[cnt+7];
+        if      (cnt > 11) o_imm = imm[31];
+        else if (cnt > 10) o_imm = imm[7];
+        else if (cnt > 4) o_imm = imm[cnt+20];
+        else if (cnt > 0) o_imm = imm[cnt+7];
         else               o_imm = 1'b0;
       else if (opcode == OP_STORE)
-        if (cnt > 10)     o_imm = i_i_rd_dat[31];
-        else if (cnt > 4) o_imm = i_i_rd_dat[cnt+20];
-        else              o_imm = i_i_rd_dat[cnt+7];
+        if (cnt > 10)     o_imm = imm[31];
+        else if (cnt > 4) o_imm = imm[cnt+20];
+        else              o_imm = imm[cnt+7];
    end
 
-   wire go = i_i_rd_vld & o_i_rd_rdy;
+   reg go = 1'b0;
+   always @(posedge clk)
+     go <= i_wb_en;
 
    wire cnt_en =
         (state == RUN) |
@@ -247,11 +264,6 @@ module serv_decode
 
       cnt <= cnt + {4'd0,cnt_en};
 
-      if (go) begin
-        o_i_rd_rdy <= 1'b0;
-      end else if (cnt_done & (state == RUN)) begin
-         o_i_rd_rdy <= 1'b1;
-      end
    end
 
 endmodule
diff --git a/rtl/serv_mem_if.v b/rtl/serv_mem_if.v
index bf8dcaa..0afe4fb 100644
--- a/rtl/serv_mem_if.v
+++ b/rtl/serv_mem_if.v
@@ -13,22 +13,17 @@ module serv_mem_if
    output        o_rd,
    output reg    o_busy = 1'b0,
    //External interface
-   output        o_d_ca_cmd,
-   output [31:0] o_d_ca_adr,
-   output reg    o_d_ca_vld = 1'b0,
-   input         i_d_ca_rdy,
-   output [31:0] o_d_dm_dat,
-   output [3:0]  o_d_dm_msk,
-   output reg    o_d_dm_vld = 1'b0,
-   input         i_d_dm_rdy,
-   input [31:0]  i_d_rd_dat,
-   input         i_d_rd_vld,
-   output        o_d_rd_rdy);
+   output [31:0] o_wb_adr,
+   output [31:0] o_wb_dat,
+   output [3:0]  o_wb_sel,
+   output        o_wb_we ,
+   output reg    o_wb_cyc = 1'b0,
+   output        o_wb_stb,
+   input [31:0]  i_wb_rdt,
+   input         i_wb_ack);
 
-   wire          ca_en = o_d_ca_vld & i_d_ca_rdy;
-   wire          dm_en = o_d_dm_vld & i_d_dm_rdy;
-   wire          rd_en = i_d_rd_vld & o_d_rd_rdy;
-   
+   wire          wb_en = o_wb_cyc & i_wb_ack;
+   assign o_wb_stb = o_wb_cyc;
    reg           init_r;
    reg           en_r;
    reg           en_2r;
@@ -50,27 +45,24 @@ module serv_mem_if
       .clk   (i_clk),
       .i_en  (i_init),
       .i_d   (adr),
-      .o_q   (o_d_ca_adr[0]),
-      .o_par (o_d_ca_adr[31:1])
+      .o_q   (o_wb_adr[0]),
+      .o_par (o_wb_adr[31:1])
       );
 
-   assign o_d_ca_cmd = i_cmd;
-
    wire is_signed = ~i_funct3[2];
    assign o_rd = i_dat_valid ? dat[0] : signbit & is_signed;
 
-   assign o_d_rd_rdy = !i_en; //Likely bug, but probably doesn't matter
-
    wire is_word = i_funct3[1];
    wire is_half = i_funct3[0];
    wire is_byte = !(|i_funct3[1:0]);
 
    wire       upper_half = bytepos[1];
 
-   assign o_d_dm_dat = dat;
-   assign o_d_dm_msk = is_word ? 4'b1111 :
-                       is_half ? {{2{upper_half}}, ~{2{upper_half}}} :
-                       4'd1 << bytepos;
+   assign o_wb_dat = dat;
+   assign o_wb_sel = (is_word ? 4'b1111 :
+                      is_half ? {{2{upper_half}}, ~{2{upper_half}}} :
+                      4'd1 << bytepos);
+   assign o_wb_we = i_cmd;
    reg [1:0]  bytepos;
    reg [4:0]  cnt = 5'd0;
    reg        dat_en;
@@ -78,14 +70,14 @@ module serv_mem_if
    always @(i_funct3, cnt, bytepos)
      casez(i_funct3[1:0])
        2'b1? : dat_en = 1'b1;
-       2'b?1 : dat_en = bytepos[1] ? (cnt<16) : 1'b1;
+       2'b01 : dat_en = bytepos[1] ? (cnt<16) : 1'b1;
        2'b00 : dat_en = (bytepos == 2'd3) ? (cnt <8) :
                         (bytepos == 2'd2) ? (cnt < 16) :
                         (bytepos == 2'd1) ? (cnt < 24) : 1'b1;
      endcase
        
    always @(posedge i_clk) begin
-      cnt <= cnt + i_init;
+      cnt <= cnt + {4'd0,i_init};
       
       if (i_en & !en_r)
         bytepos[0] <= adr;
@@ -97,33 +89,28 @@ module serv_mem_if
 
       if (i_en & i_init)
         o_busy <= 1'b1;
-      else if (rd_en | dm_en)
+      else if (wb_en)
         o_busy <= 1'b0;
 
-      if (rd_en) begin
-         dat[31:16] <= i_d_rd_dat[31:16];
-         dat[15:8]  <= (is_half & upper_half) ? i_d_rd_dat[31:24] : i_d_rd_dat[15:8];
-         dat[7:0]   <= (is_byte & (bytepos == 2'b11)) ? i_d_rd_dat[31:24] :
-                       (is_byte & (bytepos == 2'b10)) ? i_d_rd_dat[23:16] :
-                       (is_half & upper_half)         ? i_d_rd_dat[23:16] :
-                       (is_byte & (bytepos == 2'b01)) ? i_d_rd_dat[15:8] :
-                       i_d_rd_dat[7:0];
+      if (wb_en & !o_wb_we) begin
+         dat[31:16] <= i_wb_rdt[31:16];
+         dat[15:8]  <= (is_half & upper_half) ? i_wb_rdt[31:24] : i_wb_rdt[15:8];
+         dat[7:0]   <= (is_byte & (bytepos == 2'b11)) ? i_wb_rdt[31:24] :
+                       (is_byte & (bytepos == 2'b10)) ? i_wb_rdt[23:16] :
+                       (is_half & upper_half)         ? i_wb_rdt[23:16] :
+                       (is_byte & (bytepos == 2'b01)) ? i_wb_rdt[15:8] :
+                       i_wb_rdt[7:0];
       end
 
       en_r <= i_en;
       en_2r <= en_r;
       init_r <= i_init;
-      if (ca_en)
-        o_d_ca_vld <= 1'b0;
+      if (wb_en)
+        o_wb_cyc <= 1'b0;
       else if (init_r & !i_init) begin //Optimize?
-         o_d_ca_vld <= 1'b1;
+         o_wb_cyc <= 1'b1;
       end
 
-      if (dm_en)
-        o_d_dm_vld <= 1'b0;
-      else if (init_r & !i_init)
-        o_d_dm_vld <= i_cmd;
-      
       if (i_en & dat_en)
         dat <= {i_rs2,dat[31:1]};
    end
diff --git a/rtl/serv_top.v b/rtl/serv_top.v
index 4fa2ad6..c27590c 100644
--- a/rtl/serv_top.v
+++ b/rtl/serv_top.v
@@ -30,25 +30,23 @@ module serv_top
    output reg [3:0]  rvfi_mem_wmask,
    output reg [31:0] rvfi_mem_rdata,
    output reg [31:0] rvfi_mem_wdata,
-`endif 
-   output [31:0]     o_i_ca_adr, 
-   output            o_i_ca_vld,
-   input             i_i_ca_rdy,
-   input [31:0]      i_i_rd_dat,
-   input             i_i_rd_vld,
-   output            o_i_rd_rdy,
-   output            o_d_ca_cmd,
-   output [31:0]     o_d_ca_adr,
-   output            o_d_ca_vld,
-   input             i_d_ca_rdy,
-   output [31:0]     o_d_dm_dat,
-   output [3:0]      o_d_dm_msk,
-   output            o_d_dm_vld,
-   input             i_d_dm_rdy,
-   input [31:0]      i_d_rd_dat,
-   input             i_d_rd_vld,
-   output            o_d_rd_rdy);
+`endif
+   output [31:0]     o_ibus_adr,
+   output            o_ibus_cyc,
+   output            o_ibus_stb,
+   input [31:0]      i_ibus_rdt,
+   input             i_ibus_ack,
+   output [31:0]     o_dbus_adr,
+   output [31:0]     o_dbus_dat,
+   output [3:0]      o_dbus_sel,
+   output            o_dbus_we ,
+   output            o_dbus_cyc,
+   output            o_dbus_stb,
+   input [31:0]      i_dbus_rdt,
+   input             i_dbus_ack);
 
+   assign o_ibus_stb = o_ibus_cyc;
+   
 `include "serv_params.vh"
 
    wire [4:0]    rd_addr;
@@ -98,15 +96,16 @@ module serv_top
    
    wire          mem_init;
    wire          mem_busy;
+
    
    parameter RESET_PC = 32'd8;
 
    serv_decode decode
      (
       .clk (clk),
-      .i_i_rd_dat     (i_i_rd_dat),
-      .i_i_rd_vld     (i_i_rd_vld),
-      .o_i_rd_rdy     (o_i_rd_rdy),
+      .i_wb_rdt       (i_ibus_rdt),
+      .i_wb_en        (o_ibus_cyc & i_ibus_ack),
+      .o_ibus_active  (),
       .o_ctrl_en      (ctrl_en),
       .o_ctrl_jump    (jump),
       .o_ctrl_jalr    (jalr),
@@ -150,9 +149,9 @@ module serv_top
       .i_jalr     (jalr),
       .i_auipc    (auipc),
       .o_rd       (ctrl_rd),
-      .o_i_ca_adr (o_i_ca_adr),
-      .o_i_ca_vld (o_i_ca_vld),
-      .i_i_ca_rdy (i_i_ca_rdy));
+      .o_ibus_adr (o_ibus_adr),
+      .o_ibus_cyc (o_ibus_cyc),
+      .i_ibus_ack (i_ibus_ack));
 
    assign offset = (offset_source == OFFSET_SOURCE_IMM) ? imm :
                    (offset_source == OFFSET_SOURCE_RS1) ? rs1 : 1'bx;
@@ -161,12 +160,11 @@ module serv_top
                (rd_source == RD_SOURCE_ALU)  ? alu_rd  :
                (rd_source == RD_SOURCE_IMM)  ? imm     :
                (rd_source == RD_SOURCE_MEM)  ? mem_rd  : 1'bx;
-   
 
    assign op_b = (op_b_source == OP_B_SOURCE_IMM) ? imm :
                  (op_b_source == OP_B_SOURCE_RS2) ? rs2 :
                  1'bx;
-   
+
    serv_alu alu
      (
       .clk        (clk),
@@ -196,14 +194,14 @@ module serv_top
       .i_rs_en    (rs_en),
       .o_rs1      (rs1),
       .o_rs2      (rs2));
-   
+
    serv_mem_if mem_if
      (
-      .i_clk (clk),
-      .i_en  (mem_en),
-      .i_init (mem_init),
+      .i_clk    (clk),
+      .i_en     (mem_en),
+      .i_init   (mem_init),
       .i_dat_valid (mem_dat_valid),
-      .i_cmd (mem_cmd),
+      .i_cmd    (mem_cmd),
       .i_funct3 (funct3),
       .i_rs1    (rs1),
       .i_rs2    (rs2),
@@ -211,23 +209,20 @@ module serv_top
       .o_rd     (mem_rd),
       .o_busy   (mem_busy),
       //External interface
-      .o_d_ca_cmd (o_d_ca_cmd),
-      .o_d_ca_adr (o_d_ca_adr),
-      .o_d_ca_vld (o_d_ca_vld),
-      .i_d_ca_rdy (i_d_ca_rdy),
-      .o_d_dm_dat (o_d_dm_dat),
-      .o_d_dm_msk (o_d_dm_msk),
-      .o_d_dm_vld (o_d_dm_vld),
-      .i_d_dm_rdy (i_d_dm_rdy),
-      .i_d_rd_dat (i_d_rd_dat),
-      .i_d_rd_vld (i_d_rd_vld),
-      .o_d_rd_rdy (o_d_rd_rdy));
+      .o_wb_adr   (o_dbus_adr),
+      .o_wb_dat   (o_dbus_dat),
+      .o_wb_sel   (o_dbus_sel),
+      .o_wb_we    (o_dbus_we ),
+      .o_wb_cyc   (o_dbus_cyc),
+      .o_wb_stb   (o_dbus_stb),
+      .i_wb_rdt   (i_dbus_rdt),
+      .i_wb_ack   (i_dbus_ack));
 
 `ifdef RISCV_FORMAL
    reg [31:0]    rs1_fv, rs2_fv, rd_fv;
    reg [31:0]    pc = RESET_PC;
    reg           ctrl_en_r = 1'b0;
-   
+
    always @(posedge clk) begin
       ctrl_en_r <= ctrl_en;
       if (rs_en) begin
@@ -239,10 +234,10 @@ module serv_top
       end
       rvfi_valid <= 1'b0;
       if (ctrl_en_r & !ctrl_en) begin
-         pc <= o_i_ca_adr;
+         pc <= o_ibus_adr;
          rvfi_valid <= 1'b1;
          rvfi_order <= rvfi_order + 1;
-         rvfi_insn  <= i_i_rd_dat;
+         rvfi_insn  <= i_ibus_rdt;
          rvfi_trap <= 1'b0;
          rvfi_halt <= 1'b0;
          rvfi_intr <= 1'b0;
@@ -254,14 +249,14 @@ module serv_top
          rvfi_rd_addr <= rd_addr;
          rvfi_rd_wdata <= rd_fv;
          rvfi_pc_rdata <= pc;
-         rvfi_pc_wdata <= o_i_ca_adr;
-         rvfi_mem_addr <= o_d_ca_adr;
+         rvfi_pc_wdata <= o_ibus_adr;
+         rvfi_mem_addr <= o_dbus_adr;
          rvfi_mem_rmask <= 4'bxxxx;
-         rvfi_mem_wmask <= o_d_dm_msk;
-         rvfi_mem_rdata <= i_d_rd_dat;
-         rvfi_mem_wdata <= o_d_dm_dat;
+         rvfi_mem_wmask <= o_dbus_sel;
+         rvfi_mem_rdata <= i_dbus_rdt;
+         rvfi_mem_wdata <= o_dbus_dat;
       end
    end
-`endif   
-      
+`endif
+
 endmodule
diff --git a/serv.core b/serv.core
index 89fce3f..05e16d0 100644
--- a/serv.core
+++ b/serv.core
@@ -19,7 +19,7 @@ filesets:
       - rtl/serv_regfile.v
       - rtl/serv_top.v
     file_type : verilogSource
-    depend : [wb_ram, "yosys:techlibs:ice40"]
+    depend : [wb_ram]
 
   ser_add_tb:
     files:
@@ -38,16 +38,16 @@ filesets:
   serv_top_tb:
     files:
       - firmware.hex : {copyto : firmware.hex, file_type : user}
+      - testprint.v
       - bench/serv_top_tb.v
     file_type : verilogSource
-    depend : [vlog_tb_utils, "yosys:techlibs:ice40"]
+    depend : [vlog_tb_utils, wb_intercon, "yosys:techlibs:ice40"]
 
   pcf:
     files: [data/dummy.pcf : {file_type : PCF}]
 targets:
   default:
-    default_tool : icarus
-    filesets : [core, tb]
+    filesets : [core]
     toplevel : serv_top_tb
 
   synth:
@@ -79,6 +79,7 @@ targets:
   serv_top_tb:
     default_tool: icarus
     filesets : [core, serv_top_tb]
+    generate : [wb_intercon]
     parameters : [RISCV_FORMAL=true]
     toplevel : serv_top_tb
 
@@ -86,3 +87,24 @@ parameters:
   RISCV_FORMAL:
     datatype  : bool
     paramtype : vlogdefine
+
+generate:
+  wb_intercon:
+    generator: wb_intercon_gen
+    parameters:
+      masters:
+        cpu_ibus:
+          slaves : [mem]
+        cpu_dbus:
+          slaves : [mem, testprint, testpassed]
+      slaves:
+        mem:
+          offset : 0
+          size   : 65536
+        testprint:
+          offset : 0x10000000
+          size   : 4
+        testpassed:
+          offset : 0x20000000
+          size   : 4
+    
diff --git a/testprint.v b/testprint.v
new file mode 100644
index 0000000..d827e4d
--- /dev/null
+++ b/testprint.v
@@ -0,0 +1,22 @@
+`default_nettype none
+module testprint
+  (
+   input        i_wb_clk,
+
+   input [31:0] i_wb_dat,
+   input        i_wb_we,
+   input        i_wb_cyc,
+   input        i_wb_stb,
+   output reg   o_wb_ack = 1'b0);
+
+   always @(posedge i_wb_clk) begin
+      if (i_wb_cyc & i_wb_stb) begin
+         $write("%c", i_wb_dat[7:0]);
+`ifndef VERILATOR
+         $fflush();
+`endif
+      end
+      if (i_wb_cyc & i_wb_stb & !o_wb_ack)
+        o_wb_ack <= 1'b1;
+   end
+endmodule