;------------------------------------------------------------------------------
;-    ATMEL Microcontroller Software Support  -   ROUSSET -
;------------------------------------------------------------------------------
; The software is delivered "AS IS" without warranty or condition of any
; kind, either express, implied or statutory. This includes without
; limitation any warranty or condition with respect to merchantability or
; fitness for any particular purpose, or against the infringements of
; intellectual property rights of others.
;------------------------------------------------------------------------------
;- File source          : irq_arm.mac
;- Object               : Entry and Exit Macro for IRQ and FIQ.
;-
;- 1.0 13/Feb/03 JPP    : New clean 
;------------------------------------------------------------------------------

;------------------------------------------------------------------------------
;- LISR vector handler for system peripherals
;--------------------------------------------
;- These macro save the context, call the LISR dispatch routine, and restore
;- the context
;------------------------------------------------------------------------------

;--------------------------------
;- ARM Core Mode and Status Bits
;--------------------------------

ARM_MODE_USER           EQU     0x10
ARM_MODE_FIQ            EQU     0x11
ARM_MODE_IRQ            EQU     0x12
ARM_MODE_SVC            EQU     0x13
ARM_MODE_ABORT          EQU     0x17
ARM_MODE_UNDEF          EQU     0x1B
ARM_MODE_SYS            EQU     0x1F

I_BIT                   EQU     0x80
F_BIT                   EQU     0x40
T_BIT                   EQU     0x20
        IMPORT  __OSIntCtxSw                      ;中斷中任務切換函數
        IMPORT  OSIntExit                       ;中斷退出函數
        IMPORT  OSTCBCur                        ;指向當前任務TCB的指針
        IMPORT  OSTCBHighRdy                    ;指向將要運行的任務TCB的指針  
        IMPORT  OSIntNesting                    ;中斷嵌套計數器

;- IRQ Entry
;- Register initialization
;-      Banked Registers        ; Irq_register
;-      SPSR need to be saved for nested interrupt
;-      R13_irq <- Irq Stack register
;-      R14_irq <- Irq link register
;-      Outpout in ARM_MODE_SVC
;-----------
    	MACRO
	IRQ_ENTRY_T0	
        SUB     LR, LR, #4                      ; 計算返回地址
        STMFD   SP!, {R0-R3, R12, LR}           ; 保存任務環境
;- Write in the IVR to support Protect Mode
;- No effect in Normal Mode
;- De-assert the NIRQ and clear the source in Protect Mode
            ldr         r14, =AT91C_BASE_AIC
            str         r14, [r14, #AIC_IVR]
        MRS     R3, SPSR                        ; 保存狀態
        STMFD   SP!, {R3}
;- Enable Interrupt and Switch in SYS Mode
		mrs         r0, CPSR
		bic         r0, r0, #I_BIT
;=============================================		
		orr         r0, r0, #ARM_MODE_SYS
		msr         CPSR_c, r0
;- Save scratch/used registers and LR in User Stack
            stmfd       sp!, { r0-r3, r12, r14}
            
        LDR     R2,  =OSIntNesting              ; OSIntNesting++
        LDRB    R1, [R2]
        ADD     R1, R1, #1
        STRB    R1, [R2]
        MEND        
;        BL      $IRQ_Exception1                  ; 調用c語言的中斷處理程序
            MACRO
     	    IRQ_EXIT_T0 
     	BL      OSIntExit     	                
;- Restore scratch/used registers and LR from User Stack
            ldmia       sp!, { r0-r3, r12, r14}        
;- Disable Interrupt and switch back in IRQ mode
		mrs         r0, CPSR
		bic         r0, r0, #ARM_MODE_SYS
		orr         r0, r0, #I_BIT:OR:ARM_MODE_IRQ
		msr         CPSR_c, r0
;- Mark the End of Interrupt on the AIC
            ldr         r14, =AT91C_BASE_AIC
            str         r14, [r14, #AIC_EOICR]
;=====================================================                                       
        LDR     R0, =OSTCBHighRdy
        LDR     R0, [R0]
        LDR     R1, =OSTCBCur
        LDR     R1, [R1]
        CMP     R0, R1
        
        LDMFD   SP!, {R3}
        MSR     SPSR_cxsf, R3

        LDMEQFD SP!, {R0-R3, R12, PC}^          ; 不進行任務切換
        LDR     PC, =__OSIntCtxSw                 ; 進行任務切換
    	MEND 
    
    
            MACRO
            IRQ_ENTRY

;- Adjust and save LR_irq in IRQ stack
            sub         lr, lr, #4
            stmfd       sp!, {lr}
;- Write in the IVR to support Protect Mode
;- No effect in Normal Mode
;- De-assert the NIRQ and clear the source in Protect Mode
            ldr         r14, =AT91C_BASE_AIC
            str         r14, [r14, #AIC_IVR]
;- Save SPSR and r0 in IRQ stack
            mrs         r14, SPSR
            stmfd       sp!, {r14}
;- Enable Interrupt and Switch in Supervisor Mode
           msr         CPSR_c, #ARM_MODE_SVC
;- Save scratch/used registers and LR in User Stack
            stmfd       sp!, { r0-r3, r12, r14}
            MEND
            
;- IRQ Exit
            MACRO
     	    IRQ_EXIT
;- Restore scratch/used registers and LR from User Stack
            ldmia       sp!, { r0-r3, r12, r14}

;- Disable Interrupt and switch back in IRQ mode
            msr         CPSR_c, #I_BIT :OR: ARM_MODE_IRQ;切換到中斷模式

;- Mark the End of Interrupt on the AIC
            ldr         r14, =AT91C_BASE_AIC
            str         r14, [r14, #AIC_EOICR]
;- Restore SPSR_irq and r0 from IRQ stack
            ldmia       sp!, {r14}
            msr         SPSR_cxsf, r14
;- Restore adjusted  LR_irq from IRQ stack directly in the PC
            ldmia       sp!, {pc}^
            MEND

;-------------------
;- FIQ Entry Macro -
;-------------------
;- FIQ Entry with no nested (reentrant) interrupt allowed.
;- Register initialization
;-      Banked Registers        ; FIQ_register
;-      SPSR does not need to be save because we do not want nested FIQ
;-      R13_fiq <- Same as above. No need to save it.
;-      R14_irq <- link register
;------------------------------------------------------------------------

            MACRO
            FIQ_ENTRY

;- Switch in SYS/User Mode to allow User Stack access for C code and Disable FIQ and IRQ Interrupt for this mode
; because the FIQ is not yet acknowledged

            msr         CPSR_c, #I_BIT:OR:F_BIT:OR:ARM_MODE_SVC

;- Save scratch/used registers and LR in User Stack
            stmfd       sp!, { r0-r3, r12, lr}
            MEND

;------------
;- FIQ Exit -
;------------
            MACRO
            FIQ_EXIT

;- Restore scratch/used registers and LR from User Stack
            ldmia       sp!, { r0-r3, r12, lr}

;- Leave Interrupts disabled and switch back in FIQ mode
            msr         CPSR_c, #I_BIT :OR: F_BIT :OR: ARM_MODE_FIQ

;- Restore the Program Counter using the LR_fiq directly in the PC
            subs        pc,lr,#4
            
            MEND
            
	    
	END