arm9 OS_vramExclusive

2 files changed, 88 insertions, 1 deletions

diff --git a/arm9/lib/src/OS_init.c b/arm9/lib/src/OS_init.c
index 152a5534..5bd435e7 100644
--- a/arm9/lib/src/OS_init.c
+++ b/arm9/lib/src/OS_init.c
@@ -3,7 +3,6 @@
 
 extern void PXI_Init(void);
 extern void MI_Init(void);
-extern void OSi_InitVramExclusive(void);
 extern void CTRDG_Init(void);
 extern void CARD_Init(void);
 extern void PM_Init(void);
diff --git a/arm9/lib/src/OS_vramExclusive.c b/arm9/lib/src/OS_vramExclusive.c
new file mode 100644
index 00000000..5ce10f4a
--- /dev/null
+++ b/arm9/lib/src/OS_vramExclusive.c
@@ -0,0 +1,88 @@
+#include "OS_vramExclusive.h"
+#include "function_target.h"
+#include "OS_system.h"
+
+static u32 OSi_vramExclusive;
+static u16 OSi_vramLockId[9];
+
+static u32 OsCountZeroBits(register u32 bitmap);
+
+ARM_FUNC static asm u32 OsCountZeroBits(register u32 bitmap)
+{
+    clz r0, r0
+    bx lr
+}
+
+ARM_FUNC void OSi_InitVramExclusive(void)
+{
+    OSi_vramExclusive = 0x0000;
+
+    for (s32 i = 0; i < 9; i++)
+    {
+        OSi_vramLockId[i] = 0;
+    }
+}
+
+ARM_FUNC BOOL OSi_TryLockVram(u16 bank, u16 lockId)
+{
+    u32 workMap;
+    s32 zeroBits;
+    OSIntrMode enabled = OS_DisableInterrupts();
+
+    workMap = (u32)(bank & OSi_vramExclusive);
+    while (TRUE)
+    {
+        zeroBits = (s32)(31 - OsCountZeroBits(workMap));
+        if (zeroBits < 0)
+        {
+            break;
+        }
+        workMap &= ~(0x00000001 << zeroBits);
+        if (OSi_vramLockId[zeroBits] != lockId)
+        {
+            (void)OS_RestoreInterrupts(enabled);
+            return FALSE;
+        }
+    }
+
+    workMap = (u32)(bank & 0x01ff);
+    while (TRUE)
+    {
+        zeroBits = (s32)(31 - OsCountZeroBits(workMap));
+        if (zeroBits < 0)
+        {
+            break;
+        }
+        workMap &= ~(0x00000001 << zeroBits);
+        OSi_vramLockId[zeroBits] = lockId;
+        OSi_vramExclusive |= (0x00000001 << zeroBits);
+    }
+
+    (void)OS_RestoreInterrupts(enabled);
+    return TRUE;
+}
+
+ARM_FUNC void OSi_UnlockVram(u16 bank, u16 lockId)
+{
+    u32 workMap;
+    s32 zeroBits;
+    OSIntrMode enabled = OS_DisableInterrupts();
+
+    workMap = (u32)(bank & OSi_vramExclusive & 0x01ff);
+    while (TRUE)
+    {
+        zeroBits = (s32)(31- OsCountZeroBits((u32)workMap));
+        if (zeroBits < 0)
+        {
+            break;
+        }
+        workMap &= ~(0x00000001 << zeroBits);
+        if (OSi_vramLockId[zeroBits] == lockId)
+        {
+            OSi_vramLockId[zeroBits] = 0;
+            OSi_vramExclusive &= ~(0x00000001 << zeroBits);
+        }
+    }
+
+    (void)OS_RestoreInterrupts(enabled);
+}