#include <hidef.h>      /* common defines and macros */
#include <mc9s12p128.h>     /* derivative information */
#include "flash.h"


/************************* global variables **********************************/
tU16 * __far data_ptr;
tU16 * near_ptr;
extern tU16 _Startup;

/******************************************************************************
Function Name	:	FLASH_p_program_flash_configuration_field
Engineer		:	b19005
Date			:	30/11/08
Arguments	:	   
Return		: 0/success; others/fail 
Notes			:	This function programs 0xFFFF,0xFFFF,0xFFFF,0xFFFE to 0x3ff08~0x3ff0f 
            of Flash Configuration Field to unsecure the flash. The SEC[1:0] bits 
            define the security state of the MCU as shown in Table 15-10. P524 of
            MC9S12HY/HA-Family Reference Manual, Rev. 0.12.
******************************************************************************/
#pragma CODE_SEG  VERI_SEG
tU16 FLASH_p_program_flash_configuration_field(tU08 keyen, tU08 unsec)
{
  tU16 wfsec = 0xFFFF, vect0;

  if (unsec)
    wfsec = 0xFFFE; 
  if (keyen)
    wfsec &= 0xFFBF; 
  
  if(LaunchFlashCommand(2, ERASE_P_FLASH_SECTOR, FLASH_P_ADDRGH, FLASH_P_CONFIG_SSADDRGL, 0, 0, 0, 0) != CCIF_MASK) 
    return FLASH_PROGRAM_ERROR; 
  
  if(LaunchFlashCommand(3, ERASE_VERIFY_P_FLASH_SECTION, FLASH_P_ADDRGH, FLASH_P_CONFIG_SSADDRGL, FLASH_P_SECTSIZE>>3, 0, 0, 0) != CCIF_MASK) 
    return FLASH_PROGRAM_ERROR;      
	
	if(LaunchFlashCommand(6, PROGRAM_P_FLASH, FLASH_P_ADDRGH, FLASH_P_CONFIG_SADDRGL, 
	      BOOKDOOR_ACCESS_KEY0, BOOKDOOR_ACCESS_KEY1, BOOKDOOR_ACCESS_KEY2, BOOKDOOR_ACCESS_KEY3) != CCIF_MASK) 
	  return FLASH_PROGRAM_ERROR;
	
	if(LaunchFlashCommand(6, PROGRAM_P_FLASH, FLASH_P_ADDRGH, FLASH_P_CONFIG_SADDRGL+8, 0xFFFF, 0xFFFF, 0xFFFF, wfsec) != CCIF_MASK) 
    return FLASH_PROGRAM_ERROR;
	
	vect0 = (tU16)&(_Startup);
	if(LaunchFlashCommand(6, PROGRAM_P_FLASH, FLASH_P_ADDRGH, FLASH_P_VECTOR0_SADDRGL, 0xFFFF, 0xFFFF, 0xFFFF, vect0) != CCIF_MASK) 
    return FLASH_PROGRAM_ERROR;
  	
	/* verify */
	near_ptr = (tU16 *) FLASH_P_CONFIG_SADDRLOCAL;
	
	if (*near_ptr++ != BOOKDOOR_ACCESS_KEY0 || *near_ptr++ != BOOKDOOR_ACCESS_KEY1 ||
	    *near_ptr++ != BOOKDOOR_ACCESS_KEY2 || *near_ptr++ != BOOKDOOR_ACCESS_KEY3 ||
	    *near_ptr++ != 0xFFFF || *near_ptr++ != 0xFFFF || 
	    *near_ptr++ != 0xFFFF || *near_ptr++ != wfsec )
	    return FLASH_PROGRAM_ERROR; 
	
	near_ptr = (tU16 *) FLASH_P_VECTOR0_SADDRLOCAL;
	   
	if (*near_ptr != vect0)
	    return FLASH_PROGRAM_ERROR; 
	   
	return FLASH_OK;
}

  
/******************************************************************************
Function Name	:	LaunchFlashCommand
Engineer		:	b19005
Date			:	29/11/08
Arguments	:	
Return		:
Notes			:	This function does not check if the Flash is erased.
            This function does not explicitly verify that the data has been 
					  sucessfully programmed.
					  This function must be located in RAM or a flash block not 
					  being programmed.
******************************************************************************/
#pragma CODE_SEG  VERI_SEG
tU08
LaunchFlashCommand(char params, tU08 command, tU08 ccob0, tU16 ccob1, tU16 ccob2, tU16 ccob3, tU16 ccob4, tU16 ccob5)
{
  if(FSTAT_CCIF == 1)
	{																	
		/* Clear any error flags*/	
	  FSTAT = (FPVIOL_MASK | ACCERR_MASK); 

    /* Write the command id / ccob0 */
		FCCOBIX = 0;
		FCCOBHI = command;
		FCCOBLO = ccob0;

    if(++FCCOBIX != params) {
      FCCOB = ccob1; 			/* Write next data word to CCOB buffer. */ 
      if(++FCCOBIX != params) {
  		  FCCOB = ccob2; 			/* Write next data word to CCOB buffer. */
        if(++FCCOBIX != params) {
   		    FCCOB = ccob3; 			/* Write next data word to CCOB buffer. */
          if(++FCCOBIX != params) {
            FCCOB = ccob4; 			/* Write next data word to CCOB buffer. */
            if(++FCCOBIX != params) 
         		  FCCOB = ccob5; 			/* Write next data word to CCOB buffer. */
          } 											
        }  
	    }
    }
	  FCCOBIX = params-1;
	  
	 	/* Clear command buffer empty flag by writing a 1 to it */
		FSTAT = CCIF_MASK;
    while (!FSTAT_CCIF) {					/* wait for the command to complete */
      /* Return status. */
    }
    return(FSTAT);							/* command completed */
  } 
  
	return(FLASH_BUSY);								   /* state machine busy */
}
  

/******************************************************************************
Function Name	:	ReadWordFromPhrase
Engineer		:	b19005
Date			:	04/12/08
Arguments	:	index: 0,1,2,3
Return		:
Notes			:	This function reads the selected word from a reserved 64 byte field 
            (8 phrases) in the nonvolatile information register located in P-Flash. 
******************************************************************************/
#pragma CODE_SEG  VERI_SEG
tU16 ReadWordFromPhrase(tU08 index) {

  	FCCOBIX = 2 + index;
  	
  	return FCCOB; 
}