add thumb mode porting.

git-svn-id: https://rt-thread.googlecode.com/svn/trunk@1854 bbd45198-f89e-11dd-88c7-29a3b14d5316

libcpu/arm/lpc214x/context_rvds.S

@@ -10,9 +10,21 @@
 ; * Change Logs:
 ; * Date           Author       Notes
 ; * 2009-01-20     Bernard      first version
+; * 2011-07-22     Bernard      added thumb mode porting
 ; */
 
-NOINT	EQU		0xc0	; disable interrupt in psr
+Mode_USR        EQU     0x10
+Mode_FIQ        EQU     0x11
+Mode_IRQ        EQU     0x12
+Mode_SVC        EQU     0x13
+Mode_ABT        EQU     0x17
+Mode_UND        EQU     0x1B
+Mode_SYS        EQU     0x1F
+
+I_Bit           EQU     0x80            ; when I bit is set, IRQ is disabled
+F_Bit           EQU     0x40            ; when F bit is set, FIQ is disabled
+
+NOINT	        EQU     0xc0	; disable interrupt in psr
 
 	AREA |.text|, CODE, READONLY, ALIGN=2
 	ARM
@@ -50,19 +62,21 @@ rt_hw_context_switch	PROC
 	STMFD	sp!, {r0-r12, lr}	; push lr & register file
 
 	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
 
-	LDMFD	sp!, {r4}			; pop new task spsr
+    LDMFD   sp!, {r4}               ; pop new task cpsr to spsr
 	MSR	spsr_cxsf, r4
-	LDMFD	sp!, {r4}			; pop new task cpsr
+    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 +87,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
+    LDMFD   sp!, {r4}               ; pop new task cpsr to spsr
 	MSR	spsr_cxsf, r4
-	LDMFD	sp!, {r4}			; pop new task cpsr
+    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
 
 ;/*
@@ -103,5 +117,48 @@ _reswitch
 	STR r1, [r2]
 	BX	lr
 	ENDP
-
+
+; /*
+; * void rt_hw_context_switch_interrupt_do(rt_base_t flag)
+; */
+rt_hw_context_switch_interrupt_do	PROC
+	EXPORT rt_hw_context_switch_interrupt_do
+	MOV		r1,  #0			; clear flag
+	STR		r1,  [r0]
+
+	LDMFD	sp!, {r0-r12,lr}; reload saved registers
+	STMFD	sp!, {r0-r3}	; save r0-r3
+	MOV		r1,  sp
+	ADD		sp,  sp, #16	; restore sp
+	SUB		r2,  lr, #4		; save old task's pc to r2
+
+	MRS		r3,  spsr		; get cpsr of interrupt thread
+
+	; switch to SVC mode and no interrupt
+	MSR     cpsr_c, #I_Bit:OR:F_Bit:OR:Mode_SVC
+
+	STMFD	sp!, {r2}		; push old task's pc
+	STMFD	sp!, {r4-r12,lr}; push old task's lr,r12-r4
+	MOV		r4,  r1			; Special optimised code below
+	MOV		r5,  r3
+	LDMFD	r4!, {r0-r3}
+	STMFD	sp!, {r0-r3}	; push old task's r3-r0
+	STMFD	sp!, {r5}		; push old task's cpsr
+
+	LDR		r4,  =rt_interrupt_from_thread
+	LDR		r5,  [r4]
+	STR		sp,  [r5]		; store sp in preempted tasks's TCB
+
+	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 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, copy spsr to cpsr
+	ENDP
+
 	END

libcpu/arm/lpc214x/cpuport.c

@@ -56,8 +56,12 @@ rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter,
 	*(--stk) = 0;							/* r2 */
 	*(--stk) = 0;							/* r1 */
 	*(--stk) = (unsigned long)parameter;	/* r0 : argument */
-	*(--stk) = SVCMODE;						/* cpsr */
-	*(--stk) = SVCMODE;						/* spsr */
+
+	/* 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;

libcpu/arm/lpc214x/start_rvds.S

@@ -421,6 +421,7 @@ MEMMAP          EQU     0xE01FC040      ; Memory Mapping Control
 				IMPORT rt_interrupt_from_thread
 				IMPORT rt_interrupt_to_thread
 				IMPORT rt_hw_trap_irq
+				IMPORT rt_hw_context_switch_interrupt_do
 
 IRQ_Handler		PROC
 				EXPORT IRQ_Handler
@@ -440,51 +441,6 @@ IRQ_Handler		PROC
 				SUBS	pc, lr, #4
 				ENDP
 
-; /*
-; * void rt_hw_context_switch_interrupt_do(rt_base_t flag)
-; */
-rt_hw_context_switch_interrupt_do	PROC
-				EXPORT rt_hw_context_switch_interrupt_do
-				MOV		r1,  #0			; clear flag
-				STR		r1,  [r0]
-
-				LDMFD	sp!, {r0-r12,lr}; reload saved registers
-				STMFD	sp!, {r0-r3}	; save r0-r3
-				MOV		r1,  sp
-				ADD		sp,  sp, #16	; restore sp
-				SUB		r2,  lr, #4		; save old task's pc to r2
-
-				MRS		r3,  spsr		; get cpsr of interrupt thread
-
-				; switch to SVC mode and no interrupt
-                MSR     cpsr_c, #I_Bit|F_Bit|Mode_SVC
-
-				STMFD	sp!, {r2}		; push old task's pc
-				STMFD	sp!, {r4-r12,lr}; push old task's lr,r12-r4
-				MOV		r4,  r1			; Special optimised code below
-				MOV		r5,  r3
-				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]
-				STR		sp,  [r5]		; store sp in preempted tasks's TCB
-
-				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
-				MSR		cpsr_cxsf, r4
-
-				LDMFD	sp!, {r0-r12,lr,pc}	; pop new task's r0-r12,lr & pc
-				ENDP
-
                 IF      :DEF:__MICROLIB
 
                 EXPORT  __heap_base