/*
 * master_intr.S
 */

#include <xparameters.h>

#define portTASK_CONTEXT_SIZE        (4 * 40)
#define portTASK_CONTEXT_OFFSET(reg) (4 * (reg + 7))
#define BRIDGE_SWAP_ADDR             XPAR_KERNEL_BRAM_IF_CNTLR_BASEADDR

/* External function declarations. */
.extern XIntc_DeviceInterruptHandler
//.extern vTaskSwitchContext
//.extern vPortDebugContextSwitch
//.extern xPortDebugVerifyCurrentContext
.extern xPortDeviceHandler
/* External variable declarations. */
//.extern pxCurrentTCBs
//.extern pulMainStack
//.extern uxCriticalNesting
.extern _stack_end
.extern _stack
.extern _STACK_SIZE
#if ( configUSE_PREEMPTION == 1 )
.extern ulYieldFlag
#endif

/**
 * @def prologue
 * @ingroup freertosmbport
 *
 * Creates a local stack frame of the given \size. Pushes
 * the return address saved in register \ret onto the
 * newly created stack. Uses R19 as the frame pointer.
 */
.macro prologue ret, size
	addik	r1,		r1,		-\size
	swi		\ret,	r1, 	0
	swi		r19,	r1,		\size - 4
	addk	r19,	r1,		r0
.endm

/**
 * @def pushargs
 * @ingroup freertosmbport
 *
 * Pushes all local function parameters from the caller onto
 * the caller's stack frame using the local frame pointer in
 * R19. Its up to the caller to ensure that there is enough
 * room on the stack for these parameters.
 */
.macro pushargs
	swi		r5,		r19,	12
	swi		r6,		r19,	16
	swi		r7,		r19,	20
	swi		r8,		r19,	24
	swi		r9,		r19,	28
	swi		r10,	r19,	32
.endm

/**
 * @def epilogue
 * @ingroup freertosmbport
 *
 * Destroys a local stack frame of the given \size. Pops the
 * return address from the stack into register \ret. Uses R19
 * as the frame pointer.
 */
.macro epilogue ret, size
	lwi		\ret,	r1,		0
	addk	r1,		r19,	r0
	lwi		r19,	r1,		\size - 4
	addik	r1,		r1,		\size
.endm

/**
 * @def memalign
 * @ingroup freertosmbport
 *
 * Aligns the value in the given register to the lower word boundary
 * so that if the register is used to access a memory location,
 * the access will be word-aligned.
 */
.macro memalign reg
	andni	\reg	\reg	3
.endm

.global _interrupt_handler
.section .text
.align 4
.ent _interrupt_handler
.type _interrupt_handler, @function
_interrupt_handler:
	prologue	r14,	portTASK_CONTEXT_SIZE

												/* save volatile registers */
												/* save r3 before restoring the bridge
												 * swap value as we need a spare register */
	swi			r3,		r1,		portTASK_CONTEXT_OFFSET(3)

												/* Now that there's a register available, get the bridge value back */
	mfs			r3,		rpvr0					/* Get the contents of the PVR register */
	andi		r3,		r3,		0xFF			/* Put the value of the high byte (the USER1/CPU ID information) in r3 */

	beqi		r3,		_no_bridge_swap			/* Ensure CPU 0 (master CPU) never uses the bridge swap */
	muli		r3,		r3,		4
	lwi			r18,	r3,		BRIDGE_SWAP_ADDR

_no_bridge_swap:

	swi			r4,		r1,		portTASK_CONTEXT_OFFSET(4)
	swi			r5,		r1,		portTASK_CONTEXT_OFFSET(5)
	swi			r6,		r1,		portTASK_CONTEXT_OFFSET(6)
	swi			r7,		r1,		portTASK_CONTEXT_OFFSET(7)
	swi			r8,		r1,		portTASK_CONTEXT_OFFSET(8)
	swi			r9,		r1,		portTASK_CONTEXT_OFFSET(9)
	swi			r10,	r1,		portTASK_CONTEXT_OFFSET(10)
												/* save sub-routine return address */
	swi			r15,	r1,		portTASK_CONTEXT_OFFSET(15)

	addk		r5,		r1,		r0				/* R5 = context pointer */
	addk		r6,		r14,	r0				/* R6 = return address (R14) */
	addik		r7,		r0,		1				/* R7 = TRUE */
												/* R8 (state) = interrupted */
	addik		r8,		r0,		portTASK_CONTEXT_STATE_INTERRUPTED
	brlid		r15,	vPortSaveContext		/* call context saver */
	nop

#if ( portGENERATE_ISR_SIGNAL == 1 )
												/* MEM[address] |= mask */
	lwi			r3,		r0,		(portISR_SIGNAL_ADDRESS)
	ori			r3,		r3,		(portISR_SIGNAL_MASK)
	swi			r3,		r0,		(portISR_SIGNAL_ADDRESS)
#endif

	mfs			r18,	rpvr0					/* Get the contents of the PVR register */
	andi		r18,	r18,	0xFF			/* Put the value of the high byte (the USER1/CPU ID information) in r18 */
	muli		r18,	r18,	4				/* Find the relative address of a word for the current CPU */
	lwi			r1,		r18,	pulMainStack	/* Load the current CPU's interrupt stack pointer */

	addk		r15,	r0,		r0				/* clear return address */
	prologue	r15,	36						/* create local stack frame */
												/* load the intial critical nesting value
												 * into R3 and store it in the global
												 * critical nesting variable */
	addik		r3,		r0,		portINITIAL_CRITICAL_NESTING
	swi			r3,		r0,		uxCriticalNesting

	mfs			r5,		rpvr0					/* Get the contents of the PVR register */
	andi		r5,		r5,		0xFF			/* Convert to CPU_ID(equal to intc device ID here) */
	#addik		r5,		r0,		portXPS_INTC_DEVICE_ID
	/*
	 * The low-level device interrupt handler (XIntc_DeviceInterruptHandler) called must
	 * be the one specific to the current core. Slave cores will have their low-level
	 * handlers compiled to a location in their private BRAM, while the master core will
	 * have its handler in shared memory. The addresses of the handlers are
	 * placed into the ignition communication block on startup. Here we delegate to
	 * xPortDeviceHandler to execute the appropriate device interrupt handler.
	 */
    bralid 		r15,	xPortDeviceHandler
    nop

_no_device_handler:

#if ( configUSE_PREEMPTION == 1 )

	mfs			r18,	rpvr0					/* Get the contents of the PVR register */
	andi		r18,	r18,	0xFF			/* Put the value of the high byte (the USER1/CPU ID information) in r18 */
	muli		r18,	r18,	4				/* Find the relative address of a word for the current CPU */
	#lwi			r3,		r18,	ulYieldFlag		/* load the yield flag into R3 */
												/* is the yield flag zero? if so,
												 * branch to the restore context */
	lwi			r3,		r18,	0x90000400

	beqi		r3,		__interrupt_handler_restore_context
	nop

#if ( portDEBUG_CONTEXT_SWITCHES == 1 )
	bralid		r15,	vPortDebugContextSwitch	/* handle debug context switch */
	nop
#else
	bralid		r15,	vTaskSwitchContext		/* handle context switch */
	nop
#endif
#endif

__interrupt_handler_restore_context:

#if ( configUSE_PREEMPTION == 1 )
	mfs			r18,	rpvr0					/* Get the contents of the PVR register */
	andi		r18,	r18,	0xFF			/* Put the value of the high byte (the USER1/CPU ID information) in r18 */
	muli		r18,	r18,	4				/* Find the relative address of a word for the current CPU */
	#swi			r0,		r18,	ulYieldFlag		/* clear the yield flag */
	swi			r0,		r18,	0x90000400
#endif

#if ( portGENERATE_ISR_SIGNAL == 1 )
												/* MEM[address] &= ~mask */
	lwi			r3,		r0,		(portISR_SIGNAL_ADDRESS)
	andi		r3,		r3,		~(portISR_SIGNAL_MASK)
	swi			r3,		r0,		(portISR_SIGNAL_ADDRESS)

#endif

    brlid		r15,	vPortRestoreContext		/* function never returns */
    nop
.end _interrupt_handler

.global vPortYieldASM
.section .text
.align 4
.ent vPortYieldASM
.type vPortYieldASM, @function
vPortYieldASM:
	prologue	r15,	portTASK_CONTEXT_SIZE	/* create stack frame for task context */

	addk		r5,		r1,		r0				/* R5 = context pointer */
	addk		r6,		r15,	r0				/* R6 = return address */
	addk		r7,		r0,		r0				/* R7 = FALSE */
												/* R8 (state) = yielded */
	addik		r8,		r0,		portTASK_CONTEXT_STATE_YIELDED

	brlid		r15,	vPortSaveContext		/* call context save function */
	nop

	mfs			r18,	rpvr0					/* Get the contents of the PVR register */
	andi		r18,	r18,	0xFF			/* Put the value of the high byte (the USER1/CPU ID information) in r18 */
	muli		r18,	r18,	4				/* Find the relative address of a word for the current CPU */
	lwi			r1,		r18,	pulMainStack	/* load the current CPU's interrupt stack pointer */

	addk		r15,	r0,		r0				/* clear return address */
	prologue	r15,	36						/* create local stack frame */
												/* load the intial critical nesting value
												 * into R3 and store it in the global
												 * critical nesting variable */
	addik		r3,		r0,		portINITIAL_CRITICAL_NESTING
	swi			r3,		r0,		uxCriticalNesting

#if ( portDEBUG_CONTEXT_SWITCHES == 1 )
	bralid		r15,	vPortDebugContextSwitch	/* handle debug context switch */
	nop
#else
	bralid		r15,	vTaskSwitchContext		/* handle context switch */
	nop
#endif

    brlid		r15,	vPortRestoreContext		/* function never returns */
    nop
.end vPortYieldASM

.global vPortStartFirstTask
.section .text
.align 4
.ent vPortStartFirstTask
.type vPortStartFirstTask, @function
vPortStartFirstTask:
	prologue	r15,	8						/* local stack frame */

#if ( INCLUDE_uxTaskGetStackHighWaterMark == 1 )
												/* R18 = stack fill value */
	addik		r18,	r0,		portSTACK_FILL_VALUE
	addik		r3,		r0,		_stack_end		/* R3 = bottom of stack */
	rsub		r4,		r3,		r1				/* R4 = R1 - R3 (R4 = offset from stack pointer) */
	memalign	r4								/* MEMALIGN(R4) */

_again:
	addik		r4,		r4,		-4				/* R4 -= 4 */
	bltid		r4,		_done					/* done if R4 < 0 */
	nop
	sw			r18,	r3,		r4				/* MEM(R3 + R4) = R18 */
	brid		_again
	nop
_done:
	nop
	nop
#endif
    brlid		r15,	vPortRestoreContext		/* function never returns */
    nop
	lwi			r1,		r0,		pulMainStack	/* load the main stack pointer */
												/* load the intial critical nesting value
												 * into R3 and store it in the global
												 * critical nesting variable */


	addik		r3,		r0,		portINITIAL_CRITICAL_NESTING
	swi			r3,		r0,		uxCriticalNesting
    epilogue	r15,	8
    rtsd		r15,	8
    nop
.end vPortStartFirstTask
