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

3 files changed, 100 insertions, 131 deletions

diff --git a/arm7/asm/OS_mutex.s b/arm7/asm/OS_mutex.s
deleted file mode 100644
index 981842b8..00000000
--- a/arm7/asm/OS_mutex.s
+++ /dev/null
@@ -1,131 +0,0 @@
-	.include "asm/macros.inc"
-	.include "global.inc"
-
-    .text
-
-	arm_func_start OSi_DequeueItem
-OSi_DequeueItem: ; 0x037F99E8
-	ldr	r2, [r1, #16]
-	ldr	r1, [r1, #20]
-	cmp	r2, #0
-	streq	r1, [r0, #112]	; 0x70
-	strne	r1, [r2, #20]
-	cmp	r1, #0
-	streq	r2, [r0, #108]	; 0x6c
-	strne	r2, [r1, #16]
-	bx	lr
-
-	arm_func_start OSi_EnqueueTail
-OSi_EnqueueTail: ; 0x037F9A0C
-	ldr	r2, [r0, #112]	; 0x70
-	cmp	r2, #0
-	streq	r1, [r0, #108]	; 0x6c
-	strne	r1, [r2, #16]
-	str	r2, [r1, #20]
-	mov	r2, #0
-	str	r2, [r1, #16]
-	str	r1, [r0, #112]	; 0x70
-	bx	lr
-
-	arm_func_start OSi_UnlockAllMutex
-OSi_UnlockAllMutex: ; 0x037F9A30
-	stmdb	sp!, {r4, r5, r6, lr}
-	mov	r6, r0
-	add	r5, r6, #108	; 0x6c
-	mov	r4, #0
-	b	_037F9A58
-_037F9A44:
-	mov	r0, r5
-	bl	OSi_RemoveMutexLinkFromQueue
-	str	r4, [r0, #12]
-	str	r4, [r0, #8]
-	bl	OS_WakeupThread
-_037F9A58:
-	ldr	r0, [r6, #108]	; 0x6c
-	cmp	r0, #0
-	bne	_037F9A44
-	ldmia	sp!, {r4, r5, r6, lr}
-	bx	lr
-
-	arm_func_start OS_UnlockMutex
-OS_UnlockMutex: ; 0x037F9A6C
-	stmdb	sp!, {r4, r5, lr}
-	sub	sp, sp, #4
-	mov	r5, r0
-	bl	OS_DisableInterrupts
-	mov	r4, r0
-	ldr	r0, _037F9AD8	; =OSi_ThreadInfo
-	ldr	r0, [r0, #4]
-	ldr	r1, [r5, #8]
-	cmp	r1, r0
-	bne	_037F9AC4
-	ldr	r1, [r5, #12]
-	sub	r1, r1, #1
-	str	r1, [r5, #12]
-	ldr	r1, [r5, #12]
-	cmp	r1, #0
-	bne	_037F9AC4
-	mov	r1, r5
-	bl	OSi_DequeueItem
-	mov	r0, #0
-	str	r0, [r5, #8]
-	mov	r0, r5
-	bl	OS_WakeupThread
-_037F9AC4:
-	mov	r0, r4
-	bl	OS_RestoreInterrupts
-	add	sp, sp, #4
-	ldmia	sp!, {r4, r5, lr}
-	bx	lr
-_037F9AD8:	.word	OSi_ThreadInfo
-
-	arm_func_start OS_LockMutex
-OS_LockMutex: ; 0x037F9ADC
-	stmdb	sp!, {r4, r5, r6, r7, lr}
-	sub	sp, sp, #4
-	mov	r5, r0
-	bl	OS_DisableInterrupts
-	mov	r4, r0
-	ldr	r0, _037F9B64	; =OSi_ThreadInfo
-	ldr	r7, [r0, #4]
-	mov	r6, #0
-_037F9AFC:
-	ldr	r0, [r5, #8]
-	cmp	r0, #0
-	bne	_037F9B28
-	str	r7, [r5, #8]
-	ldr	r0, [r5, #12]
-	add	r0, r0, #1
-	str	r0, [r5, #12]
-	mov	r0, r7
-	mov	r1, r5
-	bl	OSi_EnqueueTail
-	b	_037F9B50
-_037F9B28:
-	cmp	r0, r7
-	ldreq	r0, [r5, #12]
-	addeq	r0, r0, #1
-	streq	r0, [r5, #12]
-	beq	_037F9B50
-	str	r5, [r7, #104]	; 0x68
-	mov	r0, r5
-	bl	OS_SleepThread
-	str	r6, [r7, #104]	; 0x68
-	b	_037F9AFC
-_037F9B50:
-	mov	r0, r4
-	bl	OS_RestoreInterrupts
-	add	sp, sp, #4
-	ldmia	sp!, {r4, r5, r6, r7, lr}
-	bx	lr
-_037F9B64:	.word	OSi_ThreadInfo
-
-	arm_func_start OS_InitMutex
-OS_InitMutex: ; 0x037F9B68
-	mov	r2, #0
-	str	r2, [r0, #4]
-	ldr	r1, [r0, #4]
-	str	r1, [r0]
-	str	r2, [r0, #8]
-	str	r2, [r0, #12]
-	bx	lr
diff --git a/arm7/lib/include/OS_mutex.h b/arm7/lib/include/OS_mutex.h
index b41ac9f8..920d5594 100644
--- a/arm7/lib/include/OS_mutex.h
+++ b/arm7/lib/include/OS_mutex.h
@@ -4,6 +4,11 @@
 #include "OS_context.h"
 #include "nitro/OS_mutex_shared.h"
 
+void OSi_DequeueItem(OSThread * thread, OSMutex * mutex);
+void OSi_EnqueueTail(OSThread * thread, OSMutex * mutex);
+void OS_InitMutex(OSMutex* mutex);
+void OS_LockMutex(OSMutex* mutex);
+void OS_UnlockMutex(OSMutex* mutex);
 void OSi_UnlockAllMutex(OSThread * thread);
 
 #endif //POKEDIAMOND_ARM7_OS_MUTEX_H
diff --git a/arm7/lib/src/OS_mutex.c b/arm7/lib/src/OS_mutex.c
new file mode 100644
index 00000000..7e9b05ec
--- /dev/null
+++ b/arm7/lib/src/OS_mutex.c
@@ -0,0 +1,95 @@
+#include "function_target.h"
+#include "OS_mutex.h"
+#include "OS_system.h"
+#include "OS_thread.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 mode;
+    OSThread* current;
+    
+    mode = OS_DisableInterrupts();
+    current = OS_GetCurrentThread();
+    while (1) {
+        OSThread* owner = mutex->thread;
+        if (owner == NULL) {
+            mutex->thread = current;
+            mutex->count++;
+            OSi_EnqueueTail(current, mutex);
+            break;
+        }
+        else if (owner == current) {
+            mutex->count++;
+            break;
+        }
+        else {
+            current->mutex = mutex;
+            OS_SleepThread(&mutex->queue);
+            current->mutex = NULL;
+        }
+    }
+    (void)OS_RestoreInterrupts(mode);
+}
+
+ARM_FUNC void OS_UnlockMutex(OSMutex* mutex) {
+    OSIntrMode mode;
+    OSThread* current;
+    
+    mode = OS_DisableInterrupts();
+    current = OS_GetCurrentThread();
+    if (mutex->thread == current) {
+        if (--mutex->count == 0) {
+            OSi_DequeueItem(current, mutex);
+            mutex->thread = NULL;
+            OS_WakeupThread(&mutex->queue);
+        }
+    }
+    (void)OS_RestoreInterrupts(mode);
+}
+
+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 void OSi_EnqueueTail(OSThread * thread, OSMutex * mutex) {
+    OSMutex * tail = thread->mutexQueue.tail;
+    if (tail == NULL) {
+        thread->mutexQueue.head = mutex;
+    }
+    else {
+        tail->link.next = mutex;
+    }
+    mutex->link.prev = tail;
+    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 == NULL) {
+        thread->mutexQueue.tail = prev;
+    }
+    else {
+        next->link.prev = prev;
+    }
+
+    if (prev == NULL) {
+        thread->mutexQueue.head = next;
+    }
+    else {
+        prev->link.next = next;
+    }
+}