The core component of the scheduler consists of the exception handler associated with the system interrupt events, such as PendSV and SVCall. On Cortex-M, a PendSV exception can be triggered at any time by the software, setting the PENDSET flag, corresponding to bit 28 of the interrupt control and state register, located in the SCB at address 0xE000ED04. A simple macro is defined to initiate the context switch by setting the flag:

#define SCB_ICSR (*((volatile uint32_t *)0xE000ED04))
#define schedule() SCB_ICSR |= (1 << 28)

The call to schedule from the kernel, and all the subsequent calls, would cause a context switch, which can now be implemented in the PendSV handler. To complete a context switch, the handler has to perform the following steps:

1. Store the current stack pointer from the SP register to the task block
2. Push the extra stack frame to the stack, by calling store_context
3. Change the state of the current task into TASK_READY
4. Select a new task to resume
5. Change the state of the new task into TASK_RUNNING
6. Retrieve the new stack pointer from the associated task block
7. Pop the extra stack frame from the stack, by calling restore_context
8. Set a special return value for the interrupt handler, to activate the thread mode at the end of the PendSV service routine

The isr_pendsv function must be naked, because it accesses the CPU register directly through the store/restore_context functions:

void __attribute__((naked)) isr_pendsv(void)
{
    store_context();
    asm volatile("mrs %0, msp" : "=r"(TASKS[running_task_id].sp));
    TASKS[running_task_id].state = TASK_READY;
    running_task_id++;
    if (running_task_id >= n_tasks)
        running_task_id = 0;
    TASKS[running_task_id].state = TASK_RUNNING;
    asm volatile("msr msp, %0"::"r"(TASKS[running_task_id].sp));
    restore_context();
    asm volatile("mov lr, %0" ::"r"(0xFFFFFFF9));
    asm volatile("bx lr");
}

The value loaded in the LR before returning is used to indicate that we are returning to the thread mode at the end of this interrupt. Depending on the value of the last three bits, the service routine informs the CPU which stack pointer to use when returning from the interrupt. The 0xFFFFFFF9 value used in this case corresponds to the thread mode using the main stack pointer. Different values will be needed later on, when the example is expanded to support separate stack pointers between the kernel and the process.

The complete context switch is now implemented inside the PendSV service routine, which for now is simply selecting the next task, and wraps around to execute the kernel, with ID 0, after the last task in the array. The service routine is triggered to run in handler mode every time that the schedule macro is called.