Программное восстановление SAM-BA для SAM7S Опции

[aaarrr]

Специально для тех, кто замучался восстанавливать самбу при помощи нечеловеческой свистопляски с TST, но не хочет по каким-либо причинам использовать свой загрузчик, хочу предложить простую процедурку восстановления самбы непосредственно из приложения:

Код

#define    FMCN1        (48 << 16)
#define    FMCN1_5        (72 << 16)

int restore_samba(void)
{
    unsigned int a, p, *data = (unsigned int *)0x300e00;

    disable_irq();

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

    for(p = 0; p < 32; p++)
    {

        for(a = 0; a < AT91C_IFLASH_PAGE_SIZE / 4; a++)
            ((volatile unsigned int *)AT91C_IFLASH)[a] = *data++;

        AT91C_BASE_MC->MC_FCR = (0x5a << 24) | ((p << 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))
            return 0;
    }
    return 1;
}

Процедура должна быть расположена в RAM (по крайней мере та ее часть, что непосредственно отвечает за запись страницы).
Тестировалось на AT91SAM7S64 58814G.