;************************************************************************************************
;Beijing instittue of technology
;File Name:		Init.s
;Description:		Init.
;Author:		chaisc
;Date:			2003.8.13
;SDRAM Configuration:	Use ljg.txt,SDRAM remap to address 0x0~0x100,0000(16MB) 
;************************************************************************************************	
	GET	mem.a
	GET	casia1.a
	IMPORT main_entry

	CODE32
	AREA    Init,CODE,READONLY
	ENTRY 
	B       Reset_Handler		
        	NOP       ;IRQ_Handler
        	NOP        ;IRQ_Handler
        	NOP       ;IRQ_Handler
        	NOP       ;IRQ_Handler
	NOP 
        	NOP       ;IRQ_Handler
        	NOP       ;IRQ_Handler


Reset_Handler	
 ;==========================================================
 ;Change Base address and dram
 ;==========================================================
 	B INITIALIZE_STACK
			;/* set the SPSR under super */ 
;*********************************	
;mask all interrupt
;*********************************
INITIALIZE_STACK
	
;******************************************************
;Initialise system stack for different processor modes
;******************************************************
	mov	r0, #0xd2
	MSR	cpsr_c, r0
	mov 	sp,#0x70000
	mov	r0,#0xd3
	MSR	cpsr_c, r0				;/* change to FIQ MODE */
	mov 	sp,#0x80000
	mov	r0,#0x13:OR:0x40
;*******************************************************
;Disable all interrupt
;*******************************************************
	MRS	R0,CPSR
	BIC	R0,R0,#MODE_MASK
	ORR	R0,R0,#SUP_MODE
	ORR	R0,R0,#IBit
	ORR	R0,R0,#FBit 
	MSR	CPSR_cf,R0
;********************************************************
;Timer setting
;********************************************************
	LDR	R1,=0x3FF6000
	LDR	R0,=&01
	STR	R0,[R1]

	LDR	R1,=0x3FF6004
	LDR	R0,=&400000
	STR	R0,[R1]

;********************************************************
;Mask off all IRQ sources
;********************************************************


	LDR	R1,=INTMOD
	LDR	R0,=&0
	STR	R0,[R1]
	
**********************************************************
	    IMPORT  C_Entry
    	    BL      C_Entry
	    B  		.
*********************************************************
	
SAVED_LR_SVC	DCD   0x00000000        ; some variables for temparal use
SAVED_LR_IRQ	DCD   0x00000000
SAVED_SPSR	DCD   0x00000000
	
	
	IMPORT  OSIntEnter
	IMPORT  OSIntExit
	IMPORT  OSTimeTick
	IMPORT  _OSCtxSw
	IMPORT	OSIntCtxSwFlag
	EXPORT	TimeISR

TimeISR
	sub	lr,lr,#4
	STMFD 	sp!, {r0-r12,lr}         ; push lr & register file
        MRS 	r4, spsr
	STMFD 	sp!, {r4}                ; push current spsr_cxsf
	ldr	r0,=0xffffff
	ldr	r1,=0x3ff4004
	str     r0,[r1]
	BL	OSIntEnter
	BL 	OSTimeTick
	BL	OSIntExit

	LDR	r0, =OSIntCtxSwFlag      ; check if OSIntCtxFlag is marked as true
	LDR 	r1, [r0]
	CMP	r1, #1	
	beq	_IntCtxSw
	
	LDMFD 	sp!, {r4}                ; get cpsr_svc from stack
	MSR     spsr_cxsf, r4                 ; prepare spsr_cxsf to return svc mode	
	LDMFD	sp!, {r0-r12, pc}^  

_IntCtxSw
        LDMFD 	sp!, {r4}                ; restore spsr_cxsf_irq 
	MSR 	spsr_cxsf, r4 	
  	str	r4, SAVED_SPSR
	LDMFD	sp!, {r0-r12, lr}        ; recover the irq stack pointer

	STR	lr, SAVED_LR_IRQ         ; save lr_irq to SAVED_LR_IRQ 	
	MOV	lr, #0x000000d3          ; change forcely cpsr to svc mode
	MSR	cpsr_cxsf, lr	
	STR     lr, SAVED_LR_SVC         ; save lr_svc to SAVED_LR_SVC
	LDR	lr, SAVED_LR_IRQ         ; get lr_irq value saved in SAVED_LR_IRQ 
	STMFD 	sp!, {lr}                ; push future task pc (lr_irq should be pushed in place of PC)
	LDR	lr, SAVED_LR_SVC         ; get lr_svc value saved in SAVED_LR_SVC 

        STMFD 	sp!, {r0-r12,lr}         ; push lr & r0-r12 register file
        ldr		r4, SAVED_SPSR
		 ;	    MRS 	r4, spsr
        STMFD 	sp!, {r4}                ; push current psr
		  ;     MRS 	r4, spsr
        STMFD 	sp!, {r4}                ; push spsr_cxsf

	B	_OSCtxSw

	
	
	
	END