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