diff --git a/libcpu/arm/rm48x50/context_ccs.asm b/libcpu/arm/rm48x50/context_ccs.asm
index b57674313b80faa2e6dfae5710dafda64d09e86d..60e2f27671c5f958c58bdb488c268fc1a6abdd6e 100644
--- a/libcpu/arm/rm48x50/context_ccs.asm
+++ b/libcpu/arm/rm48x50/context_ccs.asm
@@ -53,10 +53,8 @@ rt_hw_context_switch
 
     MRS     r4, cpsr
     TST     lr, #0x01
-    BEQ     _ARM_MODE
-    ORR     r4, r4, #0x20       ; it's thumb code
+    ORRNE   r4, r4, #0x20       ; it's thumb code
 
-_ARM_MODE
     STMFD   sp!, {r4}           ; push cpsr
 
     STR     sp, [r0]            ; store sp in preempted tasks TCB