Diff: ATParser.cpp

Revision:

1:66a14afe650a

Parent:

0:c741e144517c

Child:

2:4d68f546861c

diff -r c741e144517c -r 66a14afe650a ATParser.cpp
--- 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);