arm9 OS_mutex

6 files changed, 161 insertions, 18 deletions

diff --git a/arm7/lib/include/OS_thread.h b/arm7/lib/include/OS_thread.h
index b1d3a01b..701acc94 100644
--- a/arm7/lib/include/OS_thread.h
+++ b/arm7/lib/include/OS_thread.h
@@ -4,20 +4,10 @@
 #include "OS_context.h"
 #include "nitro/OS_thread_shared.h"
 
-static s32 OSi_GetUnusedThreadId(void);
-static void OSi_InsertLinkToQueue(OSThreadQueue *queue, OSThread *thread);
-static OSThread *OSi_RemoveLinkFromQueue(OSThreadQueue *queue);
-static OSThread *OSi_RemoveSpecifiedLinkFromQueue(OSThreadQueue *queue, OSThread *thread);
 OSMutex *OSi_RemoveMutexLinkFromQueue(OSMutexQueue *queue);
-static void OSi_InsertThreadToList(OSThread *thread);
-static void OSi_RemoveThreadFromList(OSThread *thread);
-static void OSi_RescheduleThread(void);
 void OS_InitThread(void);
 void OS_CreateThread(OSThread *thread, void (*func) (void *), void *arg, void *stack, u32 stackSize, u32 prio);
 void OS_ExitThread(void);
-static void OSi_ExitThread_ArgSpecified(OSThread *thread, void *arg);
-static void OSi_ExitThread(void *arg);
-static void OSi_ExitThread_Destroy(void);
 void OS_JoinThread(OSThread *thread);
 BOOL OS_IsThreadTerminated(const OSThread *thread);
 void OS_SleepThread(OSThreadQueue *queue);
@@ -27,7 +17,6 @@ OSThread *OS_SelectThread(void);
 void OS_RescheduleThread(void);
 BOOL OS_SetThreadPriority(OSThread *thread, u32 prio);
 void OS_Sleep(u32 msec);
-static void OSi_SleepAlarmCallback(void *arg);
 OSSwitchThreadCallback OS_SetSwitchThreadCallback(OSSwitchThreadCallback callback);
 u32 OS_DisableScheduler(void);
 u32 OS_EnableScheduler(void);
diff --git a/arm7/lib/src/OS_thread.c b/arm7/lib/src/OS_thread.c
index be3f611d..6db0b4f7 100644
--- a/arm7/lib/src/OS_thread.c
+++ b/arm7/lib/src/OS_thread.c
@@ -29,6 +29,18 @@ static s32 OSi_ThreadIdCount = 0;
 OSThread OSi_LauncherThread;
 OSThread OSi_IdleThread;
 
+static s32 OSi_GetUnusedThreadId(void);
+static void OSi_InsertLinkToQueue(OSThreadQueue *queue, OSThread *thread);
+static OSThread *OSi_RemoveLinkFromQueue(OSThreadQueue *queue);
+static OSThread *OSi_RemoveSpecifiedLinkFromQueue(OSThreadQueue *queue, OSThread *thread);
+static void OSi_InsertThreadToList(OSThread *thread);
+static void OSi_RemoveThreadFromList(OSThread *thread);
+static void OSi_RescheduleThread(void);
+static void OSi_ExitThread_ArgSpecified(OSThread *thread, void *arg);
+static void OSi_ExitThread(void *arg);
+static void OSi_ExitThread_Destroy(void);
+static void OSi_SleepAlarmCallback(void *arg);
+
 ARM_FUNC static s32 OSi_GetUnusedThreadId(void)
 {
     return ++OSi_ThreadIdCount;
diff --git a/arm9/lib/include/OS_mutex.h b/arm9/lib/include/OS_mutex.h
index 9da89c69..23e4337e 100644
--- a/arm9/lib/include/OS_mutex.h
+++ b/arm9/lib/include/OS_mutex.h
@@ -4,7 +4,14 @@
 #include "OS_thread.h"
 #include "OS_context.h"
 #include "nitro/OS_mutex_shared.h"
+#include "nitro/types.h"
 
+void OS_InitMutex(OSMutex *mutex);
+void OS_LockMutex(OSMutex *mutex);
+void OS_UnlockMutex(OSMutex *mutex);
 void OSi_UnlockAllMutex(OSThread * thread);
+BOOL OS_TryLockMutex(OSMutex *mutex);
+void OSi_EnqueueTail(OSThread *thread, OSMutex *mutex);
+void OSi_DequeueItem(OSThread *thread, OSMutex *mutex);
 
 #endif //POKEDIAMOND_ARM9_OS_MUTEX_H
diff --git a/arm9/lib/src/OS_mutex.c b/arm9/lib/src/OS_mutex.c
new file mode 100644
index 00000000..5eb999a9
--- /dev/null
+++ b/arm9/lib/src/OS_mutex.c
@@ -0,0 +1,142 @@
+#include "nitro/types.h"
+#include "function_target.h"
+#include "OS_mutex.h"
+#include "OS_system.h"
+
+ARM_FUNC void OS_InitMutex(OSMutex *mutex)
+{
+    OS_InitThreadQueue(&mutex->queue);
+    mutex->thread = NULL;
+    mutex->count = 0;
+}
+
+ARM_FUNC void OS_LockMutex(OSMutex *mutex)
+{
+    OSIntrMode prevIntrMode = OS_DisableInterrupts();
+    OSThread *currentThread = OS_GetCurrentThread();
+
+    OSThread *ownerThread;
+    for (;;)
+    {
+        ownerThread = ((volatile OSMutex *)mutex)->thread;
+
+        if (ownerThread == NULL)
+        {
+            mutex->thread = currentThread;
+            mutex->count++;
+            OSi_EnqueueTail(currentThread, mutex);
+            break;
+        }
+        else if (ownerThread == currentThread)
+        {
+            mutex->count++;
+            break;
+        }
+        else
+        {
+            currentThread->mutex = mutex;
+            OS_SleepThread(&mutex->queue);
+            currentThread->mutex = NULL;
+        }
+    }
+
+    (void)OS_RestoreInterrupts(prevIntrMode);
+}
+
+ARM_FUNC void OS_UnlockMutex(OSMutex *mutex)
+{
+    OSIntrMode prevIntrMode = OS_DisableInterrupts();
+    OSThread *currentThread = OS_GetCurrentThread();
+
+    if (mutex->thread == currentThread && --mutex->count == 0)
+    {
+        OSi_DequeueItem(currentThread, mutex);
+        mutex->thread = NULL;
+
+        OS_WakeupThread(&mutex->queue);
+    }
+
+    (void)OS_RestoreInterrupts(prevIntrMode);
+}
+
+ARM_FUNC void OSi_UnlockAllMutex(OSThread *thread)
+{
+    OSMutex *mutex;
+    while (thread->mutexQueue.head)
+    {
+        mutex = OSi_RemoveMutexLinkFromQueue(&thread->mutexQueue);
+
+        mutex->count = 0;
+        mutex->thread = NULL;
+        OS_WakeupThread(&mutex->queue);
+    }
+}
+
+ARM_FUNC BOOL OS_TryLockMutex(OSMutex *mutex)
+{
+    OSIntrMode prevIntrMode = OS_DisableInterrupts();
+    OSThread *currentThread = OS_GetCurrentThread();
+    BOOL locked;
+
+    if (mutex->thread == NULL)
+    {
+        mutex->thread = currentThread;
+        mutex->count++;
+        OSi_EnqueueTail(currentThread, mutex);
+        locked = TRUE;
+    }
+    else if (mutex->thread == currentThread)
+    {
+        mutex->count++;
+        locked = TRUE;
+    }
+    else
+    {
+        locked = FALSE;
+    }
+
+    (void)OS_RestoreInterrupts(prevIntrMode);
+    return locked;
+}
+
+ARM_FUNC void OSi_EnqueueTail(OSThread *thread, OSMutex *mutex)
+{
+    OSMutex *prev = thread->mutexQueue.tail;
+
+    if (!prev)
+    {
+        thread->mutexQueue.head = mutex;
+    }
+    else
+    {
+        prev->link.next = mutex;
+    }
+
+    mutex->link.prev = prev;
+    mutex->link.next = NULL;
+    thread->mutexQueue.tail = mutex;
+}
+
+ARM_FUNC void OSi_DequeueItem(OSThread *thread, OSMutex *mutex)
+{
+    OSMutex *next = mutex->link.next;
+    OSMutex *prev = mutex->link.prev;
+
+    if (!next)
+    {
+        thread->mutexQueue.tail = prev;
+    }
+    else
+    {
+        next->link.prev = prev;
+    }
+
+    if(!prev)
+    {
+        thread->mutexQueue.head = next;
+    }
+    else
+    {
+        prev->link.next = next;
+    }
+}
diff --git a/arm9/lib/src/SND_bank.c b/arm9/lib/src/SND_bank.c
index 7f5bd94a..44075454 100644
--- a/arm9/lib/src/SND_bank.c
+++ b/arm9/lib/src/SND_bank.c
@@ -1,8 +1,6 @@
 #include "SND_bank.h"
 #include "OS_mutex.h"
 
-void OS_LockMutex(struct OSMutex *);
-void OS_UnlockMutex(struct OSMutex *);
 void DC_StoreRange(const void *, u32);
 
 /*
diff --git a/arm9/lib/src/SND_main.c b/arm9/lib/src/SND_main.c
index 34b839b1..f97b0873 100644
--- a/arm9/lib/src/SND_main.c
+++ b/arm9/lib/src/SND_main.c
@@ -8,11 +8,6 @@
 static struct OSMutex sSndMutex;
 static s32 sSndInitialized;
 
-// TODO remove these declarations once we have the functions in the headers
-void OS_InitMutex(struct OSMutex *);
-void OS_UnlockMutex(struct OSMutex *);
-void OS_LockMutex(struct OSMutex *);
-
 ARM_FUNC void SND_Init(void) {
     if (sSndInitialized)
         return;