diff --git a/id_stage.sv b/id_stage.sv
index 66f12d5c..64788a9a 100644
--- a/id_stage.sv
+++ b/id_stage.sv
@@ -498,7 +498,7 @@ module id_stage
     .rst_n        ( rst_n              ),
 
     // Read port a
-    .raddr_a_i    ( (dbg_reg_mux_i == 1'b0) ? regfile_addr_ra_id : dbg_reg_addr_i ),
+    .raddr_a_i    ( regfile_addr_ra_id ),
     .rdata_a_o    ( regfile_data_ra_id ),
 
     // Read port b
@@ -506,7 +506,7 @@ module id_stage
     .rdata_b_o    ( regfile_data_rb_id ),
 
     // Read port c
-    .raddr_c_i    ( regfile_addr_rc_id ),
+    .raddr_c_i    ( (dbg_reg_mux_i == 1'b0) ? regfile_addr_rc_id : dbg_reg_addr_i ),
     .rdata_c_o    ( regfile_data_rc_id ),
 
     // Write port a
diff --git a/if_stage.sv b/if_stage.sv
index 97bb4f2f..4fff8e6f 100644
--- a/if_stage.sv
+++ b/if_stage.sv
@@ -101,8 +101,7 @@ module if_stage
   logic        fetch_req;
   logic [31:0] fetch_rdata;
   logic        fetch_valid;
-  logic [31:0] fetch_addr, fetch_addr_n;
-  logic [31:0] fetch_addr_Q;
+  logic [31:0] fetch_addr_n, fetch_addr_Q, fetch_addr_QQ;
 
   logic [31:0] instr_rdata_int;
 
@@ -117,27 +116,27 @@ module if_stage
   begin
     // default values for regular aligned access
     instr_rdata_int   = fetch_rdata;
-    current_pc_if_o   = {fetch_addr[31:2], 2'b00};
+    current_pc_if_o   = {fetch_addr_Q[31:2], 2'b00};
 
     if (unaligned) begin
       if (crossword) begin
         // cross-word access, regular instruction
         instr_rdata_int   = {fetch_rdata[15:0], data_arr};
-        current_pc_if_o   = {fetch_addr_Q[31:2], 2'b10};
+        current_pc_if_o   = {fetch_addr_QQ[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[15:0], fetch_rdata[31:16]};
-        current_pc_if_o   = {fetch_addr[31:2], 2'b10};
+        current_pc_if_o   = {fetch_addr_Q[31:2], 2'b10};
       end
     end
   end
 
 
   // compressed instruction detection
-  assign is_compressed[0] = fetch_rdata[1:0]   != 2'b11;
-  assign is_compressed[1] = fetch_rdata[17:16] != 2'b11;
+  assign is_compressed[0] = (fetch_rdata[1:0]   != 2'b11);
+  assign is_compressed[1] = (fetch_rdata[17:16] != 2'b11);
 
 
   // exception PC selection mux
@@ -158,7 +157,7 @@ module if_stage
       `PC_BOOT:      fetch_addr_n = {boot_addr_i[31:5], `EXC_OFF_RST};
       `PC_JUMP:      fetch_addr_n = {jump_target_id_i[31:2], 2'b0};
       `PC_BRANCH:    fetch_addr_n = {jump_target_ex_i[31:2], 2'b0};
-      `PC_INCR:      fetch_addr_n = fetch_addr + 32'd4; // incremented PC
+      `PC_INCR:      fetch_addr_n = fetch_addr_Q + 32'd4; // incremented PC
       `PC_EXCEPTION: fetch_addr_n = exc_pc;             // set PC to exception handler
       `PC_ERET:      fetch_addr_n = exception_pc_reg_i; // PC is restored when returning from IRQ/exception
       `PC_HWLOOP:    fetch_addr_n = pc_from_hwloop_i;   // PC is taken from hwloop start addr
@@ -195,16 +194,12 @@ module if_stage
     .valid_o        ( fetch_valid    ),
     .addr_i         ( fetch_addr_n   ),
     .rdata_o        ( fetch_rdata    ),
-    .last_addr_o    ( fetch_addr     ),
 
     .instr_req_o    ( instr_req_o    ),
     .instr_addr_o   ( instr_addr_o   ),
     .instr_gnt_i    ( instr_gnt_i    ),
     .instr_rvalid_i ( instr_rvalid_i ),
-    .instr_rdata_i  ( instr_rdata_i  ),
-
-    .stall_if_i     ( 1'b0           ),
-    .drop_request_i ( 1'b0           )  // TODO: Remove?
+    .instr_rdata_i  ( instr_rdata_i  )
   );
 
 
@@ -403,15 +398,20 @@ module if_stage
 
 
   // store instr_core_if data in local cache
+  // store last address used to fetch an instruction
+  // we also store the address we used before, so
+  // fetch_addr_n -> fetch_addr_Q -> fetch_addr_QQ
   always_ff @(posedge clk, negedge rst_n)
   begin
     if (rst_n == 1'b0) begin
-      data_arr     <= 16'b0;
-      fetch_addr_Q <= 32'b0;
+      data_arr      <= 16'b0;
+      fetch_addr_Q  <= 32'b0;
+      fetch_addr_QQ <= 32'b0;
     end else begin
       if (fetch_req) begin
-        data_arr     <= fetch_rdata[31:16];
-        fetch_addr_Q <= fetch_addr;
+        data_arr      <= fetch_rdata[31:16];
+        fetch_addr_Q  <= fetch_addr_n;
+        fetch_addr_QQ <= fetch_addr_Q;
       end
     end
   end
@@ -421,7 +421,7 @@ module if_stage
                        offset_fsm_cs == VALID_JUMPED_UNALIGNED ||
                        offset_fsm_cs == VALID_ALIGNED          ||
                        offset_fsm_cs == VALID_UNALIGNED_32     ||
-                       offset_fsm_cs == UNALIGNED_16) || instr_req_o;
+                       offset_fsm_cs == UNALIGNED_16)          || instr_req_o;
 
 
   // IF-ID pipeline registers, frozen when the ID stage is stalled
diff --git a/include/defines.sv b/include/defines.sv
index 1c3fef44..1e86ad0f 100644
--- a/include/defines.sv
+++ b/include/defines.sv
@@ -359,7 +359,6 @@ endfunction // prettyPrintInstruction
 `define PC_EXCEPTION     3'b100
 `define PC_ERET          3'b101
 `define PC_HWLOOP        3'b110
-`define PC_BRANCH_PRED   3'b111
 
 // Exception PC mux selector defines
 `define EXC_PC_NO_INCR   2'b00
diff --git a/instr_core_interface.sv b/instr_core_interface.sv
index 3daa5035..f7225d7a 100644
--- a/instr_core_interface.sv
+++ b/instr_core_interface.sv
@@ -39,16 +39,11 @@ module instr_core_interface
   output logic [31:0] instr_addr_o,
   input  logic        instr_gnt_i,
   input  logic        instr_rvalid_i,
-  input  logic [31:0] instr_rdata_i,
-
-  input  logic        stall_if_i,
-
-  input  logic        drop_request_i
+  input  logic [31:0] instr_rdata_i
 );
 
-  enum logic [2:0] {IDLE, PENDING, WAIT_RVALID, WAIT_IF_STALL, WAIT_GNT, ABORT} CS, NS;
+  enum logic [1:0] {IDLE, PENDING, WAIT_RVALID, WAIT_GNT } CS, NS;
 
-  logic        save_rdata;
   logic [31:0] rdata_Q;
 
   logic        wait_gnt;
@@ -72,46 +67,40 @@ module instr_core_interface
       if (wait_gnt)
         addr_Q <= addr_i;
 
-      if (save_rdata)
+      if (instr_rvalid_i)
         rdata_Q <= instr_rdata_i;
     end
   end
 
-
-  assign last_addr_o = addr_Q;
-
+  assign valid_o      = instr_rvalid_i;
 
   always_comb
   begin
     instr_req_o   = 1'b0;
-    valid_o         = 1'b0;
-    save_rdata    = 1'b0;
     rdata_o       = instr_rdata_i;
     instr_addr_o  = addr_i;
     wait_gnt      = 1'b0;
 
     unique case(CS)
+      // default state, not waiting for requested data
       IDLE:
       begin
-        instr_req_o = req_i;
-        valid_o       = 1'b0;
         rdata_o     = rdata_Q;
 
-        if(req_i)
-        begin
+        NS          = IDLE;
+        instr_req_o = req_i;
+
+        if(req_i) begin
           if(instr_gnt_i) //~>  granted request
             NS = PENDING;
           else begin //~> got a request but no grant
             NS       = WAIT_GNT;
             wait_gnt = 1'b1;
           end
-        end //~> if(req_i == 0)
-        else
-        begin
-           NS = IDLE;
         end
       end // case: IDLE
 
+      // we sent a request but did not yet get a grant
       WAIT_GNT:
       begin
         instr_addr_o = addr_Q;
@@ -121,81 +110,39 @@ module instr_core_interface
           NS = PENDING;
         else
         begin
-//        if (drop_request_i)
-//          NS = IDLE;
-//        else
-            NS = WAIT_GNT;
+          NS = WAIT_GNT;
         end
       end // case: WAIT_GNT
 
+      // we got a grant, so now we wait for the rvalid
       PENDING:
       begin
         if (instr_rvalid_i) begin
-          save_rdata = 1'b1;
-          valid_o = 1'b1;
+          NS          = IDLE;
+          instr_req_o = req_i;
 
-          if (stall_if_i) begin
-            NS          = WAIT_IF_STALL;
-            instr_req_o = 1'b0;
-          end else begin
-            NS          = IDLE;
-            instr_req_o = req_i;
-
-            if (req_i) begin
-              if (instr_gnt_i) begin
-                NS = PENDING;
-              end else begin
-                NS       = WAIT_GNT;
-                wait_gnt = 1'b1;
-              end
+          if (req_i) begin
+            if (instr_gnt_i) begin
+              NS = PENDING;
+            end else begin
+              NS       = WAIT_GNT;
+              wait_gnt = 1'b1;
             end
           end
         end else begin
           NS          = WAIT_RVALID;
           instr_req_o = 1'b0;
-          valid_o       = 1'b0;
         end
       end // case: PENDING
 
+      // we wait for rvalid, after that we are ready to serve a new request
       WAIT_RVALID :
       begin
         NS          = WAIT_RVALID;
-        valid_o       = 1'b0;
         instr_req_o = 1'b0;
 
         if (instr_rvalid_i) begin
-          valid_o       = 1'b1;
-          save_rdata  = 1'b1;
-
-          if (stall_if_i) begin
-            instr_req_o = 1'b0;
-            NS          = WAIT_IF_STALL;
-          end else begin
-            instr_req_o = req_i;
-            if (req_i) begin
-              if (instr_gnt_i)
-                NS =  PENDING;
-              else begin
-                NS       = WAIT_GNT;
-                wait_gnt = 1'b1;
-              end
-            end else
-              NS = IDLE;
-          end
-        end
-      end // case: WAIT_RVALID
-
-      WAIT_IF_STALL:
-      begin
-        valid_o   = 1'b1;
-        rdata_o = rdata_Q;
-
-        if (stall_if_i) begin
-          NS          = WAIT_IF_STALL;
-          instr_req_o = 1'b0;
-        end
-        else
-        begin
+          NS          = IDLE;
           instr_req_o = req_i;
 
           if (req_i) begin
@@ -205,26 +152,9 @@ module instr_core_interface
               NS       = WAIT_GNT;
               wait_gnt = 1'b1;
             end
-          end else
-            NS = IDLE;
-        end
-      end // case: WAIT_IF_STALL
-
-      ABORT:
-      begin
-        NS = IDLE;
-        valid_o = 1'b1;
-        instr_req_o  = 1'b1;
-
-        if (req_i) begin
-          if(instr_gnt_i) //~>  granted request
-            NS = PENDING;
-          else begin //~> got a request but no grant
-            NS       = WAIT_GNT;
-            wait_gnt = 1'b1;
           end
         end
-      end // case: ABORT
+      end // case: WAIT_RVALID
 
       default:
       begin