//     AREA	|subr|, CODE, READONLY

/*;/***********************************************************************
;
; 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()
;
;*********************************************************************/
	.global 	OSStartHighRdy
	.extern    OSTaskSwHook,OSTCBHighRdy,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

//; next two lines removed by YJ
//;        LDMFD 	sp!, {r4}             //   ; pop new task s spsr_cxsf
//;        MSR 	spsr_cxsf, r4
        LDMFD 	sp!, {r4}              //  ; pop new task s psr
        MSR 	cpsr, r4
        LDMFD 	sp!, {r0-r12,lr,pc}    //  ; pop new task s r0-r12,lr & pc

 	
/***********************************************************************
;
; 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.
;
;*********************************************************************/
        .global 	OSCtxSw
        .extern	OSPrioCur,OSPrioHighRdy,OSTCBCur,OSTaskSwHook,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
//; next two lines removed by YJ
   //;     MRS 	r4, spsr
    //;    STMFD 	sp!, {r4}              //  ; push current spsr_cxsf


        
        LDR 	r4, =OSTCBCur          //  ; Get current task TCB address
        LDR		r5, [r4]
        STR 	sp, [r5]               //  ; store sp in preempted tasks s TCB

_OSCtxSw:
		BL 	OSTaskSwHook                // call Task Switch Hook

        LDR 	r5, =OSTCBHighRdy      //; Get highest priority task TCB address
        LDR		r5, [r5]
        STR		r5, [r4]			//; OSTCBCur = OSTCBHighRdy
        
        LDR 	r6, =OSPrioHighRdy       
        LDRB 	r6, [r6]
        LDR 	r4, =OSPrioCur           
        STRB 	r6, [r4]			//; OSPrioCur = OSPrioHighRdy
        
        LDR 	sp, [r5]              // ; get new task s stack pointer

//;; next two lines removed by YJ
//;;        LDMFD 	sp!, {r4}                ; pop new task spsr_cxsf
//;;        MSR 	spsr_cxsf, r4

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

/*;***********************************************************************
;
; 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:
;   Sets up the stacks and registers to call the task level
;   context switch
;
;*********************************************************************/
       .global 	OSIntCtxSw
//;----------------------------------------------------------------
//; modefied by YJ
//;----------------------------------------------------------------
//; added starts --------------<<
		.extern 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, r4
		LDMFD 	sp!, {r0-r12,lr,pc}	//	 ; pop new task r0-r12,lr & pc
/*; >>---------------- added ends

; removed starts ------------<<
;        IMPORT	OSIntCtxSwFlag
;OSIntCtxSw
;        LDR 	r0, =OSIntCtxSwFlag      ; OSIntCtxSwFlag = true
;        MOV 	r1, #1
;        STR 	r1, [r0]
;        
;        MOV 	pc, lr                   ; return 
; >>-------------- removed ends
;----------------------------------------------------------------
; end of modification
;----------------------------------------------------------------*/

/***********************************************************************
;
;yangye 2003-2-14
;changed this function name from OSTickISR to OSISR(it is not a TICK isr)
; Function: OSISR
;
; Purpose:
; 	   The IRQ interrupt handler
;
; Processing:
;    Saves context
;    Calls the IRQ dispatcher
;    Checks if context switch necessary
;    If not, restores context and returns from interrupt
;    If switch required, branches without link to IRQContextSwap
;       which performs context switch if interrupts not nested
;       and returns from interrupt to new context
;
; Parameters: void
;
; Outputs:  None
;
; Returns:  void
;
; Notes:
;   (1) here we use OSIntCtxSwFlag to indicate a request for int-level 
;       context switch 
;   (2) _IntCtxSw is used to perform a real switch operation
;
;*********************************************************************/	

//;----------------------------------------------------------------------
//; modified by YJ
//;----------------------------------------------------------------------
//; added starts <<------------------------------------------------------
	.global 	OSTickISR
	.extern	OSIntEnter,OSTimeTick,tick_hook,OSIntExit

LINK_SAVE: .word	0
PSR_SAVE:  .word	0

OSTickISR:
    ldmia    r13!,{r0-r8,r12,r14}
	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, 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  DmpStk
	BL	OSIntEnter
	BL 	OSTimeTick
	BL	tick_hook  	                 //; here do_IRQ is used to clear some virtual-hardware flags
	BL	OSIntExit
//;	BL  DmpStk

//;	LDR		r4, =OSTCBHighRdy
 //   stmdb    r13!,{r0-r8,r12,r14}

	LDMFD 	sp!, {r4}               // ; pop new task cpsr
	MSR 	cpsr, r4
	LDMFD 	sp!, {r0-r12,lr,pc}     // ; pop new task r0-r12,lr & pc
/*; --------------------------------------------------------->> added ends


; --------------------------------------------------------->> removed ends*/
	
	
/***********************************************************************
;
; 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".
;
;*********************************************************************/
        .global 	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

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