#include "FreeRTOS.h"
/**********************************FUNCTIONS**********************************/

/*
 * CFMinit: configure write times and sector protection
 *
 * Parameters: none.
 *
 * Return: none.
 */
void CFMinit()
{
	/* Select FLASH frequency to 200 KHz. Section 15.4.3.1 MCF5213RM */
	MCF_CFM_CFMCLKD = MCF_CFM_CFMCLKD_DIV(25)
					| MCF_CFM_CFMCLKD_PRDIV8;
	/* clear the lock bit */
	MCF_CFM_CFMMCR &= ~MCF_CFM_CFMMCR_LOCK;
	/* clear sector protection registers if set from reset */
	MCF_CFM_CFMPROT = 0x0;
	MCF_CFM_CFMSACC = 0x0;
	MCF_CFM_CFMDACC = 0x0;

	return;
}

/*
 * CFMrom2ram: function that copy from FLASH to SRAM CFMcommand function to
 *				allow user modify FLASH
 *
 * Parameters: address: where to write in FLASH (32-bit aligned)
 *			   value: what to write in FLASH
 *			   command: what to do with address and value
 *
 * Return: none.
 */
void CFMrom2ram(uint32 address, uint32 value, uint32 command)
{
	/* variable to store function in RAM */
	static char storage[200];
	/* new RAM function point to variable to store function */
	static void (*pRAMfunction)(uint32,uint32,uint32) =
 			   (void(*)(uint32,uint32,uint32))storage;
	/* copy FLASH function to variable to store function in RAM */
	if(flashsemaphore)
	{
		/* execute this routine just one time */
		memcpy(storage,
			  (void *)CFMcommand,
			  (char *)CFMcommandEND - (char *)CFMcommand);
		flashsemaphore = 0;
	}
	/* execute function that points to variable that store function in RAM */
	pRAMfunction(address ,value, command);

	return;
}

/*
 * CFMcommand: modify FLASH content during runtime
 *			   this code must be run out of internal FLASH
 *			   only can be called directly if entire code
 *			   is running in SRAM
 *
 * Parameters: address: where to write in FLASH (32-bit aligned)
 *			   value: what to write in FLASH
 *			   command: what to do with address and value
 * Return: none.
 */
void CFMcommand(uint32 address, uint32 value, uint32 command)
{
	/* if buffer command is ready to accept, then write it in buffer */
	if(MCF_CFM_CFMUSTAT & MCF_CFM_CFMUSTAT_CBEIF)
	{
		FLASH_PROGRAM_ADDR(address) = value;		/* write in FLASH */
		MCF_CFM_CFMCMD = command;					/* command to execute */
		MCF_CFM_CFMUSTAT = MCF_CFM_CFMUSTAT_CBEIF;	/* clear buffer */
	}
	/* wait for commands to finish */
	while(!(MCF_CFM_CFMUSTAT & MCF_CFM_CFMUSTAT_CCIF));

	return;
}
void CFMcommandEND(){}
/* leave this immediately after CFMcommand */
