//********************************************************************************
******
// Тут у нас все что касается записи во флеш
//********************************************************************************
******

extern void AT91F_Enable_Interrupt(void);
extern void AT91F_Disable_Interrupt(void);

#define RAMFUNC __attribute__ ((section (".ram_func")))

#define EFC_PAGE_SIZE 256
#define EFC_PAGE_COUNT 1024
#define EFC_PagesInTheLockRegion 64
#define EFC_PAGE_SIZE_UINT (EFC_PAGE_SIZE/4) // Количество unsigned int

unsigned long int RAMFUNC EFC_WritePage(unsigned long int adr, unsigned long int *pbuf) {
unsigned int *pflash;
unsigned int page;
unsigned int region;
unsigned int i;

pflash = (unsigned int *)adr;
page = (adr & 0x3FFFF)/EFC_PAGE_SIZE;
region = (page/EFC_PagesInTheLockRegion);

//Init
while (!(AT91C_BASE_MC->MC_FSR & AT91C_MC_FRDY)) ;
AT91C_BASE_MC->MC_FMR = ((AT91C_MC_FMCN)&(100 <<16)) | AT91C_MC_FWS_1FWS ;
while (!(AT91C_BASE_MC->MC_FSR & AT91C_MC_FRDY)) ;

if (AT91C_BASE_MC->MC_FSR & (region << 16)) {
// lock set, clear it
AT91C_BASE_MC->MC_FMR = ((AT91C_MC_FMCN)&(50 <<16)) | AT91C_MC_FWS_2FWS ;
AT91C_BASE_MC->MC_FCR = (0x5A << 24) | (region << 8 ) |AT91C_MC_FCMD_UNLOCK; //!!!проблемное место!!!
}

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

for (i = 0; i < EFC_PAGE_SIZE_UINT; i++)
*(pflash + i ) = *(pbuf + i); //!!!проблемное место!!!

AT91F_PIO_SetOutput( AT91C_BASE_PIOA, 0x400 ) ;
AT91C_BASE_MC->MC_FCR = (0x5A << 24) | (page << 8 ) |AT91C_MC_FCMD_START_PROG; //!!!проблемное место!!!
while (!(AT91C_BASE_MC->MC_FSR & AT91C_MC_FRDY)) ;

return 1;
}

void RAMFUNC EFCWrite(unsigned long int adr, unsigned char *pbuf, unsigned long int Len, unsigned long int NeedReset) {
unsigned long int l;
if (NeedReset) {
l=0;
}
l=0;
while (l<Len) {
EFC_WritePage(adr, (unsigned long int*)&(pbuf[l]));
l+=EFC_PAGE_SIZE;
adr+=EFC_PAGE_SIZE;
}
if (NeedReset) {
__asm("sub r0, r0,r0;");
__asm("bx r0;");
}
}



//********************************************************************************
******

unsigned int buf1[1];
#define Tester 0x110001
buf1[1] = 1;
AT91F_Disable_Interrupt();
EFCWrite(Tester,(unsigned char*)buf1,4,0);
AT91F_Enable_Interrupt();

MEMORY
{
  FLASH (r) : ORIGIN = 0x00100000, LENGTH = 0x00040000
  DATA (rw)  : ORIGIN = 0x00200000, LENGTH = 0x00010000
  STACK (rw) : ORIGIN = 0x00210000, LENGTH = 0x00000000
}

...

  .data : AT (_etext)
  {
    _data = .;
    *(.data)
    SORT(CONSTRUCTORS)
    *(.ramfunc)
  } >DATA
  . = ALIGN(4);

  _edata = .;
   PROVIDE (edata = .);

*(.ramfunc)
.ramfunc       0x00200008       0x14 main.o
                0x00200008                EFC_WritePage

ENTRY(_start)
IRQ_STACK_SIZE = 0x100;
SYS_STACK_SIZE = 0x1800;

/* memory layout */
MEMORY
{
VECTORS (rx) : ORIGIN = 0x00100000, LENGTH = 0x40
ROM (rx) : ORIGIN = 0x00100040, LENGTH = 0x00001000 - 0x40
RAM (rwx) : ORIGIN = 0x00200000, LENGTH = 0x00004000
REMAPPED (rwx) : ORIGIN = 0x00000000, LENGTH = LENGTH(RAM)
}

SECTIONS
{
.vectors :
{
KEEP(*(.vectors))
} > REMAPPED AT > VECTORS

.text :
{
__ctors_start = .;
KEEP(SORT(*)(.ctors))
KEEP(SORT(*)(.init_array))
__ctors_end = .;
__dtors_start = .;
KEEP(SORT(*)(.dtors))
__dtors_end = .;

. = ALIGN(4);

*(.text*) /* code */

*(.rodata) /* read-only data (constants) */
*(.rodata*)

*(.glue_7)
*(.glue_7t)
} > ROM

. = ALIGN(4);
_etext = .;

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

*(.data*)

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

} > RAM AT > ROM

/* .bss section which is used for uninitialized data */
.bss: (NOLOAD) :
{
. = ALIGN(4);
__bss_start = . ;
__bss_start__ = . ;
*(.bss*)
*(COMMON)
. = ALIGN(4);
PROVIDE (__bss_end = .);
} > RAM

.noinit (NOLOAD) :
{
. = ALIGN(4);
PROVIDE (__noinit_start = .) ;
*(.noinit*)
PROVIDE (__noinit_end = .) ;
PROVIDE (__heap_start = .) ;
} > RAM

.stack :
{
. = ALIGN(4);
. += SYS_STACK_SIZE;
PROVIDE (__stack = .);
. += IRQ_STACK_SIZE;
PROVIDE (__stack_irq = .);
/* allocate stacks if needed */
PROVIDE (__stack_fiq = .);
PROVIDE (__stack_und = .);
PROVIDE (__stack_abort = .);
PROVIDE (__stack_svc = .);

} > RAM

_end = . ;
PROVIDE (end = .);

/* Stabs debugging sections. */
.stab 0 : { *(.stab) }
.stabstr 0 : { *(.stabstr) }
.stab.excl 0 : { *(.stab.excl) }
.stab.exclstr 0 : { *(.stab.exclstr) }
.stab.index 0 : { *(.stab.index) }
.stab.indexstr 0 : { *(.stab.indexstr) }
.comment 0 : { *(.comment) }
/* DWARF debug sections.
Symbols in the DWARF debugging sections are relative to the beginning
of the section so we begin them at 0. */
/* DWARF 1 */
.debug 0 : { *(.debug) }
.line 0 : { *(.line) }
/* GNU DWARF 1 extensions */
.debug_srcinfo 0 : { *(.debug_srcinfo) }
.debug_sfnames 0 : { *(.debug_sfnames) }
/* DWARF 1.1 and DWARF 2 */
.debug_aranges 0 : { *(.debug_aranges) }
.debug_pubnames 0 : { *(.debug_pubnames) }
/* DWARF 2 */
.debug_info 0 : { *(.debug_info .gnu.linkonce.wi.*) }
.debug_abbrev 0 : { *(.debug_abbrev) }
.debug_line 0 : { *(.debug_line) }
.debug_frame 0 : { *(.debug_frame) }
.debug_str 0 : { *(.debug_str) }
.debug_loc 0 : { *(.debug_loc) }
.debug_macinfo 0 : { *(.debug_macinfo) }
/* SGI/MIPS DWARF 2 extensions */
.debug_weaknames 0 : { *(.debug_weaknames) }
.debug_funcnames 0 : { *(.debug_funcnames) }
.debug_typenames 0 : { *(.debug_typenames) }
.debug_varnames 0 : { *(.debug_varnames) }
}

#define RAMFUNC __attribute__ ((section (".ramfunc")))

void RAMFUNC FlashTest_RAM(uint32_t command)
{
    AT91C_BASE_MC->MC_FCR = command;
    //AT91C_BASE_PIOA->PIO_SODR = 0x400; // Тут необъяснимое явление. Кстати 0х400 - это 10ый PIO, к нему подключен светодиод.
    while(!(AT91C_BASE_MC->MC_FSR & AT91C_MC_FRDY));
}

void FlashTest(uint32_t command, uint32_t mode)
{
    AT91F_Disable_Interrupt();
    AT91C_BASE_MC->MC_FMR = mode;
    FlashTest_RAM(command);
    AT91F_Enable_Interrupt();
}

#define MCK_CYCLES          ((15ULL * MCK + 5000000) / 10000000)
#define FLASH_WAITSTATES    MCK_CYCLES < 30000000ULL ? AT91C_MC_FWS_0FWS : AT91C_MC_FWS_1FWS

inline void RewritePage(uint32_t const *addr)
{
    FlashTest( (0x5AULL *  AT91C_MC_KEY / 0xFF) | (((uint32_t)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) | (MCK_CYCLES * AT91C_MC_FMCN / 0xFF));
}

#define I_BIT 0x00000080
#define F_BIT 0x00000040

mrs r0,CPSR
orr r0, r0, #(I_BIT | F_BIT)
msr CPSR_c,r0
mrs r0,CPSR
ands r0,r0, #(I_BIT | F_BIT)
beq AT91F_Disable_Interrupt
bx lr