#include "arm9.h"
#include "at91sam9260.h"

/************************************************
 * DEFINITIONS                                  *
 ************************************************/

#define STACK_SIZE   0x2000


/************************************************
 * VECTOR TABLE                                 *
 ************************************************/

	.section .excvec
	.globl _reset

_reset:

/* Exception vectors */
	b 	reset_vector    /* reset */
	b 	undef_vector 	/* Undefined Instruction */
	b 	swi_vector   	/* Software Interrupt */
	b 	pabt_vector  	/* Prefetch Abort */
	b 	dabt_vector  	/* Data Abort */
	.word	_edata		/* Size of the image for SAM-BA */
	ldr pc, [pc, #-0xf20]   /* IRQ : read the AIC */
	b 	fiq_vector      /* FIQ */

/************************************************
 * EXCEPTION HANDLERS                           *
 ************************************************/

undef_vector:
	ldr		r0, =eh_undef
	blx		r0
	b		reset_vector

swi_vector:
	ldr		r0, =eh_swi
	blx		r0
	b		reset_vector

pabt_vector:
	ldr 	r0, =eh_pabt
	bx		r0
	b		reset_vector

dabt_vector:
	ldr 	r0, =eh_dabt
	bx		r0
	b		reset_vector

fiq_vector:
	ldr		r0, =eh_fiq
	bx		r0
	b		reset_vector

reset_vector:
	ldr		r0, =_start
	blx 	r0
	
	.text
	.globl _start

/************************************************
 * INITIALIZE STACK POINTER                     *
 ************************************************/

_start:

	ldr     r0,=TOP_OF_MEM

/* SUPERVISOR Mode */
	mov     r1, #ARM_MODE_SVC | I_BIT | F_BIT
	msr     cpsr_c, r1
	mov     sp, r0

/* IRQ Mode */
	sub     r0, r0, #STACK_SIZE
	mov     r1, #ARM_MODE_IRQ | I_BIT | F_BIT
	msr     cpsr_c, r1
	mov     sp, r0

/* FIQ Mode */
	sub     r0, r0, #STACK_SIZE
	mov     r1, #ARM_MODE_FIQ | I_BIT | F_BIT
	msr     cpsr_c, r1
	mov     sp, r0

/* ABORT Mode */
	sub     r0, r0, #STACK_SIZE
	mov     r1, #ARM_MODE_ABORT | I_BIT | F_BIT
	msr     cpsr_c, r1
	mov     sp, r0

/* UDEF Mode */
	sub     r0, r0, #STACK_SIZE
	mov     r1, #ARM_MODE_UDEF | I_BIT | F_BIT
	msr     cpsr_c, r1
	mov     sp, r0

/* USER/SYSTEM Mode */
	sub     r0, r0, #STACK_SIZE
	mov     r1, #ARM_MODE_SYSTEM | I_BIT | F_BIT
	msr     cpsr_c, r1
	mov     sp, r0

/************************************************
 * COPY VECTOR TABLE                            *
 ************************************************/

		ldr     r0, =_sexcvec
		ldr     r1, =_eexcvec
		mov     r2, #0
1:
		cmp     r0, r1
		ldmltia r0!, {r3}
		stmltia r2!, {r3}
		blt     1b

/************************************************
 * zero the .bss section                        *
 ************************************************/

        ldr     r1, =_sbss
        ldr     r2, =_ebss
        mov     r0, #0
1:
        cmp     r1, r2
        stmltia r1!, {r0}
		blt     1b

/************************************************
 * ENABLE IRQ                                   *
 ************************************************/

/* Enable IRQ */
	mov     r1, #ARM_MODE_SYSTEM | F_BIT
	msr     cpsr_c, r1

/************************************************
 * BRANCH TO MAIN                               *
 ************************************************/

	ldr		r1,	=main
	blx		r1

	/* if main ever returns */
	ldr		r0, =eh_return
	blx		r0
	b		reset_vector
	
	

	.end