diff --git a/if_stage.sv b/if_stage.sv
index 44f0ccac..17529f0e 100644
--- a/if_stage.sv
+++ b/if_stage.sv
@@ -80,17 +80,17 @@ module if_stage
     input  logic        stall_id_i
 );
 
-
-  logic [31:0] exc_pc;
+  // offset FSM
+  enum logic[3:0] {IDLE, WAIT_ALIGNED, WAIT_UNALIGNED, VALID_ALIGNED, VALID_UNALIGNED,
+                   HANDLE_BRANCH, FETCH_UNALIGNED} offset_fsm_cs, offset_fsm_ns, offset_fsm_stored;
 
   logic  [1:0] is_compressed;
+  logic        crossword_n, crossword_Q;
   logic        unaligned, unaligned_Q;
   logic        unaligned_jump;
-  logic        branch_taken;
 
+  logic        handle_branch;
   logic        force_nop_int;
-  logic [31:0] instr_rdata_int;
-  logic [31:0] current_pc_id_int;
 
   // instr_core_interface
   logic        fetch_req;
@@ -99,40 +99,32 @@ module if_stage
   logic [31:0] fetch_addr, fetch_addr_n;
   logic [31:0] fetch_addr_Q;
 
+  logic [31:0] instr_rdata_int;
+
+  logic [31:0] exc_pc;
+
   // local cache
   logic [15:0] data_arr;
 
 
-  // store instr_core_if data in local cache
-  always_ff @(posedge clk, negedge rst_n)
-  begin
-    if (rst_n == 1'b0) begin
-      data_arr <= 16'b0;
-    end else begin
-      if (fetch_valid) begin
-        data_arr <= fetch_rdata[31:16];
-      end
-    end
-  end
-
-
+  // output data and PC mux
   always_comb
   begin
     // default values for regular aligned access
     instr_rdata_int   = fetch_rdata;
-    current_pc_id_int = {fetch_addr[31:2], 2'b00};
+    current_pc_if_o   = {fetch_addr[31:2], 2'b00};
 
     if (unaligned) begin
-      if (unaligned_Q) begin
-        // cross-word access
+      if (crossword_Q) begin
+        // cross-word access, regular instruction
         instr_rdata_int   = {fetch_rdata[15:0], data_arr};
-        current_pc_id_int = {fetch_addr_Q[31:2], 2'b10};
+        current_pc_if_o   = {fetch_addr_Q[31:2], 2'b10};
       end else begin
         // unaligned compressed instruction
         // don't care about upper half-word, insert good value for
         // optimization
         instr_rdata_int   = {fetch_rdata[31:16], fetch_rdata[31:16]};
-        current_pc_id_int = {fetch_addr[31:2], 2'b10};
+        current_pc_if_o   = {fetch_addr[31:2], 2'b10};
       end
     end
 
@@ -141,8 +133,6 @@ module if_stage
       instr_rdata_int = {25'b0, `OPCODE_OPIMM};
   end
 
-  assign current_pc_if_o = current_pc_id_int;
-
 
   // compressed instruction detection
   assign is_compressed[0] = fetch_rdata[1:0]   != 2'b11;
@@ -153,13 +143,14 @@ module if_stage
   always_comb
   begin : EXC_PC_MUX
     unique case (exc_pc_mux_i)
-      `EXC_PC_ILLINSN: begin exc_pc = {boot_addr_i[31:5], `EXC_OFF_ILLINSN }; end
-      `EXC_PC_IRQ:     begin exc_pc = {boot_addr_i[31:5], `EXC_OFF_IRQ     }; end
-      `EXC_PC_IRQ_NM:  begin exc_pc = {boot_addr_i[31:5], `EXC_OFF_IRQ_NM  }; end
+      `EXC_PC_ILLINSN: exc_pc = { boot_addr_i[31:5], `EXC_OFF_ILLINSN };
+      `EXC_PC_IRQ:     exc_pc = { boot_addr_i[31:5], `EXC_OFF_IRQ     };
+      `EXC_PC_IRQ_NM:  exc_pc = { boot_addr_i[31:5], `EXC_OFF_IRQ_NM  };
+      default:         exc_pc = { boot_addr_i[31:5], `EXC_OFF_RST     };
     endcase
   end
 
-  // next PC selection
+  // fetch address selection
   always_comb
   begin
     unique case (pc_mux_sel_i)
@@ -203,30 +194,35 @@ module if_stage
   );
 
 
-  // offset FSM
-  enum logic[3:0] {IDLE, WAIT_ALIGNED, WAIT_UNALIGNED, VALID_ALIGNED, VALID_UNALIGNED,
-                   HANDLE_BRANCH, FETCH_UNALIGNED} offset_fsm_cs, offset_fsm_ns;
-
+  // offset FSM state
   always_ff @(posedge clk, negedge rst_n)
   begin
     if (rst_n == 1'b0) begin
-      offset_fsm_cs <= IDLE;
+      offset_fsm_cs     <= IDLE;
+      offset_fsm_stored <= IDLE;
     end else begin
-      offset_fsm_cs <= offset_fsm_ns;
+      if (handle_branch) begin
+        offset_fsm_cs     <= HANDLE_BRANCH;
+        offset_fsm_stored <= offset_fsm_ns;
+      end
+      else
+        offset_fsm_cs     <= offset_fsm_ns;
     end
   end
 
-  // offset FSM state logic
+  // offset FSM state transition logic
   always_comb
   begin
     offset_fsm_ns = offset_fsm_cs;
 
-    fetch_req = 1'b0;
-    valid_o = 1'b0;
+    handle_branch = 1'b0;
 
-    unaligned = 1'b0;
+    fetch_req = 1'b0;
+    valid_o   = 1'b0;
+
+    unaligned     = unaligned_Q;
+    crossword_n   = crossword_Q;
     force_nop_int = 1'b0;
-    branch_taken = 1'b0;
 
     unique case (offset_fsm_cs)
       // no valid instruction data for ID stage
@@ -238,70 +234,87 @@ module if_stage
         end
       end
 
+      // We are currently in an ALIGNED state, serving PC[1] == 1'b0
       VALID_ALIGNED,
       WAIT_ALIGNED: begin
+        unaligned = 1'b0;
+
         if (fetch_valid || offset_fsm_cs == VALID_ALIGNED) begin
           valid_o = 1'b1;
           offset_fsm_ns = VALID_ALIGNED;
 
           if (req_i && ~stall_if_i) begin
-            if (jump_in_id_i == `BRANCH_NONE) begin
-              // ----------------------------------------------------------------------
-              // no branch in ID, do regular fetch
-              // ----------------------------------------------------------------------
-              if (is_compressed[0]) begin
-                // compressed instruction
-                if (is_compressed[1]) begin
-                  // upper half contains compressed instruction and is available
-                  // from register
-                  offset_fsm_ns = VALID_UNALIGNED;
-                end else begin
-                  // cross-word access, upper half is regular instruction
-                  fetch_req     = 1'b1;
-                  offset_fsm_ns = WAIT_UNALIGNED;
-                end
-              end else begin
-                // regular instruction
-                fetch_req     = 1'b1;
-                offset_fsm_ns = WAIT_ALIGNED;
-              end
+            crossword_n = 1'b0;
 
+            // ----------------------------------------------------------------------
+            // no branch in ID, do regular fetch
+            // ----------------------------------------------------------------------
+            if (is_compressed[0]) begin
+              // compressed instruction
+              if (is_compressed[1]) begin
+                // upper half contains compressed instruction and is available
+                // from register, don't start fetch now
+                offset_fsm_ns = VALID_UNALIGNED;
+              end else begin
+                // cross-word access, upper half is beginning of 32 bit instruction
+                crossword_n   = 1'b1;
+                fetch_req     = 1'b1;
+                offset_fsm_ns = WAIT_UNALIGNED;
+              end
             end else begin
+              // regular instruction
+              fetch_req     = 1'b1;
+              offset_fsm_ns = WAIT_ALIGNED;
+            end
+
+            if (jump_in_id_i != `BRANCH_NONE) begin
               // ----------------------------------------------------------------------
               // need to handle branch
               // ----------------------------------------------------------------------
-              offset_fsm_ns = HANDLE_BRANCH;
+              handle_branch = 1'b1;
             end
           end
         end
       end
 
+      // We are currently in an unaligned state, serving PC[1] == 1'b1
       WAIT_UNALIGNED,
       VALID_UNALIGNED: begin
         unaligned = 1'b1;
 
         if (fetch_valid || offset_fsm_cs == VALID_UNALIGNED) begin
           valid_o = 1'b1;
+          offset_fsm_ns = VALID_UNALIGNED;
 
           if (req_i && ~stall_if_i) begin
-            if (jump_in_id_i == `BRANCH_NONE) begin
-              // ----------------------------------------------------------------------
-              // no branch in ID, do regular fetch
-              // ----------------------------------------------------------------------
-              fetch_req = 1'b1;
+            crossword_n = 1'b0;
 
+            // ----------------------------------------------------------------------
+            // no branch in ID, do regular fetch
+            // ----------------------------------------------------------------------
+            if (crossword_Q) begin
+              // last instruction was 32 bit crossword, unaligned
               if (is_compressed[1]) begin
-                // compressed instruction, next instruction will be aligned
-                offset_fsm_ns = WAIT_ALIGNED;
+                // compressed instruction, next instruction will be
+                // unaligned
+                offset_fsm_ns = VALID_UNALIGNED;
               end else begin
                 // regular instruction, fetch following instruction
+                fetch_req     = 1'b1;
+                crossword_n   = 1'b1;
                 offset_fsm_ns = WAIT_UNALIGNED;
               end
             end else begin
-              // ----------------------------------------------------------------------
-              // need to handle branch
-              // ----------------------------------------------------------------------
-              offset_fsm_ns = HANDLE_BRANCH;
+              // compressed instruction because no cross-word access done,
+              // next instruction will be aligned
+              fetch_req = 1'b1;
+              offset_fsm_ns = WAIT_ALIGNED;
+
+              assert(is_compressed[1]) else $error("Not compressed, but compressed expected");
+            end
+
+            if (jump_in_id_i != `BRANCH_NONE) begin
+              handle_branch = 1'b1;
             end
           end
         end
@@ -310,17 +323,34 @@ module if_stage
       HANDLE_BRANCH: begin
         // assume jump/branch instruction is in EX stage
         if (jump_in_ex_i == `BRANCH_COND && ~branch_decision_i) begin
-          // TODO: Optimize this, already send request & valid
-          if (unaligned_Q)
-            offset_fsm_ns = VALID_UNALIGNED;
-          else
-            offset_fsm_ns = VALID_ALIGNED;
+          // branch not taken
+
+          // let's go to one instruction after the one we already put into the
+          // pipeline
+          if (unaligned_Q) begin
+            // last state was unaligned, go back
+            if (crossword_Q) begin
+              fetch_req     = 1'b1;
+              offset_fsm_ns = WAIT_UNALIGNED;
+            end else begin
+              fetch_req     = 1'b1;
+              offset_fsm_ns = WAIT_ALIGNED;
+            end
+          end else begin
+            crossword_n   = 1'b0;
+
+            if (is_compressed[0]) begin
+              offset_fsm_ns = VALID_UNALIGNED;
+            end else begin
+              offset_fsm_ns = VALID_ALIGNED;
+            end
+          end
+
         end else begin
-          // branch/jump taken
-          branch_taken = 1'b1;
-          fetch_req    = 1'b1;
+          // branch taken or jump
+          fetch_req   = 1'b1;
+          crossword_n = 1'b0;
           if (unaligned_jump) begin
-            unaligned = 1'b1;
             // if the target address is unaligned, we need to fetch the lower
             // word first
             offset_fsm_ns = FETCH_UNALIGNED;
@@ -330,22 +360,33 @@ module if_stage
         end
       end
 
+      // can be cross-word or compressed
       FETCH_UNALIGNED: begin
         unaligned = 1'b1;
 
         if (fetch_valid) begin
           if (is_compressed[1]) begin
             // no cross-word access
-            valid_o = 1'b1;
-            offset_fsm_ns = WAIT_ALIGNED;
+            crossword_n   = 1'b0;
+            valid_o       = 1'b1;
+            if (req_i && ~stall_if_i) begin
+              fetch_req     = 1'b1;
+              offset_fsm_ns = WAIT_ALIGNED;
+            end else begin
+              offset_fsm_ns = VALID_UNALIGNED;
+            end
           end else begin
             // cross-word access, fetch next word
-            fetch_req = 1'b1;
+            fetch_req     = 1'b1;
+            crossword_n   = 1'b1;
             offset_fsm_ns = WAIT_UNALIGNED;
           end
         end
       end
 
+      default: begin
+        offset_fsm_ns = IDLE;
+      end
     endcase
   end
 
@@ -362,20 +403,33 @@ module if_stage
   end
 
 
+  // store instr_core_if data in local cache
+  always_ff @(posedge clk, negedge rst_n)
+  begin
+    if (rst_n == 1'b0) begin
+      data_arr     <= 16'b0;
+      fetch_addr_Q <= 32'b0;
+    end else begin
+      if (~stall_if_i) begin
+        data_arr     <= fetch_rdata[31:16];
+        fetch_addr_Q <= fetch_addr;
+      end
+    end
+  end
+
+
   // IF PC register
   always_ff @(posedge clk, negedge rst_n)
   begin : IF_PIPELINE
     if (rst_n == 1'b0)
     begin
+      crossword_Q  <= 1'b0;
       unaligned_Q  <= 1'b0;
-      fetch_addr_Q <= 32'b0;
     end
     else
     begin
-      if (fetch_valid) begin
-        unaligned_Q  <= unaligned;
-        fetch_addr_Q <= fetch_addr;
-      end
+      crossword_Q  <= crossword_n;
+      unaligned_Q  <= unaligned;
     end
   end
 
@@ -392,7 +446,7 @@ module if_stage
       if (stall_id_i == 1'b0)
       begin : ENABLED_PIPE
         instr_rdata_id_o <= instr_rdata_int;
-        current_pc_id_o  <= current_pc_id_int;
+        current_pc_id_o  <= current_pc_if_o;
       end
     end
   end