context_switch(void** old_sp, void* new_sp) r0 = old_sp (pointer to save location for current SP) r1 = new_sp (stack pointer to restore) Uses MSP/PSP dual stack pointer mechanism: - Runner (SPSEL=0): saves to MSP, loads PSP, sets SPSEL=1 - Coroutine (SPSEL=1): saves to PSP, loads MSP, clears SPSEL=0 Saves: r4-r11, lr (core callee-saved) + FPSCR, s16-s31 (FPU callee-saved) Total: 26 words = 104
| 143 | // [0..15]=s16..s31 [16]=FPSCR [17..24]=r4..r11 [25]=lr |
| 144 | // |
| 145 | __attribute__((naked, noinline, used)) |
| 146 | void context_switch(void** /*old_sp*/, void* /*new_sp*/) { |
| 147 | asm volatile( |
| 148 | // Save callee-saved regs to current stack (MSP or PSP) |
| 149 | "push {r4-r11, lr} \n" |
| 150 | "vmrs r4, fpscr \n" |
| 151 | "push {r4} \n" |
| 152 | "vpush {s16-s31} \n" |
| 153 | |
| 154 | // Save current SP to *old_sp |
| 155 | "str sp, [r0] \n" |
| 156 | |
| 157 | // Read CONTROL to determine active stack pointer |
| 158 | "mrs r4, control \n" |
| 159 | "tst r4, #2 \n" // test SPSEL bit |
| 160 | "bne 1f \n" // branch if PSP active |
| 161 | |
| 162 | // --- SPSEL=0 (MSP active, runner -> coroutine) --- |
| 163 | "msr psp, r1 \n" // PSP = new_sp (coroutine's stack) |
| 164 | "orr r4, r4, #2 \n" // set SPSEL bit |
| 165 | "cpsid i \n" // disable interrupts (race window) |
| 166 | "msr control, r4 \n" // switch sp to PSP |
| 167 | "isb \n" // flush pipeline |
| 168 | "cpsie i \n" // re-enable interrupts |
| 169 | "b 2f \n" |
| 170 | |
| 171 | "1: \n" |
| 172 | // --- SPSEL=1 (PSP active, coroutine -> runner) --- |
| 173 | "msr msp, r1 \n" // MSP = new_sp (runner's stack) |
| 174 | "bic r4, r4, #2 \n" // clear SPSEL bit |
| 175 | "cpsid i \n" // disable interrupts (race window) |
| 176 | "msr control, r4 \n" // switch sp to MSP |
| 177 | "isb \n" // flush pipeline |
| 178 | "cpsie i \n" // re-enable interrupts |
| 179 | |
| 180 | "2: \n" |
| 181 | // Restore from new stack (SPSEL now selects it) |
| 182 | "vpop {s16-s31} \n" |
| 183 | "pop {r4} \n" |
| 184 | "vmsr fpscr, r4 \n" |
| 185 | "pop {r4-r11, pc} \n" |
| 186 | ); |
| 187 | } |
| 188 | |
| 189 | //============================================================================= |
| 190 | // Coroutine Platform — ARM Cortex-M7 context switching |