Проблемы при работе со встроенной flash, AT91SAM7A3, bootloader Опции

[Costa]

Здравствуйте!

Делаю загрузчик (bootloader) для AT91SAM7A3, одной из функций которого должно быть обновление (через USART, но это не суть) собственной прошивки. Все получается, из под JTAG, т.е. когда код загрузчика расположен и исполняется в RAM, все замечательно работает.
Проблема возникает, когда этот же код зашивается во flash. Когда дело доходит собственно до работы с flash (lock/unlock, write page), программа зависает, а поскольку в устройстве есть внешний watchdog, происходит reset.

Сделал маленькую функцию, которая блокирует (lock) регион 0 flash:

Код

__ramfunc void Lock(void)
{
    unsigned int interrupt_mask;
    unsigned int flash_status;
    unsigned short lock_status = *AT91C_MC_FSR >> 16;
    if (lock_status & 0x1)
        return;
  *AT91C_MC_FMR = FMCN_FOR_LOCK | AT91C_MC_FWS_1FWS;
    interrupt_mask = *AT91C_AIC_IMR;
  *AT91C_AIC_IDCR = AT91C_ALL_INT;
  *AT91C_MC_FCR = AT91C_MC_KEY | AT91C_MC_FCMD_LOCK;
  do
  {
    flash_status = *AT91C_MC_FSR & AT91C_MC_EOL;
  }
  while (!flash_status);
  *AT91C_AIC_IECR = interrupt_mask;
}

Штука в том, что собственно lock происходит, т.к. после reset функция выскакивает по первому return; - но вот если регион в состоянии unlock, то все стопорится в цикле do while. Причем пробовал тупо вставлять счетчик в тело цикла, чтобы по его окончании выходить из цикла - все равно виснет...
Уже весь мозг сломал и не знаю, в какую сторону рыть. То ли проблема в том, что как-то некорректно выключаются прерывания на время операций с flash, т.к. хотя сами обработчики прерываний и сделаны __ramfunc, но есть еще вектора прерываний, обертка... То ли вышеприведенный код не попадает на самом деле в RAM, хотя в стартапе __segment_init прописан, и соотв. библиотека к проекту подключена... Помогите разобраться, где накосячил.