add thumb mode porting.

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

3 files changed, 74 insertions, 57 deletions

diff --git a/libcpu/arm/lpc214x/context_rvds.S b/libcpu/arm/lpc214x/context_rvds.S
index e24fa62a3d..b3a3b5bf8e 100644
--- a/libcpu/arm/lpc214x/context_rvds.S
+++ b/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
\ No newline at end of file
diff --git a/libcpu/arm/lpc214x/cpuport.c b/libcpu/arm/lpc214x/cpuport.c
index 702db0b21e..7986dad54b 100644
--- a/libcpu/arm/lpc214x/cpuport.c
+++ b/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;
diff --git a/libcpu/arm/lpc214x/start_rvds.S b/libcpu/arm/lpc214x/start_rvds.S
index 50e3fdcf94..bee5c64de3 100644
--- a/libcpu/arm/lpc214x/start_rvds.S
+++ b/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