void flash_init(void)
{
unsigned portLONG zz;

zz = AT91C_MC_FWS_3FWS | (0x72 << 16);
printf("Init: 0x%08X\n\r",zz);
vTaskDelay(10);
portENTER_CRITICAL();
AT91F_MC_EFC_CfgModeReg(AT91C_BASE_MC,zz);
portEXIT_CRITICAL();
}

void flash_write(unsigned portLONG page)
{
unsigned portLONG * flash;
unsigned portLONG * plReceivedFirmware;
unsigned portLONG i;
unsigned portCHAR usPage;
unsigned portLONG zz1,zz2;

/* Setup the pointers to flash and RAM  */
flash = (unsigned portLONG *) (APP_START + (page*FLASH_PAGE_SIZE));
plReceivedFirmware = (unsigned portLONG *) (RAM_FIRMWARE_START + (page*FLASH_PAGE_SIZE));
usPage = page + (APP_START/FLASH_PAGE_SIZE);

/* Setup FCR commands */
zz1=AT91C_MC_FCMD_UNLOCK |    AT91C_MC_CORRECT_KEY | (usPage << 8);
zz2=AT91C_MC_FCMD_START_PROG | AT91C_MC_CORRECT_KEY | (usPage << 8);
printf("Flash: %08X RAM: %08X Unlock: %08X Write: %08X\n\r",flash,plReceivedFirmware,zz1,zz2);
vTaskDelay(50);

portENTER_CRITICAL();
/* Set iterator to DWORDS in FLASH_PAGE_SIZE */
i = FLASH_PAGE_SIZE/sizeof(portLONG);

/* Copy data from RAM to flash _DWORD_ */
do
  {
   *(flash++) = *(plReceivedFirmware++);    
  } while (--i);

/* Unlock the page */
FlashCommand_RAM(zz1);
/* Issue WRITE PAGE command */
FlashCommand_RAM(zz2);
portEXIT_CRITICAL();
}

__ramfunc void FlashCommand_RAM(unsigned portLONG command)
{
    AT91C_BASE_MC->MC_FCR = command;
    while(!(AT91C_BASE_MC->MC_FSR & AT91C_MC_FRDY));
}