arm9 CP_context

1 files changed, 48 insertions, 0 deletions

diff --git a/arm9/lib/src/CP_context.c b/arm9/lib/src/CP_context.c
new file mode 100644
index 00000000..4e928c06
--- /dev/null
+++ b/arm9/lib/src/CP_context.c
@@ -0,0 +1,48 @@
+#include "CP_context.h"
+#include "function_target.h"
+#include "consts.h"
+
+ARM_FUNC asm void CP_SaveContext(register CPContext *context)
+{
+    ldr r1, =REG_DIV_NUMER_ADDR
+    stmfd sp!, {r4}
+    ldmia r1, {r2-r4, r12}
+    stmia r0!, {r2-r4, r12}
+    ldrh r12, [r1, #-16]
+
+    add r1, r1, #40
+    ldmia r1, {r2-r3}
+    stmia r0!, {r2-r3}
+
+    and r12, r12, #3
+    ldrh r2, [r1, #-8]
+    strh r12, [r0]
+    and r2, r2, #1
+    strh r2, [r0, #2]
+    ldmfd sp!, {r4}
+
+    bx lr
+}
+
+ARM_FUNC asm void CPi_RestoreContext(register const CPContext *context)
+{
+    stmfd sp!, {r4}
+    ldr r1, =REG_DIV_NUMER_ADDR
+    ldmia r0, {r2-r4, r12}
+    stmia r1, {r2-r4, r12}
+    ldrh r2, [r0, #24] //CPContext.div_mode
+    ldrh r3, [r0, #26] //CPContext.sqrt_mode
+
+    strh r2, [r1, #-16]
+    strh r3, [r1, #32]
+
+    add r0, r0, #16 //CPContext.sqrt
+    add r1, r1, #40
+
+    ldmia r0, {r2-r3}
+    stmia r1, {r2-r3}
+
+    ldmfd sp!, {r4}
+
+    bx lr
+}