//*--------------------------------------------------------------------------------------
//*      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               : FlashSemiHosting.c
//* Object                  : FLASH programmer for SAM7 product
//*
//* Creation                : JPP  18/Aug/2004
//* V 1.1 24/Feb/2005 JPP   : caste the page value & init Pll
//*--------------------------------------------------------------------------------------

/* Include Standard c Libraries to allow stand alone compiling and operation */
#include <stdio.h>
#include <ioat91sam7s32.h>

#include "interface.h"
#include "driverConfig.h"
#include "AT91SAM7SFlash.h"
extern void AT91F_LowLevelInit( void) ;
static char message[70];
static unsigned char RAMSectorData[FLASH_PAGE_SIZE_BYTE];
static int basseAddress ;
static unsigned int NbByte;
static int page,page_prev;
//*----------------------------------------------------------------------------
//* \fn    AT91F_LowLevelInit
//* \brief This function performs very low level HW initialization
//*        this function can be use a Stack, depending the compilation
//*        optimization mode
//*----------------------------------------------------------------------------
void AT91F_LowLevelInit( void)
{

 AT91PS_PMC     pPMC = AT91C_BASE_PMC;
    //* Set Flash Waite sate
	//  Single Cycle Access at Up to 30 MHz, or 40
	//  if MCK = 47923200 I have 50 Cycle for 1 useconde ( flied MC_FMR->FMCN
	    AT91C_BASE_MC->MC_FMR = ((AT91C_MC_FMCN)&(50 <<16)) | AT91C_MC_FWS_1FWS ;

    //* Watchdog Disable
        AT91C_BASE_WDTC->WDTC_WDMR= AT91C_WDTC_WDDIS;

	//* Set MCK at 47 923 200
    // 1 Enabling the Main Oscillator:
        // SCK = 1/32768 = 30.51 uSeconde
    	// Start up time = 8 * 6 / SCK = 56 * 30.51 = 1,46484375 ms
       pPMC->PMC_MOR = (( AT91C_CKGR_OSCOUNT & (0x06 <<8) | AT91C_CKGR_MOSCEN ));
        // Wait the startup time
        while(!(pPMC->PMC_SR & AT91C_PMC_MOSCS));
	// 2 Checking the Main Oscillator Frequency (Optional)
	// 3 Setting PLL and divider:
		// - div by 5 Fin = 3,6864 =(18,432 / 5)
		// - Mul 25+1: Fout =	95,8464 =(3,6864 *26)
		// for 96 MHz the erroe is 0.16%
		// Field out NOT USED = 0
		// PLLCOUNT pll startup time esrtimate at : 0.844 ms
		// PLLCOUNT 28 = 0.000844 /(1/32768)
       pPMC->PMC_PLLR = ((AT91C_CKGR_DIV & 0x05) |
                         (AT91C_CKGR_PLLCOUNT & (28<<8)) |
                         (AT91C_CKGR_MUL & (25<<16)));

        // Wait the startup time
        while(!(pPMC->PMC_SR & AT91C_PMC_LOCK));
	// 4. Selection of Master Clock and Processor Clock
        // select the PLL clock divided by 2
           pPMC->PMC_MCKR =  AT91C_PMC_PRES_CLK_2 ;
           while(!(pPMC->PMC_SR & AT91C_PMC_MCKRDY));
  
           pPMC->PMC_MCKR |= AT91C_PMC_CSS_PLL_CLK  ;
           while(!(pPMC->PMC_SR & AT91C_PMC_MCKRDY));

}

//*----------------------------------------------------------------------------
//* \fn    AT91F_Flash_Ready
//* \brief Wait the flash ready
//*----------------------------------------------------------------------------
void AT91F_Flash_Ready (void)
{
    //* Wait the and of command
     while ((AT91C_BASE_MC->MC_FSR & AT91C_MC_FRDY) != AT91C_MC_FRDY ) {};
}

//*----------------------------------------------------------------------------
//* \fn    AT91F_Flash_Unlock
//* \brief Clear the Non Volatile Memory Bits and set at 1 FSR bit=0
//* \input page number (0-1023) a same region have some page (16)
//* \output Region
//*----------------------------------------------------------------------------
void AT91F_Flash_Unlock(unsigned int Flash_Lock_Page)
{    //* Write the Errase All command
        AT91C_BASE_MC->MC_FCR = AT91C_MC_CORRECT_KEY | AT91C_MC_FCMD_UNLOCK | (AT91C_MC_PAGEN & (Flash_Lock_Page << 8) ) ;
    //* Wait the and of command
        AT91F_Flash_Ready();
}

//*----------------------------------------------------------------------------
//* \fn    AT91F_Flash_Write
//* \brief Write in one Flash page located in AT91C_IFLASH,  size in 32 bits
//* \input Flash_Address: start at 0x0010 0000 size: in byte
//*----------------------------------------------------------------------------
int AT91F_Flash_Write( unsigned int Flash_Address ,int size ,unsigned int * buff)
{    //* set the Flasc controler base address
    AT91PS_MC ptMC = AT91C_BASE_MC;
    unsigned int i, page;
    unsigned int * Flash;
//    sprintf(message,"Download:add %x\n",Flash_Address);
//    FlMessageLog(message);

    //* init flash pointer
        Flash = (unsigned int *) Flash_Address;
    //* Get the Flasg page number
        page = ((Flash_Address - (unsigned int)AT91C_IFLASH ) /FLASH_PAGE_SIZE_BYTE) & AT91C_PAGE_MASK;
   //* copy the new value
	for (i=0; (i < FLASH_PAGE_SIZE_BYTE) & (size > 0) ;i++, Flash++,buff++,size-=4 ){
	//* copy the flash to the write buffer ensure that code generation
	    *Flash=*buff;
	}
    //* Write the Errase_All command
        ptMC->MC_FCR = AT91C_MC_CORRECT_KEY | AT91C_MC_FCMD_START_PROG | (AT91C_MC_PAGEN & (page <<8)) ;
    //* Wait the end of command
         AT91F_Flash_Ready();
    //* Check the result
        if ( (ptMC->MC_FSR & ( AT91C_MC_PROGE | AT91C_MC_LOCKE ))!=0) return false;
  return true;
}
//*----------------------------------------------------------------------------
//* \fn    FlashWriteByte
//* \brief The bytes are buffered in sectorbuf. The sectorbuf is written to
//*        the flash when it overflows.
//* \input Flash_Address: start at 0x0010 0000 size: in byte
//*----------------------------------------------------------------------------

static void FlashWriteByte(unsigned long  addr, int byte)
{
  // get page
  page  = ((basseAddress + addr) - FLASH_BASE_ADDRESS ) /FLASH_PAGE_SIZE_BYTE;
#ifdef DEBUG
  FlMessageLog("+");
#endif
  if (byte == -1)
  { // Flush pending data.
     page = page_prev + 1 ;
  } // end flush

  //* Check if page must write
  if (page != page_prev)
  {// New Page
           sprintf(message,"Download:page %d\n",page_prev);
           FlMessageLog(message);
        //* Unlock current sector base address - curent address by sector size
        AT91F_Flash_Unlock(page_prev);
        //* Write page and get status
        if (!AT91F_Flash_Write( (page_prev*FLASH_PAGE_SIZE_BYTE)+ FLASH_ADDRESS, FLASH_PAGE_SIZE_BYTE , (unsigned int *) &RAMSectorData)) {
            sprintf(message,"Flasher cannot write page: %d\n",page_prev);
            FlMessageBox(message);
            FlErrorExit();
        }
 	// Copy Rom page to RAM page
        for (NbByte = 0; NbByte < FLASH_PAGE_SIZE_BYTE; NbByte++)
        {
           RAMSectorData[NbByte] = ((unsigned char*) ((page*FLASH_PAGE_SIZE_BYTE)+basseAddress)) [NbByte];
        }
        //* set works page
        page_prev = page ;
   }
   //* Write date
    RAMSectorData[addr & 0x7F] = byte;
}

//*----------------------------------------------------------------------------
//* \fn    FlashDriverInitialize
//* \brief Init the Flash driver
//*----------------------------------------------------------------------------
void FlashDriverInitialize(int argc, char const* argv[])
{    // check param

    int i,status;
    char val;
    basseAddress = 0;
    AT91F_LowLevelInit();
    sprintf(message,"Downloader Version 1.1\n");
    FlMessageLog(message);

    // get the Flash base address
    if (argc >= 2){
        NbByte=sprintf(message,"%s",argv[1]);
        if (message[1]!='x'){
            FlMessageBox("Param ERROR ! must be 0xYYYYYY");

         FlErrorExit();
        }
        for (i=0 ; NbByte > 1 ; NbByte--, i++){
            val = message[NbByte-1] & 0x0f;
            if ( message[NbByte-1]  > 0x40)  val += 9;
            basseAddress +=  val << (4 *i) ;
        }
        status = true;
        sprintf(message,"Download: AT91SAM7Sx At:%s",argv[1]);
    }
    else
    {
        basseAddress = FLASH_ADDRESS;
        sprintf(message,"Download1: AT91SAM7Sx At: 0x%X\n",basseAddress);
        status = false;
    }

     //* Get the Flash version
    FlMessageLog(message);
    //* Get the Flash version
    sprintf(message,"Download: AT91SAM7Sx Version: 0x%X\n",*AT91C_MC_FVR);
    FlMessageLog(message);
   // get page
    page  =  (((basseAddress )- (unsigned int) FLASH_ADDRESS ) /FLASH_PAGE_SIZE_BYTE)&AT91C_PAGE_MASK;
    page_prev = page;
    for (NbByte = 0; NbByte < FLASH_PAGE_SIZE_BYTE; NbByte++)
    {
        RAMSectorData[NbByte] = ((unsigned char*) ( (page_prev*FLASH_PAGE_SIZE_BYTE)+basseAddress)) [NbByte];
    }
    if (status) basseAddress = 0;

    //* Set Flash Waite sate
    //  Single Cycle Access at Up to 30 MHz
    //  if MCK = 47923200 I have 50 Cycle for 1 useconde ( flied MC_FMR->FMCN)
    //  = A page erase is performed before programming.
        AT91C_BASE_MC->MC_FMR = ((AT91C_MC_FMCN)&(50 <<16)) | AT91C_MC_FWS_1FWS ;

    // Register the flash write function.
    FlRegisterWriteFunction(FlashWriteByte);
}