--- libaitmqtt/src/aitmqtt.c	2012/04/25 13:49:15	1.1.1.1.2.5
+++ libaitmqtt/src/aitmqtt.c	2012/06/20 15:02:24	1.2
@@ -3,7 +3,7 @@
 *  by Michael Pounov <misho@openbsd-bg.org>
 *
 * $Author: misho $
-* $Id: aitmqtt.c,v 1.1.1.1.2.5 2012/04/25 13:49:15 misho Exp $
+* $Id: aitmqtt.c,v 1.2 2012/06/20 15:02:24 misho Exp $
 *
 **************************************************************************
 The ELWIX and AITNET software is distributed under the following
@@ -191,6 +191,38 @@ mqtt_msgRealloc(mqtt_msg_t * __restrict msg, u_short l
 }
 
 /*
+ * mqtt_msgDup() - Duplicate message buffer
+ *
+ * @msg = Message
+ * return: NULL error or !=NULL duplicated message, after use must call mqtt_msgFree() with all!=0
+ */
+inline mqtt_msg_t *
+mqtt_msgDup(mqtt_msg_t * __restrict msg)
+{
+	mqtt_msg_t *m = NULL;
+
+	m = malloc(sizeof(mqtt_msg_t));
+	if (!m) {
+		LOGERR;
+		return NULL;
+	} else
+		memset(m, 0, sizeof(mqtt_msg_t));
+
+	if (msg->msg_len) {
+		m->msg_len = msg->msg_len;
+		m->msg_base = malloc(m->msg_len);
+		if (!m->msg_base) {
+			LOGERR;
+			free(m);
+			return NULL;
+		} else
+			memcpy(m->msg_base, msg->msg_base, m->msg_len);
+	}
+
+	return m;
+}
+
+/*
  * mqtt_encodeLen() Encode number to MQTT length field
  *
  * @num = number for encode
@@ -270,16 +302,36 @@ mqtt_sizeLen(u_int len)
 }
 
 /*
- * mqtt_str2sub Create MQTT subscribe variable from string(s)
+ * mqtt_pktLen() - Get total packet length
  *
- * @csStr = strings
- * @strnum = number of strings elements
+ * @hdr = MQTT packet header
+ * return: packet length
+ */
+inline u_int
+mqtt_pktLen(struct mqtthdr * __restrict hdr)
+{
+	int siz, n = 0;
+
+	if (!hdr)
+		return 0;
+
+	siz = mqtt_decodeLen(hdr->mqtt_len, &n);
+	siz += sizeof(struct mqtthdr) + n - 1;
+
+	return siz;
+}
+
+/*
+ * mqtt_str2subs Create MQTT subscribe variable from string(s)
+ *
+ * @csStr = null terminated string array
+ * @strnum = copy at most number of strings elements
  * @qoses = QoS elements applied to subscribe variable, 
  * 		count of elements must be equal with csStr elements
  * return: NULL error or != subscribe variables array, must be free after use with mqtt_freeSub()
  */
 inline mqtt_subscr_t *
-mqtt_str2sub(const char **csStr, u_short strnum, u_char *qoses)
+mqtt_str2subs(const char **csStr, u_short strnum, u_char *qoses)
 {
 	mqtt_subscr_t *v;
 	register int i, items;
@@ -287,12 +339,11 @@ mqtt_str2sub(const char **csStr, u_short strnum, u_cha
 
 	if (!csStr)
 		return NULL;
-	for (items = 0, strs = csStr; *strs; items++, strs++)
-		if (strnum && items >= strnum) {
-			items = strnum;
-			break;
-		}
 
+	for (items = 0, strs = csStr; 
+			(!strnum || (strnum && items < strnum)) && *strs; 
+			items++, strs++);
+
 	if (!(v = malloc((items + 1) * sizeof(mqtt_subscr_t)))) {
 		LOGERR;
 		return NULL;
@@ -368,18 +419,85 @@ mqtt_subAlloc(u_short num)
  * return: NULL error or subscribe array, after use must call mqtt_subFree()
  */
 inline mqtt_subscr_t *
-mqtt_subRealloc(mqtt_subscr_t * __restrict subs, u_short num)
+mqtt_subRealloc(mqtt_subscr_t ** __restrict subs, u_short num)
 {
 	mqtt_subscr_t *s = NULL;
 
-	s = realloc(subs, (num + 1) * sizeof(mqtt_subscr_t));
+	if (!subs)
+		return NULL;
+
+	s = realloc(*subs, (num + 1) * sizeof(mqtt_subscr_t));
 	if (!s) {
 		LOGERR;
 		return NULL;
+	} else {
+		memset(s + num, 0, sizeof(mqtt_subscr_t));
+		*subs = s;
 	}
 
-	return s;
+	return *subs;
 }
+
+/*
+ * mqtt_subCopy() - Copy subscription structure to another one
+ *
+ * @dst = destination subscription
+ * @src = source subscription
+ * return: =NULL error or !=NULL successful copied a structure
+ */
+inline mqtt_subscr_t *
+mqtt_subCopy(mqtt_subscr_t * __restrict dst, mqtt_subscr_t * __restrict src)
+{
+	if (!dst || !src)
+		return NULL;
+
+	if (src->sub_topic.msg_base) {
+		dst->sub_topic.msg_base = malloc(src->sub_topic.msg_len + 1);
+		if (!dst->sub_topic.msg_base) {
+			LOGERR;
+			memset(dst, 0, sizeof(mqtt_subscr_t));
+			return NULL;
+		} else {
+			dst->sub_topic.msg_len = src->sub_topic.msg_len;
+			((char*) dst->sub_topic.msg_base)[dst->sub_topic.msg_len] = 0;
+			memcpy(dst->sub_topic.msg_base, src->sub_topic.msg_base, 
+					dst->sub_topic.msg_len);
+		}
+	} else {
+		/*
+		if (dst->sub_topic.msg_base)
+			free(dst->sub_topic.msg_base);
+			*/
+		dst->sub_topic.msg_base = NULL;
+		dst->sub_topic.msg_len = 0;
+	}
+	if (src->sub_value.msg_base) {
+		dst->sub_value.msg_base = malloc(src->sub_value.msg_len + 1);
+		if (!dst->sub_value.msg_base) {
+			LOGERR;
+			if (dst->sub_topic.msg_base)
+				free(dst->sub_topic.msg_base);
+			memset(dst, 0, sizeof(mqtt_subscr_t));
+			return NULL;
+		} else {
+			dst->sub_value.msg_len = src->sub_value.msg_len;
+			((char*) dst->sub_value.msg_base)[dst->sub_value.msg_len] = 0;
+			memcpy(dst->sub_value.msg_base, src->sub_value.msg_base, 
+					dst->sub_value.msg_len);
+		}
+	} else {
+		/*
+		if (dst->sub_value.msg_base)
+			free(dst->sub_value.msg_base);
+			*/
+		dst->sub_value.msg_base = NULL;
+		dst->sub_value.msg_len = 0;
+	}
+
+	dst->sub_ret = src->sub_ret;
+	return dst;
+}
+
 
 /*
  * mqtt_expandTopic() - Expanding topic to regular expression