//*------------------------------------------------------------------------------
//*- Function             : IRQ_Handler_Entry
//*- Treatments           : IRQ Controller Interrupt Handler.
//*- Called Functions     : AIC_IVR[interrupt]
//*------------------------------------------------------------------------------*/
        .global IRQ_Handler_Entry
        .func   IRQ_Handler_Entry

IRQ_Handler_Entry:

/*- Manage Exception Entry  */
/*- Adjust and save LR_irq in IRQ stack  */
            sub         lr, lr, #4
            stmfd       sp!, {lr}

/*- Save SPSR need to be saved for nested interrupt */
            mrs         r14, SPSR
            stmfd       sp!, {r14}

/*- Save and r0 in IRQ stack  */
            stmfd       sp!, {r0}

/*- 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
        ldr         r0 , [r14, #AIC_IVR]
        str         r14, [r14, #AIC_IVR]

/*- Enable Interrupt and Switch in Supervisor Mode */
            msr         CPSR_c, #ARM_MODE_SVC

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

/*- Branch to the routine pointed by the AIC_IVR  */
            mov         r14, pc
            bx          r0
/*- Restore scratch/used registers and LR from User Stack*/
            ldmia       sp!, { r1-r3, r12, r14}

/*- Disable Interrupt and switch back in IRQ mode */
            msr         CPSR_c, #I_BIT | 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!, {r0}

/*- 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}^
    
        .size   IRQ_Handler_Entry, . - IRQ_Handler_Entry
        .endfunc

void InitUSART(AT91S_USART *pUSART, DWORD imode, DWORD baudrate)
{
//--------------------------------------------------------------------------
//clear all
//--------------------------------------------------------------------------
pUSART->US_CR =    AT91C_US_RSTRX    |            // Reset Receiver
                AT91C_US_RSTTX    |            // Reset Transmitterp
                AT91C_US_RXDIS    |            // Receiver Disable
                AT91C_US_TXDIS;            // Transmitter Disable
AT91C_BASE_PDC_US1->PDC_PTCR = AT91C_PDC_RXTDIS |AT91C_PDC_TXTDIS; //PDC off
AT91C_BASE_US1->US_TCR = 0;
AT91C_BASE_US1->US_RCR = 0;
//--------------------------------------------------------------------------
//configure interrupt
//--------------------------------------------------------------------------
AT91F_AIC_ConfigureIt    (AT91C_BASE_AIC,             //base adr
                        AT91C_ID_US1,                //int number
                        AT91C_AIC_PRIOR_HIGHEST - 1,//priority
                        //AT91C_AIC_PRIOR_HIGHEST,    //priority
                        3,                            //int source type
                        USARTInterrupt                //handler
                        );            
//--------------------------------------------------------------------------                        
//USART on
//--------------------------------------------------------------------------
AT91F_PMC_EnablePeriphClock(AT91C_BASE_PMC,(1 << AT91C_ID_US1));
//--------------------------------------------------------------------------
//configure interrupt
//--------------------------------------------------------------------------
pUSART->US_IDR = 0xFFFFFFFF;
pUSART->US_IER = imode;
//--------------------------------------------------------------------------
//set baudrate
//--------------------------------------------------------------------------
AT91F_US_SetBaudrate(pUSART,48054857,baudrate);
//--------------------------------------------------------------------------
//set  timeouts
//--------------------------------------------------------------------------
pUSART->US_TTGR = 20;
pUSART->US_RTOR = 200;
//--------------------------------------------------------------------------
//set mode
//--------------------------------------------------------------------------
pUSART->US_MR = AT91C_US_USMODE_NORMAL    |    // Normal Mode
                AT91C_US_CLKS_CLOCK       |    // Clock = MCK
                AT91C_US_CHRL_8_BITS       |    // 8-bit Data
                AT91C_US_PAR_NONE        |    // No Parity
                AT91C_US_NBSTOP_2_BIT;        // 1 Stop Bit
//--------------------------------------------------------------------------
//Init DMA
//--------------------------------------------------------------------------
AT91C_BASE_PDC_US1->PDC_PTCR = AT91C_PDC_RXTDIS;
AT91C_BASE_PDC_US1->PDC_PTCR = AT91C_PDC_TXTDIS;
//active buffer (start from buf 0)
act_buf = 0;                            
//first RX buffer
pUSART->US_RPR = (DWORD)&rx_buf[0];    //set pointer as register value
pUSART->US_RCR = sizeof(BUF);        //base buffer size
// enable the RX and TX PDC transfer requests
AT91C_BASE_PDC_US1->PDC_PTCR = AT91C_PDC_RXTEN;
AT91C_BASE_PDC_US1->PDC_PTCR = AT91C_PDC_TXTEN;
//--------------------------------------------------------------------------
//enable device
//--------------------------------------------------------------------------
pUSART->US_CR =    AT91C_US_RXEN | AT91C_US_TXEN;            
//--------------------------------------------------------------------------
//enable interrupt
//--------------------------------------------------------------------------
AT91F_AIC_EnableIt (AT91C_BASE_AIC, AT91C_ID_US1);
}

void AT91F_US_Get(void) {
  int d[10];
  memset(d, 0, sizeof(d));
  int j = 0;
  while(1) {
    while (!AT91F_US_RxReady(COM0));
    d[j] = AT91F_US_GetChar(COM0);
        if (d[j] == 0x4B || j > 10) break;
        else j++;
  }
  d[++j] = 0;
  AT91F_US_Put((char*)&d);
}

__inline int AT91F_US_GetChar (
    const AT91PS_USART pUSART)
{
    return((pUSART->US_RHR) & 0x1FF);
}

while(1) {
    while (!AT91F_US_RxReady(COM0));