LDR     R0, ??OS_TaskSwHook     ; OSTaskSwHook();
        MOV     LR, PC
        BX      R0

        LDR     R4,??OS_PrioCur         ; OSPrioCur = OSPrioHighRdy
        LDR     R5,??OS_PrioHighRdy
        LDRB    R6,[R5]
        STRB    R6,[R4]

        LDR     R4,??OS_TCBCur          ; OSTCBCur  = OSTCBHighRdy;
        LDR     R6,??OS_TCBHighRdy
        LDR     R6,[R6]
        STR     R6,[R4]

        LDR     SP,[R6]                 ; SP = OSTCBHighRdy->OSTCBStkPtr;

                                        ; RESTORE NEW TASK'S CONTEXT
        LDMFD   SP!, {R4}               ;    Pop new task's CPSR
        MSR     SPSR_cxsf, R4

        LDMFD   SP!, {R0-R12,LR,PC}^    ;    Pop new task's context


;*********************************************************************************************************
;                                      IRQ Interrupt Service Routine
;*********************************************************************************************************

        RSEG CODE:CODE:NOROOT(2)
        CODE32

OS_CPU_IRQ_ISR

	STMFD	sp!, {r4}                ;/* 這個SP是IRQ模式下的*/
	
	LDR		r4, =LINK_SAVE
	STR		lr, [r4]				 ;/* LINK_SAVE = lr_irq */

	LDR		lr, =PSR_SAVE
	MRS		r4,	spsr
	STR		r4, [lr]			 ;/*	 PSR_SAVE = spsr_irq  */


	LDMFD	sp!, {r4}                ;/* 重新恢復R4 */
	
	MRS		lr,	spsr
	ORR		lr, lr,	#0x80		;/*	 Mask irq for context switching before */
	MSR		cpsr_cxsf, lr 		;/*	returning back from irq mode.我們還沒有 */
								;	/* 保存好現(xiàn)場,如果打開中斷，就有可能發(fā)生新的 */
								;	/* 搶占式調(diào)度，于是這個現(xiàn)場就OVER了。現(xiàn)場保 */
								;	/* 護和現(xiàn)場恢復都要一氣呵成 */

	SUB		sp, sp, #4			;	 /* Space for PC  這個SP是svc(任務)模式下的*/
	STMFD	sp!, {r0-r12, lr}

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

	LDR		r4, =PSR_SAVE
	LDR		r4, [r4]			; /* r4 = PSR_SAVE */
	STMFD	sp!, {r4}		      ;   /* CPSR of the task */

 	LDR		r5, =OSIntNesting
 	LDRB 	r4, [r5]
 	ADD     r4, r4, #1   ;/* OSIntNesting++ */
 	STRB     r4, [r5]
 	CMP		r4, #1
 	
 	BNE		NOT_FIRST
 	
	LDR		r4, =OSTCBCur
	LDR		r4, [r4]
	STR		sp, [r4]				; /* OSTCBCur -> stkptr = sp  保存現(xiàn)場完畢 */

NOT_FIRST

	LDR  r4, =0xFFFFF100  ;AT91C_AIC_IVR
	LDR  r4, [r4]				;/*獲得中斷向量,同時清除中斷，邊沿觸發(fā)時功能同上面注釋得的代碼*/
	CMP	 r4, #0
	BEQ   EXIT_INT

    MRS     R0,CPSR                     ;/* 開中斷*/
    AND     R1,R0,#0xffffff7f;
    MSR     CPSR_c,R1

	MOV  lr, pc
	BX   r4	

EXIT_INT
    MRS     R0,CPSR                     ;/*關閉中斷*/
    ORR     R4,R0,#0x80;
    MSR     CPSR_c,R4
								
	LDR  r4, =0xFFFFF130  ;AT91C_AIC_EOICR
	STR  r0, [r4]				;/*中斷服務程序處理完成*/
	
	BL	OSIntExit
	
	LDMFD 	sp!, {r4}              ;  /* pop current task cpsr */
	MSR 	cpsr_cxsf, r4
	LDMFD 	sp!, {r0-r12,lr,pc}    ;  /*  pop current task r0-r12,lr & pc */



;*********************************************************************************************************
;                                      FIQ Interrupt Service Routine
;*********************************************************************************************************

        RSEG CODE:CODE:NOROOT(2)
        CODE32

OS_CPU_FIQ_ISR

        STMFD   SP!, {R1-R3}                   ; PUSH WORKING REGISTERS ONTO FIQ STACK

        MOV     R1, SP                         ; Save   FIQ stack pointer

        ADD     SP, SP,#12                     ; Adjust FIQ stack pointer

        SUB     R2, LR,#4                      ; Adjust PC for return address to task

        MRS     R3, SPSR                       ; Copy SPSR (i.e. interrupted task's CPSR) to R3

        MSR     CPSR_c, #(NO_INT | SVC32_MODE) ; Change to SVC mode

                                               ; SAVE TASK'S CONTEXT ONTO TASK'S STACK
        STMFD   SP!, {R2}                      ;    Push task's Return PC
        STMFD   SP!, {LR}                      ;    Push task's LR
        STMFD   SP!, {R4-R12}                  ;    Push task's R12-R4

        LDMFD   R1!, {R4-R6}                   ;    Move task's R1-R3 from FIQ stack to SVC stack
        STMFD   SP!, {R4-R6}
        STMFD   SP!, {R0}                      ;    Push task's R0    onto task's stack
        STMFD   SP!, {R3}                      ;    Push task's CPSR (i.e. FIQ's SPSR)

                                               ; HANDLE NESTING COUNTER
        LDR     R0, ??OS_IntNesting            ; OSIntNesting++;
        LDRB    R1, [R0]
        ADD     R1, R1,#1
        STRB    R1, [R0]

        CMP     R1, #1                         ; if (OSIntNesting == 1) {
        BNE     OS_CPU_FIQ_ISR_1

        LDR     R4, ??OS_TCBCur                ;     OSTCBCur->OSTCBStkPtr = SP
        LDR     R5, [R4]
        STR     SP, [R5]                       ; }

OS_CPU_FIQ_ISR_1
        MSR     CPSR_c, #(NO_INT | FIQ32_MODE) ; Change to FIQ mode (to use the FIQ stack to handle interrupt)

      ;  LDR     R0, ??OS_CPU_FIQ_ISR_Handler   ; OS_CPU_FIQ_ISR_Handler();
      ;  MOV     LR, PC
      ;  BX      R0

        MSR     CPSR_c, #(NO_INT | SVC32_MODE) ; Change to SVC mode

        LDR     R0, ??OS_IntExit               ; OSIntExit();
        MOV     LR, PC
        BX      R0

                                               ; RESTORE NEW TASK'S CONTEXT
        LDMFD   SP!, {R4}                      ;    Pop new task's CPSR
        MSR     SPSR_cxsf, R4

        LDMFD   SP!, {R0-R12,LR,PC}^           ;    Pop new task's context


;*********************************************************************************************************
;                                     POINTERS TO VARIABLES
;*********************************************************************************************************

        DATA

??OS_TaskSwHook:
        DC32    OSTaskSwHook

??OS_CPU_IRQ_ISR_Handler:
        DC32    OS_CPU_IRQ_ISR_Handler

??OS_CPU_FIQ_ISR_Handler:
        DC32    0; OS_CPU_FIQ_ISR_Handler

??OS_IntExit:
        DC32    OSIntExit

??OS_IntNesting:
        DC32    OSIntNesting

??OS_PrioCur:
        DC32    OSPrioCur

??OS_PrioHighRdy:
        DC32    OSPrioHighRdy

??OS_Running:
        DC32    OSRunning

??OS_TCBCur:
        DC32    OSTCBCur

??OS_TCBHighRdy:
        DC32    OSTCBHighRdy


LINK_SAVE:
        DC32	0x00
PSR_SAVE:
        DC32	0x00
INTMSK_SAVE:
        DC32	0x00


        END