diff options
Diffstat (limited to 'src/mm-port-serial-qcdm.c')
| -rw-r--r-- | src/mm-port-serial-qcdm.c | 134 |
1 files changed, 66 insertions, 68 deletions
diff --git a/src/mm-port-serial-qcdm.c b/src/mm-port-serial-qcdm.c index b63927ff..7732851f 100644 --- a/src/mm-port-serial-qcdm.c +++ b/src/mm-port-serial-qcdm.c @@ -59,10 +59,69 @@ find_qcdm_start (GByteArray *response, gsize *start) return FALSE; } -static gboolean -parse_response (MMPortSerial *port, GByteArray *response, GError **error) +static MMPortSerialResponseType +parse_response (MMPortSerial *port, + GByteArray *response, + GByteArray **parsed_response, + GError **error) { - return find_qcdm_start (response, NULL); + gsize start = 0; + gsize used = 0; + gsize unescaped_len = 0; + guint8 *unescaped_buffer; + qcdmbool more = FALSE; + + /* Get the offset into the buffer of where the QCDM frame starts */ + if (!find_qcdm_start (response, &start)) { + /* Discard the unparsable data right away, we do need a QCDM + * start, and anything that comes before it is unknown data + * that we'll never use. */ + return MM_PORT_SERIAL_RESPONSE_NONE; + } + + /* If there is anything before the start marker, remove it */ + g_byte_array_remove_range (response, 0, start); + if (response->len == 0) + return MM_PORT_SERIAL_RESPONSE_NONE; + + /* Try to decapsulate the response into a buffer */ + unescaped_buffer = g_malloc (1024); + if (!dm_decapsulate_buffer ((const char *)(response->data), + response->len, + (char *)unescaped_buffer, + 1024, + &unescaped_len, + &used, + &more)) { + /* Report an error right away. Not being able to decapsulate a QCDM + * packet once we got message start marker likely means that this + * data that we got is not a QCDM message. */ + g_set_error (error, + MM_SERIAL_ERROR, + MM_SERIAL_ERROR_PARSE_FAILED, + "Failed to unescape QCDM packet"); + g_free (unescaped_buffer); + return MM_PORT_SERIAL_RESPONSE_ERROR; + } + + if (more) { + /* Need more data, we leave the original byte array untouched so that + * we can retry later when more data arrives. */ + g_free (unescaped_buffer); + return MM_PORT_SERIAL_RESPONSE_NONE; + } + + /* Successfully decapsulated the DM command. We'll build a new byte array + * with the response, and leave the input buffer cleaned up. */ + g_assert (unescaped_len <= 1024); + unescaped_buffer = g_realloc (unescaped_buffer, unescaped_len); + *parsed_response = g_byte_array_new_take (unescaped_buffer, unescaped_len); + + /* Remove the data we used from the input buffer, leaving out any + * additional data that may already been received (e.g. from the following + * message). */ + g_byte_array_remove_range (response, 0, used); + return MM_PORT_SERIAL_RESPONSE_BUFFER; } /*****************************************************************************/ @@ -83,75 +142,14 @@ serial_command_ready (MMPortSerial *port, GAsyncResult *res, GSimpleAsyncResult *simple) { - GByteArray *response_buffer; GByteArray *response; GError *error = NULL; - gsize used = 0; - gsize start = 0; - guint8 *unescaped_buffer = NULL; - gboolean success = FALSE; - qcdmbool more = FALSE; - gsize unescaped_len = 0; - - response_buffer = mm_port_serial_command_finish (port, res, &error); - if (!response_buffer) - goto out; - - /* Get the offset into the buffer of where the QCDM frame starts */ - start = 0; - if (!find_qcdm_start (response_buffer, &start)) { - error = g_error_new_literal (MM_SERIAL_ERROR, - MM_SERIAL_ERROR_FRAME_NOT_FOUND, - "QCDM frame start not found"); - /* Discard the unparsable data */ - used = response_buffer->len; - goto out; - } - - unescaped_buffer = g_malloc (1024); - success = dm_decapsulate_buffer ((const char *)(response_buffer->data + start), - response_buffer->len - start, - (char *)unescaped_buffer, - 1024, - &unescaped_len, - &used, - &more); - if (!success) { - error = g_error_new_literal (MM_SERIAL_ERROR, - MM_SERIAL_ERROR_PARSE_FAILED, - "Failed to unescape QCDM packet"); - g_free (unescaped_buffer); - unescaped_buffer = NULL; - goto out; - } - - if (more) { - /* Need more data; we shouldn't have gotten here since the parse - * function checks for the end-of-frame marker, but whatever. - */ - error = g_error_new_literal (MM_CORE_ERROR, - MM_CORE_ERROR_FAILED, - "QCDM packet is not complete"); - g_free (unescaped_buffer); - unescaped_buffer = NULL; - goto out; - } - /* Successfully decapsulated the DM command */ - g_assert (error == NULL); - g_assert (unescaped_len <= 1024); - unescaped_buffer = g_realloc (unescaped_buffer, unescaped_len); - response = g_byte_array_new_take (unescaped_buffer, unescaped_len); - g_simple_async_result_set_op_res_gpointer (simple, response, (GDestroyNotify)g_byte_array_unref); - -out: - if (error) + response = mm_port_serial_command_finish (port, res, &error); + if (!response) g_simple_async_result_take_error (simple, error); - if (start + used) - g_byte_array_remove_range (response_buffer, 0, start + used); - if (response_buffer) - g_byte_array_unref (response_buffer); - + else + g_simple_async_result_set_op_res_gpointer (simple, response, (GDestroyNotify)g_byte_array_unref); g_simple_async_result_complete (simple); g_object_unref (simple); } |