/*
 * stm32f4xx_ctx.c
 *
 *  Created on: Oct 1, 2015
 *      Author: tkl
 */
#include <stdbool.h>

#include "stm32f4xx.h"
#include "stack.h"
#include "queue.h"
#include "thread.h"
#include "stm32f4xx_ctx.h"

#ifdef __NVIC_PRIO_BITS
	/* __BVIC_PRIO_BITS will be specified when CMSIS is being used. */
	#define configPRIO_BITS       		__NVIC_PRIO_BITS
#else
	#define configPRIO_BITS       		4        /* 15 priority levels */
#endif
#define configLIBRARY_LOWEST_INTERRUPT_PRIORITY			0xf
#define configKERNEL_INTERRUPT_PRIORITY	(configLIBRARY_LOWEST_INTERRUPT_PRIORITY << (8 - configPRIO_BITS))
#define portNVIC_SYSPRI2_REG	(*((volatile uint32_t *)0xe000ed20))
#define portNVIC_PENDSV_PRI		(((uint32_t)configKERNEL_INTERRUPT_PRIORITY) << 16UL)
#define portNVIC_SYSTICK_PRI	(((uint32_t)configKERNEL_INTERRUPT_PRIORITY) << 24UL)

extern volatile struct thread_context *current_thread;

void start_first_task(void) __attribute__ ((naked));
void PendSV_Handler(void) __attribute__ ((naked));
void SVC_Handler(void) __attribute__ ((naked));

void start_first_task(void)
{
	portNVIC_SYSPRI2_REG |= portNVIC_PENDSV_PRI;
	portNVIC_SYSPRI2_REG |= portNVIC_SYSTICK_PRI;

	__asm volatile("ldr r0, =0xE000ED08");
	__asm volatile("ldr r0, [r0]");
	__asm volatile("ldr r0, [r0]");
	__asm volatile("msr msp, r0");
	__asm volatile("cpsie i");
	__asm volatile("cpsie f");
	__asm volatile("dsb");
	__asm volatile("isb");
	__asm volatile("svc 0");
	__asm volatile("nop");
}

void arch_schedule(void)
{
	*((volatile unsigned long *) 0xe000ed04) = 0x10000000;
}

#define MAX_SYSCALL_INTERRUPT_PRIORITY 	( 5 << (8 - __NVIC_PRIO_BITS) )

void PendSV_Handler (void)
{
	__asm volatile("mrs r0, psp");
	__asm volatile("isb");
	__asm volatile("ldr	r3, =current_thread");
	__asm volatile("ldr	r2, [r3]");
	__asm volatile("tst r14, #0x10");
	__asm volatile("it eq");
	__asm volatile("vstmdbeq r0!, {s16-s31}");
	__asm volatile("stmdb r0!, {r4-r11, r14}");
	__asm volatile("str r0, [r2]");
	__asm volatile("stmdb sp!, {r3}");
	__asm volatile("mov r0, %0"::"i"(MAX_SYSCALL_INTERRUPT_PRIORITY));
	__asm volatile("msr basepri, r0");
	__asm volatile("dsb");
	__asm volatile("isb");
	__asm volatile("bl thread_switch_context");
	__asm volatile("mov r0, #0");
	__asm volatile("msr basepri, r0");
	__asm volatile("ldmia sp!, {r3}");
	__asm volatile("ldr r1, [r3]");
	__asm volatile("ldr r0, [r1]");
	__asm volatile("ldmia r0!, {r4-r11, r14}");
	__asm volatile("tst r14, #0x10");
	__asm volatile("it eq");
	__asm volatile("vldmiaeq r0!, {s16-s31}");
	__asm volatile("msr psp, r0");
	__asm volatile("isb");
	__asm volatile("bx r14");
}

void SVC_Handler(void)
{
	__asm volatile("ldr	r3, =current_thread");
	__asm volatile("ldr r1, [r3]");
	__asm volatile("ldr r0, [r1]");
	__asm volatile("ldmia r0!, {r4-r11, r14}");
	__asm volatile("msr psp, r0");
	__asm volatile("isb");
	__asm volatile("mov r0, #0");
	__asm volatile("msr	basepri, r0");
	__asm volatile("bx r14");
}