diff --git a/libcpu/arm/lpc24xx/context_rvds.S b/libcpu/arm/lpc24xx/context_rvds.S
index 82c5adc574304888b47f94c4007090505ced887f..c889e9328160ea88213b19f2e7627b9e96282ed6 100644
--- a/libcpu/arm/lpc24xx/context_rvds.S
+++ b/libcpu/arm/lpc24xx/context_rvds.S
@@ -49,20 +49,23 @@ rt_hw_context_switch	PROC
 	STMFD	sp!, {lr}			; push pc (lr should be pushed in place of PC)
 	STMFD	sp!, {r0-r12, lr}	; push lr & register file
 
-	MRS		r4, cpsr
+	MRS		r4, cpsr
+	TST     lr, #0x01
+	BEQ     _ARM_MODE
+	ORR     r4, r4, #0x20       ; it's thumb code
+
+_ARM_MODE
 	STMFD	sp!, {r4}			; push cpsr
-	MRS		r4, spsr
-	STMFD	sp!, {r4}			; push spsr
 
-	STR	sp, [r0]				; store sp in preempted tasks TCB
-	LDR	sp, [r1]				; get new task stack pointer
+	STR	    sp, [r0]			; store sp in preempted tasks TCB
+	LDR	    sp, [r1]			; get new task stack pointer
 
-	LDMFD	sp!, {r4}			; pop new task spsr
-	MSR	spsr_cxsf, r4
-	LDMFD	sp!, {r4}			; pop new task cpsr
-	MSR	cpsr_cxsf, r4
+	LDMFD	sp!, {r4}			; pop new task cpsr to spsr
+	MSR     spsr_cxsf, r4
+	BIC     r4, r4, #0x20       ; must be ARM mode
+	MSR	    cpsr_cxsf, r4
 
-	LDMFD	sp!, {r0-r12, lr, pc}	; pop new task r0-r12, lr & pc
+	LDMFD	sp!, {r0-r12, lr, pc}^	; pop new task r0-r12, lr & pc, copy spsr to cpsr
 	ENDP
 
 ;/*
@@ -73,12 +76,12 @@ rt_hw_context_switch_to	PROC
 	EXPORT rt_hw_context_switch_to
 	LDR	sp, [r0]				; get new task stack pointer
 
-	LDMFD	sp!, {r4}			; pop new task spsr
-	MSR	spsr_cxsf, r4
-	LDMFD	sp!, {r4}			; pop new task cpsr
-	MSR	cpsr_cxsf, r4
+	LDMFD	sp!, {r4}			; pop new task cpsr to spsr
+	MSR	    spsr_cxsf, r4
+	BIC     r4, r4, #0x20       ; must be ARM mode
+	MSR	    cpsr_cxsf, r4
 
-	LDMFD	sp!, {r0-r12, lr, pc}	; pop new task r0-r12, lr & pc
+	LDMFD	sp!, {r0-r12, lr, pc}^	; pop new task r0-r12, lr & pc, copy spsr to cpsr
 	ENDP
 
 ;/*
diff --git a/libcpu/arm/lpc24xx/stack.c b/libcpu/arm/lpc24xx/stack.c
index bfd4bc652cdd6efd0ccbc4981dff460947d5b859..c07176a8742eaa14dce3a61ec1c6782401cc5a9b 100644
--- a/libcpu/arm/lpc24xx/stack.c
+++ b/libcpu/arm/lpc24xx/stack.c
@@ -12,7 +12,7 @@
  * 2008-12-11     XuXinming    first version
  */
 #include <rtthread.h>
-#include "LPC24xx.h"
+#include "LPC24xx.h"
 
 /**
  * @addtogroup LPC2478
@@ -49,9 +49,13 @@ rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter,
 	*(--stk) = 0;							/* r3 */
 	*(--stk) = 0;							/* r2 */
 	*(--stk) = 0;							/* r1 */
-	*(--stk) = (unsigned long)parameter;	/* r0 : argument */
-	*(--stk) = SVCMODE;						/* cpsr */
-	*(--stk) = SVCMODE;						/* spsr */
+	*(--stk) = (unsigned long)parameter;	/* r0 : argument */
+
+	/* cpsr */
+	if ((rt_uint32_t)tentry & 0x01)
+		*(--stk) = SVCMODE | 0x20;			/* thumb mode */
+	else
+		*(--stk) = SVCMODE;					/* arm mode   */
 
 	/* return task's current stack address */
 	return (rt_uint8_t *)stk;
diff --git a/libcpu/arm/lpc24xx/start_rvds.S b/libcpu/arm/lpc24xx/start_rvds.S
index 34e86940d9ef9a8746eb25b4212235b78acec1e4..1b166b133e307ae850415250bdc19bddc207fe33 100644
--- a/libcpu/arm/lpc24xx/start_rvds.S
+++ b/libcpu/arm/lpc24xx/start_rvds.S
@@ -1538,8 +1538,6 @@ rt_hw_context_switch_interrupt_do   PROC
                 LDMFD   r4!, {r0-r3}
                 STMFD   sp!, {r0-r3}    ; push old task's r3-r0
                 STMFD   sp!, {r5}       ; push old task's cpsr
-                MRS     r4,  spsr
-                STMFD   sp!, {r4}       ; push old task's spsr
 
                 LDR     r4,  =rt_interrupt_from_thread
                 LDR     r5,  [r4]
@@ -1548,13 +1546,13 @@ rt_hw_context_switch_interrupt_do   PROC
                 LDR     r6,  =rt_interrupt_to_thread
                 LDR     r6,  [r6]
                 LDR     sp,  [r6]       ; get new task's stack pointer
-            
-                LDMFD   sp!, {r4}       ; pop new task's spsr
-                MSR     spsr_cxsf, r4
-                LDMFD   sp!, {r4}       ; pop new task's psr
+
+                LDMFD   sp!, {r4}       ; pop new task's cpsr to spsr
+				MSR     spsr_cxsf, r4
+				BIC     r4, r4, #0x20   ; must be ARM mode
                 MSR     cpsr_cxsf, r4
 
-                LDMFD   sp!, {r0-r12,lr,pc} ; pop new task's r0-r12,lr & pc
+                LDMFD   sp!, {r0-r12,lr,pc}^ ; pop new task's r0-r12,lr & pc, copy spsr to cpsr
                 ENDP
 
                 IF      :DEF:__MICROLIB