1 files changed, 66 insertions, 4 deletions

diff --git a/libwmc/src/commands.c b/libwmc/src/commands.c
index 08bfd4da..cda4a5be 100644
--- a/libwmc/src/commands.c
+++ b/libwmc/src/commands.c
@@ -27,16 +27,22 @@
 /**********************************************************************/
 
 static int
-check_command (const char *buf, gsize len, guint8 cmd, gsize min_len)
+check_command (const char *buf, gsize len, u_int8_t cmd, size_t min_len)
 {
     if (len < 1) {
         wmc_err (0, "Zero-length response");
         return -WMC_ERROR_RESPONSE_BAD_LENGTH;
     }
 
-    if (buf[0] != cmd) {
-        wmc_err (0, "Unexpected WMC command response (expected %d, got %d)",
-                 cmd, buf[0]);
+    if ((u_int8_t) buf[0] != WMC_CMD_MARKER) {
+        wmc_err (0, "Missing WMC command marker (expected 0x%02X, got 0x%02X)",
+                 WMC_CMD_MARKER, (u_int8_t) buf[0]);
+        return -WMC_ERROR_RESPONSE_UNEXPECTED;
+    }
+
+    if ((u_int8_t) buf[1] != cmd) {
+        wmc_err (0, "Unexpected WMC command response (expected 0x%02X, got 0x%02X)",
+                 (u_int8_t) cmd, (u_int8_t) buf[1]);
         return -WMC_ERROR_RESPONSE_UNEXPECTED;
     }
 
@@ -51,6 +57,61 @@ check_command (const char *buf, gsize len, guint8 cmd, gsize min_len)
 
 /**********************************************************************/
 
+/**
+ * wmc_cmd_init_new:
+ * @buf: buffer in which to store constructed command
+ * @buflen: size of @buf
+ * @wmc2: if %TRUE add additional data that later-model devices (UML290) want
+ *
+ * Returns: size of the constructed command on success, or 0 on failure
+ */
+size_t
+wmc_cmd_init_new (char *buf, size_t buflen, int wmc2)
+{
+    wmc_return_val_if_fail (buf != NULL, 0);
+
+    if (wmc2) {
+        WmcCmdInit2 *cmd = (WmcCmdInit2 *) buf;
+        const char data[] = { 0xda, 0x07, 0x0c, 0x00, 0x1e, 0x00, 0x09, 0x00, 0x39,
+                              0x00, 0x18, 0x00, 0x04, 0x00 };
+
+        wmc_return_val_if_fail (buflen >= sizeof (*cmd), 0);
+
+        memset (cmd, 0, sizeof (*cmd));
+        cmd->hdr.marker = WMC_CMD_MARKER;
+        cmd->hdr.cmd = WMC_CMD_INIT;
+        memcpy (cmd->_unknown1, data, sizeof (data));
+        return sizeof (*cmd);
+    } else {
+        WmcCmdHeader *cmd = (WmcCmdHeader *) buf;
+
+        wmc_return_val_if_fail (buflen >= sizeof (*cmd), 0);
+
+        memset (cmd, 0, sizeof (*cmd));
+        cmd->marker = WMC_CMD_MARKER;
+        cmd->cmd = WMC_CMD_INIT;
+        return sizeof (*cmd);
+    }
+}
+
+WmcResult *
+wmc_cmd_init_result (const char *buf, gsize buflen, int wmc2)
+{
+    g_return_val_if_fail (buf != NULL, NULL);
+
+    if (wmc2) {
+        if (check_command (buf, buflen, WMC_CMD_INIT, sizeof (WmcCmdInit2Rsp)) < 0)
+            return NULL;
+    } else {
+        if (check_command (buf, buflen, WMC_CMD_INIT, sizeof (WmcCmdHeader)) < 0)
+            return NULL;
+    }
+
+    return wmc_result_new ();
+}
+
+/**********************************************************************/
+
 size_t
 wmc_cmd_device_info_new (char *buf, size_t buflen)
 {
@@ -60,6 +121,7 @@ wmc_cmd_device_info_new (char *buf, size_t buflen)
     wmc_return_val_if_fail (buflen >= sizeof (*cmd), 0);
 
     memset (cmd, 0, sizeof (*cmd));
+    cmd->marker = WMC_CMD_MARKER;
     cmd->cmd = WMC_CMD_DEVICE_INFO;
     return sizeof (*cmd);
 }