;------------------------------------------------------------------------------
;  Handles incoming interrupt requests by branching to the corresponding
;  handler, as defined in the AIC. Supports interrupt nesting.
;  IRQ_STACK_SIZE = (3*8*4) - 3 words to be saved per interrupt priority level
;------------------------------------------------------------------------------

irq_handler:

       ; Adjust and save LR_irq in IRQ stack
                sub     lr, lr, #4
                stmfd   sp!, {lr}

       ; Save r0 and SPSR (need to be saved for nested interrupt)
                mrs     lr, SPSR
                stmfd   sp!, {r0, 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     lr, =AT91C_BASE_AIC
                ldr     r0, [r14, #AIC_IVR]
                str     lr, [r14, #AIC_IVR]

                nop    ; see http://www.arm.com/support/faqip/3682.html
                       ; On the ARM9TDMI and later cores, the STR write to
                       ; external memory may occur as little as one CLK cycle
                       ; before the interrupts are re-enabled by the MSR
                       ; instruction. Hence if the interrupt controller takes
                       ; longer than one ARM CLK cycle to clear the interrupt
                       ; signals into the core, it will re-enter the interrupt
                       ; exception handler.
                       ; If nested interrupts are being used, programmers
                       ; should ensure that there is some padding between the
                       ; acknowledge and re-enable of interrupts to allow time
                       ; for the interrupt signals to change

       ; Enable Interrupt and Switch in Supervisor Mode
                msr     CPSR_c, #SVC_MODE

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

       ; Branch to interrupt handler pointed by the AIC_IVR
                mov     lr, pc
                bx      r0

       ; Restore scratch/used registers and LR from User Stack
                ldmia   sp!, {r1-r3, r12, lr}
                msr     CPSR_c, #IRQ_MODE | I_BIT

       ; Acknowledge interrupt
                ldr     lr, =AT91C_BASE_AIC
                str     lr, [r14, #AIC_EOICR]

       ; Restore SPSR_irq and r0 from IRQ stack
                ldmia   sp!, {r0, lr}
                msr     SPSR_cxsf, lr

       ; Restore adjusted  LR_irq from IRQ stack directly in the PC
                ldmia   sp!, {pc}^