Merge pull request #155 from red031000/master

arm9 OS_thread

10 files changed, 815 insertions, 989 deletions

diff --git a/arm9/asm/OS_thread.s b/arm9/asm/OS_thread.s
deleted file mode 100644
index 05c0f5bc..00000000
--- a/arm9/asm/OS_thread.s
+++ /dev/null
@@ -1,985 +0,0 @@
-    .include "asm/macros.inc"
-    .include "global.inc"
-
-	.section .bss
-
-	.global OSi_StackForDestructor
-OSi_StackForDestructor: ; 0x021D3480
-	.space 0x4
-
-	.global OSi_RescheduleCount
-OSi_RescheduleCount: ; 0x021D3484
-	.space 0x4
-
-	.global OSi_ThreadIdCount
-OSi_ThreadIdCount: ; 0x021D3488
-	.space 0x4
-
-	.global OSi_SystemCallbackInSwitchThread
-OSi_SystemCallbackInSwitchThread: ; 0x021D348C
-	.space 0x4
-
-	.global OSi_CurrentThreadPtr
-OSi_CurrentThreadPtr: ; 0x021D3490
-	.space 0x4
-
-	.global OSi_IsThreadInitialized
-OSi_IsThreadInitialized: ; 0x021D3494
-	.space 0x4
-
-	.global OSi_ThreadInfo
-OSi_ThreadInfo: ; 0x021D3498
-	.space 0x4
-
-	.global UNK_021D349C
-UNK_021D349C: ; 0x021D349C
-	.space 0xc
-
-	.global OSi_IdleThread
-OSi_IdleThread: ; 0x021D34A8
-	.space 0xc0
-
-	.global OSi_LauncherThread
-OSi_LauncherThread: ; 0x021D3568
-	.space 0xC0
-
-	.global OSi_IdleThreadStack
-OSi_IdleThreadStack: ; 0x021D3628
-	.space 0xC8
-
-    .text
-
-	arm_func_start OS_IsThreadAvailable
-OS_IsThreadAvailable: ; 0x020CAED0
-	ldr r0, _020CAEDC ; =OSi_IsThreadInitialized
-	ldr r0, [r0, #0x0]
-	bx lr
-	.balign 4
-_020CAEDC: .word OSi_IsThreadInitialized
-
-	arm_func_start OS_SetThreadDestructor
-OS_SetThreadDestructor:
-	str r1, [r0, #0xb4]
-	bx lr
-
-	arm_func_start OS_EnableScheduler
-OS_EnableScheduler: ; 0x020CAEE8
-	stmdb sp!, {r4,lr}
-	bl OS_DisableInterrupts
-	ldr r1, _020CAF1C ; =OSi_RescheduleCount
-	mov r4, #0x0
-	ldr r3, [r1, #0x0]
-	cmp r3, #0x0
-	subne r2, r3, #0x1
-	movne r4, r3
-	strne r2, [r1, #0x0]
-	bl OS_RestoreInterrupts
-	mov r0, r4
-	ldmia sp!, {r4,lr}
-	bx lr
-	.balign 4
-_020CAF1C: .word OSi_RescheduleCount
-
-	arm_func_start OS_DisableScheduler
-OS_DisableScheduler: ; 0x020CAF20
-	stmdb sp!, {r4,lr}
-	bl OS_DisableInterrupts
-	ldr r2, _020CAF54 ; =OSi_RescheduleCount
-	mvn r1, #0x0
-	ldr r3, [r2, #0x0]
-	cmp r3, r1
-	addcc r1, r3, #0x1
-	movcc r4, r3
-	strcc r1, [r2, #0x0]
-	bl OS_RestoreInterrupts
-	mov r0, r4
-	ldmia sp!, {r4,lr}
-	bx lr
-	.balign 4
-_020CAF54: .word OSi_RescheduleCount
-
-	arm_func_start OSi_IdleThreadProc
-OSi_IdleThreadProc: ; 0x020CAF58
-	stmdb sp!, {lr}
-	sub sp, sp, #0x4
-	bl OS_EnableInterrupts
-_020CAF64:
-	bl OS_Halt
-	b _020CAF64
-
-	arm_func_start OS_SetSwitchThreadCallback
-OS_SetSwitchThreadCallback: ; 0x020CAF6C
-	stmdb sp!, {r4-r5,lr}
-	sub sp, sp, #0x4
-	mov r5, r0
-	bl OS_DisableInterrupts
-	ldr r1, _020CAF9C ; =OSi_ThreadInfo
-	ldr r4, [r1, #0xc]
-	str r5, [r1, #0xc]
-	bl OS_RestoreInterrupts
-	mov r0, r4
-	add sp, sp, #0x4
-	ldmia sp!, {r4-r5,lr}
-	bx lr
-	.balign 4
-_020CAF9C: .word OSi_ThreadInfo
-
-	arm_func_start OSi_SleepAlarmCallback
-OSi_SleepAlarmCallback: ; 0x020CAFA0
-	ldr r2, [r0, #0x0]
-	mov r1, #0x0
-	str r1, [r0, #0x0]
-	ldr ip, _020CAFBC ; =OS_WakeupThreadDirect
-	mov r0, r2
-	str r1, [r2, #0xb0]
-	bx r12
-	.balign 4
-_020CAFBC: .word OS_WakeupThreadDirect
-
-	arm_func_start OS_Sleep
-OS_Sleep: ; 0x020CAFC0
-	stmdb sp!, {r4-r5,lr}
-	sub sp, sp, #0x34
-	mov r4, r0
-	add r0, sp, #0x8
-	bl OS_CreateAlarm
-	ldr r0, _020CB060 ; =OSi_CurrentThreadPtr
-	ldr r0, [r0, #0x0]
-	ldr r0, [r0, #0x0]
-	str r0, [sp, #0x4]
-	bl OS_DisableInterrupts
-	ldr r1, _020CB064 ; =0x000082EA
-	mov r2, #0x0
-	umull r5, r3, r4, r1
-	mla r3, r4, r2, r3
-	mla r3, r2, r1, r3
-	mov r1, r5, lsr #0x6
-	mov r4, r0
-	ldr r5, [sp, #0x4]
-	add r0, sp, #0x8
-	add r2, sp, #0x4
-	str r0, [r5, #0xb0]
-	str r2, [sp, #0x0]
-	mov r2, r3, lsr #0x6
-	orr r1, r1, r3, lsl #0x1a
-	ldr r3, _020CB068 ; =OSi_SleepAlarmCallback
-	bl OS_SetAlarm
-	ldr r0, [sp, #0x4]
-	cmp r0, #0x0
-	beq _020CB04C
-	mov r5, #0x0
-_020CB038:
-	mov r0, r5
-	bl OS_SleepThread
-	ldr r0, [sp, #0x4]
-	cmp r0, #0x0
-	bne _020CB038
-_020CB04C:
-	mov r0, r4
-	bl OS_RestoreInterrupts
-	add sp, sp, #0x34
-	ldmia sp!, {r4-r5,lr}
-	bx lr
-	.balign 4
-_020CB060: .word OSi_CurrentThreadPtr
-_020CB064: .word 0x000082EA
-_020CB068: .word OSi_SleepAlarmCallback
-
-	arm_func_start OS_GetThreadPriority
-OS_GetThreadPriority: ; 0x020CB06C
-	ldr r0, [r0, #0x70]
-	bx lr
-
-	arm_func_start OS_SetThreadPriority
-OS_SetThreadPriority: ; 0x020CB074
-	stmdb sp!, {r4-r8,lr}
-	ldr r2, _020CB120 ; =OSi_ThreadInfo
-	mov r6, r0
-	mov r5, r1
-	ldr r8, [r2, #0x8]
-	mov r7, #0x0
-	bl OS_DisableInterrupts
-	mov r4, r0
-	b _020CB0A0
-_020CB098:
-	mov r7, r8
-	ldr r8, [r8, #0x68]
-_020CB0A0:
-	cmp r8, #0x0
-	beq _020CB0B0
-	cmp r8, r6
-	bne _020CB098
-_020CB0B0:
-	cmp r8, #0x0
-	beq _020CB0C4
-	ldr r0, _020CB124 ; =OSi_IdleThread
-	cmp r8, r0
-	bne _020CB0D8
-_020CB0C4:
-	mov r0, r4
-	bl OS_RestoreInterrupts
-_020CB0CC:
-	mov r0, #0x0
-	ldmia sp!, {r4-r8,lr}
-	bx lr
-_020CB0D8:
-	ldr r0, [r8, #0x70]
-	cmp r0, r5
-	beq _020CB10C
-	cmp r7, #0x0
-	ldreq r1, [r6, #0x68]
-	ldreq r0, _020CB120 ; =OSi_ThreadInfo
-	streq r1, [r0, #0x8]
-	ldrne r0, [r6, #0x68]
-	strne r0, [r7, #0x68]
-	mov r0, r6
-	str r5, [r6, #0x70]
-	bl OSi_InsertThreadToList
-	bl OSi_RescheduleThread
-_020CB10C:
-	mov r0, r4
-	bl OS_RestoreInterrupts
-	mov r0, #0x1
-	ldmia sp!, {r4-r8,lr}
-	bx lr
-	.balign 4
-_020CB120: .word OSi_ThreadInfo
-_020CB124: .word OSi_IdleThread
-
-	arm_func_start OS_YieldThread
-OS_YieldThread: ; 0x020CB128
-	stmdb sp!, {r4-r8,lr}
-	ldr r0, _020CB1E0 ; =OSi_ThreadInfo
-	mov r7, #0x0
-	mov r6, r7
-	mov r5, r7
-	ldr r8, [r0, #0x4]
-	bl OS_DisableInterrupts
-	ldr r1, _020CB1E0 ; =OSi_ThreadInfo
-	mov r4, r0
-	ldr r2, [r1, #0x8]
-	mov r0, r7
-	cmp r2, #0x0
-	beq _020CB188
-	ldr r1, [r8, #0x70]
-_020CB160:
-	cmp r2, r8
-	moveq r7, r0
-	ldr r0, [r2, #0x70]
-	cmp r1, r0
-	moveq r6, r2
-	mov r0, r2
-	ldr r2, [r2, #0x68]
-	addeq r5, r5, #0x1
-	cmp r2, #0x0
-	bne _020CB160
-_020CB188:
-	cmp r5, #0x1
-	ble _020CB198
-	cmp r6, r8
-	bne _020CB1A8
-_020CB198:
-	mov r0, r4
-	bl OS_RestoreInterrupts
-	ldmia sp!, {r4-r8,lr}
-	bx lr
-_020CB1A8:
-	cmp r7, #0x0
-	ldreq r1, [r8, #0x68]
-	ldreq r0, _020CB1E0 ; =OSi_ThreadInfo
-	streq r1, [r0, #0x8]
-	ldrne r0, [r8, #0x68]
-	strne r0, [r7, #0x68]
-	ldr r0, [r6, #0x68]
-	str r0, [r8, #0x68]
-	str r8, [r6, #0x68]
-	bl OSi_RescheduleThread
-	mov r0, r4
-	bl OS_RestoreInterrupts
-	ldmia sp!, {r4-r8,lr}
-	bx lr
-	.balign 4
-_020CB1E0: .word OSi_ThreadInfo
-
-	arm_func_start OS_RescheduleThread
-OS_RescheduleThread: ; 0x020CB1E4
-	stmdb sp!, {r4,lr}
-	bl OS_DisableInterrupts
-	mov r4, r0
-	bl OSi_RescheduleThread
-	mov r0, r4
-	bl OS_RestoreInterrupts
-	ldmia sp!, {r4,lr}
-	bx lr
-
-	arm_func_start OS_SelectThread
-OS_SelectThread: ; 0x020CB204
-	ldr r0, _020CB22C ; =OSi_ThreadInfo
-	ldr r0, [r0, #0x8]
-	b _020CB214
-_020CB210:
-	ldr r0, [r0, #0x68]
-_020CB214:
-	cmp r0, #0x0
-	bxeq lr
-	ldr r1, [r0, #0x64]
-	cmp r1, #0x1
-	bne _020CB210
-	bx lr
-	.balign 4
-_020CB22C: .word OSi_ThreadInfo
-
-	arm_func_start OS_WakeupThreadDirect
-OS_WakeupThreadDirect: ; 0x020CB230
-	stmdb sp!, {r4-r5,lr}
-	sub sp, sp, #0x4
-	mov r5, r0
-	bl OS_DisableInterrupts
-	mov r1, #0x1
-	mov r4, r0
-	str r1, [r5, #0x64]
-	bl OSi_RescheduleThread
-	mov r0, r4
-	bl OS_RestoreInterrupts
-	add sp, sp, #0x4
-	ldmia sp!, {r4-r5,lr}
-	bx lr
-
-	arm_func_start OS_WakeupThread
-OS_WakeupThread: ; 0x020CB264
-	stmdb sp!, {r4-r7,lr}
-	sub sp, sp, #0x4
-	mov r5, r0
-	bl OS_DisableInterrupts
-	ldr r1, [r5, #0x0]
-	mov r4, r0
-	cmp r1, #0x0
-	beq _020CB2D0
-	cmp r1, #0x0
-	beq _020CB2BC
-	mov r7, #0x1
-	mov r6, #0x0
-_020CB294:
-	mov r0, r5
-	bl OSi_RemoveLinkFromQueue
-	str r7, [r0, #0x64]
-	str r6, [r0, #0x78]
-	str r6, [r0, #0x80]
-	ldr r1, [r0, #0x80]
-	str r1, [r0, #0x7c]
-	ldr r0, [r5, #0x0]
-	cmp r0, #0x0
-	bne _020CB294
-_020CB2BC:
-	mov r0, #0x0
-	str r0, [r5, #0x4]
-	ldr r0, [r5, #0x4]
-	str r0, [r5, #0x0]
-	bl OSi_RescheduleThread
-_020CB2D0:
-	mov r0, r4
-	bl OS_RestoreInterrupts
-	add sp, sp, #0x4
-	ldmia sp!, {r4-r7,lr}
-	bx lr
-
-	arm_func_start OS_SleepThread
-OS_SleepThread:
-	stmdb sp!, {r4-r6,lr}
-	mov r6, r0
-	bl OS_DisableInterrupts
-	ldr r1, _020CB334 ; =OSi_CurrentThreadPtr
-	mov r5, r0
-	ldr r0, [r1, #0x0]
-	cmp r6, #0x0
-	ldr r4, [r0, #0x0]
-	beq _020CB318
-	mov r0, r6
-	mov r1, r4
-	str r6, [r4, #0x78]
-	bl OSi_InsertLinkToQueue
-_020CB318:
-	mov r0, #0x0
-	str r0, [r4, #0x64]
-	bl OSi_RescheduleThread
-	mov r0, r5
-	bl OS_RestoreInterrupts
-	ldmia sp!, {r4-r6,lr}
-	bx lr
-	.balign 4
-_020CB334: .word OSi_CurrentThreadPtr
-
-	arm_func_start OS_IsThreadTerminated
-OS_IsThreadTerminated: ; 0x020CB338
-	ldr r0, [r0, #0x64]
-	cmp r0, #0x2
-	moveq r0, #0x1
-	movne r0, #0x0
-	bx lr
-
-	arm_func_start OS_JoinThread
-OS_JoinThread: ; 0x020CB34C
-	stmdb sp!, {r4-r5,lr}
-	sub sp, sp, #0x4
-	mov r5, r0
-	bl OS_DisableInterrupts
-	ldr r1, [r5, #0x64]
-	mov r4, r0
-	cmp r1, #0x2
-	beq _020CB374
-	add r0, r5, #0x9c
-	bl OS_SleepThread
-_020CB374:
-	mov r0, r4
-	bl OS_RestoreInterrupts
-	add sp, sp, #0x4
-	ldmia sp!, {r4-r5,lr}
-	bx lr
-
-	arm_func_start OSi_CancelThreadAlarmForSleep
-OSi_CancelThreadAlarmForSleep: ; 0x020CB388
-	stmdb sp!, {lr}
-	sub sp, sp, #0x4
-	ldr r0, [r0, #0xb0]
-	cmp r0, #0x0
-	addeq sp, sp, #0x4
-	ldmeqia sp!, {lr}
-	bxeq lr
-	bl OS_CancelAlarm
-	add sp, sp, #0x4
-	ldmia sp!, {lr}
-	bx lr
-
-	arm_func_start OS_DestroyThread
-OS_DestroyThread: ; 0x020CB3B4
-	stmdb sp!, {r4-r5,lr}
-	sub sp, sp, #0x4
-	mov r5, r0
-	bl OS_DisableInterrupts
-	ldr r1, _020CB438 ; =OSi_ThreadInfo
-	mov r4, r0
-	ldr r0, [r1, #0x4]
-	cmp r0, r5
-	bne _020CB3DC
-	bl OSi_ExitThread_Destroy
-_020CB3DC:
-	bl OS_DisableScheduler
-	mov r0, r5
-	bl OSi_UnlockAllMutex
-	mov r0, r5
-	bl OSi_CancelThreadAlarmForSleep
-	ldr r0, [r5, #0x78]
-	cmp r0, #0x0
-	beq _020CB404
-	mov r1, r5
-	bl OSi_RemoveSpecifiedLinkFromQueue
-_020CB404:
-	mov r0, r5
-	bl OSi_RemoveThreadFromList
-	mov r1, #0x2
-	add r0, r5, #0x9c
-	str r1, [r5, #0x64]
-	bl OS_WakeupThread
-	bl OS_EnableScheduler
-	mov r0, r4
-	bl OS_RestoreInterrupts
-	bl OS_RescheduleThread
-	add sp, sp, #0x4
-	ldmia sp!, {r4-r5,lr}
-	bx lr
-	.balign 4
-_020CB438: .word OSi_ThreadInfo
-
-	arm_func_start OSi_ExitThread_Destroy
-OSi_ExitThread_Destroy: ; 0x020CB43C
-	stmdb sp!, {r4,lr}
-	ldr r0, _020CB498 ; =OSi_CurrentThreadPtr
-	ldr r0, [r0, #0x0]
-	ldr r4, [r0, #0x0]
-	bl OS_DisableScheduler
-	mov r0, r4
-	bl OSi_UnlockAllMutex
-	ldr r0, [r4, #0x78]
-	cmp r0, #0x0
-	beq _020CB46C
-	mov r1, r4
-	bl OSi_RemoveSpecifiedLinkFromQueue
-_020CB46C:
-	mov r0, r4
-	bl OSi_RemoveThreadFromList
-	mov r1, #0x2
-	add r0, r4, #0x9c
-	str r1, [r4, #0x64]
-	bl OS_WakeupThread
-	bl OS_EnableScheduler
-	bl OS_RescheduleThread
-	bl OS_Terminate
-	ldmia sp!, {r4,lr}
-	bx lr
-	.balign 4
-_020CB498: .word OSi_CurrentThreadPtr
-
-	arm_func_start OSi_ExitThread_Destroy2
-OSi_ExitThread_Destroy2: ; 0x020CB49C
-	stmdb sp!, {lr}
-	sub sp, sp, #0x4
-	ldr r1, _020CB4DC ; =OSi_CurrentThreadPtr
-	ldr r1, [r1, #0x0]
-	ldr r3, [r1, #0x0]
-	ldr r2, [r3, #0xb4]
-	cmp r2, #0x0
-	beq _020CB4CC
-	mov r1, #0x0
-	str r1, [r3, #0xb4]
-	blx r2
-	bl OS_DisableInterrupts
-_020CB4CC:
-	bl OSi_ExitThread_Destroy
-	add sp, sp, #0x4
-	ldmia sp!, {lr}
-	bx lr
-	.balign 4
-_020CB4DC: .word OSi_CurrentThreadPtr
-
-	arm_func_start OSi_ExitThread_ArgSpecified
-OSi_ExitThread_ArgSpecified: ; 0x020CB4E0
-	stmdb sp!, {r4-r5,lr}
-	sub sp, sp, #0x4
-	ldr r2, _020CB548 ; =OSi_StackForDestructor
-	mov r5, r0
-	ldr r2, [r2, #0x0]
-	mov r4, r1
-	cmp r2, #0x0
-	beq _020CB534
-	ldr r1, _020CB54C ; =OSi_ExitThread_Destroy2
-	bl OS_InitContext
-	str r4, [r5, #0x4]
-	ldr r1, [r5, #0x0]
-	mov r0, r5
-	orr r1, r1, #0x80
-	str r1, [r5, #0x0]
-	mov r1, #0x1
-	str r1, [r5, #0x64]
-	bl OS_LoadContext
-	add sp, sp, #0x4
-	ldmia sp!, {r4-r5,lr}
-	bx lr
-_020CB534:
-	mov r0, r4
-	bl OSi_ExitThread_Destroy2
-	add sp, sp, #0x4
-	ldmia sp!, {r4-r5,lr}
-	bx lr
-	.balign 4
-_020CB548: .word OSi_StackForDestructor
-_020CB54C: .word OSi_ExitThread_Destroy2
-
-	arm_func_start OS_ExitThread
-OS_ExitThread: ; 0x020CB550
-	stmdb sp!, {lr}
-	sub sp, sp, #0x4
-	bl OS_DisableInterrupts
-	ldr r0, _020CB578 ; =OSi_ThreadInfo
-	mov r1, #0x0
-	ldr r0, [r0, #0x4]
-	bl OSi_ExitThread_ArgSpecified
-	add sp, sp, #0x4
-	ldmia sp!, {lr}
-	bx lr
-	.balign 4
-_020CB578: .word OSi_ThreadInfo
-
-	arm_func_start OS_CreateThread
-OS_CreateThread: ; 0x020CB57C
-	stmdb sp!, {r4-r8,lr}
-	mov r8, r0
-	mov r5, r1
-	mov r7, r2
-	mov r6, r3
-	bl OS_DisableInterrupts
-	mov r4, r0
-	bl OSi_GetUnusedThreadId
-	ldr r2, [sp, #0x1c]
-	mov r1, #0x0
-	str r2, [r8, #0x70]
-	str r0, [r8, #0x6c]
-	str r1, [r8, #0x64]
-	str r1, [r8, #0x74]
-	mov r0, r8
-	bl OSi_InsertThreadToList
-	mov r1, r5
-	str r6, [r8, #0x94]
-	ldr r0, [sp, #0x18]
-	mov r12, #0x0
-	sub r5, r6, r0
-	sub r2, r6, #0x4
-	str r5, [r8, #0x90]
-	str r12, [r8, #0x98]
-	ldr r3, _020CB678 ; =0xFDDB597D
-	ldr r0, [r8, #0x94]
-	ldr r6, _020CB67C ; =0x7BF9DD5B
-	str r3, [r0, #-0x4]
-	ldr r3, [r8, #0x90]
-	mov r0, r8
-	str r6, [r3, #0x0]
-	str r12, [r8, #0xa0]
-	ldr r3, [r8, #0xa0]
-	str r3, [r8, #0x9c]
-	bl OS_InitContext
-	str r7, [r8, #0x4]
-	add r1, r5, #0x4
-	ldr r2, _020CB680 ; =OS_ExitThread
-	mov r0, #0x0
-	str r2, [r8, #0x3c]
-	ldr r2, [sp, #0x18]
-	sub r2, r2, #0x8
-	bl MIi_CpuClear32
-	mov r1, #0x0
-	str r1, [r8, #0x84]
-	str r1, [r8, #0x88]
-	str r1, [r8, #0x8c]
-	mov r0, r8
-	bl OS_SetThreadDestructor
-_020CB640:
-	mov r0, #0x0
-	str r0, [r8, #0x78]
-	str r0, [r8, #0x80]
-	ldr r2, [r8, #0x80]
-	add r1, r8, #0xA4
-	str r2, [r8, #0x7C]
-	mov r2, #0xC
-	bl MIi_CpuClear32
-	mov r0, r4
-	mov r1, #0x0
-	str r1, [r8, #0xB0]
-	bl OS_RestoreInterrupts
-	ldmia sp!, {r4-r8, lr}
-	bx lr
-_020CB678: .word 0xFDDB597D
-_020CB67C: .word 0x7BF9DD5B
-_020CB680: .word OS_ExitThread
-
-	arm_func_start OS_InitThread
-OS_InitThread: ; 0x020CB684
-	stmdb sp!, {r4-r6,lr}
-	sub sp, sp, #0x8
-	ldr r3, _020CB7A4 ; =OSi_IsThreadInitialized
-	ldr r0, [r3, #0x0]
-	cmp r0, #0x0
-	addne sp, sp, #0x8
-	ldmneia sp!, {r4-r6,lr}
-	bxne lr
-	ldr ip, _020CB7A8 ; =0x00000000
-	ldr r1, _020CB7AC ; =OSi_LauncherThread
-	mov lr, #0x0
-	ldr r0, _020CB7B0 ; =OSi_ThreadInfo
-	mov r6, #0x1
-	mov r4, #0x10
-	str r1, [r0, #0x8]
-	str r1, [r0, #0x4]
-	cmp r12, #0x0
-	ldrle r0, _020CB7B4 ; =0x027E0080
-	str r4, [r1, #0x70]
-	suble r4, r0, r12
-	str lr, [r1, #0x6c]
-	str r6, [r1, #0x64]
-	str lr, [r1, #0x68]
-	str lr, [r1, #0x74]
-	ldrgt r1, _020CB7B8 ; =0x027E0000
-	ldrgt r0, _020CB7BC ; =0x00000400
-	addgt r1, r1, #0x3f80
-	subgt r0, r1, r0
-	subgt r4, r0, r12
-	ldr r1, _020CB7B8 ; =0x027E0000
-	ldr r5, _020CB7C0 ; =UNK_021D349C
-	ldr r2, _020CB7C4 ; =OSi_CurrentThreadPtr
-	ldr r0, _020CB7BC ; =0x00000400
-	str r5, [r2, #0x0]
-	ldr r2, _020CB7AC ; =OSi_LauncherThread
-	add r1, r1, #0x3f80
-	str r6, [r3, #0x0]
-	sub r3, r1, r0
-	mov r0, #0x0
-	ldr r1, _020CB7C8 ; =0xFDDB597D
-	str r3, [r2, #0x94]
-	str r4, [r2, #0x90]
-	str r0, [r2, #0x98]
-	str r1, [r3, #-0x4]
-	ldr r3, [r2, #0x90]
-	ldr ip, _020CB7CC ; =0x7BF9DD5B
-	ldr r1, _020CB7B0 ; =OSi_ThreadInfo
-	str r12, [r3, #0x0]
-	ldr r3, _020CB7D0 ; =0x027FFFA0
-	str r0, [r2, #0xa0]
-	str r0, [r2, #0x9c]
-	strh r0, [r1, #0x0]
-	strh r0, [r1, #0x2]
-	str r1, [r3, #0x0]
-	bl OS_SetSwitchThreadCallback
-	mov r2, #0xc8
-	str r2, [sp, #0x0]
-	mov r12, #0x1f
-	ldr r0, _020CB7D4 ; =OSi_IdleThread
-	ldr r1, _020CB7D8 ; =OSi_IdleThreadProc
-	ldr r3, _020CB7DC ; =OSi_Initialized
-	mov r2, #0x0
-	str r12, [sp, #0x4]
-	bl OS_CreateThread
-	ldr r0, _020CB7D4 ; =OSi_IdleThread
-	mov r2, #0x20
-	mov r1, #0x1
-	str r2, [r0, #0x70]
-	str r1, [r0, #0x64]
-	add sp, sp, #0x8
-	ldmia sp!, {r4-r6,lr}
-	bx lr
-	.balign 4
-_020CB7A4: .word OSi_IsThreadInitialized
-_020CB7A8: .word SDK_SYS_STACKSIZE
-_020CB7AC: .word OSi_LauncherThread
-_020CB7B0: .word OSi_ThreadInfo
-_020CB7B4: .word SDK_SECTION_ARENA_DTCM_START
-_020CB7B8: .word SDK_AUTOLOAD_DTCM_START
-_020CB7BC: .word SDK_IRQ_STACKSIZE
-_020CB7C0: .word OSi_ThreadInfo+4
-_020CB7C4: .word OSi_CurrentThreadPtr
-_020CB7C8: .word 0xFDDB597D
-_020CB7CC: .word 0x7BF9DD5B
-_020CB7D0: .word 0x027FFFA0
-_020CB7D4: .word OSi_IdleThread
-_020CB7D8: .word OSi_IdleThreadProc
-_020CB7DC: .word OSi_IdleThreadStack+0xc8
-
-	arm_func_start OSi_RescheduleThread
-OSi_RescheduleThread: ; 0x020CB7E0
-	stmdb sp!, {r4-r6,lr}
-	ldr r0, _020CB8BC ; =OSi_RescheduleCount
-	ldr r0, [r0, #0x0]
-	cmp r0, #0x0
-	ldmneia sp!, {r4-r6,lr}
-	bxne lr
-	ldr r4, _020CB8C0 ; =OSi_ThreadInfo
-	ldrh r0, [r4, #0x2]
-	cmp r0, #0x0
-	bne _020CB814
-	bl OS_GetProcMode
-	cmp r0, #0x12
-	bne _020CB824
-_020CB814:
-	mov r0, #0x1
-	strh r0, [r4, #0x0]
-	ldmia sp!, {r4-r6,lr}
-	bx lr
-_020CB824:
-	ldr r0, _020CB8C4 ; =OSi_CurrentThreadPtr
-	ldr r0, [r0, #0x0]
-	ldr r6, [r0, #0x0]
-	bl OS_SelectThread
-	mov r5, r0
-	cmp r6, r5
-	ldmeqia sp!, {r4-r6,lr}
-	bxeq lr
-	cmp r5, #0x0
-	ldmeqia sp!, {r4-r6,lr}
-	bxeq lr
-	ldr r0, [r6, #0x64]
-	cmp r0, #0x2
-	beq _020CB870
-	mov r0, r6
-	bl OS_SaveContext
-_020CB864:
-	cmp r0, #0x0
-	ldmneia sp!, {r4-r6,lr}
-	bxne lr
-_020CB870:
-	ldr r0, _020CB8C8 ; =OSi_SystemCallbackInSwitchThread
-	ldr r2, [r0, #0x0]
-	cmp r2, #0x0
-	beq _020CB88C
-	mov r0, r6
-	mov r1, r5
-	blx r2
-_020CB88C:
-	ldr r2, [r4, #0xc]
-	cmp r2, #0x0
-	beq _020CB8A4
-	mov r0, r6
-	mov r1, r5
-	blx r2
-_020CB8A4:
-	ldr r1, _020CB8C0 ; =OSi_ThreadInfo
-	mov r0, r5
-	str r5, [r1, #0x4]
-	bl OS_LoadContext
-	ldmia sp!, {r4-r6,lr}
-	bx lr
-	.balign 4
-_020CB8BC: .word OSi_RescheduleCount
-_020CB8C0: .word OSi_ThreadInfo
-_020CB8C4: .word OSi_CurrentThreadPtr
-_020CB8C8: .word OSi_SystemCallbackInSwitchThread
-
-	arm_func_start OSi_RemoveThreadFromList
-OSi_RemoveThreadFromList: ; 0x020CB8CC
-	ldr r1, _020CB910 ; =OSi_ThreadInfo
-	mov r2, #0x0
-	ldr r1, [r1, #0x8]
-	b _020CB8E4
-_020CB8DC:
-	mov r2, r1
-	ldr r1, [r1, #0x68]
-_020CB8E4:
-	cmp r1, #0x0
-	beq _020CB8F4
-	cmp r1, r0
-	bne _020CB8DC
-_020CB8F4:
-	cmp r2, #0x0
-	ldreq r1, [r0, #0x68]
-	ldreq r0, _020CB910 ; =OSi_ThreadInfo
-	streq r1, [r0, #0x8]
-	ldrne r0, [r0, #0x68]
-	strne r0, [r2, #0x68]
-	bx lr
-	.balign 4
-_020CB910: .word OSi_ThreadInfo
-
-	arm_func_start OSi_InsertThreadToList
-OSi_InsertThreadToList: ; 0x020CB914
-	stmdb sp!, {lr}
-	sub sp, sp, #0x4
-	ldr r1, _020CB978 ; =OSi_ThreadInfo
-	mov r12, #0x0
-	ldr r3, [r1, #0x8]
-	mov lr, r3
-	b _020CB938
-_020CB930:
-	mov r12, lr
-	ldr lr, [lr, #0x68]
-_020CB938:
-	cmp lr, #0x0
-	beq _020CB950
-	ldr r2, [lr, #0x70]
-	ldr r1, [r0, #0x70]
-	cmp r2, r1
-	blo _020CB930
-_020CB950:
-	cmp r12, #0x0
-	ldreq r1, _020CB978 ; =OSi_ThreadInfo
-	streq r3, [r0, #0x68]
-	streq r0, [r1, #0x8]
-	ldrne r1, [r12, #0x68]
-	strne r1, [r0, #0x68]
-	strne r0, [r12, #0x68]
-	add sp, sp, #0x4
-	ldmia sp!, {lr}
-	bx lr
-	.balign 4
-_020CB978: .word OSi_ThreadInfo
-
-	arm_func_start OSi_RemoveMutexLinkFromQueue
-OSi_RemoveMutexLinkFromQueue: ; 0x020CB97C
-	ldr r2, [r0, #0x0]
-	cmp r2, #0x0
-	beq _020CB9A4
-	ldr r1, [r2, #0x10]
-	str r1, [r0, #0x0]
-	cmp r1, #0x0
-	movne r0, #0x0
-	strne r0, [r1, #0x14]
-	moveq r1, #0x0
-	streq r1, [r0, #0x4]
-_020CB9A4:
-	mov r0, r2
-	bx lr
-
-	arm_func_start OSi_RemoveSpecifiedLinkFromQueue
-OSi_RemoveSpecifiedLinkFromQueue: ; 0x020CB9AC
-	ldr r2, [r0, #0x0]
-	mov r12, r2
-	cmp r2, #0x0
-	beq _020CB9F8
-_020CB9BC:
-	cmp r12, r1
-	ldr r3, [r12, #0x80]
-	bne _020CB9EC
-	cmp r2, r12
-	ldr r2, [r12, #0x7c]
-	streq r3, [r0, #0x0]
-	strne r3, [r2, #0x80]
-	ldr r1, [r0, #0x4]
-	cmp r1, r12
-	streq r2, [r0, #0x4]
-	strne r2, [r3, #0x7c]
-	b _020CB9F8
-_020CB9EC:
-	mov r12, r3
-	cmp r3, #0x0
-	bne _020CB9BC
-_020CB9F8:
-	mov r0, r12
-	bx lr
-
-	arm_func_start OSi_RemoveLinkFromQueue
-OSi_RemoveLinkFromQueue: ; 0x020CBA00
-	ldr r2, [r0, #0x0]
-	cmp r2, #0x0
-	beq _020CBA2C
-	ldr r1, [r2, #0x80]
-	str r1, [r0, #0x0]
-	cmp r1, #0x0
-	movne r0, #0x0
-	strne r0, [r1, #0x7c]
-	moveq r1, #0x0
-	streq r1, [r0, #0x4]
-	streq r1, [r2, #0x78]
-_020CBA2C:
-	mov r0, r2
-	bx lr
-
-	arm_func_start OSi_InsertLinkToQueue
-OSi_InsertLinkToQueue:
-	ldr r12, [r0, #0x0]
-	b _020CBA48
-_020CBA3C:
-	cmp r12, r1
-	bxeq lr
-	ldr r12, [r12, #0x80]
-_020CBA48:
-	cmp r12, #0x0
-	beq _020CBA60
-	ldr r3, [r12, #0x70]
-	ldr r2, [r1, #0x70]
-	cmp r3, r2
-	bls _020CBA3C
-_020CBA60:
-	cmp r12, #0x0
-	bne _020CBA8C
-	ldr r2, [r0, #0x4]
-	cmp r2, #0x0
-	streq r1, [r0, #0x0]
-	strne r1, [r2, #0x80]
-	str r2, [r1, #0x7c]
-	mov r2, #0x0
-	str r2, [r1, #0x80]
-	str r1, [r0, #0x4]
-	bx lr
-_020CBA8C:
-	ldr r2, [r12, #0x7c]
-	cmp r2, #0x0
-	streq r1, [r0, #0x0]
-	strne r1, [r2, #0x80]
-	str r2, [r1, #0x7c]
-	str r12, [r1, #0x80]
-	str r1, [r12, #0x7c]
-	bx lr
-
-	arm_func_start OSi_GetUnusedThreadId
-OSi_GetUnusedThreadId: ; 0x020CBAAC
-	ldr r1, _020CBAC0 ; =OSi_ThreadIdCount
-	ldr r0, [r1, #0x0]
-	add r0, r0, #0x1
-	str r0, [r1, #0x0]
-	bx lr
-	.balign 4
-_020CBAC0: .word OSi_ThreadIdCount
diff --git a/arm9/lib/include/MI_memory.h b/arm9/lib/include/MI_memory.h
index 0ebb3560..bf6c0ca5 100644
--- a/arm9/lib/include/MI_memory.h
+++ b/arm9/lib/include/MI_memory.h
@@ -1,6 +1,8 @@
 #ifndef NITRO_MI_MEMORY_H_
 #define NITRO_MI_MEMORY_H_
 
+#include "nitro/types.h"
+
 void MI_CpuFill8(void *dest, u8 data, u32 size);
 void MI_CpuCopy8(void const *src, void *dest, u32 size);
 static inline void MI_CpuClear8(void *dest, u32 size) {
@@ -8,4 +10,16 @@ static inline void MI_CpuClear8(void *dest, u32 size) {
 }
 void MIi_CpuCopy16(const void *src, void *dst, u32 size);
 
+void MIi_CpuClear32(u32 data, void *destp, u32 size);
+
+static inline void MI_CpuFill32(void *dest, u32 data, u32 size)
+{
+    MIi_CpuClear32(data, dest, size);
+}
+
+static inline void MI_CpuClear32(void *dest, u32 size)
+{
+    MI_CpuFill32(dest, 0, size);
+}
+
 #endif //NITRO_MI_MEMORY_H_
diff --git a/arm9/lib/include/OS_alarm.h b/arm9/lib/include/OS_alarm.h
index d6b0c210..a9734d1b 100644
--- a/arm9/lib/include/OS_alarm.h
+++ b/arm9/lib/include/OS_alarm.h
@@ -3,6 +3,7 @@
 
 #include "nitro/types.h"
 #include "OS_thread.h"
+#include "OS_tick.h"
 
 typedef void (*OSAlarmHandler) (void *);
 
@@ -20,4 +21,8 @@ struct OSiAlarm
     OSTick start;
 };
 
+void OS_CancelAlarm(OSAlarm *alarm);
+void OS_CreateAlarm(OSAlarm *alarm);
+void OS_SetAlarm(OSAlarm *alarm, OSTick tick, OSAlarmHandler handler, void *arg);
+
 #endif //POKEDIAMOND_OS_ALARM_H
diff --git a/arm9/lib/include/OS_context.h b/arm9/lib/include/OS_context.h
index cded1acb..7a58a2b1 100644
--- a/arm9/lib/include/OS_context.h
+++ b/arm9/lib/include/OS_context.h
@@ -15,4 +15,8 @@ typedef struct OSContext
     CPContext cp_context;
 } OSContext;
 
+extern u32 OS_SaveContext(OSContext *context);
+extern void OS_LoadContext(OSContext *context);
+extern void OS_InitContext(OSContext *context, u32 func, u32 stack);
+
 #endif //POKEDIAMOND_OS_CONTEXT_H
diff --git a/arm9/lib/include/OS_mutex.h b/arm9/lib/include/OS_mutex.h
index a2cdff23..0fc261a4 100644
--- a/arm9/lib/include/OS_mutex.h
+++ b/arm9/lib/include/OS_mutex.h
@@ -11,4 +11,6 @@ struct OSMutex {
     OSMutexLink link;
 };
 
+void OSi_UnlockAllMutex(OSThread * thread);
+
 #endif //POKEDIAMOND_OS_MUTEX_H
diff --git a/arm9/lib/include/OS_thread.h b/arm9/lib/include/OS_thread.h
index bf62e66b..a80a3b96 100644
--- a/arm9/lib/include/OS_thread.h
+++ b/arm9/lib/include/OS_thread.h
@@ -52,6 +52,8 @@ typedef enum {
     OS_THREAD_STATE_TERMINATED = 2
 } OSThreadState;
 
+typedef void (*OSSwitchThreadCallback) (OSThread *from, OSThread *to);
+
 typedef void (*OSThreadDestructor) (void *);
 
 struct _OSThread
@@ -83,14 +85,67 @@ struct _OSThread
     u32 systemErrno;
 };
 
+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);
+void OS_InitThread(void);
+BOOL OS_IsThreadAvailable(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_DestroyThread(OSThread *thread);
+static void OSi_CancelThreadAlarmForSleep(OSThread *thread);
+void OS_JoinThread(OSThread *thread);
+BOOL OS_IsThreadTerminated(const OSThread *thread);
+void OS_SleepThread(OSThreadQueue *queue);
+void OS_WakeupThread(OSThreadQueue *queue);
+void OS_WakeupThreadDirect(OSThread *thread);
+OSThread *OS_SelectThread(void);
+void OS_RescheduleThread(void);
+void OS_YieldThread(void);
+BOOL OS_SetThreadPriority(OSThread *thread, u32 prio);
+u32 OS_GetThreadPriority(const OSThread *thread);
+void OS_Sleep(u32 msec);
+static void OSi_SleepAlarmCallback(void *arg);
+OSSwitchThreadCallback OS_SetSwitchThreadCallback(OSSwitchThreadCallback callback);
+static void OSi_IdleThreadProc(void *);
+u32 OS_DisableScheduler(void);
+u32 OS_EnableScheduler(void);
+void OS_SetThreadDestructor(OSThread *thread, OSThreadDestructor dtor);
+
 extern OSThreadInfo OSi_ThreadInfo;
 
-void OS_SleepThread(OSThreadQueue * queue);
-void OS_WakeupThread(OSThreadQueue * queue);
+static inline OSThreadInfo *OS_GetThreadInfo(void)
+{
+    return &OSi_ThreadInfo;
+}
+
+static inline BOOL OS_IsThreadRunnable(const OSThread *thread)
+{
+    return thread->state == OS_THREAD_STATE_READY;
+}
 
 static inline void OS_InitThreadQueue(OSThreadQueue * queue)
 {
     queue->head = queue->tail = NULL;
 }
 
+static inline OSThread *OS_GetCurrentThread(void)
+{
+    return OS_GetThreadInfo()->current;
+}
+
+static inline void OS_SetCurrentThread(OSThread *thread)
+{
+    OS_GetThreadInfo()->current = thread;
+}
+
+#define OSi_GetCurrentThread()          (*OSi_CurrentThreadPtr)
+
 #endif //POKEDIAMOND_OS_THREAD_H
diff --git a/arm9/lib/include/OS_tick.h b/arm9/lib/include/OS_tick.h
index fb4ce7e6..f1c7145d 100644
--- a/arm9/lib/include/OS_tick.h
+++ b/arm9/lib/include/OS_tick.h
@@ -1,8 +1,12 @@
 #ifndef POKEDIAMOND_OS_TICK_H
 #define POKEDIAMOND_OS_TICK_H
 
-#include "nitro/types.h"
+#include "consts.h"
 
 typedef u64 OSTick;
 
+#define OS_SYSTEM_CLOCK HW_SYSTEM_CLOCK
+
+#define OS_MilliSecondsToTicks(msec) ((OSTick)(((OS_SYSTEM_CLOCK/1000) * (u64)(msec)) / 64))
+
 #endif //POKEDIAMOND_OS_TICK_H
diff --git a/arm9/lib/include/consts.h b/arm9/lib/include/consts.h
index 885deff0..c09e8cd9 100644
--- a/arm9/lib/include/consts.h
+++ b/arm9/lib/include/consts.h
@@ -27,6 +27,8 @@
 #define HW_C6_PR_2GB               0x3c
 #define HW_C6_PR_4GB               0x3e
 
+#define HW_SYSTEM_CLOCK            33514000
+
 #define PXI_PROC_ARM7              0x01
 
 #define OSi_CONSOLE_NOT_DETECT     0xffffffff
@@ -36,6 +38,12 @@
 #define OS_CONSOLE_SIZE_MASK       0x00000003
 #define OS_CONSOLE_SIZE_4MB        0x00000001
 
+#define OS_THREAD_LAUNCHER_PRIORITY 0x10
+#define OS_THREAD_PRIORITY_MIN     0x00
+#define OS_THREAD_PRIORITY_MAX     0x1F
+
+#define OS_THREAD_SPECIFIC_MAX     0x03
+
 #define OSi_TCM_REGION_BASE_MASK   0xfffff000
 
 #define OSi_TRUNC(n, a)            (((u32) (n)) & ~((a) - 1))
diff --git a/arm9/lib/src/OS_init.c b/arm9/lib/src/OS_init.c
index e1c7e63a..f919f521 100644
--- a/arm9/lib/src/OS_init.c
+++ b/arm9/lib/src/OS_init.c
@@ -10,7 +10,6 @@ extern void OS_InitException(void);
 extern void MI_Init(void);
 extern void OS_InitVAlarm(void);
 extern void OSi_InitVramExclusive(void);
-extern void OS_InitThread(void);
 extern void CTRDG_Init(void);
 extern void CARD_Init(void);
 extern void PM_Init(void);
diff --git a/arm9/lib/src/OS_thread.c b/arm9/lib/src/OS_thread.c
new file mode 100644
index 00000000..2ecde69a
--- /dev/null
+++ b/arm9/lib/src/OS_thread.c
@@ -0,0 +1,720 @@
+#include "function_target.h"
+#include "MI_memory.h"
+#include "OS_thread.h"
+#include "OS_mutex.h"
+#include "OS_alarm.h"
+#include "OS_system.h"
+#include "OS_context.h"
+#include "OS_terminate_proc.h"
+#include "consts.h"
+
+extern void SDK_SYS_STACKSIZE(void);
+extern void SDK_IRQ_STACKSIZE(void);
+
+extern void SDK_SECTION_ARENA_DTCM_START(void);
+
+static s32 OSi_ThreadIdCount = 0;
+
+u32 OSi_RescheduleCount = 0;
+
+void *OSi_StackForDestructor = NULL;
+
+BOOL OSi_IsThreadInitialized = FALSE;
+
+OSThreadInfo OSi_ThreadInfo;
+
+OSThread **OSi_CurrentThreadPtr = NULL;
+
+void *OSi_SystemCallbackInSwitchThread = NULL;
+
+OSThread OSi_LauncherThread;
+OSThread OSi_IdleThread;
+
+u32 OSi_IdleThreadStack[50];
+
+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)SDK_SECTION_ARENA_DTCM_START - (s32)SDK_SYS_STACKSIZE) :
+              (void *)((u32)(HW_DTCM_SVC_STACK - (s32)SDK_IRQ_STACKSIZE) - (s32)SDK_SYS_STACKSIZE);
+
+    OSi_LauncherThread.stackBottom = (u32)(HW_DTCM_SVC_STACK - (s32)SDK_IRQ_STACKSIZE);
+    OSi_LauncherThread.stackTop = (u32)stackLo;
+    OSi_LauncherThread.stackWarningOffset = 0;
+
+    //checksums
+    *(u32 *)(OSi_LauncherThread.stackBottom - sizeof(u32)) = 0xfddb597dUL;
+    *(u32 *)OSi_LauncherThread.stackTop = 0x7bf9dd5bUL;
+
+    OS_InitThreadQueue(&OSi_LauncherThread.joinQueue);
+
+    OSi_ThreadInfo.isNeedRescheduling = FALSE;
+    OSi_ThreadInfo.irqDepth = 0;
+
+    OS_GetSystemWork()->threadinfo_mainp = &OSi_ThreadInfo;
+
+    (void)OS_SetSwitchThreadCallback(NULL);
+
+    OS_CreateThread(&OSi_IdleThread, OSi_IdleThreadProc, NULL,
+            OSi_IdleThreadStack + 50, 200, OS_THREAD_PRIORITY_MAX);
+
+    OSi_IdleThread.priority = OS_THREAD_PRIORITY_MAX + 1;
+    OSi_IdleThread.state = OS_THREAD_STATE_READY;
+}
+
+ARM_FUNC asm BOOL OS_IsThreadAvailable(void)
+{
+    ldr r0, =OSi_IsThreadInitialized
+    ldr r0, [r0, #0]
+    bx lr
+}
+
+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)) = 0xfddb597dUL;
+    *(u32 *)thread->stackTop = 0x7bf9dd5bUL;
+
+    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_DestroyThread(OSThread *thread)
+{
+    OSIntrMode enabled = OS_DisableInterrupts();
+
+    if (OS_GetCurrentThread() == thread)
+    {
+        OSi_ExitThread_Destroy();
+    }
+
+    (void)OS_DisableScheduler();
+
+    OSi_UnlockAllMutex(thread);
+
+    OSi_CancelThreadAlarmForSleep(thread);
+
+    if (thread->queue)
+    {
+        (void)OSi_RemoveSpecifiedLinkFromQueue(thread->queue, thread);
+    }
+
+    OSi_RemoveThreadFromList(thread);
+
+    thread->state = OS_THREAD_STATE_TERMINATED;
+
+    OS_WakeupThread(&thread->joinQueue);
+
+    (void)OS_EnableScheduler();
+    (void)OS_RestoreInterrupts(enabled);
+
+    OS_RescheduleThread();
+}
+
+ARM_FUNC static void OSi_CancelThreadAlarmForSleep(OSThread *thread)
+{
+    OSAlarm *alarm = thread->alarmForSleep;
+
+    if (alarm)
+    {
+        OS_CancelAlarm(alarm);
+    }
+}
+
+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 void OS_YieldThread(void)
+{
+    OSThread *current = OS_GetCurrentThread();
+    OSThread *pre = NULL;
+    OSThread *lastThread = NULL;
+    s32 samePriorityThread = 0;
+    OSIntrMode enabled = OS_DisableInterrupts();
+
+    OSThread *t = OSi_ThreadInfo.list;
+    OSThread *tPre = NULL;
+
+    while (t)
+    {
+        if (t == current)
+        {
+            pre = tPre;
+        }
+
+        if (current->priority == t->priority)
+        {
+            lastThread = t;
+            samePriorityThread++;
+        }
+
+        tPre = t;
+        t = t->next;
+    }
+
+    if (samePriorityThread <= 1 || lastThread == current)
+    {
+        (void)OS_RestoreInterrupts(enabled);
+        return;
+    }
+
+    if (!pre)
+    {
+        OSi_ThreadInfo.list = current->next;
+    }
+    else
+    {
+        pre->next = current->next;
+    }
+
+    current->next = lastThread->next;
+    lastThread->next = current;
+
+    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 u32 OS_GetThreadPriority(const OSThread *thread)
+{
+    return thread->priority;
+}
+
+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 static void OSi_IdleThreadProc(void *)
+{
+    (void)OS_EnableInterrupts();
+    while (1)
+    {
+        OS_Halt();
+    }
+}
+
+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;
+}