#include <stdint.h>

uint32_t pxCurrentTCB = 0;
#ifdef __cplusplus
extern "C"
#endif
void vTaskSwitchContext()  { }


void vPortStartFirstTask( void )
{
	/* The MSP stack is not reset as, unlike on M3/4 parts, there is no vector
	table offset register that can be used to locate the initial stack value.
	Not all M0 parts have the application vector table at address 0. */
#if 1
	__asm volatile(
	"	.syntax unified				\n"
	"	ldr  r2, pxCurrentTCBConst2	\n" /* Obtain location of pxCurrentTCB. */
	"	ldr  r3, [r2]				\n"
	"	ldr  r0, [r3]				\n" /* The first item in pxCurrentTCB is the task top of stack. */
	"	adds r0, #32					\n" /* Discard everything up to r0. */
	"	msr  psp, r0					\n" /* This is now the new top of stack to use in the task. */
	"	movs r0, #2					\n" /* Switch to the psp stack. */
	"	msr  CONTROL, r0				\n"
	"	isb							\n"
	"	pop  {r0-r5}					\n" /* Pop the registers that are saved automatically. */
	"	mov  lr, r5					\n" /* lr is now in r5. */
	"	pop  {r3}					\n" /* Return address is now in r3. */
	"	pop  {r2}					\n" /* Pop and discard XPSR. */
	"	cpsie i						\n" /* The first task has its context and interrupts can be enabled. */
	"	bx   r3						\n" /* Finally, jump to the user defined task code. */
	"								\n"
	"	.align 4					\n"
	"pxCurrentTCBConst2: .word pxCurrentTCB	  "
				  );
#endif
}


void xPortPendSVHandler( void )
{
	/* This is a naked function. */

	__asm volatile
	(
	"	.syntax unified						\n"
	"	mrs r0, psp							\n"
	"										\n"
	"	ldr	r3, pxCurrentTCBConst			\n" /* Get the location of the current TCB. */
	"	ldr	r2, [r3]						\n"
	"										\n"
	"	subs r0, r0, #32					\n" /* Make space for the remaining low registers. */
	"	str r0, [r2]						\n" /* Save the new top of stack. */
	"	stmia r0!, {r4-r7}					\n" /* Store the low registers that are not saved automatically. */
	" 	mov r4, r8							\n" /* Store the high registers. */
	" 	mov r5, r9							\n"
	" 	mov r6, r10							\n"
	" 	mov r7, r11							\n"
	" 	stmia r0!, {r4-r7}					\n"
	"										\n"
	"	push {r3, r14}						\n"
	"	cpsid i								\n"
	"	bl vTaskSwitchContext				\n"
	"	cpsie i								\n"
	"	pop {r2, r3}						\n" /* lr goes in r3. r2 now holds tcb pointer. */
	"										\n"
	"	ldr r1, [r2]						\n"
	"	ldr r0, [r1]						\n" /* The first item in pxCurrentTCB is the task top of stack. */
	"	adds r0, r0, #16					\n" /* Move to the high registers. */
	"	ldmia r0!, {r4-r7}					\n" /* Pop the high registers. */
	" 	mov r8, r4							\n"
	" 	mov r9, r5							\n"
	" 	mov r10, r6							\n"
	" 	mov r11, r7							\n"
	"										\n"
	"	msr psp, r0							\n" /* Remember the new top of stack for the task. */
	"										\n"
	"	subs r0, r0, #32					\n" /* Go back for the low registers that are not automatically restored. */
	" 	ldmia r0!, {r4-r7}					\n" /* Pop low registers.  */
	"										\n"
	"	bx r3								\n"
	"										\n"
	"	.align 4							\n"
	"pxCurrentTCBConst: .word pxCurrentTCB	  "
	);
}

int main() {

	//test call
	xPortPendSVHandler();
	vPortStartFirstTask();

	return 0;
}

void _start() {
	main();
}