at-serial-port: implement built-in echo/garbage removal

We expect the responses to start always with <CR><LF>. We just remove anything
that comes before that.

1 files changed, 26 insertions, 0 deletions

diff --git a/src/mm-at-serial-port.c b/src/mm-at-serial-port.c
index 30da3a3b..f49e9f0e 100644
--- a/src/mm-at-serial-port.c
+++ b/src/mm-at-serial-port.c
@@ -58,6 +58,26 @@ mm_at_serial_port_set_response_parser (MMAtSerialPort *self,
     priv->response_parser_notify = notify;
 }
 
+void
+mm_at_serial_port_remove_echo (GByteArray *response)
+{
+    guint i;
+
+    if (response->len <= 2)
+        return;
+
+    for (i = 0; i < (response->len - 2); i++) {
+        /* If there is any content before the first
+         * <CR><LF>, assume it's echo or garbage, and skip it */
+        if (response->data[i] == '\r' && response->data[i + 1] == '\n') {
+            if (i > 0)
+                g_byte_array_remove_range (response, 0, i);
+            /* else, good, we're already started with <CR><LF> */
+            break;
+        }
+    }
+}
+
 static gboolean
 parse_response (MMSerialPort *port, GByteArray *response, GError **error)
 {
@@ -68,6 +88,9 @@ parse_response (MMSerialPort *port, GByteArray *response, GError **error)
 
     g_return_val_if_fail (priv->response_parser_fn != NULL, FALSE);
 
+    /* Remove echo */
+    mm_at_serial_port_remove_echo (response);
+
     /* Construct the string that AT-parsing functions expect */
     string = g_string_sized_new (response->len + 1);
     g_string_append_len (string, (const char *) response->data, response->len);
@@ -159,6 +182,9 @@ parse_unsolicited (MMSerialPort *port, GByteArray *response)
     MMAtSerialPortPrivate *priv = MM_AT_SERIAL_PORT_GET_PRIVATE (self);
     GSList *iter;
 
+    /* Remove echo */
+    mm_at_serial_port_remove_echo (response);
+
     for (iter = priv->unsolicited_msg_handlers; iter; iter = iter->next) {
         MMAtUnsolicitedMsgHandler *handler = (MMAtUnsolicitedMsgHandler *) iter->data;
         GMatchInfo *match_info;