1 files changed, 92 insertions, 0 deletions
diff --git a/arm9/lib/src/OS_context.c b/arm9/lib/src/OS_context.c
new file mode 100644
index 00000000..38b722ba
--- /dev/null
+++ b/arm9/lib/src/OS_context.c
@@ -0,0 +1,92 @@
+#include "OS_context.h"
+#include "consts.h"
+#include "function_target.h"
+
+ARM_FUNC asm void OS_InitContext(register OSContext *context, register u32 newpc, register u32 newsp)
+{
+    add newpc, newpc, #4
+    str newpc, [context, #0x40]
+
+    str newsp, [context, #0x44]
+    sub newsp, newsp, #HW_SVC_STACK_SIZE
+
+    tst newsp, #4
+    subne newsp, newsp, #4
+    str newsp, [context, #0x38]
+
+    ands r1, newpc, #1
+    movne r1, #0x3f
+    moveq r1, #0x1f
+    str r1, [context]
+
+    mov r1, #0
+    str r1, [context, #0x4]
+    str r1, [context, #0x8]
+    str r1, [context, #0xc]
+    str r1, [context, #0x10]
+    str r1, [context, #0x14]
+    str r1, [context, #0x18]
+    str r1, [context, #0x1c]
+    str r1, [context, #0x20]
+    str r1, [context, #0x24]
+    str r1, [context, #0x28]
+    str r1, [context, #0x2c]
+    str r1, [context, #0x30]
+    str r1, [context, #0x34]
+    str r1, [context, #0x3c]
+
+    bx lr
+}
+
+ARM_FUNC asm BOOL OS_SaveContext(register OSContext* context)
+{
+    stmfd sp!, {lr, r0}
+    add r0, r0, #0x48
+    ldr r1, =CP_SaveContext
+    blx r1
+    ldmfd sp!, {lr, r0}
+
+    add r1, r0, #0
+
+    mrs r2, cpsr
+    str r2, [r1], #0x4
+
+    mov r0, #0xd3
+    msr cpsr_c, r0
+    str sp, [r1, #0x40]
+    msr cpsr_c, r2
+
+    mov r0, #1
+    stmia r1, {r0-r14}
+    add r0, pc, #8
+    str r0, [r1, #0x3c]
+
+    mov r0, #0
+    bx lr
+}
+
+ARM_FUNC asm void OS_LoadContext(register OSContext* context)
+{
+    stmfd sp!, {lr, r0}
+    add r0, r0, #0x48
+
+    ldr r1, =CPi_RestoreContext
+    blx r1
+    ldmfd sp!, {lr, r0}
+
+    mrs r1, cpsr
+    bic r1, r1, #HW_PSR_CPU_MODE_MASK
+    orr r1, r1, #0xd3
+    msr cpsr_c, r1
+
+    ldr r1, [r0], #0x4
+    msr spsr_fsxc, r1
+
+    ldr sp, [r0, #0x40]
+
+    ldr lr, [r0, #0x3c]
+    ldmia r0, {r0-r14}^
+    nop
+
+    subs pc, lr, #4
+}