AREA	|subr|, CODE, READONLY

;//
	EXPORT 	OSStartHighRdy
	IMPORT	OSTaskSwHook
	IMPORT  OSTCBHighRdy 
	IMPORT  OSRunning
OSStartHighRdy
        BL 	OSTaskSwHook             ; Call user-defined hook function
        LDR 	r4,=OSRunning            ; Indicate that multitasking has started
        MOV 	r5, #1                   
        STRB 	r5, [r4]                 ; OSRunning = true
        LDR 	r4, =OSTCBHighRdy        ; Get highest priority task TCB address
        LDR 	r4, [r4]                 ; get stack pointer
        LDR 	sp, [r4]                 ; switch to the new stack
        LDMFD 	sp!, {r4}                ; pop new task s psr
        MSR 	cpsr_cxsf, r4
        LDMFD 	sp!, {r0-r12,lr,pc}      ; pop new task s r0-r12,lr & pc

;//
        EXPORT 	OSCtxSw
        IMPORT	OSPrioCur
        IMPORT	OSPrioHighRdy
        IMPORT	OSTCBCur
        IMPORT	OSTaskSwHook
        IMPORT	OSTCBHighRdy
OSCtxSw
        STMFD 	sp!, {lr}                ; push pc (lr is actually be pushed in place of PC)
        STMFD 	sp!, {r0-r12,lr}         ; push lr & register file
        MRS 	r4, cpsr
        STMFD 	sp!, {r4}                ; push current psr
        LDR 	r4, =OSTCBCur            ; Get current task TCB address
        LDR		r5, [r4]
        STR 	sp, [r5]                 ; store sp in preempted tasks s TCB
		;//
        EXPORT 	OSIntCtxSw
		IMPORT OSTaskSwHook
OSIntCtxSw
		BL		OSTaskSwHook
		LDR		r4, =OSTCBHighRdy
		LDR		r4, [r4]
		LDR		r5, =OSTCBCur
		STR		r4, [r5]				 ; OSTCBCur = OSTCBHighRdy
		LDR		r6, =OSPrioHighRdy
		LDRB	r6, [r6]
		LDR		r5, =OSPrioCur
		STRB	r6, [r5]				 ; OSPrioCur = OSPrioHighRdy
		LDR		sp, [r4]
		LDMFD 	sp!, {r4}				 ; pop new task cpsr
		MSR 	cpsr_cxsf, r4
		LDMFD 	sp!, {r0-r12,lr,pc}		 ; pop new task r0-r12,lr & pc

;//
	EXPORT 	OSTickISR
	IMPORT	OSIntEnter
	IMPORT	OSTimeTick
	IMPORT	tick_hook	
	IMPORT	OSIntExit

LINK_SAVE	DCD		0
PSR_SAVE	DCD		0

OSTickISR
	EXPORT	OSTickISR
	STMFD	sp!, {r4}
	
	LDR		r4, =LINK_SAVE
	STR		lr, [r4]				 ; LINK_SAVE = lr_irq

	MRS		lr,	spsr
	STR		lr, [r4, #4]			 ; PSR_SAVE = spsr_irq
	
	LDMFD	sp!, {r4}
		
	ORR		lr, lr,	#0x80			 ; Mask irq for context switching before 
	MSR		cpsr_cxsf, lr 			 ; returning back from irq mode.

	SUB		sp, sp, #4				 ; Space for PC
	STMFD	sp!, {r0-r12, lr}

	LDR		r4, =LINK_SAVE
	LDR		lr, [r4, #0]
	SUB		lr, lr, #4				 ; lr = LINK_SAVE - 4,
	STR		lr, [sp, #(14*4)]			 ; the return address for pc.

	LDR		r4, [r4, #4]			 ; r4 = PSR_SAVE,
	STMFD	sp!, {r4}		         ; CPSR of the task
 
	LDR		r4, =OSTCBCur
	LDR		r4, [r4]
	STR		sp, [r4]				 ; OSTCBCur -> stkptr = sp
	
	BL	tick_hook
	BL	OSIntEnter
	BL 	OSTimeTick
	BL	OSIntExit

	LDMFD 	sp!, {r4}                ; pop new task cpsr
	MSR 	cpsr_cxsf, r4
	LDMFD 	sp!, {r0-r12,lr,pc}      ; pop new task r0-r12,lr & pc

;//
        EXPORT 	ARMDisableInt
ARMDisableInt
	STMFD	sp!, {r0}	
	MRS	r0, cpsr
	ORR	r0, r0, #0xC0
	MSR	cpsr_c, r0		 ; disable IRQ Int s
	LDMIA	sp!, {r0}
	MOV	pc, lr

        EXPORT 	ARMEnableInt
ARMEnableInt
	STMDB	sp!, {r0}
	MRS	r0, CPSR
	BIC r0, r0, #0x80
	MSR	CPSR_c, r0
	LDMIA	sp!, {r0}
	MOV	pc, lr
    	
	END