17 files changed, 1309 insertions, 673 deletions

diff --git a/arm7/Makefile b/arm7/Makefile
index 1ade0ed4..1b0cd3c3 100644
--- a/arm7/Makefile
+++ b/arm7/Makefile
@@ -196,7 +196,6 @@ DUMMY != mkdir -p $(ALL_DIRS)
 %.pal: ;
 
 # Included files
-baserom.%: ;
 %.h: ;
 %.inc: ;
 
diff --git a/arm7/arm7.lsf b/arm7/arm7.lsf
index 038c37c5..cbffa45a 100644
--- a/arm7/arm7.lsf
+++ b/arm7/arm7.lsf
@@ -68,8 +68,8 @@ Autoload WRAM
 	Object SPI_pm.o
 	Object SPI_mic.o
 	Object CTRDG.o
+	Object CTRDG_sp.o
 	Object wram2.o
-	Object wram_data.o
 	Object wram_bss.o
 }
 
diff --git a/arm7/asm/CARD_pullOut.s b/arm7/asm/CARD_pullOut.s
index ed919647..b4d52c6c 100644
--- a/arm7/asm/CARD_pullOut.s
+++ b/arm7/asm/CARD_pullOut.s
@@ -215,4 +215,3 @@ _038010C0:
 	bx	lr
 _038010CC:	.word	_038099A4
 _038010D0:	.word	FUN_03801048
-	.balign 2, 0 ; Don't pad with nop
diff --git a/arm7/asm/CTRDG_sp.s b/arm7/asm/CTRDG_sp.s
new file mode 100644
index 00000000..72c502c3
--- /dev/null
+++ b/arm7/asm/CTRDG_sp.s
@@ -0,0 +1,542 @@
+	.include "asm/macros.inc"
+	.include "global.inc"
+
+    .section .data
+
+	.global _03807704
+_03807704:
+	.word	0x00000001
+
+	.global _03807708
+_03807708:
+	.word	0xFFFFFFFF
+
+    .section .bss
+
+	.global _03809FBC
+_03809FBC: ;0x03809FBC
+	.space 0x03809FC0 - 0x03809FBC
+
+	.global current_vib
+current_vib: ;0x03809FC0
+	.space 0x03809FC4 - 0x03809FC0
+
+	.global _03809FC4
+_03809FC4: ;0x03809FC4
+	.space 0x03809FC8 - 0x03809FC4
+
+	.global _03809FC8
+_03809FC8: ;0x03809FC8
+	.space 0x03809FCC - 0x03809FC8
+
+	.global _03809FCC
+_03809FCC: ;0x03809FCC
+	.space 0x03809FD0 - 0x03809FCC
+
+	.global _03809FD0
+_03809FD0: ;0x03809FD0
+	.space 0x03809FD4 - 0x03809FD0
+
+	.global _03809FD4
+_03809FD4: ;0x03809FD4
+	.space 0x03809FEC - 0x03809FD4
+
+	.global _03809FEC
+_03809FEC: ;0x03809FEC
+	.space 0x0380A018 - 0x03809FEC
+
+    .section .text
+
+	arm_func_start FUN_03803BD4
+FUN_03803BD4: ;@ 0x03803BD4
+	stmdb	sp!, {r4, r5, r6, r7, lr}
+	sub	sp, sp, #4
+	and	r0, r1, #63	;@ 0x3f
+	cmp	r0, #3
+	bne	_03803C40
+	ldr	r0, _03803C50	;@ =0x01FFFFC0
+	and	r0, r1, r0
+	mov	r2, r0, lsr #6
+	ldr	r1, _03803C54	;@ =0x04000204
+	ldrh	r0, [r1]
+	bic	r0, r0, #96	;@ 0x60
+	orr	r0, r0, r2, lsl #5
+	strh	r0, [r1]
+	mov	r7, #1
+	mov	r6, #17
+	mov	r5, #18
+	mov	r4, #0
+	b	_03803C24
+_03803C1C:
+	mov	r0, r7
+	bl	FUN_037F8CB4
+_03803C24:
+	mov	r0, r6
+	mov	r1, r5
+	mov	r2, r4
+	bl	PXI_SendWordByFifo
+	cmp	r0, #0
+	bne	_03803C1C
+	b	_03803C44
+_03803C40:
+	bl	OS_Terminate
+_03803C44:
+	add	sp, sp, #4
+	ldmia	sp!, {r4, r5, r6, r7, lr}
+	bx	lr
+_03803C50:	.word	0x01FFFFC0
+_03803C54:	.word	0x04000204
+
+	arm_func_start CTRDGi_TerminateARM7
+CTRDGi_TerminateARM7: ;@ 0x03803C58
+	stmfd	sp!, {lr}
+	sub	sp, sp, #4
+	mov	r0, #0
+	bl	CTRDG_VibPulseEdgeUpdate
+	bl	SND_BeginSleep
+	bl	WVR_Shutdown
+	bl	OS_Terminate
+	add	sp, sp, #4
+	ldmia	sp!, {lr}
+	bx	lr
+
+	arm_func_start CTRDG_CheckPullOut_Polling
+CTRDG_CheckPullOut_Polling: ;@ 0x03803C80
+	stmdb	sp!, {r4, r5, r6, r7, lr}
+	sub	sp, sp, #4
+	ldr	r1, _03803D80	;@ =_03807708
+	ldr	r3, [r1]
+	mvn	r0, #0
+	cmp	r3, r0
+	ldreq	r0, _03803D84	;@ =0x027FFC3C
+	ldreq	r0, [r0]
+	addeq	r0, r0, #10
+	streq	r0, [r1]
+	beq	_03803D74
+	ldr	r0, _03803D88	;@ =_03809FCC
+	ldr	r0, [r0]
+	cmp	r0, #0
+	bne	_03803D74
+	ldr	r0, _03803D8C	;@ =_03809FC8
+	ldr	r0, [r0]
+	cmp	r0, #0
+	bne	_03803D74
+	ldr	r2, _03803D84	;@ =0x027FFC3C
+	ldr	r0, [r2]
+	cmp	r0, r3
+	bcc	_03803D74
+	ldr	r0, [r2]
+	add	r0, r0, #10
+	str	r0, [r1]
+	bl	CTRDG_IsPulledOut
+	ldr	r1, _03803D8C	;@ =_03809FC8
+	str	r0, [r1]
+	bl	CTRDG_IsExisting
+	cmp	r0, #0
+	bne	_03803D28
+	ldr	r0, _03803D90	;@ =_03807704
+	ldr	r0, [r0]
+	cmp	r0, #0
+	movne	r1, #1
+	ldrne	r0, _03803D88	;@ =_03809FCC
+	strne	r1, [r0]
+	bne	_03803D74
+	mov	r1, #1
+	ldr	r0, _03803D8C	;@ =_03809FC8
+	str	r1, [r0]
+_03803D28:
+	mov	r7, #0
+	ldr	r0, _03803D90	;@ =_03807704
+	str	r7, [r0]
+	ldr	r0, _03803D8C	;@ =_03809FC8
+	ldr	r0, [r0]
+	cmp	r0, #0
+	beq	_03803D74
+	mov	r6, #100	;@ 0x64
+	mov	r5, #13
+	mov	r4, #17
+	b	_03803D5C
+_03803D54:
+	mov	r0, r6
+	bl	OS_Sleep
+_03803D5C:
+	mov	r0, r5
+	mov	r1, r4
+	mov	r2, r7
+	bl	PXI_SendWordByFifo
+	cmp	r0, #0
+	bne	_03803D54
+_03803D74:
+	add	sp, sp, #4
+	ldmia	sp!, {r4, r5, r6, r7, lr}
+	bx	lr
+_03803D80:	.word	_03807708
+_03803D84:	.word	0x027FFC3C
+_03803D88:	.word	_03809FCC
+_03803D8C:	.word	_03809FC8
+_03803D90:	.word	_03807704
+
+	arm_func_start CTRDGi_IsNinLogoOfAgb
+CTRDGi_IsNinLogoOfAgb: ;@ 0x03803D94
+	stmdb	sp!, {r4, r5, r6, r7, r8, lr}
+	mov	r7, r0
+	mov	r0, #134217728	;@ 0x8000000
+	add	r6, r0, #4
+	mov	r4, #1
+	bl	OS_GetLockID
+	mov	r0, r0, lsl #16
+	mov	r5, r0, lsr #16
+	mov	r0, r5
+	bl	OS_LockCartridge
+	mov	r1, #0
+	ldr	r0, _03803E3C	;@ =0x0000FFFF
+	eor	r2, r0, #3
+	mov	r2, r2, lsl #16
+	mov	lr, r2, lsr #16
+	eor	r2, r0, #132	;@ 0x84
+	mov	r2, r2, lsl #16
+	mov	r8, r2, lsr #16
+	b	_03803E18
+_03803DE0:
+	mov	ip, r0
+	cmp	r1, #76	;@ 0x4c
+	moveq	ip, r8
+	beq	_03803DF8
+	cmp	r1, #77	;@ 0x4d
+	moveq	ip, lr
+_03803DF8:
+	mov	r3, r1, lsl #1
+	ldrh	r2, [r7, r3]
+	and	ip, ip, r2
+	ldrh	r2, [r6, r3]
+	cmp	ip, r2
+	movne	r4, #0
+	bne	_03803E20
+	add	r1, r1, #1
+_03803E18:
+	cmp	r1, #78	;@ 0x4e
+	blt	_03803DE0
+_03803E20:
+	mov	r0, r5
+	bl	OS_UnLockCartridge
+	mov	r0, r5
+	bl	OS_ReleaseLockID
+	mov	r0, r4
+	ldmia	sp!, {r4, r5, r6, r7, r8, lr}
+	bx	lr
+_03803E3C:	.word	0x0000FFFF
+
+	arm_func_start FUN_03803E40
+FUN_03803E40: ;@ 0x03803E40
+	ldr	r1, _03803E54	;@ =current_vib
+	str	r0, [r1]
+	ldr	r1, _03803E58	;@ =0x08001000
+	strh	r0, [r1]
+	bx	lr
+_03803E54:	.word	current_vib
+_03803E58:	.word	0x08001000
+
+	arm_func_start CTRDG_VibPulseEdgeUpdate
+CTRDG_VibPulseEdgeUpdate: ;@ 0x03803E5C
+	stmdb	sp!, {r4, r5, r6, r7, r8, r9, sl, fp, lr}
+	sub	sp, sp, #4
+	movs	r5, r0
+	beq	_03803E9C
+	ldr	r0, [r5]
+	cmp	r0, #0
+	bne	_03803E9C
+	ldr	r0, [r5, #68]	;@ 0x44
+	add	r0, r0, #1
+	str	r0, [r5, #68]	;@ 0x44
+	ldr	r1, [r5, #64]	;@ 0x40
+	cmp	r1, #0
+	beq	_03803E9C
+	ldr	r0, [r5, #68]	;@ 0x44
+	cmp	r0, r1
+	movhi	r5, #0
+_03803E9C:
+	cmp	r5, #0
+	beq	_03803EB0
+	ldr	r0, [r5, #60]	;@ 0x3c
+	cmp	r0, #0
+	bne	_03803F48
+_03803EB0:
+	bl	OS_DisableInterrupts
+	mov	r9, r0
+	ldr	r0, _03804068	;@ =current_vib
+	ldr	r0, [r0]
+	cmp	r0, #2
+	bne	_03803F34
+	mov	r8, #0
+	ldr	r7, _0380406C	;@ =0x027FFFE8
+	ldr	r4, _03804070	;@ =_03809FBC
+	ldr	fp, _03804074	;@ =0x000080E8
+	mov	r6, r8
+	mov	r5, #1
+	b	_03803F2C
+_03803EE4:
+	mov	r0, r7
+	bl	OS_ReadOwnerOfLockWord
+	ands	sl, r0, #128	;@ 0x80
+	bne	_03803F04
+	ldrh	r0, [r4]
+	bl	OS_TryLockCartridge
+	cmp	r0, #0
+	bne	_03803F24
+_03803F04:
+	mov	r0, r6
+	bl	FUN_03803E40
+	mov	r8, r5
+	cmp	sl, #0
+	bne	_03803F2C
+	ldrh	r0, [r4]
+	bl	OS_UnlockCartridge
+	b	_03803F2C
+_03803F24:
+	mov	r0, fp
+	bl	OS_SpinWait
+_03803F2C:
+	cmp	r8, #0
+	beq	_03803EE4
+_03803F34:
+	ldr	r0, _03804078	;@ =_03809FEC
+	bl	OS_CancelAlarm
+	mov	r0, r9
+	bl	OS_RestoreInterrupts
+	b	_0380405C
+_03803F48:
+	cmp	r5, #0
+	beq	_0380405C
+	ldr	r0, _0380406C	;@ =0x027FFFE8
+	bl	OS_ReadOwnerOfLockWord
+	ands	r4, r0, #128	;@ 0x80
+	bne	_03803F74
+	ldr	r0, _03804070	;@ =_03809FBC
+	ldrh	r0, [r0]
+	bl	OS_TryLockCartridge
+	cmp	r0, #0
+	bne	_03804044
+_03803F74:
+	ldr	r1, [r5]
+	ldr	r0, [r5, #4]
+	cmp	r1, r0
+	bne	_03803FB0
+	mov	r0, #0
+	bl	FUN_03803E40
+	str	r5, [sp]
+	ldr	r0, _03804078	;@ =_03809FEC
+	ldr	r1, [r5, #8]
+	mov	r2, #0
+	ldr	r3, _0380407C	;@ =CTRDG_VibPulseEdgeUpdate
+	bl	OS_SetAlarm
+	mov	r0, #0
+	str	r0, [r5]
+	b	_0380402C
+_03803FB0:
+	ands	r0, r1, #1
+	beq	_03803FF4
+	mov	r0, #0
+	bl	FUN_03803E40
+	str	r5, [sp]
+	ldr	r0, _03804078	;@ =_03809FEC
+	ldr	r1, [r5]
+	mov	r1, r1, lsr #1
+	add	r1, r5, r1, lsl #2
+	ldr	r1, [r1, #36]	;@ 0x24
+	mov	r2, #0
+	ldr	r3, _0380407C	;@ =CTRDG_VibPulseEdgeUpdate
+	bl	OS_SetAlarm
+	ldr	r0, [r5]
+	add	r0, r0, #1
+	str	r0, [r5]
+	b	_0380402C
+_03803FF4:
+	mov	r0, #2
+	bl	FUN_03803E40
+	str	r5, [sp]
+	ldr	r0, _03804078	;@ =_03809FEC
+	ldr	r1, [r5]
+	mov	r1, r1, lsr #1
+	add	r1, r5, r1, lsl #2
+	ldr	r1, [r1, #12]
+	mov	r2, #0
+	ldr	r3, _0380407C	;@ =CTRDG_VibPulseEdgeUpdate
+	bl	OS_SetAlarm
+	ldr	r0, [r5]
+	add	r0, r0, #1
+	str	r0, [r5]
+_0380402C:
+	cmp	r4, #0
+	bne	_0380405C
+	ldr	r0, _03804070	;@ =_03809FBC
+	ldrh	r0, [r0]
+	bl	OS_UnlockCartridge
+	b	_0380405C
+_03804044:
+	str	r5, [sp]
+	ldr	r0, _03804078	;@ =_03809FEC
+	ldr	r1, _03804080	;@ =0x0000020B
+	mov	r2, #0
+	ldr	r3, _0380407C	;@ =CTRDG_VibPulseEdgeUpdate
+	bl	OS_SetAlarm
+_0380405C:
+	add	sp, sp, #4
+	ldmia	sp!, {r4, r5, r6, r7, r8, r9, sl, fp, lr}
+	bx	lr
+_03804068:	.word	current_vib
+_0380406C:	.word	0x027FFFE8
+_03804070:	.word	_03809FBC
+_03804074:	.word	0x000080E8
+_03804078:	.word	_03809FEC
+_0380407C:	.word	CTRDG_VibPulseEdgeUpdate
+_03804080:	.word	0x0000020B
+_03804084:
+	mov	r0, r1
+	ldr	ip, _03804090	;@ =CTRDG_VibPulseEdgeUpdate
+	bx	ip
+_03804090:	.word	CTRDG_VibPulseEdgeUpdate
+
+	arm_func_start CTRDGi_CallbackForPulledOut
+CTRDGi_CallbackForPulledOut: ;@ 0x03804094
+	stmfd	sp!, {lr}
+	sub	sp, sp, #4
+	and	r0, r1, #63	;@ 0x3ff
+	cmp	r0, #2
+	bne	_038040B0
+	bl	CTRDGi_TerminateARM7
+	b	_038040B4
+_038040B0:
+	bl	OS_Terminate
+_038040B4:
+	add	sp, sp, #4
+	ldmia	sp!, {lr}
+	bx	lr
+
+	arm_func_start CTRDGi_CallbackForInitModuleInfo
+CTRDGi_CallbackForInitModuleInfo: ;@ 0x038040C0
+	stmfd	sp!, {lr}
+	sub	sp, sp, #4
+	and	r0, r1, #63	;@ 0x3f
+	cmp	r0, #1
+	ldreq	r0, _038040F4	;@ =_03809FD4
+	streq	r1, [r0]
+	moveq	r1, #1
+	streq	r1, [r0, #16]
+	beq	_038040E8
+	bl	OS_Terminate
+_038040E8:
+	add	sp, sp, #4
+	ldmia	sp!, {lr}
+	bx	lr
+_038040F4:	.word	_03809FD4
+
+	arm_func_start CTRDGi_InitModuleInfo
+CTRDGi_InitModuleInfo: ;@ 0x038040F8
+	stmdb	sp!, {r4, r5, r6, r7, lr}
+	sub	sp, sp, #4
+	ldr	r0, _038041C8	;@ =_03809FC4
+	ldr	r1, [r0]
+	cmp	r1, #0
+	bne	_038041BC
+	mov	r1, #1
+	str	r1, [r0]
+	ldr	r0, _038041CC	;@ =0x04000300
+	ldrh	r0, [r0]
+	ands	r0, r0, #1
+	beq	_038041BC
+	mov	r0, #262144	;@ 0x40000
+	bl	OS_SetIrqMask
+	mov	r5, r0
+	ldr	r1, _038041D0	;@ =0x04000208
+	ldrh	r4, [r1]
+	mov	r0, #1
+	strh	r0, [r1]
+	mov	r7, #256	;@ 0x100
+	ldr	r6, _038041D4	;@ =_03809FD4
+	b	_03804158
+_03804150:
+	mov	r0, r7
+	bl	FUN_037F8CB4
+_03804158:
+	ldr	r0, [r6, #16]
+	cmp	r0, #1
+	bne	_03804150
+	ldr	r1, [r6]
+	ldr	r0, _038041D8	;@ =0x01FFFFC0
+	and	r0, r1, r0
+	mov	r0, r0, lsr #6
+	mov	r0, r0, lsl #5
+	add	r0, r0, #33554432	;@ 0x2000000
+	add	r0, r0, #4
+	bl	CTRDGi_IsNinLogoOfAgb
+	ldr	r2, _038041DC	;@ =0x027FFC30
+	ldrb	r1, [r2, #5]
+	bic	r1, r1, #1
+	and	r0, r0, #255	;@ 0xff
+	and	r0, r0, #1
+	orr	r0, r1, r0
+	strb	r0, [r2, #5]
+	mov	r0, #1
+	bl	CTRDGi_SendtoPxi
+	ldr	r1, _038041D0	;@ =0x04000208
+	ldrh	r0, [r1]
+	strh	r4, [r1]
+	mov	r0, r5
+	bl	OS_SetIrqMask
+_038041BC:
+	add	sp, sp, #4
+	ldmia	sp!, {r4, r5, r6, r7, lr}
+	bx	lr
+_038041C8:	.word	_03809FC4
+_038041CC:	.word	0x04000300
+_038041D0:	.word	0x04000208
+_038041D4:	.word	_03809FD4
+_038041D8:	.word	0x01FFFFC0
+_038041DC:	.word	0x027FFC30
+
+	arm_func_start CTRDG_Init
+CTRDG_Init: ;@ 0x038041E0
+	stmfd	sp!, {lr}
+	sub	sp, sp, #4
+	bl	OS_InitTick
+	bl	OS_InitAlarm
+	ldr	r0, _03804270	;@ =_03809FEC
+	bl	OS_CreateAlarm
+	ldr	r0, _03804274	;@ =_03809FD0
+	ldr	r1, [r0]
+	cmp	r1, #0
+	bne	_03804264
+	mov	r1, #1
+	str	r1, [r0]
+	bl	CTRDGi_InitCommon
+	bl	OS_GetLockID
+	mvn	r1, #2
+	cmp	r0, r1
+	beq	_03804264
+	ldr	r1, _03804278	;@ =_03809FBC
+	strh	r0, [r1]
+	bl	PXI_Init
+	mov	r0, #13
+	ldr	r1, _0380427C	;@ =CTRDGi_CallbackForInitModuleInfo
+	bl	PXI_SetFifoRecvCallback
+	bl	CTRDGi_InitModuleInfo
+	mov	r0, #13
+	ldr	r1, _03804280	;@ =CTRDGi_CallbackForPulledOut
+	bl	PXI_SetFifoRecvCallback
+	mov	r0, #16
+	ldr	r1, _03804284	;@ =_03804084
+	bl	PXI_SetFifoRecvCallback
+	mov	r0, #17
+	ldr	r1, _03804288	;@ =FUN_03803BD4
+	bl	PXI_SetFifoRecvCallback
+_03804264:
+	add	sp, sp, #4
+	ldmia	sp!, {lr}
+	bx	lr
+_03804270:	.word	_03809FEC
+_03804274:	.word	_03809FD0
+_03804278:	.word	_03809FBC
+_0380427C:	.word	CTRDGi_CallbackForInitModuleInfo
+_03804280:	.word	CTRDGi_CallbackForPulledOut
+_03804284:	.word	_03804084
+_03804288:	.word	FUN_03803BD4
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/asm/wram2.s b/arm7/asm/wram2.s
index 4e48222d..68b31543 100644
--- a/arm7/asm/wram2.s
+++ b/arm7/asm/wram2.s
@@ -3,500 +3,6 @@
 
 	.section .text
 
-	arm_func_start FUN_03803BD4
-FUN_03803BD4: ;@ 0x03803BD4
-	stmdb	sp!, {r4, r5, r6, r7, lr}
-	sub	sp, sp, #4
-	and	r0, r1, #63	;@ 0x3f
-	cmp	r0, #3
-	bne	_03803C40
-	ldr	r0, _03803C50	;@ =0x01FFFFC0
-	and	r0, r1, r0
-	mov	r2, r0, lsr #6
-	ldr	r1, _03803C54	;@ =0x04000204
-	ldrh	r0, [r1]
-	bic	r0, r0, #96	;@ 0x60
-	orr	r0, r0, r2, lsl #5
-	strh	r0, [r1]
-	mov	r7, #1
-	mov	r6, #17
-	mov	r5, #18
-	mov	r4, #0
-	b	_03803C24
-_03803C1C:
-	mov	r0, r7
-	bl	FUN_037F8CB4
-_03803C24:
-	mov	r0, r6
-	mov	r1, r5
-	mov	r2, r4
-	bl	PXI_SendWordByFifo
-	cmp	r0, #0
-	bne	_03803C1C
-	b	_03803C44
-_03803C40:
-	bl	OS_Terminate
-_03803C44:
-	add	sp, sp, #4
-	ldmia	sp!, {r4, r5, r6, r7, lr}
-	bx	lr
-_03803C50:	.word	0x01FFFFC0
-_03803C54:	.word	0x04000204
-
-	arm_func_start FUN_03803C58
-FUN_03803C58: ;@ 0x03803C58
-	stmfd	sp!, {lr}
-	sub	sp, sp, #4
-	mov	r0, #0
-	bl	CTRDG_VibPulseEdgeUpdate
-	bl	SND_BeginSleep
-	bl	WVR_Shutdown
-	bl	OS_Terminate
-	add	sp, sp, #4
-	ldmia	sp!, {lr}
-	bx	lr
-
-	arm_func_start CTRDG_CheckPullOut_Polling
-CTRDG_CheckPullOut_Polling: ;@ 0x03803C80
-	stmdb	sp!, {r4, r5, r6, r7, lr}
-	sub	sp, sp, #4
-	ldr	r1, _03803D80	;@ =_03807708
-	ldr	r3, [r1]
-	mvn	r0, #0
-	cmp	r3, r0
-	ldreq	r0, _03803D84	;@ =0x027FFC3C
-	ldreq	r0, [r0]
-	addeq	r0, r0, #10
-	streq	r0, [r1]
-	beq	_03803D74
-	ldr	r0, _03803D88	;@ =_03809FCC
-	ldr	r0, [r0]
-	cmp	r0, #0
-	bne	_03803D74
-	ldr	r0, _03803D8C	;@ =_03809FC8
-	ldr	r0, [r0]
-	cmp	r0, #0
-	bne	_03803D74
-	ldr	r2, _03803D84	;@ =0x027FFC3C
-	ldr	r0, [r2]
-	cmp	r0, r3
-	bcc	_03803D74
-	ldr	r0, [r2]
-	add	r0, r0, #10
-	str	r0, [r1]
-	bl	CTRDG_IsPulledOut
-	ldr	r1, _03803D8C	;@ =_03809FC8
-	str	r0, [r1]
-	bl	CTRDG_IsExisting
-	cmp	r0, #0
-	bne	_03803D28
-	ldr	r0, _03803D90	;@ =_03807704
-	ldr	r0, [r0]
-	cmp	r0, #0
-	movne	r1, #1
-	ldrne	r0, _03803D88	;@ =_03809FCC
-	strne	r1, [r0]
-	bne	_03803D74
-	mov	r1, #1
-	ldr	r0, _03803D8C	;@ =_03809FC8
-	str	r1, [r0]
-_03803D28:
-	mov	r7, #0
-	ldr	r0, _03803D90	;@ =_03807704
-	str	r7, [r0]
-	ldr	r0, _03803D8C	;@ =_03809FC8
-	ldr	r0, [r0]
-	cmp	r0, #0
-	beq	_03803D74
-	mov	r6, #100	;@ 0x64
-	mov	r5, #13
-	mov	r4, #17
-	b	_03803D5C
-_03803D54:
-	mov	r0, r6
-	bl	OS_Sleep
-_03803D5C:
-	mov	r0, r5
-	mov	r1, r4
-	mov	r2, r7
-	bl	PXI_SendWordByFifo
-	cmp	r0, #0
-	bne	_03803D54
-_03803D74:
-	add	sp, sp, #4
-	ldmia	sp!, {r4, r5, r6, r7, lr}
-	bx	lr
-_03803D80:	.word	_03807708
-_03803D84:	.word	0x027FFC3C
-_03803D88:	.word	_03809FCC
-_03803D8C:	.word	_03809FC8
-_03803D90:	.word	_03807704
-
-	arm_func_start FUN_03803D94
-FUN_03803D94: ;@ 0x03803D94
-	stmdb	sp!, {r4, r5, r6, r7, r8, lr}
-	mov	r7, r0
-	mov	r0, #134217728	;@ 0x8000000
-	add	r6, r0, #4
-	mov	r4, #1
-	bl	OS_GetLockID
-	mov	r0, r0, lsl #16
-	mov	r5, r0, lsr #16
-	mov	r0, r5
-	bl	OS_LockCartridge
-	mov	r1, #0
-	ldr	r0, _03803E3C	;@ =0x0000FFFF
-	eor	r2, r0, #3
-	mov	r2, r2, lsl #16
-	mov	lr, r2, lsr #16
-	eor	r2, r0, #132	;@ 0x84
-	mov	r2, r2, lsl #16
-	mov	r8, r2, lsr #16
-	b	_03803E18
-_03803DE0:
-	mov	ip, r0
-	cmp	r1, #76	;@ 0x4c
-	moveq	ip, r8
-	beq	_03803DF8
-	cmp	r1, #77	;@ 0x4d
-	moveq	ip, lr
-_03803DF8:
-	mov	r3, r1, lsl #1
-	ldrh	r2, [r7, r3]
-	and	ip, ip, r2
-	ldrh	r2, [r6, r3]
-	cmp	ip, r2
-	movne	r4, #0
-	bne	_03803E20
-	add	r1, r1, #1
-_03803E18:
-	cmp	r1, #78	;@ 0x4e
-	blt	_03803DE0
-_03803E20:
-	mov	r0, r5
-	bl	OS_UnLockCartridge
-	mov	r0, r5
-	bl	OS_ReleaseLockID
-	mov	r0, r4
-	ldmia	sp!, {r4, r5, r6, r7, r8, lr}
-	bx	lr
-_03803E3C:	.word	0x0000FFFF
-
-	arm_func_start FUN_03803E40
-FUN_03803E40: ;@ 0x03803E40
-	ldr	r1, _03803E54	;@ =current_vib
-	str	r0, [r1]
-	ldr	r1, _03803E58	;@ =0x08001000
-	strh	r0, [r1]
-	bx	lr
-_03803E54:	.word	current_vib
-_03803E58:	.word	0x08001000
-
-	arm_func_start CTRDG_VibPulseEdgeUpdate
-CTRDG_VibPulseEdgeUpdate: ;@ 0x03803E5C
-	stmdb	sp!, {r4, r5, r6, r7, r8, r9, sl, fp, lr}
-	sub	sp, sp, #4
-	movs	r5, r0
-	beq	_03803E9C
-	ldr	r0, [r5]
-	cmp	r0, #0
-	bne	_03803E9C
-	ldr	r0, [r5, #68]	;@ 0x44
-	add	r0, r0, #1
-	str	r0, [r5, #68]	;@ 0x44
-	ldr	r1, [r5, #64]	;@ 0x40
-	cmp	r1, #0
-	beq	_03803E9C
-	ldr	r0, [r5, #68]	;@ 0x44
-	cmp	r0, r1
-	movhi	r5, #0
-_03803E9C:
-	cmp	r5, #0
-	beq	_03803EB0
-	ldr	r0, [r5, #60]	;@ 0x3c
-	cmp	r0, #0
-	bne	_03803F48
-_03803EB0:
-	bl	OS_DisableInterrupts
-	mov	r9, r0
-	ldr	r0, _03804068	;@ =current_vib
-	ldr	r0, [r0]
-	cmp	r0, #2
-	bne	_03803F34
-	mov	r8, #0
-	ldr	r7, _0380406C	;@ =0x027FFFE8
-	ldr	r4, _03804070	;@ =_03809FBC
-	ldr	fp, _03804074	;@ =0x000080E8
-	mov	r6, r8
-	mov	r5, #1
-	b	_03803F2C
-_03803EE4:
-	mov	r0, r7
-	bl	OS_ReadOwnerOfLockWord
-	ands	sl, r0, #128	;@ 0x80
-	bne	_03803F04
-	ldrh	r0, [r4]
-	bl	OS_TryLockCartridge
-	cmp	r0, #0
-	bne	_03803F24
-_03803F04:
-	mov	r0, r6
-	bl	FUN_03803E40
-	mov	r8, r5
-	cmp	sl, #0
-	bne	_03803F2C
-	ldrh	r0, [r4]
-	bl	OS_UnlockCartridge
-	b	_03803F2C
-_03803F24:
-	mov	r0, fp
-	bl	OS_SpinWait
-_03803F2C:
-	cmp	r8, #0
-	beq	_03803EE4
-_03803F34:
-	ldr	r0, _03804078	;@ =_03809FEC
-	bl	OS_CancelAlarm
-	mov	r0, r9
-	bl	OS_RestoreInterrupts
-	b	_0380405C
-_03803F48:
-	cmp	r5, #0
-	beq	_0380405C
-	ldr	r0, _0380406C	;@ =0x027FFFE8
-	bl	OS_ReadOwnerOfLockWord
-	ands	r4, r0, #128	;@ 0x80
-	bne	_03803F74
-	ldr	r0, _03804070	;@ =_03809FBC
-	ldrh	r0, [r0]
-	bl	OS_TryLockCartridge
-	cmp	r0, #0
-	bne	_03804044
-_03803F74:
-	ldr	r1, [r5]
-	ldr	r0, [r5, #4]
-	cmp	r1, r0
-	bne	_03803FB0
-	mov	r0, #0
-	bl	FUN_03803E40
-	str	r5, [sp]
-	ldr	r0, _03804078	;@ =_03809FEC
-	ldr	r1, [r5, #8]
-	mov	r2, #0
-	ldr	r3, _0380407C	;@ =CTRDG_VibPulseEdgeUpdate
-	bl	OS_SetAlarm
-	mov	r0, #0
-	str	r0, [r5]
-	b	_0380402C
-_03803FB0:
-	ands	r0, r1, #1
-	beq	_03803FF4
-	mov	r0, #0
-	bl	FUN_03803E40
-	str	r5, [sp]
-	ldr	r0, _03804078	;@ =_03809FEC
-	ldr	r1, [r5]
-	mov	r1, r1, lsr #1
-	add	r1, r5, r1, lsl #2
-	ldr	r1, [r1, #36]	;@ 0x24
-	mov	r2, #0
-	ldr	r3, _0380407C	;@ =CTRDG_VibPulseEdgeUpdate
-	bl	OS_SetAlarm
-	ldr	r0, [r5]
-	add	r0, r0, #1
-	str	r0, [r5]
-	b	_0380402C
-_03803FF4:
-	mov	r0, #2
-	bl	FUN_03803E40
-	str	r5, [sp]
-	ldr	r0, _03804078	;@ =_03809FEC
-	ldr	r1, [r5]
-	mov	r1, r1, lsr #1
-	add	r1, r5, r1, lsl #2
-	ldr	r1, [r1, #12]
-	mov	r2, #0
-	ldr	r3, _0380407C	;@ =CTRDG_VibPulseEdgeUpdate
-	bl	OS_SetAlarm
-	ldr	r0, [r5]
-	add	r0, r0, #1
-	str	r0, [r5]
-_0380402C:
-	cmp	r4, #0
-	bne	_0380405C
-	ldr	r0, _03804070	;@ =_03809FBC
-	ldrh	r0, [r0]
-	bl	OS_UnlockCartridge
-	b	_0380405C
-_03804044:
-	str	r5, [sp]
-	ldr	r0, _03804078	;@ =_03809FEC
-	ldr	r1, _03804080	;@ =0x0000020B
-	mov	r2, #0
-	ldr	r3, _0380407C	;@ =CTRDG_VibPulseEdgeUpdate
-	bl	OS_SetAlarm
-_0380405C:
-	add	sp, sp, #4
-	ldmia	sp!, {r4, r5, r6, r7, r8, r9, sl, fp, lr}
-	bx	lr
-_03804068:	.word	current_vib
-_0380406C:	.word	0x027FFFE8
-_03804070:	.word	_03809FBC
-_03804074:	.word	0x000080E8
-_03804078:	.word	_03809FEC
-_0380407C:	.word	CTRDG_VibPulseEdgeUpdate
-_03804080:	.word	0x0000020B
-_03804084:
-	mov	r0, r1
-	ldr	ip, _03804090	;@ =CTRDG_VibPulseEdgeUpdate
-	bx	ip
-_03804090:	.word	CTRDG_VibPulseEdgeUpdate
-
-	arm_func_start FUN_03804094
-FUN_03804094: ;@ 0x03804094
-	stmfd	sp!, {lr}
-	sub	sp, sp, #4
-	and	r0, r1, #63	;@ 0x3f
-	cmp	r0, #2
-	bne	_038040B0
-	bl	FUN_03803C58
-	b	_038040B4
-_038040B0:
-	bl	OS_Terminate
-_038040B4:
-	add	sp, sp, #4
-	ldmia	sp!, {lr}
-	bx	lr
-
-	arm_func_start FUN_038040C0
-FUN_038040C0: ;@ 0x038040C0
-	stmfd	sp!, {lr}
-	sub	sp, sp, #4
-	and	r0, r1, #63	;@ 0x3f
-	cmp	r0, #1
-	ldreq	r0, _038040F4	;@ =_03809FD4
-	streq	r1, [r0]
-	moveq	r1, #1
-	streq	r1, [r0, #16]
-	beq	_038040E8
-	bl	OS_Terminate
-_038040E8:
-	add	sp, sp, #4
-	ldmia	sp!, {lr}
-	bx	lr
-_038040F4:	.word	_03809FD4
-
-	arm_func_start CTRDGi_InitModuleInfo
-CTRDGi_InitModuleInfo: ;@ 0x038040F8
-	stmdb	sp!, {r4, r5, r6, r7, lr}
-	sub	sp, sp, #4
-	ldr	r0, _038041C8	;@ =_03809FC4
-	ldr	r1, [r0]
-	cmp	r1, #0
-	bne	_038041BC
-	mov	r1, #1
-	str	r1, [r0]
-	ldr	r0, _038041CC	;@ =0x04000300
-	ldrh	r0, [r0]
-	ands	r0, r0, #1
-	beq	_038041BC
-	mov	r0, #262144	;@ 0x40000
-	bl	OS_SetIrqMask
-	mov	r5, r0
-	ldr	r1, _038041D0	;@ =0x04000208
-	ldrh	r4, [r1]
-	mov	r0, #1
-	strh	r0, [r1]
-	mov	r7, #256	;@ 0x100
-	ldr	r6, _038041D4	;@ =_03809FD4
-	b	_03804158
-_03804150:
-	mov	r0, r7
-	bl	FUN_037F8CB4
-_03804158:
-	ldr	r0, [r6, #16]
-	cmp	r0, #1
-	bne	_03804150
-	ldr	r1, [r6]
-	ldr	r0, _038041D8	;@ =0x01FFFFC0
-	and	r0, r1, r0
-	mov	r0, r0, lsr #6
-	mov	r0, r0, lsl #5
-	add	r0, r0, #33554432	;@ 0x2000000
-	add	r0, r0, #4
-	bl	FUN_03803D94
-	ldr	r2, _038041DC	;@ =0x027FFC30
-	ldrb	r1, [r2, #5]
-	bic	r1, r1, #1
-	and	r0, r0, #255	;@ 0xff
-	and	r0, r0, #1
-	orr	r0, r1, r0
-	strb	r0, [r2, #5]
-	mov	r0, #1
-	bl	CTRDGi_SendtoPxi
-	ldr	r1, _038041D0	;@ =0x04000208
-	ldrh	r0, [r1]
-	strh	r4, [r1]
-	mov	r0, r5
-	bl	OS_SetIrqMask
-_038041BC:
-	add	sp, sp, #4
-	ldmia	sp!, {r4, r5, r6, r7, lr}
-	bx	lr
-_038041C8:	.word	_03809FC4
-_038041CC:	.word	0x04000300
-_038041D0:	.word	0x04000208
-_038041D4:	.word	_03809FD4
-_038041D8:	.word	0x01FFFFC0
-_038041DC:	.word	0x027FFC30
-
-	arm_func_start CTRDG_Init
-CTRDG_Init: ;@ 0x038041E0
-	stmfd	sp!, {lr}
-	sub	sp, sp, #4
-	bl	OS_InitTick
-	bl	OS_InitAlarm
-	ldr	r0, _03804270	;@ =_03809FEC
-	bl	OS_CreateAlarm
-	ldr	r0, _03804274	;@ =_03809FD0
-	ldr	r1, [r0]
-	cmp	r1, #0
-	bne	_03804264
-	mov	r1, #1
-	str	r1, [r0]
-	bl	CTRDGi_InitCommon
-	bl	OS_GetLockID
-	mvn	r1, #2
-	cmp	r0, r1
-	beq	_03804264
-	ldr	r1, _03804278	;@ =_03809FBC
-	strh	r0, [r1]
-	bl	PXI_Init
-	mov	r0, #13
-	ldr	r1, _0380427C	;@ =FUN_038040C0
-	bl	PXI_SetFifoRecvCallback
-	bl	CTRDGi_InitModuleInfo
-	mov	r0, #13
-	ldr	r1, _03804280	;@ =FUN_03804094
-	bl	PXI_SetFifoRecvCallback
-	mov	r0, #16
-	ldr	r1, _03804284	;@ =_03804084
-	bl	PXI_SetFifoRecvCallback
-	mov	r0, #17
-	ldr	r1, _03804288	;@ =FUN_03803BD4
-	bl	PXI_SetFifoRecvCallback
-_03804264:
-	add	sp, sp, #4
-	ldmia	sp!, {lr}
-	bx	lr
-_03804270:	.word	_03809FEC
-_03804274:	.word	_03809FD0
-_03804278:	.word	_03809FBC
-_0380427C:	.word	FUN_038040C0
-_03804280:	.word	FUN_03804094
-_03804284:	.word	_03804084
-_03804288:	.word	FUN_03803BD4
-
 	arm_func_start FUN_0380428C
 FUN_0380428C: ;@ 0x0380428C
 	stmfd	sp!, {lr}
diff --git a/arm7/asm/wram_bss.s b/arm7/asm/wram_bss.s
index 5d53542a..b8123702 100644
--- a/arm7/asm/wram_bss.s
+++ b/arm7/asm/wram_bss.s
@@ -2,38 +2,6 @@
 	.include "global.inc"
 	.section .bss
 
-	.global _03809FBC
-_03809FBC: ;0x03809FBC
-	.space 0x03809FC0 - 0x03809FBC
-
-	.global current_vib
-current_vib: ;0x03809FC0
-	.space 0x03809FC4 - 0x03809FC0
-
-	.global _03809FC4
-_03809FC4: ;0x03809FC4
-	.space 0x03809FC8 - 0x03809FC4
-
-	.global _03809FC8
-_03809FC8: ;0x03809FC8
-	.space 0x03809FCC - 0x03809FC8
-
-	.global _03809FCC
-_03809FCC: ;0x03809FCC
-	.space 0x03809FD0 - 0x03809FCC
-
-	.global _03809FD0
-_03809FD0: ;0x03809FD0
-	.space 0x03809FD4 - 0x03809FD0
-
-	.global _03809FD4
-_03809FD4: ;0x03809FD4
-	.space 0x03809FEC - 0x03809FD4
-
-	.global _03809FEC
-_03809FEC: ;0x03809FEC
-	.space 0x0380A018 - 0x03809FEC
-
 	.global _0380A018
 _0380A018: ;0x0380A018
 	.space 0x0380A038 - 0x0380A018
diff --git a/arm7/asm/wram_data.s b/arm7/asm/wram_data.s
deleted file mode 100644
index 6b7aa989..00000000
--- a/arm7/asm/wram_data.s
+++ /dev/null
@@ -1,13 +0,0 @@
-	.include "asm/macros.inc"
-	.include "global.inc"
-
-	.section .data
-
-	.global _03807704
-_03807704:
-	.word	0x00000001
-
-	.global _03807708
-_03807708:
-	.word	0xFFFFFFFF
-	.balign 2, 0 ; Don't pad with nop
diff --git a/arm7/lib/include/MI_memory.h b/arm7/lib/include/MI_memory.h
index cb0c53fc..53398ec5 100644
--- a/arm7/lib/include/MI_memory.h
+++ b/arm7/lib/include/MI_memory.h
@@ -12,4 +12,14 @@ void MIi_CpuCopyFast(register u32 * src, register u32 * dst, register u32 size);
 void MI_CpuFill8(register u8 value, register u8 * dst, register u32 size);
 void MI_CpuCopy8(register u8 * src, register u8 * dst, register 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
diff --git a/arm7/lib/include/OS_alarm.h b/arm7/lib/include/OS_alarm.h
new file mode 100644
index 00000000..8ae89e97
--- /dev/null
+++ b/arm7/lib/include/OS_alarm.h
@@ -0,0 +1,9 @@
+#ifndef POKEDIAMOND_ARM7_OS_ALARM_H
+#define POKEDIAMOND_ARM7_OS_ALARM_H
+
+#include "nitro/OS_alarm_shared.h"
+
+void OS_CreateAlarm(OSAlarm *alarm);
+void OS_SetAlarm(OSAlarm *alarm, OSTick tick, OSAlarmHandler handler, void *arg);
+
+#endif //POKEDIAMOND_ARM7_OS_ALARM_H
diff --git a/arm7/lib/include/OS_context.h b/arm7/lib/include/OS_context.h
index 7819e29c..b01ccb8b 100644
--- a/arm7/lib/include/OS_context.h
+++ b/arm7/lib/include/OS_context.h
@@ -13,4 +13,8 @@ typedef struct OSContext
     u32 sp_svc;
 } 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_ARM7_OS_CONTEXT_H
diff --git a/arm7/lib/include/OS_mutex.h b/arm7/lib/include/OS_mutex.h
new file mode 100644
index 00000000..920d5594
--- /dev/null
+++ b/arm7/lib/include/OS_mutex.h
@@ -0,0 +1,14 @@
+#ifndef POKEDIAMOND_ARM7_OS_MUTEX_H
+#define POKEDIAMOND_ARM7_OS_MUTEX_H
+
+#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/include/OS_thread.h b/arm7/lib/include/OS_thread.h
index 04e4fdcb..b1d3a01b 100644
--- a/arm7/lib/include/OS_thread.h
+++ b/arm7/lib/include/OS_thread.h
@@ -4,4 +4,33 @@
 #include "OS_context.h"
 #include "nitro/OS_thread_shared.h"
 
+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);
+static void OSi_RescheduleThread(void);
+void OS_InitThread(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_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);
+BOOL OS_SetThreadPriority(OSThread *thread, u32 prio);
+void OS_Sleep(u32 msec);
+static void OSi_SleepAlarmCallback(void *arg);
+OSSwitchThreadCallback OS_SetSwitchThreadCallback(OSSwitchThreadCallback callback);
+u32 OS_DisableScheduler(void);
+u32 OS_EnableScheduler(void);
+void OS_SetThreadDestructor(OSThread *thread, OSThreadDestructor dtor);
+
 #endif //POKEDIAMOND_ARM7_OS_THREAD_H
diff --git a/arm7/lib/include/consts.h b/arm7/lib/include/consts.h
index 966b5a4b..a6c4c393 100644
--- a/arm7/lib/include/consts.h
+++ b/arm7/lib/include/consts.h
@@ -4,5 +4,6 @@
 #include "nitro/consts_shared.h"
 #include "mmap.h"
 #include "registers.h"
+#include "systemWork.h"
 
 #endif //POKEDIAMOND_ARM7_CONSTS_H
diff --git a/arm7/lib/include/systemWork.h b/arm7/lib/include/systemWork.h
new file mode 100644
index 00000000..95a17845
--- /dev/null
+++ b/arm7/lib/include/systemWork.h
@@ -0,0 +1,7 @@
+#ifndef POKEDIAMOND_ARM7_SYSTEMWORK_H
+#define POKEDIAMOND_ARM7_SYSTEMWORK_H
+
+#include "OS_context.h"
+#include "nitro/OS_systemWork_shared.h"
+
+#endif //POKEDIAMOND_ARM7_SYSTEMWORK_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;
+    }
+}
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;
+}