aqhome: fixed mqtt message handling

PUBLISH: the message itself is NOT preceeded by size

aqhome/mqtt/msg_mqtt.c

@@ -15,6 +15,8 @@
 
 #include <gwenhywfar/debug.h>
 
+#include <string.h>
+
 // maximum remaining length: fff ffff
 
 
@@ -205,3 +207,53 @@ int AQH_MqttMsg_DumpString(const uint8_t *ptr, uint32_t len, GWEN_BUFFER *buf)
 
 
 
+char *AQH_MqttMsg_ExtractStringAt(const uint8_t *ptr, uint32_t len)
+{
+  if (len>1) {
+    int slen;
+
+    slen=(ptr[0]<<8)+ptr[1];
+    if (slen) {
+      char *result;
+
+      if (slen>(len-2)) {
+        DBG_ERROR(AQH_LOGDOMAIN, "Invalid string length (%lu, remaining %lu)",
+                  (unsigned long int) slen, (unsigned long int) len);
+        return NULL;
+      }
+
+      result=(char*) malloc(slen+1);
+      if (result==NULL) {
+        DBG_ERROR(AQH_LOGDOMAIN, "Error on malloc");
+        return NULL;
+      }
+      memmove(result, ptr+2, slen);
+      result[slen]=0;
+      return result;
+    }
+  }
+  return NULL;
+}
+
+
+
+int AQH_MqttMsg_SkipStringAt(const uint8_t *ptr, uint32_t len)
+{
+  if (len>1) {
+    int slen;
+
+    slen=(ptr[0]<<8)+ptr[1];
+    if (slen) {
+      if (slen>(len-2)) {
+        DBG_ERROR(AQH_LOGDOMAIN, "Invalid string length (%lu, remaining %lu)",
+                  (unsigned long int) slen, (unsigned long int) len);
+        return GWEN_ERROR_BAD_DATA;
+      }
+    }
+    return slen+2;
+  }
+  return GWEN_ERROR_BAD_DATA;
+}
+
+
+