BIT_TIMER0	EQU		(0x1<<13)
I_ISPC		EQU		0x1e00024
INTMSK		EQU		0x1e0000c

	AREA	|subr|, CODE, READONLY
	
	IMPORT	OSTCBCur
addr_OSTCBCur		DCD	OSTCBCur
	IMPORT	OSTCBHighRdy
addr_OSTCBHighRdy	DCD	OSTCBHighRdy
	IMPORT	OSPrioCur
addr_OSPrioCur		DCD	OSPrioCur
	IMPORT	OSPrioHighRdy
addr_OSPrioHighRdy	DCD	OSPrioHighRdy	
	
	IMPORT  OSRunning

	IMPORT	OSIntEnter
	IMPORT	OSIntExit
	IMPORT	OSTimeTick
	IMPORT	EInt4567Isr
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; Function: 	OSStartHighRdy																			;
; Purpose:		To start the task with the highest priority during OS startup							;
; Processing:	See uC/OS-II Task Level Context Switch flow chart										;
; Parameters: 	void																					;
; Outputs:  	None																					;
; Returns:  	void																					;
; Notes:		Called once during OSStart()															;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
	EXPORT	OSStartHighRdy
OSStartHighRdy
	LDR		r0, =OSRunning			; OSRunning = 1
	MOV		r1, #1
	STRB	r1, [r0]
	
;	LDR		r4, addr_OSTCBCur		; Get current task TCB address
	LDR		r5, addr_OSTCBHighRdy	; Get highest priority task TCB address

	LDR		r5, [r5]				; get stack pointer
	LDR		sp, [r5]				; switch to the new stack

;	STR		r5, [r4]				; set new current task TCB address

	LDMFD	sp!, {r4}				; get new state from top of the stack
	MSR		CPSR_cxsf, r4			; CPSR should be SVC32Mode
	LDMFD	sp!, {r0-r12, lr, pc }	; start the new task

	
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; Function: 	OS_TASK_SW 																				;
; Purpose: 		To perform a context switch from the Task Level.										;
; Processing:	See uC/OS-II Task Level Context Switch flow chart										;
; Parameters:	void																					;
; Outputs:  	None																					;
; Returns:		void																					;
; Notes:		The whole function is executed in CRITICAL state. See OSSched().						;
;																										;
; On entry, OSTCBCur and OSPrioCur hold the current TCB and priority									;
; and OSTCBHighRdy and OSPrioHighRdy contain the same for the task										;
; to be switched to.																					;
; The following code assumes that the virtual memory is directly										;
; mapped into  physical memory. If this is not true, the cache must 									;
; be flushed at context switch to avoid address aliasing.												;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
	EXPORT	OSCtxSw
OSCtxSw
	STMFD	sp!, {lr}				; save pc
	STMFD	sp!, {lr}				; save lr
	STMFD	sp!, {r0-r12}			; save register file and ret address
	MRS		r4, CPSR
	STMFD	sp!, {r4}				; save current PSR
	
	LDR		r4, addr_OSTCBCur
	LDR		r5, [r4]
	STR		sp, [r5]				; store sp in preempted tasks TCB

	; OSPrioCur = OSPrioHighRdy
	LDR		r4, addr_OSPrioCur
	LDR		r5, addr_OSPrioHighRdy
	LDRB	r6, [r5]
	STRB	r6, [r4]
	
	; OSTCBCur = OSTCBHighRdy
	LDR		r4, addr_OSTCBHighRdy
	LDR		r5, addr_OSTCBCur	
	LDR		r4, [r4]
	STR		r4, [r5] 

	LDR		sp, [r4]
	LDMFD	sp!, {r4}				; YYY+
	MSR		CPSR_cxsf, r4			; YYY+
	LDMFD	sp!, {r0-r12, lr, pc}	; YYY+


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; Function: 	OSIntCtxSW 																				;
; Purpose: 		To perform a context switch from the Interrupt Level.									;
; Processing:	See uC/OS-II Interrupt Level Context Switch flow chart									;
; Parameters:	void																					;
; Outputs:  	None																					;
; Returns:		void																					;
; Notes:		The whole function is executed in CRITICAL state. See OSIntExit().						;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
	EXPORT 	OSIntCtxSw
OSIntCtxSw
	;OSPrioCur = OSPrioHighRdy
	LDR		r6, addr_OSPrioHighRdy
	LDR		r5, addr_OSPrioCur	
	LDRB	r6, [r6]
	STRB	r6, [r5]			

	;OSTCBCur = OSTCBHighRdy
	LDR		r4, addr_OSTCBHighRdy
	LDR		r5, addr_OSTCBCur	
	LDR		r4, [r4]
	STR		r4, [r5] 

	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


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; Function:		OSTickISR																				;
; Purpose:		Timer0 Interrupt Sever																	;
; Processing:	Call OSTimeTick																			;
; Parameters: 	void																					;
; Outputs:		None																					;
; Returns:		void																					;
; Notes:																								;					
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
LINK_SAVE	DCD		0
PSR_SAVE	DCD		0
	EXPORT 	OSTickISR
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, addr_OSTCBCur
	LDR		r4, [r4]
	STR		sp, [r4]				 ; OSTCBCur -> stkptr = sp

	LDR		r0, =I_ISPC
	LDR		r1, =BIT_TIMER0
	STR		r1, [r0]

	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
	
	
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; Functions: 	ARMDisableInt	ARMEnableInt															;
; Purpose:		Disable and enable IRQ and FIQ preserving current CPU mode.								;
; Processing:	Push the cpsr onto the stack, Disable IRQ and FIQ interrupts, Return					;
; Parameters: 	void																					;
; Outputs:  	None																					;
; Returns:  	void																					;
; Notes:																								;
;   (1) Can be called from SVC mode to protect Critical Sections. 										;
;   (2) Do not use these calls at interrupt level.														;
;   (3) Used in pairs within the same function level;													;
;   (4) Will restore interrupt state when called; i.e., if interrupts									;
;       are disabled when DisableInt is called, interrupts will still									;
;       still be disabled when the matching EnableInt is called.										;
;   (5) Uses the method described by Labrosse as "Method 2".											;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
	EXPORT 	ARMDisableInt
ARMDisableInt
	MRS		r0, cpsr
	STMFD	sp!, {r0}	         	; push current PSR
	ORR		r0, r0, #0xC0
	MSR		cpsr_c, r0		 		; disable IRQ Int s
	MOV	pc, lr

	EXPORT 	ARMEnableInt
ARMEnableInt
	LDMFD	sp!, {r0}				; pop current PSR
	MSR		cpsr_c, r0				; restore original cpsr	
	MOV		pc, lr

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;                                   CRITICAL SECTION METHOD 3 FUNCTIONS
;
; Description: Disable/Enable interrupts by preserving the state of interrupts.  Generally speaking you
;              would store the state of the interrupt disable flag in the local variable 'cpu_sr' and then
;              disable interrupts.  'cpu_sr' is allocated in all of uC/OS-II's functions that need to 
;              disable interrupts.  You would restore the interrupt disable state by copying back 'cpu_sr'
;              into the CPU's status register.
;              OS_CPU_SR OSCPUSaveSR()
; Arguments  : none
; Returns    : OS_CPU_SR
;              OSCPURestoreSR(OS_CPU_SR cpu_sr)
; Arguments  : OS_CPU_SR
; Returns    : none
; Note(s)    : These functions are used in general like this,
;
;            void Task (void *data)
;            {
;                    #if OS_CRITICAL_METHOD == 3 /* Allocate storage for CPU status register */
;                          OS_CPU_SR  cpu_sr;
;                    #endif
;                         :
;                         :
;                    OS_ENTER_CRITICAL(); /* cpu_sr = OSCPUSaveSR(); */
;                         :
;                         :
;                    OS_EXIT_CRITICAL();  /* OSCPURestoreSR(cpu_sr); */
;                         :
;                         :
;            }
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
	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

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;																										;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
K_LINK_SAVE	DCD		0
K_PSR_SAVE	DCD		0
	EXPORT 	OSEINT4567ISR
OSEINT4567ISR
	STMFD	sp!, {r4}
	LDR		r4, =K_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, =K_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, addr_OSTCBCur
	LDR		r4, [r4]
	STR		sp, [r4]				 ; OSTCBCur -> stkptr = sp

	BL	OSIntEnter
	BL 	EInt4567Isr
	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
	
	END