uint_32 _mcf5441_int_init
(
// [IN} Interrupt number
PSP_INTERRUPT_TABLE_INDEX irq,
// [IN} Interrupt priority level
_int_level level,
// [IN} Unmask the interrupt now?
boolean unmask
)
{
_mqx_int idx;
uint_32 temp;
if (irq >= PSP_INT_FIRST_EXTERNAL) {
idx = irq - PSP_INT_FIRST_EXTERNAL;
temp = _psp_get_sr();
_psp_set_sr(temp | 0x0700);
if (idx < 64) {
PSP_GET_ICTRL0_BASE()->ICR[idx] = level & 7;
if (unmask)
PSP_GET_ICTRL0_BASE()->CIMR = MCF54XX_ICTRL_IMR_N(idx);
else
PSP_GET_ICTRL0_BASE()->SIMR = MCF54XX_ICTRL_IMR_N(idx);
}
else if (idx < 128) {
idx -= 64;
PSP_GET_ICTRL1_BASE()->ICR[idx] = level & 7;
if (unmask)
PSP_GET_ICTRL1_BASE()->CIMR = MCF54XX_ICTRL_IMR_N(idx);
else
PSP_GET_ICTRL1_BASE()->SIMR = MCF54XX_ICTRL_IMR_N(idx);
}
else {
idx -= 128;
PSP_GET_ICTRL2_BASE()->ICR[idx] = level & 7;
if (unmask)
PSP_GET_ICTRL2_BASE()->CIMR = MCF54XX_ICTRL_IMR_N(idx);
else
PSP_GET_ICTRL2_BASE()->SIMR = MCF54XX_ICTRL_IMR_N(idx);
}
_psp_set_sr(temp);
return MQX_OK;
}
return MQX_INVALID_PARAMETER;
}
/*FUNCTION*-------------------------------------------------------------------
*
* Function Name : mcf52xx_erase_sector
* Returned Value : true if success, or false
* Comments :
* Erase a flash memory block
*
*END*----------------------------------------------------------------------*/
bool mcf52xx_flash_erase_sector
(
/* [IN] File pointer */
IO_FLASHX_STRUCT_PTR dev_ptr,
/* [IN] Erased sector address */
char *from_ptr,
/* [IN] Erased sector size */
_mem_size size
)
{
MCF52XX_FLASH_INTERNAL_STRUCT_PTR dev_spec_ptr = (MCF52XX_FLASH_INTERNAL_STRUCT_PTR) dev_ptr->DEVICE_SPECIFIC_DATA;
VMCF52XX_CFM_STRUCT_PTR cfm_ptr;
int (*RunInRAM)(char *);
uint32_t temp;
/* select the proper ramcode function */
RunInRAM = (int(*)(char *))dev_spec_ptr->flash_execute_code_ptr;
/* get the pointer to cfm registers structure */
cfm_ptr = (VMCF52XX_CFM_STRUCT_PTR)dev_spec_ptr->cfm_ptr;
_int_disable();
/* prepare flash write operation, clear flags and wait for ready state */
cfm_ptr->CFMUSTAT = (MCF52XX_CFM_CFMUSTAT_PVIOL | MCF52XX_CFM_CFMUSTAT_ACCERR);
while (!(cfm_ptr->CFMUSTAT & MCF52XX_CFM_CFMUSTAT_CBEIF)) {
/* wait */
};
/* latch address in Flash */
(*(volatile uint32_t *)(FLASH_START_ADDRESS + from_ptr)) = (volatile uint32_t)-1;
/* issue erase command */
cfm_ptr->CFMCMD = MCF52XX_CFM_CFMCMD_PAGE_ERASE;
/* run command and wait for it to finish (must execute from RAM) */
temp = _psp_get_sr();
_psp_set_sr(temp | 0x0700);
RunInRAM((char *) &cfm_ptr->CFMUSTAT);
_psp_set_sr(temp);
if (cfm_ptr->CFMUSTAT & (MCF52XX_CFM_CFMUSTAT_ACCERR | MCF52XX_CFM_CFMUSTAT_PVIOL)) {
_int_enable();
return FALSE;
}
_int_enable();
return(TRUE);
}
/*FUNCTION*-------------------------------------------------------------------
*
* Function Name : mcf51xx_erase_sector_ext
* Returned Value : true if success, or false
* Comments :
* Erase a flash memory block
*
*END*----------------------------------------------------------------------*/
boolean mcf51xx_erase_sector(IO_FLASHX_STRUCT_PTR flash_ptr, uchar_ptr from,
_mem_size size )
{
VMCF51XX_FTSR_STRUCT_PTR ftsr_ptr;
INTERNAL_51XX_STRUCT_PTR dev_spec_struct_ptr;
int (*RunInRAM)( uchar_ptr );
uint_32 temp;
dev_spec_struct_ptr = flash_ptr->DEVICE_SPECIFIC_DATA;
/* select the proper ramcode function */
*RunInRAM = (int(*)( uchar_ptr ))dev_spec_struct_ptr->flash_execute_code_ptr;
/* get the pointer to cfm registers structure */
ftsr_ptr = (VMCF51XX_FTSR_STRUCT_PTR)dev_spec_struct_ptr->ftsr_ptr;
_int_disable();
/* prepare flash write operation, clear flags and wait for ready state */
ftsr_ptr->FSTAT = (MCF51XX_FTSR_FSTAT_FPVIOL | MCF51XX_FTSR_FSTAT_FACCERR);
while (!(ftsr_ptr->FSTAT & MCF51XX_FTSR_FSTAT_FCBEF)){
/* wait */
};
/* latch address in Flash */
(*(volatile vuint_32 *)(from)) = (vuint_32)-1;
/* issue erase command */
ftsr_ptr->FCMD = MCF51XX_FTSR_FCMD_SECTOR_ERASE;
/* run command and wait for it to finish (must execute from RAM) */
temp = _psp_get_sr();
_psp_set_sr(temp | 0x0700);
RunInRAM( (volatile uchar_ptr)&ftsr_ptr->FSTAT );
_psp_set_sr(temp);
if (ftsr_ptr->FSTAT & (MCF51XX_FTSR_FSTAT_FACCERR | MCF51XX_FTSR_FSTAT_FPVIOL)) {
_int_enable();
return FALSE;
}
_int_enable();
return(TRUE);
}
void _bsp_exit_handler
(
void
)
{ /* Body */
uint_16 temp;
temp = _psp_set_sr(0x2700);
_mcf51EM_int_mask_all();
} /* Endbody */
/*FUNCTION*-------------------------------------------------------------------
*
* Function Name : ftfl_flash_command_sequence
* Returned Value : uint_32 an error code
* Comments :
* Run command in FTFL device.
*
*END*-----------------------------------------------------------------------*/
static uint_32 ftfl_flash_command_sequence
(
/* [IN] Flash specific structure */
FTFL_FLASH_INTERNAL_STRUCT_PTR dev_spec_ptr,
/* [IN] Command byte array */
uint_8_ptr command_array,
/* [IN] Number of values in the array */
uint_8 count,
/* [IN] Read the result back? */
boolean read
)
{
uint_8 fstat;
uint_32 result, temp;
void (* RunInRAM)(char_ptr);
FTFL_MemMapPtr ftfl_ptr;
uint_8 cnt;
ftfl_ptr = (FTFL_MemMapPtr)dev_spec_ptr->ftfl_ptr;
/* get pointer to RunInRAM function */
#if PSP_MQX_CPU_IS_COLDFIRE
RunInRAM = (void(*)(char_ptr))((char_ptr)dev_spec_ptr->flash_execute_code_ptr);
#elif PSP_MQX_CPU_IS_KINETIS
/* Increment by one means for Cortex CPU Thumb2 instruction set */
RunInRAM = (void(*)(char_ptr))((char_ptr)dev_spec_ptr->flash_execute_code_ptr + 1);
#endif
/* set the default return as FTFL_OK */
result = FTFL_OK;
/* check CCIF bit of the flash status register */
while (0 == (ftfl_ptr->FSTAT & FTFL_FSTAT_CCIF_MASK))
{ /* void */ }
/* clear RDCOLERR & ACCERR & FPVIOL error flags in flash status register */
if (ftfl_ptr->FSTAT & FTFL_FSTAT_RDCOLERR_MASK)
{
ftfl_ptr->FSTAT |= FTFL_FSTAT_RDCOLERR_MASK;
}
if (ftfl_ptr->FSTAT & FTFL_FSTAT_ACCERR_MASK)
{
ftfl_ptr->FSTAT |= FTFL_FSTAT_ACCERR_MASK;
}
if (ftfl_ptr->FSTAT & FTFL_FSTAT_FPVIOL_MASK)
{
ftfl_ptr->FSTAT |= FTFL_FSTAT_FPVIOL_MASK;
}
cnt = count;
switch (cnt)
{
case 12: ftfl_ptr->FCCOBB = command_array[--cnt];
case 11: ftfl_ptr->FCCOBA = command_array[--cnt];
case 10: ftfl_ptr->FCCOB9 = command_array[--cnt];
case 9: ftfl_ptr->FCCOB8 = command_array[--cnt];
case 8: ftfl_ptr->FCCOB7 = command_array[--cnt];
case 7: ftfl_ptr->FCCOB6 = command_array[--cnt];
case 6: ftfl_ptr->FCCOB5 = command_array[--cnt];
case 5: ftfl_ptr->FCCOB4 = command_array[--cnt];
case 4: ftfl_ptr->FCCOB3 = command_array[--cnt];
case 3: ftfl_ptr->FCCOB2 = command_array[--cnt];
case 2: ftfl_ptr->FCCOB1 = command_array[--cnt];
case 1: ftfl_ptr->FCCOB0 = command_array[--cnt];
default: break;
}
#if PSP_MQX_CPU_IS_COLDFIRE
temp = _psp_get_sr();
_psp_set_sr(temp | 0x0700);
#elif PSP_MQX_CPU_IS_KINETIS
__disable_interrupt ();
#endif /* PSP_MQX_CPU_IS_KINETIS */
/* run command and wait for it to finish (must execute from RAM) */
RunInRAM ((char_ptr)&ftfl_ptr->FSTAT);
#if PSP_MQX_CPU_IS_COLDFIRE
_psp_set_sr(temp);
#elif PSP_MQX_CPU_IS_KINETIS
__enable_interrupt ();
#endif /* PSP_MQX_CPU_IS_KINETIS */
cnt = count;
if (read) {
switch (cnt)
{
case 12: command_array[--cnt] = ftfl_ptr->FCCOBB;
case 11: command_array[--cnt] = ftfl_ptr->FCCOBA;
case 10: command_array[--cnt] = ftfl_ptr->FCCOB9;
case 9: command_array[--cnt] = ftfl_ptr->FCCOB8;
case 8: command_array[--cnt] = ftfl_ptr->FCCOB7;
case 7: command_array[--cnt] = ftfl_ptr->FCCOB6;
case 6: command_array[--cnt] = ftfl_ptr->FCCOB5;
case 5: command_array[--cnt] = ftfl_ptr->FCCOB4;
case 4: command_array[--cnt] = ftfl_ptr->FCCOB3;
case 3: command_array[--cnt] = ftfl_ptr->FCCOB2;
case 2: command_array[--cnt] = ftfl_ptr->FCCOB1;
case 1: command_array[--cnt] = ftfl_ptr->FCCOB0;
default: break;
}
}
/* get flash status register value */
fstat = ftfl_ptr->FSTAT;
#if PSP_MQX_CPU_IS_KINETIS
/* invalidate flash cache to expose flash changes */
kinetis_flash_invalidate_cache(FLASHX_INVALIDATE_CACHE_ALL);
#endif
/* checking access error */
if (0 != (fstat & FTFL_FSTAT_ACCERR_MASK))
{
/* return an error code FTFL_ERR_ACCERR */
result = FTFL_ERR_ACCERR;
}
/* checking protection error */
else if (0 != (fstat & FTFL_FSTAT_FPVIOL_MASK))
{
/* return an error code FTFL_ERR_PVIOL */
result = FTFL_ERR_PVIOL;
}
/* checking MGSTAT0 non-correctable error */
else if (0 != (fstat & FTFL_FSTAT_MGSTAT0_MASK))
{
/* return an error code FTFL_ERR_MGSTAT0 */
result = FTFL_ERR_MGSTAT0;
}
return result;
}
/*FUNCTION*-------------------------------------------------------------------
*
* Function Name : ftfl_flash_command_sequence
* Returned Value : uint32_t an error code
* Comments :
* Run command in FTFL device.
*
*END*-----------------------------------------------------------------------*/
static uint32_t ftfl_flash_command_sequence
(
/* [IN] Flash specific structure */
FTFL_FLASH_INTERNAL_STRUCT_PTR dev_spec_ptr,
/* [IN] Command byte array */
uint8_t *command_array,
/* [IN] Number of values in the array */
uint8_t count,
/* [IN] Read the result back? */
bool read,
/* [IN] The address which will be affected by command */
void *affected_addr,
/* [IN] The address which will be affected by command */
uint32_t affected_size
)
{
volatile uint8_t fstat;
uint32_t result;
void (* RunInRAM)(volatile uint8_t *, void (_CODE_PTR_) (volatile uint32_t));
void (* RunInvalidateInRAM)(volatile uint32_t);
RunInvalidateInRAM = NULL;
FTFL_MemMapPtr ftfl_ptr;
uint8_t cnt;
#if PSP_MQX_CPU_IS_COLDFIRE
uint32_t temp;
#endif
ftfl_ptr = (FTFL_MemMapPtr)dev_spec_ptr->ftfl_ptr;
/* get pointer to RunInRAM function */
RunInRAM = (void(*)(volatile uint8_t *, void (_CODE_PTR_)(volatile uint32_t)))(dev_spec_ptr->flash_execute_code_ptr);
#if PSP_MQX_CPU_IS_KINETIS
RunInvalidateInRAM = (void(*)(volatile uint32_t))(dev_spec_ptr->flash_invalidate_code_ptr);
#endif
/* set the default return as FTFL_OK */
result = FTFL_OK;
/* check CCIF bit of the flash status register */
while (0 == (ftfl_ptr->FSTAT & FTFL_FSTAT_CCIF_MASK))
{ /* void */ }
/* clear RDCOLERR & ACCERR & FPVIOL error flags in flash status register */
if (ftfl_ptr->FSTAT & FTFL_FSTAT_RDCOLERR_MASK)
{
ftfl_ptr->FSTAT |= FTFL_FSTAT_RDCOLERR_MASK;
}
if (ftfl_ptr->FSTAT & FTFL_FSTAT_ACCERR_MASK)
{
ftfl_ptr->FSTAT |= FTFL_FSTAT_ACCERR_MASK;
}
if (ftfl_ptr->FSTAT & FTFL_FSTAT_FPVIOL_MASK)
{
ftfl_ptr->FSTAT |= FTFL_FSTAT_FPVIOL_MASK;
}
cnt = count;
switch (cnt)
{
case 12: ftfl_ptr->FCCOBB = command_array[--cnt];
case 11: ftfl_ptr->FCCOBA = command_array[--cnt];
case 10: ftfl_ptr->FCCOB9 = command_array[--cnt];
case 9: ftfl_ptr->FCCOB8 = command_array[--cnt];
case 8: ftfl_ptr->FCCOB7 = command_array[--cnt];
case 7: ftfl_ptr->FCCOB6 = command_array[--cnt];
case 6: ftfl_ptr->FCCOB5 = command_array[--cnt];
case 5: ftfl_ptr->FCCOB4 = command_array[--cnt];
case 4: ftfl_ptr->FCCOB3 = command_array[--cnt];
case 3: ftfl_ptr->FCCOB2 = command_array[--cnt];
case 2: ftfl_ptr->FCCOB1 = command_array[--cnt];
case 1: ftfl_ptr->FCCOB0 = command_array[--cnt];
default: break;
}
#if PSP_MQX_CPU_IS_COLDFIRE
temp = _psp_get_sr();
_psp_set_sr(temp | 0x0700);
#elif PSP_MQX_CPU_IS_KINETIS
__disable_interrupt ();
#endif /* PSP_MQX_CPU_IS_KINETIS */
/* run command and wait for it to finish (must execute from RAM) */
RunInRAM(&ftfl_ptr->FSTAT, (void (_CODE_PTR_)(volatile uint32_t))RunInvalidateInRAM);
cnt = count;
if (read) {
switch (cnt)
{
case 12: command_array[--cnt] = ftfl_ptr->FCCOBB;
case 11: command_array[--cnt] = ftfl_ptr->FCCOBA;
case 10: command_array[--cnt] = ftfl_ptr->FCCOB9;
case 9: command_array[--cnt] = ftfl_ptr->FCCOB8;
case 8: command_array[--cnt] = ftfl_ptr->FCCOB7;
case 7: command_array[--cnt] = ftfl_ptr->FCCOB6;
case 6: command_array[--cnt] = ftfl_ptr->FCCOB5;
case 5: command_array[--cnt] = ftfl_ptr->FCCOB4;
case 4: command_array[--cnt] = ftfl_ptr->FCCOB3;
case 3: command_array[--cnt] = ftfl_ptr->FCCOB2;
case 2: command_array[--cnt] = ftfl_ptr->FCCOB1;
case 1: command_array[--cnt] = ftfl_ptr->FCCOB0;
default: break;
}
}
/* get flash status register value */
fstat = ftfl_ptr->FSTAT;
/*
invalidate data cache of 'affected_addr' address and 'affected_size' size
because reading flash through code-bus may show incorrect data
*/
#if defined(_DCACHE_INVALIDATE_MLINES) || defined(_ICACHE_INVALIDATE_MLINES)
if (affected_size)
{
#if defined(_DCACHE_INVALIDATE_MLINES)
_DCACHE_INVALIDATE_MLINES(affected_addr, affected_size);
#endif
#if defined(_ICACHE_INVALIDATE_MLINES)
_ICACHE_INVALIDATE_MLINES(affected_addr, affected_size);
#endif
}
#endif
#if PSP_MQX_CPU_IS_COLDFIRE
_psp_set_sr(temp);
#elif PSP_MQX_CPU_IS_KINETIS
__enable_interrupt ();
#endif /* PSP_MQX_CPU_IS_KINETIS */
/* checking access error */
if (0 != (fstat & FTFL_FSTAT_ACCERR_MASK))
{
/* return an error code FTFL_ERR_ACCERR */
result = FTFL_ERR_ACCERR;
}
/* checking protection error */
else if (0 != (fstat & FTFL_FSTAT_FPVIOL_MASK))
{
/* return an error code FTFL_ERR_PVIOL */
result = FTFL_ERR_PVIOL;
}
/* checking MGSTAT0 non-correctable error */
else if (0 != (fstat & FTFL_FSTAT_MGSTAT0_MASK))
{
/* return an error code FTFL_ERR_MGSTAT0 */
result = FTFL_ERR_MGSTAT0;
}
return result;
}
/*FUNCTION*-------------------------------------------------------------------
*
* Function Name : mcf51xx_write_sector
* Returned Value : true if success, or false
* Comments :
* Performs a write into flash memory
*
*END*----------------------------------------------------------------------*/
boolean mcf51xx_write_sector(IO_FLASHX_STRUCT_PTR flash_ptr,
uchar_ptr from_ptr, uchar_ptr to_ptr, _mem_size size )
{
#define BYTES_IN_WORD 4
VMCF51XX_FTSR_STRUCT_PTR ftsr_ptr;
VMCF51XX_PMC_STRUCT_PTR pmc_ptr;
INTERNAL_51XX_STRUCT_PTR dev_spec_struct_ptr;
int (*RunInRAM)( uchar_ptr );
uint_32 temp, temp_data = 0xFFFFFFFF;
uchar_ptr temp_data_ptr;
uchar byte_data_counter = 0;
/* get the offset in write word */
uint_32 offset = (uint_32)to_ptr & 0x00000003;;
(uint_32_ptr)temp_data_ptr = &temp_data;
dev_spec_struct_ptr = flash_ptr->DEVICE_SPECIFIC_DATA;
/* select the proper ramcode function */
*RunInRAM = (int(*)( uchar_ptr ))dev_spec_struct_ptr->flash_execute_code_ptr;
/* get the pointer to cfm registers structure */
ftsr_ptr = (VMCF51XX_FTSR_STRUCT_PTR)dev_spec_struct_ptr->ftsr_ptr;
/* get the pointer to pmc registers structure */
pmc_ptr = _bsp_get_pmc_address();
/* Clear any errors */
_int_disable();
ftsr_ptr->FSTAT = (MCF51XX_FTSR_FSTAT_FPVIOL | MCF51XX_FTSR_FSTAT_FACCERR);
/* if the start address != doesn't correspond with hardware, prepare
variables for 1st word write */
if(offset){
/* Align pointer to writable address */
to_ptr -= offset;
/* jump over old data */
byte_data_counter = offset;
}
/* while are some data to write */
while(size){
/* move data to write word */
while( byte_data_counter < BYTES_IN_WORD && size ){
*(temp_data_ptr+byte_data_counter) = *from_ptr++;
byte_data_counter++;
size--;
}
/* test the LVDF flag - if 1, we need to write data in 2 steps */
if( pmc_ptr->SPMSC1 & MCF51XX_PMC_SPMSC1_LVDF) {
/* write odd bytes */
(*(vuint_32 *)(to_ptr)) = temp_data | 0x00FF00FF;
/* write command to CFMCMD */
ftsr_ptr->FCMD = MCF51XX_FTSR_FCMD_BURST_PROGRAM;
/* run command and wait for it to finish (must execute from RAM) */
temp = _psp_get_sr();
_psp_set_sr(temp | 0x0700);
RunInRAM( (volatile uchar_ptr)&ftsr_ptr->FSTAT );
_psp_set_sr(temp);
/* write even bytes */
(*(vuint_32 *)(to_ptr)) = temp_data | 0xFF00FF00;
/* write command to CFMCMD */
ftsr_ptr->FCMD = MCF51XX_FTSR_FCMD_BURST_PROGRAM;
/* run command and wait for it to finish (must execute from RAM) */
temp = _psp_get_sr();
_psp_set_sr(temp | 0x0700);
RunInRAM( (volatile uchar_ptr)&ftsr_ptr->FSTAT );
_psp_set_sr(temp);
}
else {
/* move write data to register */
(*(vuint_32 *)(to_ptr)) = temp_data;
/* write command to CFMCMD */
ftsr_ptr->FCMD = MCF51XX_FTSR_FCMD_BURST_PROGRAM;
/* run command and wait for it to finish (must execute from RAM) */
temp = _psp_get_sr();
_psp_set_sr(temp | 0x0700);
RunInRAM( (volatile uchar_ptr)&ftsr_ptr->FSTAT );
_psp_set_sr(temp);
}
/* Check for Errors */
if (ftsr_ptr->FSTAT & (MCF51XX_FTSR_FSTAT_FPVIOL | MCF51XX_FTSR_FSTAT_FACCERR)) {
_int_enable();
return (FALSE);
}
/* init variables for next loop */
to_ptr += BYTES_IN_WORD;
byte_data_counter = 0;
temp_data = 0xFFFFFFFF;
}
_int_enable();
return (TRUE);
}
/*FUNCTION*-------------------------------------------------------------------
*
* Function Name : mcf52xx_write_sector
* Returned Value : true if success, or false
* Comments :
* Performs a write into flash memory
*
*END*----------------------------------------------------------------------*/
bool mcf52xx_flash_write_sector
(
/* [IN] File pointer */
IO_FLASHX_STRUCT_PTR dev_ptr,
/* [IN] Source address */
char *from_ptr,
/* [IN] Destination address */
char *to_ptr,
/* [IN] Number of bytes to write */
_mem_size size
)
{
MCF52XX_FLASH_INTERNAL_STRUCT_PTR dev_spec_ptr = (MCF52XX_FLASH_INTERNAL_STRUCT_PTR) dev_ptr->DEVICE_SPECIFIC_DATA;
VMCF52XX_CFM_STRUCT_PTR cfm_ptr;
int (*RunInRAM)(char *);
uint32_t temp, temp_data = 0xFFFFFFFF;
char *temp_data_ptr;
unsigned char byte_data_counter = 0;
#define BYTES_IN_WORD 4
#define WRITED_BYTES 4
/* get the offset in write word */
uint32_t offset = (uint32_t)to_ptr & 0x00000003;;
temp_data_ptr = (char *)&temp_data;
/* select the proper ramcode function */
RunInRAM = (int (*)(char *)) dev_spec_ptr->flash_execute_code_ptr;
/* get the pointer to cfm registers structure */
cfm_ptr = (VMCF52XX_CFM_STRUCT_PTR) dev_spec_ptr->cfm_ptr;
/* Clear any errors */
_int_disable();
cfm_ptr->CFMUSTAT = (MCF52XX_CFM_CFMUSTAT_PVIOL | MCF52XX_CFM_CFMUSTAT_ACCERR);
/* if the start address != doesn't correspond with hardware, prepare
variables for 1st word write */
if(offset) {
/* Align pointer to writable address */
to_ptr -= offset;
/* jump over old data */
byte_data_counter = offset;
}
/* while are some data to write */
while(size) {
/* move data to write word */
while( byte_data_counter < BYTES_IN_WORD && size ) {
*(unsigned char *)(temp_data_ptr+byte_data_counter) = *from_ptr++;
byte_data_counter++;
size--;
}
/* move write data to register */
(*(volatile uint32_t *)(FLASH_START_ADDRESS + (uint32_t)to_ptr)) = temp_data;
/* init variables for next loop */
to_ptr += WRITED_BYTES;
byte_data_counter = 0;
temp_data = 0xFFFFFFFF;
/* write command to CFMCMD */
cfm_ptr->CFMCMD = MCF52XX_CFM_CFMCMD_WORD_PROGRAM;
/* run command and wait for it to finish (must execute from RAM) */
temp = _psp_get_sr();
_psp_set_sr(temp | 0x0700);
RunInRAM((char *)&cfm_ptr->CFMUSTAT);
_psp_set_sr(temp);
/* Check for Errors */
if (cfm_ptr->CFMUSTAT & (MCF52XX_CFM_CFMUSTAT_PVIOL | MCF52XX_CFM_CFMUSTAT_ACCERR)) {
_int_enable();
return (FALSE);
}
}
_int_enable();
return (TRUE);
}
