void AT91F_USB_SetInt(void)
{
//Init interupt
AT91F_AIC_ConfigureIt (AT91C_BASE_AIC, AT91C_ID_UDP, UDP_INTERRUPT_LEVEL, AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL, UDP_IRQ_Handler);
AT91F_UDP_DisableIt (AT91C_BASE_UDP, AT91C_UDP_SOFINT);
AT91F_UDP_InterruptClearRegister (AT91C_BASE_UDP, 0xFFFFFFFF);
AT91F_AIC_EnableIt (AT91C_BASE_AIC, AT91C_ID_UDP);
}

void UDP_IRQ_Handler(void)
{
AT91PS_UDP pUDP = AT91C_BASE_UDP;
AT91_REG isr = pUDP->UDP_ISR;
AT91F_DBGU_Printk ("UDP_int\n\r");
        
if (isr & AT91C_UDP_ENDBUSRES)
  {
  AT91F_UDP_InterruptClearRegister (AT91C_BASE_UDP, AT91C_UDP_ENDBUSRES);
                // reset configuration
                (&USB)->currentConfiguration = 0;
  // reset all endpoints
  pUDP->UDP_RSTEP  = (unsigned int) -1;
  pUDP->UDP_RSTEP  = 0;
  // Enable the function
  pUDP->UDP_FADDR = AT91C_UDP_FEN;
  // Configure endpoint 0
  pUDP->UDP_CSR[0] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_CTRL);
  // Enable Int endpoint 0
  AT91F_UDP_EnableIt (AT91C_BASE_UDP, AT91C_UDP_EPINT0);
  AT91F_DBGU_Printk(" -Bus reset\n\r");
  }
if (isr & AT91C_UDP_RXSUSP)
  {
  AT91F_UDP_InterruptClearRegister (AT91C_BASE_UDP, AT91C_UDP_RXSUSP);
  AT91F_DBGU_Printk(" -Suspend int\n\r");
  }
if (isr & AT91C_UDP_RXRSM)
  {
  AT91F_UDP_InterruptClearRegister (AT91C_BASE_UDP, AT91C_UDP_RXRSM);
  AT91F_DBGU_Printk(" -Resume int\n\r");
  }
if (isr & AT91C_UDP_EXTRSM)
  {
  AT91F_UDP_InterruptClearRegister (AT91C_BASE_UDP, AT91C_UDP_EXTRSM);
  AT91F_DBGU_Printk(" -External resume int\n\r");
  }
if (isr & AT91C_UDP_SOFINT)
  {
  AT91F_UDP_InterruptClearRegister (AT91C_BASE_UDP, AT91C_UDP_SOFINT);
  AT91F_DBGU_Printk(" -Start of frame int\n\r");
  }
if (isr & AT91C_UDP_WAKEUP)
  {
  AT91F_UDP_InterruptClearRegister (AT91C_BASE_UDP, AT91C_UDP_WAKEUP);
  AT91F_DBGU_Printk(" -External resume int\n\r");
  }    
if (isr & AT91C_UDP_EPINT0)
  {
  AT91F_DBGU_Printk (" -EP0 Int!\n\r");
  AT91F_UDP_EP0_Int();
  AT91F_UDP_InterruptClearRegister (AT91C_BASE_UDP, AT91C_UDP_EPINT0);
  }
if (isr & AT91C_UDP_EPINT1)
  {    
  AT91F_DBGU_Printk (" -EP1 Int!\n\r");
  AT91F_UDP_EP1_Int();
  AT91F_UDP_InterruptClearRegister (AT91C_BASE_UDP, AT91C_UDP_EPINT1);
  }
if (isr & AT91C_UDP_EPINT2)
  {    
  AT91F_DBGU_Printk (" -EP2 Int!\n\r");
  AT91F_UDP_EP2_Int();
  AT91F_UDP_InterruptClearRegister (AT91C_BASE_UDP, AT91C_UDP_EPINT2);
  }  
}

void AT91F_UDP_EP0_Int(void)
{
AT91PS_UDP pUDP = AT91C_BASE_UDP;
AT91_REG CSR0 = pUDP->UDP_CSR[0];

if (CSR0 & AT91C_UDP_TXCOMP)
  {
  AT91F_DBGU_Printk ("  -IN transaction acknoledged\n\r");
  UDP_EP_ClrFlag(AT91C_BASE_UDP, 0, AT91C_UDP_TXCOMP)
  AT91S_UDP_EP[0].Ready=1;;
  }
if (CSR0 & AT91C_UDP_RX_DATA_BK0)
  {
  AT91F_DBGU_Printk ("  -BANK0 recived packed\n\r");
  UDP_EP_ClrFlag(AT91C_BASE_UDP, 0, AT91C_UDP_RX_DATA_BK0);
  }
if (CSR0 & AT91C_UDP_RXSETUP)
  {
  AT91F_DBGU_Printk ("  -Setup packed recived\n\r");
  AT91F_USB_Enumerate(&USB);
  //UDP_EP_ClrFlag(AT91C_BASE_UDP, 0, AT91C_UDP_RXSETUP);
  }
if (CSR0 & AT91C_UDP_ISOERROR)
  {
  //AT91F_DBGU_Printk ("  -CRC error in esochronous transfer\n\r");
  AT91F_DBGU_Printk ("  -Host sends STALL\n\r");
  UDP_EP_ClrFlag(AT91C_BASE_UDP, 0, AT91C_UDP_ISOERROR);
  }
if (CSR0 & AT91C_UDP_RX_DATA_BK1)
  {
  AT91F_DBGU_Printk ("  -BANK1 recived packed\n\r");
  UDP_EP_ClrFlag(AT91C_BASE_UDP, 0, AT91C_UDP_RX_DATA_BK1);
  }
}