From ae2fa13183141353dd1a12a9b8fd19220f7f4d61 Mon Sep 17 00:00:00 2001 From: "Jim Leonard (Xuth)" Date: Mon, 23 Feb 2015 23:50:36 -0500 Subject: [PATCH] Fix findUntil and general timeouts in Stream library --- hardware/arduino/avr/cores/arduino/Stream.cpp | 180 +++++++++++++----- hardware/arduino/avr/cores/arduino/Stream.h | 21 ++ hardware/arduino/sam/cores/arduino/Stream.cpp | 180 +++++++++++++----- hardware/arduino/sam/cores/arduino/Stream.h | 21 ++ 4 files changed, 302 insertions(+), 100 deletions(-) diff --git a/hardware/arduino/avr/cores/arduino/Stream.cpp b/hardware/arduino/avr/cores/arduino/Stream.cpp index 9c581bee1b4..65b0867bd55 100644 --- a/hardware/arduino/avr/cores/arduino/Stream.cpp +++ b/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,36 +29,58 @@ #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; @@ -64,6 +88,14 @@ int Stream::peekNextDigit() } } +// 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; +} + diff --git a/hardware/arduino/avr/cores/arduino/Stream.h b/hardware/arduino/avr/cores/arduino/Stream.h index 5cf5ddf0171..c497dc7ae13 100644 --- a/hardware/arduino/avr/cores/arduino/Stream.h +++ b/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 diff --git a/hardware/arduino/sam/cores/arduino/Stream.cpp b/hardware/arduino/sam/cores/arduino/Stream.cpp index 9c581bee1b4..65b0867bd55 100644 --- a/hardware/arduino/sam/cores/arduino/Stream.cpp +++ b/hardware/arduino/sam/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,36 +29,58 @@ #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; @@ -64,6 +88,14 @@ int Stream::peekNextDigit() } } +// 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; +} + diff --git a/hardware/arduino/sam/cores/arduino/Stream.h b/hardware/arduino/sam/cores/arduino/Stream.h index 5cf5ddf0171..c497dc7ae13 100644 --- a/hardware/arduino/sam/cores/arduino/Stream.h +++ b/hardware/arduino/sam/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