9 files changed, 157 insertions, 2 deletions
diff --git a/arm7/lib/include/OS_context.h b/arm7/lib/include/OS_context.h
new file mode 100644
index 00000000..7819e29c
--- /dev/null
+++ b/arm7/lib/include/OS_context.h
@@ -0,0 +1,16 @@
+#ifndef POKEDIAMOND_ARM7_OS_CONTEXT_H
+#define POKEDIAMOND_ARM7_OS_CONTEXT_H
+
+#include "nitro/types.h"
+
+typedef struct OSContext
+{
+    u32 cpsr;
+    u32 r[13];
+    u32 sp;
+    u32 lr;
+    u32 pc_plus4;
+    u32 sp_svc;
+} OSContext;
+
+#endif //POKEDIAMOND_ARM7_OS_CONTEXT_H
diff --git a/arm7/lib/include/OS_init.h b/arm7/lib/include/OS_init.h
index c4e1ca37..03d64ae1 100644
--- a/arm7/lib/include/OS_init.h
+++ b/arm7/lib/include/OS_init.h
@@ -7,6 +7,9 @@
 #include "OS_alloc.h"
 #include "OS_reset.h"
 #include "OS_terminate_proc.h"
+#include "OS_spinLock.h"
+#include "OS_context.h"
+#include "OS_interrupt.h"
 
 void OS_Init(void);
 
diff --git a/arm7/lib/include/OS_interrupt.h b/arm7/lib/include/OS_interrupt.h
index afcb9b5f..fb869483 100644
--- a/arm7/lib/include/OS_interrupt.h
+++ b/arm7/lib/include/OS_interrupt.h
@@ -2,5 +2,22 @@
 #define POKEDIAMOND_ARM7_OS_INTERRUPT_H
 
 #include "nitro/OS_interrupt_shared.h"
+#include "consts.h"
+
+extern OSIrqFunction OS_IRQTable[];
+extern OSIrqCallbackInfo OSi_IrqCallbackInfo[9];
+
+static inline void OSi_SetVBlankCount(u32 count)
+{
+    *(u32 *)HW_VBLANK_COUNT_BUF = count;
+}
+
+void OS_InitIrqTable(void);
+void OS_SetIrqFunction(OSIrqMask intrBit, OSIrqFunction function);
+void OSi_EnterTimerCallback(u32 timerNo, void (*callback) (void *), void *arg);
+OSIrqMask OS_SetIrqMask(OSIrqMask mask);
+OSIrqMask OS_EnableIrqMask(OSIrqMask mask);
+OSIrqMask OS_DisableIrqMask(OSIrqMask mask);
+OSIrqMask OS_ResetRequestIrqMask(OSIrqMask mask);
 
 #endif //POKEDIAMOND_ARM7_OS_INTERRUPT_H
diff --git a/arm7/lib/include/OS_thread.h b/arm7/lib/include/OS_thread.h
new file mode 100644
index 00000000..04e4fdcb
--- /dev/null
+++ b/arm7/lib/include/OS_thread.h
@@ -0,0 +1,7 @@
+#ifndef POKEDIAMOND_ARM7_OS_THREAD_H
+#define POKEDIAMOND_ARM7_OS_THREAD_H
+
+#include "OS_context.h"
+#include "nitro/OS_thread_shared.h"
+
+#endif //POKEDIAMOND_ARM7_OS_THREAD_H
diff --git a/arm7/lib/include/consts.h b/arm7/lib/include/consts.h
index dd297402..966b5a4b 100644
--- a/arm7/lib/include/consts.h
+++ b/arm7/lib/include/consts.h
@@ -3,5 +3,6 @@
 
 #include "nitro/consts_shared.h"
 #include "mmap.h"
+#include "registers.h"
 
 #endif //POKEDIAMOND_ARM7_CONSTS_H
diff --git a/arm7/lib/include/mmap.h b/arm7/lib/include/mmap.h
index 064938e2..1b403dc5 100644
--- a/arm7/lib/include/mmap.h
+++ b/arm7/lib/include/mmap.h
@@ -23,6 +23,7 @@
 
 #define HW_PRV_WRAM_SYSRV           (HW_PRV_WRAM + HW_PRV_WRAM_SIZE - HW_PRV_WRAM_SYSRV_SIZE)
 
+#define HW_VBLANK_COUNT_BUF         (HW_MAIN_MEM + 0x007ffc3c)
 #define HW_LOCK_ID_FLAG_SUB         (HW_MAIN_MEM + 0x007fffb8)
 
 #endif //POKEDIAMOND_ARM7_MMAP_H
diff --git a/arm7/lib/include/registers.h b/arm7/lib/include/registers.h
new file mode 100644
index 00000000..4ef8bdfb
--- /dev/null
+++ b/arm7/lib/include/registers.h
@@ -0,0 +1,6 @@
+#ifndef POKEDIAMOND_ARM7_REGISTERS_H
+#define POKEDIAMOND_ARM7_REGISTERS_H
+
+#include "nitro/registers_shared.h"
+
+#endif //POKEDIAMOND_ARM7_REGISTERS_H
diff --git a/arm7/lib/src/OS_init.c b/arm7/lib/src/OS_init.c
index 583bb75e..271d067b 100644
--- a/arm7/lib/src/OS_init.c
+++ b/arm7/lib/src/OS_init.c
@@ -2,8 +2,6 @@
 #include "OS_init.h"
 
 extern void PXI_Init(void);
-extern void OS_InitLock(void);
-extern void OS_InitIrqTable(void);
 extern void OS_InitTick(void);
 extern void OS_InitAlarm(void);
 extern void OS_InitThread(void);
diff --git a/arm7/lib/src/OS_interrupt.c b/arm7/lib/src/OS_interrupt.c
new file mode 100644
index 00000000..e9fc3347
--- /dev/null
+++ b/arm7/lib/src/OS_interrupt.c
@@ -0,0 +1,106 @@
+#include "function_target.h"
+#include "consts.h"
+#include "OS_interrupt.h"
+#include "OS_thread.h"
+
+extern OSThreadQueue OSi_IrqThreadQueue;
+
+ARM_FUNC void OS_InitIrqTable(void)
+{
+    OS_InitThreadQueue(&OSi_IrqThreadQueue);
+
+    OSi_SetVBlankCount(0);
+}
+
+ARM_FUNC void OS_SetIrqFunction(OSIrqMask intrBit, OSIrqFunction function)
+{
+    s32 i;
+    OSIrqCallbackInfo *info;
+
+    for (i = 0; i < 25; i++)
+    {
+        if (intrBit & 1)
+        {
+            info = NULL;
+
+            if (8 <= i && i <= 11)
+            {
+                info = &OSi_IrqCallbackInfo[i - 8];
+            }
+            else if (3 <= i && i <= 6)
+            {
+                info = &OSi_IrqCallbackInfo[i - 3 + 4];
+            }
+            else if (0 == i)
+            {
+                info = &OSi_IrqCallbackInfo[8];
+            }
+            else
+            {
+                OS_IRQTable[i] = function;
+            }
+
+            if (info)
+            {
+                info->func = (void (*)(void *))function;
+                info->arg = 0;
+                info->enable = TRUE;
+            }
+        }
+        intrBit >>= 1;
+    }
+}
+
+ARM_FUNC void OSi_EnterTimerCallback(u32 timerNo, void (*callback) (void *), void *arg)
+{
+    OSIrqMask mask = 1UL << (timerNo + 3);
+    OSi_IrqCallbackInfo[timerNo + 4].func = callback;
+    OSi_IrqCallbackInfo[timerNo + 4].arg = arg;
+
+    (void)OS_EnableIrqMask(mask);
+    OSi_IrqCallbackInfo[timerNo + 4].enable = TRUE;
+}
+
+ARM_FUNC OSIrqMask OS_SetIrqMask(OSIrqMask mask)
+{
+    u16 regIme = reg_OS_IME;
+    reg_OS_IME = 0;
+    OSIrqMask regIe = reg_OS_IE;
+    reg_OS_IE = mask;
+    u16 unused = reg_OS_IME; //needed because otherwise it doesn't match
+    reg_OS_IME = regIme;
+    return regIe;
+}
+
+ARM_FUNC OSIrqMask OS_EnableIrqMask(OSIrqMask mask)
+{
+    u16 regIme = reg_OS_IME;
+    reg_OS_IME = 0;
+    OSIrqMask regIe = reg_OS_IE;
+    reg_OS_IE = regIe | mask;
+    u16 unused = reg_OS_IME;
+    reg_OS_IME = regIme;
+    return regIe;
+}
+
+ARM_FUNC OSIrqMask OS_DisableIrqMask(OSIrqMask mask)
+{
+    u16 regIme = reg_OS_IME;
+    reg_OS_IME = 0;
+    OSIrqMask regIe = reg_OS_IE;
+    reg_OS_IE = regIe & ~mask;
+    u16 unused = reg_OS_IME;
+    reg_OS_IME = regIme;
+    return regIe;
+}
+
+ARM_FUNC OSIrqMask OS_ResetRequestIrqMask(OSIrqMask mask)
+{
+    u16 regIme = reg_OS_IME;
+    reg_OS_IME = 0;
+    OSIrqMask regIf = reg_OS_IF;
+    reg_OS_IF = mask;
+    u16 unused = reg_OS_IME;
+    reg_OS_IME = regIme;
+    return regIf;
+}