#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

    .section .text.HardFault_Handler,"ax",%progbits
    .global HardFault_Handler
    .thumb_func
HardFault_Handler:
    tst lr, #4
    ite eq
    mrseq r0, msp
    mrsne r0, psp
    mov r1, lr
    b HardFault_Handler_C



    .align 4
    .end