Merge branch 'wifishield-bugfix'

18 files changed, 1525 insertions, 768 deletions

diff --git a/firmwares/wifishield/wifiHD/src/SOFTWARE_FRAMEWORK/BOARDS/ARDUINO/arduino.h b/firmwares/wifishield/wifiHD/src/SOFTWARE_FRAMEWORK/BOARDS/ARDUINO/arduino.h
index 2b78ab6..e687723 100644
--- a/firmwares/wifishield/wifiHD/src/SOFTWARE_FRAMEWORK/BOARDS/ARDUINO/arduino.h
+++ b/firmwares/wifishield/wifiHD/src/SOFTWARE_FRAMEWORK/BOARDS/ARDUINO/arduino.h
@@ -93,6 +93,7 @@
 #define LED1_GPIO   AVR32_PIN_PB20
 #define LED2_GPIO   AVR32_PIN_PB21
 #define DEB_PIN_GPIO AVR32_PIN_PA20
+#define DEB2_PIN_GPIO AVR32_PIN_PB00
 //! @}
 
 /*! \name PWM Channels of LEDs
diff --git a/firmwares/wifishield/wifiHD/src/SOFTWARE_FRAMEWORK/SERVICES/LWIP/lwip-1.3.2/src/include/lwip/opt.h b/firmwares/wifishield/wifiHD/src/SOFTWARE_FRAMEWORK/SERVICES/LWIP/lwip-1.3.2/src/include/lwip/opt.h
index f70f646..a7cdbd8 100644
--- a/firmwares/wifishield/wifiHD/src/SOFTWARE_FRAMEWORK/SERVICES/LWIP/lwip-1.3.2/src/include/lwip/opt.h
+++ b/firmwares/wifishield/wifiHD/src/SOFTWARE_FRAMEWORK/SERVICES/LWIP/lwip-1.3.2/src/include/lwip/opt.h
@@ -1226,7 +1226,7 @@
  * in seconds. (does not require sockets.c, and will affect tcp.c)
  */
 #ifndef LWIP_TCP_KEEPALIVE
-#define LWIP_TCP_KEEPALIVE              0
+#define LWIP_TCP_KEEPALIVE              1
 #endif
 
 /**
diff --git a/firmwares/wifishield/wifiHD/src/SOFTWARE_FRAMEWORK/UTILS/DEBUG/debug.c b/firmwares/wifishield/wifiHD/src/SOFTWARE_FRAMEWORK/UTILS/DEBUG/debug.c
index c7c0a03..fe8a2a0 100644
--- a/firmwares/wifishield/wifiHD/src/SOFTWARE_FRAMEWORK/UTILS/DEBUG/debug.c
+++ b/firmwares/wifishield/wifiHD/src/SOFTWARE_FRAMEWORK/UTILS/DEBUG/debug.c
@@ -46,6 +46,7 @@
 
 #include "compiler.h"
 #include "debug.h"
+#include "util.h"
 
 
 #if (defined __GNUC__)
@@ -117,3 +118,16 @@ void uc3_round_trace(U32 val)
 
   //Enable_global_interrupt();
 }
+
+void dump(char* _buf, uint16_t _count) {
+
+	int i;
+	for (i = 0; i < _count; ++i)
+	{
+		printk("0x%x ", _buf[i]);
+		if ((i!=0)&&(i % 10 == 0))
+			printk("\n\t");
+	}
+	printk("\n");
+}
+
diff --git a/firmwares/wifishield/wifiHD/src/ard_spi.c b/firmwares/wifishield/wifiHD/src/ard_spi.c
index a5ef895..8bd288b 100644
--- a/firmwares/wifishield/wifiHD/src/ard_spi.c
+++ b/firmwares/wifishield/wifiHD/src/ard_spi.c
@@ -24,6 +24,9 @@
 #include "timer.h"
 #include "lwip/dns.h"
 #include <board_init.h>
+#include "util.h"
+#include "lwip/udp.h"
+#include "lwip_setup.h"
 
 extern const char* fwVersion;
 
@@ -92,7 +95,7 @@ bool end_write = false;	//TODO only for debug
 // Signal indicating a new command is coming from SPI interface
 static volatile Bool startRecvCmdSignal = FALSE;
 
-#define MAX_CMD_NUM 30
+#define MAX_CMD_NUM 36
 typedef struct sCmd_spi_list{
 	cmd_spi_cb_t cb;
 	char cmd_id;
@@ -136,6 +139,7 @@ void initStatSpi()
 
 void printStatSpi()
 {
+	printk("totSpiCmds\t: 0x%x\n", cmdCorr);
 	printk("lastCmd  \t: 0x%x\n", statSpi.lastCmd);
 	printk("lastErr  \t: 0x%x\n", statSpi.lastError);
 	printk("spiStatus\t: 0x%X\n", statSpi.status);
@@ -169,7 +173,28 @@ cmd_resetStatSpi(int argc, char* argv[], void* ctx)
 
 int result = WL_CONNECT_FAILED; //Store the result of the last operation
 
-void* mapSockTCP[MAX_SOCK_NUM];
+void* mapSockTCP[MAX_SOCK_NUM][MAX_MODE_NUM];
+
+//Udp RemoteIp and remote Port
+static tRemoteClient remoteClients[MAX_SOCK_NUM] = {{0,0}};
+
+void setRemoteClient(uint16_t sock, uint32_t _ipaddr, uint16_t _port)
+{
+	if (sock < MAX_SOCK_NUM)
+	{
+		remoteClients[sock].ipaddr = _ipaddr;
+		remoteClients[sock].port = _port;
+	}
+}
+
+tRemoteClient* getRemoteClient(uint16_t sock)
+{
+	if (sock < MAX_SOCK_NUM)
+	{
+		return &remoteClients[sock];
+	}
+	return NULL;
+}
 
 struct netif* ard_netif = NULL;
 
@@ -180,10 +205,10 @@ struct ip_addr _hostIpAddr;
 
 static bool hostIpAddrFound = false;
 
-void* getTTCP(uint8_t sock)
+void* getTTCP(uint8_t sock, uint8_t mode)
 {
 	if (sock < MAX_SOCK_NUM)
-		return mapSockTCP[sock];
+		return mapSockTCP[sock][mode];
 	return NULL;
 }
 
@@ -194,26 +219,31 @@ int getSock(void * _ttcp)
 		int i = 0;
 		for (; i<MAX_SOCK_NUM; i++)
 		{
-			if (_ttcp == mapSockTCP[i])
+			if (_ttcp == mapSockTCP[i][GET_TCP_MODE(_ttcp)])
 				return i;
 		}
 	}
 	return -1;
 }
 
+void setMapSockMode(uint8_t sock, void* _ttcp, uint8_t _tcp_mode)
+{
+	if ((IS_VALID_SOCK(sock))&&(_ttcp!=NULL))
+		mapSockTCP[sock][_tcp_mode]=_ttcp;
+	INFO_TCP("Map [%d, %p, %s]\n", sock, _ttcp, Mode2Str(_tcp_mode));
+}
+
 void setMapSock(uint8_t sock, void* _ttcp)
 {
-	if (sock < MAX_SOCK_NUM)
-		mapSockTCP[sock]=_ttcp;
-	INFO_TCP("Map [%d, %p]\n", sock, _ttcp);
+	setMapSockMode(sock, _ttcp, GET_TCP_MODE(_ttcp));
 }
 
-void clearMapSockTcp(uint8_t sock)
+void clearMapSockTcp(uint8_t sock, uint8_t mode)
 {
 	if (sock < MAX_SOCK_NUM)
 	{
 		//printk("UnMap [%d, %p]\n", sock, mapSockTCP[sock]);
-		mapSockTCP[sock] = NULL;
+		mapSockTCP[sock][mode] = NULL;
 	}
 
 }
@@ -223,6 +253,7 @@ void initMapSockTcp()
 	memset(mapSockTCP, 0, sizeof(mapSockTCP));
 }
 
+#if 0
 /**
  * Calculate bitrate based on number of bytes transmitted and elapsed time
  */
@@ -235,10 +266,10 @@ static void ard_tcp_print_stats(struct ttcp *ttcp) {
 		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");
+			bytes / ms, bytes % ms, ProtMode2Str(ttcp->udp),
+					Mode2Str(ttcp->mode));
 }
-
+#endif
 
 void showTTCPstatus()
 {
@@ -248,19 +279,45 @@ void showTTCPstatus()
 	int i = 0;
 	for (; i<MAX_SOCK_NUM; i++)
 	{
-		void* p = getTTCP(i);
-		if (p)
+		int ii=0;
+		for (; ii<MAX_MODE_NUM; ii++)
 		{
-			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-%p]-Status:%d\n", _ttcp->tpcb, _ttcp->tpcb->state);
-			}
-			if (_ttcp->lpcb){
-				printk("[tlcp-%p]-Status:%d\n", _ttcp->lpcb, _ttcp->lpcb->state);
+			void* p = getTTCP(i, ii);
+			if (p)
+			{
+				ttcp_t* _ttcp = (ttcp_t* )p;
+				printk("Socket n.:%d(%d) [0x%x] %s %s addr:%s port:%d\n", i, ii, _ttcp, 
+					ProtMode2Str(_ttcp->udp), Mode2Str(_ttcp->mode), ip2str(_ttcp->addr), _ttcp->port);
+				if (_ttcp->udp == TCP_MODE)
+				{
+					int j = 0;
+					for (; j<MAX_CLIENT_ACCEPTED; ++j)
+					{
+						if (_ttcp->tpcb[j]){
+							printk("[%d tpcp-%p]-Status:%d\n", j, _ttcp->tpcb[j], _ttcp->tpcb[j]->state);
+						}
+					}
+
+					if (_ttcp->lpcb){
+						printk("[tlcp-%p]-Status:%d\n", _ttcp->lpcb, _ttcp->lpcb->state);
+					}
+				}else{
+					if (_ttcp->upcb){
+						struct ip_addr loc = _ttcp->upcb->local_ip;
+						printk("[upcp-%p] flags:0x%x  local:%s[0x%x]-%d\n", 
+								_ttcp->upcb, _ttcp->upcb->flags,
+								ip2str(loc), loc, _ttcp->upcb->local_port);				
+						tRemoteClient remote = {0,0};;
+						getRemoteData(i, ii, &remote);
+						struct ip_addr ipaddr = { remote.ipaddr };
+						printk("remote:%s(0x%x)-%d\n", ip2str(ipaddr), remote.ipaddr, remote.port);
+						}					
+				}
+				//ard_tcp_print_stats(_ttcp);
+				printk("Data avail:%s\n", isAvailTcpDataByte(i)?"YES":"NO");
+				printk("------------------------------\n");
 			}
-			ard_tcp_print_stats(_ttcp);
-		}
+		}			
 	}
 
 	tcp_debug_print_pcbs();
@@ -270,12 +327,8 @@ int write_stream(volatile avr32_spi_t *spi, const char *stream, uint16_t len)
 {
 	uint16_t _len = 0;
 	unsigned short dummy=0;
-    bool streamExit = false;
 
      	do {
-             if (*stream == END_CMD)
-                 streamExit = true;
-
             //SIGN1_DN();
     		if (spi_write(spi, *stream) == SPI_ERROR_TIMEOUT)
     		{
@@ -294,16 +347,7 @@ int write_stream(volatile avr32_spi_t *spi, const char *stream, uint16_t len)
     			spi_read(spi,&dummy);
     		}
     		//SIGN1_UP();
-	}while ((!streamExit)&&(_len <= len));
-
-     if (!streamExit)
-     {
-#ifdef _SPI_STATS_
-    	 statSpi.wrongFrame++;
-    	 statSpi.lastError = SPI_ERROR_ARGUMENT;
-#endif
-    	 return SPI_ERROR_ARGUMENT;
-     }
+    }while (_len < len);
 	return SPI_OK;
 }
 
@@ -323,8 +367,7 @@ void sendError()
 	volatile avr32_spi_t *spi = ARD_SPI;							\
     Bool global_interrupt_enabled = Is_global_interrupt_enabled();	\
     if (global_interrupt_enabled) Disable_global_interrupt();		\
-    spi->IER.rdrf = 1;												\
-    spi->IER.rxbuff = 1;	spi->IER.endrx = 1;						\
+    spi->IER.rdrf = 1; spi->IER.rxbuff = 1;	spi->IER.endrx = 1;		\
     if (global_interrupt_enabled) Enable_global_interrupt();		\
 }while(0);
 
@@ -340,54 +383,6 @@ void sendError()
 		eic_clear_interrupt_line(&AVR32_EIC, AVR32_SPI0_IRQ);	\
 	}while(0);
 
-void dump(char* _buf, uint16_t _count) {
-
-	int i;
-	for (i = 0; i < _count; ++i)
-		printk("0x%x ", _buf[i]);
-	printk("\n");
-}
-#ifdef _APP_DEBUG_
-#define DUMP dump
-#endif
-
-#ifdef _APP_DEBUG_
-#define DUMP_SPI_DATA(BUF, COUNT) do {		\
-	if (verboseDebug & INFO_SPI_FLAG) {		\
-	int i = 0;								\
-	for (; i < COUNT; ++i) 					\
-	{										\
-		printk("0x%x ", BUF[i]);			\
-		if (i % 20 == 0)					\
-			printk("\n");					\
-	}										\
-	printk("\n");							\
-	}										\
-}while(0);
-#else
-#define DUMP_SPI_DATA(BUF, COUNT) do {}while(0);
-#endif
-
-
-#ifdef _APP_DEBUG_
-#define DUMP_SPI_CMD(BUF) do {				\
-	if (verboseDebug & INFO_SPI_FLAG) {		\
-	int i = 0;								\
-	for (; i < CMD_MAX_LEN; ++i) 			\
-	{										\
-		printk("0x%x ", BUF[i]);			\
-		if (BUF[i] == END_CMD)				\
-			break;							\
-	}										\
-	printk("\n");							\
-	}										\
-}while(0);
-#else
-#define DUMP_SPI_CMD(BUF) do {}while(0);
-#endif
-
-
-
 int spi_add_cmd(char _cmd_id, cmd_spi_cb_t cb, cmd_spi_rcb_t rcb, void* ctx,
 		char flag) {
 	U32 i;
@@ -541,6 +536,114 @@ int set_passphrase_cmd_cb(int numParam, char* buf, void* ctx) {
     RETURN_ERR(err)
 }
 
+int set_ip_config_cmd_cb(int numParam, char* buf, void* ctx) {
+    struct ip_addr lwip_addr;
+    struct ctx_server *hs = ctx;
+    struct net_cfg *ncfg = &(hs->net_cfg);
+    struct netif *nif = ncfg->netif;
+    uint8_t parmsToChange=0;
+    const uint8_t MAX_IP_CONFIG_PARAMS = 3;
+
+	wl_err_t err = WL_SUCCESS;
+	tParam* params = (tParam*) buf;
+
+    if (params->paramLen == 1)
+	{
+		GET_PARAM_NEXT(BYTE, params, _parmsToChange);
+		parmsToChange = _parmsToChange;
+	}		
+    else
+    	RETURN_ERR(WL_FAILURE)
+
+    INFO_SPI("%p numParam=%d parmsToChange=%d\n", ctx, numParam, parmsToChange);
+
+    if (parmsToChange <= MAX_IP_CONFIG_PARAMS)
+    {
+		int i=0;
+    	for (; i<parmsToChange; ++i)
+    	{
+    	    if (params->paramLen == 4)
+    	    {
+				GET_PARAM_NEXT(LONG, params, _ip_addr);
+				lwip_addr.addr = _ip_addr;
+				INFO_SPI("%d] nif:%p lwip_addr=0x%x\n", i, nif, lwip_addr.addr);
+    	    	switch (i)
+    	    	{
+    	    		case 0:	// local_ip
+					{ 	    			
+    	    			netif_set_ipaddr(nif, &lwip_addr);
+    	    			break;
+					}								
+    	    		case 1:	// gateway
+					{
+    	    			netif_set_gw(nif, &lwip_addr);
+    	    			break;
+					}					
+    	    		case 2:	// subnet
+					{
+    	    			netif_set_netmask(nif, &lwip_addr);
+    	    			break;
+					}					
+    	    	}
+    	    }else{
+    	    	RETURN_ERR(WL_FAILURE)
+    	    }
+
+    	}
+    	/* Disable DHCP */
+    	ncfg->dhcp_enabled = STATIC_IP_CONFIG;
+    }else
+    	RETURN_ERR(WL_FAILURE)
+		
+    RETURN_ERR(err)
+}
+
+int set_dns_config_cmd_cb(int numParam, char* buf, void* ctx) {
+    struct ip_addr lwip_addr;
+    struct ctx_server *hs = ctx;
+    struct net_cfg *ncfg = &(hs->net_cfg);
+    struct netif *nif = ncfg->netif;
+    uint8_t parmsToChange=0;
+    const uint8_t MAX_DNS_CONFIG_PARAMS = 2;
+
+	wl_err_t err = WL_SUCCESS;
+	tParam* params = (tParam*) buf;
+
+    if (params->paramLen == 1)
+	{
+		GET_PARAM_NEXT(BYTE, params, _parmsToChange);
+		parmsToChange = _parmsToChange;
+	}		
+    else
+    	RETURN_ERR(WL_FAILURE)
+
+    INFO_SPI("%p numParam=%d parmsToChange=%d\n", ctx, numParam, parmsToChange);
+
+    if (parmsToChange <= MAX_DNS_CONFIG_PARAMS)
+    {
+		int i=0;
+    	for (; i<parmsToChange; ++i)
+    	{
+    	    if (params->paramLen == 4)
+    	    {
+				GET_PARAM_NEXT(LONG, params, _ip_addr);
+				lwip_addr.addr = _ip_addr;
+				INFO_SPI("%d] nif:%p lwip_addr=0x%x\n", i, nif, lwip_addr.addr);
+				dns_setserver(i, &lwip_addr);
+    	    }else{
+    	    	RETURN_ERR(WL_FAILURE)
+    	    }
+    	}
+    	/* Disable DHCP */
+    	ncfg->dhcp_enabled = STATIC_IP_CONFIG;
+    }else
+    	RETURN_ERR(WL_FAILURE)
+		
+    RETURN_ERR(err)
+}
+
+
+
 void set_result(wl_status_t _status)
 {
 	result = _status;
@@ -578,7 +681,7 @@ extern int ttcp_start(struct ip_addr addr, uint16_t port, void *opaque,
            void *done_cb, int mode, uint16_t nbuf, uint16_t buflen, int udp, int verbose);
 
 
-int start_server_tcp(uint16_t port, uint8_t sock)
+int start_server_tcp(uint16_t port, uint8_t sock, uint8_t protMode)
 {
 	struct ip_addr addr = { 0 };
     uint16_t buflen = 1024;
@@ -590,7 +693,7 @@ int start_server_tcp(uint16_t port, uint8_t sock)
 #else
     int verbose = 0;
 #endif
-    int udp = 0;
+    int udp = protMode;
     int mode = 1;   //RECEIVE
     void* _ttcp = NULL;
 
@@ -605,20 +708,20 @@ int start_server_tcp(uint16_t port, uint8_t sock)
 
     if (!ifStatus)
      {
-    	WARN("IF down...wait\n");
+    	WARN_VER("IF down...wait\n");
      	return WIFI_SPI_ERR;
      }
 
 
     if (ard_tcp_start(addr, port, NULL, NULL, mode, nbuf, buflen, udp, verbose, sock, &_ttcp) == 0)
     {
-    	INFO_SPI("Start Server [%d, %d] OK!\n", port, sock);
+    	INFO_SPI("Start Server %s [%d, %d] OK!\n", ProtMode2Str(protMode), port, sock);
     	setMapSock(sock, _ttcp);
         err = WL_SUCCESS;
     }else{
 
-    	WARN("Start Server [%d, %d] FAILED!\n", port, sock);
-    	clearMapSockTcp(sock);
+    	WARN("Start Server %s [%d, %d] FAILED!\n", ProtMode2Str(protMode), port, sock);
+    	clearMapSockTcp(sock, TTCP_MODE_RECEIVE);
     }
     return err;
 }
@@ -627,59 +730,72 @@ int start_server_tcp(uint16_t port, uint8_t sock)
 int start_server_tcp_cmd_cb(int numParam, char* buf, void* ctx) {
 	wl_err_t err = WL_FAILURE;
 	tParam* params = (tParam*) buf;
-    if (numParam == 2)
+    if (numParam == 3)
     {
     	GET_PARAM_NEXT(INT, params, port);
     	GET_PARAM_NEXT(BYTE, params, sock);
-    	err = start_server_tcp(port, sock);
+    	GET_PARAM_NEXT(BYTE, params, protMode);
+    	err = start_server_tcp(port, sock, protMode);
     }
     return (err==WL_SUCCESS) ? WIFI_SPI_ACK : WIFI_SPI_ERR;
 }
 
-int start_client_tcp_cmd_cb(int numParam, char* buf, void* ctx) {
-	wl_err_t err = WL_FAILURE;
-	tParam* params = (tParam*) buf;
-    if (numParam == 3)
-    {
-    	GET_PARAM_NEXT(LONG, params, _addr);
-    	GET_PARAM_NEXT(INT, params, port);
-     	GET_PARAM_NEXT(BYTE, params, sock);
+int start_client_tcp(uint32_t _addr, uint16_t port, uint8_t sock, uint8_t protMode)
+{
+    uint16_t buflen = 1024;
+    uint16_t nbuf = 1024;
+    wl_err_t err = WL_FAILURE;
+    struct ip_addr addr = { .addr = _addr};
 
-        INFO_SPI("Addr:0x%x, port:%d, sock:%d\n", _addr, port, sock);
+    INFO_SPI("Addr:0x%x, port:%d, sock:%d, prot:%s\n", _addr, port, sock,  ProtMode2Str(protMode));
 
-        uint16_t buflen = 1024;
-        uint16_t nbuf = 1024;
-    	struct ip_addr addr = { .addr = _addr};
-#ifdef _APP_DEBUG_
-        int verbose = 1;
-#else
-        int verbose = 0;
-#endif
-        int udp = 0;
-        int mode = 0;   //TRANSMIT
-        void* _ttcp = NULL;
+	#ifdef _APP_DEBUG_
+    int verbose = 1;
+	#else
+    int verbose = 0;
+	#endif
+
+    int udp = protMode;
+    int mode = 0;   //TRANSMIT
+    void* _ttcp = NULL;
 
         if (sock >= MAX_SOCK_NUM)
         	return WIFI_SPI_ERR;
 
-        // Check previous connection
-    	_ttcp = getTTCP(sock);
-    	if (_ttcp != NULL)
-    	{
-    		WARN("Previous client %p not stopped !\n", _ttcp);
-    		ard_tcp_stop(_ttcp);
-    		clearMapSockTcp(sock);
-    	}
+    // Check previous connection
+	_ttcp = getTTCP(sock, TTCP_MODE_TRANSMIT);
+	if (_ttcp != NULL)
+	{
+		WARN("Previous client %p not stopped !\n", _ttcp);
+		ard_tcp_stop(_ttcp);
+		clearMapSockTcp(sock, TTCP_MODE_TRANSMIT);
+	}
 
-        if (ard_tcp_start((struct ip_addr)addr, port, NULL, NULL, mode, nbuf, buflen, udp, verbose, sock, &_ttcp) == 0)
-        {
-        	INFO_SPI("Start Client [0x%x, %d, %d] OK!\n", addr, port, sock);
-        	setMapSock(sock, _ttcp);
-            err = WL_SUCCESS;
-        }else{
-        	INFO_SPI("Start Client [0x%x, %d, %d] FAILED!\n", addr, port, sock);
-        	clearMapSockTcp(sock);
-        }
+	if (ard_tcp_start(addr, port, NULL, NULL, mode, nbuf, buflen, udp, verbose, sock, &_ttcp) == 0)
+	{
+		INFO_SPI("Start Client %s %p [0x%x, %d, %d] OK!\n", ProtMode2Str(protMode),
+				_ttcp, addr, port, sock);
+		setMapSock(sock, _ttcp);
+		err = WL_SUCCESS;
+	}else{
+		INFO_SPI("Start Client %s %p [0x%x, %d, %d] FAILED!\n", ProtMode2Str(protMode), 
+				_ttcp, addr, port, sock);
+		clearMapSockTcp(sock, TTCP_MODE_TRANSMIT);
+	}
+	return err;
+}
+
+
+int start_client_tcp_cmd_cb(int numParam, char* buf, void* ctx) {
+	wl_err_t err = WL_FAILURE;
+	tParam* params = (tParam*) buf;
+    if (numParam == 4)
+    {
+    	GET_PARAM_NEXT(LONG, params, _addr);
+    	GET_PARAM_NEXT(INT, params, port);
+     	GET_PARAM_NEXT(BYTE, params, sock);
+     	GET_PARAM_NEXT(BYTE, params, protMode);
+     	err = start_client_tcp(_addr, port, sock, protMode);
     }
     return (err==WL_SUCCESS) ? WIFI_SPI_ACK : WIFI_SPI_ERR;
 }
@@ -697,7 +813,7 @@ int stop_client_tcp_cmd_cb(int numParam, char* buf, void* ctx) {
 
         if (sock < MAX_SOCK_NUM)
         {
-        	_ttcp = getTTCP(sock);
+        	_ttcp = getTTCP(sock, TTCP_MODE_TRANSMIT);
         	ard_tcp_stop(_ttcp);
             err = WL_SUCCESS;
         }
@@ -705,6 +821,37 @@ int stop_client_tcp_cmd_cb(int numParam, char* buf, void* ctx) {
     return (err==WL_SUCCESS) ? WIFI_SPI_ACK : WIFI_SPI_ERR;
 }
 
+int insert_data_cmd_cb(int numParam, char* buf, void* ctx) {
+
+	tDataParam* msg = (tDataParam*) buf;
+    if ((numParam == 2)&&(msg->dataLen == 1))
+    {
+        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);
+        insertBuf(sock, (uint8_t*)(buf+5), len);
+    }
+    return WIFI_SPI_ACK;
+}
+
+int send_data_udp_cmd_cb(int numParam, char* buf, void* ctx) {
+	wl_err_t err = WL_FAILURE;
+
+	tParam* params = (tParam*) buf;
+    if ((numParam == 1)&&(params->paramLen == 1))
+    {
+    	GET_PARAM_NEXT(BYTE, params, sock);
+        uint16_t len = 0;
+        uint8_t* p = mergeBuf(sock, NULL, &len);
+        err = sendUdpData(getTTCP(sock, TTCP_MODE_TRANSMIT), p, len);
+		clearBuf(sock);
+        free(p);
+    }
+
+    return (err==WL_SUCCESS) ? WIFI_SPI_ACK : WIFI_SPI_ERR;
+}
+
+
 int send_data_tcp_cmd_cb(int numParam, char* buf, void* ctx) {
 	wl_err_t err = WL_FAILURE;
 	DATA_LED_ON();
@@ -714,7 +861,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);
+        err = sendTcpData(getTTCP(sock, TTCP_MODE_TRANSMIT), (uint8_t*)(buf+5), len);
     }
     DATA_LED_OFF();
     return (err==WL_SUCCESS) ? WIFI_SPI_ACK : WIFI_SPI_ERR;
@@ -726,7 +873,6 @@ int ack_cmd_cb(int numParam, char* buf, void* ctx) {
 
 int get_result_cmd_cb(int numParam, char* buf, void* ctx) {
 	INFO_SPI("ifStatus:%d result:%d\n", ifStatus, result);
-	*buf=(ifStatus)?WL_CONNECTED:result;
 	return WIFI_SPI_ACK;
 }
 
@@ -744,7 +890,7 @@ cmd_spi_state_t get_reply_cb(char* recv, char* reply, void* ctx, uint16_t* count
 	if (ctx != NULL) {
 		reply[4] = (*(uint8_t*)ctx); //param
 	} else {
-		reply[4] = result; //param
+		reply[4] = (ifStatus)?WL_CONNECTED:result; //param
 	}
 
     END_HEADER_REPLY(reply, 5, *count);
@@ -784,6 +930,51 @@ cmd_spi_state_t get_reply_ipaddr_cb(char* recv, char* reply, void* ctx, uint16_t
     return SPI_CMD_DONE;
 }
 
+void getRemoteData(uint8_t sock, uint8_t mode, tRemoteClient* remoteData)
+{
+	if ((sock>=0) && (sock<MAX_SOCK_NUM))
+	{
+		void* p = getTTCP(sock, mode);
+		if (p)
+		{
+			ttcp_t* _ttcp = (ttcp_t* )p;
+			if ((_ttcp->udp == UDP_MODE))
+			{
+				if (_ttcp->mode == TTCP_MODE_RECEIVE)
+				{
+					remoteData->ipaddr = getRemoteClient(sock)->ipaddr;
+					remoteData->port = getRemoteClient(sock)->port;
+				}else{
+					remoteData->ipaddr = (_ttcp->upcb) ? _ttcp->upcb->remote_ip.addr : 0;
+					remoteData->port = (_ttcp->upcb) ? _ttcp->upcb->remote_port : 0;
+				}
+			}
+		}
+	}
+}
+
+
+cmd_spi_state_t get_reply_remote_data_cb(char* recv, char* reply, void* ctx, uint16_t* count) {
+
+    CHECK_ARD_NETIF(recv, reply, count);
+    DUMP_SPI_CMD(recv);
+
+    GET_DATA_BYTE(sock, recv+4);
+
+    CREATE_HEADER_REPLY(reply, recv, 2);
+	tRemoteClient remoteData = {0,0};
+	//TODO pass the mode
+	getRemoteData(sock, TTCP_MODE_RECEIVE, &remoteData);
+
+    PUT_LONG_IN_BYTE_NO(remoteData.ipaddr, reply, 3);
+    PUT_DATA_INT(remoteData.port, reply, 8);
+
+    END_HEADER_REPLY(reply, 11, *count);
+
+    return SPI_CMD_DONE;
+}
+
+
 void foundHostByName(const char *name, struct ip_addr *ipaddr, void *callback_arg)
 {
 	_hostIpAddr.addr = (ipaddr)?ipaddr->addr:0xffffffff;
@@ -1071,11 +1262,11 @@ cmd_spi_state_t get_state_tcp_cmd_cb(char* recv, char* reply, void* ctx, uint16_
     uint8_t _state = CLOSED;
     if ((recv[3]==1)&&(recv[4]>=0)&&(recv[4]<MAX_SOCK_NUM))
     {
-    	_state = getStateTcp(getTTCP((uint8_t)recv[4]), 0);
+    	_state = getStateTcp(getTTCP((uint8_t)recv[4], TTCP_MODE_RECEIVE), 0);
     }
     PUT_DATA_BYTE(_state, reply, 3);
     END_HEADER_REPLY(reply, 5, *count);
-    INFO_SPI("state:%d\n", _state);
+    INFO_SPI_POLL("state:%d\n", _state);
 
     return SPI_CMD_DONE;
 }
@@ -1087,33 +1278,20 @@ cmd_spi_state_t get_client_state_tcp_cmd_cb(char* recv, char* reply, void* ctx,
     CREATE_HEADER_REPLY(reply, recv, PARAM_NUMS_1);
 
     uint8_t _state = CLOSED;
-    if ((recv[3]==1)&&(recv[4]>=0)&&(recv[4]<MAX_SOCK_NUM))
+    uint8_t _sock = recv[4];
+    if ((recv[3]==1)&&(_sock>=0)&&(_sock<MAX_SOCK_NUM))
     {
-    	void * p= getTTCP((uint8_t)recv[4]);
+    	void * p= getTTCP(_sock, TTCP_MODE_TRANSMIT);
     	if (p!=NULL)
     	{
-			// get if we are in server or Transmit mode (0)
-			if (getModeTcp(p) == TTCP_MODE_TRANSMIT)
-			{
-
-				_state = getStateTcp(p, 1);
-				INFO_SPI_VER("CLI> p=%p _ttcp=%p state(tpcb):%d state:%d\n",
-									p, ((struct ttcp*) p)->tpcb,
-									((struct ttcp*) p)->tpcb->state,
-									_state);
-			}else {
-				_state = getStateTcp(p, 1);
-				INFO_SPI_VER("SER> p=%p _ttcp=%p state(tpcb):%d state(lpcb):%d state:%d\n",
-									p, ((struct ttcp*) p)->tpcb,
-									((struct ttcp*) p)->tpcb->state,
-									((struct ttcp*) p)->lpcb->state,
-									_state);
-			}
+			_state = getStateTcp(p, 1);	
+    	}else{
+    		WARN_VER("TTCP not found for sock:%d\n", _sock);
     	}
     }
     PUT_DATA_BYTE(_state, reply, 3);
     END_HEADER_REPLY(reply, 5, *count);
-    INFO_SPI("state:%d\n", _state);
+    INFO_SPI_POLL("sock:%d state:%d\n", _sock, _state);
 
     return SPI_CMD_DONE;
 }
@@ -1123,15 +1301,15 @@ cmd_spi_state_t avail_data_tcp_cmd_cb(char* recv, char* reply, void* ctx, uint16
 	CHECK_ARD_NETIF(recv, reply, count);
 
 	CREATE_HEADER_REPLY(reply, recv, PARAM_NUMS_1);
-	uint8_t dataAvail = 0;
+	uint16_t dataAvail = 0;
     if ((recv[3]==1)&&(recv[4]>=0)&&(recv[4]<MAX_SOCK_NUM))
     {
-    	dataAvail = isAvailTcpDataByte((uint8_t)recv[4]) ? 1 : 0;
+    	dataAvail = getAvailTcpDataByte((uint8_t)recv[4]);
     }
-	PUT_DATA_BYTE(dataAvail, reply, 3);
-	END_HEADER_REPLY(reply, 5, *count);
+	PUT_DATA_INT_NO(dataAvail, reply, 3);
+	END_HEADER_REPLY(reply, 6, *count);
 
-	INFO_SPI("dataAvail:%d\n", dataAvail);
+	INFO_SPI_POLL("dataAvail:%d\n", dataAvail);
 
     return SPI_CMD_DONE;
 }
@@ -1155,7 +1333,7 @@ cmd_spi_state_t data_sent_tcp_cmd_cb(char* recv, char* reply, void* ctx, uint16_
 	uint8_t dataSent = 0;
     if ((recv[3]==1)&&(recv[4]>=0)&&(recv[4]<MAX_SOCK_NUM))
     {
-    	dataSent = isDataSent(getTTCP((uint8_t)recv[4]));
+    	dataSent = isDataSent(getTTCP((uint8_t)recv[4], TTCP_MODE_TRANSMIT));
     }
 	PUT_DATA_BYTE(dataSent, reply, 3);
 	END_HEADER_REPLY(reply, 5, *count);
@@ -1199,14 +1377,15 @@ cmd_spi_state_t get_databuf_tcp_cmd_cb(char* recv, char* reply, void* ctx, uint1
 
     CHECK_ARD_NETIF(recv, reply, count);
 
-    if ((recv[3]==1)&&(recv[4]>=0)&&(recv[4]<MAX_SOCK_NUM))
+    GET_DATA_BYTE(sock, buf+5);
+    if ((sock>=0)&&(sock<MAX_SOCK_NUM))
     {
-    	if (getTcpData((uint8_t)recv[4], (void**)&data, &len))
+    	if (getTcpData((uint8_t)sock, (void**)&data, &len))
     	{
     		CREATE_HEADER_REPLY(reply, recv, PARAM_NUMS_1);
     		PUT_BUFDATA_INT(data, len, reply, 3);
-    		END_HEADER_REPLY(reply, 3+len+2+1, *count);
-    		freeTcpData((uint8_t)recv[4]);
+    		END_HEADER_REPLY(reply, 3+len+2, *count);
+    		freeTcpData((uint8_t)sock);
     	}else{
     		CREATE_HEADER_REPLY(reply, recv, PARAM_NUMS_0);
     		END_HEADER_REPLY(reply, 3, *count);
@@ -1264,16 +1443,17 @@ int sendReply(int cmdIdx, char* recv, char* reply, void* resultCmd)
     AVAIL_FOR_SPI();
     _result = write_stream(ARD_SPI, &reply[0], _count);
 #ifdef _SPI_STATS_
-    if ( result != SPI_OK)
+    if ( _result != SPI_OK)
     {
     	statSpi.lastCmd = cmd_spi_list[cmdIdx].cmd_id;
     }
 #endif
     BUSY_FOR_SPI();
 
-    //unsigned char status = spi_getStatus(ARD_SPI);
-    //INFO_SPI("Status after write: 0x%x\n",status);
-
+    IF_SPI_DUMP(printk("==>"));
+    DUMP_SPI(recv, count);
+    IF_SPI_DUMP(printk("<=="));
+	DUMP_SPI(reply, _count);
     replyCount = _count;
     return _result;
 }
@@ -1282,21 +1462,22 @@ unsigned char* getStartCmdSeq(unsigned char* _recv, int len, int *offset)
 {
 	int i = 0;
 	*offset = 0;
-	DEB_PIN_UP();
+	//DEB_PIN_UP();
 	for (; i<len; ++i)
 	{
 		if (_recv[i]==START_CMD)
 		{
 			if (i!=0)
 			{
-				DEB_PIN_DN();
+				DEB_PIN_TRIGGER();
+				IF_WARN_VER(dump((char*)_recv, (uint16_t)len));
 				WARN("%d] Disall. %d/%d cmd:%d\n", cmdCorr, i, len,_recv[i+1]);
 			}
 			*offset = i;
 			return &_recv[i];
 		}
 	}
-	DEB_PIN_DN();
+	//DEB_PIN_DN();
 	WARN("%d] Disall. %d\n", cmdCorr, i);
 
 	return NULL;
@@ -1330,12 +1511,12 @@ int call_reply_cb(char* recv, char* reply) {
                  {
                 	 tSpiMsg* spiMsg = (tSpiMsg*) recv;
                 	 _result = cmd_spi_list[i].cb(spiMsg->nParam,
-     					(char*) &(spiMsg->params[0]), NULL);
+     					(char*) &(spiMsg->params[0]), cmd_spi_list[i].ctx);
            		 }else
            		 {
            			tSpiMsgData* spiMsg = (tSpiMsgData*) recv;
                    	 _result = cmd_spi_list[i].cb(spiMsg->nParam,
-         					(char*) &(spiMsg->params[0]), NULL);
+         					(char*) &(spiMsg->params[0]), cmd_spi_list[i].ctx);
            		 }
 
                  if (_result != WIFI_SPI_ACK)
@@ -1380,10 +1561,12 @@ int call_reply_cb(char* recv, char* reply) {
 	return REPLY_NO_ERR;
 }
 
-void init_spi_cmds() {
+void init_spi_cmds(void* ctx) {
 	spi_add_cmd(SET_NET_CMD, set_net_cmd_cb, ack_reply_cb, NULL, CMD_SET_FLAG);
 	spi_add_cmd(SET_PASSPHRASE_CMD, set_passphrase_cmd_cb, ack_reply_cb, NULL, CMD_SET_FLAG);
 	spi_add_cmd(SET_KEY_CMD, set_key_cmd_cb, ack_reply_cb, NULL, CMD_SET_FLAG);
+	spi_add_cmd(SET_IP_CONFIG_CMD, set_ip_config_cmd_cb, ack_reply_cb, ctx, CMD_SET_FLAG);
+	spi_add_cmd(SET_DNS_CONFIG_CMD, set_dns_config_cmd_cb, ack_reply_cb, ctx, CMD_SET_FLAG);
 	spi_add_cmd(GET_CONN_STATUS_CMD, get_result_cmd_cb, get_reply_cb, NULL, CMD_GET_FLAG);
 	spi_add_cmd(GET_IPADDR_CMD, ack_cmd_cb, get_reply_ipaddr_cb, NULL, CMD_GET_FLAG);
 	spi_add_cmd(GET_MACADDR_CMD, ack_cmd_cb, get_reply_mac_cb, NULL, CMD_GET_FLAG);
@@ -1411,6 +1594,9 @@ void init_spi_cmds() {
 	spi_add_cmd(GET_CLIENT_STATE_TCP_CMD, ack_cmd_cb, get_client_state_tcp_cmd_cb, NULL, CMD_GET_FLAG);
 	spi_add_cmd(GET_FW_VERSION_CMD, ack_cmd_cb, get_firmware_version_cmd_cb, NULL, CMD_GET_FLAG);
 	spi_add_cmd(GET_TEST_CMD, ack_cmd_cb, get_test_cmd_cb, NULL, CMD_GET_FLAG);
+	spi_add_cmd(INSERT_DATABUF_CMD, insert_data_cmd_cb, ack_reply_cb, NULL, CMD_IMM_SET_FLAG);
+	spi_add_cmd(SEND_DATA_UDP_CMD, send_data_udp_cmd_cb, ack_reply_cb, NULL, CMD_SET_FLAG);
+	spi_add_cmd(GET_REMOTE_DATA_CMD, ack_cmd_cb, get_reply_remote_data_cb, NULL, CMD_GET_FLAG);
 }
 
 
@@ -1452,8 +1638,9 @@ bool checkMsgFormat(uint8_t* _recv, int len, int* offset)
 	unsigned char* recv = getStartCmdSeq(_recv, len, offset);
 	if ((recv == NULL)||(recv!=_recv))
 	{
-		if ((enableDebug & INFO_WARN_FLAG)&&(len < 20))	//TODO stamp only short messages wrong
-			dump((char*)_recv, len);
+		DEB_PIN_TRIGGER();
+
+		IF_WARN_VER(DUMP((char*)_recv, len));
 
 		STATSPI_DISALIGN_ERROR();
 
@@ -1468,7 +1655,7 @@ bool checkMsgFormat(uint8_t* _recv, int len, int* offset)
 			paramLenTot = checkMsgParam8(recv);
 		else
 		{
-			DUMP_SPI_DATA(_recv, len);
+			DUMP_SPI(_recv, len);
 			paramLenTot = checkMsgParam16(recv);
 		}
 
@@ -1512,8 +1699,8 @@ void spi_poll(struct netif* netif) {
 			int err = call_reply_cb(buf, &reply[0]);
 			if (err != REPLY_NO_ERR)
 			{
-				DUMP_SPI_DATA(buf, count);
-				DUMP_SPI_DATA(reply, replyCount);
+				DUMP_SPI(buf, count);
+				DUMP_SPI(reply, replyCount);
 			}
 			receivedChars = 0;
 			count = 0;
@@ -1523,8 +1710,7 @@ void spi_poll(struct netif* netif) {
 		{
 			sendError();
 			WARN("%d] Check format msg failed!\n", cmdCorr);
-			if (enableDebug & INFO_WARN_FLAG)
-				dump((char*)_receiveBuffer, receivedChars);
+			IF_WARN_VER(dump((char*)_receiveBuffer, receivedChars));
 			state = SPI_CMD_IDLE;
 			count=0;
 			//mark as buffer used
@@ -1552,11 +1738,7 @@ inline int spi_slaveReceiveInt(volatile avr32_spi_t *spi)
 	int index = 0;
 	int err = SPI_OK;
 	state = SPI_CMD_INPUT;
-
-	if (_receiveBuffer[0] != 0)
-	{
-		STATSPI_OVERRIDE_ERROR();
-	}
+	bool endOfFrame = false;
 
 	do {
 		unsigned int timeout = SPI_TIMEOUT;
@@ -1569,8 +1751,20 @@ inline int spi_slaveReceiveInt(volatile avr32_spi_t *spi)
 				break;
 			}
 		}
-		_receiveBuffer[index] = (spi->rdr >> AVR32_SPI_RDR_RD_OFFSET) & 0x00ff;
+		//DEB_PIN_TG();
+		#if 0
+#ifdef _SPI_STATS_		
+		if (spi->sr & AVR32_SPI_SR_OVRES_MASK)
+		{
+			STATSPI_OVERRIDE_ERROR();
+		}
+#endif	
+#endif					
 		if (err == SPI_OK) {
+			_receiveBuffer[index] = (spi->rdr >> AVR32_SPI_RDR_RD_OFFSET) & 0x00ff;
+			DEB_PIN_UP(2);
+			if ((index==0) && (_receiveBuffer[index] != START_CMD))
+				DEB_PIN_TRIGGER();
 			++index;
 			++receivedChars;
 		}else{
@@ -1585,7 +1779,35 @@ inline int spi_slaveReceiveInt(volatile avr32_spi_t *spi)
 			err = SPI_ERROR_OVERRUN_AND_MODE_FAULT;
 			break;
 		}
-	} while (_receiveBuffer[index - 1] != END_CMD);
+
+		if (_receiveBuffer[index - 1] == END_CMD)
+		{
+			int8_t numParams = 0;
+			int idx = PARAM_LEN_POS+1;
+			bool islen16bit = ((_receiveBuffer[CMD_POS] & DATA_FLAG) == DATA_FLAG);
+			if (index >= idx)
+			{
+				numParams = _receiveBuffer[PARAM_LEN_POS];					
+				while (((index-1) > idx)&&(numParams>0))
+				{
+					if (islen16bit)
+						idx += (_receiveBuffer[idx]<<8) + _receiveBuffer[idx+1]+2;
+					else
+						idx += _receiveBuffer[idx]+1;
+					--numParams;
+				}
+				if (((index-1) == idx) && (numParams == 0))
+					endOfFrame = true;
+			}
+			if (!endOfFrame){
+				WARN("Wrong termination index:%d nParam:%d idx:%d 16bit:%d\n", index, numParams, idx, islen16bit);
+				#ifdef _DEBUG_
+					dump((char*)_receiveBuffer, receivedChars); 
+					while(0);
+				#endif
+			}		
+		}
+	} while (!endOfFrame);
 	return err;
 }
 
@@ -1597,8 +1819,7 @@ __interrupt
 static void spi_int_handler(void)
 {
 	volatile avr32_spi_t *spi = ARD_SPI;
-	//DEB_PIN_DN();
-	//AVAIL_FOR_SPI();
+	DEB_PIN_DN(2);
 	DISABLE_SPI_INT();
 
 	if ((spi->sr & AVR32_SPI_SR_RDRF_MASK) != 0)
@@ -1610,12 +1831,10 @@ static void spi_int_handler(void)
         	startReply=true;
         	++cmdCorr;
         	//maintain disable interrupt to send the reply command
-        	//DEB_PIN_UP();
         	return;
         }
    	}
 	ENABLE_SPI_INT();
-	//DEB_PIN_UP();
 }
 
 inline spi_status_t spi_read8(volatile avr32_spi_t *spi, unsigned char *data)
@@ -1682,7 +1901,7 @@ void initExtInt()
 	  Enable_global_interrupt();
 }
 
-int initSpi()
+int initSpi(void* ctx)
 {
 	volatile avr32_spi_t *spi = &AVR32_SPI0;
 	gpio_map_t spi_piomap = {          \
@@ -1734,7 +1953,7 @@ int initSpi()
 #ifdef _SPI_STATS_
 	initStatSpi();
 #endif
-	init_spi_cmds();
+	init_spi_cmds(ctx);
 
 	memset(_receiveBuffer, 0, sizeof(_receiveBuffer));
 	memset(buf, 0, sizeof(buf));
diff --git a/firmwares/wifishield/wifiHD/src/ard_spi.h b/firmwares/wifishield/wifiHD/src/ard_spi.h
index 628f115..27ec33e 100644
--- a/firmwares/wifishield/wifiHD/src/ard_spi.h
+++ b/firmwares/wifishield/wifiHD/src/ard_spi.h
@@ -37,19 +37,27 @@ typedef enum {
 	CMD_IMM_SET_FLAG = 0x04,
 }cmd_flags;
 
+typedef enum eProtMode {TCP_MODE, UDP_MODE}tProtMode;
+
 #define TIMEOUT_SPI  		200
 #define SPI_ALIGN_ERROR		0xF0
 #define SPI_OVERRIDE_ERROR	0xF1
+#define SPI_TIMEOUT_ERROR	0xF2
 #define DUMMY_DATA			0xFF
 
 typedef int (*cmd_spi_cb_t)(int numParam, char* buf, void* ctx);
 typedef cmd_spi_state_t (*cmd_spi_rcb_t)(char* recv, char* reply, void* ctx, uint16_t* _count);
 
+typedef struct eRemoteClient{
+	uint32_t ipaddr;
+	uint16_t port;
+}tRemoteClient;
+
 void set_result_cmd(int err) ;
 
 void set_result(wl_status_t _status);
 
-int initSpi(void);
+int initSpi(void* ctx);
 
 void initExtInt();
 
@@ -61,10 +69,20 @@ void showTTCPstatus();
 
 int getSock(void * _ttcp);
 
-void* getTTCP(uint8_t sock);
+void* getTTCP(uint8_t sock, uint8_t mode);
+
+void setMapSockMode(uint8_t sock, void* _ttcp, uint8_t _tcp_mode);
+
+void clearMapSockTcp(uint8_t sock, uint8_t mode);
+
+int start_server_tcp(uint16_t port, uint8_t sock, uint8_t protMode);
+
+int start_client_tcp(uint32_t _addr, uint16_t port, uint8_t sock, uint8_t protMode);
+
+void setRemoteClient(uint16_t sock, uint32_t _ipaddr, uint16_t _port);
 
-void clearMapSockTcp(uint8_t sock);
+tRemoteClient* getRemoteClient(uint16_t sock);
 
-int start_server_tcp(uint16_t port, uint8_t sock);
+void getRemoteData(uint8_t sock, uint8_t mode, tRemoteClient* remoteData);
 
 #endif /* ARD_SPI_H_ */
diff --git a/firmwares/wifishield/wifiHD/src/ard_tcp.c b/firmwares/wifishield/wifiHD/src/ard_tcp.c
index b70549a..0a73b20 100644
--- a/firmwares/wifishield/wifiHD/src/ard_tcp.c
+++ b/firmwares/wifishield/wifiHD/src/ard_tcp.c
@@ -27,21 +27,20 @@
 #include "getopt.h"
 #include "ard_utils.h"
 #include "debug.h"
+#include "trace.h"
 
 unsigned int startTime = 0;
 extern bool ifStatus;
-static uint8_t tcp_poll_retries = 0;
-static int isDataSentCount = 0;
-
-bool pending_close = false;
-bool pending_accept = false;
 
 static err_t tcp_data_sent(void *arg, struct tcp_pcb *pcb, u16_t len);
 
-static void atcp_init_pend_flags()
+static void atcp_init_pend_flags(struct ttcp* _ttcp)
 {
-	pending_close = false;
-	pending_accept = false;
+	int i = 0;
+	for (; i<MAX_CLIENT_ACCEPTED; ++i)
+	{
+		if (_ttcp) _ttcp->pending_close[i] = false;
+	}		
 }
 
 /**
@@ -51,17 +50,23 @@ static void ard_tcp_destroy(struct ttcp* ttcp) {
 	err_t err = ERR_OK;
 	DUMP_TCP_STATE(ttcp);
 
-	if (getSock(ttcp)==-1)
+	uint8_t sock = getSock(ttcp);
+	if (sock == -1)
 		WARN("ttcp already deallocated!\n");
 
-	if (ttcp->tpcb) {
-		tcp_arg(ttcp->tpcb, NULL);
-		tcp_sent(ttcp->tpcb, NULL);
-		tcp_recv(ttcp->tpcb, NULL);
-		tcp_err(ttcp->tpcb, NULL);
-		//TEMPORAQARY
-		//err = tcp_close(ttcp->tpcb);
-		INFO_TCP("Closing tpcb: state:0x%x err:%d\n", ttcp->tpcb->state, err);
+	freeAllTcpData(sock);
+	int i = 0;
+	for (; i<MAX_CLIENT_ACCEPTED; ++i)
+	{
+		if (ttcp->tpcb[i]) {
+			tcp_arg(ttcp->tpcb[i], NULL);
+			tcp_sent(ttcp->tpcb[i], NULL);
+			tcp_recv(ttcp->tpcb[i], NULL);
+			tcp_err(ttcp->tpcb[i], NULL);
+			//TEMPORAQARY
+			//err = tcp_close(ttcp->tpcb);
+			INFO_TCP("Closing tpcb: state:0x%x err:%d\n", ttcp->tpcb[i]->state, err);
+		}		
 	}
 
 	if (ttcp->lpcb) {
@@ -76,45 +81,11 @@ static void ard_tcp_destroy(struct ttcp* ttcp) {
 		udp_remove(ttcp->upcb);
 	}
 
-	if (ttcp->payload)
-		free(ttcp->payload);
-
+	FREE_PAYLOAD(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) {
@@ -125,26 +96,27 @@ static void ard_tcp_done(struct ttcp* ttcp, int result) {
 		ttcp->done_cb(ttcp->opaque, result);
 
 	ard_tcp_destroy(ttcp);
-	clearMapSockTcp(getSock(ttcp));
+	clearMapSockTcp(getSock(ttcp), GET_TCP_MODE(ttcp));
 }
 
-static void
-tcp_timeout_cb(void *ctx);
-
 /**
  * Only used in TCP mode. 
  * Will transmit a maximum of pbuf->tot_len bytes. 
  * Called upon connect and when there's space available in the TCP send window
  * 
  */
-static void tcp_send_data(struct ttcp *ttcp) {
+static err_t tcp_send_data_pcb(struct ttcp *ttcp, struct tcp_pcb *pcb) {
 	err_t err = ERR_OK;
-	uint32_t len, orig_len;
+	uint32_t len;
 
-	len = ttcp->left;
-	ttcp->buff_sent = 0;
-	INFO_TCP_VER("left=%d len:%d tcp_sndbuf:%d\n", ttcp->left, len, tcp_sndbuf(ttcp->tpcb));
+	GET_CLIENT_ID(ttcp, pcb);
+	
+	len = ttcp->left[id];
+	ttcp->buff_sent[id] = 0;
 
+	if (len == 0) return ERR_MEM;
+
+	INFO_TCP_VER("left=%d len:%d\n", ttcp->left[id], len);
 
 	/* don't send more than we have in the payload */
 	if (len > ttcp->buflen)
@@ -152,91 +124,95 @@ static void tcp_send_data(struct ttcp *ttcp) {
 
 	/* We cannot send more data than space available in the send
 	 buffer. */
-	if (len > tcp_sndbuf(ttcp->tpcb))
-		len = tcp_sndbuf(ttcp->tpcb);
-
-	orig_len = len;
-	uint8_t count = 0;
-	do {
-		startTime = timer_get_ms();
-		err = tcp_write(ttcp->tpcb, ttcp->payload, len, TCP_WRITE_FLAG_COPY);
-		INFO_TCP_VER("%d) tcp_write %p state:%d len:%d err:%d\n", count++, ttcp->tpcb, ttcp->tpcb->state, len, err);
-		if (err == ERR_MEM)
-		{
-			len /= 2;
-			ttcp->buff_sent = 0;
-		}else if (err == ERR_OK){
-			ttcp->buff_sent = 1;
-			isDataSentCount = 0;
-		}
-	} while (err == ERR_MEM && len > 1);
+	if (len > tcp_sndbuf(pcb))
+		len = tcp_sndbuf(pcb);
 
-	if (err == ERR_OK){
-		//tcp_output(ttcp->tpcb);
-		INFO_TCP_VER("tcp_output: left=%d new left:%d\n",
-				ttcp->left, ttcp->left-len);
-		ttcp->left -= len;
+	IF_TCP(startTime = timer_get_ms());
+	err = tcp_write(pcb, ttcp->payload[id], len, TCP_WRITE_FLAG_COPY);
+	if (err != ERR_OK)
+	{
+		INFO_TCP("tcp_write failed %p state:%d len:%d err:%d\n", 
+				pcb, pcb->state, len, err);
+		ttcp->buff_sent[id] = 0;
+	}else{
+		ttcp->buff_sent[id] = 1;
+		ttcp->left[id] -= len;
 	}
-	else
-		WARN("TTCP [%p-%p]: tcp_write failed err:%d origLen:%d len:%d\n",
-				ttcp, ttcp->tpcb, err, orig_len, len);
-	//
-	//        ttcp->tid = timer_sched_timeout_cb(0, TIMEOUT_ONESHOT,
-	//                                           tcp_timeout_cb, ttcp);
+
+	return err;
 }
 
+
 /**
  * Only used in TCP mode.
  */
 static err_t tcp_connect_cb(void *arg, struct tcp_pcb *tpcb, err_t err) {
-	struct ttcp* ttcp = arg;
+	struct ttcp* _ttcp = arg;
 
-	INFO_TCP("TTCP [%p-%p]: connect %d %d\n", ttcp, tpcb, err, ttcp->tpcb->state);
+	if (_ttcp == NULL) return ERR_ARG;
 
-	_connected =  ( ttcp->tpcb->state == ESTABLISHED) ? 1 : 0;
-    tcp_poll_retries = 0;
+	GET_CLIENT_ID(_ttcp, tpcb);
+	INFO_TCP("TTCP [%p-%p]: connect %d %d\n", _ttcp, tpcb, err, tpcb->state);
 
-	ttcp->start_time = timer_get_ms();
+	_connected =  ( tpcb->state == ESTABLISHED) ? 1 : 0;
+	_ttcp->tcp_poll_retries[id] = 0;
+
+	_ttcp->start_time = timer_get_ms();
 
 	return ERR_OK;
 }
 
 static void cleanSockState_cb(void *ctx) {
-	struct ttcp* ttcp = ctx;
+	struct ttcp* _ttcp = ctx;
+
+	if (_ttcp == NULL) return;
 
-	int sock = getSock(ttcp);
+	int sock = getSock(_ttcp);
 	if (sock != -1)
-		clearMapSockTcp(sock);
-	INFO_TCP("TTCP [%p]: cleanSockState_cb %d\n", ttcp, sock);
+		clearMapSockTcp(sock, GET_TCP_MODE(_ttcp));
+	INFO_TCP("TTCP [%p]: cleanSockState_cb %d\n", _ttcp, sock);
 	_connected = false;
 }
 
-static void cleanSockStateDelayed(void * arg)
-{
-	INFO_TCP("arg %p\n", arg);
-	timer_sched_timeout_cb(1000, TIMEOUT_ONESHOT,
-			cleanSockState_cb, arg);
-}
-
 /** 
  * Only used in TCP mode.
  */
+
+static err_t close_conn_pcb(struct tcp_pcb* tpcb) {
+
+	err_t err = tcp_close(tpcb);
+	if (err== ERR_OK)
+	{
+		tcp_arg(tpcb, NULL);
+		tcp_sent(tpcb, NULL);
+		tcp_recv(tpcb, NULL);
+	}
+
+	INFO_TCP("Closing tpcb[%p]: state:0x%x err:%d\n", tpcb, tpcb->state, err);
+	return err;
+}
+
 static void atcp_conn_err_cb(void *arg, err_t err) {
 	struct ttcp* _ttcp = arg;
 
-	WARN("TTCP [%p]: connection error: %d arg:%p\n",
-			_ttcp, err, arg);
+	WARN("TTCP [%p]: connection error: %d currId:%d\n",
+			_ttcp, err, getCurrClientConnId());
 
 	if (ifStatus == false)
 		printk("Abort connection\n");
-	cleanSockState_cb(_ttcp);
 
-	atcp_init_pend_flags();
+	if (err == ERR_ABRT)
+	{
+		removeNewClientConn(_ttcp, GET_CURR_PCB(_ttcp));
+		FREE_PAYLOAD_ID(_ttcp, getCurrClientConnId());
+	}
 }
 
 static void atcp_conn_cli_err_cb(void *arg, err_t err) {
 	struct ttcp* _ttcp = arg;
 
+	if (_ttcp == NULL) return;
+
 	WARN("TTCP [%p]: connection error: %d arg:%p\n",
 			_ttcp, err, arg);
 
@@ -246,47 +222,54 @@ static void atcp_conn_cli_err_cb(void *arg, err_t err) {
 	if ((_ttcp)&&(err == ERR_ABRT))
 	{
 		WARN("TTCP [%p]: free memory\n", _ttcp);
-		tcp_poll_retries = 0;
 		cleanSockState_cb(_ttcp);
-		if (_ttcp->payload)
-			free(_ttcp->payload);
-		free(_ttcp);
+		// TODO
+		FREE_PAYLOAD(_ttcp);
 	}
 
-	atcp_init_pend_flags();
+	//atcp_init_pend_flags(_ttcp);
 }
 
+static err_t close_conn(struct ttcp *_ttcp, struct tcp_pcb* tpcb) {
+
+	if (_ttcp == NULL) return ERR_MEM;
+	
+	GET_CLIENT_ID(_ttcp, tpcb);
 
-static void close_conn(struct ttcp *_ttcp) {
-	tcp_arg(_ttcp->tpcb, NULL);
-	tcp_sent(_ttcp->tpcb, NULL);
-	tcp_recv(_ttcp->tpcb, NULL);
-	err_t err = tcp_close(_ttcp->tpcb);
-	INFO_TCP("Closing tpcb[%p]: state:0x%x err:%d\n",_ttcp->tpcb, _ttcp->tpcb->state, err);
+	err_t err = close_conn_pcb(_ttcp->tpcb[id]);
 
 	if (err == ERR_MEM)
-		pending_close = true;
+	{
+		WARN("Cannot close id:%d-%p put pending\n", id, _ttcp->tpcb[id]);
+		_ttcp->pending_close[id] = true;
+	}		
 	else{
-		atcp_init_pend_flags();
-		WARN("----------------------\n");
+		_ttcp->pending_close[id] = false;
+		removeNewClientConn(_ttcp, _ttcp->tpcb[id]);
+		FREE_PAYLOAD_ID(_ttcp, id);
+		INFO_TCP("----------------------\n");
 	}
+	return err;
 }
 
 void closeConnections()
 {
-	int i = 0;
-	for (; i<MAX_SOCK_NUM; i++)
+	int ii=0;
+	for (; ii<MAX_MODE_NUM; ii++)
 	{
-		void* p = getTTCP(i);
-		if (p)
+		int i = 0;
+		for (; i<MAX_SOCK_NUM; i++)
 		{
-			ttcp_t* _ttcp = (ttcp_t* )p;
-
-			INFO_TCP("Closing connections tpcb[%p] state:0x%x - lpcb[%p] state: 0x%x\n",
-					_ttcp->tpcb, _ttcp->tpcb->state, _ttcp->lpcb, _ttcp->lpcb->state);
-			//tcp_close(_ttcp->tpcb);
-			ard_tcp_destroy(_ttcp);
-	    	clearMapSockTcp(getSock(_ttcp));
+			void* p = getTTCP(i, ii);
+			if (p)
+			{
+				ttcp_t* _ttcp = (ttcp_t* )p;
+				if (_ttcp->udp == TCP_MODE)
+				{
+					ard_tcp_destroy(_ttcp);
+					clearMapSockTcp(getSock(_ttcp), GET_TCP_MODE(_ttcp));
+				}
+			}
 		}
 	}
 }
@@ -299,7 +282,6 @@ static err_t atcp_recv_cb(void *arg, struct tcp_pcb *pcb, struct pbuf *p,
 	struct ttcp* ttcp = arg;
 
 	if (err == ERR_OK && p != NULL) {
-		INFO_TCP("pcb:%p pbuf: %p err:%d len:%d\n", pcb, p, err, p->tot_len);
 		DATA_LED_ON();
 		/* for print_stats() */
 		ttcp->recved += p->tot_len;
@@ -310,16 +292,17 @@ static err_t atcp_recv_cb(void *arg, struct tcp_pcb *pcb, struct pbuf *p,
 			ttcp->print_cnt++;
 		}
 
-		insert_pBuf(p, ttcp->sock, (void*) pcb);
-		tcp_recved(pcb, p->tot_len);
+		uint8_t* pBufferStore = insert_pBuf(p, ttcp->sock, (void*) pcb);
+		INFO_TCP("sock:%d pcb:%p pbuf:%p err:%d bufStore:%p len:%d\n",
+				ttcp->sock, pcb, p, err, pBufferStore, p->tot_len);
 		pbuf_free(p);
 		DATA_LED_OFF();
 	}
 
 	/* p will be NULL when remote end is done */
 	if (err == ERR_OK && p == NULL) {
-		INFO_TCP("atcp_recv_cb p=NULL\n");
-		close_conn(ttcp);
+		INFO_TCP("atcp_recv_cb p=NULL on sock:%d pcb:%p\n",	ttcp->sock, pcb);
+		close_conn(ttcp, pcb);
 	}
 
 	if (err!=ERR_OK)
@@ -329,81 +312,206 @@ static err_t atcp_recv_cb(void *arg, struct tcp_pcb *pcb, struct pbuf *p,
 
 void ack_recved(void* pcb, int len) {
 	// Comment the call because it is activated on atcp_recv_cb
-	//tcp_recved(pcb, len);
+	INFO_TCP("Received %p len:%d\n", pcb, len);
+	tcp_recved(pcb, len);
 }
 
 static err_t atcp_poll(void *arg, struct tcp_pcb *pcb) {
 	struct ttcp* _ttcp = arg;
-	if ((_ttcp) && (_ttcp->left>0))
-		++tcp_poll_retries;
 
-	if (tcp_poll_retries > 4) {
-		WARN("ARD TCP [%p] arg=%p retries=%d\n",
-				pcb, arg, tcp_poll_retries);
-		tcp_poll_retries = 0;
+	if (_ttcp == NULL) return ERR_ARG;
+	
+	GET_CLIENT_ID(_ttcp, pcb);
+
+	if (_ttcp->left[id]>0)
+		++_ttcp->tcp_poll_retries[id];
+
+	if (_ttcp->tcp_poll_retries[id] > 4) {
+		WARN("ARD TCP [%p] arg=%p retries=%d abort\n",
+				pcb, arg, _ttcp->tcp_poll_retries[id]);
+		_ttcp->tcp_poll_retries[id] = 0;
 		tcp_abort(pcb);
-		atcp_init_pend_flags();
+		_ttcp->pending_close[id] = false;
 	    return ERR_ABRT;
 	}
+	
+	if (pcb)
+		INFO_TCP_POLL("keepAliveCnt:%d keep_idle:%d persist_cnt:%d\n", 
+						pcb->keep_cnt_sent, pcb->keep_idle, pcb->persist_cnt);
+	
+	if (_ttcp->left[id] > 0)
+		INFO_TCP("ARD TCP [%p-%p] arg=%p retries=%d pend.close:%d len:%d\n",
+			(_ttcp)?GET_FIRST_CLIENT_TCP(_ttcp):0, pcb, arg,
+			_ttcp->tcp_poll_retries[id], _ttcp->pending_close[id], (_ttcp)?_ttcp->left[id]:0);
+	tcp_send_data_pcb(_ttcp, pcb);
+
+	if (_ttcp->pending_close[id])
+	{		
+		err_t err = ERR_OK;
+		if (id >=0){
+			err = tcp_close(pcb);
+			if (err == ERR_MEM)
+			{
+				_ttcp->pending_close[id] = true;
+			}
+			else
+			{
+				_ttcp->pending_close[id] = false;
+				removeNewClientConn(_ttcp, _ttcp->tpcb[id]);
+				FREE_PAYLOAD_ID(_ttcp, id);
+				INFO_TCP("----------------------\n");
+			}
+		}	
+		INFO_TCP("ARD TCP [%p-%p] try to close pending:%d err:%d id:%d\n", pcb, 
+			(_ttcp)?GET_FIRST_CLIENT_TCP(_ttcp):0, _ttcp->pending_close[id], err, id);
+	}
+	return ERR_OK;
+}
+
+static err_t atcp_poll_conn(void *arg, struct tcp_pcb *pcb) {
+	struct ttcp* _ttcp = arg;
+
+	if (_ttcp == NULL) return ERR_ARG;
+
+	GET_CLIENT_ID(_ttcp, pcb)
+	
+	INFO_TCP_POLL("ARD TCP [%p-%p] arg=%p retries=%d pend.close:%d conn:%d\n",
+		(_ttcp)?GET_FIRST_CLIENT_TCP(_ttcp):0, pcb, arg,
+		_ttcp->tcp_poll_retries[id], _ttcp->pending_close[id], _connected);
+
+	if (id != NO_VALID_ID)
+	{
+		if (_ttcp->pending_close[id])
+			++(_ttcp->tcp_poll_retries[id]);
+	}
+
+	if (_ttcp->tcp_poll_retries[id] > 8) {
+		WARN("ARD TCP [%p-%p] arg=%p retries=%d\n",
+				pcb, GET_FIRST_CLIENT_TCP(_ttcp), arg, _ttcp->tcp_poll_retries[id]);
+		_ttcp->tcp_poll_retries[id] = 0;
+		tcp_abort(pcb);
+		return ERR_ABRT;
+	}
 
-	WARN("ARD TCP [%p-%p] arg=%p retries=%d pend.close:%d\n", (_ttcp)?_ttcp->tpcb:0, pcb, arg,
-			tcp_poll_retries, pending_close);
-	if (_ttcp) tcp_send_data(_ttcp);
+	if ((_ttcp)&&(_connected)) tcp_send_data_pcb(_ttcp, pcb);
 
-	if (pending_close)
+	if ((id != NO_VALID_ID) && (_ttcp->pending_close[id]))
 	{
 		err_t err = tcp_close(pcb);
 		if (err == ERR_MEM)
 		{
-			pending_close = true;
+			_ttcp->pending_close[id] = true;
 		}
 		else
 		{
-			atcp_init_pend_flags();
+			cleanSockState_cb(_ttcp);
+			FREE_PAYLOAD_ID(_ttcp, id);
+			_ttcp->pending_close[id] = false;
 		}
 
-		INFO_TCP("ARD TCP [%p-%p] try to close pending:%d\n", pcb, (_ttcp)?_ttcp->tpcb:0, pending_close);
+		INFO_TCP("ARD TCP [%p-%p] try to close pending:%d\n", pcb, (_ttcp)?GET_FIRST_CLIENT_TCP(_ttcp):0, _ttcp->pending_close[id]);
 	}
 	return ERR_OK;
 }
 
-static err_t atcp_poll_conn(void *arg, struct tcp_pcb *pcb) {
-	struct ttcp* _ttcp = arg;
-	++tcp_poll_retries;
+int8_t currConnId = 0;
 
-	if (tcp_poll_retries > 8) {
-		WARN("ARD TCP [%p-%p] arg=%p retries=%d\n",
-				pcb, _ttcp->tpcb, arg, tcp_poll_retries);
-		tcp_poll_retries = 0;
-		tcp_abort(pcb);
-		return ERR_ABRT;
-	}
+int8_t getCurrClientConnId() { return currConnId;}
 
-	WARN("ARD TCP [%p-%p] arg=%p retries=%d pend.close:%d conn:%d\n", (_ttcp)?_ttcp->tpcb:0, pcb, arg,
-			tcp_poll_retries, pending_close, _connected);
+int8_t getNewClientConnId(struct ttcp* _ttcp, struct tcp_pcb *newpcb)
+{
+	if (_ttcp != NULL){
+		int i = 0;
+		for (; i<MAX_CLIENT_ACCEPTED; ++i)
+		{
+			int idx = GET_IDX_CONN(i);
 
-	if ((_ttcp)&&(_connected)) tcp_send_data(_ttcp);
+			if (_ttcp->tpcb[idx] == newpcb)
+			{
+				INFO_TCP_VER("ttcp:%p id=%d, tpcb=%p\n", _ttcp, idx, newpcb);
+				return idx;
+			}				
+		}
+	}
+	WARN("No Valid Id for ttcp:%p pcb:%p\n",  _ttcp, newpcb);
+	return NO_VALID_ID;
+}
 
-	if (pending_close)
-	{
-		err_t err = tcp_close(pcb);
-		if (err == ERR_MEM)
+struct tcp_pcb * getFirstClient(struct ttcp* _ttcp, bool verbose)
+{
+	if (_ttcp != NULL){
+		int i = 0;
+		for (; i<MAX_CLIENT_ACCEPTED; ++i)
 		{
-			pending_close = true;
+			int idx = GET_IDX_CONN(i);
+			if (_ttcp->tpcb[idx] != NULL)
+			{
+				if (verbose) INFO_TCP("ttcp:%p id=%d, tpcb=%p\n", _ttcp, idx, _ttcp->tpcb[idx]);
+				currConnId = idx;
+				return _ttcp->tpcb[idx];
+			}				
 		}
-		else
+	}
+	if (verbose) WARN("No Valid client for ttcp:%p\n",  _ttcp);
+	return NULL;
+}
+
+
+int8_t setNewClientConn(struct ttcp* _ttcp, struct tcp_pcb *newpcb, uint8_t id)
+{
+	if ((_ttcp != NULL)&&(id>=0)&&(id<MAX_CLIENT_ACCEPTED)){
+		INFO_TCP("ttcp:%p id=%d, tpcb=%p\n", _ttcp, id, newpcb);
+		_ttcp->tpcb[id] = newpcb;
+		return id;
+	}
+	return NO_VALID_ID;
+}
+
+int8_t insertNewClientConn(struct ttcp* _ttcp, struct tcp_pcb *newpcb)
+{
+	if (_ttcp != NULL){
+		int i = 0;
+		for (; i<MAX_CLIENT_ACCEPTED; ++i)
 		{
-			cleanSockState_cb(_ttcp);
-			if (_ttcp->payload)
-				free(_ttcp->payload);
-			free(_ttcp);
-			pending_close = false;
+			int idx = GET_IDX_CONN(i);
+			if ((_ttcp->tpcb[idx] == NULL)||(_ttcp->tpcb[idx] == newpcb))
+			{
+				INFO_TCP("ttcp:%p id=%d, tpcb=%p\n", _ttcp, idx, newpcb);
+				_ttcp->tpcb[idx] = newpcb;
+				return idx;
+			}
+		}
+	}
+	return NO_VALID_ID;
+}
 
+int8_t removeNewClientConn(struct ttcp* _ttcp, struct tcp_pcb *newpcb)
+{
+	if (_ttcp != NULL){
+		int i = 0;
+		for (; i<MAX_CLIENT_ACCEPTED; ++i)
+		{
+			int idx = GET_IDX_CONN(i);
+			if (_ttcp->tpcb[idx] == newpcb)
+			{
+				INFO_TCP("ttcp:%p id=%d, tpcb=%p\n", _ttcp, idx, newpcb);
+				_ttcp->tpcb[idx] = NULL;
+				return idx;
+			}
 		}
+	}
+	return NO_VALID_ID;
+}
 
-		INFO_TCP("ARD TCP [%p-%p] try to close pending:%d\n", pcb, (_ttcp)?_ttcp->tpcb:0, pending_close);
+bool cleanNewClientConn(struct ttcp* _ttcp)
+{
+	if (_ttcp != NULL){
+		int i = 0;
+		for (; i<MAX_CLIENT_ACCEPTED; ++i)
+			_ttcp->tpcb[i] = NULL;
+		return true;
 	}
-	return ERR_OK;
+	return false;
 }
 
 
@@ -411,25 +519,31 @@ static err_t atcp_poll_conn(void *arg, struct tcp_pcb *pcb) {
  * Only used in TCP mode.
  */
 static err_t atcp_accept_cb(void *arg, struct tcp_pcb *newpcb, err_t err) {
-	struct ttcp* ttcp = arg;
+	struct ttcp* _ttcp = arg;
+
+	if (_ttcp == NULL) return ERR_ARG;
 
-	INFO_TCP("ARD TCP [%p]: accept new [%p]\n", ttcp, newpcb);
+	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);
 
-	if (pending_accept)
-	{
-		WARN("Accepting another connection: %p-%p\n", ttcp->tpcb, newpcb);
-		return ERR_OK;
-	}
-	pending_accept = true;
-    tcp_setprio(newpcb, TCP_PRIO_MIN);
-    tcp_poll_retries = 0;
-	ttcp->tpcb = newpcb;
-	tcp_recv(ttcp->tpcb, atcp_recv_cb);
-	tcp_err(ttcp->tpcb, atcp_conn_err_cb);
-	tcp_poll(ttcp->tpcb, atcp_poll, 4);
-
-	ttcp->start_time = timer_get_ms();
+	int8_t id = insertNewClientConn(_ttcp, newpcb);
+
+	ASSERT((_ttcp->payload[id]==NULL), "payload not freed!");
+	_ttcp->payload[id] = malloc(_ttcp->buflen);
+	INFO_TCP("Alloc payload %d-%p\n", id, _ttcp->payload[id]);
+	if (_ttcp->payload[id] == NULL) {
+		WARN("TTCP [%p]: could not allocate payload\n", _ttcp);
+		return -1;
+	}
+	tcp_arg(_ttcp->tpcb[id], _ttcp);
+	tcp_recv(_ttcp->tpcb[id], atcp_recv_cb);
+	tcp_err(_ttcp->tpcb[id], atcp_conn_err_cb);
+	tcp_poll(_ttcp->tpcb[id], atcp_poll, 4);
+	// Copy the pointer to ttcp also to TRANSMIT mode for the clients connected to the server
+	int _sock = getSock(_ttcp);
+	if ((_sock != -1)&&(IS_VALID_SOCK(_sock)))
+		setMapSockMode(_sock, _ttcp, TTCP_MODE_TRANSMIT);
+	_ttcp->start_time = timer_get_ms();
 	return ERR_OK;
 }
 
@@ -439,55 +553,54 @@ static err_t atcp_accept_cb(void *arg, struct tcp_pcb *newpcb, err_t err) {
 static int atcp_start(struct ttcp* ttcp) {
 	err_t err = ERR_OK;
 
-	ttcp->tpcb = tcp_new();
-	if (ttcp->tpcb == NULL) {
+	struct tcp_pcb * p = tcp_new();
+	
+	if (p == NULL) {
 		WARN("TTCP [%p]: could not allocate pcb\n", ttcp);
 		return -1;
 	}
 
-	ttcp->payload = malloc(ttcp->buflen);
-	if (ttcp->payload == NULL) {
-		WARN("TTCP [%p]: could not allocate payload\n", ttcp);
-		return -1;
-	}
-
-	tcp_arg(ttcp->tpcb, ttcp);
-	atcp_init_pend_flags();
+	currConnId = 0;
+	tcp_arg(p, ttcp);
+	atcp_init_pend_flags(ttcp);
 
 	if (ttcp->mode == TTCP_MODE_TRANSMIT) {
-		tcp_err(ttcp->tpcb, atcp_conn_cli_err_cb);
-		tcp_recv(ttcp->tpcb, atcp_recv_cb);
-		tcp_sent(ttcp->tpcb, tcp_data_sent);
-		tcp_poll(ttcp->tpcb, atcp_poll_conn, 4);
+		int8_t id = insertNewClientConn(ttcp, p);
+		ttcp->payload[id] = malloc(ttcp->buflen);
+		INFO_TCP("Alloc payload %d-%p\n", id, ttcp->payload[id]);
+		if (ttcp->payload[id] == NULL) {
+			WARN("TTCP [%p]: could not allocate payload\n", ttcp);
+			return -1;
+		}
+		
+		struct tcp_pcb * pcb = p;
+		tcp_err(pcb, atcp_conn_cli_err_cb);
+		tcp_recv(pcb, atcp_recv_cb);
+		tcp_sent(pcb, tcp_data_sent);
+		tcp_poll(pcb, atcp_poll_conn, 4);
 		_connected = false;
-		INFO_TCP("[tpcb]-%p payload:%p\n", ttcp->tpcb, ttcp->payload);
+		INFO_TCP("[tpcb]-%p payload:%p\n", pcb, ttcp->payload[id]);
 		DUMP_TCP_STATE(ttcp);
-		if (tcp_connect(ttcp->tpcb, &ttcp->addr, ttcp->port, tcp_connect_cb)
+		if (tcp_connect(pcb, &ttcp->addr, ttcp->port, tcp_connect_cb)
 				!= ERR_OK) {
 			WARN("TTCP [%p]: tcp connect failed\n", ttcp);
 			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);
+		INFO_TCP("BEFORE BIND ttcp:%p lpcb:%p pcb:%p\n", ttcp, ttcp->lpcb, GET_FIRST_CLIENT_TCP(ttcp));
 
-		err = tcp_bind(ttcp->tpcb, IP_ADDR_ANY, ttcp->port);
+		err = tcp_bind(p, IP_ADDR_ANY, ttcp->port);
 		if (err != ERR_OK){
 			WARN("TTCP [%p]: bind failed err=%d Port already used\n", ttcp, err);
 			return -1;
 		}
 
-		ttcp->lpcb = tcp_listen(ttcp->tpcb);
+		ttcp->lpcb = tcp_listen(p);
 		if (ttcp->lpcb == NULL) {
 			WARN("TTCP [%p]: listen failed\n", ttcp);
 			return -1;
 		}
-		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);
@@ -496,130 +609,64 @@ static int atcp_start(struct ttcp* ttcp) {
 	return 0;
 }
 
-static void
-udp_send_data(struct ttcp* ttcp);
-
-/**
- * Only used in UDP mode. Scheduled after data has been sent in udp_send_data()
- * if we have more data to send.
- */
-static void udp_timeout_cb(void *ctx) {
-	struct ttcp* ttcp = ctx;
-	udp_send_data(ttcp);
-}
-
-static int udp_send_bytes(struct ttcp* ttcp, uint32_t len) {
-	struct pbuf* p = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM);
-	if (p == NULL) {
-		WARN("TTCP [%p]: could not allocate pbuf\n", ttcp);
-		return -1;
-	}
-
-	if (udp_send(ttcp->upcb, p) != ERR_OK) {
-		WARN("TTCP [%p]: udp_send() failed\n", ttcp);
-		pbuf_free(p);
-		return -1;
-	}
-
-	pbuf_free(p);
-	return 0;
-}
-
-/**
- * Only used in UDP mode. First call will send the start marker. When all 
- * ttcp data has been sent, a number of end markers will be sent. After
- * end marker transmission, this function will complete the ttcp process.
- */
-static void udp_send_data(struct ttcp* ttcp) {
-	/* send start marker first time */
-	if (!ttcp->udp_started) {
-		if (udp_send_bytes(ttcp, 4) == 0) {
-			ttcp->udp_started = 1;
-			ttcp->start_time = timer_get_ms();
-		}
-	}
-
-	/* normal case */
-	else if (ttcp->left) {
-		/* send data */
-		if (udp_send_bytes(ttcp, ttcp->buflen) == 0)
-			ttcp->left -= ttcp->buflen;
-	}
-
-	/* end marker? */
-	else if (ttcp->left == 0 && ttcp->udp_end_marker_left) {
-		if (udp_send_bytes(ttcp, 4) == 0)
-			ttcp->udp_end_marker_left--;
-	}
-
-	/* all end markers sent */
-	else if (ttcp->left == 0) {
-		ard_tcp_done(ttcp, 0);
-		return;
-	}
-
-	ttcp->tid
-			= timer_sched_timeout_cb(0, TIMEOUT_ONESHOT, udp_timeout_cb, ttcp);
-}
-
 /**
  * Only used in UDP mode. Will finalize the ttcp process when an end marker
  * is seen.
  */
-static void udp_recv_cb(void *arg, struct udp_pcb *upcb, struct pbuf *p,
+static void audp_recv_cb(void *arg, struct udp_pcb *upcb, struct pbuf *p,
 		struct ip_addr *addr, u16_t port) {
 	struct ttcp* ttcp = arg;
 
-	/* got start marker? we might lose this so if we get it just reset
-	 * the timer
-	 */
-	if (!ttcp->udp_started && p->tot_len <= 4) {
-		ttcp->start_time = timer_get_ms();
-		ttcp->udp_started = 1;
-		goto out;
-	}
-
-	/* after receiving at least 1 byte, check end marker
-	 * don't check udp_started since we might have lost the start marker
-	 */
-	if (ttcp->recved && p->tot_len <= 4) {
-		ard_tcp_done(ttcp, 0);
-		goto out;
-	}
-
 	/* for print_stats() */
 	ttcp->recved += p->tot_len;
+	DUMP(p->payload,p->tot_len);
 	if (ttcp->verbose) {
 		printk(".");
 		if (ttcp->print_cnt % 80 == 0)
 			printk("\n");
 		ttcp->print_cnt++;
 	}
+	INFO_TCP("UDP Insert %p sock:%d addr:%s port:%d\n", p, ttcp->sock,
+			ip2str(*addr), port);
+	insert_pBuf(p, ttcp->sock, (void*) upcb);
+	setRemoteClient(ttcp->sock, addr->addr, port);
 
-	out: pbuf_free(p);
+	pbuf_free(p);
 }
 
 /**
  * Start UDP transfer.
  */
 static int udp_start(struct ttcp* ttcp) {
+	err_t err = ERR_OK;
 	ttcp->udp_end_marker_left = 5;
 	ttcp->upcb = udp_new();
 	if (ttcp->upcb == NULL) {
-		printk("TTCP [%p]: could not allocate pcb\n", ttcp);
+		WARN("TTCP [%p]: could not allocate pcb\n", ttcp);
 		return -1;
 	}
 
+	INFO_TCP("%s, upcb:%p %s:%d\n", __FUNCTION__, ttcp->upcb, ip2str(ttcp->addr), ttcp->port);
 	if (ttcp->mode == TTCP_MODE_TRANSMIT) {
-		if (udp_connect(ttcp->upcb, &ttcp->addr, ttcp->port) != ERR_OK) {
-			printk("TTCP [%p]: udp connect failed\n", ttcp);
+		if (udp_connect(ttcp->upcb, &(ttcp->addr), ttcp->port) != ERR_OK) {
+			WARN("TTCP [%p]: udp connect failed\n", ttcp);
 			return -1;
 		}
-		udp_send_data(ttcp);
+		udp_recv(ttcp->upcb, audp_recv_cb, ttcp);
 	} else {
-		udp_recv(ttcp->upcb, udp_recv_cb, ttcp);
+		/* bind to any IP address on port specified */
+		err = udp_bind(ttcp->upcb, IP_ADDR_ANY, ttcp->port);
+		if  (err!= ERR_OK) {
+			WARN("TTCP [%p]: bind failed err=%d Port already used\n", ttcp, err);
+		    return -1;
+		}
+		// clear remote client data
+		setRemoteClient(ttcp->sock, 0, 0);
+		udp_recv(ttcp->upcb, audp_recv_cb, ttcp);
 	}
-
+	INFO_TCP("%s, loc:0x%x-%d rem:0x%x-%d\n", __FUNCTION__, 
+		 ttcp->upcb->local_ip.addr, ttcp->upcb->local_port, 
+		 ttcp->upcb->remote_ip.addr, ttcp->upcb->remote_port);
 	return 0;
 }
 
@@ -660,12 +707,12 @@ int ard_tcp_start(struct ip_addr addr, uint16_t port, void *opaque,
 	ttcp->port = port;
 	ttcp->nbuf = nbuf;
 	ttcp->mode = mode;
-	ttcp->left = nbuf * buflen;
 	ttcp->done_cb = done_cb;
 	ttcp->opaque = opaque;
 	ttcp->udp = udp;
 	ttcp->verbose = verbose;
 	ttcp->buflen = buflen;
+	cleanNewClientConn(ttcp);
 
 	if (ttcp->udp)
 		status = udp_start(ttcp);
@@ -677,16 +724,14 @@ int ard_tcp_start(struct ip_addr addr, uint16_t port, void *opaque,
 		goto fail;
 	}
 	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");
+			((ttcp->udp==1)?(void*)ttcp->upcb:GET_FIRST_CLIENT_TCP(ttcp)), ttcp->nbuf, ttcp->buflen, 
+			ttcp->port, ProtMode2Str(ttcp->udp), Mode2Str(ttcp->mode));
 
 	*_ttcp = (void*) ttcp;
 	ttcp->sock = sock;
-	ttcp->buff_sent = 1;
 
 	return 0;
 
-	//fail: ard_tcp_abort(ttcp);
 	fail: ard_tcp_destroy(ttcp);
 	return -1;
 }
@@ -699,18 +744,22 @@ void ard_tcp_stop(void* ttcp) {
 		return;
 	}
 	if (_ttcp->mode == TTCP_MODE_TRANSMIT) {
-		INFO_TCP("Destroy TCP connection...state:%d\n", _ttcp->tpcb->state);
+		int i = getCurrClientConnId();
 		ard_tcp_destroy(_ttcp);
-    	clearMapSockTcp(getSock(_ttcp));
-    	tcp_poll_retries = 0;
+    	clearMapSockTcp(getSock(_ttcp), GET_TCP_MODE(_ttcp));
+    	_ttcp->tcp_poll_retries[i] = 0;
 	}else{
-		INFO_TCP("Closing connection...state:%d\n", _ttcp->tpcb->state);
 		DUMP_TCP_STATE(_ttcp);
-		if ((_ttcp)&&(_ttcp->tpcb)&&(_ttcp->tpcb->state!=LAST_ACK)&&(_ttcp->tpcb->state!=CLOSED))
+
+		int i = getCurrClientConnId();
+		if ((_ttcp)&&(_ttcp->tpcb[i])&&(_ttcp->tpcb[i]->state!=LAST_ACK)&&(_ttcp->tpcb[i]->state!=CLOSED))
 		{
-			close_conn(_ttcp);
+			// Flush all the data
+			err_t err=tcp_output(_ttcp->tpcb[i]);
+			INFO_TCP("flush data: tpcb:%p err:%d\n", _ttcp->tpcb[i], err);
+			// if any socket  cannot be close stop the close connection
+			close_conn(_ttcp, _ttcp->tpcb[i]);
 		}
-		pending_accept = false;
 	}
 }
 
@@ -719,15 +768,22 @@ uint8_t getStateTcp(void* p, bool client) {
 
 	if (ifStatus == false)
 		return CLOSED;
-	if ((_ttcp != NULL) && (_ttcp->tpcb != NULL)) {
-		//DUMP_TCP_STATE(_ttcp);
+	struct tcp_pcb * pcb = GET_FIRST_CLIENT_TCP_NV(_ttcp);
+	if ((_ttcp != NULL) && ((pcb != NULL) || (client==0))) {
+		IF_SPI_POLL(DUMP_TCP_STATE(_ttcp));
 		if (client)
-			return _ttcp->tpcb->state;
+		{
+			if ((pcb->state != ESTABLISHED)&&(pcb->state != CLOSED))
+				DUMP_TCP_STATE(_ttcp);
+			return pcb->state;
+		}			
 		else
+		{
 			return _ttcp->lpcb->state;
+		}			
 	} else {
-		INFO_TCP_VER("TCP not initialized ttcp:%p tpcb:%p lpcb:%p\n",
-				_ttcp, ((_ttcp)?_ttcp->tpcb:0), ((_ttcp)?_ttcp->lpcb:0));
+		WARN_POLL("TCP not initialized ttcp:%p tpcb:%p lpcb:%p\n",
+				_ttcp, ((_ttcp)?pcb:0), ((_ttcp)?_ttcp->lpcb:0));
 	}
 	return CLOSED;
 }
@@ -743,9 +799,9 @@ uint8_t getModeTcp(void* p) {
 uint8_t isDataSent(void* p) {
 	struct ttcp *_ttcp = (struct ttcp *)p;
 
-	if ((_ttcp)&&(!_ttcp->buff_sent))
+	int8_t id = getCurrClientConnId();
+	if ((_ttcp)&&(!_ttcp->buff_sent[id]))
 	{
-		INFO_TCP_VER("%d) Wait to send data\n", ++isDataSentCount);
 		return 0;
 	}
 
@@ -759,26 +815,24 @@ static err_t tcp_data_sent(void *arg, struct tcp_pcb *pcb, u16_t len) {
 
 	_ttcp = arg;
 
-	tcp_poll_retries = 0;
-	if (_ttcp) _ttcp->buff_sent = 1;
-
+	if (_ttcp == NULL) return ERR_ARG;
 
-	INFO_TCP("Packet sent pcb:%p len:%d dur:%d left:%d count:%d\n", pcb, len, timer_get_ms() - startTime,
-			(_ttcp)?(_ttcp->left):0, isDataSentCount);
+	GET_CLIENT_ID(_ttcp, pcb);
+	_ttcp->tcp_poll_retries[id] = 0;
+	_ttcp->buff_sent[id] = 1;
 
-	isDataSentCount = 0;
+	INFO_TCP("Packet sent pcb:%p len:%d dur:%d left:%d\n", pcb, len, timer_get_ms() - startTime,
+			(_ttcp)?(_ttcp->left[id]):0);
 
-	if ((_ttcp)&&(_ttcp->left > 0)) {
-		tcp_send_data(_ttcp);
+	if ((_ttcp)&&(_ttcp->left[id] > 0)) {
+		tcp_send_data_pcb(_ttcp, pcb);
 	}
 
 	return ERR_OK;
 }
 
-int sendTcpData(void* p, uint8_t* buf, uint16_t len) {
-	INFO_TCP("buf:%p len:%d\n", buf, len);
-	DUMP_TCP(buf,len);
-
+int sendTcpData(void* p, uint8_t* buf, uint16_t len) 
+{
 	struct ttcp* _ttcp = (struct ttcp*) p;
 
 	if (_ttcp==NULL)
@@ -786,25 +840,25 @@ int sendTcpData(void* p, uint8_t* buf, uint16_t len) {
 		WARN("ttcp == NULL!\n");
 		return WL_FAILURE;
 	}
+	
+	struct tcp_pcb * pcb = GET_FIRST_CLIENT_TCP_NV(_ttcp);
+	GET_CLIENT_ID(_ttcp, pcb);
+	
+	INFO_TCP_VER("ttcp:%p pcb:%p buf:%p len:%d\n", _ttcp, pcb, buf, len);
+	DUMP_TCP(buf,len);
+	IF_TCP_VER(DUMP_TCP_STATE(_ttcp));
 
-	INFO_TCP_VER("CLI> p=%p _ttcp=%p state(tpcb):%d state(lpcb):%d\n",
-	    		    				p, ((struct ttcp*) p)->tpcb,
-	    		    				((struct ttcp*) p)->tpcb->state,
-	    		    				((struct ttcp*) p)->lpcb->state);
-
-	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) {
+	if ((_ttcp != NULL) && (pcb != NULL) &&
+			(buf != NULL) && (len != 0) && (_ttcp->payload[id] != NULL)) {
+		if (pcb->state == ESTABLISHED || pcb->state == CLOSE_WAIT ||
+			pcb->state == SYN_SENT || pcb->state == SYN_RCVD) {
 
-		memcpy(_ttcp->payload, buf, len);
-		_ttcp->payload[len]='\0';
-		INFO_TCP_VER("%s\n", _ttcp->payload);
-		_ttcp->left = len;
-		tcp_sent(_ttcp->tpcb, tcp_data_sent);
-		tcp_send_data(_ttcp);
+		memcpy(_ttcp->payload[id], buf, len);
+		_ttcp->payload[id][len]='\0';
+		INFO_TCP_VER("'%s'\n", _ttcp->payload[id]);
+		_ttcp->left[id] = len;
+		tcp_sent(pcb, tcp_data_sent);
+		tcp_send_data_pcb(_ttcp, pcb);
 
 		return WL_SUCCESS;
 		}
@@ -813,6 +867,34 @@ int sendTcpData(void* p, uint8_t* buf, uint16_t len) {
 	return WL_FAILURE;
 }
 
+int sendUdpData(void* ttcp, uint8_t* buf, uint16_t len) {
+	struct ttcp* _ttcp = (struct ttcp*) ttcp;
+	if ((_ttcp != NULL) && (buf != NULL) && (len != 0))
+	{
+		INFO_TCP("buf:%p len:%d\n", buf, len);
+		DUMP_TCP(buf,len);
+	}else{
+		return WL_FAILURE;
+	}
+
+	struct pbuf* p = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM);
+	if (p == NULL) {
+		WARN("TTCP [%p]: could not allocate pbuf\n", ttcp);
+		return WL_FAILURE;
+	}
+	memcpy(p->payload, buf, len);
+	if (udp_send(_ttcp->upcb, p) != ERR_OK) {
+		WARN("TTCP [%p]: udp_send() failed\n", _ttcp);
+		pbuf_free(p);
+		return WL_FAILURE;
+	}
+
+	pbuf_free(p);
+	return WL_SUCCESS;
+}
+
+
+
 char
 		usage[] =
 				"Usage: ttcp -t/-r [-options] host\n\
diff --git a/firmwares/wifishield/wifiHD/src/ard_tcp.h b/firmwares/wifishield/wifiHD/src/ard_tcp.h
index 010b976..078e0b0 100644
--- a/firmwares/wifishield/wifiHD/src/ard_tcp.h
+++ b/firmwares/wifishield/wifiHD/src/ard_tcp.h
@@ -15,6 +15,41 @@ typedef void (ard_tcp_done_cb_t)(void *opaque, int result);
 
 #define TTCP_MODE_TRANSMIT          0
 #define TTCP_MODE_RECEIVE           1
+#define MAX_MODE_NUM 				2
+
+#define GET_TCP_MODE(X)	((X!=NULL)?((struct ttcp*)(X))->mode:0)
+#define IS_VALID_SOCK(SOCK) ((SOCK>=0)&&(SOCK<MAX_SOCK_NUM))
+#define IS_UDP_SOCK(SOCK)	((getTTCP(SOCK, TTCP_MODE_RECEIVE)!=NULL)?((struct ttcp*)(getTTCP(SOCK, TTCP_MODE_RECEIVE)))->udp:0)
+
+// Maximum number of client connection accepted by server
+#define MAX_CLIENT_ACCEPTED			4
+#define NO_VALID_ID					0xff
+
+#define GET_FIRST_CLIENT_TCP(TTCP)		getFirstClient(TTCP, 1)
+#define GET_FIRST_CLIENT_TCP_NV(TTCP)	getFirstClient(TTCP, 0)
+#define GET_CLIENT_TCP(TTCP,ID)	(((TTCP!=NULL)&&(ID>=0)&&(ID<MAX_CLIENT_ACCEPTED))?TTCP->tpcb[ID] : NULL)
+#define GET_CLIENT_ID(TTCP, PCB)	uint8_t id = NO_VALID_ID; do {		\
+		id = getNewClientConnId(TTCP, PCB);	\
+		if (id == NO_VALID_ID) return ERR_MEM;		\
+	}while(0);	
+#define GET_IDX_CONN(I) ((I+currConnId)<MAX_CLIENT_ACCEPTED ? (I+currConnId) : (I+currConnId-MAX_CLIENT_ACCEPTED))
+#define GET_CURR_PCB(TTCP) GET_CLIENT_TCP(TTCP,getCurrClientConnId())
+
+#define FREE_PAYLOAD(TTCP) do { \
+	int id = getCurrClientConnId(); \
+	INFO_TCP("Freeing payload %d-%p\n", id, TTCP->payload[id]); \
+	if (TTCP->payload[id]) { \
+		free(TTCP->payload[id]); \
+		TTCP->payload[id] = NULL; } \
+}while(0);		
+
+#define FREE_PAYLOAD_ID(TTCP,ID) do { \
+	INFO_TCP("Freeing payload %d-%p\n", ID, TTCP->payload[ID]); \
+	if (TTCP->payload[ID]) { \
+		free(TTCP->payload[ID]); \
+		TTCP->payload[ID] = NULL; } \
+}while(0);	
+
 
 typedef struct ttcp {
 
@@ -26,12 +61,12 @@ typedef struct ttcp {
 	int verbose; /* -v */
 	int udp; /* -u */
 	uint8_t sock;
-	uint8_t buff_sent;
+	uint8_t buff_sent[MAX_CLIENT_ACCEPTED];
 
 	/* common */
 	uint16_t print_cnt;
 	uint32_t start_time;
-	uint32_t left;
+	uint32_t left[MAX_CLIENT_ACCEPTED];
 	uint32_t recved;
 	ard_tcp_done_cb_t* done_cb;
 	void* opaque;
@@ -39,9 +74,11 @@ typedef struct ttcp {
 	uint32_t tid;
 
 	/* TCP specific */
-	struct tcp_pcb* tpcb;
+	struct tcp_pcb* tpcb[MAX_CLIENT_ACCEPTED];
 	struct tcp_pcb* lpcb;
-	char* payload;
+	char* payload[MAX_CLIENT_ACCEPTED];
+	uint8_t tcp_poll_retries[MAX_CLIENT_ACCEPTED];
+	bool pending_close[MAX_CLIENT_ACCEPTED];
 
 	/* UDP specific */
 	int udp_started;
@@ -62,10 +99,26 @@ uint8_t getModeTcp(void* p);
 
 int sendTcpData(void* p, uint8_t* buf, uint16_t len);
 
+int sendUdpData(void* p, uint8_t* buf, uint16_t len);
+
 uint8_t isDataSent(void* p );
 
 cmd_state_t cmd_ttcp(int argc, char* argv[], void* ctx);
 
+int8_t setNewClientConn(struct ttcp* _ttcp, struct tcp_pcb *newpcb, uint8_t id);
+
+int8_t insertNewClientConn(struct ttcp* _ttcp, struct tcp_pcb *newpcb);
+
+int8_t removeNewClientConn(struct ttcp* _ttcp, struct tcp_pcb *newpcb);
+
+bool cleanNewClientConn(struct ttcp* _ttcp);
+
+int8_t getNewClientConnId(struct ttcp* _ttcp, struct tcp_pcb *newpcb);
+
+int8_t getCurrClientConnId();
+
+struct tcp_pcb * getFirstClient(struct ttcp* _ttcp, bool verbose);
+
 void closeConnections();
 
 #endif
diff --git a/firmwares/wifishield/wifiHD/src/ard_utils.c b/firmwares/wifishield/wifiHD/src/ard_utils.c
index 9c31f40..c2937d8 100644
--- a/firmwares/wifishield/wifiHD/src/ard_utils.c
+++ b/firmwares/wifishield/wifiHD/src/ard_utils.c
@@ -4,38 +4,144 @@
  *  Created on: Jul 4, 2010
  *      Author: mlf by Metodo2 srl
  */
-#undef _APP_DEBUG_
+//#define _APP_DEBUG_
 
 #include "lwip/pbuf.h"
 #include "wifi_spi.h"
 #include "ard_utils.h"
 #include "debug.h"
+#include "ard_spi.h"
+#include "ard_tcp.h"
 
 #define MAX_PBUF_STORED	30
 
 tData pBufStore[MAX_PBUF_STORED][MAX_SOCK_NUM];
 
-unsigned char headBuf = 0;
-unsigned char tailBuf = 0;
+unsigned char headBuf[MAX_SOCK_NUM] = {0};
+unsigned char tailBuf[MAX_SOCK_NUM] = {0};
 
-#define IS_BUF_AVAIL() (tailBuf!=headBuf)
-#define IS_BUF_EMPTY() ((tailBuf == 0) && (headBuf == 0))
+#define IS_BUF_AVAIL(x) (tailBuf[x] != headBuf[x])
+#define IS_BUF_EMPTY(x) ((tailBuf[x] == 0) && (headBuf[x] == 0))
 
 void init_pBuf()
 {
 	memset(pBufStore, 0, sizeof(pBufStore));
 }
 
-void insert_pBuf(struct pbuf* q, uint8_t sock, void* _pcb)
+uint8_t* insertBuf(uint8_t sock, uint8_t* buf, uint16_t len)
+{
+	DUMP(buf,len);
+	if (sock>= MAX_SOCK_NUM)
+	{
+		WARN("Sock out of range: sock=%d", sock);
+		return NULL;
+	}		
+	if (pBufStore[headBuf[sock]][sock].data != NULL)
+	{
+		WARN("Overwriting buffer %p idx:%d!\n", pBufStore[headBuf[sock]][sock].data, headBuf[sock]);
+		// to avoid memory leak free the oldest buffer
+		freetDataIdx(headBuf[sock], sock);
+	}
+
+	u8_t* p = (u8_t*)calloc(len,sizeof(u8_t));
+    if(p != NULL) {
+    	memcpy(p, buf, len);
+
+    	pBufStore[headBuf[sock]][sock].data = p;
+    	pBufStore[headBuf[sock]][sock].len = len;
+    	pBufStore[headBuf[sock]][sock].idx = 0;
+    	pBufStore[headBuf[sock]][sock].pcb = getTTCP(sock, TTCP_MODE_TRANSMIT);
+    	headBuf[sock]++;
+
+    	if (headBuf[sock] == MAX_PBUF_STORED)
+    		headBuf[sock] = 0;
+    	if (headBuf[sock] == tailBuf[sock])
+    	{
+    		WARN("Avoid to Overwrite data [%d-%d]!\n", headBuf[sock], tailBuf[sock]);
+    		if (headBuf[sock] != 0)
+    			--headBuf[sock];
+    		else
+    			headBuf[sock] = MAX_PBUF_STORED-1;
+    	}
+    	INFO_UTIL("Insert[%d]: %p:%d-%d [%d,%d]\n", sock, p, len, p[0], headBuf[sock], tailBuf[sock]);
+    }
+    return p;
+}
+
+
+uint16_t calcMergeLen(uint8_t sock)
+{
+	uint16_t len = 0;
+
+	unsigned char index = tailBuf[sock];
+	do {
+	if (pBufStore[index][sock].data != NULL)
+	{
+		len += pBufStore[index][sock].len;
+		len -= pBufStore[index][sock].idx;
+		INFO_UTIL_VER(" [%d]: len:%d idx:%d tot:%d\n", sock, pBufStore[index][sock].len, pBufStore[index][sock].idx, len);
+	}
+	++index;
+	if (index == MAX_PBUF_STORED)
+		index = 0;
+	}while (index!=headBuf[sock]);
+	return len;
+}
+
+uint16_t clearBuf(uint8_t sock)
+{
+	uint16_t len = 0;
+
+	unsigned char index = tailBuf[sock];
+	do {
+	if (pBufStore[index][sock].data != NULL)
+	{
+		freetDataIdx(index,sock);
+	}
+	++index;
+	if (index == MAX_PBUF_STORED)
+		index = 0;
+	}while (index!=headBuf[sock]);
+	tailBuf[sock]=index;
+	return len;
+}
+
+uint8_t* mergeBuf(uint8_t sock, uint8_t** buf, uint16_t* _len)
+{
+	uint16_t len = calcMergeLen(sock);
+	uint8_t* p = (u8_t*)calloc(len,sizeof(u8_t));
+	uint8_t* _p = p;
+	if(p != NULL) {
+		unsigned char index = tailBuf[sock];
+		do {
+		if (pBufStore[index][sock].data != NULL)
+		{
+			memcpy(p, pBufStore[index][sock].data, pBufStore[index][sock].len);
+			p += pBufStore[index][sock].len;
+		}
+		++index;
+		if (index == MAX_PBUF_STORED)
+			index = 0;
+		}while (index!=headBuf[sock]);
+	}
+	DUMP(_p,len);
+	if (buf != NULL)
+		*buf = _p;
+	if (_len != NULL)
+		*_len = len;
+	return _p;
+}
+
+uint8_t* insert_pBuf(struct pbuf* q, uint8_t sock, void* _pcb)
 {
 	if (q == NULL)
-		return;
+		return NULL;
 
-	if (pBufStore[headBuf][sock].data != NULL)
+	if (pBufStore[headBuf[sock]][sock].data != NULL)
 	{
-		WARN("Overwriting buffer %p idx:%d!\n", pBufStore[headBuf][sock].data, headBuf);
+		WARN("Overwriting buffer %p idx:%d!\n", pBufStore[headBuf[sock]][sock].data, headBuf[sock]);
 		// to avoid memory leak free the oldest buffer
-		freetDataIdx(headBuf, sock);
+		freetDataIdx(headBuf[sock], sock);
 	}
 
 	u8_t* p = (u8_t*)calloc(q->tot_len,sizeof(u8_t));
@@ -44,32 +150,55 @@ void insert_pBuf(struct pbuf* q, uint8_t sock, void* _pcb)
     	  WARN("pbuf_copy_partial failed: src:%p, dst:%p, len:%d\n", q, p, q->tot_len);
     	  free(p);
     	  p = NULL;
-    	  return;
+    	  return p;
       }
 
-      pBufStore[headBuf][sock].data = p;
-      pBufStore[headBuf][sock].len = q->tot_len;
-      pBufStore[headBuf][sock].idx = 0;
-      pBufStore[headBuf][sock].pcb = _pcb;
-      headBuf++;
-
-  	  if (headBuf == MAX_PBUF_STORED)
-  		headBuf = 0;
-  	  if (headBuf == tailBuf)
-  		  WARN("Overwriting data [%d-%d]!\n", headBuf, tailBuf);
-  	  INFO_UTIL("Insert: %p:%d-%d [%d,%d]\n", p, q->tot_len, p[0], headBuf, tailBuf);
+      pBufStore[headBuf[sock]][sock].data = p;
+      pBufStore[headBuf[sock]][sock].len = q->tot_len;
+      pBufStore[headBuf[sock]][sock].idx = 0;
+      pBufStore[headBuf[sock]][sock].pcb = _pcb;
+      headBuf[sock]++;
+
+  	  if (headBuf[sock] == MAX_PBUF_STORED)
+  		headBuf[sock] = 0;
+  	  if (headBuf[sock] == tailBuf[sock])
+  	  {
+  		  WARN("Avoid to Overwrite data [%d-%d]!\n", headBuf[sock], tailBuf[sock]);
+  		  if (headBuf[sock] != 0)
+  			  --headBuf[sock];
+  		  else
+  			  headBuf[sock] = MAX_PBUF_STORED-1;
+  	  }
+  	  INFO_UTIL("Insert[%d]: %p:%d-%d [%d,%d]\n", sock, p, q->tot_len, p[0], headBuf[sock], tailBuf[sock]);
     }
+    return p;
+}
+
+void dumpPbuf(uint8_t sock)
+{
+	unsigned char index = tailBuf[sock];
+	printk("headBuf=%d tailBuf=%d\n", headBuf[sock], tailBuf[sock]);
+	do {
+	if (pBufStore[index][sock].data != NULL)
+	{
+		printk("%d] pcb:%p Buf: %p Len:%d\n", pBufStore[index][sock].idx, pBufStore[index][sock].pcb, 
+			pBufStore[index][sock].data, pBufStore[index][sock].len);
+	}
+	++index;
+	if (index == MAX_PBUF_STORED)
+		index = 0;
+	}while (index!=headBuf[sock]);
 }
 
 tData* get_pBuf(uint8_t sock)
 {
-	if (IS_BUF_EMPTY())
+	if (IS_BUF_EMPTY(sock))
 		return NULL;
 
-	if (IS_BUF_AVAIL())
+	if (IS_BUF_AVAIL(sock))
 	{
-		tData* p = &(pBufStore[tailBuf][sock]);
-		INFO_UTIL_VER("%p [%d,%d]\n", p, headBuf, tailBuf);
+		tData* p = &(pBufStore[tailBuf[sock]][sock]);
+		INFO_UTIL_VER("%p [%d,%d]\n", p, headBuf[sock], tailBuf[sock]);
 		return p;
 	}
 	return NULL;
@@ -83,14 +212,14 @@ void freetData(void * buf, uint8_t sock)
 		return;
 	}
 
-    pBufStore[tailBuf][sock].data = NULL;
-    pBufStore[tailBuf][sock].len = 0;
-    pBufStore[tailBuf][sock].idx = 0;
-    pBufStore[tailBuf][sock].pcb = 0;
+    pBufStore[tailBuf[sock]][sock].data = NULL;
+    pBufStore[tailBuf[sock]][sock].len = 0;
+    pBufStore[tailBuf[sock]][sock].idx = 0;
+    pBufStore[tailBuf[sock]][sock].pcb = 0;
 
-	if (++tailBuf == MAX_PBUF_STORED)
-		tailBuf = 0;
-	INFO_UTIL("%p [%d,%d]\n", buf, headBuf, tailBuf);
+	if (++tailBuf[sock] == MAX_PBUF_STORED)
+		tailBuf[sock] = 0;
+	INFO_UTIL("%p [%d,%d]\n", buf, headBuf[sock], tailBuf[sock]);
 	free(buf);
 }
 
@@ -117,6 +246,16 @@ void freetDataIdx(uint8_t idxBuf, uint8_t sock)
 
 void ack_recved(void* pcb, int len);
 
+void ackAndFreeData(void* pcb, int len, uint8_t sock, uint8_t* data)
+{
+	INFO_TCP("Ack pcb:%p len:%d sock:%d data:%p\n", pcb, len, sock, data);
+	if (!IS_UDP_SOCK(sock))
+		ack_recved(pcb, len);
+	if (data != NULL)
+		freetData(data, sock);
+}
+
+
 bool isAvailTcpDataByte(uint8_t sock)
 {
 	tData* p = get_pBuf(sock);
@@ -126,11 +265,10 @@ bool isAvailTcpDataByte(uint8_t sock)
 		INFO_UTIL_VER("check:%d %d %p\n",p->idx, p->len, p->data);
 		if (p->idx == p->len)
 		{
-			freetData(p->data, sock);
-			ack_recved(p->pcb, p->len);
 			INFO_UTIL("Free %p other buf %d tail:%d head:%d\n",
-					p->data, IS_BUF_AVAIL(), tailBuf, headBuf);
-			return (IS_BUF_AVAIL());
+					p->data, IS_BUF_AVAIL(sock), tailBuf[sock], headBuf[sock]);
+			ackAndFreeData(p->pcb, p->len, sock, p->data);						
+			return (IS_BUF_AVAIL(sock));
 		}else{
 			return true;
 		}
@@ -138,6 +276,12 @@ bool isAvailTcpDataByte(uint8_t sock)
 	return false;
 }
 
+uint16_t getAvailTcpDataByte(uint8_t sock)
+{
+	uint16_t len = calcMergeLen(sock);
+	INFO_UTIL_VER("Availabled data: %d\n", len);
+	return len;
+}
 
 
 bool getTcpDataByte(uint8_t sock, uint8_t* payload, uint8_t peek)
@@ -155,12 +299,11 @@ bool getTcpDataByte(uint8_t sock, uint8_t* payload, uint8_t peek)
 		else
 			*payload = buf[p->idx++];
 		INFO_UTIL_VER("get:%d %p %d\n",p->idx, p->data, *payload);
+		if (p->idx == p->len)
+			ackAndFreeData(p->pcb, p->len, sock, p->data);
 		return true;
 		}else{
-			//dealloc current buffer
-			INFO_UTIL("Free %p\n", p->data);
-			freetData(p->data, sock);
-			ack_recved(p->pcb, p->len);
+			ackAndFreeData(p->pcb, p->len, sock, p->data);
 		}
 	}
 	return false;
@@ -185,12 +328,20 @@ bool freeTcpData(uint8_t sock)
 	p = get_pBuf(sock);
 	if (p != NULL)
 	{
-		freetData(p->data, sock);
-		ack_recved(p->pcb, p->len);
+		ackAndFreeData(p->pcb, p->len, sock, p->data);
 		return true;
 	}
 	return false;
 }
 
+void freeAllTcpData(uint8_t sock)
+{
+	tData* p = NULL;
+	do{
+		p = get_pBuf(sock);
+		if (p != NULL)
+			freetData(p->data, sock);
+	}while(p!=NULL);
+}
 
 
diff --git a/firmwares/wifishield/wifiHD/src/ard_utils.h b/firmwares/wifishield/wifiHD/src/ard_utils.h
index 4b31c46..323b328 100644
--- a/firmwares/wifishield/wifiHD/src/ard_utils.h
+++ b/firmwares/wifishield/wifiHD/src/ard_utils.h
@@ -36,9 +36,12 @@
 #define SIGN2_DN 		LED2_DN
 #define SIGN2_TL 		LED2_TL
 
-#define DEB_PIN_UP() 			gpio_set_gpio_pin(DEB_PIN_GPIO)
-#define DEB_PIN_DN() 			gpio_clr_gpio_pin(DEB_PIN_GPIO)
-#define DEB_PIN_ENA() 			gpio_enable_gpio_pin(DEB_PIN_GPIO);
+#define DEB_PIN_UP(X) 			gpio_set_gpio_pin(DEB##X##_PIN_GPIO)
+#define DEB_PIN_DN(X) 			gpio_clr_gpio_pin(DEB##X##_PIN_GPIO)
+#define DEB_PIN_ENA(X) 			gpio_enable_gpio_pin(DEB##X##_PIN_GPIO)
+#define DEB_PIN_TOGGLE(X)		gpio_tgl_gpio_pin(DEB##X##_PIN_GPIO)
+#define DEB_PIN_TRIGGER(X)		DEB_PIN_DN(X);  DEB_PIN_UP(X);
+
 
 #else
 #define SIGN0_UP()
@@ -51,9 +54,11 @@
 #define SIGN2_DN()
 #define SIGN2_TL()
 
-#define DEB_PIN_UP()
-#define DEB_PIN_DN()
-#define DEB_PIN_ENA()
+#define DEB_PIN_UP(X)
+#define DEB_PIN_DN(X)
+#define DEB_PIN_ENA(X)
+#define DEB_PIN_TOGGLE(X)
+#define DEB_PIN_TRIGGER(X)
 
 //#define TOGGLE_SIG0
 #endif
@@ -119,8 +124,16 @@
 
 
 #define PUT_DATA_INT(INT, BYTE, IDX)   {           		\
-    uint16_t _int = INT;                              	\
-    BYTE[IDX] = (uint8_t)((_int & 0xff00)>>8);       	\
+    uint16_t _int = INT;								\
+	BYTE[IDX] = 2;                            			\
+    BYTE[IDX+1] = (uint8_t)((_int & 0xff00)>>8);       	\
+    BYTE[IDX+2] = (uint8_t)(_int & 0xff);              	\
+}
+
+#define PUT_DATA_INT_NO(INT, BYTE, IDX)   {           		\
+    uint16_t _int = INT;								\
+	BYTE[IDX] = 2;                            			\
+    BYTE[IDX+2] = (uint8_t)((_int & 0xff00)>>8);       	\
     BYTE[IDX+1] = (uint8_t)(_int & 0xff);              	\
 }
 
@@ -201,7 +214,7 @@
 #define STATSPI_TIMEOUT_ERROR()		\
 		statSpi.timeoutIntErr++;	\
 		statSpi.rxErr++;			\
-		statSpi.lastError = err;	\
+		statSpi.lastError = SPI_TIMEOUT_ERROR;	\
 		statSpi.status = spi_getStatus(ARD_SPI);
 
 #define STATSPI_DISALIGN_ERROR()		\
@@ -228,11 +241,16 @@
 #define STATSPI_OVERRIDE_ERROR()
 #endif
 
-#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);
-
+#define DUMP_TCP_STATE(TTCP) do {\
+		int i = getCurrClientConnId(); \
+		INFO_TCP("%d] ttcp:%p tpcb:%p state:%d lpcb:%p state:%d left:%d sent:%d\n", \
+			i, TTCP, TTCP->tpcb[i], (TTCP->tpcb[i])?TTCP->tpcb[i]->state:0, \
+			TTCP->lpcb, (TTCP->lpcb)?TTCP->lpcb->state:0, \
+			(TTCP->tpcb[i])?TTCP->left[i]:0, (TTCP->tpcb[i])?TTCP->buff_sent[i]:0); \
+			} while(0);
+			
+#define Mode2Str(_Mode) ((_Mode==0)?"TRANSMIT":"RECEIVE")			
+#define ProtMode2Str(_protMode) ((_protMode==0)?"TCP":"UDP")
 
 typedef struct sData
 {
@@ -246,7 +264,13 @@ struct pbuf;
 
 void init_pBuf();
 
-void insert_pBuf(struct pbuf* q, uint8_t sock, void* _pcb);
+uint8_t* insert_pBuf(struct pbuf* q, uint8_t sock, void* _pcb);
+
+uint8_t* insertBuf(uint8_t sock, uint8_t* buf, uint16_t len);
+
+uint8_t* mergeBuf(uint8_t sock, uint8_t** buf, uint16_t* _len);
+
+uint16_t clearBuf(uint8_t sock);
 
 tData* get_pBuf(uint8_t sock);
 
@@ -260,8 +284,12 @@ bool getTcpData(uint8_t sock, void** payload, uint16_t* len);
 
 bool getTcpDataByte(uint8_t sock, uint8_t* payload, uint8_t peek);
 
+uint16_t getAvailTcpDataByte(uint8_t sock);
+
 bool isAvailTcpDataByte(uint8_t sock);
 
 uint8_t freeTcpData(uint8_t sock);
 
+void freeAllTcpData(uint8_t sock);
+
 #endif /* ARD_UTILS_H_ */
diff --git a/firmwares/wifishield/wifiHD/src/cmd_wl.c b/firmwares/wifishield/wifiHD/src/cmd_wl.c
index 61634ef..a210dec 100644
--- a/firmwares/wifishield/wifiHD/src/cmd_wl.c
+++ b/firmwares/wifishield/wifiHD/src/cmd_wl.c
@@ -43,24 +43,11 @@
 #include "lwip/dns.h"
 #include "debug.h"
 #include "ard_spi.h"
+#include "ard_tcp.h"
+#include "ard_utils.h"
 
 extern void showTTCPstatus();
 
-#define ENABLE_DEBUG_LEVEL 1
-#define VERBOSE_DEBUG_LEVEL 2
-
-#define CHECK_ENA_DEBUG(LEVEL, FLAG)  		\
-	do{										\
-		if (LEVEL >= ENABLE_DEBUG_LEVEL) enableDebug |= FLAG;		\
-		else enableDebug &= ~FLAG;			\
-		}while(0);
-
-#define CHECK_VERB_DEBUG(LEVEL, FLAG)  		\
-	do{										\
-		if (LEVEL >= VERBOSE_DEBUG_LEVEL) verboseDebug |= FLAG;		\
-		else verboseDebug &= ~FLAG;			\
-		}while(0);
-
 #define  _DNS_CMD_
 
 /**
@@ -195,23 +182,26 @@ cmd_ibss(int argc, char* argv[], void* ctx)
 cmd_state_t 
 cmd_set_ip(int argc, char* argv[], void* ctx)
 {
-        struct net_cfg *ncfg = ctx;
+	struct ctx_server *hs = ctx;
+    struct net_cfg *ncfg = &(hs->net_cfg);
         struct ip_addr lwip_addr;
         struct netif *nif = ncfg->netif;
 
         if (argc == 2 && 
             (strncmp(argv[1], "none", 4) == 0)) {
-                ncfg->dhcp_enabled = 1;
+                ncfg->dhcp_enabled = DYNAMIC_IP_CONFIG;
                 
                 return CMD_DONE;
         }
         else if (argc != 4 ) {
-                printk("usage: ip <ip> <netmask> <gateway-ip>\n");
-                printk("  or : ip none (to enable DHCP)\n");
+                printk("usage: ipconfig <ip> <netmask> <gateway-ip>\n");
+                printk("  or : ipconfig none (to enable DHCP)\n");
                 return CMD_DONE;
         }
+
         /* IP address */
         lwip_addr = str2ip(argv[1]);
+        INFO_SPI("nif:%p lwip_addr=0x%x\n", nif, lwip_addr.addr);
         netif_set_ipaddr(nif, &lwip_addr);
         /* Netmask */
         lwip_addr = str2ip(argv[2]);
@@ -220,7 +210,7 @@ cmd_set_ip(int argc, char* argv[], void* ctx)
         lwip_addr = str2ip(argv[3]);
         netif_set_gw(nif, &lwip_addr);
         /* Disable DHCP */
-        ncfg->dhcp_enabled = 0;
+        ncfg->dhcp_enabled = STATIC_IP_CONFIG;
 
         return CMD_DONE;
 }
@@ -373,14 +363,15 @@ cmd_setDnsServer(int argc, char* argv[], void* ctx)
  *
  */
 cmd_state_t
-cmd_startTcpSrv(int argc, char* argv[], void* ctx)
+cmd_startSrv(int argc, char* argv[], void* ctx)
 {
-        const char *usage = "usage: startTcpSrv <port> <sock>\n";
+        const char *usage = "usage: startSrv <port> <sock> <tcp(0)/udp(1)>\n";
 
         int port = 0;
         int sock = 0;
+		int protMode = 0;
 
-        if (argc < 3) {
+        if (argc < 4) {
                 printk(usage);
                 return CMD_DONE;
         }
@@ -389,15 +380,50 @@ cmd_startTcpSrv(int argc, char* argv[], void* ctx)
         port = atoi(argv[1]);
         /* socket index */
         sock = atoi(argv[2]);
+		/* Protocol Mode */
+		protMode = atoi(argv[3]);
 
-        printk("Start TCP server on port %d sock %d\n", port, sock);
-        if (start_server_tcp(port, sock) != -1)
+        printk("Start %s server on port %d sock %d\n", ProtMode2Str(protMode), port, sock);
+        if (start_server_tcp(port, sock, protMode) == -1)
         {
-        	printk("Start TCP server on port %d sock %d FAILED\n", port, sock);
+        	WARN("Start %s server on port %d sock %d FAILED\n", ProtMode2Str(protMode), port, sock);
         }
         return CMD_DONE;
 }
 
+/**
+ *
+ */
+cmd_state_t
+cmd_startCli(int argc, char* argv[], void* ctx)
+{
+        const char *usage = "usage: startCli <ipaddr> <port> <sock> <tcp(0)/udp(1)>\n";
+		struct ip_addr addr = {0};
+        int port = 0;
+        int sock = 0;
+		int protMode = 0;
+
+        if (argc < 5) {
+                printk(usage);
+                return CMD_DONE;
+        }
+
+        /* IP address */
+        addr = str2ip(argv[1]);
+        /* TCP port */
+        port = atoi(argv[2]);
+        /* socket index */
+        sock = atoi(argv[3]);
+		/* Protocol Mode */
+		protMode = atoi(argv[4]);
+
+        printk("Start client on addr 0x%x, port %d sock %d mode %d\n", addr, port, sock, protMode);
+        if (start_client_tcp(addr.addr, port, sock, protMode) == -1)
+        {
+        	WARN("Start client on port %d sock %d prot %d mode %d FAILED\n", port, sock, protMode);
+        }
+        return CMD_DONE;
+}
 
 #endif
 
@@ -432,11 +458,15 @@ cmd_status(int argc, char* argv[], void* ctx)
         
         /* print ip address */
         if (netif_is_up(netif_default))
-                printk("ip addr: %s\n", ip2str(netif_default->ip_addr));
+		{
+                printk("ip addr: %s - ", ip2str(netif_default->ip_addr));
+				printk("netmask: %s - ", ip2str(netif_default->netmask));
+				printk("gateway: %s\n", ip2str(netif_default->gw));
+		}					
         else
                 printk("ip interface is down\n");
         printk("dhcp : ");
-        if (ncfg->dhcp_enabled) {
+        if (ncfg->dhcp_enabled == DYNAMIC_IP_CONFIG) {
                 printk("enabled\n");
         }
         else {
@@ -445,8 +475,8 @@ cmd_status(int argc, char* argv[], void* ctx)
         struct ip_addr addr1 = dns_getserver(0);
         struct ip_addr addr2 = dns_getserver(1);
 
-        printk("==> DNS1: %s\n", ip2str(addr1), addr1);
-        printk("==> DNS2: %s\n", ip2str(addr2), addr2);
+        printk("DNS: %s - ", ip2str(addr1));
+		printk("%s\n", ip2str(addr2));
 
         showTTCPstatus();
         return CMD_DONE;
@@ -595,16 +625,14 @@ cmd_debug(int argc, char* argv[], void* ctx)
 
         if (argc == 2 && strcmp(argv[1], "off") == 0) {
                 printk("Debug OFF\n");
-                enableDebug = DEFAULT_INFO_FLAG;
-                verboseDebug = 0;
+                INIT_DEBUG_VARIABLES()
                 return CMD_DONE;
         }else if (argc == 2 && strcmp(argv[1], "print") == 0) {
-            printk("Debug enabled: 0x%x\n", enableDebug);
-            printk("Verbose enabled: 0x%x\n", verboseDebug);
+        	PRINT_DEBUG_VARIABLES()
             return CMD_DONE;
         }else if (argc == 2 && strcmp(argv[1], "on") == 0) {
             printk("Debug ON\n");
-            enableDebug = 0xff;
+            TURNON_DEBUG_VARIABLES();
             return CMD_DONE;
         }
         if (argc < 3) {
@@ -613,23 +641,91 @@ cmd_debug(int argc, char* argv[], void* ctx)
         }
         level = atoi(argv[2]);
         if (argc == 3 && strcmp(argv[1], "init") == 0) {
-        	CHECK_ENA_DEBUG(level, INFO_INIT_FLAG);
-        	CHECK_VERB_DEBUG(level, INFO_INIT_FLAG);
+        	CHECK_DEBUG_LEVEL(level, INFO_INIT_FLAG);
         }else if (argc == 3 && strcmp(argv[1], "spi") == 0) {
-        	CHECK_ENA_DEBUG(level, INFO_SPI_FLAG);
-        	CHECK_VERB_DEBUG(level, INFO_SPI_FLAG);
+        	CHECK_DEBUG_LEVEL(level, INFO_SPI_FLAG);
         }else if (argc == 3 && strcmp(argv[1], "tcp") == 0) {
-        	CHECK_ENA_DEBUG(level, INFO_TCP_FLAG);
-        	CHECK_VERB_DEBUG(level, INFO_TCP_FLAG);
+        	CHECK_DEBUG_LEVEL(level, INFO_TCP_FLAG);
         }else if (argc == 3 && strcmp(argv[1], "cm") == 0) {
-        	CHECK_ENA_DEBUG(level, INFO_CM_FLAG);
-        	CHECK_VERB_DEBUG(level, INFO_CM_FLAG);
+        	CHECK_DEBUG_LEVEL(level, INFO_CM_FLAG);
         }else if (argc == 3 && strcmp(argv[1], "util") == 0) {
-        	CHECK_ENA_DEBUG(level, INFO_UTIL_FLAG);
-        	CHECK_VERB_DEBUG(level, INFO_UTIL_FLAG);
+        	CHECK_DEBUG_LEVEL(level, INFO_UTIL_FLAG);
         }else if (argc == 3 && strcmp(argv[1], "warn") == 0) {
-        	CHECK_ENA_DEBUG(level, INFO_WARN_FLAG);
-        	CHECK_VERB_DEBUG(level, INFO_WARN_FLAG);
+        	CHECK_DEBUG_LEVEL(level, INFO_WARN_FLAG);
         }
         return CMD_DONE;
 }
+
+extern void dumpPbuf(uint8_t sock);
+
+/**
+ *
+ */
+cmd_state_t
+cmd_dumpBuf(int argc, char* argv[], void* ctx)
+{
+    const char *usage = "usage: dumpPbuf [sock]\n\t"\
+    		"sock: socket Number\n";
+
+    if (argc == 2 && strcmp(argv[1], "all") == 0) {
+    	printk("Dump All Buffers\n");
+    	int i = 0;
+    	for (; i<MAX_SOCK_NUM; ++i)
+    	{
+			printk("Socket: %d\n", i);
+    		dumpPbuf(i);
+    	}
+    }else if (argc == 2) {
+    	uint8_t sock = atoi(argv[1]);
+        printk("Socket: %d\n", sock);
+        dumpPbuf(sock);
+    }else {
+		printk(usage);
+	}		
+	return CMD_DONE;
+}
+
+
+
+/**
+ *
+ */
+cmd_state_t
+cmd_sendUdpData(int argc, char* argv[], void* ctx)
+{
+	const char pattern[]={'M', 'I', 'M', 'L', 'F', 'D'};
+	const char* pattern2[]={"Prova", "1234567890","FineTest"};
+    const char *usage = "usage: sendUdp [sock]\n\t"\
+    		"sock: socket Number\n";
+
+    if (argc < 2)
+    	printk(usage);
+
+    if (argc >= 2) {
+
+		uint8_t sock = atoi(argv[1]);
+		printk("Socket: %d\n", sock);
+
+		if (argc >= 3) {
+			uint8_t patternType = atoi(argv[2]);
+			printk("PatternType: %d\n", patternType);
+			if (patternType == 1)
+			{
+				insertBuf(sock, (uint8_t*)pattern2[0], strlen(pattern2[0]));
+				insertBuf(sock, (uint8_t*)pattern2[1], strlen(pattern2[1]));
+				insertBuf(sock, (uint8_t*)pattern2[2], strlen(pattern2[2]));
+			}
+			if (patternType == 2)
+			{
+				mergeBuf(sock, NULL, NULL);
+			}
+		}else{
+			if (sock < MAX_SOCK_NUM)
+			{
+				sendUdpData(getTTCP(sock, TTCP_MODE_TRANSMIT), (uint8_t*)pattern, sizeof(pattern)/sizeof(char));
+			}
+		}
+
+    }
+	return CMD_DONE;
+}
diff --git a/firmwares/wifishield/wifiHD/src/cmd_wl.h b/firmwares/wifishield/wifiHD/src/cmd_wl.h
index 3953b95..a1d1a0f 100644
--- a/firmwares/wifishield/wifiHD/src/cmd_wl.h
+++ b/firmwares/wifishield/wifiHD/src/cmd_wl.h
@@ -54,7 +54,10 @@ cmd_state_t cmd_statSpi(int argc, char* argv[], void* ctx);
 cmd_state_t cmd_resetStatSpi(int argc, char* argv[], void* ctx);
 cmd_state_t cmd_gethostbyname(int argc, char* argv[], void* ctx);
 cmd_state_t cmd_setDnsServer(int argc, char* argv[], void* ctx);
-cmd_state_t cmd_startTcpSrv(int argc, char* argv[], void* ctx);
+cmd_state_t cmd_startSrv(int argc, char* argv[], void* ctx);
+cmd_state_t cmd_startCli(int argc, char* argv[], void* ctx);
+cmd_state_t cmd_dumpBuf(int argc, char* argv[], void* ctx);
+cmd_state_t cmd_sendUdpData(int argc, char* argv[], void* ctx);
 #ifdef WFE_6_12
 cmd_state_t cmd_ibss(int argc, char* argv[], void* ctx);
 #endif
diff --git a/firmwares/wifishield/wifiHD/src/console.c b/firmwares/wifishield/wifiHD/src/console.c
index 5ead91e..e54943f 100644
--- a/firmwares/wifishield/wifiHD/src/console.c
+++ b/firmwares/wifishield/wifiHD/src/console.c
@@ -35,7 +35,7 @@
 #include <board_init.h>
 #include <usart.h>
 
-#define MAX_CMD_CONSOLE_NUM 12
+#define MAX_CMD_CONSOLE_NUM 15
 struct {
         cmd_cb_t cb;
         const char* str;
diff --git a/firmwares/wifishield/wifiHD/src/debug.h b/firmwares/wifishield/wifiHD/src/debug.h
index 18608ff..154b799 100644
--- a/firmwares/wifishield/wifiHD/src/debug.h
+++ b/firmwares/wifishield/wifiHD/src/debug.h
@@ -18,92 +18,174 @@
 #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_WARN_FLAG (1<<0xF)     // Warning
 #define DEFAULT_INFO_FLAG 0 //INFO_WARN_FLAG
 
-extern uint16_t enableDebug;
-extern uint16_t verboseDebug;
-
-#ifdef _INFO_DEBUG_
-#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)
+#ifdef _DEBUG_
+#define DEFINE_DEBUG_VARIABLES()							\
+uint16_t enableDebug = DEFAULT_INFO_FLAG | INFO_WARN_FLAG;	\
+uint16_t verboseDebug = 0;									\
+uint16_t dumpDebug = 0;										\
+uint16_t pollDebug = 0;
+#else
+#define DEFINE_DEBUG_VARIABLES()							\
+uint16_t enableDebug = DEFAULT_INFO_FLAG;					\
+uint16_t verboseDebug = 0;									\
+uint16_t dumpDebug = 0;										\
+uint16_t pollDebug = 0;
+#endif
 
-#define INFO_TCP_VER(msg, args...) do { 			\
-if ((enableDebug & INFO_TCP_FLAG)&&(verboseDebug & INFO_TCP_FLAG)) \
-	printk("I-[%s] " msg , __func__ , ##args );	\
-} while (0)
+#define INIT_DEBUG_VARIABLES()	\
+		enableDebug = DEFAULT_INFO_FLAG | INFO_WARN_FLAG;	\
+		verboseDebug = 0;									\
+		dumpDebug = 0; pollDebug = 0;
 
 
-#define INFO_SPI(msg, args...) do { 			\
-if (enableDebug & INFO_SPI_FLAG) printk("I-[%s] " msg , __func__ , ##args );	\
-} while (0)
+#define PRINT_DEBUG_VARIABLES()	\
+		printk("Debug 	enabled: 0x%x\n", enableDebug);	\
+        printk("Verbose enabled: 0x%x\n", verboseDebug);	\
+        printk("Dump 	enabled: 0x%x\n", dumpDebug);	\
+        printk("POoll 	enabled: 0x%x\n", pollDebug);
 
-#define INFO_SPI_VER(msg, args...) do { 			\
-if ((enableDebug & INFO_SPI_FLAG)&&(verboseDebug & INFO_SPI_FLAG)) \
-	printk("I-[%s] " msg , __func__ , ##args );	\
-} while (0)
+#define TURNON_DEBUG_VARIABLES()	\
+		enableDebug = 0xff;
 
+extern uint16_t enableDebug;
+extern uint16_t verboseDebug;
+extern uint16_t dumpDebug;
+extern uint16_t pollDebug;
+
+#define ENABLE_DEBUG_LEVEL 	1
+#define VERBOSE_DEBUG_LEVEL 2
+#define DUMP_DEBUG_LEVEL 	3
+#define POLL_DEBUG_LEVEL 	4
+
+#define CHECK_DEBUG(VAR, LEVEL, LEVEL_LIMIT, FLAG)  		\
+	do{										\
+		if (LEVEL >= LEVEL_LIMIT) VAR |= FLAG;		\
+		else VAR &= ~FLAG;			\
+		}while(0);
+
+#define CHECK_ENA_DEBUG(LEVEL, FLAG)	\
+		CHECK_DEBUG(enableDebug, LEVEL, ENABLE_DEBUG_LEVEL, FLAG)
+#define CHECK_VERB_DEBUG(LEVEL, FLAG) 	\
+		CHECK_DEBUG(verboseDebug, LEVEL, VERBOSE_DEBUG_LEVEL, FLAG)
+#define CHECK_DUMP_DEBUG(LEVEL, FLAG) 	\
+		CHECK_DEBUG(dumpDebug, LEVEL, DUMP_DEBUG_LEVEL, FLAG)
+#define CHECK_POLL_DEBUG(LEVEL, FLAG) 	\
+		CHECK_DEBUG(pollDebug, LEVEL, POLL_DEBUG_LEVEL, FLAG)
+
+
+#define CHECK_DEBUG_LEVEL(LEVEL, INFO_FLAG)		\
+		CHECK_ENA_DEBUG(LEVEL, INFO_FLAG) 		\
+		CHECK_VERB_DEBUG(LEVEL, INFO_FLAG) 		\
+		CHECK_DUMP_DEBUG(LEVEL, INFO_FLAG)		\
+		CHECK_POLL_DEBUG(LEVEL, INFO_FLAG)
 
-#define INFO_UTIL(msg, args...) do { 			\
-if (enableDebug & INFO_UTIL_FLAG) printk("I-[%s] " msg , __func__ , ##args );	\
+#ifdef _INFO_DEBUG_
+#define PRINT_DEBUG(msg, args...) do { 			\
+	printk("[%s] " msg , __func__ , ##args );	\
 } while (0)
 
-#define INFO_UTIL_VER(msg, args...) do { 			\
-if ((enableDebug & INFO_UTIL_FLAG)&&(verboseDebug & INFO_UTIL_FLAG)) \
+#define INFO_DEBUG(msg, args...) do { 			\
 	printk("I-[%s] " msg , __func__ , ##args );	\
 } while (0)
 
+#define WARN_DEBUG(msg, args...) do { 			\
+	printk("W-[%s] " msg , __func__ , ##args );	\
+} while (0)
 
 #else
-#define INFO_INIT(msg, args...) do {}while(0);
-#define INFO_TCP(msg, args...) do {}while(0);
-#define INFO_TCP_VER(msg, args...) do { }while(0);
-#define INFO_SPI(msg, args...) do {}while(0);
-#define INFO_SPI_VER(msg, args...) do { }while(0);
-#define INFO_UTIL(msg, args...) do {}while(0);
-#define INFO_UTIL_VER(msg, args...) do { }while(0);
+do { }while(0);
 #endif
 
-#ifdef _APP_DEBUG_
-#define INFO(msg, args...) do { 			\
-printk("I-[%s] " msg , __func__ , ##args );	\
+#define IF_DEBUG(X,Y) do { 			\
+if (enableDebug & INFO_##X##_FLAG) \
+Y;	\
+} while (0)
+
+#define IF_DEBUG_VER(X,Y) do { 			\
+if (verboseDebug & INFO_##X##_FLAG) \
+Y;	\
 } while (0)
 
-#else /* !defined(_DEBUG_) */
-//#define INFO(msg, args...) do {} while (0)
-#endif /* !defined(_DEBUG_) */
+#define IF_DEBUG_DUMP(X,Y) do { 			\
+if (dumpDebug & INFO_##X##_FLAG) \
+Y;	\
+} while (0)
 
-#if 1
-#define WARN(msg, args...) do {	\
-	if (enableDebug & INFO_WARN_FLAG) printk("W-[%s] " msg , __func__ , ##args );		\
-	} while (0)
+#define IF_DEBUG_POLL(X,Y) do { 			\
+if (pollDebug & INFO_##X##_FLAG) {\
+Y;	\
+}} while (0)
+
+
+
+#define IF_WARN(Y) IF_DEBUG(WARN,Y)
+#define IF_WARN_VER(Y) IF_DEBUG_VER(WARN,Y)
+#define IF_TCP(Y) IF_DEBUG(TCP,Y)
+#define IF_TCP_VER(Y) IF_DEBUG_VER(TCP,Y)
+#define IF_TCP_POLL(Y) IF_DEBUG_POLL(TCP,Y)
+#define IF_TCP_DUMP(Y) IF_DEBUG_DUMP(TCP,Y)
+#define IF_SPI(Y) IF_DEBUG(SPI,Y)
+#define IF_SPI_VER(Y) IF_DEBUG_VER(SPI,Y)
+#define IF_SPI_DUMP(Y) IF_DEBUG_DUMP(SPI,Y)
+#define IF_SPI_POLL(Y) IF_DEBUG_POLL(SPI,Y)
+#define IF_UTIL(Y) IF_DEBUG(UTIL,Y)
+#define IF_UTIL_VER(Y) IF_DEBUG_VER(UTIL,Y)
+
+#define WARN(msg, args...)			IF_DEBUG(WARN,WARN_DEBUG(msg, ##args))
+#define WARN_VER(msg, args...)		IF_DEBUG_VER(WARN,WARN_DEBUG(msg, ##args))
+#define WARN_POLL(msg, args...)		IF_DEBUG_POLL(WARN,WARN_DEBUG(msg, ##args))
+#if 0	// disable to reduce the size of binary
+#define INFO_INIT(msg, args...) 	IF_DEBUG(INIT,PRINT_DEBUG(msg, ##args))
+#define INFO_INIT_VER(msg, args...) IF_DEBUG_VER(INIT,PRINT_DEBUG(msg, ##args))
 #else
-#define WARN(msg, args...) do {		} while (0)
+#define INFO_INIT(msg, args...) 	
+#define INFO_INIT_VER(msg, args...) 
 #endif
+#define INFO_TCP(msg, args...) 		IF_DEBUG(TCP,PRINT_DEBUG(msg, ##args))
+#define INFO_TCP_VER(msg, args...) 	IF_DEBUG_VER(TCP,PRINT_DEBUG(msg, ##args))
+#define INFO_TCP_DUMP(msg, args...) IF_DEBUG_DUMP(TCP,PRINT_DEBUG(msg, ##args))
+#define INFO_TCP_POLL(msg, args...) IF_DEBUG_POLL(TCP,PRINT_DEBUG(msg, ##args))
+#define INFO_SPI(msg, args...) 		IF_DEBUG(SPI,PRINT_DEBUG(msg, ##args))
+#define INFO_SPI_VER(msg, args...) 	IF_DEBUG_VER(SPI,PRINT_DEBUG(msg, ##args))
+#define INFO_SPI_DUMP(msg, args...) IF_DEBUG_DUMP(SPI,PRINT_DEBUG(msg, ##args))
+#define INFO_SPI_POLL(msg, args...) IF_DEBUG_POLL(SPI,PRINT_DEBUG(msg, ##args))
+#define INFO_UTIL(msg, args...) 	IF_DEBUG(UTIL,PRINT_DEBUG(msg, ##args))
+#define INFO_UTIL_VER(msg, args...) IF_DEBUG_VER(UTIL,PRINT_DEBUG(msg, ##args))
+#define CM_DPRINTF(msg, args...) 	IF_DEBUG(CM,PRINT_DEBUG(msg, ##args))
 
 extern void dump(char* _buf, uint16_t _count);
 
-#ifdef _APP_DEBUG_
-#define DUMP(BUF, COUNT) do {		\
-	printk("[%s]\n", __func__);		\
+#define _DUMP(BUF, COUNT) do {		\
+	printk("[%s]: ", __func__);		\
 	dump((char*)BUF, COUNT);				\
 	} while (0)
+
+#ifdef _APP_DEBUG_
+#define DUMP(BUF, COUNT) _DUMP(BUF, COUNT)
 #else
 #define DUMP(BUF, COUNT) do {} while (0)
 #endif
 #endif
 
-#define DUMP_TCP(BUF, COUNT) do {			\
-	if (verboseDebug & INFO_TCP_FLAG) {		\
-	printk("[%s]\n", __func__);				\
-	dump((char*)BUF, COUNT);				\
-	}} while (0)
+#define DUMP_TCP(BUF, COUNT) IF_TCP_DUMP(_DUMP(BUF, COUNT))
+#define DUMP_SPI(BUF, COUNT) IF_SPI_DUMP(_DUMP(BUF, COUNT))
+
+#define DUMP_SPI_CMD(BUF) do {				\
+	if (dumpDebug & INFO_SPI_FLAG) {		\
+	int i = 0;								\
+	for (; i < CMD_MAX_LEN; ++i) 			\
+	{										\
+		printk("0x%x ", BUF[i]);			\
+		if (BUF[i] == END_CMD)				\
+			break;							\
+	}										\
+	printk("\n");							\
+	}										\
+}while(0);
 
diff --git a/firmwares/wifishield/wifiHD/src/lwip_setup.h b/firmwares/wifishield/wifiHD/src/lwip_setup.h
index 7a3ec6f..7edf2b5 100644
--- a/firmwares/wifishield/wifiHD/src/lwip_setup.h
+++ b/firmwares/wifishield/wifiHD/src/lwip_setup.h
@@ -1,12 +1,21 @@
 #ifndef _LWIP_SETUP_H
 #define _LWIP_SETUP_H
 
+#define INIT_IP_CONFIG		0xff
+#define STATIC_IP_CONFIG	0
+#define DYNAMIC_IP_CONFIG	1
+
 struct net_cfg {
         struct netif *netif; /* lwip network interface */
         uint8_t dhcp_enabled;
         uint8_t dhcp_running;
 };
 
+struct ctx_server {
+	struct net_cfg net_cfg;
+	uint8_t wl_init_complete;
+};
+
 /*! Start the IP stack.
  * If cfg->netif must have been allocated and lwip_init()
  * must have been called before this function is called
diff --git a/firmwares/wifishield/wifiHD/src/lwipopts.h b/firmwares/wifishield/wifiHD/src/lwipopts.h
index 64965bf..7b08b84 100644
--- a/firmwares/wifishield/wifiHD/src/lwipopts.h
+++ b/firmwares/wifishield/wifiHD/src/lwipopts.h
@@ -111,7 +111,7 @@
  * MEMP_NUM_TCP_PCB: the number of simulatenously active TCP connections.
  * (requires the LWIP_TCP option)
  */
-#define MEMP_NUM_TCP_PCB                2
+#define MEMP_NUM_TCP_PCB                4
 
 /**
  * MEMP_NUM_TCP_PCB_LISTEN: the number of listening TCP connections.
diff --git a/firmwares/wifishield/wifiHD/src/main.c b/firmwares/wifishield/wifiHD/src/main.c
index 8961e35..fffb34e 100644
--- a/firmwares/wifishield/wifiHD/src/main.c
+++ b/firmwares/wifishield/wifiHD/src/main.c
@@ -39,7 +39,7 @@
 #include <lwip_setup.h>
 
 /* FIRMWARE version */
-const char* fwVersion = "1.0.0";
+const char* fwVersion = "1.1.0";
 
 #if BOARD == ARDUINO
 #if !defined(DATAFLASH)
@@ -78,25 +78,13 @@ void fw_download_cb(void* ctx, uint8_t** buf, uint32_t* len)
 #endif
 #endif
 
-struct ctx_server {
-	struct net_cfg net_cfg;
-	uint8_t wl_init_complete;
-};
-
 bool ifStatus = false;
 bool scanNetCompleted = false;
 
 static bool initSpiComplete = false;
 
 // variable used as enable flag for debug prints
-#ifdef _DEBUG_
-uint16_t enableDebug = DEFAULT_INFO_FLAG | INFO_WARN_FLAG;// | INFO_SPI_FLAG;
-uint16_t verboseDebug = 0;
-#else
-uint16_t enableDebug = DEFAULT_INFO_FLAG;
-uint16_t verboseDebug = 0;
-#endif
-
+DEFINE_DEBUG_VARIABLES();
 
 /**
  *
@@ -121,7 +109,7 @@ wl_cm_conn_cb(struct wl_network_t* net, void* ctx)
 	INFO_INIT("Connection cb...\n");
 
 	printk("link up, connected to \"%s\"\n", ssid2str(&net->ssid));
-    if ( hs->net_cfg.dhcp_enabled ) {
+    if ( hs->net_cfg.dhcp_enabled == DYNAMIC_IP_CONFIG ) {
 			INFO_INIT("Start DHCP...\n");
 		    printk("requesting dhcp ... ");
             int8_t result = dhcp_start(hs->net_cfg.netif);
@@ -129,7 +117,7 @@ wl_cm_conn_cb(struct wl_network_t* net, void* ctx)
             hs->net_cfg.dhcp_running = 1;
     }
     else {
-            netif_set_up(hs->net_cfg.netif);
+        netif_set_up(hs->net_cfg.netif);
     }
 
     INFO_INIT("Start DNS...\n");
@@ -277,7 +265,7 @@ poll(struct ctx_server* hs)
 #endif
 }
 
-void initShell()
+void initShell(void* ctx)
 {
 	/* initialize shell */
 	INFO_INIT("Shell init...\n");
@@ -285,9 +273,10 @@ void initShell()
         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("status", cmd_status, ctx);
         console_add_cmd("debug", cmd_debug, NULL);
-
+        console_add_cmd("dumpBuf", cmd_dumpBuf, NULL);
+		console_add_cmd("ipconfig", cmd_set_ip, ctx);
 #ifdef ADD_CMDS
         console_add_cmd("powersave", cmd_power, NULL);
         console_add_cmd("psconf", cmd_psconf, NULL);
@@ -307,8 +296,11 @@ void initShell()
 #ifdef _DNS_CMD_
         console_add_cmd("getHost", cmd_gethostbyname, NULL);
         console_add_cmd("setDNS", cmd_setDnsServer, NULL);
-        console_add_cmd("startTcpSrv", cmd_startTcpSrv, NULL);
 #endif
+        console_add_cmd("startSrv", cmd_startSrv, NULL);
+        console_add_cmd("startCli", cmd_startCli, NULL);
+        console_add_cmd("sendUdp", cmd_sendUdpData, NULL);
+
 }
 
 /**
@@ -321,12 +313,16 @@ wl_init_complete_cb(void* ctx)
     struct ip_addr ipaddr, netmask, gw;
 	wl_err_t wl_status;
 	
-	IP4_ADDR(&gw, 0,0,0,0);
-    IP4_ADDR(&ipaddr, 0,0,0,0);
-    IP4_ADDR(&netmask, 0,0,0,0);
-        
-    /* default is dhcp enabled */
-    hs->net_cfg.dhcp_enabled = 1;
+	if (hs->net_cfg.dhcp_enabled == INIT_IP_CONFIG)
+	{
+		IP4_ADDR(&gw, 0,0,0,0);
+		IP4_ADDR(&ipaddr, 0,0,0,0);
+		IP4_ADDR(&netmask, 0,0,0,0);
+			
+		/* default is dhcp enabled */
+		hs->net_cfg.dhcp_enabled = DYNAMIC_IP_CONFIG;
+	}
+
     start_ip_stack(&hs->net_cfg,
                    ipaddr,
                    netmask,
@@ -341,7 +337,7 @@ wl_init_complete_cb(void* ctx)
 
     wl_scan();
 
-    if (initSpi()){
+    if (initSpi(hs)){
     	WARN("Spi not initialized\n");
     }else
     {
@@ -359,9 +355,13 @@ void startup_init(void)
 
 	// if DEBUG enabled use DEB_PIN_GPIO for debug purposes
     DEB_PIN_ENA();
+	DEB_PIN_ENA(2);
     DEB_PIN_UP();
+	DEB_PIN_UP(2);
 }
 
+const char timestamp[] = __TIMESTAMP__;
+
 /**
  *
  */
@@ -381,8 +381,6 @@ main(void)
 
     tc_init();
 
-    initShell();
-
     delay_init(FOSC0);
 
 #ifdef _TEST_SPI_
@@ -396,7 +394,7 @@ main(void)
 
      }
 #else
-    printk("Arduino Wifi Startup... [%s]\n", __TIMESTAMP__);
+    printk("Arduino Wifi Startup... [%s]\n", timestamp);
 
     size_t size_ctx_server = sizeof(struct ctx_server);
 	hs = calloc(1, size_ctx_server);
@@ -405,10 +403,11 @@ main(void)
 	size_t size_netif = sizeof(struct netif);
 	hs->net_cfg.netif = calloc(1, size_netif);
 	ASSERT(hs->net_cfg.netif, "out of memory");
+	hs->net_cfg.dhcp_enabled = INIT_IP_CONFIG;
 
 	INFO_INIT("hs:%p size:0x%x netif:%p size:0x%x\n", hs, size_ctx_server,
 			hs->net_cfg.netif, size_netif);
-
+    initShell(hs);
 	timer_init(NULL, NULL);
     lwip_init();
         
diff --git a/firmwares/wifishield/wifiHD/src/wifi_spi.h b/firmwares/wifishield/wifiHD/src/wifi_spi.h
index 6e6b4bb..e2e262c 100644
--- a/firmwares/wifishield/wifiHD/src/wifi_spi.h
+++ b/firmwares/wifishield/wifiHD/src/wifi_spi.h
@@ -24,12 +24,16 @@
 #define START_CMD   0xE0
 #define END_CMD     0xEE
 #define ERR_CMD   	0xEF
+#define CMD_POS			1		// Position of Command OpCode on SPI stream
+#define PARAM_LEN_POS	2		// Position of Param len on SPI stream
   
 enum {
 	SET_NET_CMD 		= 0x10,
 	SET_PASSPHRASE_CMD	= 0x11,
 	SET_KEY_CMD	        = 0x12,
 	TEST_CMD	        = 0x13,
+	SET_IP_CONFIG_CMD	= 0x14,
+	SET_DNS_CONFIG_CMD  = 0x15,
 
 	GET_CONN_STATUS_CMD	= 0x20,
 	GET_IPADDR_CMD		= 0x21,
@@ -56,11 +60,14 @@ enum {
 	START_SCAN_NETWORKS	= 0x36,
 	GET_FW_VERSION_CMD	= 0x37,
 	GET_TEST_CMD		= 0x38,
+	SEND_DATA_UDP_CMD	= 0x39,
+	GET_REMOTE_DATA_CMD = 0x3A,
 
     // All command with DATA_FLAG 0x40 send a 16bit Len
 
 	SEND_DATA_TCP_CMD		= 0x44,
     GET_DATABUF_TCP_CMD		= 0x45,
+    INSERT_DATABUF_CMD		= 0x46,
 
 };
 
@@ -150,3 +157,4 @@ typedef struct  __attribute__((__packed__))
 }tByteParam;
 
 #endif
+uint8_t param;
\ No newline at end of file
diff --git a/firmwares/wifishield/wifiHD/src/wl_cm.c b/firmwares/wifishield/wifiHD/src/wl_cm.c
index bef1afc..ebc0e45 100644
--- a/firmwares/wifishield/wifiHD/src/wl_cm.c
+++ b/firmwares/wifishield/wifiHD/src/wl_cm.c
@@ -59,15 +59,9 @@
  */
 #define ROAMING_RSSI_DIFF 10
 
-
-#if 1
 # include "printf-stdarg.h"
 #include "ard_utils.h"
-# define CM_DPRINTF(fmt...) if (enableDebug & INFO_CM_FLAG) printk(fmt)
-#else
-# define CM_DPRINTF(fmt...)
-#endif
-
+#include "debug.h"
 
 /** \defgroup wl_cm Connection Manager
  *