Merge pull request #285 from red031000/master

arm9 CP_context

4 files changed, 53 insertions, 49 deletions

diff --git a/arm9/asm/CP_context.s b/arm9/asm/CP_context.s
deleted file mode 100644
index 6ab2c53f..00000000
--- a/arm9/asm/CP_context.s
+++ /dev/null
@@ -1,42 +0,0 @@
-	.include "asm/macros.inc"
-	.include "global.inc"
-	.section .text
-
-	arm_func_start CP_SaveContext
-CP_SaveContext: ; 0x020D3648
-	ldr r1, _020D3684 ; =0x04000290
-	stmdb sp!, {r4}
-	ldmia r1, {r2-r4,r12}
-	stmia r0!, {r2-r4,r12}
-	ldrh r12, [r1, #-0x10]
-	add r1, r1, #0x28
-	ldmia r1, {r2-r3}
-	stmia r0!, {r2-r3}
-	and r12, r12, #0x3
-	ldrh r2, [r1, #-0x8]
-	strh r12, [r0, #0x0]
-	and r2, r2, #0x1
-	strh r2, [r0, #0x2]
-	ldmia sp!, {r4}
-	bx lr
-	.balign 4
-_020D3684: .word 0x04000290
-
-	arm_func_start CPi_RestoreContext
-CPi_RestoreContext: ; 0x020D3688
-	stmdb sp!, {r4}
-	ldr r1, _020D36C0 ; =0x04000290
-	ldmia r0, {r2-r4,r12}
-	stmia r1, {r2-r4,r12}
-	ldrh r2, [r0, #0x18]
-	ldrh r3, [r0, #0x1a]
-	strh r2, [r1, #-0x10]
-	strh r3, [r1, #0x20]
-	add r0, r0, #0x10
-	add r1, r1, #0x28
-	ldmia r0, {r2-r3}
-	stmia r1, {r2-r3}
-	ldmia sp!, {r4}
-	bx lr
-	.balign 4
-_020D36C0: .word 0x04000290
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
+}