__ramfunc int EFCWrite(u32 adr, u8 *pbuf, u32 Len, u32 NeedReset)
{
    u32 l;
    unsigned int *pflash;
    unsigned int page;
    unsigned int region;
    unsigned int i;  

    l=0;    
    while (l<Len)
    {    
          pflash = (unsigned int *)adr;
          page = (adr & 0x3FFFF)/EFC_PAGE_SIZE;
          region = (page/EFC_PagesInTheLockRegion);      
      
      if (AT91C_BASE_MC->MC_FSR & (region << 16))
      {
          // lock set, clear it
          AT91C_BASE_MC->MC_FMR = ((AT91C_MC_FMCN)&(50 <<16)) | AT91C_MC_FWS_2FWS;
          AT91C_BASE_MC->MC_FCR = (0x5A << 24) | (region << 8 ) |AT91C_MC_FCMD_UNLOCK;
      }
      while (!(AT91C_BASE_MC->MC_FSR & AT91C_MC_FRDY));
      
      for (i = 0; i < EFC_PAGE_SIZE_UINT; i++)
           *(pflash + i ) = *(pbuf + i);      
      
      AT91C_BASE_MC->MC_FCR = (0x5A << 24) | (page << 8 ) |AT91C_MC_FCMD_START_PROG;
      while (!(AT91C_BASE_MC->MC_FSR & AT91C_MC_FRDY));
      
        l+=EFC_PAGE_SIZE;
        adr+=EFC_PAGE_SIZE;
    }
    
    if (NeedReset)
    {
      __asm("sub r0, r0,r0;");
      __asm("bx r0;");
    }
    
    return (Len * AT91C_IFLASH_PAGE_SIZE);
}

for (i = 0; i < EFC_PAGE_SIZE_UINT; i++)
           *(pflash + i ) = *(pbuf + i);

#define TestAddr (AT91C_IFLASH+AT91C_IFLASH_SIZE-sizeof(buf1))

    volatile uint32_t* pTmp = (uint32_t*)((uint32_t)&Config_flash & ~0x7F);
    uint32_t* pSegmentEnd = (uint32_t*)(((uint32_t)&Config_flash & ~0x7F) + 128);
    while(pTmp < pSegmentEnd) // copy to shadow buffer
    {
        uint32_t Data = *pTmp;
        *pTmp++ = Data;
    }
    uint32_t *pSrc = (uint32_t*)&TmpConfig;
    uint32_t *pDst = (uint32_t*)&Config_flash;
    while(pDst < (uint32_t*)((config_t*)&Config_flash + 1))
    {
        *pDst++ = *pSrc++;
    }

    Flash::RewritePage((uint32_t const*)&Config_flash);

void EFC_Init(void)
{ unsigned long clkus;
    
// Calculate Flash Microsecond Cycle Number - Approximate (no Library Code)
//clkus = (1074*(clk >> 10)) >> 20;            // Master Clock Cycles in 1.0us
  clkus = (1611*(MCK >> 10)) >> 20;            // Master Clock Cycles in 1.5us
   // Set Flash Microsecond Cycle Number
  // Set Flash Waite State to max. (Single Cycle Access at Up to 30 MHz)
  AT91C_BASE_MC->MC_FMR = ((AT91C_MC_FMCN) & (clkus << 16)) | AT91C_MC_FWS_3FWS;
  AT91C_BASE_WDTC->WDTC_WDMR = AT91C_WDTC_WDDIS;
}


int EFC_WritePage(DWORD adr, DWORD *pbuf)
{
  
    unsigned int *pflash;
    unsigned int page;
    unsigned int i;  

            
pflash = (unsigned int *)adr;
page = (adr - 0x00100000)/AT91C_IFLASH_PAGE_SIZE;
          
AT91C_BASE_MC->MC_FCR = AT91C_MC_CORRECT_KEY | AT91C_MC_FCMD_UNLOCK |AT91C_MC_PAGEN & (page << 8));
// Wait until the end of Command
while ((AT91C_BASE_MC->MC_FSR & AT91C_MC_EOL) != AT91C_MC_EOL);

for (i = 0; i <AT91C_IFLASH_PAGE_SIZE/4; i++)   *(pflash + i ) = *(pbuf + i);      

AT91C_BASE_MC->MC_FCR = AT91C_MC_CORRECT_KEY | AT91C_MC_FCMD_START_PROG |(AT91C_MC_PAGEN & (page << 8));

  // Wait until the end of Command
  while ((AT91C_BASE_MC->MC_FSR & AT91C_MC_EOP) != AT91C_MC_EOP);

// Check for Errors
  if (AT91C_BASE_MC->MC_FSR & (AT91C_MC_PROGE | AT91C_MC_LOCKE)) return (1);
  return (0);
    }

#define TestAddr AT91C_IFLASH+AT91C_IFLASH_SIZE-AT91C_IFLASH_PAGE_SIZE
BYTE TestEFC(void)
{
DWORD buf1[256];
  int i;
  BYTE result;
  for(i=1;i<=63;i++) buf1[i]=i;
  EFC_Init();  
  result=(BYTE)EFC_WritePage((DWORD)TestAddr,(DWORD*)buf1);
  return     result;
}

while ((AT91C_BASE_MC->MC_FSR & AT91C_MC_EOL) != AT91C_MC_EOL);