/***************************************************************************
 *   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;
}

/* TODO: make it static */
uint32_t nested_count = 0;

__attribute__((naked)) void IRQ_Handler(void)
{
	asm volatile (
		".equ AIC_IVR_OFF,	(256)		\n\t"
		".equ AIC_EOICR_OFF,	(304)		\n\t"
		".equ AT91C_BASE_AIC,	(0xFFFFF000)	\n\t"
		".equ ARM_MODE_IRQ,	0x12		\n\t"
		".equ ARM_MODE_SYS,	0x1F		\n\t"
		".equ I_BIT,		0x80		\n\t"
		".equ F_BIT,		0x40		\n\t"

		/* Manage Exception Entry */
		/* Save LR_irq to IRQ stack */
		"sub	lr, lr, #4			\n\t"
		"stmdb	sp!, { lr }			\n\t"

		/* Save r0 and SPSR to IRQ stack */
		"mrs	r14, SPSR			\n\t"
		"stmdb	sp!, { r0, r14 }		\n\t"

		/* count nested interrupts */
		"ldr	r14, =nested_count		\n\t"
		"ldr	r0, [r14]			\n\t"
		"add	r0, r0, #1			\n\t"
		"str	r0, [r14]			\n\t"

		/* De-assert the NIRQ and clear the source in Protect Mode */
		"ldr	r14, =AT91C_BASE_AIC		\n\t"
		"ldr	r0, [r14, #AIC_IVR_OFF]		\n\t"
		"str	r14, [r14, #AIC_IVR_OFF]	\n\t"

		/* Enable Interrupt and Switch in Supervisor Mode */
		"msr	CPSR_c, #ARM_MODE_SYS		\n\t"

		/* Save scratch/used registers and LR in User Stack */
		"stmdb	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 User Stack */
		"ldmia	sp!, { r1-r3, r12, r14 }	\n\t"

		/* Disable Interrupt and switch back in 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_OFF]	\n\t"

		/* count nested interrupts */
		"ldr	r14, =nested_count		\n\t"
		"ldr	r0, [r14]			\n\t"
		"subs	r0, r0, #1			\n\t"
		"str	r0, [r14]			\n\t"
		"beq	irq_ctx_save			\n\t"

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

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

		/* save the userspace context to current_context */
		"irq_ctx_save:				\n\t"

		/* get top of struct register_context */
		"ldr	r0, =current_context		\n\t"
		"ldr	r0, [r0]			\n\t"
		"add	r0, r0, #68			\n\t"

		/* save usermode cpsr & r2-14 */
		"mrs	r1, spsr		\n\t"
		"stmdb	r0, {r1-r14}^		\n\t"
		"nop				\n\t"
		"sub	r0, r0, #56		\n\t"

		/* save r0-1 and svc_lr (= pc) */
		"ldmia	sp!, {r1, r2}		\n\t"
		"stmdb	r0!, {r1, r2, r14}	\n\t"

	);
}