#include <stdint.h>
#include "os.h"
#include "msp.h"

Task * volatile current_task;
Task * volatile next_task;

void OS_init(void) {
	*(uint32_t volatile *)0xE000ED20 |= (0xFFU << 16); // Set PendSV priority
}

void OS_sched(void) {
	if (next_task != current_task) {
		*(uint32_t volatile *)0xE000ED04 |= (1U << 28); // PendSV interrupt
	}
}

void Task_start(Task *t, TaskHandler th, void *stack, uint32_t stackSz) {
	uint32_t *sp = (uint32_t *)((((uint32_t)stack + stackSz) / 8) * 8);
	uint32_t *stack_limit;

	*(--sp) = (1U << 24);  /* xPSR */
	*(--sp) = (uint32_t)th; /* PC */
	*(--sp) = 0x0000000EU; /* LR  */
	*(--sp) = 0x0000000CU; /* R12 */
	*(--sp) = 0x00000003U; /* R3  */
	*(--sp) = 0x00000002U; /* R2  */
	*(--sp) = 0x00000001U; /* R1  */
	*(--sp) = 0x00000000U; /* R0  */
	/* additionally, fake registers R4-R11 */
	*(--sp) = 0x0000000BU; /* R11 */
	*(--sp) = 0x0000000AU; /* R10 */
	*(--sp) = 0x00000009U; /* R9 */
	*(--sp) = 0x00000008U; /* R8 */
	*(--sp) = 0x00000007U; /* R7 */
	*(--sp) = 0x00000006U; /* R6 */
	*(--sp) = 0x00000005U; /* R5 */
	*(--sp) = 0x00000004U; /* R4 */

	t->sp = sp;
	stack_limit = (uint32_t *)(((((uint32_t)stack - 1U) / 8) + 1U) * 8);

	for (sp = sp - 1U; sp >= stack_limit; --sp) {
	    *sp = 0xCDCDCDCD;
	}
}

void PendSV_Handler(void) {
//	__asm(
//		"  IMPORT current_task\n"
//		"  IMPORT next_task\n"
//	//__disable_irq();
//		"  cpsid 	i\n"
//	/* if (OS_curr != (OSThread *)0) { */
//		"  LDR 	r1,=current_task\n"
//		"  LDR 	r1,r1\n"
//		"  CBZ 	r1,PendSV_restore\n"
//	/*     push registers r4-r11 on the stack */
//		"  PUSH 	{r4-r11}\n"
//	/*     OS_curr->sp = sp; */
//		"  LDR 	r1,=current_task\n"
//		"  LDR 	r1,r1\n"
//		"  STR 	sp,r1\n"
//		"  PendSV_restore\n"
//	/* sp = OS_next->sp; */
//		"  LDR 	r1,=next_task\n"
//		"  LDR 	r1,r1\n"
//		"  LDR 	sp,r1\n"
//	/* OS_curr = OS_next; */
//	    "  LDR 	r1,=next_task\n"
//	    "  LDR 	r1,r1\n"
//	    "  LDR 	r2,=current_task\n"
//	    "  STR 	r1,r1\n"
//	/* pop registers r4-r11 */
//	    "  POP 	{r4-r11}\n"
//	//__enable_irq();
//		"  cpsie 	i\n"
//		"  BX  	lr\n"
//	);

	__asm(bl         #0x414
	ldr        r0, [pc, #0x28]
	ldr        r0, [r0]
	cbz        r0, #0x546
	push	   {r4-r11}
	ldr        r1, [pc, #0x24]
	ldr        r0, [sp]
	ldr        r1, [r1]
	str        r0, [r1]
	$C$L4
	ldr        r0, [pc, #0x18]
	ldr        r0, [r0]
	ldr        r0, [r0]
	str        r0, [sp]
	ldr        r0, [pc, #0x10]
	ldr        r1, [pc, #0x10]
	ldr        r0, [r0]
	str        r0, [r1]
	pop        {r4-r11}
	bl         #0x410

	void *sp;

	__disable_irq();
	if (current_task != (Task *)0) {
		/* push r4 r11 on stack */
		current_task->sp = sp;
	}

	sp = next_task->sp;
	current_task = next_task;
	__enable_irq();
}