    .syntax unified
    .cpu    cortex-m4
    .fpu    fpv4-sp-d16
    .thumb

    .extern os_curr_task
    .extern os_next_task

    .global PendSV_Handler
    .type   PendSV_Handler, %function
PendSV_Handler:
    /* Disable interrupts during switch */
    cpsid   i

    /* r0 = PSP (the task stack pointer before exception) */
    mrs     r0, psp

    /* Save current context */
    ldr     r1, =os_curr_task
    ldr     r2, [r1]            /* r2 = os_curr_task (ptr to TCB) */
    cbz     r2, load_next       /* first switch: nothing to save */

    stmdb   r0!, {r4-r11}       /* push r4-r11 onto task stack */
    str     r0, [r2]            /* tcb->sp = updated PSP */

load_next:
    /* Switch curr = next */
    ldr     r3, =os_next_task
    ldr     r2, [r3]            /* r2 = os_next_task */
    str     r2, [r1]            /* os_curr_task = os_next_task */

    /* Restore next context */
    ldr     r0, [r2]            /* r0 = next tcb->sp */
    ldmia   r0!, {r4-r11}       /* pop r4-r11 from task stack */

    msr     psp, r0             /* restore PSP */

    /* Re-enable IRQ */
    cpsie   i

    /* Return using PSP (thread mode, MSP for handler) */
    ldr     r0, =0xFFFFFFFD
    bx      r0

    .size PendSV_Handler, . - PendSV_Handler
