4 files changed, 128 insertions, 0 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_irqHandler.h b/arm9/lib/include/OS_irqHandler.h
index 17bde1c9..f052016d 100644
--- a/arm9/lib/include/OS_irqHandler.h
+++ b/arm9/lib/include/OS_irqHandler.h
@@ -14,6 +14,8 @@ 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_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/src/OS_irqHandler.c b/arm9/lib/src/OS_irqHandler.c
index 31f4453e..b1211b98 100644
--- a/arm9/lib/src/OS_irqHandler.c
+++ b/arm9/lib/src/OS_irqHandler.c
@@ -3,11 +3,132 @@
 #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();