From 2622f51496339b7f8ae235fa469a659780bb74c5 Mon Sep 17 00:00:00 2001 From: wuyangyong Date: Tue, 20 Dec 2011 16:06:20 +0000 Subject: [PATCH] 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 | 77 +++++++++++++++++++++++++++---- libcpu/arm/lpc214x/cpuport.c | 8 +++- libcpu/arm/lpc214x/start_rvds.S | 46 +----------------- 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 -- GitLab