From 943dd711a9065adfd1645ecb396afea43d13cdce Mon Sep 17 00:00:00 2001
From: Christopher Haster <chaster@utexas.edu>
Date: Wed, 20 Apr 2016 19:30:16 -0500
Subject: [PATCH] Added WFI to save power in temporary polling implementation

---
 Socket.cpp    |  4 ++++
 Socket.h      |  1 +
 TCPServer.cpp |  9 +++++++--
 TCPSocket.cpp | 18 ++++++++++++++----
 UDPSocket.cpp | 18 ++++++++++++++----
 5 files changed, 40 insertions(+), 10 deletions(-)

diff --git a/Socket.cpp b/Socket.cpp
index d21f984e7e..3b853e19ec 100644
--- a/Socket.cpp
+++ b/Socket.cpp
@@ -109,6 +109,10 @@ int Socket::getsockopt(int level, int optname, void *optval, unsigned *optlen)
 
 }
 
+void Socket::wakeup()
+{
+}
+
 void Socket::thunk(void *data)
 {
     Socket *self = (Socket *)data;
diff --git a/Socket.h b/Socket.h
index 195da38204..64e7325214 100644
--- a/Socket.h
+++ b/Socket.h
@@ -169,6 +169,7 @@ protected:
     int open(NetworkStack *iface, nsapi_protocol_t proto);
 
     static void thunk(void *);
+    static void wakeup();
 
     NetworkStack *_iface;
     void *_socket;
diff --git a/TCPServer.cpp b/TCPServer.cpp
index 7a65e954da..e31a6328be 100644
--- a/TCPServer.cpp
+++ b/TCPServer.cpp
@@ -44,6 +44,10 @@ int TCPServer::accept(TCPSocket *connection)
 {
     mbed::Timer timer;
     timer.start();
+    mbed::Timeout timeout;
+    if (_timeout >= 0) {
+        timeout.attach_us(&Socket::wakeup, _timeout * 1000);
+    }
 
     if (connection->_socket) {
         connection->close();
@@ -61,9 +65,10 @@ int TCPServer::accept(TCPSocket *connection)
         }
 
         if (err != NSAPI_ERROR_WOULD_BLOCK
-            || _timeout < 0
-            || timer.read_ms() > _timeout) {
+            || (_timeout >= 0 && timer.read_ms() >= _timeout)) {
             return err;
         }
+
+        __WFI();
     }
 }
diff --git a/TCPSocket.cpp b/TCPSocket.cpp
index 926473dacc..4ec5ce6ab1 100644
--- a/TCPSocket.cpp
+++ b/TCPSocket.cpp
@@ -54,6 +54,10 @@ int TCPSocket::send(const void *data, unsigned size)
 {
     mbed::Timer timer;
     timer.start();
+    mbed::Timeout timeout;
+    if (_timeout >= 0) {
+        timeout.attach_us(&Socket::wakeup, _timeout * 1000);
+    }
 
     while (true) {
         if (!_socket) {
@@ -62,10 +66,11 @@ int TCPSocket::send(const void *data, unsigned size)
 
         int sent = _iface->socket_send(_socket, data, size);
         if (sent != NSAPI_ERROR_WOULD_BLOCK
-            || _timeout < 0
-            || timer.read_ms() > _timeout) {
+            || (_timeout >= 0 && timer.read_ms() >= _timeout)) {
             return sent;
         }
+
+        __WFI();
     }
 }
 
@@ -73,6 +78,10 @@ int TCPSocket::recv(void *data, unsigned size)
 {
     mbed::Timer timer;
     timer.start();
+    mbed::Timeout timeout;
+    if (_timeout >= 0) {
+        timeout.attach_us(&Socket::wakeup, _timeout * 1000);
+    }
 
     while (true) {
         if (!_socket) {
@@ -81,9 +90,10 @@ int TCPSocket::recv(void *data, unsigned size)
     
         int recv = _iface->socket_recv(_socket, data, size);
         if (recv != NSAPI_ERROR_WOULD_BLOCK
-            || _timeout < 0
-            || timer.read_ms() > _timeout) {
+            || (_timeout >= 0 && timer.read_ms() >= _timeout)) {
             return recv;
         }
+
+        __WFI();
     }
 }
diff --git a/UDPSocket.cpp b/UDPSocket.cpp
index a8148fb38f..9494323fbd 100644
--- a/UDPSocket.cpp
+++ b/UDPSocket.cpp
@@ -45,6 +45,10 @@ int UDPSocket::sendto(const SocketAddress &address, const void *data, unsigned s
 {
     mbed::Timer timer;
     timer.start();
+    mbed::Timeout timeout;
+    if (_timeout >= 0) {
+        timeout.attach_us(&Socket::wakeup, _timeout * 1000);
+    }
 
     while (true) {
         if (!_socket) {
@@ -53,10 +57,11 @@ int UDPSocket::sendto(const SocketAddress &address, const void *data, unsigned s
     
         int sent = _iface->socket_sendto(_socket, address, data, size);
         if (sent != NSAPI_ERROR_WOULD_BLOCK
-            || _timeout < 0
-            || timer.read_ms() > _timeout) {
+            || (_timeout >= 0 && timer.read_ms() >= _timeout)) {
             return sent;
         }
+
+        __WFI();
     }
 }
 
@@ -64,6 +69,10 @@ int UDPSocket::recvfrom(SocketAddress *address, void *buffer, unsigned size)
 {
     mbed::Timer timer;
     timer.start();
+    mbed::Timeout timeout;
+    if (_timeout >= 0) {
+        timeout.attach_us(&Socket::wakeup, _timeout * 1000);
+    }
 
     while (true) {
         if (!_socket) {
@@ -72,9 +81,10 @@ int UDPSocket::recvfrom(SocketAddress *address, void *buffer, unsigned size)
     
         int recv = _iface->socket_recvfrom(_socket, address, buffer, size);
         if (recv != NSAPI_ERROR_WOULD_BLOCK
-            || _timeout < 0
-            || timer.read_ms() > _timeout) {
+            || (_timeout >= 0 && timer.read_ms() >= _timeout)) {
             return recv;
         }
+
+        __WFI();
     }
 }