AT91C_BASE_CKGR       EQU  0xFFFFFC20      
AT91C_CKGR_MOSCEN     EQU  0x1
AT91C_CKGR_OSCOUNT    EQU  0x0ff00
CKGR_MOR              EQU  0xFFFFFC20

AT91_USER_Stack_Begin EQU  0x21000000
AT91_Stack_Begin      EQU  0x21000000

ARM_MODE_USER         EQU  0x10
ARM_MODE_FIQ          EQU  0x11
ARM_MODE_IRQ          EQU  0x12
ARM_MODE_SVC          EQU  0x13
ARM_MODE_ABORT        EQU  0x17
ARM_MODE_UNDEF        EQU  0x1B
ARM_MODE_SYS          EQU  0x1F
ARM_MODE_MASK         EQU  0x1F

I_BIT                 EQU  0x80
F_BIT                 EQU  0x40
T_BIT                 EQU  0x20

IRQ_STACK_SIZE		  EQU  0x10
FIQ_STACK_SIZE		  EQU  0x04
ABT_STACK_SIZE		  EQU  0x04
UND_STACK_SIZE		  EQU  0x04
SVC_STACK_SIZE		  EQU  0x10
USER_STACK_SIZE		  EQU  0x4000
;*************************REGISTER DEFINATION*****************************************************/
AT91C_AIC_IMR              EQU  0xfffff110


;*************************************************************************************************/
    CODE32
	AREA    	Init,CODE,READONLY
	ENTRY 
	            B           Reset_Handler
	
undefvec        B           undefvec 

swivec          B           swivec   

pabtvec         B           pabtvec  

dabtvec         B           dabtvec  

rsvdvec         B           rsvdvec  

irqvec          ldr         pc, [pc,#-0xF20] 

fiqvec          ldr         pc, [pc,#-0xF20]   
    

Reset_Handler
;**********************
;Disable All Interrupts
;**********************
  ldr r0 ,=0x0
  ldr r1 ,=AT91C_AIC_IMR
  str r0,[r1]
;*******************
;- The reset handler
;*******************


; Get the CKGR Base Address
	ldr     r1, = AT91C_BASE_CKGR	
	
;-Main oscillator Enable register	APMC_MOR : Enable main oscillator , OSCOUNT = 0xFF
;	ldr 	r0, = AT91C_CKGR_MOSCEN:OR:AT91C_CKGR_OSCOUNT
	ldr 	r0, =0x0000FF01
	str     r0, [r1, #CKGR_MOR]

;**************************************
;-Low level Init (PMC, AIC, EBI,USART)
;**************************************

;- Add loop to compensate Main Oscillator startup time
	ldr 	r0, =0x00000010
LoopOsc
	subs    r0, r0, #1              
	bhi     LoopOsc
	
	ldr 	r1, = 0x00204000	

;- Set up Supervisor Mode and set SVC Stack
	msr     cpsr_c, #(ARM_MODE_SVC | I_BIT | F_BIT)
;Insure word alignement
	bic     r1, r1, #3                  
; Init stack SYS	
	mov     sp, r1                      
	IMPORT AT91F_LowLevelInit
	bl AT91F_LowLevelInit

;*******************************************
; Setup the stack for each mode
;*******************************************
   ldr		r0, =AT91_Stack_Begin

;Set up Supervisor Mode and set SVC Mode Stack
	msr     CPSR_c, #(ARM_MODE_SVC | I_BIT | F_BIT)
;Init stack Sup
	mov     r13, r0                     
 	sub     r0, r0, #SVC_STACK_SIZE

;Set up Interrupt Mode and set IRQ Mode Stack
	msr     CPSR_c, #(ARM_MODE_IRQ | I_BIT | F_BIT)
;Init stack IRQ
	mov     r13, r0                     
	sub     r0, r0, #IRQ_STACK_SIZE
 
;Set up Fast Interrupt Mode and set FIQ Mode Stack
	msr     CPSR_c, #(ARM_MODE_FIQ | I_BIT | F_BIT)
;Init stack FIQ
	mov     r13, r0
	sub     r0, r0, #FIQ_STACK_SIZE

;Set up Abort Mode and set Abort Mode Stack
	msr     CPSR_c, #(ARM_MODE_ABORT | I_BIT | F_BIT)
;Init stack Abort
	mov     r13, r0
	sub     r0, r0, #ABT_STACK_SIZE
	
;Set up Undefined Instruction Mode and set Undef Mode Stack
	msr     CPSR_c, #(ARM_MODE_UNDEF | I_BIT | F_BIT)
;Init stack Undef
	mov     r13, r0 
	sub     r0, r0, #UND_STACK_SIZE

;Set up Sys Mode, set User Mode Stack and enable interrupts  
	msr     CPSR_c, #(ARM_MODE_SYS | F_BIT)
; Init stack User
	mov     r13, r0
;Change to User Mode
    ;msr     CPSR_c, #ARM_MODE_USER
    

;*********************************
;Copy Armboot form flash to sdram
;*********************************
;source address
     ;ldr	r0, =Armboot_begin
;size of armboot(64K)	
	;ldr	r2, =0x10000		
;target address(start of sdram)
	;ldr	r1, =0x20000000 
    
;copy begin here	
;rom2ram_copy
    ;subs r2, r2, #1
    ;ldr	r3, [r0],#4
    ;str	r3, [r1],#4
    ;bne	rom2ram_copy
;Jump to target address
    ;ldr    	r0,=0x20000000
    ;mov    	pc,r0
   
;Armboot_begin
;------------------------------------------------------------------------------
;- Initialise C variables
;------------------------
;- Following labels are automatically generated by the linker. 
;- RO: Read-only = the code
;- RW: Read Write = the data pre-initialized and zero-initialized.
;- ZI: Zero-Initialized.
;- Pre-initialization values are located after the code area in the image.
;- Zero-initialized datas are mapped after the pre-initialized.
;- Note on the Data position : 
;- If using the ARMSDT, when no -rw-base option is used for the linker, the 
;- data area is mapped after the code. You can map the data either in internal
;- SRAM ( -rw-base=0x40 or 0x34) or in external SRAM ( -rw-base=0x2000000 ).
;- Note also that to improve the code density, the pre_initialized data must 
;- be limited to a minimum.
;------------------------------------------------------------------------------

	add     r2, pc,#-(8+.-CInitData)  ; @ where to read values (relative)
	ldmia   r2, {r0, r1, r3, r4}
	
	cmp         r0, r1                  ; Check that they are different
	beq         EndRW
LoopRW	
	cmp         r1, r3                  ; Copy init data
	ldrcc       r2, [r0], #4
	strcc       r2, [r1], #4
	bcc         LoopRW
EndRW

	mov         r2, #0
LoopZI	
	cmp         r3, r4                  ; Zero init
	strcc       r2, [r3], #4
	bcc         LoopZI
 
	b           EndInitC
                
CInitData
 	IMPORT      |Image$$RO$$Limit|      ; End of ROM code (=start of ROM data)
	IMPORT      |Image$$RW$$Base|       ; Base of RAM to initialise
	IMPORT      |Image$$ZI$$Base|       ; Base and limit of area
	IMPORT      |Image$$ZI$$Limit|      ; Top of zero init segment
	
	DCD     |Image$$RO$$Limit|      ; End of ROM code (=start of ROM data)
 	DCD     |Image$$RW$$Base|       ; Base of RAM to initialise
 	DCD     |Image$$ZI$$Base|       ; Base and limit of area
 	DCD     |Image$$ZI$$Limit|      ; Top of zero init segment
EndInitC
;*****************************************************         
;Jump to Main Routine	
	  IMPORT main

_main
__main
	EXPORT    _main
	EXPORT    __main
	ldr       r0, =main
	mov       lr, pc
	bx        r0
here  B  	   here

	  END