From dc693e150f807911de76f2aeaf40d0ce9414def6 Mon Sep 17 00:00:00 2001
From: Mike Purvis <mpurvis@clearpathrobotics.com>
Date: Mon, 28 Oct 2013 20:17:07 -0400
Subject: [PATCH] Implement MillisecondTimer in the unix read() and write()
 functions.

---
 src/impl/unix.cc | 120 ++++++++++++++++-------------------------------
 1 file changed, 41 insertions(+), 79 deletions(-)

diff --git a/src/impl/unix.cc b/src/impl/unix.cc
index 60be83f..8c62a0e 100755
--- a/src/impl/unix.cc
+++ b/src/impl/unix.cc
@@ -91,6 +91,14 @@ MillisecondTimer::now ()
   return time;
 }
 
+timespec
+timespec_from_ms (const uint32_t millis) 
+{
+  timespec time;
+  time.tv_sec = millis / 1e3;
+  time.tv_nsec = (millis % (int)1e3) * 1e6;
+}
+
 Serial::SerialImpl::SerialImpl (const string &port, unsigned long baudrate,
                                 bytesize_t bytesize,
                                 parity_t parity, stopbits_t stopbits,
@@ -447,59 +455,29 @@ Serial::SerialImpl::read (uint8_t *buf, size_t size)
   }
   fd_set readfds;
   size_t bytes_read = 0;
-  // Setup the total_timeout timeval
-  //  This timeout is maximum time before a timeout after read is called
-  struct timeval total_timeout;
+  
   // Calculate total timeout in milliseconds t_c + (t_m * N)
   long total_timeout_ms = timeout_.read_timeout_constant;
   total_timeout_ms += timeout_.read_timeout_multiplier*static_cast<long>(size);
-  total_timeout.tv_sec = total_timeout_ms / 1000;
-  total_timeout.tv_usec = static_cast<int>(total_timeout_ms % 1000);
-  total_timeout.tv_usec *= 1000; // To convert to micro seconds
-  // Setup the inter byte timeout
-  struct timeval inter_byte_timeout;
-  inter_byte_timeout.tv_sec = timeout_.inter_byte_timeout / 1000;
-  inter_byte_timeout.tv_usec =
-    static_cast<int> (timeout_.inter_byte_timeout % 1000);
-  inter_byte_timeout.tv_usec *= 1000; // To convert to micro seconds
+  MillisecondTimer total_timeout(total_timeout_ms);
+
   while (bytes_read < size) {
-    // Setup the select timeout timeval
-    struct timeval timeout;
-    // If the total_timeout is less than the inter_byte_timeout
-    if (total_timeout.tv_sec < inter_byte_timeout.tv_sec
-     || (total_timeout.tv_sec == inter_byte_timeout.tv_sec
-      && total_timeout.tv_usec < inter_byte_timeout.tv_sec))
-    {
-      // Then set the select timeout to use the total time
-      timeout = total_timeout;
-    } else {
-      // Else set the select timeout to use the inter byte time
-      timeout = inter_byte_timeout;
-    }
-    FD_ZERO (&readfds);
-    FD_SET (fd_, &readfds);
-    // Begin timing select
-    struct timespec start, end;
-    get_time_now (start);
-    // Call select to block for serial data or a timeout
-    int r = select (fd_ + 1, &readfds, NULL, NULL, &timeout);
-    // Calculate difference and update the structure
-    get_time_now (end);
-    // Calculate the time select took
-    struct timespec diff;
-    diff_timespec (start, end, diff);
-    // Update the timeout
-    if (total_timeout.tv_sec <= diff.tv_sec) {
-      total_timeout.tv_sec = 0;
-    } else {
-      total_timeout.tv_sec -= diff.tv_sec;
-    }
-    if (total_timeout.tv_usec <= (diff.tv_nsec / 1000)) {
-      total_timeout.tv_usec = 0;
-    } else {
-      total_timeout.tv_usec -= (diff.tv_nsec / 1000);
+    // Timeout for the next select is whichever is less of the total read
+    // timeout and the inter-byte timeout.
+    timespec timeout;
+    try {
+      timeout = timespec_from_ms(std::min(total_timeout.remaining(), 
+                                          timeout_.inter_byte_timeout));
+    } catch (TimerExpiredException) {
+      break;
     }
 
+    FD_ZERO (&readfds);
+    FD_SET (fd_, &readfds);
+    
+    // Call select to block for serial data or a timeout
+    int r = pselect (fd_ + 1, &readfds, NULL, NULL, &timeout, NULL);
+    
     // Figure out what happened by looking at select's response 'r'
     /** Error **/
     if (r < 0) {
@@ -564,41 +542,25 @@ Serial::SerialImpl::write (const uint8_t *data, size_t length)
   }
   fd_set writefds;
   size_t bytes_written = 0;
-  struct timeval timeout;
-  timeout.tv_sec =                    timeout_.write_timeout_constant / 1000;
-  timeout.tv_usec = static_cast<int> (timeout_.write_timeout_multiplier % 1000);
-  timeout.tv_usec *= 1000; // To convert to micro seconds
-  while (bytes_written < length) {
+
+  // Calculate total timeout in milliseconds t_c + (t_m * N)
+  long total_timeout_ms = timeout_.write_timeout_constant;
+  total_timeout_ms += timeout_.write_timeout_multiplier*static_cast<long>(length);
+  MillisecondTimer total_timeout(total_timeout_ms);
+
+  while (bytes_written < length) { 
+    timespec timeout;
+    try {
+      timeout = timespec_from_ms(total_timeout.remaining());
+    } catch (TimerExpiredException) {
+      break;
+    }
+
     FD_ZERO (&writefds);
     FD_SET (fd_, &writefds);
-    // On Linux the timeout struct is updated by select to contain the time
-    // left on the timeout to make looping easier, but on other platforms this
-    // does not occur.
-#if !defined(__linux__)
-    // Begin timing select
-    struct timespec start, end;
-    get_time_now(start);
-#endif
+
     // Do the select
-    int r = select (fd_ + 1, NULL, &writefds, NULL, &timeout);
-#if !defined(__linux__)
-    // Calculate difference and update the structure
-    get_time_now(end);
-    // Calculate the time select took
-    struct timespec diff;
-    diff_timespec(start, end, diff);
-    // Update the timeout
-    if (timeout.tv_sec <= diff.tv_sec) {
-      timeout.tv_sec = 0;
-    } else {
-      timeout.tv_sec -= diff.tv_sec;
-    }
-    if (timeout.tv_usec <= (diff.tv_nsec / 1000)) {
-      timeout.tv_usec = 0;
-    } else {
-      timeout.tv_usec -= (diff.tv_nsec / 1000);
-    }
-#endif
+    int r = pselect (fd_ + 1, NULL, &writefds, NULL, &timeout, NULL);
 
     // Figure out what happened by looking at select's response 'r'
     /** Error **/