__ramfunc static uint AT91F_UDP_Write(AT91PS_CDC pCdc, u8 *pData, uint txlength)// Send through endpoint 2
{  
AT91PS_UDP pUDP = pCdc->pUdp;
uint cpt = 0;

//Если FIFO свободна, то производим запись, нет
if(!(pUDP->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXPKTRDY))
{
  pUDP->UDP_CSR[AT91C_EP_IN] &= ~(AT91C_UDP_TXCOMP);
  cpt = txlength;
  while (cpt--) pUDP->UDP_FDR[AT91C_EP_IN] = *pData++;
  pUDP->UDP_CSR[AT91C_EP_IN] |= AT91C_UDP_TXPKTRDY;
}

return txlength;
}

__ramfunc static uint AT91F_UDP_Write(AT91PS_CDC pCdc, u8 *pData, uint txlength)
{  
AT91PS_UDP pUDP = pCdc->pUdp;
uint cpt = 0;

if((pUDP->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXPKTRDY)==0)
{
  Udp_ep_clr_flag(pUDP, AT91C_EP_IN, AT91C_UDP_TXCOMP);
  cpt = MIN(txlength, AT91C_EP_IN_SIZE);                        // Send the first packet
  cpt = txlength;
  txlength -= cpt;
  while (cpt--) pUDP->UDP_FDR[AT91C_EP_IN] = *pData++;
  Udp_ep_set_flag(pUDP, AT91C_EP_IN, AT91C_UDP_TXPKTRDY);
  //Udp_ep_clr_flag(pUDP, AT91C_EP_IN, AT91C_UDP_TXCOMP);                
  
  while(txlength)
    {
    cpt = MIN(txlength, AT91C_EP_IN_SIZE);                    // Fill the second bank
    txlength -= cpt;
    while(cpt--)pUDP->UDP_FDR[AT91C_EP_IN] = *pData++;
    while(!(pUDP->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP))            // Wait for first bank sented
        {
        if(!AT91F_UDP_IsConfigured(pCdc))return txlength;
                }
    Udp_ep_set_flag(pUDP, AT91C_EP_IN, AT91C_UDP_TXPKTRDY);
    Udp_ep_clr_flag(pUDP, AT91C_EP_IN, AT91C_UDP_TXCOMP);
    }
}
return txlength;  
}

while(!(pUDP->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP))            // Wait for first bank sented