/* Configure CPLD_DATA_RDY input - clear output, configure input and enable glitch filter */
AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, CPLD_DATA_RDY);
AT91F_PIO_CfgInput(AT91C_BASE_PIOA, CPLD_DATA_RDY);  
AT91F_PIO_InputFilterEnable(AT91C_BASE_PIOA, CPLD_DATA_RDY);

/* Enable PIOA interrupt on CPLD_DATA_RDY change */
AT91F_PIO_InterruptEnable(AT91C_BASE_PIOA,CPLD_DATA_RDY);

/* Configure FIQ */
AT91F_AIC_ConfigureIt(AT91C_BASE_AIC, AT91C_ID_FIQ,
                       0, AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL, (void (*)( void ))FIQ_handler);

/* enable fast forcing for PIOA interrupt */
*AT91C_AIC_FFER = (1 << AT91C_ID_PIOA);

/* Enable PIOA interrupt in AIC */
AT91F_AIC_EnableIt(AT91C_BASE_AIC, AT91C_ID_PIOA);

__arm __fiq void FIQ_handler(void)
{
unsigned portLONG pio;

/* Get the byte and confirm reseption */
AT91F_PIO_ClearOutput(AT91C_BASE_PIOA,CPLD_CNFRM);
pio = AT91F_PIO_GetInput(AT91C_BASE_PIOA);

/* Put data into the buffer */
CPLD_Buf[CPLD_BufHead] = pio;
CPLD_BufHead++;
CPLD_BufHead &= CPLD_BUF_LENGTH;
/* */
AT91F_PIO_SetOutput(AT91C_BASE_PIOA,CPLD_CNFRM);
FIQCounter++;

/* Clear interrupt cause */
pio = AT91C_BASE_PIOA->PIO_ISR;
pio = pio;

AT91F_AIC_AcknowledgeIt(AT91C_BASE_AIC);
}

__arm __interwork static inline __disable_IRQ(void)
{      
  unsigned long tmp;

  while (!((tmp=__get_CPSR()) & (1<<7)))
    {
        __set_CPSR(tmp | (1<<7));
    }
}

#define portDISABLE_INTERRUPTS() __disable_IRQ()

FIQ_Handler_Entry:

;- Switch in SVC/User Mode to allow User Stack access for C code
; because the FIQ is not yet acknowledged

;- Save and r0 in FIQ_Register
            mov         r9,r0
            ldr         r0 , [r8, #AIC_FVR]
            msr         CPSR_c,#I_BIT | F_BIT | ARM_MODE_SVC

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

;- Branch to the routine pointed by the AIC_FVR
            mov         r14, pc
            bx          r0

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

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

;- Restore the R0 ARM_MODE_SVC register
            mov         r0,r9

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