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

HardFault_Handler PROC
    EXPORT  HardFault_Handler
        tst lr, #4                           // 检查 EXC_RETURN (LR) 的位 2
        ite eq
        mrseq r0, msp                        // 如果位 2 是 0，栈帧在 MSP
        mrsne r0, psp                        // 如果位 2 是 1，栈帧在 PSP
        mov r1, lr
        b HardFault_Handler_C                // 跳转到 C 实现，r0 作为第一个参数传入
    ENDP

    END