72 lines
2.3 KiB
ArmAsm
72 lines
2.3 KiB
ArmAsm
|
PRESERVE8
|
||
|
|
THUMB
|
||
|
|
|
||
|
|
AREA |.text|, CODE, READONLY
|
||
|
|
|
||
|
|
cpu_clz PROC
|
||
|
|
EXPORT cpu_clz
|
||
|
|
CLZ R0, R0
|
||
|
|
BX LR
|
||
|
|
ENDP
|
||
|
|
|
||
|
|
|
||
|
|
SVC_Handler PROC
|
||
|
|
EXPORT SVC_Handler
|
||
|
|
IMPORT svc_exc_return
|
||
|
|
IMPORT SVC_Handler_C
|
||
|
|
tst lr, #4 // 检查 EXC_RETURN (LR) 的位 2
|
||
|
|
ite eq
|
||
|
|
mrseq r0, msp // 如果位 2 是 0，栈帧在 MSP
|
||
|
|
mrsne r0, psp // 如果位 2 是 1，栈帧在 PSP
|
||
|
|
ldr r1, =svc_exc_return
|
||
|
|
str lr, [r1]
|
||
|
|
bl SVC_Handler_C // 跳转到 C 实现，r0 作为第一个参数传入
|
||
|
|
ldr r1, =svc_exc_return
|
||
|
|
ldr lr, [r1]
|
||
|
|
bx lr
|
||
|
|
ENDP
|
||
|
|
|
||
|
|
PendSV_Handler PROC
|
||
|
|
EXPORT PendSV_Handler
|
||
|
|
IMPORT g_os_port_switch_from_sp
|
||
|
|
IMPORT g_os_port_switch_to_sp
|
||
|
|
IMPORT os_port_on_switch_success
|
||
|
|
mrs r1, primask // 获取值
|
||
|
|
cpsid i // 关闭中断
|
||
|
|
PUSH {R1}
|
||
|
|
|
||
|
|
LDR R1, =g_os_port_switch_from_sp // 加载当前任务指针地址
|
||
|
|
LDR R1, [R1] // 加载 g_os_port_switch_from_sp 的值
|
||
|
|
CBZ R1, __PendSV_SwitchTo
|
||
|
|
|
||
|
|
/* ---- 压栈 ---- */
|
||
|
|
MRS R0, PSP
|
||
|
|
MOV R2, LR
|
||
|
|
MRS R3, CONTROL
|
||
|
|
STMDB R0!, {R2-R11}
|
||
|
|
|
||
|
|
/* ---- sp值保存到变量中 ---- */
|
||
|
|
STR R0, [R1] // 将 sp 的值加载到 R0
|
||
|
|
|
||
|
|
__PendSV_SwitchTo
|
||
|
|
BL os_port_on_switch_success // 执行切换完成后的设置
|
||
|
|
|
||
|
|
/* ---- 加载目标栈 ---- */
|
||
|
|
LDR R2, =g_os_port_switch_to_sp // 获取目标任务指针变量地址
|
||
|
|
LDR R2, [R2] // 获取切换目标任务的指针
|
||
|
|
LDR R0, [R2] // 获取目标任务的 sp 指针
|
||
|
|
LDMIA R0!, {R2-R11} // 恢复 R2-R11 寄存器的值
|
||
|
|
MOV LR, R2 // 恢复 LR 寄存器值
|
||
|
|
MSR CONTROL, R3 // 恢复 CONTROL 寄存器值, 这里是在 ISR 中，设置暂时不生效，直到退出 ISR 才生效，所以没有问题
|
||
|
|
ISB // 设置 CONTROL 后确保生效
|
||
|
|
|
||
|
|
MSR PSP, R0 // 更新 PSP 指向下一个任务的栈
|
||
|
|
POP {R1}
|
||
|
|
MSR PRIMASK, R1 // 开中断
|
||
|
|
BX LR
|
||
|
|
|
||
|
|
ENDP
|
||
|
|
|
||
|
|
END
|
||
|
|
|