/**
 * Copyright (c) 2007, Freescale Semiconductor
 * Freescale Confidential Proprietary
 *
 * File name   : Flash.c
 * Project name: VKSP Software Library
 *
 * Author      : Ioseph Martinez
 * Department  : Go to Market AUTO - RTAC Americas
 *
 * Description : VKSP Transmitter Flash Driver 
 *               
 * History     :
 * 14/Jan/2008  : Version 0.1    - 
 */

#include "Flash.h"

const byte flash_cmd_r[] = {0x45, 0x18, 0x25, 0xF6, 0xAA, 0x80, 0xF7, 0x9D,
                            0x9D, 0x9D, 0x9D, 0xC6, 0x18, 0x25, 0xA5, 0x30,
                            0x26, 0x07, 0xC6, 0x18, 0x25, 0xA5, 0x40, 0x27,
                            0xF9, 0x81};
                            
void FLASH_CMD_R(void)
{
	FSTAT = FSTAT | 0x80; //Put FCBEF at 1.
	_asm NOP;//Wait 4 cycles
	_asm NOP;
	_asm NOP;
	_asm NOP;
	if (FSTAT&0x30){ //check to see if FACCERR or FVIOL are set
		return;//if so, error.
	}
	while ((FSTAT&0x40)==0){//else wait for command to complete
		;
	}
}                            

/*******************************************************************************/
/**
* \brief    FlashInit - Initializes the Flash drivers, depends on the bus frequency, check Flash.h
* \author   IM
* \param    void
* \return   void
* \todo
*/         
void FlashInit(void)
{
  if (!(FCDIV & 0x80))
  {
    if (FSTAT&0x30)        
    {
        FSTAT |= 0x30; 
    }
    // FCDIV = FLASH_DIV;
    FCDIV = 0x16;  
  }
}                            

/*******************************************************************************/
/**
* \brief    Flash_Cmd - Writes/Erases the Flash memory, depending on the command used.
* \author   IM
* \param    unsigned int Address : Indicates the used address to program
* \param    unsigned int bData : Indicates data to be programmed, not used when erasing.
* \param    unsigned int cmd : Indicates the operation: Erase or Program bytewise.
* \return   void
* \todo
*/
void Flash_Cmd(unsigned int Address, unsigned char bData, unsigned char cmd)
{
  byte fls_routine[26];
  byte count;
  
  for (count = 0; count < 26;  count++) 
  {
    //fls_routine[count] = flash_cmd_r[count];  
    fls_routine[count] = ((unsigned char *)FLASH_CMD_R)[count];
  }
  DisableInterrupts;
  /* Check to see if FACCERR is set */
	if (FSTAT&0x30){
	  /* Write a 1 to FACCERR to clear */
		FSTAT = FSTAT | 0x30;
	}
	/* Write to somewhere in flash */
	(*((volatile unsigned char *)(Address))) = bData;
	/* Set command type */ 
	FCMD = cmd;
	/* Jump to Flash routine that must be in RAM */                           	 
  ((pt2Func)(fls_routine))();
  EnableInterrupts;
}