;************************************************************************************************
;Compony:       CASIA
;File Name:		Init.s
;Description:		
;Author:		Wjh
;Date:			2005-03-11
;*************************CONST DEFINATION******************************************************/	
AT91C_BASE_CKGR       EQU  0xFFFFFC20      
AT91C_CKGR_MOSCEN     EQU  0x1
AT91C_CKGR_OSCOUNT    EQU  0x0ff00
CKGR_MOR              EQU  0xFFFFFC20

AT91_Stack_Begin      EQU  0x00204000

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

IRQ_STACK_SIZE		  EQU  0x10
FIQ_STACK_SIZE		  EQU  0x04
ABT_STACK_SIZE		  EQU  0x04
UND_STACK_SIZE		  EQU  0x04
SVC_STACK_SIZE		  EQU  0x400
USER_STACK_SIZE       EQU  0x10
;*************************************************************************************************/
    CODE32
	AREA    	Init,CODE,READONLY
		
		EXPORT	__ENTRY
__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

;------------------------------------------------------------------------------
;- 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, = AT91_Stack_Begin	

;- 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

;------------------------------------------------------------------------------
; Read/modify/write CP15 control register 
;------------------------------------------------------------------------------
; read cp15 control registre (cp15 r1) in r0
     mrc     p15, 0, r0, c1, c0, 0
; Reset bit :Little Endian end fast bus mode
     ldr     r3, =0xC0000080      
; Set bit :Asynchronous clock mode, Not Fast Bus
     ldr     r4, =0xC0000000      
     bic     r0, r0, r3             
     orr     r0, r0, r4             
; write r0 in cp15 control registre (cp15 r1)
     mcr     p15, 0, r0, c1, c0, 0 
     
;------------------------------------------------------------------------------
; Setup the stack for each mode
;------------------------------------------------------------------------------
   ldr		r0, =AT91_Stack_Begin


;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 User Mode, set User Mode Stack and enable interrupts  
	msr     CPSR_c, #(ARM_MODE_SYS | I_BIT | F_BIT)
; Init stack Sup
	mov     r13, r0
    sub     r0, r0, #USER_STACK_SIZE

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


;------------------------------------------------------------------------------
;Copy Armboot form flash to sdram    
; 若需編譯二進制文件請刪除下面幾行的注釋符號！
;------------------------------------------------------------------------------
;source address
     ;ldr	r0, =Armboot_begin
;size of armboot(20K)	
	 ;ldr	r2, =0xa000		
;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 C_Entry
	  
_main
__main
	EXPORT    _main
	EXPORT    __main
	ldr       r0, =C_Entry
	mov       lr, pc
	bx        r0

here  B  	   here

	  END