5 files changed, 140 insertions, 166 deletions

diff --git a/firmwares/wifishield/wifiHD/src/ard_spi.c b/firmwares/wifishield/wifiHD/src/ard_spi.c
index 55c51cc..b79c1fc 100644
--- a/firmwares/wifishield/wifiHD/src/ard_spi.c
+++ b/firmwares/wifishield/wifiHD/src/ard_spi.c
@@ -538,13 +538,11 @@ int set_passphrase_cmd_cb(int numParam, char* buf, void* ctx) {
 
 int set_ip_config_cmd_cb(int numParam, char* buf, void* ctx) {
     struct ip_addr lwip_addr;
-	uint32_t _ip_addr = 0;
     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;
-    const uint8_t DNS_SERVER_IDX_CHANG = 2;
 
 	wl_err_t err = WL_SUCCESS;
 	tParam* params = (tParam*) buf;
@@ -607,7 +605,6 @@ int set_dns_config_cmd_cb(int numParam, char* buf, void* ctx) {
     struct netif *nif = ncfg->netif;
     uint8_t parmsToChange=0;
     const uint8_t MAX_DNS_CONFIG_PARAMS = 2;
-    const uint8_t DNS_SERVER_IDX_CHANG = 2;
 
 	wl_err_t err = WL_SUCCESS;
 	tParam* params = (tParam*) buf;
@@ -1787,7 +1784,7 @@ inline int spi_slaveReceiveInt(volatile avr32_spi_t *spi)
 		{
 			int8_t numParams = 0;
 			int idx = PARAM_LEN_POS+1;
-			bool islen16bit = _receiveBuffer[CMD_POS] & DATA_FLAG;
+			bool islen16bit = ((_receiveBuffer[CMD_POS] & DATA_FLAG) == DATA_FLAG);
 			if (index >= idx)
 			{
 				numParams = _receiveBuffer[PARAM_LEN_POS];					
@@ -1804,6 +1801,10 @@ inline int spi_slaveReceiveInt(volatile avr32_spi_t *spi)
 			}
 			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);
diff --git a/firmwares/wifishield/wifiHD/src/ard_tcp.c b/firmwares/wifishield/wifiHD/src/ard_tcp.c
index addbeca..c15858f 100644
--- a/firmwares/wifishield/wifiHD/src/ard_tcp.c
+++ b/firmwares/wifishield/wifiHD/src/ard_tcp.c
@@ -27,12 +27,11 @@
 #include "getopt.h"
 #include "ard_utils.h"
 #include "debug.h"
+#include "trace.h"
 
 unsigned int startTime = 0;
 extern bool ifStatus;
 
-static int isDataSentCount = 0;
-
 static err_t tcp_data_sent(void *arg, struct tcp_pcb *pcb, u16_t len);
 
 static void atcp_init_pend_flags(struct ttcp* _ttcp)
@@ -82,9 +81,7 @@ static void ard_tcp_destroy(struct ttcp* ttcp) {
 		udp_remove(ttcp->upcb);
 	}
 
-	if (ttcp->payload)
-		free(ttcp->payload);
-
+	FREE_PAYLOAD(ttcp);
 	free(ttcp);
 }
 
@@ -108,45 +105,44 @@ static void ard_tcp_done(struct ttcp* ttcp, int result) {
  * Called upon connect and when there's space available in the TCP send window
  * 
  */
-static err_t 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;
+	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, len);
+	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)
 		len = ttcp->buflen;
 
-	struct tcp_pcb * pcb = GET_FIRST_CLIENT_TCP_NV(ttcp);
 	/* We cannot send more data than space available in the send
 	 buffer. */
 	if (len > tcp_sndbuf(pcb))
 		len = tcp_sndbuf(pcb);
 
-	orig_len = len;
-
 	IF_TCP(startTime = timer_get_ms());
-	err = tcp_write(pcb, ttcp->payload, len, TCP_WRITE_FLAG_COPY);
+	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 = 0;
+		ttcp->buff_sent[id] = 0;
 	}else{
-		ttcp->buff_sent = 1;
-		isDataSentCount = 0;
-		ttcp->left -= len;
+		ttcp->buff_sent[id] = 1;
+		ttcp->left[id] -= len;
 	}
 
 	return err;
 }
 
+
 /**
  * Only used in TCP mode.
  */
@@ -155,10 +151,11 @@ static err_t tcp_connect_cb(void *arg, struct tcp_pcb *tpcb, err_t err) {
 
 	if (_ttcp == NULL) return ERR_ARG;
 
+	GET_CLIENT_ID(_ttcp, tpcb);
 	INFO_TCP("TTCP [%p-%p]: connect %d %d\n", _ttcp, tpcb, err, tpcb->state);
 
 	_connected =  ( tpcb->state == ESTABLISHED) ? 1 : 0;
-	_ttcp->tcp_poll_retries = 0;
+	_ttcp->tcp_poll_retries[id] = 0;
 
 	_ttcp->start_time = timer_get_ms();
 
@@ -198,13 +195,17 @@ static err_t close_conn_pcb(struct tcp_pcb* tpcb) {
 static void atcp_conn_err_cb(void *arg, err_t err) {
 	struct ttcp* _ttcp = arg;
 
-	WARN("TTCP [%p]: connection error: %d\n",
-			_ttcp, err);
+	WARN("TTCP [%p]: connection error: %d currId:%d\n",
+			_ttcp, err, getCurrClientConnId());
 
 	if (ifStatus == false)
 		printk("Abort connection\n");
 
-	atcp_init_pend_flags(_ttcp);
+	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) {
@@ -221,30 +222,32 @@ static void atcp_conn_cli_err_cb(void *arg, err_t err) {
 	if ((_ttcp)&&(err == ERR_ABRT))
 	{
 		WARN("TTCP [%p]: free memory\n", _ttcp);
-		_ttcp->tcp_poll_retries = 0;
 		cleanSockState_cb(_ttcp);
-		if (_ttcp->payload)
-			free(_ttcp->payload);
-		free(_ttcp);
+		// TODO
+		FREE_PAYLOAD(_ttcp);
 	}
 
-	atcp_init_pend_flags(_ttcp);
+	//atcp_init_pend_flags(_ttcp);
 }
 
 static err_t close_conn(struct ttcp *_ttcp, struct tcp_pcb* tpcb) {
 
 	if (_ttcp == NULL) return ERR_MEM;
 	
-	int8_t id = getNewClientConnId(_ttcp, tpcb);
-	if (id == NO_VALID_ID) return ERR_MEM;
+	GET_CLIENT_ID(_ttcp, tpcb);
+
 	err_t err = close_conn_pcb(_ttcp->tpcb[id]);
 
 	if (err == ERR_MEM)
+	{
+		WARN("Cannot close id:%d-%p put pending\n", id, _ttcp->tpcb[id]);
 		_ttcp->pending_close[id] = true;
+	}		
 	else{
-		atcp_init_pend_flags(_ttcp);
+		_ttcp->pending_close[id] = false;
 		removeNewClientConn(_ttcp, _ttcp->tpcb[id]);
-		WARN("----------------------\n");
+		FREE_PAYLOAD_ID(_ttcp, id);
+		INFO_TCP("----------------------\n");
 	}
 	return err;
 }
@@ -317,31 +320,32 @@ static err_t atcp_poll(void *arg, struct tcp_pcb *pcb) {
 	struct ttcp* _ttcp = arg;
 
 	if (_ttcp == NULL) return ERR_ARG;
+	
+	GET_CLIENT_ID(_ttcp, pcb);
 
-	if (_ttcp->left>0)
-		++_ttcp->tcp_poll_retries;
+	if (_ttcp->left[id]>0)
+		++_ttcp->tcp_poll_retries[id];
 
-	if (_ttcp->tcp_poll_retries > 4) {
-		WARN("ARD TCP [%p] arg=%p retries=%d\n",
-				pcb, arg, _ttcp->tcp_poll_retries);
-		_ttcp->tcp_poll_retries = 0;
+	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);
+		_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);
-
-	int8_t id = getNewClientConnId(_ttcp, pcb);
-	if (_ttcp->left > 0)
+	
+	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, _ttcp->pending_close[id], (_ttcp)?_ttcp->left:0);
-	tcp_send_data(_ttcp);
+			_ttcp->tcp_poll_retries[id], _ttcp->pending_close[id], (_ttcp)?_ttcp->left[id]:0);
+	tcp_send_data_pcb(_ttcp, pcb);
 
-	if ((id != NO_VALID_ID) && (_ttcp->pending_close[id]))
+	if (_ttcp->pending_close[id])
 	{		
 		err_t err = ERR_OK;
 		if (id >=0){
@@ -352,7 +356,10 @@ static err_t atcp_poll(void *arg, struct tcp_pcb *pcb) {
 			}
 			else
 			{
-				atcp_init_pend_flags(_ttcp);
+				_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, 
@@ -366,26 +373,27 @@ static err_t atcp_poll_conn(void *arg, struct tcp_pcb *pcb) {
 
 	if (_ttcp == NULL) return ERR_ARG;
 
-	int8_t id = getNewClientConnId(_ttcp, pcb);
+	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);
+			++(_ttcp->tcp_poll_retries[id]);
 	}
 
-	if (_ttcp->tcp_poll_retries > 8) {
+	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);
-		_ttcp->tcp_poll_retries = 0;
+				pcb, GET_FIRST_CLIENT_TCP(_ttcp), arg, _ttcp->tcp_poll_retries[id]);
+		_ttcp->tcp_poll_retries[id] = 0;
 		tcp_abort(pcb);
 		return ERR_ABRT;
 	}
 
-	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, _ttcp->pending_close[id], _connected);
-
-	if ((_ttcp)&&(_connected)) tcp_send_data(_ttcp);
+	if ((_ttcp)&&(_connected)) tcp_send_data_pcb(_ttcp, pcb);
 
 	if ((id != NO_VALID_ID) && (_ttcp->pending_close[id]))
 	{
@@ -397,11 +405,8 @@ static err_t atcp_poll_conn(void *arg, struct tcp_pcb *pcb) {
 		else
 		{
 			cleanSockState_cb(_ttcp);
-			if (_ttcp->payload)
-				free(_ttcp->payload);
-			free(_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)?GET_FIRST_CLIENT_TCP(_ttcp):0, _ttcp->pending_close[id]);
@@ -410,7 +415,6 @@ static err_t atcp_poll_conn(void *arg, struct tcp_pcb *pcb) {
 }
 
 int8_t currConnId = 0;
-#define GET_IDX_CONN(I) ((I+currConnId)<MAX_CLIENT_ACCEPTED ? (I+currConnId) : (I+currConnId-MAX_CLIENT_ACCEPTED))
 
 int8_t getCurrClientConnId() { return currConnId;}
 
@@ -424,7 +428,7 @@ int8_t getNewClientConnId(struct ttcp* _ttcp, struct tcp_pcb *newpcb)
 
 			if (_ttcp->tpcb[idx] == newpcb)
 			{
-				INFO_TCP("ttcp:%p id=%d, tpcb=%p\n", _ttcp, idx, newpcb);
+				INFO_TCP_VER("ttcp:%p id=%d, tpcb=%p\n", _ttcp, idx, newpcb);
 				return idx;
 			}				
 		}
@@ -523,6 +527,14 @@ static err_t atcp_accept_cb(void *arg, struct tcp_pcb *newpcb, err_t err) {
 	INFO_TCP("local:%d remote:%d state:%d\n", newpcb->local_port, newpcb->remote_port, newpcb->state);
 
 	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);
@@ -548,12 +560,7 @@ static int atcp_start(struct ttcp* ttcp) {
 		return -1;
 	}
 
-	ttcp->payload = malloc(ttcp->buflen);
-	if (ttcp->payload == NULL) {
-		WARN("TTCP [%p]: could not allocate payload\n", ttcp);
-		return -1;
-	}
-
+	currConnId = 0;
 	tcp_arg(p, ttcp);
 	atcp_init_pend_flags(ttcp);
 
@@ -565,7 +572,7 @@ static int atcp_start(struct ttcp* ttcp) {
 		tcp_sent(pcb, tcp_data_sent);
 		tcp_poll(pcb, atcp_poll_conn, 4);
 		_connected = false;
-		INFO_TCP("[tpcb]-%p payload:%p\n", pcb, ttcp->payload);
+		INFO_TCP("[tpcb]-%p payload:%p\n", pcb, ttcp->payload[currConnId]);
 		DUMP_TCP_STATE(ttcp);
 		if (tcp_connect(pcb, &ttcp->addr, ttcp->port, tcp_connect_cb)
 				!= ERR_OK) {
@@ -595,72 +602,6 @@ 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.
@@ -759,7 +700,6 @@ int ard_tcp_start(struct ip_addr addr, uint16_t port, void *opaque,
 	ttcp->port = port;
 	ttcp->nbuf = nbuf;
 	ttcp->mode = mode;
-	ttcp->left = 0;
 	ttcp->done_cb = done_cb;
 	ttcp->opaque = opaque;
 	ttcp->udp = udp;
@@ -782,7 +722,6 @@ int ard_tcp_start(struct ip_addr addr, uint16_t port, void *opaque,
 
 	*_ttcp = (void*) ttcp;
 	ttcp->sock = sock;
-	ttcp->buff_sent = 1;
 
 	return 0;
 
@@ -798,9 +737,10 @@ void ard_tcp_stop(void* ttcp) {
 		return;
 	}
 	if (_ttcp->mode == TTCP_MODE_TRANSMIT) {
+		int i = getCurrClientConnId();
 		ard_tcp_destroy(_ttcp);
     	clearMapSockTcp(getSock(_ttcp), GET_TCP_MODE(_ttcp));
-    	_ttcp->tcp_poll_retries = 0;
+    	_ttcp->tcp_poll_retries[i] = 0;
 	}else{
 		DUMP_TCP_STATE(_ttcp);
 
@@ -825,9 +765,15 @@ uint8_t getStateTcp(void* p, bool client) {
 	if ((_ttcp != NULL) && ((pcb != NULL) || (client==0))) {
 		IF_SPI_POLL(DUMP_TCP_STATE(_ttcp));
 		if (client)
+		{
+			if ((pcb->state != ESTABLISHED)&&(pcb->state != CLOSED))
+				DUMP_TCP_STATE(_ttcp);
 			return pcb->state;
+		}			
 		else
+		{
 			return _ttcp->lpcb->state;
+		}			
 	} else {
 		WARN_POLL("TCP not initialized ttcp:%p tpcb:%p lpcb:%p\n",
 				_ttcp, ((_ttcp)?pcb:0), ((_ttcp)?_ttcp->lpcb:0));
@@ -846,9 +792,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;
 	}
 
@@ -864,17 +810,15 @@ static err_t tcp_data_sent(void *arg, struct tcp_pcb *pcb, u16_t len) {
 
 	if (_ttcp == NULL) return ERR_ARG;
 
-	_ttcp->tcp_poll_retries = 0;
-	if (_ttcp) _ttcp->buff_sent = 1;
-
+	GET_CLIENT_ID(_ttcp, pcb);
+	_ttcp->tcp_poll_retries[id] = 0;
+	_ttcp->buff_sent[id] = 1;
 
-	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);
+	INFO_TCP("Packet sent pcb:%p len:%d dur:%d left:%d\n", pcb, len, timer_get_ms() - startTime,
+			(_ttcp)?(_ttcp->left[id]):0);
 
-	isDataSentCount = 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;
@@ -891,21 +835,23 @@ int sendTcpData(void* p, uint8_t* buf, uint16_t len)
 	}
 	
 	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));
 
 	if ((_ttcp != NULL) && (pcb != NULL) &&
-			(buf != NULL) && (len != 0) && (_ttcp->payload != 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;
+		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(_ttcp);
+		tcp_send_data_pcb(_ttcp, pcb);
 
 		return WL_SUCCESS;
 		}
diff --git a/firmwares/wifishield/wifiHD/src/ard_tcp.h b/firmwares/wifishield/wifiHD/src/ard_tcp.h
index 2bd3731..078e0b0 100644
--- a/firmwares/wifishield/wifiHD/src/ard_tcp.h
+++ b/firmwares/wifishield/wifiHD/src/ard_tcp.h
@@ -23,11 +23,32 @@ typedef void (ard_tcp_done_cb_t)(void *opaque, int result);
 
 // Maximum number of client connection accepted by server
 #define MAX_CLIENT_ACCEPTED			4
-#define NO_VALID_ID					-1
+#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 {
@@ -40,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;
@@ -55,8 +76,8 @@ typedef struct ttcp {
 	/* TCP specific */
 	struct tcp_pcb* tpcb[MAX_CLIENT_ACCEPTED];
 	struct tcp_pcb* lpcb;
-	char* payload;
-	uint8_t tcp_poll_retries;
+	char* payload[MAX_CLIENT_ACCEPTED];
+	uint8_t tcp_poll_retries[MAX_CLIENT_ACCEPTED];
 	bool pending_close[MAX_CLIENT_ACCEPTED];
 
 	/* UDP specific */
diff --git a/firmwares/wifishield/wifiHD/src/ard_utils.h b/firmwares/wifishield/wifiHD/src/ard_utils.h
index 5dee3c0..323b328 100644
--- a/firmwares/wifishield/wifiHD/src/ard_utils.h
+++ b/firmwares/wifishield/wifiHD/src/ard_utils.h
@@ -242,10 +242,11 @@
 #endif
 
 #define DUMP_TCP_STATE(TTCP) do {\
-		INFO_TCP("ttcp:%p tpcb:%p state:%d lpcb:%p state:%d left:%d sent:%d\n", \
-			TTCP, TTCP->tpcb[0], (TTCP->tpcb[0])?TTCP->tpcb[0]->state:0, \
+		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)?TTCP->left:0, (TTCP)?TTCP->buff_sent: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")			
diff --git a/firmwares/wifishield/wifiHD/src/debug.h b/firmwares/wifishield/wifiHD/src/debug.h
index e48da64..154b799 100644
--- a/firmwares/wifishield/wifiHD/src/debug.h
+++ b/firmwares/wifishield/wifiHD/src/debug.h
@@ -140,8 +140,13 @@ 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 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))