#include "os/os_type.h"
#include "bsp/bsp.h"

void co_task_tick_init(int ticks){
	/* setup systick timer for 1000Hz interrupts */
	SysTick_Config(SystemCoreClock / ticks);
}

u32 arch_stack_init(u32 *stack_base, u16 stack_size, void *fptr, void *p_arg) {
	u32 *p_stk = &stack_base[stack_size];
	u32 u_stk = (u32) p_stk;
	u_stk = u_stk & 0xFFFFFFFF8;
	p_stk = (u32 *)u_stk;
    *(--p_stk) = (u32)fptr; //r14
    //r12 - r1
    p_stk -= 12;
    *(--p_stk) = (u32)p_arg; //r0
                                                               										
    return (u32)p_stk;
}

__asm void arch_start_first_task(void) {
	extern current_tcb;
	PRESERVE8
	ldr     r0, =current_tcb
	ldr     r0, [r0]
	ldr     r0, [r0] //load tcb->sp to psp
	msr     psp, r0
	
    mrs     r0, CONTROL //thread mode use psp stack pointer
    orr     r0, r0, #2
    msr     CONTROL, r0	
	isb
	pop  {r0-r12, pc}
	nop
}

__asm int arch_save_context(void){
	PRESERVE8
	STMFD   sp!, {r0-r12,r14}	

	ldr     r0, =current_tcb
	ldr     r0, [r0]
	str     sp, [r0] //save sp to tcb->sp
	bx		lr
	nop
}


__asm void arch_restore_context(void){
	PRESERVE8
	ldr     r0, =current_tcb
	ldr     r0, [r0]
	ldr     sp, [r0] //load tcb->sp to r0
	LDMFD	sp!, {r0-r12, pc}
}

extern u64 g_task_ticks64;
extern u32 g_task_ticks;

void SysTick_Handler(void) {
	g_task_ticks ++;
	g_task_ticks64 ++;
}