/******************************/
#define THREAD_R4   0x00
#define THREAD_R5   0x04
#define THREAD_R6   0x08
#define THREAD_R7   0x0C
#define THREAD_R8   0x10
#define THREAD_R9   0x14
#define THREAD_R10  0x18
#define THREAD_R11  0x1C
#define THREAD_R0   0x20
#define THREAD_R1   0x24
#define THREAD_R2   0x28
#define THREAD_R3   0x2C
#define THREAD_R12  0x30
#define THREAD_R13  0x34
#define THREAD_R14  0x38
#define THREAD_R15  0x3C
#define THREAD_CPSR 0x40
#define THREAD_TEMP 0x44
#define THREAD_LEN  0x48
/******************************/
#define THREAD_SHIFT 5
#define THREAD_LIM (1<<(THREAD_SHIFT))
/******************************/
.arm
.text
.align
/******************************/

thread_Init:
	rsb	r0, r0, #0x10000
	orr	r0, r0, #0xC00000
	mov	r1, #0x04000000
	str	r0, [r1, #0x100]

	ldr	r0, =thread_Tab
	stmia	r0, {r4-r11} @ r0-r3,r12 are destroyed by call
	str	sp, [r0, #THREAD_R13]
	str	lr, [r0, #THREAD_R15]
	mrs	r2, cpsr
	str	r2, [r0, #THREAD_CPSR]

	ldr	r0, =irq_Tab
	ldr	r2, =thread_Switch
	str	r2, [r0, #0x0C]
	ldr	r2, [r1, #0x210]
	orr	r2, r2, #0x08
	str	r2, [r1, #0x210]

	ldr	r0, =thread_Switch
	bx	r0

/******************************/
.size   thread_Init, .-thread_Init
.global thread_Init
/******************************/

thread_New:
	add	r1, r1, r2
	ldr	r2, =thread_Tab - THREAD_LEN + THREAD_R15
1:	ldr	r3, [r2, #THREAD_LEN]!
	cmp	r3, #0x00
	bne	1b

	str	r3, [r2, #THREAD_CPSR - THREAD_R15]
	str	r0, [r2, #THREAD_R15  - THREAD_R15]
	ldr	r0, =thread_Off
	str	r0, [r2, #THREAD_R14  - THREAD_R15]
	str	r1, [r2, #THREAD_R13  - THREAD_R15]

	movs	r0, r2
	bx	lr

/******************************/
.size   thread_New, .-thread_New
.global thread_New
/******************************/

thread_Off:
	ldr	r2, [r1, #THREAD_R15]
	str	r2, [r1, #THREAD_TEMP]
	ldr	r2, =0
	str	r2, [r1, #THREAD_R15]
	ldr	r0, =thread_Switch
	bx	r0

/******************************/
.size   thread_Off, .-thread_Off
.global thread_Off
/******************************/

thread_Res:
	ldr	r1, [r0, #THREAD_TEMP]
	str	r1, [r0, #THREAD_R15]
	ldr	r0, =thread_Switch
	bx	r0

/******************************/
.size   thread_Res, .-thread_Res
.global thread_Res
/******************************/
.section .itcm, "ax", %progbits
.align
/******************************/

thread_Switch:
	ldr	r0, =thread_Cur
	ldrb	r1, [r0]
	add	r2, r1, #0x01
	and	r2, r2, #THREAD_LIM-1
	strb	r2, [r0]
	ldr	r3, =THREAD_LEN
	ldr	ip, =thread_Tab
	mla	r1, r1, r3, ip
	stmia	r1!, {r4-r11} @ r4-r11
	ldmfd	sp!, {r4-r7,r8,r9}

	mrs	fp, cpsr
	mrs	ip, spsr
	msr	cpsr, ip
	stmia	r1!, {r4-r7,r8,r9,r12,r13,r14} @ r0-r3,r12,cpsr,r13,r15

	mla	r0, r2, r3, ip
	ldr	r1, [r0, #THREAD_CPSR]
	msr	spsr, r1
	ldr	sp, [r0, #THREAD_R13]
	ldr	lr, [r0, #THREAD_R14]

	msr	cpsr, fp

	ldmia	r0!, {r4-r11} @ r4-r11 [r13, r14]
	ldr	r12, [r0, #THREAD_R15] @ r4-r11, r13,r14,r15
	ldmia	r0, {r0-r3,r12,r14}
	stmfd	sp!, {r0-r3,r12} @ r0-r3,r4-r11,r12,r13,r14,

	ldr	r0, [r0] @ r0-r15, done

/******************************/
.size   thread_Switch, .-thread_Switch
.global thread_Switch
/******************************/
.section .sbss
.align
/******************************/

thread_Tab:
.rept THREAD_LIM
	.space 8*4 @ r4-r11
	.space 6*4 @ r0-r3,r12,r15
	.word  0   @ cpsr
	.word  0   @ sp
.endr

/******************************/
.size   thread_Tab, .-thread_Tab
.global thread_Tab
/******************************/

thread_Cur:
	.word 0

/******************************/
.size   thread_Cur, .-thread_Cur
.global thread_Cur
/******************************/