/***************************************************************************
 *   sam7fc - PLL Init, IRQ/FIQ Vectors                                      *
 *                                                                         *
 *   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.             *
 ***************************************************************************/
#include <stdint.h>
#include "AT91SAM7S256.h"

static void empty_isr(void) {}

/*
 * Init critical onchip hardware:
 * - disable Watchdog
 * - enable Oscillator and PLL, switch to 48MHz MCK
 * - set empty Interrupt Handlers
 */
void at91_init1(void)
{
	/* disable watchdog */
	*AT91C_WDTC_WDMR = AT91C_WDTC_WDDIS;

	/* enable user reset */
	*AT91C_RSTC_RMR = (AT91C_RSTC_KEY & 0xA5 << 24) | AT91C_RSTC_URSTEN;

	/* Set Flash Waitstates */
	*AT91C_MC_FMR = AT91C_MC_FWS_1FWS;

	/*
	 * Enable main oscillator (MAINCK)
	 * startup time: 8*6/32768 -> 1.46ms
	 */
	AT91S_PMC *pmc = AT91C_BASE_PMC;
	pmc->PMC_MOR = (AT91C_CKGR_OSCOUNT & (6<<8)) |
			AT91C_CKGR_MOSCEN;
	while (!(pmc->PMC_SR & AT91C_PMC_MOSCS));

	/*
	 * PLLCK = 18.432MHz / 24 * 125 = 96MHz -> div:24, mul:124
	 * startup time: 32/32768 -> 976us
	 */
	pmc->PMC_PLLR = (AT91C_CKGR_DIV & 24) |
			(AT91C_CKGR_MUL & (124<<16)) |
			(AT91C_CKGR_PLLCOUNT & (32<<8)) ;
	while (!(pmc->PMC_SR & AT91C_PMC_LOCK));

	/* MCK = PLLCK / 2 = 48MHz */
	pmc->PMC_MCKR = AT91C_PMC_CSS_PLL_CLK |
			AT91C_PMC_PRES_CLK_2;
	while (!(pmc->PMC_SR & AT91C_PMC_MCKRDY));

	/* enable protected mode (let AIC work with debugger) */
	AT91S_AIC *aic = AT91C_BASE_AIC;
	aic->AIC_DCR = AT91C_AIC_DCR_PROT;

	/* Disable & clear all Interrupts */
	aic->AIC_IDCR = ~0;
	aic->AIC_ICCR = ~0;

	/* default Interrupt Handlers just return */
	aic->AIC_FVR = (uint32_t)empty_isr;
	aic->AIC_IVR = (uint32_t)empty_isr;
	uint32_t i;
	for (i = 0; i < 32; i++) {
		aic->AIC_SMR[i] = 0;
		aic->AIC_SVR[i] = (uint32_t)empty_isr;
	}
	aic->AIC_SPU = (uint32_t)empty_isr;
}

__attribute__((naked)) void IRQ_Handler(void)
{
	asm volatile (
		".equ ARM_MODE_IRQ,	0x12		\n\t"
		".equ ARM_MODE_SVC,	0x13		\n\t"
		".equ I_BIT,		0x80		\n\t"

		".equ AIC_IVR,		(256)		\n\t"
		".equ AIC_EOICR,	(304)		\n\t"
		".equ AT91C_BASE_AIC,	(0xFFFFF000)	\n\t"

		/* Adjust and save lr_irq on IRQ stack */
		"sub	lr, lr, #4				\n\t"
		"stmfd	sp!, { lr }				\n\t"

		/* Save SPSR (for nested interrupts) */
		"mrs	r14, SPSR				\n\t"
		"stmfd	sp!, { r14 }				\n\t"

		/* Save r0 on IRQ stack */
		"stmfd	sp!, { r0 }				\n\t"

		/* Write in the IVR to support Protect Mode */
		"ldr	r14, =AT91C_BASE_AIC			\n\t"
		"ldr	r0, [r14, #AIC_IVR]			\n\t"
		"str	r14, [r14, #AIC_IVR]			\n\t"

		/* Enable Interrupt and switch to SVC mode */
		"msr	CPSR_c, #ARM_MODE_SVC			\n\t"

		/* Save scratch/used registers and lr on SVC Stack */
		"stmfd	sp!, { r1-r3, r12, r14 }		\n\t"

		/* Branch to the routine pointed by the AIC_IVR */
		"mov	r14, pc					\n\t"
		"bx	r0					\n\t"

		/* Restore scratch/used registers and lr from SVC Stack */
		"ldmia	sp!, { r1-r3, r12, r14 }		\n\t"

		/* Disable Interrupt and switch back to IRQ mode */
		"msr	CPSR_c, #ARM_MODE_IRQ | I_BIT		\n\t"

		/* Mark the End of Interrupt on the AIC */
		"ldr	r14, =AT91C_BASE_AIC			\n\t"
		"str	r14, [r14, #AIC_EOICR]			\n\t"

		/* Restore SPSR_irq and r0 from IRQ stack */
		"ldmia	sp!, { r0 }				\n\t"
		"ldmia	sp!, { r14 }				\n\t"
		"msr	SPSR_cxsf, r14				\n\t"

		/* Restore adjusted lr_irq from IRQ stack */
		"ldmia	sp!, { pc }^				\n\t"
	);
}

__attribute__((naked)) void FIQ_Handler(void)
{
	asm volatile (
		".equ ARM_MODE_FIQ,	0x11		\n\t"
		".equ ARM_MODE_SVC,	0x13		\n\t"
		".equ I_BIT,		0x80		\n\t"
		".equ F_BIT,		0x40		\n\t"

		".equ AIC_FVR,		(260)		\n\t"

		/* Save r0 to r9_fiq */
		"mov	r9, r0					\n\t"

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

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

		/* execute FIQ in SVC_MODE */
		"mov	r14, pc					\n\t"
		"bx	r0					\n\t"

		/* restore registers and switch back to FIQ */
		"ldmia	sp!, { r1-r3, r12, lr }			\n\t"
		"msr	CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT	\n\t"

		/* restore the r0 from r9_fiq */
		"mov	r0, r9					\n\t"

		/* restore PC using the lr_fiq directly */
		"subs	pc, lr, #4				\n\t"
	);
}