diff --git a/src/arm7tdmi/cpu.rs b/src/arm7tdmi/cpu.rs
index d168e0c..7d84f87 100644
--- a/src/arm7tdmi/cpu.rs
+++ b/src/arm7tdmi/cpu.rs
@@ -131,11 +131,10 @@ impl Core {
         let curr_index = curr_mode.bank_index();
 
         self.gpr_banked_r13[curr_index] = self.gpr[13];
-        self.gpr[13] = self.gpr_banked_r13[next_index];
-
         self.gpr_banked_r14[curr_index] = self.gpr[14];
+
+        self.gpr[13] = self.gpr_banked_r13[next_index];
         self.gpr[14] = self.gpr_banked_r14[next_index];
-        self.gpr_banked_r14[next_index] = self.get_next_pc(); // Store the return address in LR_mode
 
         if new_mode == CpuMode::Fiq {
             for r in 0..5 {
@@ -157,6 +156,9 @@ impl Core {
             self.spsr[index] = self.cpsr;
         }
         self.map_banked_registers(curr_mode, new_mode);
+        let next_index = new_mode.bank_index();
+        self.gpr_banked_r14[next_index] = self.pc.wrapping_sub(self.word_size() as u32).wrapping_add(4);
+
     }
 
     /// Resets the cpu