Merged 1.0.5

Merge remote-tracking branch 'arduino/master' into ide-1.5.x

Conflicts:
	app/src/processing/app/Base.java
	build/shared/revisions.txt
	hardware/arduino/avr/cores/arduino/malloc.c
	hardware/arduino/cores/arduino/avr-libc/malloc.c
	hardware/arduino/cores/arduino/malloc.c
	todo.txt

1 files changed, 443 insertions, 361 deletions

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\