diff --git a/data/verilator_waiver.vlt b/data/verilator_waiver.vlt
index 20b6a3c..ee428e4 100644
--- a/data/verilator_waiver.vlt
+++ b/data/verilator_waiver.vlt
@@ -1,6 +1,6 @@
 `verilator_config
 // Bits [1:0] in i_wb_rdt are not used at all
-lint_off -rule UNUSED -file "*/serv_top.v" -lines 179
+lint_off -rule UNUSED -file "*/serv_top.v" -lines 180
 
 //Some bits in the instruction word are not used in serv_decode but it's easier
 //to just send in the whole word than picking out bits
@@ -16,4 +16,4 @@ lint_off -rule UNUSED -file "*/serv_bufreg.v" -lines 19-21
 //Some signals not used in 4-bit mode
 lint_off -rule UNUSED -file "*/serv_immdec.v" -lines 12
 lint_off -rule UNUSED -file "*/serv_bufreg.v" -lines 9
-lint_off -rule UNUSED -file "*/serv_ctrl.v" -lines 17
+lint_off -rule UNUSED -file "*/serv_ctrl.v" -lines 18
diff --git a/rtl/serv_ctrl.v b/rtl/serv_ctrl.v
index 3ea2650..1cd5bf3 100644
--- a/rtl/serv_ctrl.v
+++ b/rtl/serv_ctrl.v
@@ -11,7 +11,8 @@ module serv_ctrl
    input wire 	     i_rst,
    //State
    input wire 	     i_pc_en,
-   input wire 	     i_cnt12to31,
+   input wire 	     i_cnt12to15,
+   input wire 	     i_cnt16to31,
    input wire 	     i_cnt0,
    input wire        i_cnt1,
    input wire 	     i_cnt2,
@@ -77,7 +78,7 @@ module serv_ctrl
    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 offset_b = i_utype ? (i_imm & {W{i_cnt12to15 | i_cnt16to31}}) : i_buf;
    assign {pc_plus_offset_cy,pc_plus_offset} = offset_a+offset_b+pc_plus_offset_cy_r_w;
 
    generate
diff --git a/rtl/serv_state.v b/rtl/serv_state.v
index bd73b3c..03af4eb 100644
--- a/rtl/serv_state.v
+++ b/rtl/serv_state.v
@@ -14,7 +14,8 @@ module serv_state
    output wire 	     o_init,
    output wire	     o_cnt_en,
    output wire 	     o_cnt0to3,
-   output wire 	     o_cnt12to31,
+   output wire 	     o_cnt12to15,
+   output wire 	     o_cnt16to31,
    output wire 	     o_cnt0,
    output wire 	     o_cnt1,
    output wire 	     o_cnt2,
@@ -72,7 +73,8 @@ module serv_state
    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_cnt12to15 = (!o_cnt[4] & (o_cnt[3:2] == 2'b11));
+   assign o_cnt16to31 = o_cnt[4];
    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];
diff --git a/rtl/serv_top.v b/rtl/serv_top.v
index e41775e..d971f55 100644
--- a/rtl/serv_top.v
+++ b/rtl/serv_top.v
@@ -113,7 +113,8 @@ module serv_top
    wire          init;
    wire          cnt_en;
    wire 	 cnt0to3;
-   wire 	 cnt12to31;
+   wire 	 cnt12to15;
+   wire 	 cnt16to31;
    wire          cnt0;
    wire          cnt1;
    wire          cnt2;
@@ -238,7 +239,8 @@ module serv_top
       .o_init         (init),
       .o_cnt_en       (cnt_en),
       .o_cnt0to3      (cnt0to3),
-      .o_cnt12to31    (cnt12to31),
+      .o_cnt12to15    (cnt12to15),
+      .o_cnt16to31    (cnt16to31),
       .o_cnt0         (cnt0),
       .o_cnt1         (cnt1),
       .o_cnt2         (cnt2),
@@ -434,7 +436,8 @@ module serv_top
       .i_rst      (i_rst),
       //State
       .i_pc_en    (ctrl_pc_en),
-      .i_cnt12to31 (cnt12to31),
+      .i_cnt12to15 (cnt12to15),
+      .i_cnt16to31 (cnt16to31),
       .i_cnt0     (cnt0),
       .i_cnt1     (cnt1),
       .i_cnt2     (cnt2),