diff options
| -rw-r--r-- | libqcdm/src/commands.c | 2 | ||||
| -rw-r--r-- | libqcdm/src/utils.c | 125 | ||||
| -rw-r--r-- | libqcdm/src/utils.h | 18 | ||||
| -rw-r--r-- | libqcdm/tests/test-qcdm-com.c | 33 |
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 |