diff --git a/wifiHD/src/SOFTWARE_FRAMEWORK/SERVICES/LWIP/lwip-1.3.2/src/core/tcp.c b/wifiHD/src/SOFTWARE_FRAMEWORK/SERVICES/LWIP/lwip-1.3.2/src/core/tcp.c
index 39ca95c6d..c588d21df 100644
--- a/wifiHD/src/SOFTWARE_FRAMEWORK/SERVICES/LWIP/lwip-1.3.2/src/core/tcp.c
+++ b/wifiHD/src/SOFTWARE_FRAMEWORK/SERVICES/LWIP/lwip-1.3.2/src/core/tcp.c
@@ -1420,22 +1420,22 @@ tcp_debug_print_pcbs(void)
   struct tcp_pcb *pcb;
   LWIP_DEBUGF(TCP_DEBUG, ("Active PCB states:\n"));
   for(pcb = tcp_active_pcbs; pcb != NULL; pcb = pcb->next) {
-    LWIP_DEBUGF(TCP_DEBUG, ("Local port %"U16_F", foreign port %"U16_F" snd_nxt %"U32_F" rcv_nxt %"U32_F" ",
-                       pcb->local_port, pcb->remote_port,
+    LWIP_DEBUGF(TCP_DEBUG, ("[%p]Local port %"U16_F", foreign port %"U16_F" snd_nxt %"U32_F" rcv_nxt %"U32_F" ",
+                       pcb, pcb->local_port, pcb->remote_port,
                        pcb->snd_nxt, pcb->rcv_nxt));
     tcp_debug_print_state(pcb->state);
   }    
   LWIP_DEBUGF(TCP_DEBUG, ("Listen PCB states:\n"));
   for(pcb = (struct tcp_pcb *)tcp_listen_pcbs.pcbs; pcb != NULL; pcb = pcb->next) {
-    LWIP_DEBUGF(TCP_DEBUG, ("Local port %"U16_F", foreign port %"U16_F" snd_nxt %"U32_F" rcv_nxt %"U32_F" ",
-                       pcb->local_port, pcb->remote_port,
+    LWIP_DEBUGF(TCP_DEBUG, ("[%p]Local port %"U16_F", foreign port %"U16_F" snd_nxt %"U32_F" rcv_nxt %"U32_F" ",
+                       pcb, pcb->local_port, pcb->remote_port,
                        pcb->snd_nxt, pcb->rcv_nxt));
     tcp_debug_print_state(pcb->state);
   }    
   LWIP_DEBUGF(TCP_DEBUG, ("TIME-WAIT PCB states:\n"));
   for(pcb = tcp_tw_pcbs; pcb != NULL; pcb = pcb->next) {
-    LWIP_DEBUGF(TCP_DEBUG, ("Local port %"U16_F", foreign port %"U16_F" snd_nxt %"U32_F" rcv_nxt %"U32_F" ",
-                       pcb->local_port, pcb->remote_port,
+    LWIP_DEBUGF(TCP_DEBUG, ("[%p]Local port %"U16_F", foreign port %"U16_F" snd_nxt %"U32_F" rcv_nxt %"U32_F" ",
+                       pcb, pcb->local_port, pcb->remote_port,
                        pcb->snd_nxt, pcb->rcv_nxt));
     tcp_debug_print_state(pcb->state);
   }    
diff --git a/wifiHD/src/ard_spi.c b/wifiHD/src/ard_spi.c
index 40b0d76b2..57f48f84a 100644
--- a/wifiHD/src/ard_spi.c
+++ b/wifiHD/src/ard_spi.c
@@ -20,6 +20,8 @@
 #include "debug.h"
 #include "delay.h"
 #include "eic.h"
+#include "timer.h"
+
 
 
 /*! \name USART Settings
@@ -69,6 +71,8 @@
 
 uint8_t counter = 0;
 
+extern void tcp_debug_print_pcbs(void);
+
 static char buf[CMD_MAX_LEN];
 static char reply[REPLY_MAX_LEN];
 static uint16_t count = 0;
@@ -167,6 +171,64 @@ void* getTTCP(uint8_t sock)
 	return NULL;
 }
 
+int getSock(void * _ttcp)
+{
+	int i = 0;
+	for (; i<MAX_SOCK_NUM; i++)
+	{
+		if (_ttcp == mapSockTCP[i])
+			return i;
+	}
+	return -1;
+}
+
+void clearMapSockTcp(uint8_t sock)
+{
+	if (sock < MAX_SOCK_NUM)
+		mapSockTCP[sock] = NULL;
+}
+
+/**
+ * Calculate bitrate based on number of bytes transmitted and elapsed time
+ */
+static void ard_tcp_print_stats(struct ttcp *ttcp) {
+	uint32_t ms = timer_get_ms() - ttcp->start_time;
+	uint32_t bytes = ttcp->mode == TTCP_MODE_TRANSMIT ? ttcp->nbuf
+			* ttcp->buflen : ttcp->recved;
+
+	if (ttcp->verbose)
+		printk("\n");
+
+	printk("TTCP [%p]: %d bytes processed, %d.%d KB/s (%s/%s)\n", ttcp, bytes,
+			bytes / ms, bytes % ms, ttcp->udp ? "udp" : "tcp", ttcp->mode
+					== TTCP_MODE_TRANSMIT ? "tx" : "rx");
+}
+
+
+void showTTCPstatus()
+{
+	int i = 0;
+	for (; i<MAX_SOCK_NUM; i++)
+	{
+		void* p = mapSockTCP[i];
+		if (p)
+		{
+			ttcp_t* _ttcp = (ttcp_t* )p;
+			printk("Socket n.:%d addr:0x%x port:%d\n", i, _ttcp->addr, _ttcp->port);
+			if (_ttcp->tpcb){
+				printk("[tpcp]-Status:%d\n", _ttcp->tpcb->state);
+			}
+			if (_ttcp->lpcb){
+				printk("[tlcp]-Status:%d\n", _ttcp->lpcb->state);
+			}
+			ard_tcp_print_stats(_ttcp);
+		}
+	}
+
+	tcp_debug_print_pcbs();
+}
+
+
 inline spi_status_t myspi_read(volatile avr32_spi_t *spi, unsigned short *data)
 {
 	if (spi->sr & AVR32_SPI_SR_RDRF_MASK)
@@ -323,7 +385,7 @@ int set_net_cmd_cb(int numParam, char* buf, void* ctx) {
 		memcpy(ssid.ssid, &param->param, param->paramLen);
 		ssid.len = param->paramLen;
 		ssid.ssid[ssid.len] = 0;
-		INFO("SSID:%s\n", ssid.ssid);
+		INFO_SPI("SSID:%s\n", ssid.ssid);
 		//dump(ssid.ssid, ssid.len);
 		err = wl_cm_set_network(&ssid, NULL);
 		if (err != 1)
@@ -345,7 +407,7 @@ int set_key_cmd_cb(int numParam, char* buf, void* ctx) {
 	wl_err_t err = WL_SUCCESS;
 	tParam* params = (tParam*) buf;
 
-    INFO("%s params=%d\n", __FUNCTION__, numParam);
+    INFO_SPI("%s params=%d\n", __FUNCTION__, numParam);
 
     // SSID
     memset(&ssid, 0, sizeof ssid);
@@ -353,7 +415,7 @@ int set_key_cmd_cb(int numParam, char* buf, void* ctx) {
 	if (params->paramLen < WL_SSID_MAX_LENGTH) {
 		memcpy(ssid.ssid, &params->param, params->paramLen);
 		ssid.len = params->paramLen;
-		INFO("%s\n", ssid.ssid);
+		INFO_SPI("%s\n", ssid.ssid);
 	} else {
 		//printk("SSID len out of range");
 		RETURN_ERR(WL_FAILURE)
@@ -394,7 +456,7 @@ int set_key_cmd_cb(int numParam, char* buf, void* ctx) {
     //Connect
     err = wl_cm_set_network(&ssid, NULL);
     if (err != 1)
-        printk("err=%d\n", err);
+        WARN("err=%d\n", err);
     RETURN_ERR(err)
 }
 
@@ -404,7 +466,7 @@ int set_passphrase_cmd_cb(int numParam, char* buf, void* ctx) {
 	wl_err_t err = WL_SUCCESS;
 	tParam* params = (tParam*) buf;
 
-    INFO("%s params=%d\n", __FUNCTION__, numParam);
+    INFO_SPI("%s params=%d\n", __FUNCTION__, numParam);
 
     memset(&net, 0, sizeof net);
     memset(net.bssid.octet, 0xFF, sizeof net.bssid.octet);
@@ -415,7 +477,7 @@ int set_passphrase_cmd_cb(int numParam, char* buf, void* ctx) {
 	if (params->paramLen < WL_SSID_MAX_LENGTH) {
 		memcpy(net.ssid.ssid, &params->param, params->paramLen);
 		net.ssid.len = params->paramLen;
-		INFO("%s %d\n", net.ssid.ssid, net.ssid.len);
+		INFO_SPI("%s %d\n", net.ssid.ssid, net.ssid.len);
 	} else {
 		//printk("SSID len out of range");
 		RETURN_ERR(WL_FAILURE)
@@ -425,7 +487,7 @@ int set_passphrase_cmd_cb(int numParam, char* buf, void* ctx) {
    
     strncpy(pass, (const char*)&params->param, params->paramLen);
     pass[(uint8_t)params->paramLen]='\0';
-    INFO("Pass: %s %d\n", pass, params->paramLen);
+    INFO_SPI("Pass: %s %d\n", pass, params->paramLen);
 
     if (wl_set_passphrase(&net, 
                           pass, 
@@ -475,7 +537,7 @@ void set_result_cmd(int err)
     	ERROR_LED_ON();
         break;
     }
-    printk("%s %d\n", __FUNCTION__, result);
+    INFO_SPI("%s %d\n", __FUNCTION__, result);
 }
 
 
@@ -506,12 +568,12 @@ int start_server_tcp_cmd_cb(int numParam, char* buf, void* ctx) {
         if (sock >= MAX_SOCK_NUM)
         	return WIFI_SPI_ERR;
 
-        INFO("Start Server [%d, %d]\n", port, sock);
+        INFO_SPI("Start Server [%d, %d]\n", port, sock);
         if (ard_tcp_start(addr, port, NULL, NULL, mode, nbuf, buflen, udp, verbose, sock, &_ttcp) == 0)
         {
         	if (sock < MAX_SOCK_NUM)
         		mapSockTCP[sock]=_ttcp;
-        	INFO("Map [%d, %p]\n", sock, _ttcp);
+        	INFO_SPI("Map [%d, %p]\n", sock, _ttcp);
             err = WL_SUCCESS;
         }
     }
@@ -527,7 +589,7 @@ int start_client_tcp_cmd_cb(int numParam, char* buf, void* ctx) {
     	GET_PARAM_NEXT(INT, params, port);
      	GET_PARAM_NEXT(BYTE, params, sock);
 
-        INFO("Addr:0x%x, port:%d, sock:%d\n", _addr, port, sock);
+        INFO_SPI("Addr:0x%x, port:%d, sock:%d\n", _addr, port, sock);
 
         uint16_t buflen = 1024;
         uint16_t nbuf = 1024;
@@ -544,13 +606,15 @@ int start_client_tcp_cmd_cb(int numParam, char* buf, void* ctx) {
         if (sock >= MAX_SOCK_NUM)
         	return WIFI_SPI_ERR;
 
-        INFO("Start Client [0x%x, %d, %d]\n", addr, port, sock);
+        INFO_SPI("Start Client [0x%x, %d, %d]\n", addr, port, sock);
         if (ard_tcp_start((struct ip_addr)addr, port, NULL, NULL, mode, nbuf, buflen, udp, verbose, sock, &_ttcp) == 0)
         {
         	if (sock < MAX_SOCK_NUM)
         		mapSockTCP[sock]=_ttcp;
-        	INFO("Map [%d, %p]\n", sock, _ttcp);
+        	INFO_SPI("Map [%d, %p]\n", sock, _ttcp);
             err = WL_SUCCESS;
+        }else{
+        	mapSockTCP[sock]=0;
         }
     }
     return (err==WL_SUCCESS) ? WIFI_SPI_ACK : WIFI_SPI_ERR;
@@ -565,7 +629,7 @@ int stop_client_tcp_cmd_cb(int numParam, char* buf, void* ctx) {
     {
      	GET_PARAM_NEXT(BYTE, params, sock);
 
-        INFO("Stop client sock:%d\n", sock);
+        INFO_SPI("Stop client sock:%d\n", sock);
 
         if (sock < MAX_SOCK_NUM)
         {
@@ -586,6 +650,7 @@ int send_data_tcp_cmd_cb(int numParam, char* buf, void* ctx) {
     {
         GET_DATA_BYTE(sock, buf+2);
         GET_DATA_INT(len, buf+3);
+        //printk("tcp:%p buf:%p len:%d\n", getTTCP(sock), (uint8_t*)(buf+5), len);
         err = sendTcpData(getTTCP(sock), (uint8_t*)(buf+5), len);
     }
     DATA_LED_OFF();
@@ -620,7 +685,7 @@ cmd_spi_state_t get_reply_cb(char* recv, char* reply, void* ctx, uint16_t* count
 
     END_HEADER_REPLY(reply, 5, *count);
 
-    //INFO("result:%d\n", result);
+    //INFO_SPI("result:%d\n", result);
 	return SPI_CMD_DONE;
 }
 
@@ -736,7 +801,7 @@ cmd_spi_state_t get_reply_scan_networks_cb(char* recv, char* reply, void* ctx, u
 
 	struct wl_network_t network_list[WL_NETWORKS_LIST_MAXNUM];
 
-	INFO("netif:0x%x\n", ard_netif);
+	INFO_SPI("netif:0x%x\n", ard_netif);
 	CHECK_ARD_NETIF(recv, reply, count);
 
     int network_cnt = 0;
@@ -866,7 +931,7 @@ cmd_spi_state_t ack_data_sent_reply_cb(char* recv, char* reply, void* ctx, uint1
 cmd_spi_state_t data_sent_tcp_cmd_cb(char* recv, char* reply, void* ctx, uint16_t* count) {
 
 	CHECK_ARD_NETIF(recv, reply, count);
-SIGN2_DN();
+	SIGN2_DN();
 	CREATE_HEADER_REPLY(reply, recv, PARAM_NUMS_1);
 	uint8_t dataSent = 0;
     if ((recv[3]==1)&&(recv[4]>=0)&&(recv[4]<MAX_SOCK_NUM))
@@ -946,7 +1011,7 @@ int sendReply(int cmdIdx, char* recv, char* reply, void* resultCmd)
     BUSY_FOR_SPI();
 
     //unsigned char status = spi_getStatus(ARD_SPI);
-    //INFO("Status after write: 0x%x\n",status);
+    //INFO_SPI("Status after write: 0x%x\n",status);
 
     replyCount = _count;
     return _result;
@@ -1044,7 +1109,7 @@ int call_reply_cb(char* recv, char* reply) {
 	// Command not found
 	if (i==ARRAY_SIZE(cmd_spi_list))
 	{
-		printk("Unknown cmd 0x%x\n", cmdId);
+		WARN("Unknown cmd 0x%x\n", cmdId);
 		dump(recv, count);
 		return REPLY_ERR_CMD;
 	}
@@ -1135,7 +1200,7 @@ bool checkMsgFormat(uint8_t* _recv, int len, int* offset)
 			paramLenTot = checkMsgParam16(recv);
 		}
 
-		//INFO("cmd:0x%x TotLen:%d\n", spiMsg->tcmd, paramLenTot);
+		//INFO_SPI("cmd:0x%x TotLen:%d\n", spiMsg->tcmd, paramLenTot);
 		char* p = (char*)recv + paramLenTot + sizeof(tSpiHdr);
 		if (*p == END_CMD)
 		{
@@ -1171,7 +1236,7 @@ void spi_poll(struct netif* netif) {
 			if (err != REPLY_NO_ERR)
 			{
 				//LED_On(LED1);
-				INFO("[E(0x%x):%d spiStatus:%d]\n", statSpi.lastCmd, err, statSpi.status);
+				//INFO_SPI("[E(0x%x):%d spiStatus:%d]\n", statSpi.lastCmd, err, statSpi.status);
 				PRINT_STATS_SPI();
 				DUMP(buf, count);
 				DUMP(reply, replyCount);
@@ -1288,7 +1353,7 @@ static void spi_int_handler(void)
 #ifdef _SPI_STATS_
         	//TODO verify why at the end of cmd cycle RDF bit is high without any data recv.
         	if (statSpi.lastError != SPI_ERROR_TIMEOUT)
-        		INFO("[E(0x%x):%d spiStatus:%d]\n", statSpi.lastError, err, statSpi.status);
+        		INFO_SPI("[E(0x%x):%d spiStatus:%d]\n", statSpi.lastError, err, statSpi.status);
 #endif
         }else{
         	BUSY_FOR_SPI();
@@ -1376,6 +1441,9 @@ int initSpi()
 	    {AVR32_SPI0_MOSI_0_0_PIN, AVR32_SPI0_MOSI_0_0_FUNCTION},  \
 	    {AVR32_SPI0_NPCS_0_0_PIN, AVR32_SPI0_NPCS_0_0_FUNCTION},  \
 	  };
+
+	INFO_INIT("SPI init...\n");
+
 	/* Init PIO */
 	gpio_enable_module(spi_piomap, ARRAY_SIZE(spi_piomap));
 
@@ -1393,7 +1461,7 @@ int initSpi()
 	/* Initialize as slave; bits, spi_mode */
 	if (spi_initSlave(spi, spiOptions.bits, spiOptions.spi_mode) != SPI_OK)
 	{
-		INFO("SPI initialization failed!");
+		INFO_SPI("SPI initialization failed!");
 		return 1;
 	}
 
@@ -1418,6 +1486,8 @@ int initSpi()
 #endif
 	init_spi_cmds();
 
+	memset(_receiveBuffer, 0, _BUFFERSIZE);
+
 	return 0;
 }
 
diff --git a/wifiHD/src/ard_spi.h b/wifiHD/src/ard_spi.h
index 36538ade4..ba3a2bbe5 100644
--- a/wifiHD/src/ard_spi.h
+++ b/wifiHD/src/ard_spi.h
@@ -67,4 +67,10 @@ cmd_state_t cmd_statSpi(int argc, char* argv[], void* ctx);
 
 cmd_state_t cmd_resetStatSpi(int argc, char* argv[], void* ctx);
 
+void showTTCPstatus();
+
+int getSock(void * _ttcp);
+
+void clearMapSockTcp(uint8_t sock);
+
 #endif /* ARD_SPI_H_ */
diff --git a/wifiHD/src/ard_tcp.c b/wifiHD/src/ard_tcp.c
index f723ec562..832c706ec 100644
--- a/wifiHD/src/ard_tcp.c
+++ b/wifiHD/src/ard_tcp.c
@@ -5,7 +5,7 @@
  *      Author: mlf by Metodo2 srl
  */
 
-#define _APP_DEBUG_
+//#define _APP_DEBUG_
 #include "lwip/opt.h"
 
 #include "lwip/mem.h"
@@ -20,6 +20,7 @@
 #include "lwip/udp.h"
 
 #include "ard_tcp.h"
+#include "ard_spi.h"
 #include "timer.h"
 #include "util.h"
 
@@ -27,79 +28,29 @@
 #include "ard_utils.h"
 #include "debug.h"
 
-#define TTCP_MODE_TRANSMIT          0
-#define TTCP_MODE_RECEIVE           1
-
-struct ttcp {
-
-	/* options */
-	struct ip_addr addr; /* host */
-	uint16_t port; /* -p */
-	uint16_t nbuf; /* -n */
-	int mode; /* -t */
-	int verbose; /* -v */
-	int udp; /* -u */
-	uint8_t sock;
-	uint8_t buff_sent;
-
-	/* common */
-	uint16_t print_cnt;
-	uint32_t start_time;
-	uint32_t left;
-	uint32_t recved;
-	ard_tcp_done_cb_t* done_cb;
-	void* opaque;
-	uint32_t buflen; /* -l */
-	uint32_t tid;
-
-	/* TCP specific */
-	struct tcp_pcb* tpcb;
-	struct tcp_pcb* lpcb;
-	char* payload;
-
-	/* UDP specific */
-	int udp_started;
-	uint16_t udp_end_marker_left;
-	struct udp_pcb* upcb;
-};
-
 unsigned int startTime = 0;
 
-/**
- * Calculate bitrate based on number of bytes transmitted and elapsed time
- */
-static void ard_tcp_print_stats(struct ttcp *ttcp) {
-	uint32_t ms = timer_get_ms() - ttcp->start_time;
-	uint32_t bytes = ttcp->mode == TTCP_MODE_TRANSMIT ? ttcp->nbuf
-			* ttcp->buflen : ttcp->recved;
-
-	if (ttcp->verbose)
-		printk("\n");
-
-	printk("TTCP [%p]: %d bytes processed, %d.%d KB/s (%s/%s)\n", ttcp, bytes,
-			bytes / ms, bytes % ms, ttcp->udp ? "udp" : "tcp", ttcp->mode
-					== TTCP_MODE_TRANSMIT ? "tx" : "rx");
-}
-
 /**
  * Clean up and free the ttcp structure
  */
 static void ard_tcp_destroy(struct ttcp* ttcp) {
 	err_t err = ERR_OK;
+	DUMP_TCP_STATE(ttcp);
+
 	if (ttcp->tpcb) {
 		tcp_arg(ttcp->tpcb, NULL);
 		tcp_sent(ttcp->tpcb, NULL);
 		tcp_recv(ttcp->tpcb, NULL);
 		tcp_err(ttcp->tpcb, NULL);
 		err = tcp_close(ttcp->tpcb);
-		printk("Closing tpcb: state:0x%x err:%d\n", ttcp->tpcb->state, err);
+		INFO_TCP("Closing tpcb: state:0x%x err:%d\n", ttcp->tpcb->state, err);
 	}
 
 	if (ttcp->lpcb) {
 		tcp_arg(ttcp->lpcb, NULL);
 		tcp_accept(ttcp->lpcb, NULL);
 		tcp_close(ttcp->lpcb);
-		printk("Closing lpcb: state:0x%x err:%d\n", ttcp->lpcb->state, err);
+		INFO_TCP("Closing lpcb: state:0x%x err:%d\n", ttcp->lpcb->state, err);
 	}
 
 	if (ttcp->upcb) {
@@ -113,12 +64,44 @@ static void ard_tcp_destroy(struct ttcp* ttcp) {
 	free(ttcp);
 }
 
+/**
+ * Clean up and free the ttcp structure
+ */
+static void ard_tcp_abort(struct ttcp* ttcp) {
+
+	INFO_TCP("Abort ttcb:%p tpcb:%p lpcb:%p\n", ttcp, ttcp->tpcb, ttcp->lpcb);
+	if (ttcp->tpcb) {
+		tcp_arg(ttcp->tpcb, NULL);
+		tcp_sent(ttcp->tpcb, NULL);
+		tcp_recv(ttcp->tpcb, NULL);
+		tcp_err(ttcp->tpcb, NULL);
+		tcp_abort(ttcp->tpcb);
+	}
+
+	if (ttcp->lpcb) {
+		tcp_arg(ttcp->lpcb, NULL);
+		tcp_accept(ttcp->lpcb, NULL);
+		tcp_abort(ttcp->lpcb);
+	}
+
+	if (ttcp->upcb) {
+		udp_disconnect(ttcp->upcb);
+		udp_remove(ttcp->upcb);
+	}
+
+	if (ttcp->payload)
+		free(ttcp->payload);
+
+	free(ttcp);
+}
+
+
 /**
  * Invoked when transfer is done or aborted (non-zero result).
  */
 static void ard_tcp_done(struct ttcp* ttcp, int result) {
-	if (result == 0)
-		ard_tcp_print_stats(ttcp);
+//	if (result == 0)
+//		ard_tcp_print_stats(ttcp);
 
 	if (ttcp->done_cb)
 		ttcp->done_cb(ttcp->opaque, result);
@@ -156,11 +139,12 @@ static void tcp_send_data(struct ttcp *ttcp) {
 			len /= 2;
 	} while (err == ERR_MEM && len > 1);
 
-	tcp_output(ttcp->tpcb);
-	if (err == ERR_OK)
+	if (err == ERR_OK){
+		tcp_output(ttcp->tpcb);
 		ttcp->left -= len;
+	}
 	else
-		printk("TTCP [%p]: tcp_write failed\n", ttcp);
+		WARN("TTCP [%p]: tcp_write failed\n", ttcp);
 	//
 	//        ttcp->tid = timer_sched_timeout_cb(0, TIMEOUT_ONESHOT,
 	//                                           tcp_timeout_cb, ttcp);
@@ -245,6 +229,9 @@ static void atcp_conn_err_cb(void *arg, err_t err) {
 	printk("TTCP [%p]: connection error:0x%x\n", ttcp, err);
 
 	ttcp->tpcb = NULL; /* free'd by lwip upon return */
+	int sock = getSock(ttcp);
+	if (sock)
+		clearMapSockTcp(sock);
 	ard_tcp_done(ttcp, err);
 }
 
@@ -256,7 +243,7 @@ static err_t atcp_recv_cb(void *arg, struct tcp_pcb *pcb, struct pbuf *p,
 	struct ttcp* ttcp = arg;
 	/* p will be NULL when remote end is done */
 	if (p == NULL) {
-		ard_tcp_print_stats(ttcp);
+		INFO_TCP("atcp_recv_cb p=NULL\n");
 		ard_tcp_done(ttcp, 0);
 		return ERR_OK;
 	}
@@ -265,7 +252,7 @@ static err_t atcp_recv_cb(void *arg, struct tcp_pcb *pcb, struct pbuf *p,
 	ttcp->recved += p->tot_len;
 
 	if (ttcp->verbose) {
-		INFO("Recv:%d\n",p->tot_len);
+		INFO_TCP("Recv:%d\n",p->tot_len);
 		DUMP(p->payload, p->tot_len);
 		ttcp->print_cnt++;
 	}
@@ -297,8 +284,8 @@ static err_t atcp_accept_cb(void *arg, struct tcp_pcb *newpcb, err_t err) {
 	tcp_err(ttcp->tpcb, atcp_conn_err_cb);
 	tcp_poll(ttcp->tpcb, atcp_poll, 4);
 
-	printk("ARD TCP [%p]: accept new [%p]\n", ttcp, newpcb);
-	INFO("local:%d remote:%d state:%d\n", newpcb->local_port, newpcb->remote_port, newpcb->state);
+	INFO_TCP("ARD TCP [%p]: accept new [%p]\n", ttcp, newpcb);
+	INFO_TCP("local:%d remote:%d state:%d\n", newpcb->local_port, newpcb->remote_port, newpcb->state);
 	ttcp->start_time = timer_get_ms();
 	return ERR_OK;
 }
@@ -310,13 +297,13 @@ static int atcp_start(struct ttcp* ttcp) {
 	err_t err = ERR_OK;
 	ttcp->tpcb = tcp_new();
 	if (ttcp->tpcb == NULL) {
-		printk("TTCP [%p]: could not allocate pcb\n", ttcp);
+		WARN("TTCP [%p]: could not allocate pcb\n", ttcp);
 		return -1;
 	}
 
 	ttcp->payload = malloc(ttcp->buflen);
 	if (ttcp->payload == NULL) {
-		printk("TTCP [%p]: could not allocate payload\n", ttcp);
+		WARN("TTCP [%p]: could not allocate payload\n", ttcp);
 		return -1;
 	}
 
@@ -327,26 +314,33 @@ static int atcp_start(struct ttcp* ttcp) {
 		tcp_recv(ttcp->tpcb, atcp_recv_cb);
 		if (tcp_connect(ttcp->tpcb, &ttcp->addr, ttcp->port, tcp_connect_cb)
 				!= ERR_OK) {
-			printk("TTCP [%p]: tcp connect failed\n", ttcp);
+			WARN("TTCP [%p]: tcp connect failed\n", ttcp);
 			atcp_conn_err_cb(ttcp, err);
 			return -1;
 		}
 
 	} else {
+		INFO_TCP("BEFORE BIND ttcp:%p lpcb:%p pcb:%p\n", ttcp, ttcp->lpcb, ttcp->tpcb);
+		INFO_TCP("[tpcb]-local:%d remote:%d state:%d\n", ttcp->tpcb->local_port,
+				ttcp->tpcb->remote_port, ttcp->tpcb->state);
+
 		err = tcp_bind(ttcp->tpcb, IP_ADDR_ANY, ttcp->port);
 		if (err != ERR_OK){
-			printk("TTCP [%p]: bind failed err=%d\n", ttcp, err);
+			WARN("TTCP [%p]: bind failed err=%d\n", ttcp, err);
 			return -1;
 		}
 
 		ttcp->lpcb = tcp_listen(ttcp->tpcb);
 		if (ttcp->lpcb == NULL) {
-			printk("TTCP [%p]: listen failed\n", ttcp);
+			WARN("TTCP [%p]: listen failed\n", ttcp);
 			return -1;
 		}
-		printk("ttcp:%p lpcb:%p pcb:%p\n", ttcp, ttcp->lpcb, ttcp->tpcb);
-		printk("local:%d remote:%d state:%d\n", ttcp->lpcb->local_port,
-				ttcp->lpcb->remote_port, ttcp->lpcb->state);
+		if (ttcp->lpcb == ttcp->tpcb ) {
+			WARN("TTCP [%p]: listen failed tpcb [%p] in listen mode\n", ttcp, ttcp->tpcb);
+			return -1;
+		}
+
+		DUMP_TCP_STATE(ttcp);
 		tcp_accept(ttcp->lpcb, atcp_accept_cb);
 	}
 
@@ -368,12 +362,12 @@ static void udp_timeout_cb(void *ctx) {
 static int udp_send_bytes(struct ttcp* ttcp, uint32_t len) {
 	struct pbuf* p = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM);
 	if (p == NULL) {
-		printk("TTCP [%p]: could not allocate pbuf\n", ttcp);
+		WARN("TTCP [%p]: could not allocate pbuf\n", ttcp);
 		return -1;
 	}
 
 	if (udp_send(ttcp->upcb, p) != ERR_OK) {
-		printk("TTCP [%p]: udp_send() failed\n", ttcp);
+		WARN("TTCP [%p]: udp_send() failed\n", ttcp);
 		pbuf_free(p);
 		return -1;
 	}
@@ -493,23 +487,23 @@ int ard_tcp_start(struct ip_addr addr, uint16_t port, void *opaque,
 	int status;
 
 	if (mode != TTCP_MODE_TRANSMIT && mode != TTCP_MODE_RECEIVE) {
-		printk("TTCP [-]: invalid mode\n");
+		WARN("TTCP [-]: invalid mode\n");
 		return -1;
 	}
 
 	if (nbuf == 0) {
-		printk("TTCP [-]: invalid nbuf\n");
+		WARN("TTCP [-]: invalid nbuf\n");
 		return -1;
 	}
 
 	if (buflen == 0) {
-		printk("TTCP [-]: invalid buflen\n");
+		WARN("TTCP [-]: invalid buflen\n");
 		return -1;
 	}
 
 	ttcp = calloc(1, sizeof(struct ttcp));
 	if (ttcp == NULL) {
-		printk("TTCP [-]: could not allocate memory for ttcp\n");
+		WARN("TTCP [-]: could not allocate memory for ttcp\n");
 		return -1;
 	}
 
@@ -530,10 +524,10 @@ int ard_tcp_start(struct ip_addr addr, uint16_t port, void *opaque,
 		status = atcp_start(ttcp);
 
 	if (status) {
-		WARN("Start server FAILED!");
+		WARN("Start server FAILED!\n");
 		goto fail;
 	}
-	printk("TTCP [%p-%p]: nbuf=%d, buflen=%d, port=%d (%s/%s)\n", ttcp,
+	INFO_TCP("TTCP [%p-%p]: nbuf=%d, buflen=%d, port=%d (%s/%s)\n", ttcp,
 			ttcp->tpcb, ttcp->nbuf, ttcp->buflen, ttcp->port, ttcp->udp ? "udp"
 					: "tcp", ttcp->mode == TTCP_MODE_TRANSMIT ? "tx" : "rx");
 
@@ -543,7 +537,7 @@ int ard_tcp_start(struct ip_addr addr, uint16_t port, void *opaque,
 
 	return 0;
 
-	fail: ard_tcp_destroy(ttcp);
+	fail: ard_tcp_abort(ttcp);
 	return -1;
 }
 
@@ -553,34 +547,43 @@ static void close_conn(struct ttcp *_ttcp) {
 
 void ard_tcp_stop(void* ttcp) {
 	struct ttcp* _ttcp = (struct ttcp*) ttcp;
-	printk("Stop client %p-%p-%p\n", _ttcp, _ttcp->tpcb, _ttcp->lpcb);
-	close_conn(_ttcp);
-	//asm("nop");
+	INFO_TCP("Closing connection...\n");
+	DUMP_TCP_STATE(_ttcp);
+	if ((_ttcp)&&(_ttcp->tpcb)&&(_ttcp->tpcb->state!=LAST_ACK)&&(_ttcp->tpcb->state!=CLOSED))
+		close_conn(_ttcp);
+//	else
+//		printk("Skip");
+
+	asm("nop");
 }
 
 uint8_t getStateTcp(void* p, bool client) {
 	struct ttcp* _ttcp = (struct ttcp*) p;
 
 	if ((_ttcp != NULL) && (_ttcp->tpcb != NULL)) {
-		//		INFO("ttcp:%p tpcb:%p state:%d lpcb:%p state:%d\n",
-		//				p, _ttcp->tpcb, _ttcp->tpcb->state,
-		//				_ttcp->lpcb, _ttcp->lpcb->state);
+		//DUMP_TCP_STATE(_ttcp);
 		if (client)
 			return _ttcp->tpcb->state;
 		else
 			return _ttcp->lpcb->state;
 	} else {
-		WARN("TCP not initialized ttcp:%p tpcb:%p lpcb:%p\n",
+		INFO_TCP("TCP not initialized ttcp:%p tpcb:%p lpcb:%p\n",
 				_ttcp, ((_ttcp)?_ttcp->tpcb:0), ((_ttcp)?_ttcp->lpcb:0));
 	}
 	return CLOSED;
 }
 
 uint8_t isDataSent(void* p) {
+	static int isDataSentCount = 0;
 	struct ttcp* _ttcp = (struct ttcp*) p;
 	if ((_ttcp != NULL) && (_ttcp->tpcb != NULL)) {
-		//INFO("ttcp:%p tpcb:%p sent:%d\n",p, _ttcp->tpcb, _ttcp->buff_sent);
-		return _ttcp->buff_sent;
+		//INFO_TCP("ttcp:%p tpcb:%p sent:%d\n",p, _ttcp->tpcb, _ttcp->buff_sent);
+		bool dataSent = _ttcp->buff_sent;
+		if (dataSent) isDataSentCount = 0;
+		else ++isDataSentCount;
+		if (isDataSentCount == 250)
+			WARN("data not sent\n");
+		return dataSent;
 	} else {
 		WARN("TCP null!\n");
 	}
@@ -597,20 +600,27 @@ static err_t tcp_data_sent(void *arg, struct tcp_pcb *pcb, u16_t len) {
 
 	if (_ttcp->left > 0) {
 		//send_data(pcb, hs);
-		INFO("data left: %d", _ttcp->left );
+		INFO_TCP("data left: %d", _ttcp->left );
 	}
+	if (_ttcp->buff_sent == 1)
+		WARN("Previous packet already\n");
 	_ttcp->buff_sent = 1;
-	//INFO("%s: duration: %d\n", __FUNCTION__, timer_get_ms() - startTime);
+	//INFO_TCP("%s: duration: %d\n", __FUNCTION__, timer_get_ms() - startTime);
 	return ERR_OK;
 }
 
 int sendTcpData(void* p, uint8_t* buf, uint16_t len) {
-	//INFO("buf:%p len:%d\n", buf, len);
+	//INFO_TCP("buf:%p len:%d\n", buf, len);
 	//DUMP(buf,len);
 
-	startTime = timer_get_ms();
+	//startTime = timer_get_ms();
 	struct ttcp* _ttcp = (struct ttcp*) p;
-	if ((_ttcp != NULL) && (_ttcp->tpcb != NULL) && (buf != NULL) && (len != 0)) {
+	if ((_ttcp != NULL) && (_ttcp->tpcb != NULL) &&
+			(buf != NULL) && (len != 0) && (_ttcp->payload != NULL)) {
+		if (_ttcp->tpcb->state == ESTABLISHED ||
+				_ttcp->tpcb->state == CLOSE_WAIT ||
+				_ttcp->tpcb->state == SYN_SENT ||
+				_ttcp->tpcb->state == SYN_RCVD) {
 		_ttcp->buff_sent = 0;
 		//pbuf_take(buf, len, _ttcp->);
 		memcpy(_ttcp->payload, buf, len);
@@ -619,7 +629,9 @@ int sendTcpData(void* p, uint8_t* buf, uint16_t len) {
 		tcp_send_data(_ttcp);
 
 		return WL_SUCCESS;
+		}
 	}
+	//printk("Write failure _ttcp=%p _ttcp->tpcb=%p buf=%p len=%d\n", _ttcp, _ttcp->tpcb, buf, len);
 	return WL_FAILURE;
 }
 
@@ -693,6 +705,7 @@ cmd_state_t cmd_ttcp(int argc, char* argv[], void* ctx) {
 	return CMD_DONE;
 }
 
+
 #if 0
 #include "lwip/sockets.h"
 
diff --git a/wifiHD/src/ard_tcp.h b/wifiHD/src/ard_tcp.h
index 52365ef57..b948648bb 100644
--- a/wifiHD/src/ard_tcp.h
+++ b/wifiHD/src/ard_tcp.h
@@ -9,9 +9,47 @@
 #define ARD_TCP_H
 
 #include "console.h"
+#include "lwip/tcp.h"
 
 typedef void (ard_tcp_done_cb_t)(void *opaque, int result);
 
+#define TTCP_MODE_TRANSMIT          0
+#define TTCP_MODE_RECEIVE           1
+
+typedef struct ttcp {
+
+	/* options */
+	struct ip_addr addr; /* host */
+	uint16_t port; /* -p */
+	uint16_t nbuf; /* -n */
+	int mode; /* -t */
+	int verbose; /* -v */
+	int udp; /* -u */
+	uint8_t sock;
+	uint8_t buff_sent;
+
+	/* common */
+	uint16_t print_cnt;
+	uint32_t start_time;
+	uint32_t left;
+	uint32_t recved;
+	ard_tcp_done_cb_t* done_cb;
+	void* opaque;
+	uint32_t buflen; /* -l */
+	uint32_t tid;
+
+	/* TCP specific */
+	struct tcp_pcb* tpcb;
+	struct tcp_pcb* lpcb;
+	char* payload;
+
+	/* UDP specific */
+	int udp_started;
+	uint16_t udp_end_marker_left;
+	struct udp_pcb* upcb;
+}ttcp_t;
+
+
 int ard_tcp_start(struct ip_addr addr, uint16_t port, void *opaque,
            ard_tcp_done_cb_t *done_cb, int mode, uint16_t nbuf, uint16_t buflen, int udp, int verbose, uint8_t sock, void** _ttcp);
 
diff --git a/wifiHD/src/ard_utils.c b/wifiHD/src/ard_utils.c
index 92a5a58a4..1b2e26054 100644
--- a/wifiHD/src/ard_utils.c
+++ b/wifiHD/src/ard_utils.c
@@ -44,7 +44,7 @@ void insert_pBuf(struct pbuf* q, uint8_t sock, void* _pcb)
   		headBuf = 0;
   	  if (headBuf == tailBuf)
   		  WARN("Overwriting data!");
-  	  INFO("Insert: %p:%d-%d [%d,%d]\n", p, q->tot_len, p[0], headBuf, tailBuf);
+  	  INFO_UTIL("Insert: %p:%d-%d [%d,%d]\n", p, q->tot_len, p[0], headBuf, tailBuf);
     }
 }
 
@@ -56,7 +56,7 @@ tData* get_pBuf(uint8_t sock)
 	if (IS_BUF_AVAIL())
 	{
 		tData* p = &(pBufStore[tailBuf][sock]);
-		INFO("%p [%d,%d]\n", p, headBuf, tailBuf);
+		INFO_UTIL("%p [%d,%d]\n", p, headBuf, tailBuf);
 		return p;
 	}
 	return NULL;
@@ -80,10 +80,10 @@ bool isAvailTcpDataByte(uint8_t sock)
 
 	if (p != NULL)
 	{
-		INFO("check:%d %d %p\n",p->idx, p->len, p->data);
+		INFO_UTIL("check:%d %d %p\n",p->idx, p->len, p->data);
 		if (p->idx == p->len)
 		{
-			INFO("Free %p other buf %d\n", p->data, IS_BUF_AVAIL());
+			INFO_UTIL("Free %p other buf %d\n", p->data, IS_BUF_AVAIL());
 			freetData(p->data);
 			ack_recved(p->pcb, p->len);
 			return (IS_BUF_AVAIL());
@@ -107,11 +107,11 @@ bool getTcpDataByte(uint8_t sock, uint8_t* payload)
 		{
 		uint8_t* buf = (uint8_t*)p->data;
 		*payload = buf[p->idx++];
-		INFO("get:%d %p %d\n",p->idx, p->data, *payload);
+		INFO_UTIL("get:%d %p %d\n",p->idx, p->data, *payload);
 		return true;
 		}else{
 			//dealloc current buffer
-			INFO("Free %p\n", p->data);
+			INFO_UTIL("Free %p\n", p->data);
 			freetData(p->data);
 			ack_recved(p->pcb, p->len);
 		}
diff --git a/wifiHD/src/ard_utils.h b/wifiHD/src/ard_utils.h
index 54cf77bc7..a081cc6ad 100644
--- a/wifiHD/src/ard_utils.h
+++ b/wifiHD/src/ard_utils.h
@@ -9,6 +9,7 @@
 #define ARD_UTILS_H_
 
 #include "gpio.h"
+#include "debug.h"
 #include "ARDUINO/arduino.h"
 #define INIT_SIGNAL_FOR_SPI() 	gpio_enable_pin_pull_up(ARDUINO_HANDSHAKE_PIN)
 #define BUSY_FOR_SPI() 			gpio_set_gpio_pin(ARDUINO_HANDSHAKE_PIN)
@@ -199,6 +200,10 @@
 		statSpi.lastError = SPI_ERROR_TIMEOUT;	\
 		statSpi.status = spi_getStatus(ARD_SPI);
 
+#define DUMP_TCP_STATE(TTCP) \
+		INFO_TCP("ttcp:%p tpcb:%p state:%d lpcb:%p state:%d\n", \
+			TTCP, TTCP->tpcb, (TTCP->tpcb)?TTCP->tpcb->state:0, \
+			TTCP->lpcb, (TTCP->lpcb)?TTCP->lpcb->state:0);
 
 
 typedef struct sData
diff --git a/wifiHD/src/cmd_wl.c b/wifiHD/src/cmd_wl.c
index b3439cbfa..aff74d4db 100644
--- a/wifiHD/src/cmd_wl.c
+++ b/wifiHD/src/cmd_wl.c
@@ -36,6 +36,9 @@
 #include "wl_util.h"
 #include "util.h"
 #include "lwip/netif.h"
+#include "debug.h"
+
+extern void showTTCPstatus();
 
 /**
  *
@@ -190,6 +193,7 @@ cmd_status(int argc, char* argv[], void* ctx)
         else
                 printk("ip addr: none\n");
 
+        showTTCPstatus();
         return CMD_DONE;
 }
 
@@ -346,3 +350,51 @@ cmd_setkey(int argc, char* argv[], void* ctx)
 
         return CMD_DONE;
 }
+
+cmd_state_t
+cmd_debug(int argc, char* argv[], void* ctx)
+{
+        int level;
+        const char *usage = "usage: debug <section> <level>\n\t"\
+        		"section: init, cm, spi, tcp , util, warn\n\t"
+        		"level  : 0 (off), 1 (on)\n\t"
+                "or: debug print/on/off\n";
+
+        if (argc == 2 && strcmp(argv[1], "off") == 0) {
+                printk("Debug OFF\n");
+                enableDebug = 0;
+                return CMD_DONE;
+        }else if (argc == 2 && strcmp(argv[1], "print") == 0) {
+            printk("Debug enabled: 0x%x\n", enableDebug);
+            return CMD_DONE;
+        }else if (argc == 2 && strcmp(argv[1], "on") == 0) {
+            printk("Debug ON\n");
+            enableDebug = 0xff;
+            return CMD_DONE;
+        }
+        if (argc < 3) {
+                printk(usage);
+                return CMD_DONE;
+        }
+        level = atoi(argv[2]);
+        if (argc == 3 && strcmp(argv[1], "init") == 0) {
+        	if (level) enableDebug |= INFO_INIT_FLAG;
+        	else enableDebug &= ~INFO_INIT_FLAG;
+        }else if (argc == 3 && strcmp(argv[1], "spi") == 0) {
+        	if (level) enableDebug |= INFO_SPI_FLAG;
+        	else enableDebug &= ~INFO_SPI_FLAG;
+        }else if (argc == 3 && strcmp(argv[1], "tcp") == 0) {
+        	if (level) enableDebug |= INFO_TCP_FLAG;
+        	else enableDebug &= ~INFO_TCP_FLAG;
+        }else if (argc == 3 && strcmp(argv[1], "cm") == 0) {
+        	if (level) enableDebug |= INFO_CM_FLAG;
+        	else enableDebug &= ~INFO_CM_FLAG;
+        }else if (argc == 3 && strcmp(argv[1], "util") == 0) {
+        	if (level) enableDebug |= INFO_UTIL_FLAG;
+        	else enableDebug &= ~INFO_UTIL_FLAG;
+        }else if (argc == 3 && strcmp(argv[1], "warn") == 0) {
+        	if (level) enableDebug |= INFO_WARN_FLAG;
+        	else enableDebug &= ~INFO_WARN_FLAG;
+        }
+        return CMD_DONE;
+}
diff --git a/wifiHD/src/cmd_wl.h b/wifiHD/src/cmd_wl.h
index 4df8d4c32..016e3c5a3 100644
--- a/wifiHD/src/cmd_wl.h
+++ b/wifiHD/src/cmd_wl.h
@@ -40,5 +40,6 @@ cmd_state_t cmd_power(int argc, char* argv[], void* ctx);
 cmd_state_t cmd_psconf(int argc, char* argv[], void* ctx);
 cmd_state_t cmd_setpass(int argc, char* argv[], void* ctx);
 cmd_state_t cmd_delpass(int argc, char* argv[], void* ctx);
+cmd_state_t cmd_debug(int argc, char* argv[], void* ctx);
 
 #endif
diff --git a/wifiHD/src/debug.h b/wifiHD/src/debug.h
index ccaaf60c8..49d552d69 100644
--- a/wifiHD/src/debug.h
+++ b/wifiHD/src/debug.h
@@ -13,28 +13,47 @@
 #include <stdio.h>
 #include <string.h>
 
-#define INFO_0 1
-#define INFO_1 2
-#define INFO_2 4
-#define INFO_3 8
-#define INFO_4 16
+#define INFO_INIT_FLAG 	1
+#define INFO_TCP_FLAG 	2
+#define INFO_SPI_FLAG 	4
+#define INFO_CM_FLAG 	8
+#define INFO_UTIL_FLAG 16
 #define INFO_5 32
 #define INFO_D (1<<0xD)     // Debug
 #define INFO_E (1<<0xE)     // Error
-#define INFO_F (1<<0xF)     // Warning
+#define INFO_WARN_FLAG (1<<0xF)     // Warning
+
+
+extern uint16_t enableDebug;
+#define INFO_INIT(msg, args...) do { 			\
+if (enableDebug & INFO_INIT_FLAG) printk("I-[%s] " msg , __func__ , ##args );	\
+} while (0)
+
+#define INFO_TCP(msg, args...) do { 			\
+if (enableDebug & INFO_TCP_FLAG) printk("I-[%s] " msg , __func__ , ##args );	\
+} while (0)
+
+#define INFO_SPI(msg, args...) do { 			\
+if (enableDebug & INFO_SPI_FLAG) printk("I-[%s] " msg , __func__ , ##args );	\
+} while (0)
+
+#define INFO_UTIL(msg, args...) do { 			\
+if (enableDebug & INFO_UTIL_FLAG) printk("I-[%s] " msg , __func__ , ##args );	\
+} while (0)
 
 
 #ifdef _APP_DEBUG_
 #define INFO(msg, args...) do { 			\
 printk("I-[%s] " msg , __func__ , ##args );	\
 } while (0)
+
 #else /* !defined(_DEBUG_) */
-#define INFO(msg, args...) do {} while (0)
+//#define INFO(msg, args...) do {} while (0)
 #endif /* !defined(_DEBUG_) */
 
 #if 1
 #define WARN(msg, args...) do {	\
-	printk("W-[%s] " msg , __func__ , ##args );		\
+	if (enableDebug & INFO_WARN_FLAG) printk("W-[%s] " msg , __func__ , ##args );		\
 	} while (0)
 #else
 #define WARN(msg, args...) do {		} while (0)
diff --git a/wifiHD/src/lwipopts.h b/wifiHD/src/lwipopts.h
index 8a36d04f9..48b38c722 100644
--- a/wifiHD/src/lwipopts.h
+++ b/wifiHD/src/lwipopts.h
@@ -424,7 +424,7 @@
 #undef DHCP_DOES_ARP_CHECK
 
 #if 0
-#define LWIP_DEBUG 0
+//#define LWIP_DEBUG 1
 //#define NETIF_DEBUG LWIP_DBG_ON
 //#define DHCP_DEBUG LWIP_DBG_ON
 //#define ICMP_DEBUG LWIP_DBG_ON
@@ -435,7 +435,7 @@
 //#define ETHARP_DEBUG LWIP_DBG_ON
 //#define PBUF_DEBUG LWIP_DBG_ON
 //#define TCP_INPUT_DEBUG LWIP_DBG_ON
-//#define TCP_OUTPUT_DEBUG LWIP_DBG_OFF
+//#define TCP_OUTPUT_DEBUG LWIP_DBG_ON
 #endif
 
 #define ETH_PAD_SIZE WL_HEADER_SIZE /* size of wifiengine header */
diff --git a/wifiHD/src/main.c b/wifiHD/src/main.c
index 59478540a..0b1a95b47 100644
--- a/wifiHD/src/main.c
+++ b/wifiHD/src/main.c
@@ -29,6 +29,7 @@
 #include "cmd_wl.h"
 #include "httpd.h"
 #include "ping.h"
+#include "ard_tcp.h"
 
 #if BOARD == ARDUINO
 #if !defined(DATAFLASH)
@@ -85,6 +86,9 @@ struct ctx_server {
 
 static bool initSpiComplete = false;
 
+// variable used as enable flag for debug prints
+uint16_t enableDebug = 0;
+
 /**
  *
  */
@@ -130,13 +134,12 @@ dhcp_coarse_tmr_cb(void *ctx)
 static void
 wl_cm_scan_cb(void* ctx)
 {
-#ifdef _APP_DEBUG_
 	struct ctx_server* hs = ctx;
 
 	uint8_t init = hs->wl_init_complete;
 
-    INFO("Scan networks...[ OK ] %d\n", init);
-#endif
+    INFO_INIT("Scan networks...[ OK ] %d 0x%x\n", init);
+
     set_result(WL_SCAN_COMPLETED);
 }
 
@@ -150,10 +153,13 @@ wl_cm_conn_cb(struct wl_network_t* net, void* ctx)
 
 	LINK_LED_ON();
 
-        printk("link up, connected to \"%s\"\n", ssid2str(&net->ssid));
-        printk("requesting dhcp ... ");
+	INFO_INIT("Connection cb...\n");
 
-        dhcp_start(hs->netif);
+	printk("link up, connected to \"%s\"\n", ssid2str(&net->ssid));
+    printk("requesting dhcp ... ");
+
+    INFO_INIT("Start DHCP...\n");
+    dhcp_start(hs->netif);
 }
 
 
@@ -166,23 +172,24 @@ wl_cm_disconn_cb(void* ctx)
 	struct ctx_server* hs = ctx;
 
 	LINK_LED_OFF();
+	INFO_INIT("Disconnection cb...\n");
 
-        if (netif_is_up(hs->netif)) {
-                printk("link down, release dhcp\n");
-                dhcp_release(hs->netif);
-                dhcp_stop(hs->netif);
-        } else {
-                printk("link down\n");
-        }
+    if (netif_is_up(hs->netif)) {
+    	printk("link down, release dhcp\n");
+        dhcp_release(hs->netif);
+        dhcp_stop(hs->netif);
+     } else {
+    	 printk("link down\n");
+     }
 
-        set_result_cmd(WL_FAILURE);
+     set_result_cmd(WL_FAILURE);
 }
 
 
 static void wl_cm_err_cb(void* ctx)
 {
     int err = *(int*)ctx;
-    printk("Error: %d\n", err);
+    WARN("Error: %d\n", err);
     set_result_cmd(err);
 }
 
@@ -192,11 +199,12 @@ static void wl_cm_err_cb(void* ctx)
 static void
 ip_status_cb(struct netif* netif)
 {
+	INFO_INIT("IP status cb...\n");
         if (netif_is_up(netif)) {
             set_result_cmd(WL_SUCCESS);
                 printk("bound to %s\n", ip2str(netif->ip_addr));
         }else{
-        	WARN("Interface not up!");
+        	WARN("Interface not up!\n");
         }
 }
 
@@ -300,16 +308,22 @@ poll(struct ctx_server* hs)
 void initShell()
 {
 	/* initialize shell */
+	INFO_INIT("Shell init...\n");
         console_init();
         console_add_cmd("scan", cmd_scan, NULL);
         console_add_cmd("connect", cmd_connect, NULL);
         console_add_cmd("setkey", cmd_setkey, NULL);
         console_add_cmd("status", cmd_status, NULL);
+        console_add_cmd("debug", cmd_debug, NULL);
+
 #ifdef ADD_CMDS
         console_add_cmd("powersave", cmd_power, NULL);
         console_add_cmd("psconf", cmd_psconf, NULL);
+#endif
+#ifdef PING_CMD
         console_add_cmd("ping", cmd_ping, NULL);
 #endif
+        console_add_cmd("ttcp", cmd_ttcp, NULL);
 #ifdef WITH_WPA
         console_add_cmd("wpass", cmd_setpass, NULL);
         console_add_cmd("dpass", cmd_delpass, NULL);
@@ -363,7 +377,9 @@ wl_init_complete_cb(void* ctx)
         	AVAIL_FOR_SPI();
         }
 
-        /* start connection manager */
+    /* start connection manager */
+   	INFO_INIT("Starting CM...\n");
+
 	wl_status = wl_cm_start(wl_cm_scan_cb, wl_cm_conn_cb, wl_cm_disconn_cb, wl_cm_err_cb, hs);
 	ASSERT(wl_status == WL_SUCCESS, "failed to init wl conn mgr");
 }
@@ -398,7 +414,6 @@ main(void)
     	spi_poll(NULL);
 
      }
-    printk("END Arduino Wifi\n");
 #else
     printk("Arduino Wifi Startup... [%s]\n", __TIMESTAMP__);
 
@@ -410,7 +425,7 @@ main(void)
 	hs->netif = calloc(1, size_netif);
 	ASSERT(hs->netif, "out of memory");
 
-	INFO("hs:%p size:0x%x netif:%p size:0x%x\n", hs, size_ctx_server,
+	INFO_INIT("hs:%p size:0x%x netif:%p size:0x%x\n", hs, size_ctx_server,
 			hs->netif, size_netif);
         timer_init(NULL, NULL);
         lwip_init();
diff --git a/wifiHD/src/ping.c b/wifiHD/src/ping.c
index d04128706..a27f41cab 100644
--- a/wifiHD/src/ping.c
+++ b/wifiHD/src/ping.c
@@ -29,7 +29,7 @@
  * This file is derived from a part of the lwIP TCP/IP stack.
  * 
  */
-#ifdef ADD_CMDS
+#ifdef PING_CMD
 #include "lwip/opt.h"
 
 #include "lwip/mem.h"
diff --git a/wifiHD/src/wifi_spi.h b/wifiHD/src/wifi_spi.h
index b9ce88129..40a42805d 100644
--- a/wifiHD/src/wifi_spi.h
+++ b/wifiHD/src/wifi_spi.h
@@ -55,7 +55,7 @@ enum {
     GET_DATABUF_TCP_CMD		= 0x45,
 };
 
-
+#if 0
 enum wl_tcp_state {
   CLOSED      = 0,
   LISTEN      = 1,
@@ -69,7 +69,7 @@ enum wl_tcp_state {
   LAST_ACK    = 9,
   TIME_WAIT   = 10
 };
-
+#endif
 
 enum numParams{
     PARAM_NUMS_0,
diff --git a/wifiHD/src/wl_cm.c b/wifiHD/src/wl_cm.c
index 01eac6178..a3bd9f969 100644
--- a/wifiHD/src/wl_cm.c
+++ b/wifiHD/src/wl_cm.c
@@ -30,13 +30,13 @@
 #include "wl_cm.h"
 #include "wl_util.h"
 #include <string.h>
-
+#include "debug.h"
 
 
 #if 1
 # include "printf-stdarg.h"
 #include "ard_utils.h"
-# define CM_DPRINTF(fmt...) printk(fmt)
+# define CM_DPRINTF(fmt...) if (enableDebug & INFO_CM_FLAG) printk(fmt)
 #else
 # define CM_DPRINTF(fmt...)
 #endif