diff --git a/libcpu/arm/arm926/context_gcc.S b/libcpu/arm/arm926/context_gcc.S index 5357b36d2..419c5565d 100644 --- a/libcpu/arm/arm926/context_gcc.S +++ b/libcpu/arm/arm926/context_gcc.S @@ -53,14 +53,10 @@ rt_hw_context_switch: stmfd sp!, {r0-r12, lr} @; push lr & register file mrs r4, cpsr 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 msr spsr_cxsf, r4 - ldmfd sp!, {r4} @; pop new task cpsr - msr spsr_cxsf, r4 ldmfd sp!, {r0-r12, lr, pc}^ @; pop new task r0-r12, lr & pc /* @@ -70,9 +66,6 @@ rt_hw_context_switch: .globl rt_hw_context_switch_to 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 spsr_cxsf, r4 ldmfd sp!, {r0-r12, lr, pc}^ @; pop new task r0-r12, lr & pc diff --git a/libcpu/arm/arm926/context_iar.S b/libcpu/arm/arm926/context_iar.S index 9727dbd96..14110e522 100644 --- a/libcpu/arm/arm926/context_iar.S +++ b/libcpu/arm/arm926/context_iar.S @@ -55,14 +55,10 @@ rt_hw_context_switch: STMFD SP!, {R0-R12, LR} ; push lr & register file MRS R4, CPSR 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 MSR SPSR_cxsf, R4 - LDMFD SP!, {R4} ; pop new task cpsr - MSR SPSR_cxsf, R4 LDMFD SP!, {R0-R12, LR, PC}^ ; pop new task r0-r12, lr & pc /* @@ -72,12 +68,8 @@ rt_hw_context_switch: PUBLIC rt_hw_context_switch_to 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 SPSR_cxsf, R4 - LDMFD SP!, {R0-R12, LR, PC}^ ; pop new task r0-r12, lr & pc /* diff --git a/libcpu/arm/arm926/context_rvds.S b/libcpu/arm/arm926/context_rvds.S index e5fda293d..32bd235fe 100644 --- a/libcpu/arm/arm926/context_rvds.S +++ b/libcpu/arm/arm926/context_rvds.S @@ -60,14 +60,10 @@ rt_hw_context_switch proc stmfd sp!, {r0-r12, lr} ; push lr & register file mrs r4, cpsr 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 msr spsr_cxsf, r4 - ldmfd sp!, {r4} ; pop new task cpsr - msr spsr_cxsf, r4 ldmfd sp!, {r0-r12, lr, pc}^ ; pop new task r0-r12, lr & pc endp @@ -78,11 +74,8 @@ rt_hw_context_switch proc 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 spsr_cxsf, r4 ldmfd sp!, {r0-r12, lr, pc}^ ; pop new task r0-r12, lr & pc endp diff --git a/libcpu/arm/arm926/stack.c b/libcpu/arm/arm926/stack.c index ba3a11d20..53508e5ac 100644 --- a/libcpu/arm/arm926/stack.c +++ b/libcpu/arm/arm926/stack.c @@ -66,8 +66,11 @@ rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter, *(--stk) = 0; /* r2 */ *(--stk) = 0; /* r1 */ *(--stk) = (rt_uint32_t)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/arm926/start_gcc.S b/libcpu/arm/arm926/start_gcc.S index 05b39debc..f2cceeb8f 100644 --- a/libcpu/arm/arm926/start_gcc.S +++ b/libcpu/arm/arm926/start_gcc.S @@ -291,24 +291,19 @@ rt_hw_context_switch_interrupt_do: STR R1, [R0] @; Save to flag variable LDMFD SP!, {R0-R12,LR} @; Reload saved registers - STMFD SP!, {R0-R3} @; Save R0-R3 - MOV R1, SP @; Save old task's SP to R1 - ADD SP, SP, #16 @; Restore SP + STMFD SP, {R0-R2} @; Save R0-R2 + SUB R1, SP, #4*3 @; Save old task's SP to R1 SUB R2, LR, #4 @; Save old task's PC to R2 - MRS R3, SPSR @; Get CPSR of interrupt thread - + MRS R0, SPSR @; Get CPSR of interrupt thread + MSR CPSR_c, #MODE_SVC|NOINT @; Switch to SVC mode and no interrupt 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 + STMFD SP!, {R3-R12,LR} @; Push old task's LR,R12-R3 + LDMFD R1, {R1-R3} + STMFD SP!, {R1-R3} @; Push old task's R2-R0 + STMFD SP!, {R0} @; Push old task's CPSR LDR R4, =rt_interrupt_from_thread LDR R5, [R4] @; R5 = stack ptr in old tasks's TCB @@ -320,7 +315,5 @@ rt_hw_context_switch_interrupt_do: LDMFD SP!, {R4} @; Pop new task's SPSR MSR SPSR_cxsf, R4 - LDMFD SP!, {R4} @; Pop new task's CPSR - MSR SPSR_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 SPSR 2 CPSR diff --git a/libcpu/arm/arm926/start_iar.S b/libcpu/arm/arm926/start_iar.S index b7cc9e170..187ae35bb 100644 --- a/libcpu/arm/arm926/start_iar.S +++ b/libcpu/arm/arm926/start_iar.S @@ -262,24 +262,19 @@ rt_hw_context_switch_interrupt_do: STR R1, [R0] ; Save to flag variable LDMFD SP!, {R0-R12,LR} ; Reload saved registers - STMFD SP!, {R0-R3} ; Save R0-R3 - MOV R1, SP ; Save old task's SP to R1 - ADD SP, SP, #16 ; Restore SP + STMFD SP, {R0-R2} ; Save R0-R2 + SUB R1, SP, #4*3 ; Save old task's SP to R1 SUB R2, LR, #4 ; Save old task's PC to R2 - MRS R3, SPSR ; Get CPSR of interrupt thread + MRS R0, SPSR ; Get CPSR of interrupt thread MSR CPSR_c, #MODE_SVC|NOINT ; Switch to SVC mode and no interrupt 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 + STMFD SP!, {R3-R12,LR} ; Push old task's LR,R12-R3 + LDMFD R1, {R1-R3} + STMFD SP!, {R1-R3} ; Push old task's R2-R0 + STMFD SP!, {R0} ; Push old task's CPSR LDR R4, =rt_interrupt_from_thread LDR R5, [R4] ; R5 = stack ptr in old tasks's TCB @@ -291,8 +286,6 @@ rt_hw_context_switch_interrupt_do: LDMFD SP!, {R4} ; Pop new task's SPSR MSR SPSR_cxsf, R4 - LDMFD SP!, {R4} ; Pop new task's CPSR - MSR SPSR_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 SPSR to CPSR END diff --git a/libcpu/arm/arm926/start_rvds.S b/libcpu/arm/arm926/start_rvds.S index cabcd6590..edf2fcb96 100644 --- a/libcpu/arm/arm926/start_rvds.S +++ b/libcpu/arm/arm926/start_rvds.S @@ -274,24 +274,19 @@ rt_hw_context_switch_interrupt_do PROC STR R1, [R0] ; Save to flag variable LDMFD SP!, {R0-R12,LR} ; Reload saved registers - STMFD SP!, {R0-R3} ; Save R0-R3 - MOV R1, SP ; Save old task's SP to R1 - ADD SP, SP, #16 ; Restore SP + STMFD SP, {R0-R2} ; Save R0-R2 + SUB R1, SP, #4*3 ; Save old task's SP to R1 SUB R2, LR, #4 ; Save old task's PC to R2 - MRS R3, SPSR ; Get CPSR of interrupt thread + MRS R0, SPSR ; Get CPSR of interrupt thread MSR CPSR_c, #MODE_SVC:OR:NOINT ; Switch to SVC mode and no interrupt 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 + STMFD SP!, {R3-R12,LR} ; Push old task's LR,R12-R3 + LDMFD R1, {R1-R3} + STMFD SP!, {R1-R3} ; Push old task's R2-R0 + STMFD SP!, {R0} ; Push old task's CPSR LDR R4, =rt_interrupt_from_thread LDR R5, [R4] ; R5 = stack ptr in old tasks's TCB @@ -303,10 +298,8 @@ rt_hw_context_switch_interrupt_do PROC LDMFD SP!, {R4} ; Pop new task's SPSR MSR SPSR_cxsf, R4 - LDMFD SP!, {R4} ; Pop new task's CPSR - MSR SPSR_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 SPSR to CPSR ENDP IF :DEF:__MICROLIB