diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/mm-qcdm-serial-port.c | 107 |
1 files changed, 66 insertions, 41 deletions
diff --git a/src/mm-qcdm-serial-port.c b/src/mm-qcdm-serial-port.c index 72463fbf..7f4302c5 100644 --- a/src/mm-qcdm-serial-port.c +++ b/src/mm-qcdm-serial-port.c @@ -37,22 +37,38 @@ typedef struct { /*****************************************************************************/ static gboolean -parse_response (MMSerialPort *port, GByteArray *response, GError **error) +find_qcdm_start (GByteArray *response, gsize *start) { - int i; + int i, last = -1; - /* Look for the QCDM packet termination character; if we found it, treat - * the buffer as a qcdm command. + /* Look for 3 bytes and a QCDM frame marker, ie enough data for a valid + * frame. There will usually be three cases here; (1) a QCDM frame + * starting with data and terminated by 0x7E, and (2) a QCDM frame starting + * with 0x7E and ending with 0x7E, and (3) a non-QCDM frame that still + * uses HDLC framing (like Sierra CnS) that starts and ends with 0x7E. */ for (i = 0; i < response->len; i++) { - if (response->data[i] == 0x7E) - return TRUE; + if (response->data[i] == 0x7E) { + if (i > last + 3) { + /* Got a full QCDM frame; 3 non-0x7E bytes and a terminator */ + if (start) + *start = last + 1; + return TRUE; + } + + /* Save position of the last QCDM frame marker */ + last = i; + } } - - /* Otherwise, need more data from the device */ return FALSE; } +static gboolean +parse_response (MMSerialPort *port, GByteArray *response, GError **error) +{ + return find_qcdm_start (response, NULL); +} + static gsize handle_response (MMSerialPort *port, GByteArray *response, @@ -64,41 +80,49 @@ handle_response (MMSerialPort *port, GByteArray *unescaped = NULL; GError *dm_error = NULL; gsize used = 0; + gsize start = 0; + gboolean success = FALSE, more = FALSE; + gsize unescaped_len = 0; + + if (error) + goto callback; + + /* Get the offset into the buffer of where the QCDM frame starts */ + if (!find_qcdm_start (response, &start)) { + g_set_error_literal (&dm_error, + MM_MODEM_ERROR, MM_MODEM_ERROR_GENERAL, + "Failed to parse QCDM packet."); + /* Discard the unparsable data */ + used = response->len; + goto callback; + } - /* Ignore empty frames */ - if (response->len > 0 && response->data[0] == 0x7E) - return 1; - - if (!error) { - gboolean more = FALSE, success; - gsize unescaped_len = 0; - - /* FIXME: don't munge around with byte array internals */ - unescaped = g_byte_array_sized_new (1024); - success = dm_decapsulate_buffer ((const char *) response->data, - response->len, - (char *) unescaped->data, - 1024, - &unescaped_len, - &used, - &more); - if (!success) { - g_set_error_literal (&dm_error, - MM_MODEM_ERROR, MM_MODEM_ERROR_GENERAL, - "Failed to unescape QCDM packet."); - g_byte_array_free (unescaped, TRUE); - unescaped = NULL; - } else if (more) { - /* Need more data; we shouldn't have gotten here since the parse - * function checks for the end-of-frame marker, but whatever. - */ - return 0; - } else { - /* Successfully decapsulated the DM command */ - unescaped->len = (guint) unescaped_len; - } + /* FIXME: don't munge around with byte array internals */ + unescaped = g_byte_array_sized_new (1024); + success = dm_decapsulate_buffer ((const char *) (response->data + start), + response->len - start, + (char *) unescaped->data, + 1024, + &unescaped_len, + &used, + &more); + if (!success) { + g_set_error_literal (&dm_error, + MM_MODEM_ERROR, MM_MODEM_ERROR_GENERAL, + "Failed to unescape QCDM packet."); + g_byte_array_free (unescaped, TRUE); + unescaped = NULL; + } else if (more) { + /* Need more data; we shouldn't have gotten here since the parse + * function checks for the end-of-frame marker, but whatever. + */ + return 0; + } else { + /* Successfully decapsulated the DM command */ + unescaped->len = (guint) unescaped_len; } +callback: response_callback (MM_QCDM_SERIAL_PORT (port), unescaped, dm_error ? dm_error : error, @@ -106,8 +130,9 @@ handle_response (MMSerialPort *port, if (unescaped) g_byte_array_free (unescaped, TRUE); + g_clear_error (&dm_error); - return used; + return start + used; } /*****************************************************************************/ |