qcdm: fix QCDM packet decapsulation

Rename and document the encapsulate/decapsulate functions, CRC-check
the incoming packet, and make callers aware of the difference in how
big the decapsulated packet is versus how many bytes they should
discard from the buffer (since the decapsulated packet is at least
3 bytes shorter than the incoming packet due to the CRC + framing).

4 files changed, 158 insertions, 20 deletions

diff --git a/libqcdm/src/commands.c b/libqcdm/src/commands.c
index 4e4af4b2..a3052007 100644
--- a/libqcdm/src/commands.c
+++ b/libqcdm/src/commands.c
@@ -61,7 +61,7 @@ qcdm_cmd_version_info_new (char *buf, gsize len, GError **error)
     memset (cmd, 0, sizeof (cmd));
     cmd->code = DIAG_CMD_VERSION_INFO;
 
-    return dm_prepare_buffer (cmdbuf, sizeof (*cmd), sizeof (cmdbuf), buf, len);
+    return dm_encapsulate_buffer (cmdbuf, sizeof (*cmd), sizeof (cmdbuf), buf, len);
 }
 
 QCDMResult *
diff --git a/libqcdm/src/utils.c b/libqcdm/src/utils.c
index 62574691..ab281557 100644
--- a/libqcdm/src/utils.c
+++ b/libqcdm/src/utils.c
@@ -166,12 +166,25 @@ dm_unescape (const char *inbuf,
     return outsize;
 }
 
+/**
+ * dm_encapsulate_buffer:
+ * @inbuf: buffer in which a valid QCDM packet exists
+ * @cmd_len: size of the QCDM packet contained in @inbuf
+ * @inbuf_len: total size of @inbuf itself (not just the packet)
+ * @outbuf: buffer in which to put the encapsulated QCDM packet
+ * @outbuf_len: total size of @outbuf
+ *
+ * Escapes and CRCs a QCDM packet, and finally adds the trailing control
+ * character that denotes the end of the QCDM packet.
+ *
+ * Returns: size of the encapsulated QCDM command writted to @outbuf.
+ **/
 gsize
-dm_prepare_buffer (char *inbuf,
-                   gsize cmd_len,
-                   gsize inbuf_len,
-                   char *outbuf,
-                   gsize outbuf_len)
+dm_encapsulate_buffer (char *inbuf,
+                       gsize cmd_len,
+                       gsize inbuf_len,
+                       char *outbuf,
+                       gsize outbuf_len)
 {
     guint16 crc;
     gsize escaped_len;
@@ -192,3 +205,105 @@ dm_prepare_buffer (char *inbuf,
     return escaped_len;
 }
 
+/**
+ * dm_decapsulate_buffer:
+ * @inbuf: buffer in which to look for a QCDM packet
+ * @inbuf_len: length of valid data in @inbuf
+ * @outbuf: buffer in which to put decapsulated QCDM packet
+ * @outbuf_len: max size of @outbuf
+ * @out_decap_len: on success, size of the decapsulated QCDM packet
+ * @out_used: on either success or failure, amount of data used; caller should
+ *  discard this much data from @inbuf before the next call to this function
+ * @out_need_more: when TRUE, indicates that more data is required before
+ *  and determination about a valid QCDM packet can be made; caller should add
+ *  more data to @inbuf before calling this function again.
+ *
+ * Attempts to retrieve, unescape, and CRC-check a QCDM packet from the given
+ * buffer.
+ *
+ * Returns: FALSE on error (packet was invalid or malformed, or the CRC check
+ *  failed, etc) and places number of bytes to discard from @inbuf in @out_used.
+ *  When TRUE, either more data is required (in which case @out_need_more will
+ *  be TRUE), or a QCDM packet was successfully retrieved from @inbuf and the
+ *  decapsulated packet of length @out_decap_len was placed into @outbuf.  In
+ *  all cases the caller should advance the buffer by the number of bytes
+ *  returned in @out_used before calling this function again.
+ **/
+gboolean
+dm_decapsulate_buffer (const char *inbuf,
+                       gsize inbuf_len,
+                       char *outbuf,
+                       gsize outbuf_len,
+                       gsize *out_decap_len,
+                       gsize *out_used,
+                       gboolean *out_need_more)
+{
+    gboolean escaping = FALSE;
+    gsize i, pkt_len = 0, unesc_len;
+    guint16 crc, pkt_crc;
+
+    g_return_val_if_fail (inbuf != NULL, FALSE);
+    g_return_val_if_fail (outbuf != NULL, FALSE);
+    g_return_val_if_fail (outbuf_len > 0, FALSE);
+    g_return_val_if_fail (out_decap_len != NULL, FALSE);
+    g_return_val_if_fail (out_used != NULL, FALSE);
+    g_return_val_if_fail (out_need_more != NULL, FALSE);
+
+    *out_decap_len = 0;
+    *out_used = 0;
+    *out_need_more = FALSE;
+
+    if (inbuf_len < 4) {
+        *out_need_more = TRUE;
+        return TRUE;
+    }
+
+    /* Find the async control character */
+    for (i = 0; i < inbuf_len; i++) {
+        if (inbuf[i] == DIAG_CONTROL_CHAR) {
+            /* If the control character shows up in a position before a valid
+             * QCDM packet length (4), the packet is malformed.
+             */
+            if (i < 3) {
+                /* Tell the caller to advance the buffer past the control char */
+                *out_used = i + 1;
+                return FALSE;
+            }
+
+            pkt_len = i;
+            break;
+        }
+    }
+
+    /* No control char yet, need more data */
+    if (!pkt_len) {
+        *out_need_more = TRUE;
+        return TRUE;
+    }
+
+    /* Unescape first; note that pkt_len */
+    unesc_len = dm_unescape (inbuf, pkt_len, outbuf, outbuf_len, &escaping);
+    if (!unesc_len) {
+        /* Tell the caller to advance the buffer past the control char */
+        *out_used = pkt_len + 1;
+        return FALSE;
+    }
+
+    if (escaping) {
+        *out_need_more = TRUE;
+        return TRUE;
+    }
+
+    /* Check the CRC of the packet's data */
+    crc = crc16 (outbuf, unesc_len - 2);
+    pkt_crc = *((guint16 *) &outbuf[pkt_len - 2]);
+    if (crc != GUINT_FROM_LE (pkt_crc)) {
+        *out_used = pkt_len + 1; /* packet + CRC + 0x7E */
+        return FALSE;
+    }
+
+    *out_used = pkt_len + 1; /* packet + CRC + 0x7E */
+    *out_decap_len = unesc_len - 2; /* decap_len should not include the CRC */
+    return TRUE;
+}
+
diff --git a/libqcdm/src/utils.h b/libqcdm/src/utils.h
index 15fc346b..5fccf7f9 100644
--- a/libqcdm/src/utils.h
+++ b/libqcdm/src/utils.h
@@ -36,11 +36,19 @@ gsize dm_unescape (const char *inbuf,
                    gsize outbuf_len,
                    gboolean *escaping);
 
-gsize dm_prepare_buffer (char *inbuf,
-                         gsize cmd_len,
-                         gsize inbuf_len,
-                         char *outbuf,
-                         gsize outbuf_len);
+gsize dm_encapsulate_buffer (char *inbuf,
+                             gsize cmd_len,
+                             gsize inbuf_len,
+                             char *outbuf,
+                             gsize outbuf_len);
+
+gboolean dm_decapsulate_buffer (const char *inbuf,
+                                gsize inbuf_len,
+                                char *outbuf,
+                                gsize outbuf_len,
+                                gsize *out_decap_len,
+                                gsize *out_used,
+                                gboolean *out_need_more);
 
 #endif  /* UTILS_H */
 
diff --git a/libqcdm/tests/test-qcdm-com.c b/libqcdm/tests/test-qcdm-com.c
index 5ea59f67..d8380fd7 100644
--- a/libqcdm/tests/test-qcdm-com.c
+++ b/libqcdm/tests/test-qcdm-com.c
@@ -144,9 +144,8 @@ wait_reply (TestComData *d, char *buf, gsize len)
     struct timeval timeout = { 1, 0 };
     char readbuf[1024];
     ssize_t bytes_read;
-    char *p = &readbuf[0];
     int total = 0, retries = 0;
-    gboolean escaping = FALSE;
+    gsize decap_len = 0;
 
     FD_ZERO (&in);
     FD_SET (d->fd, &in);
@@ -156,7 +155,7 @@ wait_reply (TestComData *d, char *buf, gsize len)
 
     do {
         errno = 0;
-        bytes_read = read (d->fd, p, 1);
+        bytes_read = read (d->fd, &readbuf[total], 1);
         if ((bytes_read == 0) || (errno == EAGAIN)) {
             /* Haven't gotten the async control char yet */
             if (retries > 20)
@@ -167,20 +166,36 @@ wait_reply (TestComData *d, char *buf, gsize len)
             retries++;
             continue;
         } else if (bytes_read == 1) {
-            /* Check for the async control char */
-            if (*p++ == DIAG_CONTROL_CHAR)
-                break;
+            gboolean more = FALSE, success;
+            gsize used = 0;
+
             total++;
+            decap_len = 0;
+            print_buf ("<<<", readbuf, total);
+            success = dm_decapsulate_buffer (readbuf, total, buf, len, &decap_len, &used, &more);
+
+            /* Discard used data */
+            if (used > 0) {
+                total -= used;
+                memmove (readbuf, &readbuf[used], total);
+            }
+
+            if (success && !more) {
+                /* Success; we have a packet */
+                break;
+            }
         } else {
             /* Some error occurred */
             return 0;
         }
-    } while (total <= sizeof (readbuf));
+    } while (total < sizeof (readbuf));
 
-    if (d->debug)
+    if (d->debug) {
         print_buf ("<<<", readbuf, total);
+        print_buf ("D<<", buf, decap_len);
+    }
 
-    return dm_unescape (readbuf, total, buf, len, &escaping);
+    return decap_len;
 }
 
 void