diff --git a/arm/arm_emit.h b/arm/arm_emit.h
index 91e8006..b9ded10 100644
--- a/arm/arm_emit.h
+++ b/arm/arm_emit.h
@@ -1866,9 +1866,8 @@ u32 execute_store_cpsr_body(u32 _cpsr, u32 store_mask, u32 address)
   generate_update_pc(((pc + 2) | 0x01));                                      \
   thumb_generate_load_reg(reg_a1, REG_LR);                                    \
   thumb_generate_store_reg(reg_a0, REG_LR);                                   \
-  generate_mov(reg_a0, reg_a1);                                               \
-  generate_add_imm(reg_a0, (offset * 2), 0);                                  \
-  generate_indirect_branch_cycle_update(thumb);                               \
+  generate_add_reg_reg_imm(reg_a0, reg_a1, (offset * 2), 0);                  \
+  generate_indirect_branch_cycle_update(dual_thumb);                          \
 }                                                                             \
 
 #define thumb_bx()                                                            \
diff --git a/arm/arm_stub.S b/arm/arm_stub.S
index 070152e..6386785 100644
--- a/arm/arm_stub.S
+++ b/arm/arm_stub.S
@@ -269,6 +269,7 @@ defsymbl(arm_indirect_branch_dual_arm)
   save_flags()
   tst r0, #0x01                           @ check lower bit
   bne 1f                                  @ if set going to Thumb mode
+  add r0, #2                              @ two LSB are cleared after
   call_c_function(block_lookup_address_arm)
   restore_flags()
   bx r0                                   @ keep executing arm code
@@ -296,6 +297,7 @@ defsymbl(arm_indirect_branch_dual_thumb)
   load_registers_arm()                    @ load in ARM registers
   bic r1, r1, #0x20                       @ clear Thumb mode
   str r1, [reg_base, #REG_CPSR]           @ store flags
+  add r0, #2                              @ two LSB are cleared after
   call_c_function(block_lookup_address_arm)
   restore_flags()
   bx r0