conflicts

7 files changed, 914 insertions, 5 deletions

diff --git a/arm7/lib/src/OS_init.c b/arm7/lib/src/OS_init.c
index 583bb75e..271d067b 100644
--- a/arm7/lib/src/OS_init.c
+++ b/arm7/lib/src/OS_init.c
@@ -2,8 +2,6 @@
 #include "OS_init.h"
 
 extern void PXI_Init(void);
-extern void OS_InitLock(void);
-extern void OS_InitIrqTable(void);
 extern void OS_InitTick(void);
 extern void OS_InitAlarm(void);
 extern void OS_InitThread(void);
diff --git a/arm7/lib/src/OS_interrupt.c b/arm7/lib/src/OS_interrupt.c
new file mode 100644
index 00000000..e9fc3347
--- /dev/null
+++ b/arm7/lib/src/OS_interrupt.c
@@ -0,0 +1,106 @@
+#include "function_target.h"
+#include "consts.h"
+#include "OS_interrupt.h"
+#include "OS_thread.h"
+
+extern OSThreadQueue OSi_IrqThreadQueue;
+
+ARM_FUNC void OS_InitIrqTable(void)
+{
+    OS_InitThreadQueue(&OSi_IrqThreadQueue);
+
+    OSi_SetVBlankCount(0);
+}
+
+ARM_FUNC void OS_SetIrqFunction(OSIrqMask intrBit, OSIrqFunction function)
+{
+    s32 i;
+    OSIrqCallbackInfo *info;
+
+    for (i = 0; i < 25; i++)
+    {
+        if (intrBit & 1)
+        {
+            info = NULL;
+
+            if (8 <= i && i <= 11)
+            {
+                info = &OSi_IrqCallbackInfo[i - 8];
+            }
+            else if (3 <= i && i <= 6)
+            {
+                info = &OSi_IrqCallbackInfo[i - 3 + 4];
+            }
+            else if (0 == i)
+            {
+                info = &OSi_IrqCallbackInfo[8];
+            }
+            else
+            {
+                OS_IRQTable[i] = function;
+            }
+
+            if (info)
+            {
+                info->func = (void (*)(void *))function;
+                info->arg = 0;
+                info->enable = TRUE;
+            }
+        }
+        intrBit >>= 1;
+    }
+}
+
+ARM_FUNC void OSi_EnterTimerCallback(u32 timerNo, void (*callback) (void *), void *arg)
+{
+    OSIrqMask mask = 1UL << (timerNo + 3);
+    OSi_IrqCallbackInfo[timerNo + 4].func = callback;
+    OSi_IrqCallbackInfo[timerNo + 4].arg = arg;
+
+    (void)OS_EnableIrqMask(mask);
+    OSi_IrqCallbackInfo[timerNo + 4].enable = TRUE;
+}
+
+ARM_FUNC OSIrqMask OS_SetIrqMask(OSIrqMask mask)
+{
+    u16 regIme = reg_OS_IME;
+    reg_OS_IME = 0;
+    OSIrqMask regIe = reg_OS_IE;
+    reg_OS_IE = mask;
+    u16 unused = reg_OS_IME; //needed because otherwise it doesn't match
+    reg_OS_IME = regIme;
+    return regIe;
+}
+
+ARM_FUNC OSIrqMask OS_EnableIrqMask(OSIrqMask mask)
+{
+    u16 regIme = reg_OS_IME;
+    reg_OS_IME = 0;
+    OSIrqMask regIe = reg_OS_IE;
+    reg_OS_IE = regIe | mask;
+    u16 unused = reg_OS_IME;
+    reg_OS_IME = regIme;
+    return regIe;
+}
+
+ARM_FUNC OSIrqMask OS_DisableIrqMask(OSIrqMask mask)
+{
+    u16 regIme = reg_OS_IME;
+    reg_OS_IME = 0;
+    OSIrqMask regIe = reg_OS_IE;
+    reg_OS_IE = regIe & ~mask;
+    u16 unused = reg_OS_IME;
+    reg_OS_IME = regIme;
+    return regIe;
+}
+
+ARM_FUNC OSIrqMask OS_ResetRequestIrqMask(OSIrqMask mask)
+{
+    u16 regIme = reg_OS_IME;
+    reg_OS_IME = 0;
+    OSIrqMask regIf = reg_OS_IF;
+    reg_OS_IF = mask;
+    u16 unused = reg_OS_IME;
+    reg_OS_IME = regIme;
+    return regIf;
+}
diff --git a/arm7/lib/src/OS_reset.c b/arm7/lib/src/OS_reset.c
index c40fcff3..e0f380f8 100644
--- a/arm7/lib/src/OS_reset.c
+++ b/arm7/lib/src/OS_reset.c
@@ -1,6 +1,7 @@
 #include "function_target.h"
 #include "OS_reset.h"
 #include "OS_interrupt.h"
+#include "OS_terminate_proc.h"
 
 static u16 OSi_IsInitReset = 0;
 vu16 OSi_IsResetOccurred = 0;
@@ -10,7 +11,6 @@ extern OSIrqMask OS_SetIrqMask(OSIrqMask mask);
 extern OSIrqMask OS_ResetRequestIrqMask(OSIrqMask mask);
 extern void SND_Shutdown(void);
 extern void PXI_SetFifoRecvCallback(u32 param1, void* callback);
-extern void OS_Terminate(void);
 extern u32 PXI_SendWordByFifo(u32 param1, u32 data, u32 param2);
 extern void FUN_038073EC(void); //OSi_DoResetSystem, in wram
 
diff --git a/arm7/lib/src/OS_spinLock.c b/arm7/lib/src/OS_spinLock.c
new file mode 100644
index 00000000..2857de05
--- /dev/null
+++ b/arm7/lib/src/OS_spinLock.c
@@ -0,0 +1,192 @@
+#include "function_target.h"
+#include "OS_spinLock.h"
+#include "OS_system.h"
+#include "MI_swap.h"
+
+ARM_FUNC void OS_InitLock(void)
+{
+    static BOOL isInitialized = FALSE;
+
+    if (isInitialized)
+        return;
+    isInitialized = TRUE;
+
+    OSLockWord *lockp = (OSLockWord *)HW_INIT_LOCK_BUF;
+
+    lockp->extension = 0;
+    while (lockp->ownerID != 0x7f)
+    {
+        OSi_WaitByLoop();
+    }
+
+    ((u32 *)HW_LOCK_ID_FLAG_SUB)[0] = 0xffffffff;
+    ((u32 *)HW_LOCK_ID_FLAG_SUB)[1] = 0xffff0000;
+
+    lockp->extension = 0xbf;
+}
+
+ARM_FUNC void FUN_037F8CB4(s32 ct)
+{
+    SVC_WaitByLoop((u32)ct); //todo this should be linker generated, figure out why it broke
+}
+
+ARM_FUNC s32 OSi_DoLockByWord(u16 lockId, OSLockWord *lockp, void (*ctrlFuncp) (void),
+                              BOOL disableFiq)
+{
+    s32 lastLockFlag;
+    while ((lastLockFlag = OSi_DoTryLockByWord(lockId, lockp, ctrlFuncp, disableFiq)) > 0) {
+        OSi_WaitByLoop();
+    }
+
+    return lastLockFlag;
+}
+
+ARM_FUNC s32 OSi_DoUnlockByWord(u16 lockID, OSLockWord *lockp, void (*ctrlFuncp) (void),
+                                BOOL disableFIQ)
+{
+    if (lockID != lockp->ownerID)
+    {
+        return -2;
+    }
+
+    OSIntrMode lastIntrMode = (disableFIQ) ? OS_DisableInterrupts_IrqAndFiq() : OS_DisableInterrupts();
+    lockp->ownerID = 0;
+    if (ctrlFuncp)
+    {
+        ctrlFuncp();
+    }
+    lockp->lockFlag = 0;
+    if (disableFIQ)
+    {
+        (void)OS_RestoreInterrupts_IrqAndFiq(lastIntrMode);
+    }
+    else
+    {
+        (void)OS_RestoreInterrupts(lastIntrMode);
+    }
+    return 0;
+}
+
+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, (u32 *)&lockp->lockFlag);
+
+    if (!lastLockFlag)
+    {
+        if (ctrlFuncp)
+        {
+            ctrlFuncp();
+        }
+        lockp->ownerID = lockID;
+    }
+
+    if (disableFiq)
+    {
+        (void)OS_RestoreInterrupts_IrqAndFiq(lastIntrMode);
+    }
+    else
+    {
+        (void)OS_RestoreInterrupts(lastIntrMode);
+    }
+
+    return lastLockFlag;
+}
+
+ARM_FUNC s32 OS_LockCartridge(u16 lockID)
+{
+    return OSi_DoLockByWord(lockID, (OSLockWord *)HW_CTRDG_LOCK_BUF, OSi_AllocateCartridgeBus, TRUE);
+}
+
+ARM_FUNC s32 OS_UnlockCartridge(u16 lockID)
+{
+    return OSi_DoUnlockByWord(lockID, (OSLockWord *)HW_CTRDG_LOCK_BUF, OSi_FreeCartridgeBus, TRUE);
+}
+
+ARM_FUNC s32 OS_TryLockCartridge(u16 lockID)
+{
+    return OSi_DoTryLockByWord(lockID, (OSLockWord *)HW_CTRDG_LOCK_BUF, OSi_AllocateCartridgeBus, TRUE);
+}
+
+ARM_FUNC void OSi_AllocateCartridgeBus(void)
+{
+    //noop
+}
+
+ARM_FUNC void OSi_FreeCartridgeBus(void)
+{
+    //noop
+}
+
+ARM_FUNC u16 OS_ReadOwnerOfLockWord(OSLockWord * lock)
+{
+    return lock->ownerID;
+}
+
+ARM_FUNC asm s32 OS_UnLockCartridge(u16 lockID)
+{
+    ldr r1, =OS_UnlockCartridge
+    bx r1
+}
+
+ARM_FUNC asm s32 OS_GetLockID(void)
+{
+    ldr	r3, =HW_LOCK_ID_FLAG_SUB
+    ldr	r1, [r3]
+    mov	r2, #0
+    mov	r0, #0x80000000
+_037F8A34:
+    tst	r1, r0
+    bne	_037F8A50
+    add	r2, r2, #1
+    cmp	r2, #32
+    beq	_037F8A50
+    mov	r0, r0, lsr #1
+    b	_037F8A34
+_037F8A50:
+    cmp	r2, #32
+    movne	r0, #0x80
+    bne	_037F8A98
+    add	r3, r3, #4
+    ldr	r1, [r3]
+    mov	r2, #0
+    mov	r0, #0x80000000
+_037F8A6C:
+    tst	r1, r0
+    bne	_037F8A88
+    add	r2, r2, #1
+    cmp	r2, #32
+    beq	_037F8A88
+    mov	r0, r0, lsr #1
+    b	_037F8A6C
+_037F8A88:
+    cmp	r2, #32
+    ldr	r0, =0xFFFFFFFD
+    bxeq	lr
+    mov	r0, #160
+_037F8A98:
+    add	r0, r0, r2
+    mov	r1, #0x80000000
+    mov	r1, r1, lsr r2
+    ldr	r2, [r3]
+    bic	r2, r2, r1
+    str	r2, [r3]
+    bx	lr
+}
+
+ARM_FUNC asm void OS_ReleaseLockID(register u16 lockID)
+{
+    ldr r3, =HW_LOCK_ID_FLAG_SUB
+    cmp r0, #0xa0
+    addpl r3, r3, #0x4
+    subpl r0, r0, #0xa0
+    submi r0, r0, #0x80
+    mov r1, #0x80000000
+    mov r1, r1, lsr r0
+    ldr r2, [r3, #0x0]
+    orr r2, r2, r1
+    str r2, [r3, #0x0]
+    bx lr
+}
diff --git a/arm7/lib/src/OS_system.c b/arm7/lib/src/OS_system.c
index 11417e91..503bd854 100644
--- a/arm7/lib/src/OS_system.c
+++ b/arm7/lib/src/OS_system.c
@@ -1,7 +1,6 @@
 #include "function_target.h"
 #include "OS_system.h"
-
-extern void FUN_037F8CB4(s32 count);
+#include "OS_spinLock.h"
 
 ARM_FUNC asm OSIntrMode OS_EnableInterrupts(void)
 {
diff --git a/arm7/lib/src/OS_terminate_proc.c b/arm7/lib/src/OS_terminate_proc.c
new file mode 100644
index 00000000..23793fd4
--- /dev/null
+++ b/arm7/lib/src/OS_terminate_proc.c
@@ -0,0 +1,17 @@
+#include "function_target.h"
+#include "nitro/types.h"
+#include "OS_terminate_proc.h"
+#include "OS_system.h"
+
+extern void CTRDG_VibPulseEdgeUpdate(u32 param);
+extern void FUN_037F8530(void); //SVC_Halt
+
+ARM_FUNC void OS_Terminate(void)
+{
+    CTRDG_VibPulseEdgeUpdate(0);
+    while (TRUE)
+    {
+        (void)OS_DisableInterrupts();
+        FUN_037F8530(); //SVC_Halt
+    }
+}
diff --git a/arm7/lib/src/OS_thread.c b/arm7/lib/src/OS_thread.c
new file mode 100644
index 00000000..be3f611d
--- /dev/null
+++ b/arm7/lib/src/OS_thread.c
@@ -0,0 +1,597 @@
+#include "OS_thread.h"
+#include "function_target.h"
+#include "OS_system.h"
+#include "consts.h"
+#include "OS_terminate_proc.h"
+#include "OS_mutex.h"
+#include "OS_alarm.h"
+#include "OS_context.h"
+#include "nitro/OS_systemWork_shared.h"
+#include "MI_memory.h"
+
+extern void SDK_SYS_STACKSIZE(void);
+extern void SDK_IRQ_STACKSIZE(void);
+
+u32 OSi_RescheduleCount = 0;
+
+void *OSi_StackForDestructor = NULL;
+
+OSThreadInfo OSi_ThreadInfo;
+
+BOOL OSi_IsThreadInitialized = FALSE;
+
+OSThread **OSi_CurrentThreadPtr = NULL;
+
+void *OSi_SystemCallbackInSwitchThread = NULL;
+
+static s32 OSi_ThreadIdCount = 0;
+
+OSThread OSi_LauncherThread;
+OSThread OSi_IdleThread;
+
+ARM_FUNC static s32 OSi_GetUnusedThreadId(void)
+{
+    return ++OSi_ThreadIdCount;
+}
+
+ARM_FUNC static void OSi_InsertLinkToQueue(OSThreadQueue *queue, OSThread *thread)
+{
+    OSThread *next = queue->head;
+
+    while (next && next->priority <= thread->priority)
+    {
+        if (next == thread)
+            return;
+        next = next->link.next;
+    }
+
+    if (!next)
+    {
+        OSThread *prev = queue->tail;
+
+        if (!prev)
+        {
+            queue->head = thread;
+        }
+        else
+        {
+            prev->link.next = thread;
+        }
+
+        thread->link.prev = prev;
+        thread->link.next = NULL;
+        queue->tail = thread;
+    }
+    else
+    {
+        OSThread *prev = next->link.prev;
+
+        if (!prev)
+        {
+            queue->head = thread;
+        }
+        else
+        {
+            prev->link.next = thread;
+        }
+
+        thread->link.prev = prev;
+        thread->link.next = next;
+        next->link.prev = thread;
+    }
+}
+
+ARM_FUNC static OSThread *OSi_RemoveLinkFromQueue(OSThreadQueue *queue)
+{
+    OSThread *thread = queue->head;
+
+    if (thread)
+    {
+        OSThread *next = thread->link.next;
+
+        queue->head = next;
+
+        if (next)
+        {
+            next->link.prev = NULL;
+        }
+        else
+        {
+            queue->tail = NULL;
+            thread->queue = NULL;
+        }
+    }
+
+    return thread;
+}
+
+ARM_FUNC static OSThread *OSi_RemoveSpecifiedLinkFromQueue(OSThreadQueue *queue, OSThread *thread)
+{
+    OSThread *queueHead = queue->head;
+
+    while (queueHead)
+    {
+        OSThread *next = queueHead->link.next;
+
+        if (queueHead == thread)
+        {
+            OSThread *prev = queueHead->link.prev;
+
+            if (queue->head == queueHead)
+            {
+                queue->head = next;
+            }
+            else
+            {
+                prev->link.next = next;
+            }
+
+            if (queue->tail == queueHead)
+            {
+                queue->tail = prev;
+            }
+            else
+            {
+                next->link.prev = prev;
+            }
+
+            break;
+        }
+
+        queueHead = next;
+    }
+
+    return queueHead;
+}
+
+ARM_FUNC OSMutex *OSi_RemoveMutexLinkFromQueue(OSMutexQueue *queue)
+{
+    OSMutex *mutexHead = queue->head;
+
+    if (mutexHead)
+    {
+        OSMutex *next = mutexHead->link.next;
+
+        queue->head = next;
+
+        if (next)
+        {
+            next->link.prev = NULL;
+        }
+        else
+        {
+            queue->tail = NULL;
+        }
+    }
+
+    return mutexHead;
+}
+
+ARM_FUNC static void OSi_InsertThreadToList(OSThread *thread)
+{
+    OSThread *t = OSi_ThreadInfo.list;
+    OSThread *pre = NULL;
+
+    while(t && t->priority < thread->priority)
+    {
+        pre = t;
+        t = t->next;
+    }
+
+    if (!pre)
+    {
+        thread->next = OSi_ThreadInfo.list;
+        OSi_ThreadInfo.list = thread;
+    }
+    else
+    {
+        thread->next = pre->next;
+        pre->next = thread;
+    }
+}
+
+ARM_FUNC static void OSi_RemoveThreadFromList(OSThread *thread)
+{
+    OSThread *t = OSi_ThreadInfo.list;
+    OSThread *pre = NULL;
+
+    while (t && t != thread)
+    {
+        pre = t;
+        t = t-> next;
+    }
+
+    if (!pre)
+    {
+        OSi_ThreadInfo.list = thread->next;
+    }
+    else
+    {
+        pre->next = thread->next;
+    }
+}
+
+ARM_FUNC static void OSi_RescheduleThread(void)
+{
+    if (OSi_RescheduleCount <= 0)
+    {
+        OSThreadInfo *info = &OSi_ThreadInfo;
+        if (info->irqDepth > 0 || OS_GetProcMode() == OS_PROCMODE_IRQ)
+        {
+            info->isNeedRescheduling = TRUE;
+        }
+        else
+        {
+            OSThread *currentThread = OSi_GetCurrentThread();
+            OSThread *nextThread = OS_SelectThread();
+
+            if (currentThread == nextThread || !nextThread)
+                return;
+
+            if (currentThread->state != OS_THREAD_STATE_TERMINATED
+                && OS_SaveContext(&currentThread->context))
+                return;
+
+            if (OSi_SystemCallbackInSwitchThread)
+            {
+                ((OSSwitchThreadCallback)OSi_SystemCallbackInSwitchThread) (currentThread, nextThread);
+            }
+
+            if (info->switchCallback)
+            {
+                ((OSSwitchThreadCallback)info->switchCallback) (currentThread, nextThread);
+            }
+
+            OS_SetCurrentThread(nextThread);
+
+            OS_LoadContext(&nextThread->context);
+        }
+    }
+}
+
+ARM_FUNC void OS_InitThread(void)
+{
+    if (OSi_IsThreadInitialized)
+        return;
+    OSi_IsThreadInitialized = TRUE;
+
+    OSi_CurrentThreadPtr = &(OSi_ThreadInfo.current);
+
+    OSi_LauncherThread.priority = OS_THREAD_LAUNCHER_PRIORITY;
+    OSi_LauncherThread.id = 0;
+    OSi_LauncherThread.state = OS_THREAD_STATE_READY;
+    OSi_LauncherThread.next = NULL;
+
+    OSi_LauncherThread.profiler = NULL;
+
+    OSi_ThreadInfo.list = &OSi_LauncherThread;
+
+    OS_SetCurrentThread(&OSi_LauncherThread);
+
+    void *stackLo = (((s32)SDK_SYS_STACKSIZE) <= 0) ?
+                    (void *)((u32)HW_WRAM - (s32)SDK_SYS_STACKSIZE) :
+                    (void *)((u32)(HW_PRV_WRAM_IRQ_STACK_END - (s32)SDK_IRQ_STACKSIZE) - (s32)SDK_SYS_STACKSIZE);
+
+    OSi_LauncherThread.stackBottom = (u32)(HW_PRV_WRAM_IRQ_STACK_END - (s32)SDK_IRQ_STACKSIZE);
+    OSi_LauncherThread.stackTop = (u32)stackLo;
+    OSi_LauncherThread.stackWarningOffset = 0;
+
+    //checksums
+    *(u32 *)(OSi_LauncherThread.stackBottom - sizeof(u32)) = 0xd73bfdf7UL;
+    *(u32 *)OSi_LauncherThread.stackTop = 0xfbdd37bbUL;
+
+    OS_InitThreadQueue(&OSi_LauncherThread.joinQueue);
+
+    OSi_ThreadInfo.isNeedRescheduling = FALSE;
+    OSi_ThreadInfo.irqDepth = 0;
+
+    OS_GetSystemWork()->threadinfo_subp = &OSi_ThreadInfo;
+
+    (void)OS_SetSwitchThreadCallback(NULL);
+}
+
+ARM_FUNC void OS_CreateThread(OSThread *thread, void (*func) (void *), void *arg, void *stack, u32 stackSize, u32 prio)
+{
+    OSIntrMode enable = OS_DisableInterrupts();
+
+    s32 index = OSi_GetUnusedThreadId();
+
+    thread->priority = prio;
+    thread->id = (u32)index;
+    thread->state = OS_THREAD_STATE_WAITING;
+
+    thread->profiler = NULL;
+
+    OSi_InsertThreadToList(thread);
+
+    thread->stackBottom = (u32)stack;
+    thread->stackTop = (u32)stack - stackSize;
+    thread->stackWarningOffset = 0;
+
+    *(u32 *)(thread->stackBottom - sizeof(u32)) = 0xd73bfdf7UL;
+    *(u32 *)thread->stackTop = 0xfbdd37bbUL;
+
+    OS_InitThreadQueue(&thread->joinQueue);
+
+    OS_InitContext(&thread->context, (u32)func, (u32)stack - 4);
+
+    thread->context.r[0] = (u32)arg;
+    thread->context.lr = (u32)OS_ExitThread;
+
+    MI_CpuClear32((void *)((u32)stack - stackSize + 4), stackSize - 8);
+
+    thread->mutex = NULL;
+    thread->mutexQueue.head = NULL;
+    thread->mutexQueue.tail = NULL;
+
+    OS_SetThreadDestructor(thread, NULL);
+
+    thread->queue = NULL;
+    thread->link.prev = thread->link.next = NULL;
+
+    MI_CpuClear32(&thread->specific[0], sizeof(void *) * OS_THREAD_SPECIFIC_MAX);
+
+    thread->alarmForSleep = NULL;
+
+    (void)OS_RestoreInterrupts(enable);
+}
+
+ARM_FUNC void OS_ExitThread(void)
+{
+    (void)OS_DisableInterrupts();
+    OSi_ExitThread_ArgSpecified(OS_GetCurrentThread(), 0);
+}
+
+ARM_FUNC static void OSi_ExitThread_ArgSpecified(OSThread *thread, void *arg)
+{
+    if (OSi_StackForDestructor)
+    {
+        OS_InitContext(&thread->context, (u32)OSi_ExitThread, (u32)OSi_StackForDestructor);
+        thread->context.r[0] = (u32)arg;
+        thread->context.cpsr |= HW_PSR_DISABLE_IRQ;
+        thread->state = OS_THREAD_STATE_READY;
+        OS_LoadContext(&thread->context);
+    }
+    else
+    {
+        OSi_ExitThread(arg);
+    }
+}
+
+ARM_FUNC static void OSi_ExitThread(void *arg)
+{
+    OSThread *currentThread = OSi_GetCurrentThread();
+    OSThreadDestructor destructor = currentThread->destructor;
+
+    if (destructor)
+    {
+        currentThread->destructor = NULL;
+        destructor(arg);
+        (void)OS_DisableInterrupts();
+    }
+
+    OSi_ExitThread_Destroy();
+}
+
+ARM_FUNC static void OSi_ExitThread_Destroy(void)
+{
+    OSThread *currentThread = OSi_GetCurrentThread();
+    (void)OS_DisableScheduler();
+
+    OSi_UnlockAllMutex(currentThread);
+
+    if (currentThread->queue)
+    {
+        (void)OSi_RemoveSpecifiedLinkFromQueue(currentThread->queue, currentThread);
+    }
+
+    OSi_RemoveThreadFromList(currentThread);
+
+    currentThread->state = OS_THREAD_STATE_TERMINATED;
+
+    OS_WakeupThread(&currentThread->joinQueue);
+
+    (void)OS_EnableScheduler();
+
+    OS_RescheduleThread();
+
+    OS_Terminate();
+}
+
+ARM_FUNC void OS_JoinThread(OSThread *thread)
+{
+    OSIntrMode enabled = OS_DisableInterrupts();
+
+    if (thread->state != OS_THREAD_STATE_TERMINATED)
+    {
+        OS_SleepThread(&thread->joinQueue);
+    }
+
+    (void)OS_RestoreInterrupts(enabled);
+}
+
+ARM_FUNC BOOL OS_IsThreadTerminated(const OSThread *thread)
+{
+    return (thread->state == OS_THREAD_STATE_TERMINATED) ? TRUE : FALSE;
+}
+
+ARM_FUNC void OS_SleepThread(OSThreadQueue *queue)
+{
+    OSIntrMode enabled = OS_DisableInterrupts();
+    OSThread *currentThread = OSi_GetCurrentThread();
+
+    if (queue)
+    {
+        currentThread->queue = queue;
+        OSi_InsertLinkToQueue(queue, currentThread);
+    }
+
+    currentThread->state = OS_THREAD_STATE_WAITING;
+    OSi_RescheduleThread();
+
+    (void)OS_RestoreInterrupts(enabled);
+}
+
+ARM_FUNC void OS_WakeupThread(OSThreadQueue *queue)
+{
+    OSIntrMode enabled = OS_DisableInterrupts();
+
+    if (queue->head)
+    {
+        while (queue->head)
+        {
+            OSThread *thread = OSi_RemoveLinkFromQueue(queue);
+
+            thread->state = OS_THREAD_STATE_READY;
+            thread->queue = NULL;
+            thread->link.prev = thread->link.next = NULL;
+        }
+
+        OS_InitThreadQueue(queue);
+        OSi_RescheduleThread();
+    }
+
+    (void)OS_RestoreInterrupts(enabled);
+}
+
+ARM_FUNC void OS_WakeupThreadDirect(OSThread *thread)
+{
+    OSIntrMode enabled = OS_DisableInterrupts();
+
+    thread->state = OS_THREAD_STATE_READY;
+    OSi_RescheduleThread();
+
+    (void)OS_RestoreInterrupts(enabled);
+}
+
+ARM_FUNC OSThread *OS_SelectThread(void)
+{
+    OSThread *thread = OSi_ThreadInfo.list;
+
+    while (thread && !OS_IsThreadRunnable(thread))
+    {
+        thread = thread->next;
+    }
+
+    return thread;
+}
+
+ARM_FUNC void OS_RescheduleThread(void)
+{
+    OSIntrMode enabled = OS_DisableInterrupts();
+    OSi_RescheduleThread();
+    (void)OS_RestoreInterrupts(enabled);
+}
+
+ARM_FUNC BOOL OS_SetThreadPriority(OSThread *thread, u32 prio)
+{
+    OSThread *t = OSi_ThreadInfo.list;
+    OSThread *pre = NULL;
+    OSIntrMode enabled = OS_DisableInterrupts();
+
+    while (t && t != thread)
+    {
+        pre = t;
+        t = t->next;
+    }
+
+    if (!t || t == &OSi_IdleThread)
+    {
+        (void)OS_RestoreInterrupts(enabled);
+        return FALSE;
+    }
+
+    if (t->priority != prio)
+    {
+        if (!pre)
+        {
+            OSi_ThreadInfo.list = thread->next;
+        }
+        else
+        {
+            pre->next = thread->next;
+        }
+
+        thread->priority = prio;
+        OSi_InsertThreadToList(thread);
+
+        OSi_RescheduleThread();
+    }
+
+    (void)OS_RestoreInterrupts(enabled);
+
+    return TRUE;
+}
+
+ARM_FUNC void OS_Sleep(u32 msec)
+{
+    OSAlarm alarm;
+
+    OS_CreateAlarm(&alarm);
+    OSThread *volatile p_thread = OSi_GetCurrentThread();
+    OSIntrMode enabled = OS_DisableInterrupts();
+
+    p_thread->alarmForSleep = &alarm;
+
+    OS_SetAlarm(&alarm, OS_MilliSecondsToTicks(msec), &OSi_SleepAlarmCallback,
+                (void*)&p_thread);
+    while (p_thread != NULL)
+    {
+        OS_SleepThread(NULL);
+    }
+    (void)OS_RestoreInterrupts(enabled);
+}
+
+ARM_FUNC static void OSi_SleepAlarmCallback(void *arg)
+{
+    OSThread **pp_thread = (OSThread **)arg;
+    OSThread *p_thread = *pp_thread;
+    *pp_thread = NULL;
+
+    p_thread->alarmForSleep = NULL;
+
+    OS_WakeupThreadDirect(p_thread);
+}
+
+ARM_FUNC OSSwitchThreadCallback OS_SetSwitchThreadCallback(OSSwitchThreadCallback callback)
+{
+    OSIntrMode enabled = OS_DisableInterrupts();
+    OSSwitchThreadCallback prev = OSi_ThreadInfo.switchCallback;
+    OSi_ThreadInfo.switchCallback = callback;
+
+    (void)OS_RestoreInterrupts(enabled);
+    return prev;
+}
+
+ARM_FUNC u32 OS_DisableScheduler(void)
+{
+    OSIntrMode enabled = OS_DisableInterrupts();
+    u32 count;
+
+    if (OSi_RescheduleCount < (u32)-1)
+    {
+        count = OSi_RescheduleCount++;
+    }
+    (void)OS_RestoreInterrupts(enabled);
+
+    return count;
+}
+
+ARM_FUNC u32 OS_EnableScheduler(void)
+{
+    OSIntrMode enabled = OS_DisableInterrupts();
+    u32 count = 0;
+
+    if (OSi_RescheduleCount > 0)
+    {
+        count = OSi_RescheduleCount--;
+    }
+    (void)OS_RestoreInterrupts(enabled);
+
+    return count;
+}
+
+ARM_FUNC void OS_SetThreadDestructor(OSThread *thread, OSThreadDestructor dtor)
+{
+    thread->destructor = dtor;
+}