adding prompt verification

Author: andreagilardoni

libraries/WiFiS3/src/Modem.cpp

@@ -49,7 +49,7 @@ bool ModemClass::passthrough(const uint8_t *data, size_t size) {
 /* -------------------------------------------------------------------------- */
    _serial->write(data,size);
 
-   std::string tmp, data_res; // FIXME I don't always have a command prompt provided
+   std::string tmp = DO_NOT_CHECK_CMD, data_res; // FIXME I don't always have a command prompt provided
    auto res = buf_read(tmp, data_res);
 
    if(_serial_debug && _debug_level >= 2) {
@@ -160,6 +160,7 @@ ModemClass::ParseResult ModemClass::buf_read(const string &prompt, string &data_
    unsigned int sized_read_size = 0;
    unsigned int sized_read_count = 0;
    unsigned int result_parse = 0;
+   bool restart = false;
 
 
    if(_serial_debug  && _debug_level >= 1) {
@@ -208,6 +209,7 @@ ModemClass::ParseResult ModemClass::buf_read(const string &prompt, string &data_
              */
 
             if(c == '+') {
+               commandName += c; // prompt includes also '+'
                state = at_parse_state_t::Cmd;
             } else if(c == RESULT_OK[result_parse]) { // FIXME this should also account for following characters
                state = at_parse_state_t::Ok;
@@ -231,9 +233,16 @@ ModemClass::ParseResult ModemClass::buf_read(const string &prompt, string &data_
              */
 
             if(c == ':' || c == '=') {
-               // TODO verify command is matching prompt
-               state = at_parse_state_t::Data;
-            } else {
+               commandName += c; // prompt includes also ':'
+
+               if (commandName != DO_NOT_CHECK_CMD && commandName != prompt) {
+                  // state = at_parse_state_t::Begin; // TODO decide whether to return ParseError or realign
+                  restart = true;
+               } else {
+                  state = at_parse_state_t::Data;
+               }
+               // state = at_parse_state_t::Data;
+            } else if(c != ' ') { // FIXME should we keep the space?
                commandName += c;
             }
 
@@ -318,7 +327,12 @@ ModemClass::ParseResult ModemClass::buf_read(const string &prompt, string &data_
 
             if(result_parse == strlen(RESULT_OK)) {
                res = Ok;
-               state = at_parse_state_t::Completed;
+
+               if(restart) {
+                  state = at_parse_state_t::Begin;
+               } else {
+                  state = at_parse_state_t::Completed;
+               }
             }
             break;
          case at_parse_state_t::Error:
@@ -332,12 +346,21 @@ ModemClass::ParseResult ModemClass::buf_read(const string &prompt, string &data_
 
             if(result_parse == strlen(RESULT_ERROR)) {
                res = Error;
-               state = at_parse_state_t::Completed;
+
+               if(restart) {
+                  state = at_parse_state_t::Begin;
+               } else {
+                  state = at_parse_state_t::Completed;
+               }
             }
             break;
          case at_parse_state_t::ParseError:
             res = ParseError;
-            state = at_parse_state_t::Completed;
+            if(restart) {
+               state = at_parse_state_t::Begin;
+            } else {
+               state = at_parse_state_t::Completed;
+            }
             break;
          case at_parse_state_t::Completed:
             break;