diff --git a/firmware/src/os/pcd_enumerate.c b/firmware/src/os/pcd_enumerate.c
index c360d37..4825d0e 100644
--- a/firmware/src/os/pcd_enumerate.c
+++ b/firmware/src/os/pcd_enumerate.c
@@ -78,7 +78,7 @@
 #ifdef CONFIG_DFU
 static const struct dfuapi *dfu = DFU_API_LOCATION;
 #define udp_init		dfu->udp_init
-#define udp_ep0_send_data	dfu->ep0_send_data
+#define udp_ep0_send_data	udp_ep0_send_data_local
 #define udp_ep0_send_zlp	dfu->ep0_send_zlp
 #define udp_ep0_send_stall	dfu->ep0_send_stall
 #else
@@ -107,6 +107,56 @@ static const struct epstate epstate[] = {
 		  .state_pending = RCTX_STATE_UDP_EP3_PENDING },
 };
 
+static void udp_ep0_send_data_local(const char *pData, u_int32_t length)
+{
+	AT91PS_UDP pUdp = AT91C_BASE_UDP;
+	u_int32_t cpt = 0, len_remain = length;
+	AT91_REG csr;
+
+	DEBUGE("send_data: %u bytes ", length);
+
+	do {
+		cpt = MIN(len_remain, 8);
+		len_remain -= cpt;
+
+		while (cpt--)
+			pUdp->UDP_FDR[0] = *pData++;
+
+		if (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) {
+			pUdp->UDP_CSR[0] &= ~(AT91C_UDP_TXCOMP);
+			while (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) ;
+		}
+
+		pUdp->UDP_CSR[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) {
+				pUdp->UDP_CSR[0] &= ~(AT91C_UDP_RX_DATA_BK0);
+				DEBUGE("stopped by status out ");
+				return;
+			}
+		} while (!(csr & AT91C_UDP_TXCOMP));
+
+	} while (len_remain);
+
+	if (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) {
+		pUdp->UDP_CSR[0] &= ~(AT91C_UDP_TXCOMP);
+		while (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) ;
+	}
+
+#if 0
+	if ((length % 8) == 0) {
+		/* if the length is a multiple of the EP size, we need
+		 * to send another ZLP (zero-length packet) to tell the
+		 * host the transfer has completed.  */
+		DEBUGE("set_txpktrdy_zlp ");
+		udp_ep0_send_zlp();
+	}
+#endif
+}
+
 static void reset_ep(unsigned int ep)
 {
 	AT91PS_UDP pUDP = upcd.pUdp;