.text
.extern W90P710_CPU_ISR_Handler

.global IRQ_ISR_Handler
.global FIQ_ISR_Handler
.global CPSR_Init

.equ NO_INT	 ,0xC0					//Mask used to disable interrupts (Both FIR and IRQ)
.equ SYS32_MODE  ,0x1F
.equ FIQ32_MODE  ,0x11
.equ IRQ32_MODE  ,0x12

CPSR_Init:
		mov r0,#0x5f
		msr cpsr_c,r0
		mov pc,lr

//*********************************************************************************************************
//							  IRQ Interrupt Service Routine
//*********************************************************************************************************
IRQ_ISR_Handler:
		//保存現場				
		STR	R3,		[SP, #-4]!			//將需要使用的工作寄存器壓棧
		STR	R2,		[SP, #-4]!
		STR	R1,		[SP, #-4]!
		
		MOV	R1, SP						// 保存IRQ堆棧指針
		
		ADD	SP, SP,#12					// 調整IRQ堆棧指針
		SUB	R2, LR,#4					// 保存返回地址
		MRS	R3, SPSR		   			// 保存SPSR寄存器內容
		
		MSR	CPSR_c, #(NO_INT | SYS32_MODE)  // 切換成SYS模式
		
		//保存被中斷任務的寄存器現場
		STR	R2,		[SP, #-4]!			 
		STR	LR,		[SP, #-4]!			 
		STR	R12, 	[SP, #-4]!			
		STR	R11, 	[SP, #-4]!
		STR	R10, 	[SP, #-4]!
		STR	R9,		[SP, #-4]!
		STR	R8,		[SP, #-4]!
		STR	R7,		[SP, #-4]!
		STR	R6,		[SP, #-4]!
		STR	R5,		[SP, #-4]!
		STR	R4,		[SP, #-4]!
		LDR	R4,		[R1], #4			//從IRQ堆棧中讀出R1-R3的寄存器內容
		LDR	R5,		[R1], #4	   
		LDR	R6,		[R1], #4
		STR	R6,		[SP, #-4]!   		//將原來R1-R3的寄存器內容保存至堆棧
		STR	R5,		[SP, #-4]!
		STR	R4,		[SP, #-4]!
		
		STR	R0,		[SP, #-4]!	   		//將R0保存
		STR	R3,		[SP, #-4]!	 		//被中斷任務的CPSR寄存器內容保存
		
		//做模式切換，執行真正的中斷處理
		MSR	CPSR_c, #(NO_INT | IRQ32_MODE)
		
		//調用中斷處理函數——C源程序中的 W90P710_CPU_ISR_Handler	
		BL			W90P710_CPU_ISR_Handler
		
		MSR	CPSR_c, #(NO_INT | SYS32_MODE)  // 切換成SYS模式
		
		//恢復現場，原先保存的寄存器出棧
		LDR	R4,		[SP], #4		  
		MSR	CPSR_cxsf, R4
		LDR	R0,		[SP], #4		  
		LDR	R1,		[SP], #4
		LDR	R2,		[SP], #4
		LDR	R3,		[SP], #4
		LDR	R4,		[SP], #4
		LDR	R5,		[SP], #4
		LDR	R6,		[SP], #4
		LDR	R7,		[SP], #4
		LDR	R8,		[SP], #4
		LDR	R9,		[SP], #4
		LDR	R10,	[SP], #4
		LDR	R11,	[SP], #4
		LDR	R12,	[SP], #4
		LDR	LR,		[SP], #4
		LDR	PC,		[SP], #4


//*********************************************************************************************************
//							  FIQ Interrupt Service Routine
//*********************************************************************************************************
FIQ_ISR_Handler:
								
	   STMFD	SP!,{R1-R3}			    	//PUSH WORKING REGISTERS ONTO IRQ STACK
	   
	   MOV		R1,	SP						//Save   IRQ 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 | SYS32_MODE) //Change to SYS mode

		//SAVE TASK'S CONTEXT ONTO TASK'S STACK
	   STMFD   	SP!,{R2}				  	//   Push task's Return PC 
	   STMFD   	SP!,{R4-R12,LR}				//   Push task's LR,R12-R4
	   
	   LDMFD   	R1!,{R4-R6}			    	//   Move task's R1-R3 from IRQ stack to SYS 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. IRQ's SPSR)
	   
	   MSR		CPSR_c,#(NO_INT | FIQ32_MODE) //Change to FIQ mode (to use the FIQ stack to handle interrupt)
	   
	   BL	 	W90P710_CPU_ISR_Handler	   	//OS_CPU_FIQ_ISR_Handler()//
	   
	   MSR		CPSR_c,#(NO_INT | SYS32_MODE) //Change to SYS mode
	   
									 		//RESTORE TASK'S CONTEXT and RETURN TO TASK
	   LDMFD   	SP!,{R4}				  	//pop new task's CPSR
	   MSR		CPSR_cxsf,r4
	   LDMFD   	SP!,{R0-R12,LR,PC}		  	//pop new task's R0-R12,LR & PC

.end