3 files changed, 31 insertions, 15 deletions

diff --git a/firmwares/wifishield/wifiHD/src/ard_spi.c b/firmwares/wifishield/wifiHD/src/ard_spi.c
index ae92a62..66b3ba5 100644
--- a/firmwares/wifishield/wifiHD/src/ard_spi.c
+++ b/firmwares/wifishield/wifiHD/src/ard_spi.c
@@ -326,12 +326,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)
     		{
@@ -350,16 +346,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;
 }
 
@@ -1640,6 +1627,7 @@ inline int spi_slaveReceiveInt(volatile avr32_spi_t *spi)
 	int index = 0;
 	int err = SPI_OK;
 	state = SPI_CMD_INPUT;
+	bool endOfFrame = false;
 
 	do {
 		unsigned int timeout = SPI_TIMEOUT;
@@ -1680,7 +1668,31 @@ 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;
+			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);
+			}		
+		}
+	} while (!endOfFrame);
 	return err;
 }
 
diff --git a/firmwares/wifishield/wifiHD/src/ard_utils.c b/firmwares/wifishield/wifiHD/src/ard_utils.c
index 39c19dd..7bff763 100644
--- a/firmwares/wifishield/wifiHD/src/ard_utils.c
+++ b/firmwares/wifishield/wifiHD/src/ard_utils.c
@@ -78,6 +78,8 @@ uint16_t calcMergeLen(uint8_t sock)
 	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)
diff --git a/firmwares/wifishield/wifiHD/src/wifi_spi.h b/firmwares/wifishield/wifiHD/src/wifi_spi.h
index 48da984..4094e03 100644
--- a/firmwares/wifishield/wifiHD/src/wifi_spi.h
+++ b/firmwares/wifishield/wifiHD/src/wifi_spi.h
@@ -24,6 +24,8 @@
 #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,