//******************************************************************************
//*    Flash service subroutines

#define    FMCN1        (0x30 << 0x10)
#define    FMCN1_5        (0x48 << 0x10)

int flash_write_page(u_int addr, u_char *data)
{
    u_int a, page;


    page = addr >> 8;

    AT91C_BASE_MC->MC_FMR = (AT91C_BASE_MC->MC_FMR & ~(AT91C_MC_FMCN | AT91C_MC_NEBP)) | FMCN1_5;

    a = 0;
    do
    {
        ((reg_32 *)AT91C_IFLASH)[a >> 2] =
            data[a] | (data[a + 1] << 8) | (data[a + 2] << 16) | (data[a + 3] << 24);
        a += 4;
    } while(a < AT91C_IFLASH_PAGE_SIZE);

    AT91C_BASE_MC->MC_FCR = (0x5a << 24) | ((page << 8) & AT91C_MC_PAGEN) | AT91C_MC_FCMD_START_PROG;

    while(!((a = AT91C_BASE_MC->MC_FSR) & AT91C_MC_FRDY));

    if((a & AT91C_MC_LOCKE) || (a & AT91C_MC_PROGE))
    {
        dprintf("Flash: Write Error (FSR = 0x%08x, Page = %03x).\r\n", a, page);
        return -1;
    }
    return 0;
}

__ramfunc int AT91F_Flash_Write( unsigned int Flash_Address ,int size ,unsigned int * buff){
  
  AT91PS_MC ptMC = AT91C_BASE_MC;    //* set the Flash controller base address
  unsigned int i, page, status;
  unsigned int * Flash;
  
  Flash = (unsigned int *) Flash_Address;    //* init flash pointer
  AT91C_BASE_MC->MC_FMR = ((AT91C_MC_FMCN)&(72 <<16)) | AT91C_MC_FWS_1FWS;//        AT91F_Flash_Init();
  page = ((Flash_Address - (unsigned int)AT91C_IFLASH ) / FLASH_PAGE_SIZE_BYTE);   //* Get the Flash page number
  for (i=0; (i < FLASH_PAGE_SIZE_BYTE) & (size > 0);i++, Flash++,buff++,size-=4 ){   //* copy the new value
    *Flash=*buff;    //* copy the flash to the write buffer ensuring code generation
  }
  ptMC->MC_FCR = AT91C_MC_CORRECT_KEY | AT91C_MC_FCMD_START_PROG | (AT91C_MC_PAGEN & (page <<8));    //* Write the write page command
  status = 0;
  while ((status & AT91C_MC_FRDY) != AT91C_MC_FRDY ) {    //* Wait the end of command
    status = AT91C_BASE_MC->MC_FSR;
  }
  return status;        
}

----- flash.h: ---------
#ifndef FLASH_H__
#define FLASH_H__
#include    <stdint.h>
#include    <string.h>
#include    <ioAT91SAM7Sxx.h>

class flash_t
{
public:
    INLINE static void clear_buffer();
    INLINE static void fill(uint32_t const *addr, uint32_t data) { *(uint32_t *)addr = data; }
    INLINE static void fill(uint32_t const *dst, uint32_t *src, uint32_t size);
    INLINE static uint32_t read(uint32_t const *addr) { return *addr; }
    INLINE static void read(uint32_t *dst, uint32_t const *src, uint32_t size);
    INLINE static void erase(uint32_t const *page_addr);
    INLINE static void program(uint32_t const *page_addr);

    static uint32_t const PAGE_SIZE = 128;
private:
    static void command(uint32_t command, uint32_t mode);
    NOINLINE static void command_RAM(uint32_t command) __attribute__((section(".ramfunc")));
    
    static uint32_t const MCK_CYCLES = ((15ULL * MCK + 5000000ULL) / 10000000ULL);
public:
    static uint32_t const FLASH_WAITSTATES = MCK < 30000000ULL ? AT91C_MC_FWS_0FWS : AT91C_MC_FWS_1FWS;
} static Flash __attribute__((__unused__));

void flash_t::clear_buffer()
{
    uint_fast8_t Size = PAGE_SIZE / sizeof(uint32_t);
    uint32_t const *r = (uint32_t *)0;
    do
    {
        fill(r++, ~(uint32_t)0);
    }
    while (--Size);
}

void flash_t::fill(uint32_t const *dst, uint32_t *src, uint32_t size)
{
    do
    {
        *(uint32_t *)dst++ = *src++;
    }
    while (--size);
}

void flash_t::read(uint32_t *dst, uint32_t const *src, uint32_t size)
{
    do
    {
        *dst++ = *src++;
    }
    while (--size);
}

void flash_t::erase(uint32_t const *page_addr)
{
    command( ((AT91C_MC_KEY / 0xFFU) * 0x5A) | (((uint32_t)page_addr >> 7) << 8) | AT91C_MC_FCMD_START_PROG,
                (0 * AT91C_MC_FRDY) | (0 * AT91C_MC_LOCKE) | ( 0 * AT91C_MC_PROGE) |
                (0 * AT91C_MC_NEBP) | (FLASH_WAITSTATES) | (AT91C_MC_FMCN / 0xFFU * MCK_CYCLES)
           );
}

void flash_t::program(uint32_t const *page_addr)
{
    command( ((AT91C_MC_KEY / 0xFFU) * 0x5A) | (((uint32_t)page_addr >> 7) << 8) | AT91C_MC_FCMD_START_PROG,
                (0 * AT91C_MC_FRDY) | (0 * AT91C_MC_LOCKE) | ( 0 * AT91C_MC_PROGE) |
                (1 * AT91C_MC_NEBP) | (FLASH_WAITSTATES) | (AT91C_MC_FMCN / 0xFFU * MCK_CYCLES)
           );
}
#endif  // FLASH_H__

----- flash.cpp: ---------
#include    "Flash.h"
#include    "Interrupts.h"

void flash_t::command_RAM(uint32_t command) 
{
    AT91C_BASE_MC->MC_FCR = command;
    while(!(AT91C_BASE_MC->MC_FSR & AT91C_MC_FRDY))
      ;
}

void flash_t::command(uint32_t command, uint32_t mode) 
{
    disable_interrupts();
    AT91C_BASE_MC->MC_FMR = mode;
    command_RAM(command);
    enable_interrupts();
}

#pragma push
#pragma arm section code = "ramcode"
#pragma O1

void flash_subroutine(...)
{
    ...
}

#pragma pop

* (ramcode)

// In-application flash programming for SAM7
#include "hal.h"
#include "syslib.h"
#include "iap.h"

#define FLASH_PAGE_SIZE_BYTES  256  // for SAM7S256/SAM7X256

#define FLASH_PROTECTION_KEY_FCR  (0x5A << 24)

#define CYCLES_PER_MICROSECOND ((CCLK / M))
#define CYCLES_FMCN  ((CYCLES_PER_MICROSECOND << 1)) // round up to 2 mks

typedef void __iap_exec_function( AT91PS_MC pMC, U32 FCR_val);


/***********************************************
* iap_FlashCmdFunc()                           *
*    This routine executes Flash command       *
* NOTE! THIS FUNCTION SHOULD BE COPIED INTO    *
* SRAM BEFORE EXECUTION                        *
* ---> pMC - pointer to memory controller      *
* ---> FCR_val - prepared FCR value            *
* <--- returns: nothing                        *
***********************************************/
void iap_FlashCmdFunc( AT91PS_MC pMC, U32 FCR_val)
{
    pMC->MC_FCR = FCR_val;
    // the EFC is locked... no reading from flash while writing..
    // the routine supposed to be called from RAM, lets wait until operation completes
    while (!(pMC->MC_FSR & AT91C_MC_FRDY));
}




/***********************************************
* iap_ExecCmd()                                *
*    This routine executes Flash command       *
* It places iap_FlashCmdFunc into SRAM, and    *
* does necessary stuff to call it              *
* Considering ARM and Thumb modes              *
* ---> FCR_val - prepared FCR value            *
* <--- returns: nothing                        *
***********************************************/
void iap_ExecCmd( U32 FCR_val)
{
    U8  buf[128]; // 128 bytes buffer
    U32 ThumbMode = (U32)iap_FlashCmdFunc & 0x01;

    {
        U32 iap_FuncAddr = (U32)iap_FlashCmdFunc & (~ThumbMode); // not even addr in thumb mode (have to & with ~0x01)
        AT91PS_AIC    pAIC = AT91C_BASE_AIC;
        AT91PS_MC    pMC = AT91C_BASE_MC;
        U32 iMask = pAIC->AIC_IMR;

        // copy function, that performs EFC command, into SRAM.
        memcpy_burst( (U32 *)buf, (U32 *)iap_FuncAddr, sizeof(buf));
        // disable all interrupt sources
        pAIC->AIC_IDCR = 0xFFFFFFFF;
        // execute function from SRAM (+1 to func addr in thumb mode)
        ((__iap_exec_function *)(buf + ThumbMode))( pMC, FCR_val);
        // restore interrupt sources
        pAIC->AIC_IECR = iMask;
    }
}



/***********************************************
* iap_PageWrite()                              *
*    The routine performs flash page write     *
* that uses abstract iap_ExecCmd() method      *  
*             WORKING VERSION                  *
* ---> FlashAddress - address of the flash     *
*      desired to be re-written                *
* ---> pPageData - pointer to the data array   *
* <--- returns: TRUE if flash-write performed  *
***********************************************/
int iap_PageWrite( U32 FlashAddress, U32 *pPageData, U32 size_bytes )
{
    AT91PS_MC    pMC = AT91C_BASE_MC;
    U32 FMRval = pMC->MC_FMR;
    U32 FCRval = FLASH_PROTECTION_KEY_FCR | (FlashAddress & 0x00FFF00) | AT91C_MC_FCMD_START_PROG;
    U32 *p = (U32 *)FlashAddress;
    if (size_bytes <= FLASH_PAGE_SIZE_BYTES)
    {
        U32 TotalCnt = FLASH_PAGE_SIZE_BYTES / sizeof( U32 );
        U32 Cnt = (size_bytes + 3) / sizeof( U32 );
        U32 tmp;

        // prepare Flash mode register
        // clear the NO-Erase before programming bit, and interrupt request flags
        FMRval &= ~(AT91C_MC_NEBP | AT91C_MC_FMCN | AT91C_MC_FRDY | AT91C_MC_LOCKE | AT91C_MC_PROGE);
        FMRval |=  (CYCLES_FMCN << 16);

        while (!(pMC->MC_FSR & AT91C_MC_FRDY));
        pMC->MC_FMR = FMRval;

        // write page data to the write buffer
        while(TotalCnt--)
        {
            if (Cnt)
            {
                *p++ = *pPageData++;
                Cnt -= 1;
            }
            else // read page data and write it back
            {
                tmp = *(V32 *)(p);
                *(V32 *)(p++) = tmp;
            }
        }

        // enter flash number and perform flash write operation
        iap_ExecCmd( FCRval );
        return TRUE;
    }
    return FALSE;
}

void iap_ExecCmd( U32 FCR_val)
{
    PPACKET_BUF pPacketDesc = pool_AllocPart();
    U32 ThumbMode = (U32)iap_FlashCmdFunc & 0x01;

    while (!pPacketDesc)
    {
        Kernel_Dispatch();
        pPacketDesc = pool_AllocPart();
    }
    ...

void fcmd(U32 fcr)
{
    U32 ramcmd[6] =
    {
        0xe3a01000, // MOV      r1,#0
        0xe501009c, // STR      r0,[r1,#-0x9c]; MC_FCR
        0xe5110098, // LDR      r0,[r1,#-0x98]; MC_FSR
        0xe3100001, // TST      r0,#1
        0x0afffffc, // BEQ      {pc} - 0x8
        0xe12fff1e  // BX       r14
    };
    void (*pcmd)(U32) = *(void (*)(U32))&ramcmd;

    pcmd(fcr);
}