/*
 * File:
 *
 * Notes:       
 *              
 */
#include "XTWR-MCF51JE.h"
#include "CFV1_Flash.h"
                              
void 
FlashInit(void)
{
#if 0
  const byte NVPROT_INIT @0x0000040D = 0xFF
  const byte NVOPT_INIT @0x0000040F = 0xC3;

  const byte NVBACKKEY0_INIT @0x00000400 = 0;
  const byte NVBACKKEY1_INIT @0x00000401 = 0;
  const byte NVBACKKEY0_INIT @0x00000402 = 0;
  const byte NVBACKKEY1_INIT @0x00000403 = 0;
  const byte NVBACKKEY0_INIT @0x00000404 = 0;
  const byte NVBACKKEY1_INIT @0x00000405 = 0;
  const byte NVBACKKEY0_INIT @0x00000406 = 0;
  const byte NVBACKKEY1_INIT @0x00000407 = 0;
#endif
  
  if (!(FCDIV & FCDIV_FDIVLD_MASK))
  {
    /*set flash freq*/
    FCDIV = FLASH_CLOCK;
  }
}

UINT8 /*far*/ 
Flash_Cmd(UINT32 FlashAddress, 
          UINT16 FlashDataCounter, 
          UINT32 *pFlashDataPtr, 
          UINT8 FlashCommand)
{
    /* Check to see if FACCERR or PVIOL is set */
    if (FSTAT &0x30)  
    {         
        /* Clear Flags if set*/
        FSTAT = 0x30;  
    }

    if (FlashDataCounter)
    {
      do
      {
          /* Wait for the Last Busrt Command to complete */
          while(!(FSTAT&FSTAT_FCBEF_MASK)){};/*wait until termination*/
          
          /* Write Data into Flash*/
          (*((volatile unsigned long *)(FlashAddress))) = *pFlashDataPtr;
          FlashAddress += 4;
          pFlashDataPtr++;
          
          /* Write Command */
          FCMD = FlashCommand;
          
          /* Put FCBEF at 1 */
          FSTAT = FSTAT_FCBEF_MASK;
          
          asm (NOP);
          asm (NOP);
          asm (NOP);
   
           /* Check if Flash Access Error or Protection Violation Error are Set */
          if (FSTAT&0x30)
          {     
            /* If so, finish the function returning 1 to indicate error */
            return (1);
          }
          
      }while (--FlashDataCounter);
    }
    /* wait for the last command to complete */
    while ((FSTAT&FSTAT_FCCF_MASK)==0){};/*wait until termination*/
    
    /* Return zero to indicate that the function executed OK */
    return (0);
}
