Merge branch 'master' of github.com:martmists/pokediamond into pikalax_work

15 files changed, 269 insertions, 189 deletions

diff --git a/arm9/asm/OS_reset.s b/arm9/asm/OS_reset.s
deleted file mode 100644
index bdcc0f88..00000000
--- a/arm9/asm/OS_reset.s
+++ /dev/null
@@ -1,110 +0,0 @@
-    .include "asm/macros.inc"
-    .include "global.inc"
-
-    .text
-
-	arm_func_start OS_ResetSystem
-OS_ResetSystem: ; 0x020CD3B4
-	stmdb sp!, {r4,lr}
-	ldr r1, _020CD42C ; =0x027FFC40
-	mov r4, r0
-	ldrh r0, [r1, #0x0]
-	cmp r0, #0x2
-	bne _020CD3D0
-	bl OS_Terminate
-_020CD3D0:
-	bl OS_GetLockID
-	mov r0, r0, lsl #0x10
-	mov r0, r0, lsr #0x10
-	bl CARD_LockRom
-_020CD3E0:
-	mov r0, #0x0
-	bl MI_StopDma
-	mov r0, #0x1
-	bl MI_StopDma
-	mov r0, #0x2
-	bl MI_StopDma
-	mov r0, #0x3
-	bl MI_StopDma
-	mov r0, #0x40000
-	bl OS_SetIrqMask
-	mvn r0, #0x0
-	bl OS_ResetRequestIrqMask
-	ldr r1, _020CD430
-	mov r0, #0x10
-	str r4, [r1]
-	bl OSi_SendToPxi
-	bl OSi_DoResetSystem
-	ldmia sp!, {r4, lr}
-	bx lr
-_020CD42C: .word 0x027FFC40
-_020CD430: .word 0x027FFC20
-
-	arm_func_start OSi_SendToPxi
-OSi_SendToPxi: ; 0x020CD434
-	stmdb sp!, {r4-r6,lr}
-	mov r6, r0, lsl #0x8
-	mov r5, #0xc
-	mov r4, #0x0
-_020CD444:
-	mov r0, r5
-	mov r1, r6
-	mov r2, r4
-	bl PXI_SendWordByFifo
-_020CD454:
-	cmp r0, #0x0
-	bne _020CD444
-	ldmia sp!, {r4-r6, lr}
-	bx lr
-
-	arm_func_start OSi_CommonCallback
-OSi_CommonCallback: ; OSi_CommonCallback
-	stmdb sp!, {lr}
-	sub sp, sp, #0x4
-	and r0, r1, #0x7f00
-	mov r0, r0, lsl #0x8
-	mov r0, r0, lsr #0x10
-	cmp r0, #0x10
-	ldreq r0, _020CD4A4 ; =OSi_IsResetOccurred
-	moveq r1, #0x1
-	streqh r1, [r0, #0x0]
-	addeq sp, sp, #0x4
-	ldmeqia sp!, {lr}
-	bxeq lr
-	bl OS_Terminate
-	add sp, sp, #0x4
-	ldmia sp!, {lr}
-	bx lr
-	.balign 4
-_020CD4A4: .word OSi_IsResetOccurred
-
-	arm_func_start OS_InitReset
-OS_InitReset: ; 0x020CD4A8
-	stmdb sp!, {r4-r5,lr}
-	sub sp, sp, #0x4
-	ldr r0, _020CD508 ; =0x021D37DC
-	ldrh r1, [r0, #0x0]
-	cmp r1, #0x0
-	addne sp, sp, #0x4
-	ldmneia sp!, {r4-r5,lr}
-	bxne lr
-	mov r1, #0x1
-	strh r1, [r0, #0x0]
-	bl PXI_Init
-	mov r5, #0xc
-	mov r4, #0x1
-_020CD4DC:
-	mov r0, r5
-	mov r1, r4
-	bl PXI_IsCallbackReady
-_020CD4E8:
-	cmp r0, #0x0
-	beq _020CD4DC
-	ldr r1, _020CD50C
-	mov r0, #0xC
-	bl PXI_SetFifoRecvCallback
-	add sp, sp, #0x4
-	ldmia sp!, {r4-r5, lr}
-	bx lr
-_020CD508: .word OSi_IsInitReset
-_020CD50C: .word OSi_CommonCallback
diff --git a/arm9/lib/include/MB_mb.h b/arm9/lib/include/MB_mb.h
new file mode 100644
index 00000000..128ea480
--- /dev/null
+++ b/arm9/lib/include/MB_mb.h
@@ -0,0 +1,45 @@
+//
+// Created by red031000 on 2020-05-06.
+//
+
+#ifndef POKEDIAMOND_MB_MB_H
+#define POKEDIAMOND_MB_MB_H
+
+#include "consts.h"
+
+#define MB_TYPE_ILLEGAL 0
+#define MB_TYPE_NORMAL 1
+#define MB_TYPE_MULTIBOOT 2
+
+typedef struct {
+    u16 length;
+    u16 rssi;
+    u16 bssid[3];
+    u16 ssidLength;
+    u8 ssid[32];
+    u16 capaInfo;
+    struct {
+        u16 basic;
+        u16 support;
+    } rateSet;
+    u16 beaconperiod;
+    u16 dtimPeriod;
+    u16 channel;
+    u16 cfpPeriod;
+    u16 cfpMaxDuration;
+} MBParentBssDesc;
+
+typedef struct {
+    u16 boot_type;
+    MBParentBssDesc parent_bss_desc;
+} MBParam;
+
+static inline const MBParam *MB_GetMultiBootParam() {
+    return (const MBParam *)HW_WM_BOOT_BUF;
+}
+
+static inline BOOL MB_IsMultiBootChild() {
+    return MB_GetMultiBootParam()->boot_type == MB_TYPE_MULTIBOOT;
+}
+
+#endif //POKEDIAMOND_MB_MB_H
diff --git a/arm9/lib/include/OS_init.h b/arm9/lib/include/OS_init.h
index 6fbfc826..0c74035d 100644
--- a/arm9/lib/include/OS_init.h
+++ b/arm9/lib/include/OS_init.h
@@ -16,6 +16,7 @@
 #include "OS_arena.h"
 #include "OS_alloc.h"
 #include "OS_system.h"
+#include "OS_reset.h"
 
 void OS_Init();
 
diff --git a/arm9/lib/include/OS_reset.h b/arm9/lib/include/OS_reset.h
new file mode 100644
index 00000000..c3b60c98
--- /dev/null
+++ b/arm9/lib/include/OS_reset.h
@@ -0,0 +1,20 @@
+//
+// Created by red031000 on 2020-05-06.
+//
+
+#ifndef POKEDIAMOND_OS_RESET_H
+#define POKEDIAMOND_OS_RESET_H
+
+#include "consts.h"
+#include "PXI_fifo.h"
+
+#define OS_PXI_COMMAND_MASK 0x7f00
+#define OS_PXI_COMMAND_SHIFT 8
+#define OS_PXI_COMMAND_RESET 0x10
+
+void OS_InitReset();
+void OSi_CommonCallback(PXIFifoTag tag, u32 data, BOOL err);
+void OSi_SendToPxi(u16 data);
+void OS_ResetSystem(u32 parameter);
+
+#endif //POKEDIAMOND_OS_RESET_H
diff --git a/arm9/lib/include/OS_system.h b/arm9/lib/include/OS_system.h
index 93903315..9a79c0d1 100644
--- a/arm9/lib/include/OS_system.h
+++ b/arm9/lib/include/OS_system.h
@@ -6,6 +6,7 @@
 #define POKEDIAMOND_OS_SYSTEM_H
 
 #include "consts.h"
+#include "function_target.h"
 
 typedef enum {
     OS_PROCMODE_USER=16,
diff --git a/arm9/lib/include/PXI_fifo.h b/arm9/lib/include/PXI_fifo.h
new file mode 100644
index 00000000..1d45dda2
--- /dev/null
+++ b/arm9/lib/include/PXI_fifo.h
@@ -0,0 +1,33 @@
+//
+// Created by red031000 on 2020-05-06.
+//
+
+#ifndef POKEDIAMOND_PXI_FIFO_H
+#define POKEDIAMOND_PXI_FIFO_H
+
+#include "function_target.h"
+
+ENUMS_ALWAYS_INT_ON
+typedef enum {
+    PXI_FIFO_TAG_EX = 0,               // Extension format
+    PXI_FIFO_TAG_USER_0,               // for application programmer, use it in free
+    PXI_FIFO_TAG_USER_1,               // for application programmer, use it in free
+    PXI_FIFO_TAG_SYSTEM,               // SDK inner usage
+    PXI_FIFO_TAG_NVRAM,                // NVRAM
+    PXI_FIFO_TAG_RTC,                  // RTC
+    PXI_FIFO_TAG_TOUCHPANEL,           // Touch Panel
+    PXI_FIFO_TAG_SOUND,                // Sound
+    PXI_FIFO_TAG_PM,                   // Power Management
+    PXI_FIFO_TAG_MIC,                  // Microphone
+    PXI_FIFO_TAG_WM,                   // Wireless Manager
+    PXI_FIFO_TAG_FS,                   // File System
+    PXI_FIFO_TAG_OS,                   // OS
+    PXI_FIFO_TAG_CTRDG,                // Cartridge
+    PXI_FIFO_TAG_CARD,                 // Card
+    PXI_FIFO_TAG_WVR,                  // Control driving wireless library
+    PXI_FIFO_TAG_CTRDG_Ex,             // Cartridge Ex
+    PXI_MAX_FIFO_TAG = 32              // MAX FIFO TAG
+} PXIFifoTag;
+ENUMS_ALWAYS_INT_RESET
+
+#endif //POKEDIAMOND_PXI_FIFO_H
diff --git a/arm9/lib/include/consts.h b/arm9/lib/include/consts.h
index 0e5066de..b99b4148 100644
--- a/arm9/lib/include/consts.h
+++ b/arm9/lib/include/consts.h
@@ -6,6 +6,7 @@
 #define POKEDIAMOND_CONSTS_H
 
 #include "mmap.h"
+#include "registers.h"
 
 #define HW_PSR_CPU_MODE_MASK       0x1f // CPU mode
 
@@ -13,19 +14,6 @@
 #define HW_PSR_DISABLE_IRQ         0x80 // Disable IRQ
 #define HW_PSR_DISABLE_IRQ_FIQ     0xc0 // Disable FIQ and IRQ
 
-#define HW_REG_BASE                0x04000000
-#define REG_VCOUNT_OFFSET          0x006
-#define REG_VCOUNT_ADDR            (HW_REG_BASE + REG_VCOUNT_OFFSET)
-#define reg_GX_VCOUNT              (*(REGType16v *)REG_VCOUNT_ADDR)
-
-#define REG_KEYINPUT_OFFSET        0x130
-#define REG_KEYINPUT_ADDR          (HW_REG_BASE + REG_KEYINPUT_OFFSET)
-#define reg_PAD_KEYINPUT           (*(REGType16v *)REG_KEYINPUT_ADDR)
-
-#define REG_GXSTAT_OFFSET          0x600
-#define REG_GXSTAT_ADDR            (HW_REG_BASE + REG_GXSTAT_OFFSET)
-#define reg_G3X_GXSTAT             (*(REGType32v *)REG_GXSTAT_ADDR)
-
 #define HW_C6_PR_4KB               0x16
 #define HW_C6_PR_8KB               0x18
 #define HW_C6_PR_16KB              0x1a
@@ -48,6 +36,8 @@
 #define HW_C6_PR_2GB               0x3c
 #define HW_C6_PR_4GB               0x3e
 
+#define PXI_PROC_ARM7              0x01
+
 #define OSi_CONSOLE_NOT_DETECT     0xffffffff
 
 #define OS_CONSOLE_NITRO           0x80000000
diff --git a/arm9/lib/include/fx.h b/arm9/lib/include/fx.h
index 7e74d079..b1c3aa88 100644
--- a/arm9/lib/include/fx.h
+++ b/arm9/lib/include/fx.h
@@ -46,24 +46,6 @@ typedef s64 fx64c;
 #define FX64C_INT_ABS(x)           FX_INT_ABS(FX64C, x)
 #define FX64C_FRAC(x)              FX_FRAC(FX64C, x)
 
-
-#define HW_REG_DIVCNT              0x04000280
-#define HW_REG_DIV_NUMER           0x04000290
-#define HW_REG_DIV_DENOM           0x04000298
-#define HW_REG_DIV_RESULT          0x040002A0
-#define HW_REG_DIVREM_RESULT       0x040002A8
-
-#define HW_REG_SQRTCNT             0x040002B0
-#define HW_REG_SQRT_RESULT         0x040002B4
-#define HW_REG_SQRT_PARAM          0x040002B8
-
-#define SETREG16(x, y)             ((*(vu16 *)x) = y)
-#define SETREG32(x, y)             ((*(vu32 *)x) = y)
-#define SETREG64(x, y)             ((*(vu64 *)x) = y)
-#define READREG16(x)               (*(vu16 *)x)
-#define READREG32(x)               (*(vu32 *)x)
-#define READREG64(x)               (*(vu64 *)x)
-
 #define FX32_MUL(a, b)               ((fx32)(((fx64)a * b) >> FX32_INT_SHIFT))
 #define FX32_MUL_ADD_MUL(a, b, c, d) ((fx32)(((fx64)a * b + (fx64)c * d) >> FX32_INT_SHIFT))
 //the extra term here is for rounding
diff --git a/arm9/lib/include/mmap.h b/arm9/lib/include/mmap.h
index d94df843..1f8f37f9 100644
--- a/arm9/lib/include/mmap.h
+++ b/arm9/lib/include/mmap.h
@@ -23,6 +23,8 @@ extern u32 SDK_AUTOLOAD_DTCM_START[];
 #define HW_CARD_ROM_HEADER_SIZE 0x00000160
 #define HW_DOWNLOAD_PARAMETER_SIZE      0x00000020
 
+#define HW_RESET_PARAMETER_BUF  (HW_MAIN_MEM + 0x007ffc20)
+#define HW_WM_BOOT_BUF          (HW_MAIN_MEM + 0x007ffc40)
 #define HW_ARENA_INFO_BUF       (HW_MAIN_MEM + 0x007ffda0)      // Arena data structure
 #define HW_ROM_HEADER_BUF       (HW_MAIN_MEM + 0x007ffe00)      // ROM registration area data buffer
 #define HW_RED_RESERVED         (HW_MAIN_MEM + 0x007ff800)      // Some kind of reserved data for shared memory
diff --git a/arm9/lib/include/registers.h b/arm9/lib/include/registers.h
new file mode 100644
index 00000000..45b36334
--- /dev/null
+++ b/arm9/lib/include/registers.h
@@ -0,0 +1,55 @@
+//
+// Created by red031000 on 2020-05-06.
+//
+
+#ifndef POKEDIAMOND_REGISTERS_H
+#define POKEDIAMOND_REGISTERS_H
+
+#include "types.h"
+
+#define HW_REG_BASE                0x04000000
+#define REG_VCOUNT_OFFSET          0x006
+#define REG_VCOUNT_ADDR            (HW_REG_BASE + REG_VCOUNT_OFFSET)
+#define reg_GX_VCOUNT              (*(REGType16v *)REG_VCOUNT_ADDR)
+
+#define REG_KEYINPUT_OFFSET        0x130
+#define REG_KEYINPUT_ADDR          (HW_REG_BASE + REG_KEYINPUT_OFFSET)
+#define reg_PAD_KEYINPUT           (*(REGType16v *)REG_KEYINPUT_ADDR)
+
+#define REG_DIVCNT_OFFSET          0x280
+#define REG_DIVCNT_ADDR            (HW_REG_BASE + REG_DIVCNT_OFFSET)
+#define reg_CP_DIVCNT              (*(REGType16v *)REG_DIVCNT_ADDR)
+
+#define REG_DIV_NUMER_OFFSET       0x290
+#define REG_DIV_NUMER_ADDR         (HW_REG_BASE + REG_DIV_NUMER_OFFSET)
+#define reg_CP_DIV_NUMER           (*(REGType64v *)REG_DIV_NUMER_ADDR)
+
+#define REG_DIV_DENOM_OFFSET       0x298
+#define REG_DIV_DENOM_ADDR         (HW_REG_BASE + REG_DIV_DENOM_OFFSET)
+#define reg_CP_DIV_DENOM           (*(REGType64v *)REG_DIV_DENOM_ADDR)
+
+#define REG_DIV_RESULT_OFFSET      0x2A0
+#define REG_DIV_RESULT_ADDR        (HW_REG_BASE + REG_DIV_RESULT_OFFSET)
+#define reg_CP_DIV_RESULT          (*(REGType64v *)REG_DIV_RESULT_ADDR)
+
+#define REG_DIVREM_RESULT_OFFSET   0x2A8
+#define REG_DIVREM_RESULT_ADDR     (HW_REG_BASE + REG_DIVREM_RESULT_OFFSET)
+#define reg_CP_DIVREM_RESULT       (*(REGType64v *)REG_DIVREM_RESULT_ADDR)
+
+#define REG_SQRTCNT_OFFSET         0x2B0
+#define REG_SQRTCNT_ADDR           (HW_REG_BASE + REG_SQRTCNT_OFFSET)
+#define reg_CP_SQRTCNT             (*(REGType16v *)REG_SQRTCNT_ADDR)
+
+#define REG_SQRT_RESULT_OFFSET     0x2B4
+#define REG_SQRT_RESULT_ADDR       (HW_REG_BASE + REG_SQRT_RESULT_OFFSET)
+#define reg_CP_SQRT_RESULT         (*(REGType32v *)REG_SQRT_RESULT_ADDR)
+
+#define REG_SQRT_PARAM_OFFSET      0x2B8
+#define REG_SQRT_PARAM_ADDR        (HW_REG_BASE + REG_SQRT_PARAM_OFFSET)
+#define reg_CP_SQRT_PARAM          (*(REGType64v *)REG_SQRT_PARAM_ADDR)
+
+#define REG_GXSTAT_OFFSET          0x600
+#define REG_GXSTAT_ADDR            (HW_REG_BASE + REG_GXSTAT_OFFSET)
+#define reg_G3X_GXSTAT             (*(REGType32v *)REG_GXSTAT_ADDR)
+
+#endif //POKEDIAMOND_REGISTERS_H
diff --git a/arm9/lib/src/FX_cp.c b/arm9/lib/src/FX_cp.c
index 2ca9d720..08443dc8 100644
--- a/arm9/lib/src/FX_cp.c
+++ b/arm9/lib/src/FX_cp.c
@@ -16,8 +16,8 @@ ARM_FUNC fx32 FX_Inv(fx32 x){
 ARM_FUNC fx32 FX_Sqrt(fx32 x){
     if (x > 0)
     {
-        SETREG16(HW_REG_SQRTCNT, 0x1);
-        SETREG64(HW_REG_SQRT_PARAM, (fx64)x << 32);
+        reg_CP_SQRTCNT = 0x1;
+        reg_CP_SQRT_PARAM = (fx64)x << 32;
         return FX_GetSqrtResult();
     }
     else
@@ -27,44 +27,44 @@ ARM_FUNC fx32 FX_Sqrt(fx32 x){
 }
 
 ARM_FUNC fx64c FX_GetDivResultFx64c(){
-    while (READREG16(HW_REG_DIVCNT) & 0x8000);
-    return READREG64(HW_REG_DIV_RESULT);
+    while (reg_CP_DIVCNT & 0x8000);
+    return reg_CP_DIV_RESULT;
 }
 
 ARM_FUNC fx32 FX_GetDivResult(){
-    while (READREG16(HW_REG_DIVCNT) & 0x8000);
-    return (READREG64(HW_REG_DIV_RESULT) + (1 << (0x14 - 1))) >> 0x14;
+    while (reg_CP_DIVCNT & 0x8000);
+    return (reg_CP_DIV_RESULT + (1 << (0x14 - 1))) >> 0x14;
 }
 
 ARM_FUNC void FX_InvAsync(fx32 x){
-    SETREG16(HW_REG_DIVCNT, 0x1);
-    SETREG64(HW_REG_DIV_NUMER, (fx64)0x00001000 << 32);
-    SETREG64(HW_REG_DIV_DENOM, (u32)x);
+    reg_CP_DIVCNT = 0x1;
+    reg_CP_DIV_NUMER = (fx64)0x00001000 << 32;
+    reg_CP_DIV_DENOM = (u32)x;
 }
 
 ARM_FUNC fx32 FX_GetSqrtResult(){
-    while (READREG16(HW_REG_SQRTCNT) & 0x8000);
-    return (READREG32(HW_REG_SQRT_RESULT) + (1 << (0xA - 1))) >> 0xA;
+    while (reg_CP_SQRTCNT & 0x8000);
+    return (reg_CP_SQRT_RESULT + (1 << (0xA - 1))) >> 0xA;
 }
 
 ARM_FUNC void FX_DivAsync(fx32 numerator, fx32 denominator){
-    SETREG16(HW_REG_DIVCNT, 0x1);
-    SETREG64(HW_REG_DIV_NUMER, (fx64)numerator << 32);
-    SETREG64(HW_REG_DIV_DENOM, (u32)denominator);
+    reg_CP_DIVCNT = 0x1;
+    reg_CP_DIV_NUMER = (fx64)numerator << 32;
+    reg_CP_DIV_DENOM = (u32)denominator;
 }
 
 ARM_FUNC fx32 FX_DivS32(fx32 numerator, fx32 denominator){
-    SETREG16(HW_REG_DIVCNT, 0x0);
-    SETREG32(HW_REG_DIV_NUMER, (u32)numerator); //32bit write for some reason
-    SETREG64(HW_REG_DIV_DENOM, (u32)denominator);
-    while (READREG16(HW_REG_DIVCNT) & 0x8000);
-    return READREG32(HW_REG_DIV_RESULT);
+    reg_CP_DIVCNT = 0x0;
+    *(REGType32 *)REG_DIV_NUMER_ADDR = (u32)numerator; //32bit write for some reason
+    reg_CP_DIV_DENOM = (u32)denominator;
+    while (reg_CP_DIVCNT & 0x8000);
+    return *(REGType32 *)REG_DIV_RESULT_ADDR;
 }
 
 ARM_FUNC fx32 FX_ModS32(fx32 num, fx32 mod){
-    SETREG16(HW_REG_DIVCNT, 0x0);
-    SETREG32(HW_REG_DIV_NUMER, (u32)num); //32bit write for some reason
-    SETREG64(HW_REG_DIV_DENOM, (u32)mod);
-    while (READREG16(HW_REG_DIVCNT) & 0x8000);
-    return READREG32(HW_REG_DIVREM_RESULT);
+    reg_CP_DIVCNT = 0x0;
+    *(REGType32 *)REG_DIV_NUMER_ADDR = (u32)num; //32bit write for some reason
+    reg_CP_DIV_DENOM = (u32)mod;
+    while (reg_CP_DIVCNT & 0x8000);
+    return *(REGType32 *)REG_DIVREM_RESULT_ADDR;
 }
diff --git a/arm9/lib/src/FX_vec.c b/arm9/lib/src/FX_vec.c
index af36fe89..95805f33 100644
--- a/arm9/lib/src/FX_vec.c
+++ b/arm9/lib/src/FX_vec.c
@@ -55,10 +55,10 @@ ARM_FUNC fx32 VEC_Mag(struct Vecx32 *a){
     fx64 l2 = (fx64)a->x * a->x;
     l2 += (fx64)a->y * a->y;
     l2 += (fx64)a->z * a->z;
-    SETREG16(HW_REG_SQRTCNT, 0x1);
-    SETREG64(HW_REG_SQRT_PARAM, l2 * 4);
-    while (READREG16(HW_REG_SQRTCNT) & 0x8000); //wait for coprocessor to finish
-    return ((fx32)READREG32(HW_REG_SQRT_RESULT) + 1) >> 1;
+    reg_CP_SQRTCNT = 0x1;
+    reg_CP_SQRT_PARAM = l2 * 4;
+    while (reg_CP_SQRTCNT & 0x8000); //wait for coprocessor to finish
+    return ((fx32)reg_CP_SQRT_RESULT + 1) >> 1;
 }
 
 ARM_FUNC void VEC_Normalize(struct Vecx32 *a, struct Vecx32 *dst){
@@ -66,15 +66,15 @@ ARM_FUNC void VEC_Normalize(struct Vecx32 *a, struct Vecx32 *dst){
     l2 += (fx64)a->y * a->y;
     l2 += (fx64)a->z * a->z;
     //1/sqrt(l) is computed by calculating sqrt(l)*(1/l)
-    SETREG16(HW_REG_DIVCNT, 0x2);
-    SETREG64(HW_REG_DIV_NUMER, 0x0100000000000000);
-    SETREG64(HW_REG_DIV_DENOM, l2);
-    SETREG16(HW_REG_SQRTCNT, 0x1);
-    SETREG64(HW_REG_SQRT_PARAM, l2 * 4);
-    while (READREG16(HW_REG_SQRTCNT) & 0x8000); //wait for sqrt to finish
-    fx32 sqrtresult = READREG32(HW_REG_SQRT_RESULT);
-    while (READREG16(HW_REG_DIVCNT) & 0x8000); //wait for division to finish
-    l2 = READREG64(HW_REG_DIV_RESULT);
+    reg_CP_DIVCNT = 0x2;
+    reg_CP_DIV_NUMER = 0x0100000000000000;
+    reg_CP_DIV_DENOM = l2;
+    reg_CP_SQRTCNT = 0x1;
+    reg_CP_SQRT_PARAM = l2 * 4;
+    while (reg_CP_SQRTCNT & 0x8000); //wait for sqrt to finish
+    fx32 sqrtresult = reg_CP_SQRT_RESULT;
+    while (reg_CP_DIVCNT & 0x8000); //wait for division to finish
+    l2 = reg_CP_DIV_RESULT;
     l2 = sqrtresult * l2;
     dst->x = (l2 * a->x + (1LL << (0x2D - 1))) >> 0x2D;
     dst->y = (l2 * a->y + (1LL << (0x2D - 1))) >> 0x2D;
@@ -86,15 +86,15 @@ ARM_FUNC void VEC_Fx16Normalize(struct Vecx16 *a, struct Vecx16 *dst){
     l2 += a->y * a->y;
     l2 += a->z * a->z;
     //1/sqrt(l) is computed by calculating sqrt(l)*(1/l)
-    SETREG16(HW_REG_DIVCNT, 0x2);
-    SETREG64(HW_REG_DIV_NUMER, 0x0100000000000000);
-    SETREG64(HW_REG_DIV_DENOM, l2);
-    SETREG16(HW_REG_SQRTCNT, 0x1);
-    SETREG64(HW_REG_SQRT_PARAM, l2 * 4);
-    while (READREG16(HW_REG_SQRTCNT) & 0x8000); //wait for sqrt to finish
-    fx32 sqrtresult = READREG32(HW_REG_SQRT_RESULT);
-    while (READREG16(HW_REG_DIVCNT) & 0x8000); //wait for division to finish
-    l2 = READREG64(HW_REG_DIV_RESULT);
+    reg_CP_DIVCNT = 0x2;
+    reg_CP_DIV_NUMER = 0x0100000000000000;
+    reg_CP_DIV_DENOM =  l2;
+    reg_CP_SQRTCNT = 0x1;
+    reg_CP_SQRT_PARAM = l2 * 4;
+    while (reg_CP_SQRTCNT & 0x8000); //wait for sqrt to finish
+    fx32 sqrtresult = reg_CP_SQRT_RESULT;
+    while (reg_CP_DIVCNT & 0x8000); //wait for division to finish
+    l2 = reg_CP_DIV_RESULT;
     l2 = sqrtresult * l2;
     dst->x = (l2 * a->x + (1LL << (0x2D - 1))) >> 0x2D;
     dst->y = (l2 * a->y + (1LL << (0x2D - 1))) >> 0x2D;
diff --git a/arm9/lib/src/OS_init.c b/arm9/lib/src/OS_init.c
index 90b01ffa..e2658421 100644
--- a/arm9/lib/src/OS_init.c
+++ b/arm9/lib/src/OS_init.c
@@ -14,7 +14,6 @@ extern void MI_Init();
 extern void OS_InitVAlarm();
 extern void OSi_InitVramExclusive();
 extern void OS_InitThread();
-extern void OS_InitReset();
 extern void CTRDG_Init();
 extern void CARD_Init();
 extern void PM_Init();
diff --git a/arm9/lib/src/OS_reset.c b/arm9/lib/src/OS_reset.c
new file mode 100644
index 00000000..b12703da
--- /dev/null
+++ b/arm9/lib/src/OS_reset.c
@@ -0,0 +1,63 @@
+//
+// Created by red031000 on 2020-05-06.
+//
+
+#include "function_target.h"
+#include "OS_reset.h"
+#include "MB_mb.h"
+
+extern u16 OSi_IsInitReset;
+extern vu16 OSi_IsResetOccurred;
+extern void PXI_Init();
+extern u32 PXI_IsCallbackReady(u32 param1, u32 param2);
+extern void PXI_SetFifoRecvCallback(u32 param1, void* callback);
+extern void OS_Terminate();
+extern u32 PXI_SendWordByFifo(u32 param1, u32 data, u32 param2);
+extern void CARD_LockRom(u16 lockId);
+extern u32 OS_GetLockID();
+extern void MI_StopDma(u32 dma);
+extern void OS_SetIrqMask(u32 mask);
+extern void OS_ResetRequestIrqMask(u32 mask);
+extern void OSi_DoResetSystem();
+
+ARM_FUNC void OS_InitReset() {
+    if (OSi_IsInitReset) {
+        return;
+    }
+    OSi_IsInitReset = TRUE;
+    PXI_Init();
+    while (!PXI_IsCallbackReady(PXI_FIFO_TAG_OS, PXI_PROC_ARM7)) { }
+
+    PXI_SetFifoRecvCallback(PXI_FIFO_TAG_OS, OSi_CommonCallback);
+}
+
+ARM_FUNC static void OSi_CommonCallback(PXIFifoTag tag, u32 data, BOOL err) {
+#pragma unused(tag, err) //needed because otherwise -W all errors
+    u16 command = (u16)((data & OS_PXI_COMMAND_MASK) >> OS_PXI_COMMAND_SHIFT);
+    if (command == OS_PXI_COMMAND_RESET)
+    {
+        OSi_IsResetOccurred = TRUE;
+        return;
+    }
+    OS_Terminate();
+}
+
+ARM_FUNC static void OSi_SendToPxi(u16 data) {
+    while (PXI_SendWordByFifo(PXI_FIFO_TAG_OS, (u32)data << 0x8, FALSE)) {}
+}
+
+ARM_FUNC void OS_ResetSystem(u32 parameter) {
+    if (MB_IsMultiBootChild()) {
+        OS_Terminate();
+    }
+    CARD_LockRom((u16)OS_GetLockID());
+    MI_StopDma(0);
+    MI_StopDma(1);
+    MI_StopDma(2);
+    MI_StopDma(3);
+    OS_SetIrqMask(0x40000);
+    OS_ResetRequestIrqMask((u32)~0);
+    *(u32 *)HW_RESET_PARAMETER_BUF = parameter;
+    OSi_SendToPxi(OS_PXI_COMMAND_RESET);
+    OSi_DoResetSystem(); //oh boy this is in itcm, that's gonna be fun to deal with Kappa
+}
diff --git a/arm9/lib/src/OS_system.c b/arm9/lib/src/OS_system.c
index c2b08681..e5dfcb47 100644
--- a/arm9/lib/src/OS_system.c
+++ b/arm9/lib/src/OS_system.c
@@ -2,7 +2,6 @@
 // Created by mart on 4/23/20.
 //
 
-#include "function_target.h"
 #include "OS_system.h"
 
 ARM_FUNC asm OSIntrMode OS_EnableInterrupts() {