;********************************************
;File Name:		Init.s
;Description:	boot	
;Author:		dxb
;Date:			2004-7-27
;********************************************
   INCLUDE   AT91RM9200.inc


AT91_USER_Stack_Begin EQU  0x20f00000
AT91_Stack_Begin      EQU  0x20f00000

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  0x8000

;*************************************************
  AREA    	reset,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
;------------------------------------------------------------------------------
;Disable All Interrupts
;------------------------------------------------------------------------------
 ldr r1 ,=AT91C_PIOA_PER
 ldr r0 ,=0x00400000
 str r0,[r1]
 ldr r1 ,=AT91C_PIOA_OER
 ldr r0 ,=0x00400000
 str r0,[r1]
 ldr r1 ,=AT91C_PIOA_SODR
 ldr r0 ,=0x00400000
 str r0,[r1] 
 

 ldr r0 ,=0xFFFFFFFF
 ldr r1 ,=AT91C_AIC_IDCR
 str r0,[r1]
 
 ldr   r1, = AT91C_BASE_CKGR	
 ;ldr 	r0, = AT91C_CKGR_DIVA_0
 ;str   r0, [r1, #CKGR_PLLAR]

 ;ldr 	r0, = AT91C_CKGR_DIVB_0
 ;str   r0, [r1, #CKGR_PLLBR]              
;------------------------------------------------------------------------------
;-Enabling the Main Oscillator
;-Normally First instruction in PMC initialisation
;------------------------------------------------------------------------------
;-Main oscillator Enable register	APMC_MOR : Enable main oscillator , OSCOUNT = 0xFF
	ldr 	r0, = AT91C_CKGR_MOSCEN:OR:AT91C_CKGR_OSCOUNT
	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

	msr     CPSR_c, #(ARM_MODE_SVC | I_BIT | F_BIT)
	mov     r13, r0                     
 	sub     r0, r0, #SVC_STACK_SIZE

	msr     CPSR_c, #(ARM_MODE_IRQ | I_BIT | F_BIT)
	mov     r13, r0                     
	sub     r0, r0, #IRQ_STACK_SIZE
 
	msr     CPSR_c, #(ARM_MODE_FIQ | I_BIT | F_BIT)
	mov     r13, r0
	sub     r0, r0, #FIQ_STACK_SIZE

	msr     CPSR_c, #(ARM_MODE_ABORT | I_BIT | F_BIT)
	mov     r13, r0
	sub     r0, r0, #ABT_STACK_SIZE
	
	msr     CPSR_c, #(ARM_MODE_UNDEF | I_BIT | F_BIT)
	mov     r13, r0 
	sub     r0, r0, #UND_STACK_SIZE

	msr     CPSR_c, #(ARM_MODE_SYS | F_BIT)
	mov     r13, r0
   
   ldr r0 ,=0x01
   ldr r1 ,=0xFFFFFF00
   str r0,[r1]
;source address
 ;   ldr	r0, =0x20000000
;size of armboot(64K)	
;	ldr	r2, =32
;target address(start of sdram)
;	ldr	r1, =0x200000
    
;rom2ram_copy
    ; subs   r2,  r2, #1
    ; ldr	r3, [r0],#4
    ; str	r3, [r1],#4
    ; bne	rom2ram_copy

Armboot_begin
;------------------------------------------------------------------------------      
;Jump to Main Routine	
;------------------------------------------------------------------------------
	  IMPORT   main
      BL       main
here  B  	   here

	  END