/***************************************************************************
 *   Copyright (C) 01/2008 by Olaf Rempel                                  *
 *   razzor@kopf-tisch.de                                                  *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; version 2 of the License                *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
	 	.equ AIC_IVR,         (256) 
		.equ AIC_FVR,         (260) 
		.equ AIC_EOICR,       (304)
		.equ AT91C_BASE_AIC,  (0xFFFFF000)

	    .equ	IRQ_Stack_Size,		(3 * 8 * 4)
	    .equ	FIQ_Stack_Size,		(3 * 8 * 4)
	    .equ	ABT_Stack_Size,		192
	
		.equ	ARM_MODE_FIQ,       0x11
		.equ	ARM_MODE_IRQ,       0x12
		.equ	ARM_MODE_SVC,       0x13
		.equ	ARM_MODE_ABT,       0x17
	
		.equ	I_BIT,              0x80
		.equ	F_BIT,              0x40
		
        .section .text
        .global _start
        .func   _start
    
_start:		ldr     pc, [pc, #24]		/* 0x00 Reset handler */
undefvec:	ldr     pc, [pc, #24]		/* 0x04 Undefined Instruction */
swivec:		ldr     pc, [pc, #24]		/* 0x08 Software Interrupt */
pabtvec:	ldr     pc, [pc, #24]		/* 0x0C Prefetch Abort */
dabtvec:	ldr     pc, [pc, #24]		/* 0x10 Data Abort */
rsvdvec:	ldr     pc, [pc, #24]		/* 0x14 reserved  */
irqvec:		ldr     pc, [pc, #24]		/* 0x18 IRQ	 */
fiqvec:		ldr     pc, [pc, #24]		/* 0x1c FIQ	 */

		/* 0x80000000 will result in Prefetch Abort */
		.word InitReset
		.word 0x80000000
		.word 0x80000000
		.word ABT_Handler_Entry
		.word ABT_Handler_Entry
		.word 0x80000000
		.word IRQ_Handler_Entry
		.word FIQ_Handler_Entry
		
		.endfunc

        .global InitReset
        .func   InitReset

InitReset:
		.extern   at91_init1
			/* Call Low level init function */
            ldr     sp,=__stack_top__
	        ldr	    r0,=at91_init1
            mov     lr, pc
	        bx	    r0
            mov     r0, sp

			/* Setup FIQ Mode Stack */
			msr     CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT
			mov    sp, r0
			sub    r0, r0, #FIQ_Stack_Size
			
			/* store AIC Base in ARM_MODE_FIQ:r8 for faster access */
			ldr     r8, =AT91C_BASE_AIC

			/* Setup IRQ Mode Stack */
            msr     CPSR_c, #ARM_MODE_IRQ | I_BIT | F_BIT
            mov     sp, r0
            sub     r0, r0, #IRQ_Stack_Size

			/* Setup Abort Mode Stack */
            msr     CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT
            mov     sp, r0
            sub     r0, r0, #ABT_Stack_Size
            
			/* Setup Supervisor Mode Stack (IRQ & NMI enabled) */
            msr     CPSR_c, #ARM_MODE_SVC
            mov     sp, r0

			/* Relocate .data section */
			ldr		r1, =__text_end__
			ldr		r2, =__data_start__
			ldr		r3, =__data_end__
LoopRel:	cmp		r2, r3
			ldrlo	r0, [r1], #4
			strlo	r0, [r2], #4
			blo		LoopRel

			/* Clear .bss section */
            mov		r0, #0
            ldr		r1, =__bss_start__
            ldr		r2, =__bss_end__
LoopZI:		cmp		r1, r2
			strlo	r0, [r1], #4
			BLO     LoopZI

			/* Start main() */
			ldr	lr,=exit
			ldr	r0,=main
			bx	r0

		.endfunc

		.global exit
		.func   exit
			/* exit dummy for newlib */
exit:        b    .
		.endfunc

        .global ABT_Handler_Entry
        .func   ABT_Handler_Entry
ABT_Handler_Entry:
			/* disable interrupts (F_BIT not set on entry) */
			msr     CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT

			/* store all registers */
			stmfd       sp!, { r0-r12 }
			
			/* saved cpsr (from aborted mode) */
			mrs			r0, SPSR

			/* address of abort (pc) */
			mov			r3, lr
			
			/* enter previous mode and get lr(r14), sp(r13) */
			/* TODO: interrupts might be enabled? */
			/* TODO: thumb mode enabled? */
			msr			CPSR_c, r0
			mov			r1, sp
			mov			r2, lr

			/* return to abort mode */
			msr			CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT
			
			/* store remaining registers (r1-r3 == r13-r15) */
			stmfd       sp!, { r1-r3 }
			mov			r1, sp

			/* execute C Handler (cpsr, registers) */
	        ldr	    r5,=at91_abt_handler
            mov     lr, pc
	        bx	    r5
	        b		.
		.endfunc

        .global FIQ_Handler_Entry
        .func   FIQ_Handler_Entry
FIQ_Handler_Entry:
			/* Save r0 to ARM_MODE_FIQ:r9 */
            mov         r9,r0

			/* get FIQ Vector from AIC and thus clear FIQ */     
	        ldr         r0 , [r8, #AIC_FVR]

			/* Switch to ARM_MODE_SVC and save registers there */
            msr         CPSR_c,#ARM_MODE_SVC | I_BIT | F_BIT 
            stmfd       sp!, { r1-r3, r12, lr}

			/* execute FIQ in SVC_MODE */ 
            mov         r14, pc
            bx          r0
			
			/* restore registers and switch back to ARM_MODE_FIQ */
            ldmia       sp!, { r1-r3, r12, lr}
            msr         CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT 

			/* restore the ARM_MODE_SVC:r0 */
            mov         r0,r9

			/* restore PC using the LR_fiq directly */
            subs        pc,lr,#4

		.endfunc


        .global IRQ_Handler_Entry
        .func   IRQ_Handler_Entry
IRQ_Handler_Entry:
/*- Manage Exception Entry  */
/*- Adjust and save LR_irq in IRQ stack  */
            sub         lr, lr, #4
            stmfd       sp!, {lr}

/*- Save SPSR need to be saved for nested interrupt */
            mrs         r14, SPSR
            stmfd       sp!, {r14}

/*- Save and r0 in IRQ stack  */
            stmfd       sp!, {r0}

/*- Write in the IVR to support Protect Mode  */
/*- No effect in Normal Mode  */
/*- De-assert the NIRQ and clear the source in Protect Mode */
            ldr         r14, =AT91C_BASE_AIC
			ldr         r0 , [r14, #AIC_IVR]
			str         r14, [r14, #AIC_IVR]

/*- Enable Interrupt and Switch in Supervisor Mode */
            msr         CPSR_c, #ARM_MODE_SVC

/*- Save scratch/used registers and LR in User Stack */
            stmfd       sp!, { r1-r3, r12, r14}

/*- Branch to the routine pointed by the AIC_IVR  */
            mov         r14, pc
            bx          r0
/*- Restore scratch/used registers and LR from User Stack*/
            ldmia       sp!, { r1-r3, r12, r14}

/*- Disable Interrupt and switch back in IRQ mode */
            msr         CPSR_c, #ARM_MODE_IRQ | I_BIT 

/*- Mark the End of Interrupt on the AIC */
            ldr         r14, =AT91C_BASE_AIC
            str         r14, [r14, #AIC_EOICR]

/*- Restore SPSR_irq and r0 from IRQ stack */
            ldmia       sp!, {r0}

/*- Restore SPSR_irq and r0 from IRQ stack */
            ldmia       sp!, {r14}
            msr         SPSR_cxsf, r14

/*- Restore adjusted  LR_irq from IRQ stack directly in the PC */
            ldmia       sp!, {pc}^
	
        .endfunc

		.end