1 files changed, 56 insertions, 0 deletions
diff --git a/os1_cm3.s b/os1_cm3.s
new file mode 100644
index 0000000..a9083dc
--- /dev/null
+++ b/os1_cm3.s
@@ -0,0 +1,56 @@
+.syntax unified
+.cpu cortex-m3
+.thumb
+
+.section .text
+
+/*
+
+User threads use the process stack (PSP register), kernel and exception code use the main stack (MSP register).
+
+*/
+
+.thumb_func
+.global do_first_context_switch
+// void do_first_context_switch(uint8_t *user_stack, void (task)(const void *arg));
+do_first_context_switch:
+    /* Set PSP to the user task's stack */
+    msr     psp, r0
+
+    // Set CONTROL.SPSEL=1 so that we run with two stacks
+    mov     r0, #2
+    msr     control, r0
+    isb
+
+    // Restore the data from hardware_frame_t.
+    pop     {r0 - r3, r12, lr}
+    // Pop PC and PSR. PSR is ignored, but we branch to the new PC
+    pop     {r4, r5}
+    bx      r4
+
+/*
+When this function is executed {r0-r3,r12,lr,pc} has been pushed to the stack pointed to by PSP. We push the rest of the
+registers to the PSP. The current stack pointer is the MSP.
+ */
+.thumb_func
+.global PendSV_Handler
+PendSV_Handler:
+    // Save the rest of the context to the current process' stack
+    mrs     r0, psp
+    stmdb   r0!, {r4 - r11}
+
+    // Call select_next_task_sp. after return, r0 points to a task_t
+    bl      _Z16select_next_taskPh   // task_t *select_next_task(uint8_t *current_stack)
+
+    // load task_t.stack and task_t.lr into r1 and lr
+    ldm     r0, {r1, lr}
+
+    ldmia   r1!, {r4 - r11}
+    msr     psp, r1
+
+    // Return, let the CPU restore the hardware part of the context
+    bx      lr
+.pool
+.size PendSV_Handler,.-PendSV_Handler
+
+.end