From a5c38d11a91a92ce833cd3044aca3033ee33ee36 Mon Sep 17 00:00:00 2001
From: Mimmo La Fauci <mlafauci@metodo2.it>
Date: Tue, 5 Jun 2012 14:20:22 +0200
Subject: [PATCH] Fix issue with peek function

---
 WiFi/WiFiClient.cpp         | 34 +++++++++++++++++++++++++++++-----
 WiFi/utility/server_drv.cpp |  7 ++++---
 WiFi/utility/server_drv.h   |  2 +-
 3 files changed, 34 insertions(+), 9 deletions(-)

diff --git a/WiFi/WiFiClient.cpp b/WiFi/WiFiClient.cpp
index 29b7699d4..c6a416919 100755
--- a/WiFi/WiFiClient.cpp
+++ b/WiFi/WiFiClient.cpp
@@ -35,8 +35,22 @@ int WiFiClient::connect(IPAddress ip, uint16_t port) {
     {
     	ServerDrv::startClient(uint32_t(ip), port, _sock);
     	WiFiClass::_state[_sock] = _sock;
-    	while(!connected());
+
+    	unsigned long start = millis();
+
+    	// wait 4 second for the connection to close
+    	while (!connected() && millis() - start < 10000)
+    		delay(1);
+
+    	if (!connected())
+       	{
+    		Serial.println("timeout on client connection");
+    		return 0;
+    	}
+    	else
+    		Serial.println("CONNECTED");
     }else{
+    	Serial.println("No Socket available");
     	return 0;
     }
     return 1;
@@ -94,8 +108,12 @@ int WiFiClient::read(uint8_t* buf, size_t size) {
 }
 
 int WiFiClient::peek() {
-	//TODO to be implemented
-	return 0;
+	  uint8_t b;
+	  if (!available())
+	    return -1;
+
+	  ServerDrv::getData(_sock, &b, 1);
+	  return b;
 }
 
 void WiFiClient::flush() {
@@ -112,10 +130,11 @@ void WiFiClient::stop() {
 
   unsigned long start = millis();
   
+
   // wait a second for the connection to close
   while (status() != CLOSED && millis() - start < 1000)
     delay(1);
-    
+  Serial.print("Stop client! Status:");Serial.println(status(),10);
   _sock = 255;
 }
 
@@ -125,7 +144,12 @@ uint8_t WiFiClient::connected() {
     return 0;
   } else {
     uint8_t s = status();
-
+/*
+    if (s== SYN_SENT) Serial.print("*");
+    else{
+    	Serial.print("Status:"); Serial.println(s,10);
+    }
+*/
     return !(s == LISTEN || s == CLOSED || s == FIN_WAIT_1 ||
     		s == FIN_WAIT_2 || s == TIME_WAIT ||
     		s == SYN_SENT || s== SYN_RCVD ||
diff --git a/WiFi/utility/server_drv.cpp b/WiFi/utility/server_drv.cpp
index 2dca2f3cc..a325d5a06 100644
--- a/WiFi/utility/server_drv.cpp
+++ b/WiFi/utility/server_drv.cpp
@@ -146,12 +146,13 @@ uint8_t ServerDrv::availData(uint8_t sock)
     return false;
 }
 
-bool ServerDrv::getData(uint8_t sock, uint8_t *data)
+bool ServerDrv::getData(uint8_t sock, uint8_t *data, uint8_t peek)
 {
 	WAIT_FOR_SLAVE_SELECT();
     // Send Command
-    SpiDrv::sendCmd(GET_DATA_TCP_CMD, PARAM_NUMS_1);
-    SpiDrv::sendParam(&sock, sizeof(sock), LAST_PARAM);
+    SpiDrv::sendCmd(GET_DATA_TCP_CMD, PARAM_NUMS_2);
+    SpiDrv::sendParam(&sock, sizeof(sock));
+    SpiDrv::sendParam(peek, LAST_PARAM);
 
     //Wait the reply elaboration
     SpiDrv::waitForSlaveReady();
diff --git a/WiFi/utility/server_drv.h b/WiFi/utility/server_drv.h
index 6e2eebda7..69ba593e9 100644
--- a/WiFi/utility/server_drv.h
+++ b/WiFi/utility/server_drv.h
@@ -18,7 +18,7 @@ public:
 
     static uint8_t getClientState(uint8_t sock);
 
-    static bool getData(uint8_t sock, uint8_t *data);
+    static bool getData(uint8_t sock, uint8_t *data, uint8_t peek = 0);
 
     static bool getDataBuf(uint8_t sock, uint8_t *data, uint16_t *len);