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

3 files changed, 146 insertions, 4 deletions

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) {