__arm __ramfunc t_uint8  write_page_EFC(t_uint32 Adress, t_uint32 *Buffer)
{
    t_uint16 i;

    Adress &= ~(FLASH_PAGESIZE - 1);

    disable_interrupts();
    for(i = 0; i < (FLASH_PAGESIZE/4); ++i)
    {
        *((volatile t_uint32*)Adress + i) = *(Buffer + i);
    }

    while (!(AT91C_BASE_MC->MC_FSR & AT91C_MC_FRDY))
    {
        continue;
    }

    AT91C_BASE_MC->MC_FCR = FLASH_KEY
                            |
                            AT91C_MC_FCMD_START_PROG
                            |
                            ( ((Adress - FLASH_START) / FLASH_PAGESIZE) << 8 );
    while (!(AT91C_BASE_MC->MC_FSR & AT91C_MC_FRDY))
    {
        continue;
    }
    enable_interrupts();

    if (AT91C_BASE_MC->MC_FCR & AT91C_MC_PROGE)
    {
        return 0;
    }
    return 1;  
}

INT_DISABLE_MASK   EQU      0x000000C0

    RSEG CODE_I:CODE:NOROOT(2)
    CODE32

    PUBLIC disable_interrupts
    PUBLIC enable_interrupts

enable_interrupts:
;Çàïèñàòü 0 â áèòû I è F ðåãèñòðà CPSR, ÷òîáû ðàçðåøèòü ïðåðûâàíèÿ
    mrs     r0, cpsr                   ;ñ÷èòûâàåì çíà÷åíèå ðåãèñòðà ñòàòóñà
    mvn     r1, #INT_DISABLE_MASK      ;â ñ÷èòàííîì çíà÷åíèè ñáðàñûâàåì áèòû
    and     r0, r0, r1                 ;FIQ disable è IRQ disable
    msr     cpsr_c, r0                 ;çàïèñûâåì íîâîå çíà÷åíèå â ðåãèñòð ñòàòóñà -                                        
    bx      lr                         ;ïðåðûâàíèÿ FIQ è IRQ ðàçðåøåíû

disable_interrupts:
;Çàïèñàòü 1 â áèòû I è F ðåãèñòðà CPSR, ÷òîáû çàïðåòèòü ïðåðûâàíèÿ
    mrs     r0, cpsr                   ;ñ÷èòûâàåì çíà÷åíèå ðåãèñòðà ñòàòóñà
    orr     r0, r0, #INT_DISABLE_MASK  ;â ñ÷èòàííîì çíà÷åíèè ñáðàñûâàåì áèòû
                                       ;FIQ disable è IRQ disable
    msr     cpsr_c, r0                 ;çàïèñûâåì íîâîå çíà÷åíèå â ðåãèñòð ñòàòóñà -
    bx      lr                         ;ïðåðûâàíèÿ FIQ è IRQ çàïðåùåíû
    
    END

#ifdef FlashRelease
__ramfunc
#endif
bool WritePage (int adr, int *pbuf)
{

        int *pflash = (int *)adr;
        int page = adr >> 7;
        int region = (adr >> 12);

        if (AT91C_BASE_MC->MC_FSR & (region << 16))
        {
                // lock set
                AT91C_BASE_MC->MC_FMR = ((AT91C_MC_FMCN)&(50 <<16)) | AT91C_MC_FWS_1FWS;
                AT91C_BASE_MC->MC_FCR = (0x5A << 24) | (region << 8 ) |AT91C_MC_FCMD_UNLOCK;
        }
        while (!(AT91C_BASE_MC->MC_FSR & AT91C_MC_FRDY))
               ;


        for (int i = 0; i < PAGE_SIZE; i++)
        {
                *(pflash + i ) = *(pbuf + i);
        }
         AT91C_BASE_MC->MC_FMR = ((AT91C_MC_FMCN)&(100 <<16)) | AT91C_MC_FWS_1FWS;
        AT91C_BASE_MC->MC_FCR = (0x5A << 24) | (page << 8 ) |AT91C_MC_FCMD_START_PROG;
        while (!(AT91C_BASE_MC->MC_FSR & AT91C_MC_FRDY))
               ;
        return true;
}

/*------------------------------------------------------------------------------------------------*/
__arm __ramfunc t_uint8  write_page_EFC(t_uint32 Adress, t_uint32 *Buffer)
{
    t_uint16 i;
    t_uint16 tempFMR = AT91C_BASE_MC->MC_FMR;

    Adress &= ~(FLASH_PAGESIZE - 1);

    disable_interrupts();
    for(i = 0; i < (FLASH_PAGESIZE/4); ++i)
    {
        *((volatile t_uint32*)Adress + i) = *(Buffer + i);
    }

    while (!(AT91C_BASE_MC->MC_FSR & AT91C_MC_FRDY))
    {
        continue;
    }

    AT91C_BASE_MC->MC_FMR = ((100 <<16)&(AT91C_MC_FMCN)) | AT91C_MC_FWS_1FWS;
    AT91C_BASE_MC->MC_FCR = FLASH_KEY
                            |
                            AT91C_MC_FCMD_START_PROG
                            |
                            ( ((Adress - FLASH_START) / FLASH_PAGESIZE) << 8 );
    while (!(AT91C_BASE_MC->MC_FSR & AT91C_MC_FRDY))
    {
        continue;
    }
    
    AT91C_BASE_MC->MC_FMR = tempFMR;
//    while (!(AT91C_BASE_MC->MC_FSR & AT91C_MC_FRDY))
//    {
//        continue;
//    }
    enable_interrupts();

    if (AT91C_BASE_MC->MC_FCR & AT91C_MC_PROGE)
    {
        return 0;
    }
    return 1;  
}
/*------------------------------------------------------------------------------------------------*/