//*----------------------------------------------------------------------------
//*         ATMEL Microcontroller Software Support  -  ROUSSET  -
//*----------------------------------------------------------------------------
//* The software is delivered "AS IS" without warranty or condition of any
//* kind, either express, implied or statutory. This includes without
//* limitation any warranty or condition with respect to merchantability or
//* fitness for any particular purpose, or against the infringements of
//* intellectual property rights of others.
//*----------------------------------------------------------------------------
//* File Name           : init.c
//* Object              : Low level initialisations written in C
//* Creation            : FB   23/10/2002
//*
//*----------------------------------------------------------------------------

#include "board.h"
#include <stdarg.h>
#include <stdio.h>

#define DELAY			 		100000
#define AT91C_BAUDRATE_115200	115200

#define SAMBA

void delay(int time){
	while(time--);
}
//*----------------------------------------------------------------------------
//* \fn    AT91F_DBGU_Printk
//* \brief This function is used to send a string through the DBGU channel (Very low level debugging)
//*----------------------------------------------------------------------------
void AT91F_DBGU_Printk(
	char *buffer) // \arg pointer to a string ending by \0
{
	while(*buffer != '\0') {
		while (!AT91F_US_TxReady((AT91PS_USART)AT91C_BASE_DBGU));
		AT91F_US_PutChar((AT91PS_USART)AT91C_BASE_DBGU, *buffer++);
	}
	while ( !((AT91C_BASE_DBGU->DBGU_CSR) & AT91C_US_TXEMPTY) );
}

void uprintf(char *fmt,...)
{
    va_list ap;
    char string[256];
    va_start(ap,fmt);
    vsprintf(string,fmt,ap);
    AT91F_DBGU_Printk(string);
    va_end(ap);
}

//*----------------------------------------------------------------------------
//* \fn    AT91F_Putc
//* \brief This function sends a car through the DBGU
//*----------------------------------------------------------------------------
int AT91F_Putc(int ch) 
{
    /* Our implementation of fputc */ 
    while (!AT91F_US_TxReady((AT91PS_USART)AT91C_BASE_DBGU));
    AT91F_US_PutChar((AT91PS_USART)AT91C_BASE_DBGU, (char)ch);
    return ch; 
}


//*----------------------------------------------------------------------------
//* \fn    AT91F_Getc
//* \brief This function receives a car through the DBGU
//*----------------------------------------------------------------------------
int AT91F_Getc() 
{
    while(!AT91F_US_RxReady((AT91PS_USART)AT91C_BASE_DBGU));
    return((int)AT91F_US_GetChar((AT91PS_USART)AT91C_BASE_DBGU));
}
//*----------------------------------------------------------------------------
//* \fn    AT91F_SpuriousHandler
//* \brief This function reports an Abort
//*----------------------------------------------------------------------------
void AT91F_SpuriousHandler() 
{
	AT91F_DBGU_Printk("-F- Spurious Interrupt detected\n\r");
	while (1);
}

//*----------------------------------------------------------------------------
//* \fn    AT91F_DataAbort
//* \brief This function reports an Abort
//*----------------------------------------------------------------------------
void AT91F_DataAbort() 
{
	AT91F_DBGU_Printk("-F- Data Abort detected\n\r");
	while (1);
}

//*----------------------------------------------------------------------------
//* \fn    AT91F_FetchAbort
//* \brief This function reports an Abort
//*----------------------------------------------------------------------------
void AT91F_FetchAbort()
{
	AT91F_DBGU_Printk("-F- Prefetch Abort detected\n\r");
	while (1);
}

//*----------------------------------------------------------------------------
//* \fn    AT91F_Undef
//* \brief This function reports an Abort
//*----------------------------------------------------------------------------
void AT91F_Undef() 
{
	AT91F_DBGU_Printk("-F- Undef detected\n\r");
	while (1);
}

//*----------------------------------------------------------------------------
//* \fn    AT91F_UndefHandler
//* \brief This function reports that no handler have been set for current IT
//*----------------------------------------------------------------------------
void AT91F_UndefHandler() 
{
	AT91F_DBGU_Printk("-F- UndefHandler detected\n\r");
	while (1);
}
//*----------------------------------------------------------------------------
//* \fn    AT91F_LowLevelInit
//* \brief This function performs very low level HW initialization
//*----------------------------------------------------------------------------
void AT91F_LowLevelInit()
{
	/////////////////////////////////////////////////////////////////////////////////////////////////////
	// Test PMC Initialization.
	/////////////////////////////////////////////////////////////////////////////////////////////////////

	unsigned int i;
	unsigned int	tmp	= 0;

	AT91PS_PMC pPmc = AT91C_BASE_PMC;
	// Disable watchdog
	*(AT91C_WDTC_WDMR) = AT91C_WDTC_WDDIS;


/////////////////////////////////////////////////////////////////////////////////////////////////////
// PMC Init Step 3.
// Setting PLL and Divider
/////////////////////////////////////////////////////////////////////////////////////////////////////

	AT91C_BASE_CKGR->CKGR_PLLBR = AT91C_CKGR_USBDIV_1 | AT91C_CKGR_OUTB_0 | AT91C_CKGR_PLLBCOUNT |\
								(AT91C_CKGR_MULB & (0x60<<16)) | (AT91C_CKGR_DIVB & 0x09);		// 198MHz
	// Wait for PLL stabilization LOCK bit in PMC_SR 
	tmp = 0;
	while( !(AT91C_BASE_PMC->PMC_SR & AT91C_PMC_LOCKB) && (tmp++ < DELAY) );

/////////////////////////////////////////////////////////////////////////////////////////////////////
// PMC Init Step 4.
// Selection of Master Clock MCK =99MHz (so Processor Clock PCK = 198MHz)
/////////////////////////////////////////////////////////////////////////////////////////////////////
		
	AT91C_BASE_PMC->PMC_MCKR = AT91C_PMC_CSS_PLLB_CLK|AT91C_PMC_MDIV_2;
	
	// Wait until the master clock is established
	tmp = 0;
	while( !(AT91C_BASE_PMC->PMC_SR & AT91C_PMC_MCKRDY) && (tmp++ < DELAY) );

/////////////////////////////////////////////////////////////////////////////////////////////////////
// DBGU Initialisation
/////////////////////////////////////////////////////////////////////////////////////////////////////
	// Init Interrupt Controller
	AT91F_AIC_Open(
		AT91C_BASE_AIC,          // pointer to the AIC registers
		AT91C_AIC_BRANCH_OPCODE, // IRQ exception vector
		AT91F_UndefHandler,      // FIQ exception vector
		AT91F_UndefHandler,      // AIC default handler
		AT91F_SpuriousHandler,   // AIC spurious handler
		0);                      // Protect mode

	// Perform 8 End Of Interrupt Command to make sure AIC will not Lock out nIRQ 
	AT91F_AIC_AcknowledgeIt(AT91C_BASE_AIC);
	AT91F_AIC_AcknowledgeIt(AT91C_BASE_AIC);
	AT91F_AIC_AcknowledgeIt(AT91C_BASE_AIC);
	AT91F_AIC_AcknowledgeIt(AT91C_BASE_AIC);
	AT91F_AIC_AcknowledgeIt(AT91C_BASE_AIC);
	AT91F_AIC_AcknowledgeIt(AT91C_BASE_AIC);
	AT91F_AIC_AcknowledgeIt(AT91C_BASE_AIC);
	AT91F_AIC_AcknowledgeIt(AT91C_BASE_AIC);

	AT91F_AIC_SetExceptionVector((unsigned int *)0x0C, AT91F_FetchAbort);
	AT91F_AIC_SetExceptionVector((unsigned int *)0x10, AT91F_DataAbort);
	AT91F_AIC_SetExceptionVector((unsigned int *)0x4, AT91F_Undef);

	// Open PIO for DBGU
	AT91F_DBGU_CfgPIO();

	// Configure DBGU
	AT91F_US_Configure (
		(AT91PS_USART) AT91C_BASE_DBGU,		// DBGU base address
		AT91C_MASTER_CLOCK,                  				
		AT91C_US_ASYNC_MODE,        		// mode Register to be programmed
		AT91C_BAUDRATE_115200 ,             // baudrate to be programmed
		0);                        			// timeguard to be programmed

	// Enable Transmitter
	AT91F_US_EnableTx((AT91PS_USART) AT91C_BASE_DBGU);
	// added in v 1.1
	AT91F_US_EnableRx((AT91PS_USART) AT91C_BASE_DBGU);


}