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

9 files changed, 176 insertions, 5 deletions

diff --git a/arm9/lib/include/CP_context.h b/arm9/lib/include/CP_context.h
index d1e0062c..cfed9735 100644
--- a/arm9/lib/include/CP_context.h
+++ b/arm9/lib/include/CP_context.h
@@ -15,4 +15,7 @@ typedef struct CPContext {
     u16 sqrt_mode;
 } CPContext;
 
+void CP_SaveContext(CPContext *context);
+void CPi_RestoreContext(const CPContext* context);
+
 #endif //POKEDIAMOND_CP_CONTEXT_H
diff --git a/arm9/lib/include/OS_init.h b/arm9/lib/include/OS_init.h
index 40685513..88c696cd 100644
--- a/arm9/lib/include/OS_init.h
+++ b/arm9/lib/include/OS_init.h
@@ -22,6 +22,7 @@
 #include "OS_alloc.h"
 #include "OS_system.h"
 #include "OS_terminate_proc.h"
+#include "OS_irqHandler.h"
 #include "OS_interrupt.h"
 #include "OS_reset.h"
 #include "OS_spinLock.h"
diff --git a/arm9/lib/include/OS_irqHandler.h b/arm9/lib/include/OS_irqHandler.h
new file mode 100644
index 00000000..f052016d
--- /dev/null
+++ b/arm9/lib/include/OS_irqHandler.h
@@ -0,0 +1,21 @@
+#ifndef POKEDIAMOND_OS_IRQHANDLER_H
+#define POKEDIAMOND_OS_IRQHANDLER_H
+
+#include "consts.h"
+#include "OS_interrupt.h"
+
+static inline void OS_ClearIrqCheckFlag(OSIrqMask intr)
+{
+    *(vu32 *)HW_INTR_CHECK_BUF &= (u32)~intr;
+}
+
+static inline OSIrqMask OS_GetIrqCheckFlag(void)
+{
+    return *(OSIrqMask *)HW_INTR_CHECK_BUF;
+}
+
+void OS_IrqHandler(void);
+void OS_IrqHandler_ThreadSwitch(void);
+void OS_WaitIrq(BOOL param1, u32 param2);
+
+#endif //POKEDIAMOND_OS_IRQHANDLER_H
diff --git a/arm9/lib/include/OS_system.h b/arm9/lib/include/OS_system.h
index f71c6f98..6e156f9d 100644
--- a/arm9/lib/include/OS_system.h
+++ b/arm9/lib/include/OS_system.h
@@ -35,6 +35,5 @@ OSIntrMode OS_GetCpsrIrq(void);
 OSProcMode OS_GetProcMode(void);
 void OS_SpinWait(void);
 void OS_WaitVBlankIntr(void);
-void OS_WaitIrq(BOOL, u32);
 
 #endif //POKEDIAMOND_OS_SYSTEM_H
diff --git a/arm9/lib/include/OS_thread.h b/arm9/lib/include/OS_thread.h
index adcadf2e..bbdb33c9 100644
--- a/arm9/lib/include/OS_thread.h
+++ b/arm9/lib/include/OS_thread.h
@@ -87,6 +87,8 @@ struct _OSThread
     u32 systemErrno;
 };
 
+extern OSThreadInfo OSi_ThreadInfo;
+
 void OS_SleepThread(OSThreadQueue * queue);
 void OS_WakeupThread(OSThreadQueue * queue);
 
diff --git a/arm9/lib/include/mmap.h b/arm9/lib/include/mmap.h
index 65b93e1e..8eb3f623 100644
--- a/arm9/lib/include/mmap.h
+++ b/arm9/lib/include/mmap.h
@@ -22,6 +22,9 @@ extern u32 SDK_AUTOLOAD_DTCM_START[];
 #define HW_DTCM                 ((u32)SDK_AUTOLOAD_DTCM_START)
 #define HW_DTCM_SIZE            0x00004000
 
+#define HW_DTCM_SYSRV           (HW_DTCM + 0x00003fc0)
+#define HW_INTR_CHECK_BUF       (HW_DTCM_SYSRV + 0x38)
+
 #define HW_CARD_ROM_HEADER_SIZE 0x00000160
 #define HW_DOWNLOAD_PARAMETER_SIZE      0x00000020
 
diff --git a/arm9/lib/src/OS_interrupt.c b/arm9/lib/src/OS_interrupt.c
index 4692fd54..c7c0704d 100644
--- a/arm9/lib/src/OS_interrupt.c
+++ b/arm9/lib/src/OS_interrupt.c
@@ -1,7 +1,3 @@
-//
-// Created by red031000 on 2020-05-07.
-//
-
 #include "consts.h"
 #include "function_target.h"
 #include "OS_interrupt.h"
diff --git a/arm9/lib/src/OS_irqHandler.c b/arm9/lib/src/OS_irqHandler.c
new file mode 100644
index 00000000..b1211b98
--- /dev/null
+++ b/arm9/lib/src/OS_irqHandler.c
@@ -0,0 +1,145 @@
+#include "function_target.h"
+#include "OS_irqHandler.h"
+#include "OS_system.h"
+#include "OS_thread.h"
+#include "sections.h"
+#include "CP_context.h"
+
+#pragma section DTCM begin
+OSThreadQueue OSi_IrqThreadQueue = { NULL, NULL };
+#pragma section DTCM end
+
+#pragma section ITCM begin
+ARM_FUNC asm void OS_IrqHandler(void)
+{
+    stmfd sp!, {lr}
+    mov ip, #0x04000000
+    add ip, ip, #0x210
+    ldr r1, [ip, #-8]
+    cmp r1, #0
+    ldmeqfd sp!, {pc}
+    ldmia ip, {r1, r2}
+    ands r1, r1, r2
+    ldmeqfd sp!, {pc}
+    mov r3, #0x80000000
+_01FF8028:
+    clz r0, r1
+    bics r1, r1, r3, lsr r0
+    bne _01FF8028
+    mov r1, r3, lsr r0
+    str r1, [ip, #0x4]
+    rsbs r0, r0, #0x1f
+    ldr r1, =OS_IRQTable
+    ldr r0, [r1, r0, lsl #2]
+    ldr lr, =OS_IrqHandler_ThreadSwitch
+    bx r0
+}
+
+ARM_FUNC asm void OS_IrqHandler_ThreadSwitch(void)
+{
+    ldr ip, =OSi_IrqThreadQueue
+    mov r3, #0x0
+    ldr ip, [ip]
+    mov r2, #0x1
+    cmp ip, #0x0
+    beq _01FF80A8
+_01FF8070:
+    str r2, [ip, #0x64]
+    str r3, [ip, #0x78]
+    str r3, [ip, #0x7c]
+    ldr r0, [ip, #0x80]
+    str r3, [ip, #0x80]
+    mov ip, r0
+    cmp ip, #0x0
+bne _01FF8070
+    ldr ip, =OSi_IrqThreadQueue
+    str r3, [ip]
+    str r3, [ip, #0x4]
+    ldr ip, =OSi_ThreadInfo
+    mov r1, #0x1
+    strh r1, [ip]
+_01FF80A8:
+    ldr ip, =OSi_ThreadInfo
+    ldrh r1, [ip]
+    cmp r1, #0x0
+    ldreq pc, [sp], #0x4
+    mov r1, #0x0
+    strh r1, [ip]
+    mov r3, #0xd2
+    msr CPSR_c, r3
+    add r2, ip, #0x8
+    ldr r1, [r2]
+_01FF80D0:
+    cmp r1, #0x0
+    ldrneh r0, [r1, #0x64]
+    cmpne r0, #0x1
+    ldrne r1, [r1, #0x68]
+    bne _01FF80D0
+    cmp r1, #0x0
+    bne _01FF80F8
+_01FF80EC:
+    mov r3, #0x92
+    msr CPSR_c, r3
+    ldr pc, [sp], #0x4
+_01FF80F8:
+    ldr r0, [ip, #0x4]
+    cmp r1, r0
+    beq _01FF80EC
+    ldr r3, [ip, #0xC]
+    cmp r3, #0x0
+    beq _01FF8120
+    stmdb sp!, {r0, r1, ip}
+    mov lr, pc
+    bx r3
+    ldmia sp!, {r0, r1, ip}
+_01FF8120:
+    str r1, [ip, #0x4]
+    mrs r2, SPSR
+    str r2, [r0, #0x0]!
+    stmdb sp!, {r0, r1}
+    add r0, r0, #0x0
+    add r0, r0, #0x48
+    ldr r1, =CP_SaveContext
+    blx r1
+    ldmia sp!, {r0, r1}
+    ldmib sp!, {r2, r3}
+    stmib r0!, {r2, r3}
+    ldmib sp!, {r2, r3, ip, lr}
+    stmib r0!, {r2-r14}^
+    stmib r0!, {lr}
+    mov r3, #0xd3
+    msr CPSR_c, r3
+    stmib r0!, {sp}
+    stmfd sp!, {r1}
+    add r0, r1, #0x0
+    add r0, r0, #0x48
+    ldr r1, =CPi_RestoreContext
+    blx r1
+    ldmfd sp!, {r1}
+    ldr sp, [r1, #0x44]
+    mov r3, #0xd2
+    msr CPSR_c, r3
+    ldr r2, [r1, #0x0]!
+    msr SPSR_fc, r2
+    ldr lr, [r1, #0x40]
+    ldmib r1!, {r0-r14}^
+    nop
+    stmda sp!, {r0, r1, r2, r3, ip, lr}
+    ldmfd sp!, {pc}
+}
+#pragma section ITCM end
+
+ARM_FUNC void OS_WaitIrq(BOOL clear, OSIrqMask irqFlags)
+{
+    OSIntrMode lastIntrMode = OS_DisableInterrupts();
+    if (clear)
+    {
+        OS_ClearIrqCheckFlag(irqFlags);
+    }
+    (void)OS_RestoreInterrupts(lastIntrMode);
+
+    while (!(OS_GetIrqCheckFlag() & irqFlags))
+    {
+        OS_SleepThread(&OSi_IrqThreadQueue);
+    }
+}
diff --git a/arm9/lib/src/OS_system.c b/arm9/lib/src/OS_system.c
index 1d257679..b703fc38 100644
--- a/arm9/lib/src/OS_system.c
+++ b/arm9/lib/src/OS_system.c
@@ -3,6 +3,7 @@
 //
 
 #include "OS_system.h"
+#include "OS_irqHandler.h"
 #include "syscall.h"
 
 ARM_FUNC asm OSIntrMode OS_EnableInterrupts(void) {