Fix findUntil and general timeouts in Stream library

Author: Xuth

hardware/arduino/avr/cores/arduino/Stream.cpp

@@ -18,6 +18,8 @@
 
  Created July 2011
  parsing functions based on TextFinder library by Michael Margolis
+
+ findMulti/findUntil routines written by Jim Leonard/Xuth
  */
 
 #include "Arduino.h"
@@ -27,43 +29,73 @@
 #define NO_SKIP_CHAR  1  // a magic char not found in a valid ASCII numeric field
 
 // private method to read stream with timeout
-int Stream::timedRead()
+
+void Stream::startClock() 
 {
-  int c;
   _startMillis = millis();
+}
+
+bool Stream::checkClock()
+{
+  if (millis() - _startMillis < _timeout)
+    return true;
+  return false;
+}
+
+int Stream::_timedRead()
+{
   do {
-    c = read();
+    int c = read();
     if (c >= 0) return c;
-  } while(millis() - _startMillis < _timeout);
-  return -1;     // -1 indicates timeout
+  } while(checkClock());
+  return - 1;  // -1 is timeout
+}
+
+int Stream::timedRead()
+{
+  startClock();
+  return _timedRead();
 }
 
 // private method to peek stream with timeout
-int Stream::timedPeek()
+// callers must call startClock() first
+int Stream::_timedPeek()
 {
   int c;
-  _startMillis = millis();
   do {
     c = peek();
     if (c >= 0) return c;
-  } while(millis() - _startMillis < _timeout);
+  } while(checkClock());
   return -1;     // -1 indicates timeout
 }
 
-// returns peek of the next digit in the stream or -1 if timeout
-// discards non-numeric characters
-int Stream::peekNextDigit()
+int Stream::timedPeek() 
+{
+  startClock();
+  return _timedPeek();
+}
+
+// private method that assumes startClock() has already been called
+int Stream::_peekNextDigit()
 {
   int c;
   while (1) {
-    c = timedPeek();
+    c = _timedPeek();
     if (c < 0) return c;  // timeout
     if (c == '-') return c;
     if (c >= '0' && c <= '9') return c;
     read();  // discard non-numeric
   }
 }
 
+// returns peek of the next digit in the stream or -1 if timeout
+// discards non-numeric characters
+int Stream::peekNextDigit()
+{
+  startClock();
+  return _peekNextDigit();
+}
+
 // Public Methods
 //////////////////////////////////////////////////////////////
 
@@ -75,7 +107,7 @@ void Stream::setTimeout(unsigned long timeout)  // sets the maximum number of mi
  // find returns true if the target string is found
 bool  Stream::find(char *target)
 {
-  return findUntil(target, (char*)"");
+  return findUntil(target, strlen(target), NULL, 0);
 }
 
 // reads data from the stream until the target string of given length is found
@@ -96,32 +128,13 @@ bool  Stream::findUntil(char *target, char *terminator)
 // returns true if target string is found, false if terminated or timed out
 bool Stream::findUntil(char *target, size_t targetLen, char *terminator, size_t termLen)
 {
-  size_t index = 0;  // maximum target string length is 64k bytes!
-  size_t termIndex = 0;
-  int c;
-  
-  if( *target == 0)
-    return true;   // return true if target is a null string
-  while( (c = timedRead()) > 0){
-    
-    if(c != target[index])
-      index = 0; // reset index if any char does not match
-    
-    if( c == target[index]){
-      //////Serial.print("found "); Serial.write(c); Serial.print("index now"); Serial.println(index+1);
-      if(++index >= targetLen){ // return true if all chars in the target match
-        return true;
-      }
-    }
-    
-    if(termLen > 0 && c == terminator[termIndex]){
-      if(++termIndex >= termLen)
-        return false;       // return false if terminate string found before target string
-    }
-    else
-      termIndex = 0;
+  if (terminator == NULL) {
+    MultiTarget t[1] = {{target, targetLen, 0}};
+    return findMulti(t, 1) == 0 ? true : false;
+  } else {
+    MultiTarget t[2] = {{target, targetLen, 0}, {terminator, termLen, 0}};
+    return findMulti(t, 2) == 0 ? true : false;
   }
-  return false;
 }
 
 
@@ -137,11 +150,12 @@ long Stream::parseInt()
 // this allows format characters (typically commas) in values to be ignored
 long Stream::parseInt(char skipChar)
 {
-  boolean isNegative = false;
+  bool isNegative = false;
   long value = 0;
   int c;
 
-  c = peekNextDigit();
+  startClock();
+  c = _peekNextDigit();
   // ignore non numeric leading characters
   if(c < 0)
     return 0; // zero returned if timeout
@@ -173,13 +187,14 @@ float Stream::parseFloat()
 // as above but the given skipChar is ignored
 // this allows format characters (typically commas) in values to be ignored
 float Stream::parseFloat(char skipChar){
-  boolean isNegative = false;
-  boolean isFraction = false;
+  bool isNegative = false;
+  bool isFraction = false;
   long value = 0;
-  int c;
+  char c;
   float fraction = 1.0;
 
-  c = peekNextDigit();
+  startClock();
+  c = _peekNextDigit();
     // ignore non numeric leading characters
   if(c < 0)
     return 0; // zero returned if timeout
@@ -216,9 +231,10 @@ float Stream::parseFloat(char skipChar){
 //
 size_t Stream::readBytes(char *buffer, size_t length)
 {
+  startClock();
   size_t count = 0;
   while (count < length) {
-    int c = timedRead();
+    int c = _timedRead();
     if (c < 0) break;
     *buffer++ = (char)c;
     count++;
@@ -233,10 +249,11 @@ size_t Stream::readBytes(char *buffer, size_t length)
 
 size_t Stream::readBytesUntil(char terminator, char *buffer, size_t length)
 {
+  startClock();
   if (length < 1) return 0;
   size_t index = 0;
   while (index < length) {
-    int c = timedRead();
+    int c = _timedRead();
     if (c < 0 || c == terminator) break;
     *buffer++ = (char)c;
     index++;
@@ -246,25 +263,88 @@ size_t Stream::readBytesUntil(char terminator, char *buffer, size_t length)
 
 String Stream::readString()
 {
+  startClock();
   String ret;
-  int c = timedRead();
+  int c = _timedRead();
   while (c >= 0)
   {
     ret += (char)c;
-    c = timedRead();
+    c = _timedRead();
   }
   return ret;
 }
 
 String Stream::readStringUntil(char terminator)
 {
+  startClock();
   String ret;
-  int c = timedRead();
+  int c = _timedRead();
   while (c >= 0 && c != terminator)
   {
     ret += (char)c;
-    c = timedRead();
+    c = _timedRead();
   }
   return ret;
 }
 
+int Stream::findMulti( struct Stream::MultiTarget *targets, int tCount) {
+  // any zero length target string automatically matches and would make
+  // a mess of the rest of the algorithm.
+  for (struct MultiTarget *t = targets; t < targets+tCount; ++t)
+    if (t->len <= 0)
+      return t - targets;
+  
+  startClock();
+  while(1) {
+    int c = _timedRead();
+    if (c < 0)
+      return -1;
+    
+    for (struct MultiTarget *t = targets; t < targets+tCount; ++t) {
+      // the simple case is if we match, deal with that first.
+      if (c == t->str[t->index])
+	if (++t->index == t->len)
+	  return t - targets;
+	else
+	  continue;
+      
+      // if not we need to walk back and see if we could have matched further
+      // down the stream (ie '1112' doesn't match the first position in '11112'
+      // but it will match the second position so we can't just reset the current
+      // index to 0 when we find a mismatch.
+      if (t->index == 0)
+	continue;
+      
+      int origIndex = t->index;
+      do {
+	--t->index;
+	// first check if current char works against the new current index
+	if (c != t->str[t->index])
+	  continue;
+	
+	// if it's the only char then we're good, nothing more to check
+	if (t->index == 0) {
+	  t->index++;
+	  break;
+	}
+	
+	// otherwise we need to check the rest of the found string
+	int diff = origIndex - t->index;
+	int i;
+	for (i = 0; i < t->index; ++i)
+	  if (t->str[i] != t->str[i + diff])
+	    break;
+	// if we successfully got through the previous loop then our current
+	// index is good.
+	if (i == t->index) {
+	  t->index++;
+	  break;
+	}
+	// otherwise we just try the next index
+      } while (t->index);
+    }
+  }
+  // unreachable
+  return -1;
+}
+		

hardware/arduino/avr/cores/arduino/Stream.h

@@ -41,8 +41,14 @@ class Stream : public Print
     unsigned long _timeout;      // number of milliseconds to wait for the next char before aborting timed read
     unsigned long _startMillis;  // used for timeout measurement
     int timedRead();    // private method to read stream with timeout
+    int _timedRead();   // timedRead() but assumes clock already started
     int timedPeek();    // private method to peek stream with timeout
+    int _timedPeek();   // timedPeak() but assumes clock already started
     int peekNextDigit(); // returns the next numeric digit in the stream or -1 if timeout
+    int _peekNextDigit(); // peekNextDigit() but assumes clock already started
+    void startClock();  // helper, used for all of the timed read routines
+    bool checkClock();  // helper, consistent check if we've timed out
+
 
   public:
     virtual int available() = 0;
@@ -88,6 +94,7 @@ class Stream : public Print
   // returns the number of characters placed in the buffer (0 means no valid data found)
 
   // Arduino String functions to be added here
+
   String readString();
   String readStringUntil(char terminator);
 
@@ -97,6 +104,20 @@ class Stream : public Print
   // this allows format characters (typically commas) in values to be ignored
 
   float parseFloat(char skipChar);  // as above but the given skipChar is ignored
+
+  public:
+  struct MultiTarget {
+    const char *str;  // string you're searching for
+    size_t len;       // length of string you're searching for
+    size_t index;     // index used by the search routine.
+  };
+
+  // This allows you to search for an arbitrary number of strings.
+  // Returns index of the target that is found first or -1 if timeout occurs.
+  int findMulti(struct MultiTarget *targets, int tCount);
+
+
 };
 
+
 #endif