static void AT91F_USB_SendData(AT91PS_UDP pUdp, const char *pData, uint length)
{
	uint cpt = 0;
	AT91_REG csr;

	//serial_printf("EP0 SendData()\n");

	do {
		cpt = MIN(length, 8);
		length -= cpt;

		while (cpt--)
			pUdp->UDP_FDR[0] = *pData++;

		if (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) {
			Udp_ep_clr_flag(pUdp, 0, AT91C_UDP_TXCOMP);
		}

		Udp_ep_set_flag(pUdp, 0, AT91C_UDP_TXPKTRDY);
		do {
			csr = pUdp->UDP_CSR[0];

			// Data IN stage has been stopped by a status OUT
			if (csr & AT91C_UDP_RX_DATA_BK0) {
				//serial_printf("EP0 SendData stopped by status OUT\n");
				Udp_ep_clr_flag(pUdp, 0, AT91C_UDP_RX_DATA_BK0);
				return;
			}
		} while ( !(csr & AT91C_UDP_TXCOMP) );

	} while (length);

	if (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) {
		Udp_ep_clr_flag(pUdp, 0, AT91C_UDP_TXCOMP);
	}

	//serial_printf("EP0 SendData complete\n");
}