203 lines
4.0 KiB
ArmAsm
203 lines
4.0 KiB
ArmAsm
.global port_int_disable
|
|
.global port_int_enable
|
|
|
|
.global port_cpsr_save
|
|
.global port_cpsr_restore
|
|
|
|
.global port_sched_start
|
|
.global port_context_switch
|
|
.global port_irq_context_switch
|
|
|
|
.global port_clz
|
|
|
|
.global PendSV_Handler
|
|
|
|
.extern k_curr_task
|
|
.extern k_next_task
|
|
|
|
|
|
.equ SCB_VTOR, 0xE000ED08
|
|
.equ NVIC_INT_CTRL, 0xE000ED04
|
|
.equ NVIC_SYSPRI14, 0xE000ED22
|
|
.equ NVIC_PENDSV_PRI, 0xFF
|
|
.equ NVIC_PENDSVSET, 0x10000000
|
|
|
|
|
|
.text
|
|
.align 2
|
|
.thumb
|
|
.syntax unified
|
|
|
|
|
|
.type port_int_disable, %function
|
|
port_int_disable:
|
|
CPSID I
|
|
BX LR
|
|
|
|
|
|
.type port_int_enable, %function
|
|
port_int_enable:
|
|
CPSIE I
|
|
BX LR
|
|
|
|
|
|
.type port_cpsr_save, %function
|
|
port_cpsr_save:
|
|
MRS R0, PRIMASK
|
|
CPSID I
|
|
BX LR
|
|
|
|
|
|
.type port_cpsr_restore, %function
|
|
port_cpsr_restore:
|
|
MSR PRIMASK, R0
|
|
BX LR
|
|
|
|
|
|
.type port_clz, %function
|
|
port_clz:
|
|
CLZ R0, R0
|
|
BX LR
|
|
|
|
|
|
.thumb_func
|
|
.type port_sched_start, %function
|
|
port_sched_start:
|
|
CPSID I
|
|
|
|
@ set pendsv priority lowest
|
|
@ otherwise trigger pendsv in port_irq_context_switch will cause a context switch in irq
|
|
@ that would be a disaster
|
|
MOVW R0, #:lower16:NVIC_SYSPRI14
|
|
MOVT R0, #:upper16:NVIC_SYSPRI14
|
|
|
|
MOVW R1, #:lower16:NVIC_PENDSV_PRI
|
|
MOVT R1, #:upper16:NVIC_PENDSV_PRI
|
|
STRB R1, [R0]
|
|
|
|
MOVW R0, #:lower16:SCB_VTOR
|
|
MOVT R0, #:upper16:SCB_VTOR
|
|
LDR R0, [R0]
|
|
LDR R0, [R0]
|
|
MSR MSP, R0
|
|
|
|
MOVW R0, #:lower16:k_curr_task
|
|
MOVT R0, #:upper16:k_curr_task
|
|
|
|
@ k_curr_task = k_next_task;
|
|
MOVW R1, #:lower16:k_next_task
|
|
MOVT R1, #:upper16:k_next_task
|
|
LDR R2, [R1]
|
|
STR R2, [R0]
|
|
|
|
@ sp = k_next_task->sp
|
|
LDR R0, [R2]
|
|
@ PSP = sp
|
|
MSR PSP, R0
|
|
|
|
MRS R0, CONTROL
|
|
ORR R0, R0, #2
|
|
MSR CONTROL, R0
|
|
|
|
ISB
|
|
|
|
@ restore r4-11 from new process stack
|
|
LDMFD SP!, {R4 - R11}
|
|
|
|
#if (defined(__VFP_FP__) && !defined(__SOFTFP__))
|
|
@ ignore EXC_RETURN the first switch
|
|
LDMFD SP!, {R0}
|
|
#endif
|
|
|
|
@ restore r0, r3
|
|
LDMFD SP!, {R0 - R3}
|
|
@ load R12 and LR
|
|
LDMFD SP!, {R12, LR}
|
|
@ load PC and discard xPSR
|
|
LDMFD SP!, {R1, R2}
|
|
|
|
CPSIE I
|
|
BX R1
|
|
|
|
|
|
.thumb_func
|
|
.type port_context_switch, %function
|
|
port_context_switch:
|
|
LDR R0, =NVIC_INT_CTRL
|
|
LDR R1, =NVIC_PENDSVSET
|
|
STR R1, [R0]
|
|
BX LR
|
|
|
|
|
|
.thumb_func
|
|
.type port_irq_context_switch, %function
|
|
port_irq_context_switch:
|
|
LDR R0, =NVIC_INT_CTRL
|
|
LDR R1, =NVIC_PENDSVSET
|
|
STR R1, [R0]
|
|
BX LR
|
|
|
|
|
|
.thumb_func
|
|
.type PendSV_Handler, %function
|
|
PendSV_Handler:
|
|
CPSID I
|
|
MRS R0, PSP
|
|
|
|
_context_save:
|
|
@ R0-R3, R12, LR, PC, xPSR is saved automatically here
|
|
#if (defined(__VFP_FP__) && !defined(__SOFTFP__))
|
|
@ is it extended frame?
|
|
TST LR, #0x10
|
|
IT EQ
|
|
VSTMDBEQ R0!, {S16 - S31}
|
|
@ S0 - S16, FPSCR saved automatically here
|
|
|
|
@ save EXC_RETURN
|
|
STMFD R0!, {LR}
|
|
#endif
|
|
|
|
@ save remaining regs r4 - 11 on process stack
|
|
STMFD R0!, {R4 - R11}
|
|
|
|
@ k_curr_task->sp = PSP;
|
|
MOVW R5, #:lower16:k_curr_task
|
|
MOVT R5, #:upper16:k_curr_task
|
|
LDR R6, [R5]
|
|
@ R0 is SP of process being switched out
|
|
STR R0, [R6]
|
|
|
|
_context_restore:
|
|
@ k_curr_task = k_next_task;
|
|
MOVW R1, #:lower16:k_next_task
|
|
MOVT R1, #:upper16:k_next_task
|
|
LDR R2, [R1]
|
|
STR R2, [R5]
|
|
|
|
@ R0 = k_next_task->sp
|
|
LDR R0, [R2]
|
|
|
|
@ restore R4 - R11
|
|
LDMFD R0!, {R4 - R11}
|
|
|
|
#if (defined(__VFP_FP__) && !defined(__SOFTFP__))
|
|
@ restore EXC_RETURN
|
|
LDMFD R0!, {LR}
|
|
@ is it extended frame?
|
|
TST LR, #0x10
|
|
IT EQ
|
|
VLDMIAEQ R0!, {S16 - S31}
|
|
#endif
|
|
|
|
@ Load PSP with new process SP
|
|
MSR PSP, R0
|
|
|
|
CPSIE I
|
|
|
|
@ R0-R3, R12, LR, PC, xPSR restored automatically here
|
|
@ S0 - S16, FPSCR restored automatically here if FPCA = 1
|
|
BX LR
|
|
|
|
.end
|
|
|