/* * Copyright (C) 2021 xiaoliang<1296283984@qq.com>. */ .thumb .syntax unified .text .fpu vfpv2 .global PendSV_Handler .type PendSV_Handler, %function PendSV_Handler: /* PendSV异常处理程序通过插入伪造的异常返回上下文实现抢占被中断的线程的CPU * 并在抢占结束后手动的回复被中断线程的上下文 */ /* 当发生异常时，当前的LR与被压栈的xPSR指示了被中断的线程某些配置和状态， * LR: * BIT4: 当这个位被清除时，指示被中断线程使用了浮点上下文，并在栈中预留的浮点上下文的空间 * BIT3: 当这个位被设置时，指示异常返回到Thread模式，否则返回到Handler模式，PendSV异常发生时，这个位应该为1 * BIT2: 当这个位被设置时，指示异常返回使用PSP恢复线程上下文，否则使用MSP */ /* 已压栈的xPSR: * BIT24: 当这个位被设置时，异常返回后进入Thumb模式，否则返回到ARM模式 * BIT9: 当这个位被设置时，表示栈已开启8Byte对齐，并指示在弹出线程上下文后SP+4 */ /* 备份LR的状态 */ MOV R0, LR /* 检查LR BIT4确定线程是否使用了浮点上下文 */ LSLS R1, R0, #27 BMI thread_use_fp_endif /* 当线程使用了浮点功能时，我们需要执行一条浮点指令，触发Lazy Stacking做浮点上下文保存 */ VCMP.F32 S0, #0.0 /* 我们需要插入伪造的无浮点上下文的异常返回现场，因此我们设置LR的BIT4和清除CONTROL的BIT2(FPCA) */ ORRS LR, #0x10 MRS R1, CONTROL BICS R1, #4 MSR CONTROL, R1 thread_use_fp_endif: /* 我们需要插入异常返回现场需要使用的空间，并获取它的栈顶 */ /* 检查LR BIT2确定异常返回使用PSP还是MSP */ LSLS R1, R0, #29 BMI insert_fake_exc_return_context_space_use_psp /* 因为当前处于Handler模式，因此我们可以直接使用SP来操作MSP */ SUB SP, #32 MOV R2, SP B insert_fake_exc_return_context_space_endif insert_fake_exc_return_context_space_use_psp: /* 在Handler模式下，需要使用MRS与MSR指令操作PSP */ MRS R2, PSP SUBS R2, #32 MSR PSP, R2 insert_fake_exc_return_context_space_endif: /* 我们需要保存LR的状态，在异常返回后使用它，因此将LR设置到返回后的R0中 */ STR R0, [R2, #0] /* 我们设置堆栈中返回后的位置为pendsv_exc_return_handler */ ADR R0, pendsv_exc_return_handler STR R0, [R2, #24] /* 我们设置堆栈中xPSR，使异常返回后进入Thumb模式并恢复以前的IPSR状态 */ MOVS R1, #1 LSLS R1, #24 /* 读取返回上下文中的IPSR */ LDR R0, [R2, #60] LSLS R0, #26 LSRS R0, #26 /* 设置xPSR为BIT24 + IPSR */ ORRS R1, R0 STR R1, [R2, #28] /* 异常返回，进入pendsv_exc_return_handler */ BX LR pendsv_exc_return_handler: /* 保存异常时的LR的状态 */ PUSH {R0} /* 调用调度程序 */ BL k_schedule /* 当抢占程序执行结束之后，我们需要恢复抢占前的上下文 * 我们先对浮点上下文做恢复，此后再恢复非浮点部分的上下文 * 我们还需要检查栈对齐STKALIGN标志，确定恢复之后的栈顶位置 */ POP {R0} /* 我们使用R3表示线程上下文恢复之后的栈顶位置 * 现在我们先跳过非浮点部分的上下文 */ ADD R3, SP, #32 /* 检查是否使用浮点上下文 */ LSLS R1, R0, #27 BMI thread_restore_use_fp_endif /* 恢复浮点上下文，并将空位弹出 */ VLDMIA.F32 R3!, {S0-S15} LDMIA R3!, {R1} VMSR FPSCR, R1 ADDS R3, #4 /* 清除浮点使用状态 */ MRS R1, CONTROL BICS R1, #4 MSR CONTROL, R1 thread_restore_use_fp_endif: /* 读取xPSR */ LDR R0, [SP, #28] /* 判断xPSR的BIT9，若置位则栈顶需要+4 */ LSLS R1, R0, #22 LSRS R1, #31 LSLS R1, #2 ADDS R3, R1 /* 读取线程的PC，并根据xPSR的BIT24设置Thumb或者ARM模式 */ LDR R1, [SP, #24] LSLS R2, R0, #7 LSRS R2, #31 ORRS R1, R2 /* 我们总是需要在R0-R12，LR，SP恢复之后才能执行返回 * 此时，我们没有可以使用的寄存器保存PC， * 因此需要将PC存入栈中，使用POP指令进行恢复 * 同时我们也无法在恢复R0-R3之后，再保存栈顶的位置 * 因此我们必须使用R4记录栈顶，因此还需要对R4做保存 */ STMDB R3!, {R1} STMDB R3!, {R4} MOV R4, R3 /* 恢复R12，LR，xPSR */ MSR APSR_nzcvq, R0 LDRD R12, LR, [SP, #16] /* 恢复R0-R3，SP */ POP {R0-R3} MOV SP, R4 /* 返回抢占线程 */ POP {R4, PC}