Updates to WiFi shield firmware.

1 files changed, 271 insertions, 179 deletions

diff --git a/firmwares/wifishield/wifiHD/src/ard_spi.c b/firmwares/wifishield/wifiHD/src/ard_spi.c
index a5ef895..ae92a62 100644
--- a/firmwares/wifishield/wifiHD/src/ard_spi.c
+++ b/firmwares/wifishield/wifiHD/src/ard_spi.c
@@ -24,6 +24,8 @@
 #include "timer.h"
 #include "lwip/dns.h"
 #include <board_init.h>
+#include "util.h"
+#include "lwip/udp.h"
 
 extern const char* fwVersion;
 
@@ -92,7 +94,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 34
 typedef struct sCmd_spi_list{
 	cmd_spi_cb_t cb;
 	char cmd_id;
@@ -136,6 +138,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 +172,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 +204,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 +218,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 +252,7 @@ void initMapSockTcp()
 	memset(mapSockTCP, 0, sizeof(mapSockTCP));
 }
 
+#if 0
 /**
  * Calculate bitrate based on number of bytes transmitted and elapsed time
  */
@@ -235,10 +265,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 +278,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();
@@ -323,8 +379,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 +395,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;
@@ -578,7 +585,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 +597,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 +612,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 +634,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 +717,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 +725,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 +765,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 +777,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 +794,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 +834,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 +1166,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 +1182,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 +1205,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 +1237,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 +1281,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 +1347,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 +1366,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;
@@ -1411,6 +1496,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 +1540,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 +1557,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 +1601,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 +1612,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
@@ -1553,11 +1641,6 @@ inline int spi_slaveReceiveInt(volatile avr32_spi_t *spi)
 	int err = SPI_OK;
 	state = SPI_CMD_INPUT;
 
-	if (_receiveBuffer[0] != 0)
-	{
-		STATSPI_OVERRIDE_ERROR();
-	}
-
 	do {
 		unsigned int timeout = SPI_TIMEOUT;
 		err = SPI_OK;
@@ -1569,8 +1652,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{
@@ -1597,8 +1692,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 +1704,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)