sam7fc/at91_init0.s

/***************************************************************************
 *   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
	.extern   at91_init1
InitReset:
		/* 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