#include "FreeRTOSConfig.h"

	.extern pxCurrentTCB
	.extern vTaskSwitchContext
	.extern vTaskIncrementTick
	.extern vPortISRHandler

	.global vPortStartFirstTask
	.global vPortYield
	.global vPortTickISR
	.global vPortISRWrapper

.set	BChainField, 0
.set	NextLRField, BChainField + 4
.set	MSRField,    NextLRField + 4
.set	PCField,     MSRField    + 4
.set	LRField,     PCField     + 4
.set	CTRField,    LRField     + 4
.set	XERField,    CTRField    + 4
.set	CRField,     XERField    + 4
.set	USPRG0Field, CRField     + 4
.set	r0Field,     USPRG0Field + 4
.set	r2Field,     r0Field     + 4
.set	r3r31Field,  r2Field     + 4
.set	IFrameSize,  r3r31Field  + ( ( 31 - 3 ) + 1 ) * 4


.macro portSAVE_STACK_POINTER_AND_LR

	/* Get the address of the TCB. */
	xor		R0, R0, R0
	addis	R2, R0, pxCurrentTCB@ha
	lwz		R2,	pxCurrentTCB@l( R2 )

	/* Store the stack pointer into the TCB */
	stw		SP,	0( R2 )

	/* Save the link register */
	stwu	R1, -24( R1 )
	mflr	R0
	stw		R31, 20( R1 )
	stw		R0, 28( R1 )
	mr		R31, r1

.endm

.macro portRESTORE_STACK_POINTER_AND_LR

	/* Restore the link register */
	lwz		R11, 0( R1 )
	lwz		R0, 4( R11 )
	mtlr	R0
	lwz		R31, -4( R11 )
	mr		R1, R11

	/* Get the address of the TCB. */
	xor		R0, R0, R0
	addis   SP, R0, pxCurrentTCB@ha
	lwz		SP,	pxCurrentTCB@l( R1 )

	/* Get the task stack pointer from the TCB. */
	lwz		SP, 0( SP )

.endm


vPortStartFirstTask:

	/* Get the address of the TCB. */
	xor		R0, R0, R0
    addis   SP, R0, pxCurrentTCB@ha
    lwz		SP,	pxCurrentTCB@l( SP )

	/* Get the task stack pointer from the TCB. */
	lwz		SP, 0( SP )
	
	/* Restore MSR register to SRR1. */
	lwz		R0, MSRField(R1)
	mtsrr1	R0
	
	/* Restore current PC location to SRR0. */
	lwz		R0, PCField(R1)
	mtsrr0	R0

	/* Save  USPRG0 register */
	lwz		R0, USPRG0Field(R1)
	mtspr	0x100,R0
	
	/* Restore Condition register */
	lwz		R0, CRField(R1)
	mtcr	R0
	
	/* Restore Fixed Point Exception register */
	lwz		R0, XERField(R1)
	mtxer	R0
	
	/* Restore Counter register */
	lwz		R0, CTRField(R1)
	mtctr	R0
	
	/* Restore Link register */
	lwz		R0, LRField(R1)
	mtlr	R0
	
	/* Restore remaining GPR registers. */
	lmw	R3,r3r31Field(R1)
	
	/* Restore r0 and r2. */
	lwz		R0, r0Field(R1)
	lwz		R2, r2Field(R1)
	
	/* Remove frame from stack */
	addi	R1,R1,IFrameSize

	/* Return into the first task */
	rfi



vPortYield:

	portSAVE_STACK_POINTER_AND_LR
	bl vTaskSwitchContext
	portRESTORE_STACK_POINTER_AND_LR
	blr

vPortTickISR:

	portSAVE_STACK_POINTER_AND_LR
	bl vTaskIncrementTick
	
	#if configUSE_PREEMPTION == 1
		bl vTaskSwitchContext
	#endif

	/* Clear the interrupt */
	lis		R0, 2048
	mttsr	R0

	portRESTORE_STACK_POINTER_AND_LR
	blr

vPortISRWrapper:

	portSAVE_STACK_POINTER_AND_LR
	bl vPortISRHandler
	portRESTORE_STACK_POINTER_AND_LR
	blr