No idea
Diff: ATParser.cpp
- Revision:
- 1:66a14afe650a
- Parent:
- 0:c741e144517c
- Child:
- 2:4d68f546861c
--- a/ATParser.cpp Wed Jul 15 22:39:25 2015 +0000 +++ b/ATParser.cpp Thu Jul 16 20:42:44 2015 +0000 @@ -22,7 +22,7 @@ #include <cstdarg> // This can be defined to assist in debugging -#define AT_ECHO 1 +//#define AT_ECHO 1 // getc/putc handling with timeouts @@ -57,50 +57,53 @@ _serial->getc(); } + // getline/putline handling with timeouts/bounds checking -bool ATParser::_putline(const char *line) { +bool ATParser::_putline(const char *line) { for (int i = 0; line[i]; i++) { if (_putc(line[i]) < 0) return false; } // Finish with newline - if (_putc('\r') < 0 || - _putc('\n') < 0) + if (_putc('\r') < 0 || _putc('\n') < 0) return false; #ifdef AT_ECHO printf("AT> %s\r\n", line); #endif - + return true; } -bool ATParser::_getline(int size, char *line) { - for (int i = 0; i < size; i++) { +bool ATParser::_getline(char *line, int size) { + int i = 0; + + while (i < size) { int c = _getc(); - if (c < 0) - return false; + if (c < 0) + return false; - // Finish if newline - if (c == '\r') { - if (_getc() != '\n') - return false; + // Finish when we hit a newline + if (c == '\r' || c == '\n') { + // Only handle newlines on \n + if (c != '\n') + continue; - line[i] = 0; + line[i++] = 0; #ifdef AT_ECHO printf("AT< %s\r\n", line); #endif return true; } - line[i] = c; + line[i++] = c; } // Ran out of space return false; -} +} bool ATParser::command(const char *command, const char *response, ...) { @@ -115,20 +118,76 @@ va_end(args); return false; } - - // Determine number of parameters - // this is needed for scanf's funky error signaling - int params = 0; - for (int i = 0; response[i]; i++) { - if (response[i] == '%' && response[i+1] != '%') - params++; - } - // Recieve and parse response - if (!_getline(_buffer_size, _buffer) || - vsscanf(_buffer, response, args) < params) { - va_end(args); - return false; + // Iterate through each line in the expected response + while (response && response[0]) { + // Since response is const, we need to copy it into our buffer to + // add the line's null terminator and clobber value matches with asterisks. + // + // We just use the beginning of the buffer to avoid unecessary allocations. + int i = 0; + int offset = 0; + + while (response[i]) { + // Only handle newlines on \n + if (response[i] == '\n') { + i++; + break; + } else if (response[i] == '\r') { + i++; + } else if (response[i] == '%' && + response[i+1] != '%' && + response[i+1] != '*') { + i++; + + _buffer[offset++] = '%'; + _buffer[offset++] = '*'; + } else { + _buffer[offset++] = response[i++]; + } + } + + // Scanf has very poor support for catching errors + // fortunately, we can abuse the %n specifier to determine + // if the entire string was matched. + _buffer[offset++] = '%'; + _buffer[offset++] = 'n'; + _buffer[offset++] = 0; + + // To workaround scanf's lack of error reporting, we actually + // make two passes. One checks the validity with the modified + // format string that only stores the matched characters (%n). + // The other reads in the actual matched values. + // + // We keep trying the match until we succeed or some other error + // derails us. + while (true) { + // Recieve response + if (!_getline(_buffer+offset, _buffer_size-offset)) { + va_end(args); + return false; + } + + int count; + sscanf(_buffer+offset, _buffer, &count); + + // We only succeed if all characters in the response is matched + if ((_buffer+offset)[count] == 0) { + // Reuse the front end of the buffer + int j; + for (j = 0; j < i; j++) { + _buffer[j] = response[j]; + } + _buffer[j] = 0; + + // Store the found results + vsscanf(_buffer+offset, _buffer, args); + + // Jump to next line and continue parsing + response += i; + break; + } + } } va_end(args);