//- Set up User Mode and set User Mode Stack
        msr      CPSR_c, #ARM_MODE_USR | F_BIT
//        msr      CPSR_c, #ARM_MODE_USR | I_BIT | F_BIT
        mov      sp, r0
        sub      sl, r0, r6

#define BL_DATA_UINT32_LENGTH    64
typedef struct {
    unsigned int        cmd;    // основная команда (служебное слово)
    unsigned int        param;    // дополнительные параметры команды
    unsigned int        addr;    // адрес страницы памяти
    union {
        uint8_t     asBytes[BL_DATA_UINT32_LENGTH*4];
        unsigned int    asDwords[BL_DATA_UINT32_LENGTH];
    } data;                // данные для записи в страницу памяти
    unsigned int    crc;    // контрольная сумма
} BootloaderPacket;

BootloaderPacket inPckt;
volatile AT91PS_MC mc = AT91C_BASE_MC;
volatile uint32_t *p;
...
p = (volatile uint32_t *)(inPckt.addr);
// при записи по адресу памяти FLASH данные записываются сначала в буфер записи памяти, и писать
// в него надо по 32 бита за 1 раз! поэтому используем asDwords
printf("Setup data to write @ flash addr 0x%08X:", inPckt.addr);
for (i = 0; i < BL_DATA_UINT32_LENGTH; i++) {
    p[i] = inPckt.data.asDwords[i];
    printf(" 0x%04X", inPckt.data.asDwords[i]);
};
printf("\r\n");
interrupt_mask = *AT91C_AIC_IMR;    // сохраняем набор включенных прерываний
*AT91C_AIC_IDCR = AT91C_ALL_INT;    // выключаем все прерывания
// буфер для записи в память уже заполнился, и можно писать страницу FLASH памяти
mc->MC_FCR = AT91C_MC_WRITE_KEY | AT91C_MC_FCMD_START_PROG | AT91C_MC_PAGEN(inPckt.addr/AT91C_IFLASH_PAGE_SIZE);
while (0 == (mc->MC_FSR & AT91C_MC_EOP)) {};        // ждём когда взведётся статус EndOfProgramming
// FIXME: запись происходит, но до сюда код не доходит, зависает в цикле

*AT91C_AIC_IECR = interrupt_mask;   // восстанавливаем прерывания
printf("\r\nWrote page #%04u at address 0x%08X.\r\n", inPckt.addr/AT91C_IFLASH_PAGE_SIZE, inPckt.addr);

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 const *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(void const *page_addr);
    INLINE static void program(void 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::erase(void 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(void 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)
           );
}

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)
{
    TCritSect cs;
    AT91C_BASE_MC->MC_FMR = mode;
    command_RAM(command);
}

  /* .data section which is used for initialized data */
  .data : /* place init values immediatly after .text section */
  {
    _data = .;
    *(.ramfunc*)
    
    *(.data*)

    _edata = .;
    _data_image = LOADADDR(.data);

    PROVIDE (edata = .);
  } > RAM  AT > ROM

void command_RAM(uint32_t command) __attribute__((section(".ramfunc")));

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

static int BL_ParsePacket(void *packet, uint16_t len) __attribute__ ((section(".ramfunc")));

AT91C_BASE_MC->MC_FMR = AT91C_MC_FMCN(BOARD_FMCN) | AT91C_MC_FWS_3FWS | FMCN_FOR_LOCK;

EFC_PerformCommand(pStartEfc, AT91C_MC_FCMD_UNLOCK, startPage);

static void (*BL_StartUserApplication)(void);    // указатель на старт юзер-проги
BL_StartUserApplication = (void(*)(void))FLASH_USER_START;
BL_StartUserApplication();

// we want to set the LSb in order to switch in to Thumb mode
// when we bx.
// addr = 0x2801 = (0x5 << 11) | 1
asm("mov r3, #5\n");
asm("lsl r3, r3, #11\n");
asm("mov r4, #1\n");
asm("orr r3, r4\n");
asm("bx r3\n");

extern "C" void Application() __attribute__ ((__noreturn__));
        if (CRC.ok())                   // Application Section OK
        {
            // copy application INTVEC to remapped INTVEC location (start of RAM)
            uint32_t const *Src = (uint32_t *)APP_START;
            uint32_t *Dst = (uint32_t *)0x00200000; //RAMSTART
            uint_fast8_t Size = 0x40 / sizeof *Dst;
            do  { *Dst++ = *Src++; } while (--Size);

            AT91C_BASE_MC->MC_RCR = AT91C_MC_RCB;   // REMAP
            DRIVER(MANUAL_BOOT, OUTPUT);

            Application();
        }