I’m making my own RTOS and am working on the context switch from one task to another. I’m trying to follow something similar to the FreeRTOS context switch.

I set up my task stack so that on exception return, the hardware will pop R0-R3, R12, LR, PC, and xPSR, according to this ARMv7-M manual page 542 pseudocode

PopStack(bits(32) frameptr, bits(28) EXC_RETURN) /* only stack locations, not the load order, are
architected */
  (... stuff i took out ...)
  R[0] = MemA[frameptr,4];
  R[1] = MemA[frameptr+0x4,4];
  R[2] = MemA[frameptr+0x8,4];
  R[3] = MemA[frameptr+0xC,4];
  R[12] = MemA[frameptr+0x10,4];
  LR = MemA[frameptr+0x14,4];
  BranchTo(MemA[frameptr+0x18,4]); // UNPREDICTABLE if the new PC not halfword aligned
  psr = MemA[frameptr+0x1C,4];

This is how the stack is initialized for each task. R4 to R11 are popped by my code in the context switch and the rest are popped by the hardware automatically.

static void init_task_stack(tcb_t *task, task_func_t task_func, void *parameters, uint32_t *stack, uint32_t stack_size)
{

    memset(stack, 0, stack_size * sizeof(uint32_t));

    uint32_t *stack_ptr = stack + stack_size - 1;
    stack_ptr = (uint32_t *)((uint32_t)stack_ptr & ~0x7); // Ensure 8-byte alignment

    *stack_ptr = (1U << 24);           // xPSR (set bit 24 (T-bit) for thumb)
    stack_ptr--;
    *stack_ptr = ((uint32_t)task_func) & START_ADDRESS_MASK;  // PC (task function address)
    stack_ptr--;
    *stack_ptr = (uint32_t)task_exit_error;           // LR 
    stack_ptr -= 5;                        // R12, R3, R2, R1
    *stack_ptr = (uint32_t)parameters; // R0

    stack_ptr--;
    *stack_ptr = EXEC_RETURN;

    stack_ptr -= 8;                        // R11, R10, R9, R8, R7, R6, R5 and R4.

    task->stack_top = stack_ptr; 

}

Next, here is my context switch PendSV handler, which is almost exactly like the one from FreeRTOS.

I’ve confirmed that it can switch from the current task to the next ready task from bl scheduler.

After the ldmia and msr, my PSP is for the hardware to pop the registers that it is responsible for. This will happen after bx lr.

__attribute((naked)) void PendSV_Handler(void)
{
    __asm volatile 
    (
        // Save the context of the current task
        "     mrs r0, psp                     n" // get current PSP
        "     isb                             n"
        "     ldr r3, =curr_task              n" // load address of the pointer curr_task into r1
        "     ldr r2, [r3]                    n" // load TCB pointed to by curr_task, into r2
        "     stmdb r0!, {r4-r11, r14}         n" // store r4 - r11 on process stack
        "     str r0, [r2]                    n" // Save new stack top

        "     stmdb sp!, {r0, r3}             n"
        "     mov r0, %0                      n"
        "     msr basepri, r0                 n"
        "     dsb                             n"
        "     isb                             n"
        "     bl scheduler                    n"

        "     mov r0, #0                      n"
        "     msr basepri, r0                 n"
        "     ldmia sp!, {r0, r3}             n"

        "     ldr r1, [r3]                    n"
        "     ldr r0, [r1]                    n" // get the (new) curr_task's stack_top

        "     ldmia r0!, {r4-r11, r14}        n" // restore r4-r11 from stack

        "     msr psp, r0                     n" // update PSP with stack_top
        "     isb                             n"

        "     bx r14                          n" // return to the next task
        
        "     .align 4                        n"
        ::"i"(MAX_SYSCALL_INTERRUPT_PRIORITY)
    );
}

When right before bx lr, this is what PSP looks like in GDB.
The zeroes are R0-R3, R12. Blue underline is LR which is a function that should not be returned to, and orange underline is PC, my task function.
This follows the hardware stack popping convention from earlier in the post.

However, the bx lr doesn’t lead to the next task function, and all that happens is the SysTick handler runs again (which triggers this PendSV handler).

I’d appreciate any pointers on where to start looking about this issue.

Thank you!