AREA  UCOS_ARM, CODE, READONLY

;void OSStartHighRdy(void)
	IMPORT  OSTaskSwHook
    IMPORT  OSRunning
    IMPORT  OSTCBHighRdy
    EXPORT  OSStartHighRdy

OSStartHighRdy
	;調用用戶自定義的OSTaskSwHook
	msr cpsr_c,#0x1f
	bl OSTaskSwHook 
	; OSRunning = TRUE
    ldr r4,=OSRunning            
    mov r5,#1
    strb r5,[r4]
	;得到將要恢復運行任務的堆棧指針
    ldr r4,=OSTCBHighRdy
    ldr r4,[r4]                 
    ldr sp,[r4]
	;從新任務堆棧中恢復處理器的所有寄存器
    ldmfd sp!,{r4}
    msr cpsr_cxsf,r4
    ldmfd sp!,{r0-r12,lr,pc}
    ;ldmfd sp!,{lr}
    ;ldmfd sp!,{pc}

;void OSCtxSw(void)
	IMPORT  OSTCBCur
	IMPORT  OSTaskSwHook
    IMPORT  OSTCBHighRdy
    IMPORT  OSPrioCur
    IMPORT  OSPrioHighRdy
    EXPORT  OSCtxSw

OSCtxSw
	;保存所有處理器寄存器
	stmfd sp!,{lr}
	stmfd sp!,{r0-r12,lr}
	mrs r4,cpsr
	stmfd sp!,{r4}
	;在當前任務的任務控制塊中保存當前任務的堆棧指針
	ldr r4,=OSTCBCur
	ldr r5,[r4]
	str sp,[r5]
	;OSTaskSwGook()
	bl OSTaskSwHook
	;OSTCBCur = OSTCBHighRdy
	ldr r6,=OSTCBHighRdy
	ldr r6,[r6]
	str r6,[r4]	
	;OSPrioCur = OSPrioHighRdy
	ldr r7,=OSPrioCur
	ldr r8,=OSPrioHighRdy
	ldrb r8,[r8] ;ldr r8,[r8] 
	strb r8,[r7] ;str r8,[r7] will make next instruciton to produce "Data Abort"
	;得到將來重新開始運行的任務的堆棧指針
	ldr sp,[r6]
	;從新任務的任務堆棧中恢復處理器所有的寄存器的值, 執行中斷返回指令
    ldmfd sp!,{r4}
    msr cpsr_cxsf,r4
    ldmfd sp!,{r0-r12,pc,lr}
  

;void OSTickISR(void)
	IMPORT OSIntEnter
	IMPORT OSIntNesting
	IMPORT OSTCBCur
	IMPORT OSTimeTick
	IMPORT OSIntExit
	IMPORT TRACE_ARM
	EXPORT OSTickISR

SRCPND       EQU  0x4a000000    ;Interrupt request status  
INTPND        EQU  0x4a000010    ;Interrupt request status
BIT_TIMER4  EQU  (0x1<<14)

OSTickISR
	;保存所有處理器寄存器
	sub lr,lr,#4
	stmfd sp!,{r0-r3,lr} ; 
	mrs r0,spsr
	stmfd sp!,{r0}
	
	;調用OSIntEnter()或者直接給OSIntNesting加1
	 bl OSIntEnter
	;if(1 == OSIntNesting)
	;     OSTCBCur->OSTCBStkPtr = Stack Pointer ; the "sp" is not SYS(or task) sp, it is IRQ sp
	
	;給產生中斷的設備清中斷
	;rSRCPND = BIT_TIMER4;       //Clear pending bit
	;rINTPND = BIT_TIMER4;
	ldr r0,=SRCPND
	ldr r1,=BIT_TIMER4
	str r1,[r0]
	ldr r2,=INTPND
	str r1,[r2]
	
	;重新允許中斷(可選) 
        
	;OSTimeTick()
	bl OSTimeTick
	;OSIntExit()
	bl OSIntExit
	;恢復處理器寄存器, 執行中斷返回指令
	ldmfd sp!,{r0}
	msr spsr_cxsf,r0
	ldmfd sp!,{r0-r3,pc}^ 

;void OSIntCtxSw(void) 
	IMPORT  OSTCBCur
    IMPORT  OSTaskSwHook
    IMPORT  OSTCBHighRdy
    IMPORT  OSPrioCur
    IMPORT  OSPrioHighRdy
    EXPORT  OSIntCtxSw

OSIntCtxSw
	msr cpsr_c,#0x9f ;IRQ -> SYS
	stmfd sp,{lr} ;will be replace by task's pc later 
	stmfd sp,{r4-r12,lr}
	msr cpsr_c,#0x92 ;SYS -> IRQ, to restore spsr,r0-r3,lr
	ldmfd sp,{r4-r9}
	msr cpsr_c,#0x9f
	stmfd sp,{r4-r8}
	str r9,[sp,#60] ; pc's position is sp+15*4

	;在當前任務的任務控制塊中保存當前任務的堆棧指針
	ldr r0,=OSTCBCur
	ldr r1,[r0]
	str sp,[r1]

	;OSTaskSwGook()
	bl OSTaskSwHook
	;OSTCBCur = OSTCBHighRdy
	ldr r6,=OSTCBHighRdy
	ldr r6,[r6]
	str r6,[r4]	
	;OSPrioCur = OSPrioHighRdy
	ldr r7,=OSPrioCur
	ldr r8,=OSPrioHighRdy
	ldrb r8,[r8] ;ldr r8,[r8] 
	strb r8,[r7] ;str r8,[r7] will make next instruciton to produce "Data Abort"
	;得到將來重新開始運行的任務的堆棧指針
	ldr sp,[r6]
	;從新任務的任務堆棧中恢復處理器所有的寄存器的值
    ldmfd sp!,{r4}
    msr spsr_cxsf,r4
    ldmfd sp!,{r0-r12,lr,pc}


	EXPORT  OSCPUSaveSR
OSCPUSaveSR
	
    mrs r0,CPSR
    orr r1,r0,#0xc0
    msr CPSR_c,r1
    mov pc,lr


	EXPORT  OSCPURestoreSR
OSCPURestoreSR

    msr CPSR_c,r0
    mov pc,lr
    
	EXPORT SimulatorTimer4
SimulatorTimer4
	mrs r12,cpsr
	add r11,lr,#4
	msr cpsr_csxf,#0x92
	msr spsr_cxsf,r12
	mov lr,r11
	b OSTickISR
	

        END