1 files changed, 79 insertions, 0 deletions
diff --git a/libqcdm/src/commands.c b/libqcdm/src/commands.c
index e757cef4..9f2f9a79 100644
--- a/libqcdm/src/commands.c
+++ b/libqcdm/src/commands.c
@@ -23,6 +23,36 @@
 #include "result-private.h"
 #include "utils.h"
 
+/*
+ * utils_bin2hexstr
+ *
+ * Convert a byte-array into a hexadecimal string.
+ *
+ * Code originally by Alex Larsson <alexl@redhat.com> and
+ *  copyright Red Hat, Inc. under terms of the LGPL.
+ *
+ */
+static char *
+bin2hexstr (const guint8 *bytes, int len)
+{
+    static char hex_digits[] = "0123456789abcdef";
+    char *result;
+    int i;
+    gsize buflen = (len * 2) + 1;
+
+    g_return_val_if_fail (bytes != NULL, NULL);
+    g_return_val_if_fail (len > 0, NULL);
+    g_return_val_if_fail (len < 4096, NULL);   /* Arbitrary limit */
+
+    result = g_malloc0 (buflen);
+    for (i = 0; i < len; i++) {
+        result[2*i] = hex_digits[(bytes[i] >> 4) & 0xf];
+        result[2*i+1] = hex_digits[bytes[i] & 0xf];
+    }
+    result[buflen - 1] = '\0';
+    return result;
+}
+
 static gboolean
 check_command (const char *buf, gsize len, guint8 cmd, gsize min_len, GError **error)
 {
@@ -79,6 +109,8 @@ check_command (const char *buf, gsize len, guint8 cmd, gsize min_len, GError **e
     return TRUE;
 }
 
+/**********************************************************************/
+
 gsize
 qcdm_cmd_version_info_new (char *buf, gsize len, GError **error)
 {
@@ -136,3 +168,50 @@ qcdm_cmd_version_info_result (const char *buf, gsize len, GError **error)
     return result;
 }
 
+/**********************************************************************/
+
+gsize
+qcdm_cmd_esn_new (char *buf, gsize len, GError **error)
+{
+    char cmdbuf[3];
+    DMCmdHeader *cmd = (DMCmdHeader *) &cmdbuf[0];
+
+    g_return_val_if_fail (buf != NULL, 0);
+    g_return_val_if_fail (len >= sizeof (*cmd) + DIAG_TRAILER_LEN, 0);
+
+    memset (cmd, 0, sizeof (cmd));
+    cmd->code = DIAG_CMD_ESN;
+
+    return dm_encapsulate_buffer (cmdbuf, sizeof (*cmd), sizeof (cmdbuf), buf, len);
+}
+
+QCDMResult *
+qcdm_cmd_esn_result (const char *buf, gsize len, GError **error)
+{
+    QCDMResult *result = NULL;
+    DMCmdEsnRsp *rsp = (DMCmdEsnRsp *) buf;
+    char *tmp;
+    guint8 swapped[4];
+
+    g_return_val_if_fail (buf != NULL, NULL);
+
+    if (!check_command (buf, len, DIAG_CMD_ESN, sizeof (DMCmdEsnRsp), error))
+        return NULL;
+
+    result = qcdm_result_new ();
+
+    /* Convert the ESN from binary to a hex string; it's LE so we have to
+     * swap it to get the correct ordering.
+     */
+    swapped[0] = rsp->esn[3];
+    swapped[1] = rsp->esn[2];
+    swapped[2] = rsp->esn[1];
+    swapped[3] = rsp->esn[0];
+
+    tmp = bin2hexstr (&swapped[0], sizeof (swapped));
+    qcdm_result_add_string (result, QCDM_CMD_ESN_ITEM_ESN, tmp);
+    g_free (tmp);
+
+    return result;
+}
+