diff --git a/firmware/src/dfu/dfu.c b/firmware/src/dfu/dfu.c
index 0e22256..7e55ce3 100644
--- a/firmware/src/dfu/dfu.c
+++ b/firmware/src/dfu/dfu.c
@@ -102,7 +102,7 @@ static void __dfufunc udp_ep0_send_data(const char *pData, u_int32_t length)
 	u_int32_t cpt = 0, len_remain = length;
 	AT91_REG csr;
 
-	DEBUGE("send_data: %u bytes ", length);
+	DEBUGPCR("send_data: %u bytes \r\n", length);
 
 	do {
 		cpt = MIN(len_remain, 8);
@@ -123,13 +123,14 @@ static void __dfufunc udp_ep0_send_data(const char *pData, u_int32_t length)
 			/* 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 ");
+				DEBUGPCR("stopped by status out ");
 				return;
 			}
 		} while (!(csr & AT91C_UDP_TXCOMP));
 
 	} while (len_remain);
 
+	DEBUGPCR("came here..\r\n");
 	if (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) {
 		pUdp->UDP_CSR[0] &= ~(AT91C_UDP_TXCOMP);
 		while (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) ;
@@ -139,7 +140,7 @@ static void __dfufunc udp_ep0_send_data(const char *pData, u_int32_t length)
 		/* 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 ");
+		DEBUGPCR("set_txpktrdy_zlp \r\n");
 		udp_ep0_send_zlp();
 	}
 }
@@ -201,9 +202,13 @@ static void __dfufunc udp_ep0_send_zlp(void)
 {
 	AT91PS_UDP pUdp = AT91C_BASE_UDP;
 	pUdp->UDP_CSR[0] |= AT91C_UDP_TXPKTRDY;
+	DEBUGPCR("WAITING FOR...\r\n");
 	while (!(pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP)) ;
+	DEBUGPCR("SEND ZLP\r\n");
 	pUdp->UDP_CSR[0] &= ~(AT91C_UDP_TXCOMP);
+	DEBUGPCR("WAITING AGAIN...\r\n");
 	while (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) ;
+	DEBUGPCR("DONE...\r\n");
 }
 
 /* Stall the control endpoint */
diff --git a/firmware/src/os/dbgu.c b/firmware/src/os/dbgu.c
index 113208e..3d3690d 100644
--- a/firmware/src/os/dbgu.c
+++ b/firmware/src/os/dbgu.c
@@ -45,7 +45,7 @@
 #include <asm/system.h>
 #include <compile.h>
 
-//#define DEBUG_UNBUFFERED
+#define DEBUG_UNBUFFERED
 
 #define USART_SYS_LEVEL 4
 /*---------------------------- Global Variable ------------------------------*/
diff --git a/firmware/src/os/pcd_enumerate.c b/firmware/src/os/pcd_enumerate.c
index c360d37..5c74662 100644
--- a/firmware/src/os/pcd_enumerate.c
+++ b/firmware/src/os/pcd_enumerate.c
@@ -44,10 +44,10 @@
 
 #include "../config.h"
 
-//#define DEBUG_UDP_IRQ
+#define DEBUG_UDP_IRQ
 //#define DEBUG_UDP_IRQ_IN
 //#define DEBUG_UDP_IRQ_OUT
-//#define DEBUG_UDP_EP0
+#define DEBUG_UDP_EP0
 
 #ifdef DEBUG_UDP_IRQ
 #define DEBUGI(x, args ...)	DEBUGP(x, ## args)
@@ -464,6 +467,68 @@ void udp_reset(void)
 	udp_pullup_on();
 }
 
+static void udp_ep0_send_zlp_loc(void)
+{
+	AT91PS_UDP pUdp = AT91C_BASE_UDP;
+	pUdp->UDP_CSR[0] &= ~(AT91C_UDP_TXPKTRDY);
+	DEBUGPCR("WAITING FOR...\r\n");
+	while (!(pUdp->UDP_CSR[0] & AT91C_UDP_TXPKTRDY)) ;
+	DEBUGPCR("SEND ZLP\r\n");
+	pUdp->UDP_CSR[0] &= ~(AT91C_UDP_TXCOMP);
+	DEBUGPCR("WAITING AGAIN...\r\n");
+	while (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) ;
+	DEBUGPCR("DONE...\r\n");
+}
+/* Send Data through the control endpoint */
+static void udp_ep0_send_data_loc(const char *pData, u_int32_t length)
+{
+	AT91PS_UDP pUdp = AT91C_BASE_UDP;
+	u_int32_t cpt = 0, len_remain = length;
+	AT91_REG csr;
+
+	DEBUGPCR("send_data: %u bytes \r\n", 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);
+				DEBUGPCR("stopped by status out \r\n");
+				return;
+			}
+		} while (!(csr & AT91C_UDP_TXCOMP));
+
+	} while (len_remain);
+
+	DEBUGPCR("came here..\r\n");
+	if (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) {
+		pUdp->UDP_CSR[0] &= ~(AT91C_UDP_TXCOMP);
+		while (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) ;
+	}
+
+	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.  */
+		DEBUGPCR("set_txpktrdy_zlp \r\n");
+		udp_ep0_send_zlp_loc();
+	}
+}
+
 /* Handle requests on the USB Control Endpoint */
 static void udp_ep0_handler(void)
 {
@@ -527,6 +592,7 @@ static void udp_ep0_handler(void)
 		switch (desc_type) {
 		case USB_DT_DEVICE:
 			/* Return Device Descriptor */
+			DEBUGE(" OUT \r\n");
 #ifdef CONFIG_DFU
 			if (*dfu->dfu_state != DFU_STATE_appIDLE)
 				udp_ep0_send_data((const char *) 
@@ -535,8 +601,9 @@ static void udp_ep0_handler(void)
 						      wLength));
 			else
 #endif
-			udp_ep0_send_data((const char *) &dev_descriptor,
+			udp_ep0_send_data_loc((const char *) &dev_descriptor,
 					   MIN(sizeof(dev_descriptor), wLength));
+			DEBUGE(" OUT2 \r\n");
 			break;
 		case USB_DT_CONFIG:
 			/* Return Configuration Descriptor */
@@ -802,6 +869,7 @@ static void udp_ep0_handler(void)
 		goto out_stall;
 		break;
 	}
+	DEBUGE("\r\n");
 	return;
 out_stall:
 	DEBUGE("STALL!! ");
diff --git a/firmware/src/os/usb_handler.c b/firmware/src/os/usb_handler.c
index 029891a..1754805 100644
--- a/firmware/src/os/usb_handler.c
+++ b/firmware/src/os/usb_handler.c
@@ -50,7 +50,7 @@ static int usb_in(struct req_ctx *rctx)
 	usb_cmd_fn *hdlr;
 	int ret;
 
-/*	DEBUGP("usb_in(cls=%d) ", OPENPCD_CMD_CLS(poh->cmd));*/
+	DEBUGP("usb_in(cls=%d) ", OPENPCD_CMD_CLS(poh->cmd));
 
 	if (rctx->tot_len < sizeof(*poh))
 		return -EINVAL;
@@ -71,7 +71,7 @@ static int usb_in(struct req_ctx *rctx)
 		udp_refill_ep(2);
 	}
 
-/*	DEBUGPCR("");*/
+	DEBUGPCR("");
 	return (ret & USB_RET_ERR) ? 1 : 0;
 }
 
@@ -92,8 +92,8 @@ void usb_in_process(void)
 
 	while (rctx = req_ctx_find_get(0, RCTX_STATE_UDP_RCV_DONE,
 				       RCTX_STATE_MAIN_PROCESSING)) {
-/*		DEBUGPCRF("found used ctx %u: len=%u", 
-			  req_ctx_num(rctx), rctx->tot_len);*/
+		DEBUGPCRF("found used ctx %u: len=%u", 
+			  req_ctx_num(rctx), rctx->tot_len);
 		usb_in(rctx);
 	}
 	udp_unthrottle();
diff --git a/firmware/src/simtrace/main_simtrace.c b/firmware/src/simtrace/main_simtrace.c
index f919690..1ce4ea6 100644
--- a/firmware/src/simtrace/main_simtrace.c
+++ b/firmware/src/simtrace/main_simtrace.c
@@ -91,15 +91,15 @@ void _init_func(void)
 	tc_etu_init();
 	sim_switch_init();
 
-	usbtest_init();
+	//usbtest_init();
 
 	/* high-level protocol */
 	//opicc_usbapi_init();
 	led_switch(1, 0);
 	led_switch(2, 1);
 
-	iso_uart_rx_mode();
-	simtrace_set_mode(SIMTRACE_MD_SNIFFER);
+	//iso_uart_rx_mode();
+	//simtrace_set_mode(SIMTRACE_MD_SNIFFER);
 }