From 4d50903844105eee57aa6a50bbd51d9849894701 Mon Sep 17 00:00:00 2001
From: PaulStoffregen <paul@pjrc.com>
Date: Thu, 31 Dec 2015 17:35:06 -0800
Subject: [PATCH] Socket layer rewrite (work in progress - currently broken)

---
 Ethernet.cpp       |   6 +-
 Ethernet.h         |   3 +-
 EthernetClient.cpp |  96 +++++++++-----------------------
 EthernetClient.h   |  13 +++--
 EthernetServer.cpp |  59 +++++++++++---------
 EthernetServer.h   |   2 +
 EthernetUdp.cpp    | 122 ++++++++++++-----------------------------
 EthernetUdp.h      |   6 +-
 utility/socket.cpp | 133 +++++++++++++++++++++++++++++++++------------
 utility/socket.h   |  66 +++++++++++++++-------
 10 files changed, 252 insertions(+), 254 deletions(-)

diff --git a/Ethernet.cpp b/Ethernet.cpp
index 6c06db8..e409230 100644
--- a/Ethernet.cpp
+++ b/Ethernet.cpp
@@ -3,10 +3,8 @@
 #include "Dhcp.h"
 
 // XXX: don't make assumptions about the value of MAX_SOCK_NUM.
-uint8_t EthernetClass::_state[MAX_SOCK_NUM] = { 
-  0, 0, 0, 0 };
-uint16_t EthernetClass::_server_port[MAX_SOCK_NUM] = { 
-  0, 0, 0, 0 };
+//uint16_t EthernetClass::_server_port[MAX_SOCK_NUM] = { 
+//  0, 0, 0, 0 };
 
 int EthernetClass::begin(uint8_t *mac_address, unsigned long timeout, unsigned long responseTimeout)
 {
diff --git a/Ethernet.h b/Ethernet.h
index 083df44..618a8d0 100644
--- a/Ethernet.h
+++ b/Ethernet.h
@@ -15,8 +15,7 @@ class EthernetClass {
   IPAddress _dnsServerAddress;
   DhcpClass* _dhcp;
 public:
-  static uint8_t _state[MAX_SOCK_NUM];
-  static uint16_t _server_port[MAX_SOCK_NUM];
+  //static uint16_t _server_port[MAX_SOCK_NUM];
   // Initialise the Ethernet shield to use the provided MAC address and gain the rest of the
   // configuration through DHCP.
   // Returns 0 if the DHCP configuration failed, and 1 if it succeeded
diff --git a/EthernetClient.cpp b/EthernetClient.cpp
index 60fb37c..1c0ca95 100644
--- a/EthernetClient.cpp
+++ b/EthernetClient.cpp
@@ -12,13 +12,7 @@ extern "C" {
 #include "EthernetServer.h"
 #include "Dns.h"
 
-uint16_t EthernetClient::_srcport = 49152;      //Use IANA recommended ephemeral port range 49152-65535
-
-EthernetClient::EthernetClient() : _sock(MAX_SOCK_NUM) {
-}
-
-EthernetClient::EthernetClient(uint8_t sock) : _sock(sock) {
-}
+uint16_t EthernetClient::srcport = 49152;      //Use IANA recommended ephemeral port range 49152-65535
 
 int EthernetClient::connect(const char* host, uint16_t port) {
   // Look up the host first
@@ -36,37 +30,15 @@ int EthernetClient::connect(const char* host, uint16_t port) {
 }
 
 int EthernetClient::connect(IPAddress ip, uint16_t port) {
-  if (_sock != MAX_SOCK_NUM)
-    return 0;
-
-  for (int i = 0; i < MAX_SOCK_NUM; i++) {
-    uint8_t s = socketStatus(i);
-    if (s == SnSR::CLOSED || s == SnSR::FIN_WAIT || s == SnSR::CLOSE_WAIT) {
-      _sock = i;
-      break;
-    }
-  }
-
-  if (_sock == MAX_SOCK_NUM)
-    return 0;
-
-  _srcport++;
-  if (_srcport == 0) _srcport = 49152;          //Use IANA recommended ephemeral port range 49152-65535
-  socket(_sock, SnMR::TCP, _srcport, 0);
-
-  if (!::connect(_sock, rawIPAddress(ip), port)) {
-    _sock = MAX_SOCK_NUM;
-    return 0;
-  }
+  if (ip == IPAddress(0ul) || ip == IPAddress(0xFFFFFFFFul)) return 0;
+  if (++srcport == 0) srcport = 49152; // IANA recommended ephemeral port range 49152-65535
 
+  if (!sock.begin(SnMR::TCP, srcport)) return 0;
+  sock.connect(rawIPAddress(ip), port);
   while (status() != SnSR::ESTABLISHED) {
+    if (!sock) return 0;
     delay(1);
-    if (status() == SnSR::CLOSED) {
-      _sock = MAX_SOCK_NUM;
-      return 0;
-    }
   }
-
   return 1;
 }
 
@@ -75,11 +47,7 @@ size_t EthernetClient::write(uint8_t b) {
 }
 
 size_t EthernetClient::write(const uint8_t *buf, size_t size) {
-  if (_sock == MAX_SOCK_NUM) {
-    setWriteError();
-    return 0;
-  }
-  if (!send(_sock, buf, size)) {
+  if (!sock || !sock.send(buf, size)) {
     setWriteError();
     return 0;
   }
@@ -87,69 +55,59 @@ size_t EthernetClient::write(const uint8_t *buf, size_t size) {
 }
 
 int EthernetClient::available() {
-  if (_sock != MAX_SOCK_NUM)
-    return recvAvailable(_sock);
-  return 0;
+  if (!sock) return 0;
+  return sock.recvAvailable();
 }
 
 int EthernetClient::read() {
+  if (!sock) return -1;
   uint8_t b;
-  if ( recv(_sock, &b, 1) > 0 )
-  {
+  if (sock.recv(&b, 1) > 0 ) {
     // recv worked
     return b;
-  }
-  else
-  {
+  } else {
     // No data available
     return -1;
   }
 }
 
 int EthernetClient::read(uint8_t *buf, size_t size) {
-  return recv(_sock, buf, size);
+  return sock.recv(buf, size);
 }
 
 int EthernetClient::peek() {
-  uint8_t b;
   // Unlike recv, peek doesn't check to see if there's any data available, so we must
-  if (!available())
-    return -1;
-  ::peek(_sock, &b);
+  if (!sock || !available()) return -1;
+  uint8_t b;
+  sock.peek(&b);
   return b;
 }
 
 void EthernetClient::flush() {
-  ::flush(_sock);
+  sock.flush();
 }
 
 void EthernetClient::stop() {
-  if (_sock == MAX_SOCK_NUM)
-    return;
+  if (!sock) return;
 
   // attempt to close the connection gracefully (send a FIN to other side)
-  disconnect(_sock);
+  sock.disconnect();
   unsigned long start = millis();
 
   // wait up to a second for the connection to close
   uint8_t s;
   do {
     s = status();
-    if (s == SnSR::CLOSED)
-      break; // exit the loop
+    if (s == SnSR::CLOSED) break; // exit the loop
     delay(1);
   } while (millis() - start < 1000);
 
   // if it hasn't closed, close it forcefully
-  if (s != SnSR::CLOSED)
-    close(_sock);
-
-  EthernetClass::_server_port[_sock] = 0;
-  _sock = MAX_SOCK_NUM;
+  if (s != SnSR::CLOSED) sock.close();
 }
 
 uint8_t EthernetClient::connected() {
-  if (_sock == MAX_SOCK_NUM) return 0;
+  if (!sock) return 0;
   
   uint8_t s = status();
   return !(s == SnSR::LISTEN || s == SnSR::CLOSED || s == SnSR::FIN_WAIT ||
@@ -157,17 +115,13 @@ uint8_t EthernetClient::connected() {
 }
 
 uint8_t EthernetClient::status() {
-  if (_sock == MAX_SOCK_NUM) return SnSR::CLOSED;
-  return socketStatus(_sock);
+  if (!sock) return SnSR::CLOSED;
+  return sock.socketStatus();
 }
 
 // the next function allows us to use the client returned by
 // EthernetServer::available() as the condition in an if-statement.
 
-EthernetClient::operator bool() {
-  return _sock != MAX_SOCK_NUM;
-}
-
 bool EthernetClient::operator==(const EthernetClient& rhs) {
-  return _sock == rhs._sock && _sock != MAX_SOCK_NUM && rhs._sock != MAX_SOCK_NUM;
+  return sock && rhs.sock && getSocketNumber() == rhs.getSocketNumber();
 }
diff --git a/EthernetClient.h b/EthernetClient.h
index ba59052..68ebd9b 100644
--- a/EthernetClient.h
+++ b/EthernetClient.h
@@ -4,12 +4,13 @@
 #include "Print.h"
 #include "Client.h"
 #include "IPAddress.h"
+#include "utility/socket.h"
 
 class EthernetClient : public Client {
 
 public:
-  EthernetClient();
-  EthernetClient(uint8_t sock);
+  EthernetClient() { };
+  EthernetClient(W5000socket &socket) { sock.moveTo(socket); }
 
   uint8_t status();
   virtual int connect(IPAddress ip, uint16_t port);
@@ -23,20 +24,20 @@ class EthernetClient : public Client {
   virtual void flush();
   virtual void stop();
   virtual uint8_t connected();
-  virtual operator bool();
+  virtual operator bool() { return (bool)sock; }
   virtual bool operator==(const bool value) { return bool() == value; }
   virtual bool operator!=(const bool value) { return bool() != value; }
   virtual bool operator==(const EthernetClient&);
   virtual bool operator!=(const EthernetClient& rhs) { return !this->operator==(rhs); };
-  uint8_t getSocketNumber() { return _sock; }
+  uint8_t getSocketNumber() const { return sock.getSocketNumber(); }
 
   friend class EthernetServer;
   
   using Print::write;
 
 private:
-  static uint16_t _srcport;
-  uint8_t _sock;
+  static uint16_t srcport;
+  W5000socket sock;
 };
 
 #endif
diff --git a/EthernetServer.cpp b/EthernetServer.cpp
index df60d2b..87562c5 100644
--- a/EthernetServer.cpp
+++ b/EthernetServer.cpp
@@ -15,43 +15,38 @@ EthernetServer::EthernetServer(uint16_t port)
 
 void EthernetServer::begin()
 {
-  for (int sock = 0; sock < MAX_SOCK_NUM; sock++) {
-    EthernetClient client(sock);
-    if (client.status() == SnSR::CLOSED) {
-      socket(sock, SnMR::TCP, _port, 0);
-      listen(sock);
-      EthernetClass::_server_port[sock] = _port;
-      break;
-    }
-  }  
+  sock.begin(SnMR::TCP, _port);
+  if (sock) sock.listen();
 }
 
 void EthernetServer::accept()
 {
-  int listening = 0;
-
-  for (int sock = 0; sock < MAX_SOCK_NUM; sock++) {
-    EthernetClient client(sock);
-
-    if (EthernetClass::_server_port[sock] == _port) {
-      if (client.status() == SnSR::LISTEN) {
-        listening = 1;
-      } 
-      else if (client.status() == SnSR::CLOSE_WAIT && !client.available()) {
-        client.stop();
-      }
-    } 
-  }
+  bool listening = false;
 
-  if (!listening) {
-    begin();
+  if (sock) {
+    uint8_t status = sock.socketStatus();
+    if (status == SnSR::LISTEN) {
+      listening = true;
+    } else if (status == SnSR::CLOSE_WAIT && sock.recvAvailable() <= 0) {
+      sock.disconnect();
+      // TODO: wait?
+    }
   }
+  if (!listening) begin();
 }
 
 EthernetClient EthernetServer::available()
 {
   accept();
-
+  if (sock) {
+    uint8_t stat = sock.socketStatus();
+    if (stat == SnSR::ESTABLISHED || (stat == SnSR::CLOSE_WAIT && sock.recvAvailable() > 0)) {
+      EthernetClient client(sock);
+      return client;
+    }
+  }
+  return EthernetClient();
+/*
   for (int sock = 0; sock < MAX_SOCK_NUM; sock++) {
     EthernetClient client(sock);
     if (EthernetClass::_server_port[sock] == _port) {
@@ -66,6 +61,7 @@ EthernetClient EthernetServer::available()
   }
 
   return EthernetClient(MAX_SOCK_NUM);
+*/
 }
 
 size_t EthernetServer::write(uint8_t b) 
@@ -78,7 +74,15 @@ size_t EthernetServer::write(const uint8_t *buffer, size_t size)
   size_t n = 0;
   
   accept();
-
+  if (sock) {
+    uint8_t stat = sock.socketStatus();
+    if (stat == SnSR::ESTABLISHED) {
+      return sock.send(buffer, size);
+    }
+  }
+  return 0;
+}
+/*
   for (int sock = 0; sock < MAX_SOCK_NUM; sock++) {
     EthernetClient client(sock);
 
@@ -90,3 +94,4 @@ size_t EthernetServer::write(const uint8_t *buffer, size_t size)
   
   return n;
 }
+*/
diff --git a/EthernetServer.h b/EthernetServer.h
index 86ccafe..9d21ac7 100644
--- a/EthernetServer.h
+++ b/EthernetServer.h
@@ -2,6 +2,7 @@
 #define ethernetserver_h
 
 #include "Server.h"
+#include "utility/socket.h"
 
 class EthernetClient;
 
@@ -9,6 +10,7 @@ class EthernetServer :
 public Server {
 private:
   uint16_t _port;
+  W5000socket sock;
   void accept();
 public:
   EthernetServer(uint16_t);
diff --git a/EthernetUdp.cpp b/EthernetUdp.cpp
index cab8edd..47fdd8f 100644
--- a/EthernetUdp.cpp
+++ b/EthernetUdp.cpp
@@ -32,30 +32,14 @@
 #include "Udp.h"
 #include "Dns.h"
 
-/* Constructor */
-EthernetUDP::EthernetUDP() : _sock(MAX_SOCK_NUM) {}
 
 /* Start EthernetUDP socket, listening at local port PORT */
 uint8_t EthernetUDP::begin(uint16_t port) {
-  if (_sock != MAX_SOCK_NUM)
-    return 0;
-
-  for (int i = 0; i < MAX_SOCK_NUM; i++) {
-    uint8_t s = socketStatus(i);
-    if (s == SnSR::CLOSED || s == SnSR::FIN_WAIT) {
-      _sock = i;
-      break;
-    }
-  }
-
-  if (_sock == MAX_SOCK_NUM)
-    return 0;
-
-  _port = port;
-  _remaining = 0;
-  socket(_sock, SnMR::UDP, _port, 0);
-
-  return 1;
+	if (sock) return 0;
+	if (!sock.begin(SnMR::UDP, 0)) return 0;
+	_port = port;
+	_remaining = 0;
+	return 1;
 }
 
 /* return number of bytes available in the current packet,
@@ -67,13 +51,8 @@ int EthernetUDP::available() {
 /* Release any resources being used by this EthernetUDP instance */
 void EthernetUDP::stop()
 {
-  if (_sock == MAX_SOCK_NUM)
-    return;
-
-  close(_sock);
-
-  EthernetClass::_server_port[_sock] = 0;
-  _sock = MAX_SOCK_NUM;
+	if (!sock) return;
+	sock.close();
 }
 
 int EthernetUDP::beginPacket(const char *host, uint16_t port)
@@ -95,12 +74,12 @@ int EthernetUDP::beginPacket(const char *host, uint16_t port)
 int EthernetUDP::beginPacket(IPAddress ip, uint16_t port)
 {
   _offset = 0;
-  return startUDP(_sock, rawIPAddress(ip), port);
+  return sock.startUDP(rawIPAddress(ip), port);
 }
 
 int EthernetUDP::endPacket()
 {
-  return sendUDP(_sock);
+  return sock.sendUDP();
 }
 
 size_t EthernetUDP::write(uint8_t byte)
@@ -110,7 +89,7 @@ size_t EthernetUDP::write(uint8_t byte)
 
 size_t EthernetUDP::write(const uint8_t *buffer, size_t size)
 {
-  uint16_t bytes_written = bufferData(_sock, _offset, buffer, size);
+  uint16_t bytes_written = sock.bufferData(_offset, buffer, size);
   _offset += bytes_written;
   return bytes_written;
 }
@@ -125,15 +104,13 @@ int EthernetUDP::parsePacket()
     read();
   }
 
-  if (recvAvailable(_sock) > 0)
-  {
+  if (sock.recvAvailable() > 0) {
     //HACK - hand-parse the UDP packet using TCP recv method
     uint8_t tmpBuf[8];
-    int ret =0; 
+    int ret=0; 
     //read 8 header bytes and get IP and port from it
-    ret = recv(_sock,tmpBuf,8);
-    if (ret > 0)
-    {
+    ret = sock.recv(tmpBuf, 8);
+    if (ret > 0) {
       _remoteIP = tmpBuf;
       _remotePort = tmpBuf[4];
       _remotePort = (_remotePort << 8) + tmpBuf[5];
@@ -153,8 +130,7 @@ int EthernetUDP::read()
 {
   uint8_t byte;
 
-  if ((_remaining > 0) && (recv(_sock, &byte, 1) > 0))
-  {
+  if ((_remaining > 0) && (sock.recv(&byte, 1) > 0)) {
     // We read things without any problems
     _remaining--;
     return byte;
@@ -166,35 +142,23 @@ int EthernetUDP::read()
 
 int EthernetUDP::read(unsigned char* buffer, size_t len)
 {
-
-  if (_remaining > 0)
-  {
-
+  if (_remaining > 0) {
     int got;
-
-    if (_remaining <= len)
-    {
+    if (_remaining <= len) {
       // data should fit in the buffer
-      got = recv(_sock, buffer, _remaining);
-    }
-    else
-    {
+      got = sock.recv(buffer, _remaining);
+    } else {
       // too much data for the buffer, 
       // grab as much as will fit
-      got = recv(_sock, buffer, len);
+      got = sock.recv(buffer, len);
     }
-
-    if (got > 0)
-    {
+    if (got > 0) {
       _remaining -= got;
       return got;
     }
-
   }
-
   // If we get here, there's no data available or recv failed
   return -1;
-
 }
 
 int EthernetUDP::peek()
@@ -205,7 +169,7 @@ int EthernetUDP::peek()
   // may get the UDP header
   if (!_remaining)
     return -1;
-  ::peek(_sock, &b);
+  sock.peek(&b);
   return b;
 }
 
@@ -217,33 +181,19 @@ void EthernetUDP::flush()
 /* Start EthernetUDP socket, listening at local port PORT */
 uint8_t EthernetUDP::beginMulticast(IPAddress ip, uint16_t port)
 {
-  if (_sock != MAX_SOCK_NUM)
-    return 0;
-
-  for (int i = 0; i < MAX_SOCK_NUM; i++) {
-    uint8_t s = W5100.readSnSR(i);
-    if (s == SnSR::CLOSED || s == SnSR::FIN_WAIT) {
-      _sock = i;
-      break;
-    }
-  }
-
-  if (_sock == MAX_SOCK_NUM)
-    return 0;
-
-  // Calculate MAC address from Multicast IP Address
-  byte mac[] = {  0x01, 0x00, 0x5E, 0x00, 0x00, 0x00 };
-
-  mac[3] = ip[1] & 0x7F;
-  mac[4] = ip[2];
-  mac[5] = ip[3];
-
-  W5100.writeSnDIPR(_sock, rawIPAddress(ip));   //239.255.0.1
-  W5100.writeSnDPORT(_sock, port);
-  W5100.writeSnDHAR(_sock,mac);
-
-  _remaining = 0;
-  socket(_sock, SnMR::UDP, port, SnMR::MULTI);
-  return 1;
+	if (sock) return 0;
+	if (!sock.begin(SnMR::UDP | SnMR::MULTI, 0)) return 0;
+	_port = port;
+	_remaining = 0;
+	// Calculate MAC address from Multicast IP Address
+	byte mac[] = {  0x01, 0x00, 0x5E, 0x00, 0x00, 0x00 };
+	mac[3] = ip[1] & 0x7F;
+	mac[4] = ip[2];
+	mac[5] = ip[3];
+	SOCKET s = sock.getSocketNumber();
+	W5100.writeSnDIPR(s, rawIPAddress(ip));   //239.255.0.1
+	W5100.writeSnDPORT(s, port);
+	W5100.writeSnDHAR(s, mac);
+	return 1;
 }
 
diff --git a/EthernetUdp.h b/EthernetUdp.h
index 36e015f..8b44da1 100644
--- a/EthernetUdp.h
+++ b/EthernetUdp.h
@@ -38,6 +38,7 @@
 #define ethernetudp_h
 
 #include <Udp.h>
+#include "utility/socket.h"
 
 #define UDP_TX_PACKET_MAX_SIZE 24
 
@@ -49,11 +50,12 @@ class EthernetUDP : public UDP {
   uint16_t _offset; // offset into the packet being sent
 
 protected:
-  uint8_t _sock;  // socket ID for Wiz5100
+  //uint8_t _sock;  // socket ID for Wiz5100
+  W5000socket sock;
   uint16_t _remaining; // remaining bytes of incoming packet yet to be processed
 
 public:
-  EthernetUDP();  // Constructor
+  EthernetUDP() {}  // Constructor
   virtual uint8_t begin(uint16_t);	// initialize, start listening on specified port. Returns 1 if successful, 0 if there are no sockets available to use
   virtual uint8_t beginMulticast(IPAddress, uint16_t);  // initialize, start listening on specified port. Returns 1 if successful, 0 if there are no sockets available to use
   virtual void stop();  // Finish with the UDP socket
diff --git a/utility/socket.cpp b/utility/socket.cpp
index 7430722..22e2e09 100644
--- a/utility/socket.cpp
+++ b/utility/socket.cpp
@@ -1,5 +1,8 @@
 #include "w5100.h"
 #include "socket.h"
+#include "IPAddress.h"
+
+#include "Arduino.h"
 
 #if ARDUINO >= 156 || TEENSYDUINO >= 120
 extern void yield(void);
@@ -9,16 +12,88 @@ extern void yield(void);
 
 static uint16_t local_port;
 
+uint8_t W5000socket::begin(uint8_t protocol, uint16_t port)
+{
+	Serial.printf("W5000socket begin, s=%d, protocol=%d, port=%d\n", s, protocol, port);
+	SPI.beginTransaction(SPI_ETHERNET_SETTINGS);
+	uint8_t status[MAX_SOCK_NUM];
+	if (s < MAX_SOCK_NUM) {
+		Serial.printf("W5000socket step1\n");
+		// we already have a hardware socket
+		uint8_t stat;
+		stat = W5100.readSnSR(s);
+		if (stat == SnSR::CLOSED) {
+			// closed, so just use it
+			goto makesocket;
+		} else if (stat == SnSR::ESTABLISHED || stat == SnSR::INIT
+		  || stat == SnSR::CLOSE_WAIT) {
+			// connected (TCP), try disconnect
+  			W5100.execCmdSn(s, Sock_DISCON);
+		}
+		status[s] = stat;
+	}
+	Serial.printf("W5000socket step2\n");
+	// look at all the hardware sockets, use any closed 
+	for (s=0; s < MAX_SOCK_NUM; s++) {
+		status[s] = W5100.readSnSR(s);
+		if (status[s] == SnSR::CLOSED) goto makesocket;
+	}
+	Serial.printf("W5000socket step3\n");
+	// next, use any that are effectively closed
+	for (s=0; s < MAX_SOCK_NUM; s++) {
+		uint8_t stat = status[s];
+		if (stat == SnSR::CLOSE_WAIT) goto closemakesocket;
+	}
+	Serial.printf("W5000socket step4\n");
+	// as a last resort, forcibly close any already closing
+	for (s=0; s < MAX_SOCK_NUM; s++) {
+		uint8_t stat = status[s];
+		if (stat == SnSR::LAST_ACK) goto closemakesocket;
+		if (stat == SnSR::TIME_WAIT) goto closemakesocket;
+		if (stat == SnSR::FIN_WAIT) goto closemakesocket;
+		if (stat == SnSR::CLOSING) goto closemakesocket;
+	}
+	SPI.endTransaction();
+	return 0; // all sockets are in use
+closemakesocket:
+	Serial.printf("W5000socket close\n");
+	W5100.execCmdSn(s, Sock_CLOSE);
+	W5100.writeSnIR(s, 0xFF);
+makesocket:
+	Serial.printf("W5000socket %d\n", s);
+	W5100.writeSnMR(s, protocol);
+	if (port > 0) {
+		W5100.writeSnPORT(s, port);
+	} else {
+		// if don't set the source port, set local_port number.
+		W5100.writeSnPORT(s, ++local_port);
+	}
+	W5100.execCmdSn(s, Sock_OPEN);
+	SPI.endTransaction();
+	return 1;
+}
+
+void W5000socket::moveTo(W5000socket &rhs)
+{
+	if (s < MAX_SOCK_NUM) close();
+	s = rhs.s;
+	rhs.s = MAX_SOCK_NUM;
+}
+
+
+#if 0
 /**
  * @brief	This Socket function initialize the channel in perticular mode, and set the port and wait for W5100 done it.
  * @return 	1 for success else 0.
  */
-uint8_t socket(SOCKET s, uint8_t protocol, uint16_t port, uint8_t flag)
+uint8_t W5000socket::socket(SOCKET sock, uint8_t protocol, uint16_t port, uint8_t flag)
 {
   if ((protocol == SnMR::TCP) || (protocol == SnMR::UDP) || (protocol == SnMR::IPRAW) || (protocol == SnMR::MACRAW) || (protocol == SnMR::PPPOE))
   {
-    close(s);
+    s = sock;
     SPI.beginTransaction(SPI_ETHERNET_SETTINGS);
+    W5100.execCmdSn(s, Sock_CLOSE);
+    W5100.writeSnIR(s, 0xFF);
     W5100.writeSnMR(s, protocol | flag);
     if (port != 0) {
       W5100.writeSnPORT(s, port);
@@ -34,26 +109,27 @@ uint8_t socket(SOCKET s, uint8_t protocol, uint16_t port, uint8_t flag)
 
   return 0;
 }
+#endif
 
-
-uint8_t socketStatus(SOCKET s)
+uint8_t W5000socket::socketStatus()
 {
   SPI.beginTransaction(SPI_ETHERNET_SETTINGS);
   uint8_t status = W5100.readSnSR(s);
   SPI.endTransaction();
+  if (status == SnSR::CLOSED) s = MAX_SOCK_NUM;
   return status;
 }
 
-
 /**
  * @brief	This function close the socket and parameter is "s" which represent the socket number
  */
-void close(SOCKET s)
+void W5000socket::close()
 {
   SPI.beginTransaction(SPI_ETHERNET_SETTINGS);
   W5100.execCmdSn(s, Sock_CLOSE);
   W5100.writeSnIR(s, 0xFF);
   SPI.endTransaction();
+  s = MAX_SOCK_NUM;
 }
 
 
@@ -61,7 +137,7 @@ void close(SOCKET s)
  * @brief	This function established  the connection for the channel in passive (server) mode. This function waits for the request from the peer.
  * @return	1 for success else 0.
  */
-uint8_t listen(SOCKET s)
+uint8_t W5000socket::listen()
 {
   SPI.beginTransaction(SPI_ETHERNET_SETTINGS);
   if (W5100.readSnSR(s) != SnSR::INIT) {
@@ -76,28 +152,15 @@ uint8_t listen(SOCKET s)
 
 /**
  * @brief	This function established  the connection for the channel in Active (client) mode. 
- * 		This function waits for the untill the connection is established.
- * 		
- * @return	1 for success else 0.
  */
-uint8_t connect(SOCKET s, uint8_t * addr, uint16_t port)
+void W5000socket::connect(uint8_t * addr, uint16_t port)
 {
-  if 
-    (
-  ((addr[0] == 0xFF) && (addr[1] == 0xFF) && (addr[2] == 0xFF) && (addr[3] == 0xFF)) ||
-    ((addr[0] == 0x00) && (addr[1] == 0x00) && (addr[2] == 0x00) && (addr[3] == 0x00)) ||
-    (port == 0x00) 
-    ) 
-    return 0;
-
   // set destination IP
   SPI.beginTransaction(SPI_ETHERNET_SETTINGS);
   W5100.writeSnDIPR(s, addr);
   W5100.writeSnDPORT(s, port);
   W5100.execCmdSn(s, Sock_CONNECT);
   SPI.endTransaction();
-
-  return 1;
 }
 
 
@@ -106,7 +169,7 @@ uint8_t connect(SOCKET s, uint8_t * addr, uint16_t port)
  * @brief	This function used for disconnect the socket and parameter is "s" which represent the socket number
  * @return	1 for success else 0.
  */
-void disconnect(SOCKET s)
+void W5000socket::disconnect()
 {
   SPI.beginTransaction(SPI_ETHERNET_SETTINGS);
   W5100.execCmdSn(s, Sock_DISCON);
@@ -118,7 +181,7 @@ void disconnect(SOCKET s)
  * @brief	This function used to send the data in TCP mode
  * @return	1 for success else 0.
  */
-uint16_t send(SOCKET s, const uint8_t * buf, uint16_t len)
+uint16_t W5000socket::send(const uint8_t * buf, uint16_t len)
 {
   uint8_t status=0;
   uint16_t ret=0;
@@ -157,7 +220,7 @@ uint16_t send(SOCKET s, const uint8_t * buf, uint16_t len)
     if ( W5100.readSnSR(s) == SnSR::CLOSED )
     {
       SPI.endTransaction();
-      close(s);
+      close();
       return 0;
     }
     SPI.endTransaction();
@@ -177,7 +240,7 @@ uint16_t send(SOCKET s, const uint8_t * buf, uint16_t len)
  * 		
  * @return	received data size for success else -1.
  */
-int16_t recv(SOCKET s, uint8_t *buf, int16_t len)
+int16_t W5000socket::recv(uint8_t *buf, int16_t len)
 {
   // Check how much data is available
   SPI.beginTransaction(SPI_ETHERNET_SETTINGS);
@@ -212,7 +275,7 @@ int16_t recv(SOCKET s, uint8_t *buf, int16_t len)
 }
 
 
-int16_t recvAvailable(SOCKET s)
+int16_t W5000socket::recvAvailable()
 {
   SPI.beginTransaction(SPI_ETHERNET_SETTINGS);
   int16_t ret = W5100.getRXReceivedSize(s);
@@ -226,7 +289,7 @@ int16_t recvAvailable(SOCKET s)
  * 		
  * @return
  */
-uint16_t peek(SOCKET s, uint8_t *buf)
+uint16_t W5000socket::peek(uint8_t *buf)
 {
   SPI.beginTransaction(SPI_ETHERNET_SETTINGS);
   W5100.recv_data_processing(s, buf, 1, 1);
@@ -241,7 +304,7 @@ uint16_t peek(SOCKET s, uint8_t *buf)
  * 		
  * @return	This function return send data size for success else -1.
  */
-uint16_t sendto(SOCKET s, const uint8_t *buf, uint16_t len, uint8_t *addr, uint16_t port)
+uint16_t W5000socket::sendto(const uint8_t *buf, uint16_t len, uint8_t *addr, uint16_t port)
 {
   uint16_t ret=0;
 
@@ -296,7 +359,7 @@ uint16_t sendto(SOCKET s, const uint8_t *buf, uint16_t len, uint8_t *addr, uint1
  * 	
  * @return	This function return received data size for success else -1.
  */
-uint16_t recvfrom(SOCKET s, uint8_t *buf, uint16_t len, uint8_t *addr, uint16_t *port)
+uint16_t W5000socket::recvfrom(uint8_t *buf, uint16_t len, uint8_t *addr, uint16_t *port)
 {
   uint8_t head[8];
   uint16_t data_len=0;
@@ -367,11 +430,11 @@ uint16_t recvfrom(SOCKET s, uint8_t *buf, uint16_t len, uint8_t *addr, uint16_t
 /**
  * @brief      Wait for buffered transmission to complete.
  */
-void flush(SOCKET s) {
+void W5000socket::flush() {
   // TODO
 }
 
-uint16_t igmpsend(SOCKET s, const uint8_t * buf, uint16_t len)
+uint16_t W5000socket::igmpsend(const uint8_t * buf, uint16_t len)
 {
   uint16_t ret=0;
 
@@ -394,7 +457,7 @@ uint16_t igmpsend(SOCKET s, const uint8_t * buf, uint16_t len)
       /* in case of igmp, if send fails, then socket closed */
       /* if you want change, remove this code. */
       SPI.endTransaction();
-      close(s);
+      close();
       return 0;
     }
     SPI.endTransaction();
@@ -407,7 +470,7 @@ uint16_t igmpsend(SOCKET s, const uint8_t * buf, uint16_t len)
   return ret;
 }
 
-uint16_t bufferData(SOCKET s, uint16_t offset, const uint8_t* buf, uint16_t len)
+uint16_t W5000socket::bufferData(uint16_t offset, const uint8_t* buf, uint16_t len)
 {
   uint16_t ret =0;
   SPI.beginTransaction(SPI_ETHERNET_SETTINGS);
@@ -424,7 +487,7 @@ uint16_t bufferData(SOCKET s, uint16_t offset, const uint8_t* buf, uint16_t len)
   return ret;
 }
 
-int startUDP(SOCKET s, uint8_t* addr, uint16_t port)
+int W5000socket::startUDP(uint8_t* addr, uint16_t port)
 {
   if
     (
@@ -444,7 +507,7 @@ int startUDP(SOCKET s, uint8_t* addr, uint16_t port)
   }
 }
 
-int sendUDP(SOCKET s)
+int W5000socket::sendUDP()
 {
   SPI.beginTransaction(SPI_ETHERNET_SETTINGS);
   W5100.execCmdSn(s, Sock_SEND);
diff --git a/utility/socket.h b/utility/socket.h
index 6a88bce..dd3d186 100644
--- a/utility/socket.h
+++ b/utility/socket.h
@@ -3,42 +3,66 @@
 
 #include "w5100.h"
 
-extern uint8_t socket(SOCKET s, uint8_t protocol, uint16_t port, uint8_t flag); // Opens a socket(TCP or UDP or IP_RAW mode)
-extern uint8_t socketStatus(SOCKET s);
-extern void close(SOCKET s); // Close socket
-extern uint8_t connect(SOCKET s, uint8_t * addr, uint16_t port); // Establish TCP connection (Active connection)
-extern void disconnect(SOCKET s); // disconnect the connection
-extern uint8_t listen(SOCKET s);	// Establish TCP connection (Passive connection)
-extern uint16_t send(SOCKET s, const uint8_t * buf, uint16_t len); // Send data (TCP)
-extern int16_t recv(SOCKET s, uint8_t * buf, int16_t len);	// Receive data (TCP)
-extern int16_t recvAvailable(SOCKET s);
-extern uint16_t peek(SOCKET s, uint8_t *buf);
-extern uint16_t sendto(SOCKET s, const uint8_t * buf, uint16_t len, uint8_t * addr, uint16_t port); // Send data (UDP/IP RAW)
-extern uint16_t recvfrom(SOCKET s, uint8_t * buf, uint16_t len, uint8_t * addr, uint16_t *port); // Receive data (UDP/IP RAW)
-extern void flush(SOCKET s); // Wait for transmission to complete
-
-extern uint16_t igmpsend(SOCKET s, const uint8_t * buf, uint16_t len);
-
-// Functions to allow buffered UDP send (i.e. where the UDP datagram is built up over a
-// number of calls before being sent
+class W5000socket {
+public:
+	W5000socket() : s(MAX_SOCK_NUM) { }
+
+	// Opens a socket(TCP or UDP or IP_RAW mode)
+	//uint8_t socket(SOCKET s, uint8_t protocol, uint16_t port, uint8_t flag);
+	uint8_t begin(uint8_t protocol, uint16_t port);
+
+	uint8_t socketStatus();
+	// Close socket
+	void close();
+	// Establish TCP connection (Active connection)
+	void connect(uint8_t * addr, uint16_t port);
+	// disconnect the connection
+	void disconnect();
+	// Establish TCP connection (Passive connection)
+	uint8_t listen();
+	// Send data (TCP)
+	uint16_t send(const uint8_t * buf, uint16_t len);
+	// Receive data (TCP)
+	int16_t recv(uint8_t * buf, int16_t len);
+	int16_t recvAvailable();
+	uint16_t peek(uint8_t *buf);
+	 // Send data (UDP/IP RAW)
+	uint16_t sendto(const uint8_t * buf, uint16_t len, uint8_t * addr, uint16_t port);
+	// Receive data (UDP/IP RAW)
+	uint16_t recvfrom(uint8_t * buf, uint16_t len, uint8_t * addr, uint16_t *port);
+	// Wait for transmission to complete
+	void flush();
+
+	uint16_t igmpsend(const uint8_t * buf, uint16_t len);
+
+	// Functions to allow buffered UDP send (i.e. where the UDP datagram is built up over a
+	// number of calls before being sent
 /*
   @brief This function sets up a UDP datagram, the data for which will be provided by one
   or more calls to bufferData and then finally sent with sendUDP.
   @return 1 if the datagram was successfully set up, or 0 if there was an error
 */
-extern int startUDP(SOCKET s, uint8_t* addr, uint16_t port);
+	int startUDP(uint8_t* addr, uint16_t port);
 /*
   @brief This function copies up to len bytes of data from buf into a UDP datagram to be
   sent later by sendUDP.  Allows datagrams to be built up from a series of bufferData calls.
   @return Number of bytes successfully buffered
 */
-uint16_t bufferData(SOCKET s, uint16_t offset, const uint8_t* buf, uint16_t len);
+	uint16_t bufferData(uint16_t offset, const uint8_t* buf, uint16_t len);
 /*
   @brief Send a UDP datagram built up from a sequence of startUDP followed by one or more
   calls to bufferData.
   @return 1 if the datagram was successfully sent, or 0 if there was an error
 */
-int sendUDP(SOCKET s);
+	int sendUDP();
+
+	void moveTo(W5000socket &rhs);
+	operator bool() const { return s < MAX_SOCK_NUM; }
+	SOCKET getSocketNumber() const { return s; }
+private:
+	SOCKET s;
+};
+
 
 #endif
 /* _SOCKET_H_ */