3 files changed, 95 insertions, 81 deletions
diff --git a/arm9/asm/OS_context.s b/arm9/asm/OS_context.s
deleted file mode 100644
index 2c8feeb8..00000000
--- a/arm9/asm/OS_context.s
+++ /dev/null
@@ -1,78 +0,0 @@
-    .include "asm/macros.inc"
-    .include "global.inc"
-
-    .text
-
-	arm_func_start OS_InitContext
-OS_InitContext: ; 0x020CBAC4
-	add r1, r1, #0x4
-	str r1, [r0, #0x40]
-	str r2, [r0, #0x44]
-	sub r2, r2, #0x40
-	tst r2, #0x4
-	subne r2, r2, #0x4
-	str r2, [r0, #0x38]
-	ands r1, r1, #0x1
-	movne r1, #0x3f
-	moveq r1, #0x1f
-	str r1, [r0, #0x0]
-	mov r1, #0x0
-	str r1, [r0, #0x4]
-	str r1, [r0, #0x8]
-	str r1, [r0, #0xc]
-	str r1, [r0, #0x10]
-	str r1, [r0, #0x14]
-	str r1, [r0, #0x18]
-	str r1, [r0, #0x1c]
-	str r1, [r0, #0x20]
-	str r1, [r0, #0x24]
-	str r1, [r0, #0x28]
-	str r1, [r0, #0x2c]
-	str r1, [r0, #0x30]
-	str r1, [r0, #0x34]
-	str r1, [r0, #0x3c]
-	bx lr
-
-	arm_func_start OS_SaveContext
-OS_SaveContext:
-	stmdb sp!, {r0,lr}
-	add r0, r0, #0x48
-	ldr r1, _020CBB78 ; =CP_SaveContext
-	blx r1
-	ldmia sp!, {r0,lr}
-	add r1, r0, #0x0
-	mrs r2, cpsr
-	str r2, [r1], #0x4
-	mov r0, #0xd3
-	msr cpsr_c, r0
-	str sp, [r1, #0x40]
-	msr cpsr_c, r2
-	mov r0, #0x1
-	stmia r1, {r0-lr}
-	add r0, pc, #0x8 ; =_020CBB78
-	str r0, [r1, #0x3c]
-	mov r0, #0x0
-	bx lr
-	.balign 4
-_020CBB78: .word CP_SaveContext
-
-	arm_func_start OS_LoadContext
-OS_LoadContext: ; 0x020CBB7C
-	stmdb sp!, {r0,lr}
-	add r0, r0, #0x48
-	ldr r1, _020CBBBC ; =CPi_RestoreContext
-	blx r1
-	ldmia sp!, {r0,lr}
-	mrs r1, cpsr
-	bic r1, r1, #0x1f
-	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-lr}^
-	mov r0, r0
-	subs pc, lr, #0x4
-	.balign 4
-_020CBBBC: .word CPi_RestoreContext
diff --git a/arm9/lib/include/OS_context.h b/arm9/lib/include/OS_context.h
index 7a58a2b1..7c25ce2e 100644
--- a/arm9/lib/include/OS_context.h
+++ b/arm9/lib/include/OS_context.h
@@ -15,8 +15,8 @@ typedef struct OSContext
     CPContext cp_context;
 } OSContext;
 
-extern u32 OS_SaveContext(OSContext *context);
-extern void OS_LoadContext(OSContext *context);
-extern void OS_InitContext(OSContext *context, u32 func, u32 stack);
+void OS_InitContext(register OSContext *context, register u32 newpc, register u32 newsp);
+BOOL OS_SaveContext(OSContext *context);
+void OS_LoadContext(OSContext *context);
 
 #endif //POKEDIAMOND_OS_CONTEXT_H
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
+}