arm9 OS_alarm

1 files changed, 250 insertions, 0 deletions

diff --git a/arm9/lib/src/OS_alarm.c b/arm9/lib/src/OS_alarm.c
new file mode 100644
index 00000000..1bd3ff2f
--- /dev/null
+++ b/arm9/lib/src/OS_alarm.c
@@ -0,0 +1,250 @@
+#include "function_target.h"
+#include "consts.h"
+#include "OS_alarm.h"
+#include "OS_interrupt.h"
+#include "OS_irqTable.h"
+#include "OS_terminate_proc.h"
+#include "OS_system.h"
+
+static struct OSiAlarmQueue OSi_AlarmQueue;
+
+static u16 OSi_UseAlarm = FALSE;
+
+ARM_FUNC static void OSi_SetTimer(OSAlarm *alarm)
+{
+    OSTick tick = OS_GetTick();
+
+    OS_SetTimerControl(OS_TIMER_1, 0);
+
+    s64 delta = (s64)(alarm->fire - tick);
+
+    OSi_EnterTimerCallback(OS_TIMER_1, OSi_AlarmHandler, NULL);
+
+    u16 timerCount;
+    if (delta < 0)
+    {
+        timerCount = (u16)~1;
+    }
+    else if (delta < 0x10000)
+    {
+        timerCount = (u16)(~delta);
+    }
+    else
+    {
+        timerCount = 0;
+    }
+
+    OS_SetTimerCount((OSTimer)OS_TIMER_1, timerCount);
+    OS_SetTimerControl(OS_TIMER_1, (u16)OSi_TICK_TIMERCONTROL);
+
+    (void)OS_EnableIrqMask(OS_IE_TIMER1);
+}
+
+ARM_FUNC void OS_InitAlarm(void)
+{
+    if (!OSi_UseAlarm)
+    {
+        OSi_UseAlarm = TRUE;
+
+        OSi_SetTimerReserved(OS_TIMER_1);
+
+        OSi_AlarmQueue.head = NULL;
+        OSi_AlarmQueue.tail = NULL;
+
+        (void)OS_DisableIrqMask(OS_IE_TIMER1);
+    }
+}
+
+ARM_FUNC BOOL OS_IsAlarmAvailable(void)
+{
+    return OSi_UseAlarm;
+}
+
+ARM_FUNC void OS_CreateAlarm(OSAlarm *alarm)
+{
+    alarm->handler = 0;
+    alarm->tag = 0;
+}
+
+ARM_FUNC static void OSi_InsertAlarm(OSAlarm *alarm, OSTick fire)
+{
+    if (alarm->period > 0)
+    {
+        OSTick tick = OS_GetTick();
+
+        fire = alarm->start;
+        if (alarm->start < tick)
+        {
+            fire += alarm->period * ((tick - alarm->start) / alarm->period + 1);
+        }
+    }
+
+    alarm->fire = fire;
+
+    OSAlarm *prev;
+    OSAlarm *next;
+
+    for (next = OSi_AlarmQueue.head; next; next = next->next)
+    {
+        if ((s64)(fire - next->fire) >= 0)
+        {
+            continue;
+        }
+
+        alarm->prev = next->prev;
+        next->prev = alarm;
+        alarm->next = next;
+        prev = alarm->prev;
+        if (prev)
+        {
+            prev->next = alarm;
+        }
+        else
+        {
+            OSi_AlarmQueue.head = alarm;
+            OSi_SetTimer(alarm);
+        }
+
+        return;
+    }
+
+    alarm->next = 0;
+    prev = OSi_AlarmQueue.tail;
+    OSi_AlarmQueue.tail = alarm;
+    alarm->prev = prev;
+    if (prev)
+    {
+        prev->next = alarm;
+    }
+    else
+    {
+        OSi_AlarmQueue.head = OSi_AlarmQueue.tail = alarm;
+        OSi_SetTimer(alarm);
+    }
+}
+
+ARM_FUNC void OS_SetAlarm(OSAlarm * alarm, OSTick tick, OSAlarmHandler handler, void *arg)
+{
+    if (!alarm || alarm->handler)
+    {
+        OS_Terminate();
+    }
+
+    OSIntrMode enabled = OS_DisableInterrupts();
+
+    alarm->period = 0;
+
+    alarm->handler = handler;
+    alarm->arg = arg;
+
+    OSi_InsertAlarm(alarm, OS_GetTick() + tick);
+
+    (void)OS_RestoreInterrupts(enabled);
+}
+
+ARM_FUNC void OS_CancelAlarm(OSAlarm *alarm)
+{
+    OSIntrMode enabled = OS_DisableInterrupts();
+
+    if (!alarm->handler)
+    {
+        (void)OS_RestoreInterrupts(enabled);
+        return;
+    }
+
+    OSAlarm *next = alarm->next;
+
+    if (!next)
+    {
+        OSi_AlarmQueue.tail = alarm->prev;
+    }
+    else
+    {
+        next->prev = alarm->prev;
+    }
+
+    if (alarm->prev)
+    {
+        alarm->prev->next = next;
+    }
+    else
+    {
+        OSi_AlarmQueue.head = next;
+        if (next)
+        {
+            OSi_SetTimer(next);
+        }
+    }
+
+    alarm->handler = NULL;
+    alarm->period = 0;
+
+    (void)OS_RestoreInterrupts(enabled);
+}
+
+ARM_FUNC asm void OSi_AlarmHandler(void *arg)
+{
+    stmfd sp!, {r0, lr}
+    bl OSi_ArrangeTimer
+    ldmfd sp!, {r0, lr}
+    bx lr
+}
+
+ARM_FUNC static void OSi_ArrangeTimer(void)
+{
+    OS_SetTimerControl(OS_TIMER_1, 0);
+
+    (void)OS_DisableIrqMask(OS_IE_TIMER1);
+
+    OS_SetIrqCheckFlag(OS_IE_TIMER1);
+
+    OSTick tick = OS_GetTick();
+
+    OSAlarm *alarm = OSi_AlarmQueue.head;
+
+    if (!alarm)
+    {
+        return;
+    }
+
+    if (tick < alarm->fire)
+    {
+        OSi_SetTimer(alarm);
+        return;
+    }
+
+    OSAlarm *next = alarm->next;
+    OSi_AlarmQueue.head = next;
+
+    if (!next)
+    {
+        OSi_AlarmQueue.tail = NULL;
+    }
+    else
+    {
+        next->prev = NULL;
+    }
+
+    OSAlarmHandler handler = alarm->handler;
+
+    if (!alarm->period)
+    {
+        alarm->handler = NULL;
+    }
+
+    if (handler)
+    {
+        (handler)(alarm->arg);
+    }
+
+    if (alarm->period > 0)
+    {
+        alarm->handler = handler;
+        OSi_InsertAlarm(alarm, 0);
+    }
+
+    if (OSi_AlarmQueue.head)
+    {
+        OSi_SetTimer(OSi_AlarmQueue.head);
+    }
+}