3 files changed, 53 insertions, 7 deletions
diff --git a/arm9/lib/include/CP_context.h b/arm9/lib/include/CP_context.h
index 4a925257..9d392352 100644
--- a/arm9/lib/include/CP_context.h
+++ b/arm9/lib/include/CP_context.h
@@ -1,7 +1,3 @@
-//
-// Created by red031000 on 2020-05-17.
-//
-
 #ifndef POKEDIAMOND_CP_CONTEXT_H
 #define POKEDIAMOND_CP_CONTEXT_H
 
@@ -15,7 +11,7 @@ typedef struct CPContext {
     u16 sqrt_mode;
 } CPContext;
 
-void CP_SaveContext(CPContext *context);
+void CP_SaveContext(register CPContext *context);
 void CPi_RestoreContext(const CPContext* context);
 
 #endif //POKEDIAMOND_CP_CONTEXT_H
diff --git a/arm9/lib/include/registers.h b/arm9/lib/include/registers.h
index ae5b7adc..dc1b4fdb 100644
--- a/arm9/lib/include/registers.h
+++ b/arm9/lib/include/registers.h
@@ -112,8 +112,10 @@
 #define reg_GX_VRAMCNT_I           (*(REGType8v  *)0x4000249)
 
 #define reg_CP_DIVCNT              (*(REGType16v *)0x4000280)
-#define reg_CP_DIV_NUMER           (*(REGType64v *)0x4000290)
-#define reg_CP_DIV_NUMER_L         (*(REGType32v *)0x4000290)
+
+#define REG_DIV_NUMER_ADDR         0x4000290
+#define reg_CP_DIV_NUMER           (*(REGType64v *)REG_DIV_NUMER_ADDR)
+#define reg_CP_DIV_NUMER_L         (*(REGType32v *)REG_DIV_NUMER_ADDR)
 #define reg_CP_DIV_NUMER_H         (*(REGType32v *)0x4000294)
 #define reg_CP_DIV_DENOM           (*(REGType64v *)0x4000298)
 #define reg_CP_DIV_DENOM_L         (*(REGType32v *)0x4000298)
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
+}