80 lines
2.5 KiB
ArmAsm
80 lines
2.5 KiB
ArmAsm
#include "os_config.h"
|
|
|
|
.syntax unified
|
|
.thumb
|
|
|
|
.section .text.PendSV_Handler,"ax",%progbits
|
|
.global g_os_port_switch_from_sp
|
|
.global g_os_port_switch_to_sp
|
|
.global PendSV_Handler
|
|
.thumb_func
|
|
PendSV_Handler:
|
|
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
|
|
#if(OS_CFG_CPU_FPU_PRESENT)
|
|
TST LR, #0x10 // 测试第4位如果为0压栈浮点寄存器
|
|
VSTMDBEQ R0!, {S16-S31}
|
|
#endif
|
|
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 后确保生效
|
|
#if(OS_CFG_CPU_FPU_PRESENT)
|
|
TST LR, #0x10
|
|
IT EQ
|
|
VLDMIAEQ R0!, {S16-S31}
|
|
#endif
|
|
MSR PSP, R0 // 更新 PSP 指向下一个任务的栈
|
|
POP {R1}
|
|
MSR PRIMASK, R1 // 开中断
|
|
BX LR
|
|
|
|
|
|
.section .text.SVC_Handler,"ax",%progbits
|
|
.global SVC_Handler
|
|
.global svc_exc_return
|
|
.global SVC_Handler_C
|
|
.type SVC_Handler, %function
|
|
SVC_Handler:
|
|
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
|
|
|
|
.section .text.cpu_clz,"ax",%progbits
|
|
.global cpu_clz
|
|
.thumb_func
|
|
cpu_clz:
|
|
CLZ R0, R0
|
|
BX LR
|
|
|
|
.align 4
|
|
.end |