AREA  UCOS_ARM, CODE, READONLY

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

OSStartHighRdy
	;//switch to SVC mode with IRQ & FIQ disable
	msr cpsr_c,#0xD3
	;//call user defined task switch hook
	bl OSTaskSwHook 
	; OSRunning = TRUE
	ldr r4,=OSRunning            
	mov r5,#1
	strb r5,[r4]
	;//get higest priority task TCB address
	ldr r4,=OSTCBHighRdy
	;//get stack pointer
	ldr r4,[r4]
	;//switch to the new stack
	ldr sp,[r4]
	;從新任務堆棧中恢復處理器的所有寄存器
	ldmfd sp!,{r4}
	msr spsr_cxsf,r4
	ldmfd sp!,{r0-r12,lr,pc}^

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

OSCtxSw
	;//save current task's context
	stmfd sp!,{lr}
	stmfd sp!,{r0-r12,lr}
	mrs r4,cpsr
	stmfd sp!,{r4}
	;//OSTCBCur->OSTCBStkPtr = sp;
	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 spsr_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
	;//push working registers onto IRQ stack
	stmfd sp!,{r1-r3}
	;//save IRQ stack pointer
	mov r1,sp
	;//adjust IRQ stack pointer
	add sp,sp,#12
	;//adjust pc  for return address to task
	sub r2,lr,#4
	;//copy spsr
	mrs r3,spsr
	;//change to SVC mode with IRQ & FIQ disable
	msr cpsr_c,#0xD3
	;//push task's return pc
	stmfd sp!,{r2} 
	;//push task's lr
	stmfd sp!,{lr}
	;//push task's r12-r4
	stmfd sp!,{r4-r12}
	;//move task's r1-r3 from IRQ stack to SVC stack
	ldmfd r1!,{r4-r6}
	stmfd sp!,{r4-r6}
	;//push task's r0 onto task's stack
	stmfd sp!,{r0}
	;//push task's cpsr (IRQ's spsr)
	stmfd sp!,{r3}
	
	;//調用OSIntEnter()或者直接給OSIntNesting加1
	 bl OSIntEnter
	;//if(1 == OSIntNesting)
	;//     OSTCBCur->OSTCBStkPtr = Stack Pointer ; the "sp" is not SYS(or task) sp, it is IRQ sp
	ldr r0,=OSIntNesting
	ldrb r1,[r0]
	cmp r1,#1
	bne %F0 
	ldr r4,=OSTCBCur
	ldr r5,[r4]
	str sp,[r5]
0
	;//給產生中斷的設備清中斷
	;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!,{r4}
	msr spsr_cxsf,r4
	ldmfd sp!,{r0-r12,lr,pc}^ 

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

OSIntCtxSw
	;//OSTaskSwGook()
	bl OSTaskSwHook
	;//OSTCBCur = OSTCBHighRdy
	ldr r4,=OSTCBCur
	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"
	;//sp = OSTCBHighRdy->OSTCBStkPtr
	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

        END