Запись данных во Flash Sam7s256 Опции

[Spoofing]

Доброго времени суток.
Помогите решить такую проблему. Не могу разобраться как записать данные во флеш. Функционал - прием данных с USART запись в постоянную память, выдача по требованию. Все прекрасно работает с RAM - данные получаются и выдаются, но естественно сброс по питанию все обнуляет. С помошью SAM-BA залил прошивку, все работает из флеши и выполняется в RAM но как записывать пользовательские данные?
Судя по даташиту необходимо поместить данные в буфер-защелку, после чего запись команд в регистр EFC для операции со страницей №такой то. Где этот буфер определяется? по регистрам не нашел.

Второй вопрос как определить где закончится программа - с какого момента можно будет записать данные (флеш с 0х00100000 без ремапа начинается)
Не бейте сильно в электронике почти полный нуб) пока освоил инициализацию, прерывания и com

[Spoofing]

Возможности проверить на железке дома нет. Но вроде бы компилится без ошибок. Есть замечания по коду?

Код

AT91PS_MC     f_pMC     = AT91C_BASE_MC;

int flash_write_page(int* addr, int* data, unsigned int size_data)    
{
    int page;
    unsigned int i = 0, count=0;
    int * Flash;
      
    Flash = (int*)addr;
    page =  ((int)addr >> 8);
  
    for (i=0;(i<size_data)&&(i<255); Flash++,count++,i += 4)  
        {
         *Flash= data[count];
        }
    
    f_pMC->MC_FCR = (0x5a << 24) | (page << 8) | AT91C_MC_FCMD_START_PROG;

    while(!( f_pMC->MC_FSR) & AT91C_MC_FRDY);

    if((f_pMC->MC_FSR & AT91C_MC_LOCKE) || (f_pMC->MC_FSR & AT91C_MC_PROGE))
    {
       return -1;
    }
    return 0;
}

вызов функции

Код

    //Write Flash
    s_pAIC->AIC_IDCR = NU1;
    flash_write_page (data_real,buff_ssc,32);
    s_pAIC->AIC_IECR = iMask;

data_real - начальный адрес области данных (пока фиксированный вручную)