From 1a45d65f162615b48fd56d60af37ed5483aa9bba Mon Sep 17 00:00:00 2001
From: red031000 <rubenru09@aol.com>
Date: Fri, 22 May 2020 19:04:33 +0100
Subject: OSi_DoTryLockByWord and OS_UnlockByWord

---
 arm9/asm/OS_spinLock_asm.s     | 49 ------------------------------------------
 arm9/global.inc                |  2 ++
 arm9/lib/include/OS_spinLock.h |  3 +++
 arm9/lib/src/OS_spinLock.c     | 41 +++++++++++++++++++++++++++++++----
 4 files changed, 42 insertions(+), 53 deletions(-)

diff --git a/arm9/asm/OS_spinLock_asm.s b/arm9/asm/OS_spinLock_asm.s
index 65063819..653c8380 100644
--- a/arm9/asm/OS_spinLock_asm.s
+++ b/arm9/asm/OS_spinLock_asm.s
@@ -155,52 +155,3 @@ OS_LockCartridge: ; 0x020CA208
 _020CA21C: .word OSi_DoLockByWord
 _020CA220: .word 0x027FFFE8
 _020CA224: .word OSi_AllocateCartridgeBus
-
-	arm_func_start OSi_DoTryLockByWord
-OSi_DoTryLockByWord:
-	stmdb sp!, {r4-r9,lr}
-	sub sp, sp, #0x4
-	movs r6, r3
-	mov r9, r0
-	mov r8, r1
-	mov r7, r2
-	beq _020CA250
-	bl OS_DisableInterrupts_IrqAndFiq
-	mov r5, r0
-	b _020CA258
-_020CA250:
-	bl OS_DisableInterrupts
-	mov r5, r0
-_020CA258:
-	mov r0, r9
-	mov r1, r8
-	bl MI_SwapWord
-	movs r4, r0
-	bne _020CA27C
-	cmp r7, #0x0
-	beq _020CA278
-	blx r7
-_020CA278:
-	strh r9, [r8, #0x4]
-_020CA27C:
-	cmp r6, #0x0
-	beq _020CA290
-	mov r0, r5
-	bl OS_RestoreInterrupts_IrqAndFiq
-	b _020CA298
-_020CA290:
-	mov r0, r5
-	bl OS_RestoreInterrupts
-_020CA298:
-	mov r0, r4
-	add sp, sp, #0x4
-	ldmia sp!, {r4-r9,lr}
-	bx lr
-
-	arm_func_start OS_UnlockByWord
-OS_UnlockByWord: ; 0x020CA2A8
-	ldr ip, _020CA2B4 ; =OSi_DoUnlockByWord
-	mov r3, #0x0
-	bx r12
-	.balign 4
-_020CA2B4: .word OSi_DoUnlockByWord
diff --git a/arm9/global.inc b/arm9/global.inc
index 57a462d3..e1be4932 100644
--- a/arm9/global.inc
+++ b/arm9/global.inc
@@ -6436,6 +6436,7 @@
 .extern OS_TryLockCard
 .extern OS_TryLockCartridge
 .extern OS_TryLockMutex
+.extern OS_UnlockByWord
 .extern OS_UnlockCard
 .extern OS_UnlockCartridge
 .extern OS_UnlockCartridge2
@@ -6450,6 +6451,7 @@
 .extern OSi_DebuggerHandler
 .extern OSi_DoLockByWord
 .extern OSi_DoResetSystem
+.extern OSi_DoTryLockByWord
 .extern OSi_DoUnlockByWord
 .extern OSi_EnterDmaCallback
 .extern OSi_EnterTimerCallback
diff --git a/arm9/lib/include/OS_spinLock.h b/arm9/lib/include/OS_spinLock.h
index c6e0e1fd..520b7b2f 100644
--- a/arm9/lib/include/OS_spinLock.h
+++ b/arm9/lib/include/OS_spinLock.h
@@ -25,6 +25,9 @@ s32 OSi_DoLockByWord(u16 lockId, OSLockWord *lockp, void (*ctrlFuncp) (void),
 s32 OS_TryLockByWord(u16 lockId, OSLockWord *lockp, void (*ctrlFuncp) (void));
 s32 OSi_DoUnlockByWord(u16 lockID, OSLockWord *lockp, void (*ctrlFuncp) (void),
         BOOL disableFIQ);
+s32 OS_UnlockByWord(u16 lockId, OSLockWord *lockp, void (*ctrlFuncp) (void));
+s32 OSi_DoTryLockByWord(u16 lockID, OSLockWord *lockp, void (*ctrlFuncp) (void),
+        BOOL disableFiq);
 s32 OS_GetLockID(void);
 
 #endif //POKEDIAMOND_OS_SPINLOCK_H
diff --git a/arm9/lib/src/OS_spinLock.c b/arm9/lib/src/OS_spinLock.c
index bf9204cb..aac5b295 100644
--- a/arm9/lib/src/OS_spinLock.c
+++ b/arm9/lib/src/OS_spinLock.c
@@ -8,10 +8,10 @@
 #include "consts.h"
 #include "MI_exMemory.h"
 
-extern void OS_UnlockByWord(u16 param1, OSLockWord* lockp, void* func);
 extern void MIi_CpuClear32(u32 param1, void * addr, u32 length); //not too sure about names
 extern s32 OSi_DoTryLockByWord(u16 lockId, OSLockWord *lockp, void (*ctrlFuncp) (void),
                                BOOL disableFiq);
+extern u32 MI_SwapWord(u32 data, volatile u32* destp);
 
 ARM_FUNC void OS_InitLock()
 {
@@ -43,7 +43,7 @@ ARM_FUNC void OS_InitLock()
 
     MIi_SetCartridgeProcessor(MI_PROCESSOR_ARM7);
 
-    OS_UnlockByWord(0x7e, lockp, NULL);
+    (void)OS_UnlockByWord(0x7e, lockp, NULL);
     (void)OS_TryLockByWord(0x7f, lockp, NULL);
 }
 
@@ -80,11 +80,44 @@ ARM_FUNC s32 OSi_DoUnlockByWord(u16 lockID, OSLockWord *lockp, void (*ctrlFuncp)
     lockp->lockFlag = 0;
     if (disableFIQ)
     {
-        OS_RestoreInterrupts_IrqAndFiq(lastIntrMode);
+        (void)OS_RestoreInterrupts_IrqAndFiq(lastIntrMode);
     }
     else
     {
-        OS_RestoreInterrupts(lastIntrMode);
+        (void)OS_RestoreInterrupts(lastIntrMode);
     }
     return 0;
 }
+
+ARM_FUNC s32 OS_UnlockByWord(u16 lockID, OSLockWord* lockp, void (*ctrlFuncp) (void))
+{
+    return OSi_DoUnlockByWord(lockID, lockp, ctrlFuncp, FALSE);
+}
+
+ARM_FUNC s32 OSi_DoTryLockByWord(u16 lockID, OSLockWord *lockp, void (*ctrlFuncp) (void),
+        BOOL disableFiq)
+{
+    OSIntrMode lastIntrMode = (disableFiq) ? OS_DisableInterrupts_IrqAndFiq() : OS_DisableInterrupts();
+
+    s32 lastLockFlag = (s32)MI_SwapWord(lockID, &lockp->lockFlag);
+
+    if (!lastLockFlag)
+    {
+        if (ctrlFuncp)
+        {
+            ctrlFuncp();
+        }
+        lockp->ownerID = lockID;
+    }
+
+    if (disableFiq)
+    {
+        (void)OS_RestoreInterrupts_IrqAndFiq(lastIntrMode);
+    }
+    else
+    {
+        (void)OS_RestoreInterrupts(lastIntrMode);
+    }
+
+    return lastLockFlag;
+}
-- 
cgit v1.2.3