#include "FreeRTOS.h"
#include "flashUtilities.h"
#include "string.h"
/**********************************FUNCTIONS**********************************/
vuint8 flashsemaphoreX = 1;
/*
 * 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.
 */

Nastavenia Settings __attribute__ ((section(".cfmvar"))) = {{10,0,0,150,10,0,0,1,255,255,255,0}};
Nastavenia Settings2 __attribute__ ((section(".cfmvarTemp"))) = {{10,0,0,150,10,0,0,1,255,255,255,0}};

void SaveNastavenia(void){

//		portENTER_CRITICAL();
//		CFMrom2ram(TESTADDRESS, 0, PAGEERASE);
//		/* write new word in selected address */
//
//    	CFMrom2ram(TESTADDRESS, words[i], WRITELONGWORD);
//		portEXIT_CRITICAL();

}


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 (flashsemaphoreX) {
		/* execute this routine just one time */
		memcpy(storage, (void *) CFMcommand, (char *) CFMcommandEND
				- (char *) CFMcommand);
		flashsemaphoreX = 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 */
