/********************************************************************
 * up_vectors.S
 *
 *   Copyright (C) 2007 Gregory Nutt. All rights reserved.
 *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name Gregory Nutt nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ********************************************************************/

/********************************************************************
 * Included Files
 ********************************************************************/

#include <nuttx/config.h>
#include <nuttx/irq.h>
#include "dm320.h"

/********************************************************************
 * Definitions
 ********************************************************************/

/********************************************************************
 * Global Data
 ********************************************************************/

	.data
g_irqtmp:
	.word	0		/* Saved lr */
	.word	0		/* Saved spsr */
g_undeftmp:
	.word	0		/* Saved lr */
	.word	0		/* Saved spsr */
g_aborttmp:
	.word	0		/* Saved lr */
	.word	0		/* Saved spsr */

/********************************************************************
 * Assembly Macros
 ********************************************************************/

/********************************************************************
 * Private Functions
 ********************************************************************/

	.text

/********************************************************************
 * Public Functions
 ********************************************************************/

	.text

/********************************************************************
 * Name: up_vectorirq
 *
 * Description:
 *   Interrupt excetpion. Entered in IRQ mode with spsr = SVC
 *   CPSR, lr = SVC PC
 ********************************************************************/

	.global	up_vectorirq
	.type	up_vectorirq, %function
up_vectorirq:
	/* On entry, we are in IRQ mode.  We are free to use
	 * the IRQ mode r13 and r14.
         *
	 */

	ldr	r13, .Lirqtmp
	sub	lr, lr, #4
	str	lr, [r13]			@ save lr_IRQ
	mrs	lr, spsr
	str	lr, [r13, #4]			@ save spsr_IRQ

	/* Then switch back to SVC mode */

	bic	lr, lr, #MODE_MASK	/* Keep F and T bits */
	orr	lr, lr, #(SVC_MODE | PSR_I_BIT)
	msr	cpsr_c, lr		/* Switch to SVC mode */

	/* Create a context structure.  First set aside a stack frame
	 * and store r0-r12 into the frame.
	 */

	sub	sp, sp, #XCPTCONTEXT_SIZE
	stmia	sp, {r0-r12}		/* Save the SVC mode regs */

	/* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */

	add	r1, sp, #XCPTCONTEXT_SIZE
	mov	r2, r14

	/* Get the values for r15(pc) and CPSR in r3 and r4 */

	ldr	r0, .Lirqtmp		/* Points to temp storage */
	ldmia	r0, {r3, r4}		/* Recover r1=lr_IRQ, r2=spsr_IRQ */

	add	r0, sp, #(4*REG_SP)	/* Offset to pc, cpsr storage */
	stmia	r0, {r1-r4}

	/* Then call the IRQ handler with interrupts disabled. */

	mov	fp, #0			/* Init frame pointer */
	mov	r0, sp			/* Get r1=xcp */
	bl	up_doirq		/* Call the handler */

	/* Restore the CPSR, SVC modr registers and return */
.Lnoirqset:
	ldr	r0, [sp, #(4*REG_CPSR)]	/* Setup the SVC mode SPSR */
	msr	spsr, r0
	ldmia	sp, {r0-r15}^		/* Return */

.Lirqtmp:
	.word	g_irqtmp

	.align	5

/********************************************************************
 * Function: up_vectorswi
 * 
 * Description:
 *   SWI interrupt. We enter the SWI in SVC mode
 ********************************************************************/

	.global	up_vectorswi
	.type	up_vectorswi, %function
up_vectorswi:

	/* The c547x rrload bootloader intemediates all
	 * interrupts.  For the* case of the SWI, it mucked
	 * with the stack to create some temporary registers.
	 * We'll have to recover from this mucking here.
	 */

	ldr	r14, [sp,#-0x4]		/* rrload workaround */

	/* Create a context structure.  First set aside a stack frame
	 * and store r0-r12 into the frame.
	 */

	sub	sp, sp, #XCPTCONTEXT_SIZE
	stmia	sp, {r0-r12}		/* Save the SVC mode regs */

	/* Get the correct values of r13(sp), r14(lr), r15(pc)
	 * and CPSR in r1-r4 */

	add	r1, sp, #XCPTCONTEXT_SIZE
	mov	r2, r14			/* R14 is altered on return from SWI */
	mov	r3, r14			/* Save r14 as the PC as well */
	mrs	r4, spsr		/* Get the saved CPSR */

	add	r0, sp, #(4*REG_SP)	/* Offset to pc, cpsr storage */
	stmia	r0, {r1-r4}

	/* Then call the SWI handler with interrupt disabled.
	 * void up_syscall(struct xcptcontext *xcp)
	 */

	mov	fp, #0			/* Init frame pointer */
	mov	r0, sp			/* Get r0=xcp */
	bl	up_syscall		/* Call the handler */

	/* Restore the CPSR, SVC modr registers and return */

	ldr	r0, [sp, #(4*REG_CPSR)]	/* Setup the SVC mode SPSR */
	msr	spsr, r0
	ldmia	sp, {r0-r15}^		/* Return */

	.align	5

/********************************************************************
 * Name: up_vectordata
 *
 * Description:
 *   Data abort Exception dispatcher. Give control to data
 *   abort handler.  This function is entered in ABORT mode
 *   with spsr = SVC CPSR, lr = SVC PC
 *
 ********************************************************************/
	
	.global	up_vectordata
	.type	up_vectordata, %function
up_vectordata:
	/* On entry we are free to use the ABORT mode registers
	 * r13 and r14
	 */

	ldr	r13, .Ldaborttmp	/* Points to temp storage */
	sub	lr, lr, #8		/* Fixup return */
	str	lr, [r13]		/* Save in temp storage */
	mrs	lr, spsr		/* Get SPSR */
	str	lr, [r13, #4]		/* Save in temp storage */

	/* Then switch back to SVC mode */

	bic	lr, lr, #MODE_MASK	/* Keep F and T bits */
	orr	lr, lr, #(SVC_MODE | PSR_I_BIT)
	msr	cpsr_c, lr		/* Switch to SVC mode */

	/* Create a context structure.  First set aside a stack frame
	 * and store r0-r12 into the frame.
	 */

	sub	sp, sp, #XCPTCONTEXT_SIZE
	stmia	sp, {r0-r12}		/* Save the SVC mode regs */

	/* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */

	add	r1, sp, #XCPTCONTEXT_SIZE
	mov	r2, r14

	/* Get the values for r15(pc) and CPSR in r3 and r4 */

	ldr	r0, .Ldaborttmp		/* Points to temp storage */
	ldmia	r0, {r3, r4}		/* Recover r1=lr_IRQ, r2=spsr_IRQ */

	add	r0, sp, #(4*REG_SP)	/* Offset to pc, cpsr storage */
	stmia	r0, {r1-r4}

	/* Then call the data abort handler with interrupt disabled.
	 * void up_dataabort(struct xcptcontext *xcp)
	 */

	mov	fp, #0			/* Init frame pointer */
	mov	r0, sp			/* Get r0=xcp */
	bl	up_dataabort		/* Call the handler */

	/* Restore the CPSR, SVC modr registers and return */

	ldr	r0, [sp, #(4*REG_CPSR)]	/* Setup the SVC mode SPSR */
	msr	spsr_cxsf, r0
	ldmia	sp, {r0-r15}^		/* Return */

.Ldaborttmp:
	.word	g_aborttmp

	.align	5

/********************************************************************
 * Name: up_vectorprefetch
 *
 * Description:
 *    Prefetch abort exception.  Entered in ABT mode with
 *    spsr = SVC CPSR, lr = SVC PC
 ********************************************************************/
	
	.global	up_vectorprefetch
	.type	up_vectorprefetch, %function
up_vectorprefetch:
	/* On entry we are free to use the ABORT mode registers
	 * r13 and r14
	 */

	ldr	r13, .Lpaborttmp	/* Points to temp storage */
	sub	lr, lr, #4		/* Fixup return */
	str	lr, [r13]		/* Save in temp storage */
	mrs	lr, spsr		/* Get SPSR */
	str	lr, [r13, #4]		/* Save in temp storage */

	/* Then switch back to SVC mode */

	bic	lr, lr, #MODE_MASK	/* Keep F and T bits */
	orr	lr, lr, #(SVC_MODE | PSR_I_BIT)
	msr	cpsr_c, lr		/* Switch to SVC mode */

	/* Create a context structure.  First set aside a stack frame
	 * and store r0-r12 into the frame.
	 */

	sub	sp, sp, #XCPTCONTEXT_SIZE
	stmia	sp, {r0-r12}		/* Save the SVC mode regs */

	/* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */

	add	r1, sp, #XCPTCONTEXT_SIZE
	mov	r2, r14

	/* Get the values for r15(pc) and CPSR in r3 and r4 */

	ldr	r0, .Lpaborttmp		/* Points to temp storage */
	ldmia	r0, {r3, r4}		/* Recover r1=lr_IRQ, r2=spsr_IRQ */

	add	r0, sp, #(4*REG_SP)	/* Offset to pc, cpsr storage */
	stmia	r0, {r1-r4}

	/* Then call the prefetch abort handler with interrupt disabled.
	 * void up_prefetchabort(struct xcptcontext *xcp)
	 */

	mov	fp, #0			/* Init frame pointer */
	mov	r0, sp			/* Get r0=xcp */
	bl	up_prefetchabort	/* Call the handler */

	/* Restore the CPSR, SVC modr registers and return */

	ldr	r0, [sp, #(4*REG_CPSR)]	/* Setup the SVC mode SPSR */
	msr	spsr_cxsf, r0
	ldmia	sp, {r0-r15}^		/* Return */

.Lpaborttmp:
	.word	g_aborttmp

	.align	5

/********************************************************************
 * Name: up_vectorundefinsn
 *
 * Description:
 *   Undefined instruction entry exception.  Entered in
 *   UND mode, spsr = SVC  CPSR, lr = SVC PC
 *
 ********************************************************************/
	
	.global	up_vectorundefinsn
	.type	up_vectorundefinsn, %function
up_vectorundefinsn:
	/* On entry we are free to use the UND mode registers
	 * r13 and r14
	 */

	ldr	r13, .Lundeftmp		/* Points to temp storage */
	str	lr, [r13]		/* Save in temp storage */
	mrs	lr, spsr		/* Get SPSR */
	str	lr, [r13, #4]		/* Save in temp storage */

	/* Then switch back to SVC mode */

	bic	lr, lr, #MODE_MASK	/* Keep F and T bits */
	orr	lr, lr, #(SVC_MODE | PSR_I_BIT)
	msr	cpsr_c, lr		/* Switch to SVC mode */

	/* Create a context structure.  First set aside a stack frame
	 * and store r0-r12 into the frame.
	 */

	sub	sp, sp, #XCPTCONTEXT_SIZE
	stmia	sp, {r0-r12}		/* Save the SVC mode regs */

	/* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */

	add	r1, sp, #XCPTCONTEXT_SIZE
	mov	r2, r14

	/* Get the values for r15(pc) and CPSR in r3 and r4 */

	ldr	r0, .Lundeftmp		/* Points to temp storage */
	ldmia	r0, {r3, r4}		/* Recover r1=lr_IRQ, r2=spsr_IRQ */

	add	r0, sp, #(4*REG_SP)	/* Offset to pc, cpsr storage */
	stmia	r0, {r1-r4}

	/* Then call the undef insn handler with interrupt disabled.
	 * void up_undefinedinsn(struct xcptcontext *xcp)
	 */

	mov	fp, #0			/* Init frame pointer */
	mov	r0, sp			/* Get r0=xcp */
	bl	up_undefinedinsn	/* Call the handler */

	/* Restore the CPSR, SVC modr registers and return */

	ldr	r0, [sp, #(4*REG_CPSR)]	/* Setup the SVC mode SPSR */
	msr	spsr_cxsf, r0
	ldmia	sp, {r0-r15}^		/* Return */

.Lundeftmp:
	.word	g_undeftmp

	.align	5

/********************************************************************
 * Name: up_vectorfiq
 *
 * Description:
 *   Shouldn't happen
 ********************************************************************/

	.global	up_vectorfiq
	.type	up_vectorfiq, %function
up_vectorfiq:
	subs	pc, lr, #4

/********************************************************************
 *  Name: up_vectoraddrexcption
 *
 * Description:
 *   Shouldn't happen
 *
 ********************************************************************/

	.globl	up_vectoraddrexcptn
	.type	up_vectoraddrexcptn, %function
up_vectoraddrexcptn:
	b	up_vectoraddrexcptn

/**************************************************************************
 * Vector initialization block.
 **************************************************************************/

/* These will be relocated to VECTOR_BASE. */

	.globl	_vector_start
_vector_start:
	ldr	pc, .Lresethandler		/* 0x00: Reset */
	ldr	pc, .Lundefinedhandler		/* 0x04: Undefined instruction */
	ldr	pc, .Lswihandler		/* 0x08: Software interrupt */
	ldr	pc, .Lprefetchaborthandler	/* 0x0c: Prefetch abort */
	ldr	pc, .Ldataaborthandler		/* 0x10: Data abort */
	ldr	pc, .Laddrexcptnhandler		/* 0x14: Address exception */
	ldr	pc, .Lirqhandler		/* 0x18: IRQ */
	ldr	pc, .Lfiqhandler		/* 0x1c: FIQ */

.Lresethandler:
	.long   __start
.Lundefinedhandler:
	.long	up_vectorundefinsn
.Lswihandler:
	.long	up_vectorswi
.Lprefetchaborthandler:
	.long	up_vectorprefetch
.Ldataaborthandler:
	.long	up_vectordata
.Laddrexcptnhandler:
	.long	up_vectoraddrexcptn
.Lirqhandler:
	.long	up_vectorirq
.Lfiqhandler:
	.long	up_vectorfiq
	.globl	_vector_end
_vector_end:
	.end
