#include "S08_Flash.h"

extern char __SEG_START_FLASH_ROUTINES[];
extern char __SEG_SIZE_FLASH_ROUTINES[];


#pragma CODE_SEG DEFAULT
void FlashInit(void)
{
  if (!(FCDIV & 0x80))
  {
    if (FSTAT&0x30)         //Check to see if FACCERR is set
    {
        FSTAT |= 0x30;  //write a 1 to FACCERR to clear
    }
    FCDIV = FLASH_CLOCK;
  }
}

#pragma CODE_SEG FLASH_ROUTINES
UINT8 Flash_Cmd(UINT16 FlashAddress, UINT8 FlashDataCounter, UINT8* pFlashDataPtr, UINT8 FlashCommand)
{
    /* Check to see if FACCERR or PVIOL is set */
    if (FSTAT&(FSTAT_FACCERR_MASK | FSTAT_FPVIOL_MASK))  
    {         
        /* Clear Flags if set*/
        FSTAT |= FSTAT_FACCERR_MASK | FSTAT_FPVIOL_MASK;  
    }

    if (FlashDataCounter)
    {
      do
      {
          /* Wait for the Last Busrt Command to complete */
          while(!(FSTAT_FCBEF));
          
          /* Write Data into Flash*/
          (*((volatile UINT8 *)(FlashAddress++))) = *pFlashDataPtr;
          pFlashDataPtr++;
          /* Write Command */
          FCMD = FlashCommand;
          /* Put FCBEF at 1 */
          FSTAT = 0x80;
          
          /* Wait at least 4 cycles to read the Error Flags */
          _asm NOP;
          _asm NOP;
          _asm NOP;
          _asm NOP;
          
          /* Check if Flash Access Error or Protection Violation Error are Set */
          if (FSTAT&(FSTAT_FACCERR_MASK | FSTAT_FPVIOL_MASK))
          {     
            /* If so, finish the function returning 1 to indicate error */
            return (FLASH_ERROR);
          }
      }while (--FlashDataCounter);
    }
    /* wait for the last command to complete */
    while (!(FSTAT_FCCF));
    /* Return zero to indicate that the function executed Ok */
    return (FLASH_OK);
}

#pragma CODE_SEG DEFAULT
/*
  This section of the code is the one that copies the routine into RAM. It is following the steps
  documented in Technical Note 228
*/

/*
Start_Copy_In_RAM refers to the begining of the segment ToCopyToRAM. 
This segment contains the functions after they have been copied to RAM.
*/
#define Start_Copy_In_RAM __SEG_START_FLASH_ROUTINES
#define Size_Copy_In_RAM __SEG_SIZE_FLASH_ROUTINES

void CopyInRAM(void) 
{
  char *srcPtr, *dstPtr;
  int count;
  srcPtr = (char *)Start_Copy_In_RAM;
  dstPtr = (char *)&Flash_Cmd;
  for (count = 0; count < (int) Size_Copy_In_RAM;  count++, dstPtr++, srcPtr++) 
  {
    *dstPtr = *srcPtr;
  }
}