diff options
Diffstat (limited to 'src/mm-qcdm-serial-port.c')
| -rw-r--r-- | src/mm-qcdm-serial-port.c | 134 |
1 files changed, 85 insertions, 49 deletions
diff --git a/src/mm-qcdm-serial-port.c b/src/mm-qcdm-serial-port.c index 1ce27e7..e467f2a 100644 --- a/src/mm-qcdm-serial-port.c +++ b/src/mm-qcdm-serial-port.c @@ -21,9 +21,9 @@ #include "mm-qcdm-serial-port.h" #include "mm-errors.h" -#include "mm-options.h" #include "libqcdm/src/com.h" #include "libqcdm/src/utils.h" +#include "mm-log.h" G_DEFINE_TYPE (MMQcdmSerialPort, mm_qcdm_serial_port, MM_TYPE_SERIAL_PORT) @@ -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; } /*****************************************************************************/ @@ -157,7 +182,6 @@ debug_log (MMSerialPort *port, const char *prefix, const char *buf, gsize len) { static GString *debug = NULL; const char *s = buf; - GTimeVal tv; if (!debug) debug = g_string_sized_new (512); @@ -167,12 +191,7 @@ debug_log (MMSerialPort *port, const char *prefix, const char *buf, gsize len) while (len--) g_string_append_printf (debug, " %02x", (guint8) (*s++ & 0xFF)); - g_get_current_time (&tv); - g_debug ("<%ld.%ld> (%s): %s", - tv.tv_sec, - tv.tv_usec, - mm_port_get_device (MM_PORT (port)), - debug->str); + mm_dbg ("(%s): %s", mm_port_get_device (MM_PORT (port)), debug->str); g_string_truncate (debug, 0); } @@ -196,6 +215,23 @@ mm_qcdm_serial_port_new (const char *name, MMPortType ptype) NULL)); } +MMQcdmSerialPort * +mm_qcdm_serial_port_new_fd (int fd, MMPortType ptype) +{ + MMQcdmSerialPort *port; + char *name; + + name = g_strdup_printf ("port%d", fd); + port = MM_QCDM_SERIAL_PORT (g_object_new (MM_TYPE_QCDM_SERIAL_PORT, + MM_PORT_DEVICE, name, + MM_PORT_SUBSYS, MM_PORT_SUBSYS_TTY, + MM_PORT_TYPE, ptype, + MM_SERIAL_PORT_FD, fd, + NULL)); + g_free (name); + return port; +} + static void mm_qcdm_serial_port_init (MMQcdmSerialPort *self) { |