--- libaitmqtt/src/sub.c	2012/04/07 20:54:57	1.1.1.1.2.1
+++ libaitmqtt/src/sub.c	2012/06/20 11:11:30	1.1.1.1.2.6
@@ -3,7 +3,7 @@
 *  by Michael Pounov <misho@openbsd-bg.org>
 *
 * $Author: misho $
-* $Id: sub.c,v 1.1.1.1.2.1 2012/04/07 20:54:57 misho Exp $
+* $Id: sub.c,v 1.1.1.1.2.6 2012/06/20 11:11:30 misho Exp $
 *
 **************************************************************************
 The ELWIX and AITNET software is distributed under the following
@@ -60,12 +60,14 @@ int
 mqtt_msgSUBSCRIBE(mqtt_msg_t * __restrict buf, mqtt_subscr_t * __restrict Topics, 
 		u_short msgID, u_char Dup, u_char QOS)
 {
-	int siz = 0;
+	int len, siz = 0;
+	u_int n;
 	struct mqtthdr *hdr;
 	mqtthdr_var_t *topic;
 	mqtt_len_t *mid;
 	mqtt_subscr_t *t;
 	u_char *qos;
+	void *data;
 
 	if (!buf || !Topics)
 		return -1;
@@ -78,38 +80,47 @@ mqtt_msgSUBSCRIBE(mqtt_msg_t * __restrict buf, mqtt_su
 		return -1;
 	}
 
-	if (mqtt_msgRealloc(buf, MQTTMSG_MAX) == -1)
+	/* calculate message size */
+	len = sizeof(mqtt_len_t);				/* msgid */
+	for (t = Topics; t && t->sub_topic.msg_base; t++)	/* subscribes & qos */
+		len += sizeof(mqtt_len_t) + t->sub_topic.msg_len + 1;
+
+	/* calculate header size */
+	siz = sizeof(struct mqtthdr);				/* mqtt fixed header */
+	n = mqtt_encodeLen(len);				/* message size */
+	siz += mqtt_sizeLen(n) - 1;				/* length size */
+
+	if (mqtt_msgRealloc(buf, siz + len) == -1)
 		return -1;
 	else {
-		hdr = (struct mqtthdr *) (buf->msg_base + siz);
-		siz += sizeof(struct mqtthdr);
+		data = buf->msg_base;
+		hdr = (struct mqtthdr *) data;
 	}
 
+	/* fixed header */
+	MQTTHDR_MSGINIT(hdr);
+	hdr->mqtt_msg.type = MQTT_TYPE_SUBSCRIBE;
+	hdr->mqtt_msg.qos = QOS;
+	hdr->mqtt_msg.dup = Dup ? 1 : 0;
+	hdr->mqtt_msg.retain = 0;
+	*(u_int*) hdr->mqtt_len = n;
+	data += siz;
+
 	/* variable header */
-	mid = (mqtt_len_t*) (buf->msg_base + siz);
+	mid = (mqtt_len_t*) data;
 	mid->val = htons(msgID);
-	siz += sizeof(mqtt_len_t);
+	data += sizeof(mqtt_len_t);
 
 	/* payload with subscriptions */
 	for (t = Topics; t && t->sub_topic.msg_base; t++) {
-		topic = (mqtthdr_var_t*) (buf->msg_base + siz);
+		topic = (mqtthdr_var_t*) data;
 		topic->var_sb.val = htons(t->sub_topic.msg_len);
 		memcpy(topic->var_data, t->sub_topic.msg_base, ntohs(topic->var_sb.val));
-		siz += MQTTHDR_VAR_SIZEOF(topic);
-		qos = (buf->msg_base + siz);
+		data += MQTTHDR_VAR_SIZEOF(topic);
+		qos = data++;
 		*qos = t->sub_ret;
-		siz++;
 	}
 
-	/* fixed header */
-	MQTTHDR_MSGINIT(hdr);
-	hdr->mqtt_msg.type = MQTT_TYPE_SUBSCRIBE;
-	hdr->mqtt_msg.qos = QOS;
-	hdr->mqtt_msg.dup = Dup ? 1 : 0;
-	hdr->mqtt_msg.retain = 0;
-	*hdr->mqtt_len = mqtt_encodeLen(siz - sizeof(struct mqtthdr));
-
-	mqtt_msgRealloc(buf, siz);
 	return siz;
 }
 
@@ -157,7 +168,6 @@ mqtt_msgSUBACK(mqtt_msg_t * __restrict buf, mqtt_subsc
 	hdr->mqtt_msg.type = MQTT_TYPE_SUBACK;
 	*hdr->mqtt_len = mqtt_encodeLen(siz - sizeof(struct mqtthdr));
 
-	mqtt_msgRealloc(buf, siz);
 	return siz;
 }
 
@@ -175,11 +185,13 @@ int
 mqtt_msgUNSUBSCRIBE(mqtt_msg_t * __restrict buf, mqtt_subscr_t * __restrict Topics, 
 		u_short msgID, u_char Dup, u_char QOS)
 {
-	int siz = 0;
+	int len, siz = 0;
+	u_int n;
 	struct mqtthdr *hdr;
 	mqtthdr_var_t *topic;
 	mqtt_len_t *mid;
 	mqtt_subscr_t *t;
+	void *data;
 
 	if (!buf || !Topics)
 		return -1;
@@ -192,35 +204,45 @@ mqtt_msgUNSUBSCRIBE(mqtt_msg_t * __restrict buf, mqtt_
 		return -1;
 	}
 
-	if (mqtt_msgRealloc(buf, MQTTMSG_MAX) == -1)
+	/* calculate message size */
+	len = sizeof(mqtt_len_t);				/* msgid */
+	for (t = Topics; t && t->sub_topic.msg_base; t++)	/* subscribes */
+		len += sizeof(mqtt_len_t) + t->sub_topic.msg_len;
+
+	/* calculate header size */
+	siz = sizeof(struct mqtthdr);				/* mqtt fixed header */
+	n = mqtt_encodeLen(len);				/* message size */
+	siz += mqtt_sizeLen(n) - 1;				/* length size */
+
+	if (mqtt_msgRealloc(buf, siz + len) == -1)
 		return -1;
 	else {
-		hdr = (struct mqtthdr *) (buf->msg_base + siz);
-		siz += sizeof(struct mqtthdr);
+		data = buf->msg_base;
+		hdr = (struct mqtthdr *) data;
 	}
 
+	/* fixed header */
+	MQTTHDR_MSGINIT(hdr);
+	hdr->mqtt_msg.type = MQTT_TYPE_UNSUBSCRIBE;
+	hdr->mqtt_msg.qos = QOS;
+	hdr->mqtt_msg.dup = Dup ? 1 : 0;
+	hdr->mqtt_msg.retain = 0;
+	*(u_int*) hdr->mqtt_len = n;
+	data += siz;
+
 	/* variable header */
 	mid = (mqtt_len_t*) (buf->msg_base + siz);
 	mid->val = htons(msgID);
-	siz += sizeof(mqtt_len_t);
+	data += sizeof(mqtt_len_t);
 
 	/* payload with subscriptions */
 	for (t = Topics; t && t->sub_topic.msg_base; t++) {
-		topic = (mqtthdr_var_t*) (buf->msg_base + siz);
+		topic = (mqtthdr_var_t*) data;
 		topic->var_sb.val = htons(t->sub_topic.msg_len);
 		memcpy(topic->var_data, t->sub_topic.msg_base, ntohs(topic->var_sb.val));
-		siz += MQTTHDR_VAR_SIZEOF(topic);
+		data += MQTTHDR_VAR_SIZEOF(topic);
 	}
 
-	/* fixed header */
-	MQTTHDR_MSGINIT(hdr);
-	hdr->mqtt_msg.type = MQTT_TYPE_UNSUBSCRIBE;
-	hdr->mqtt_msg.qos = QOS;
-	hdr->mqtt_msg.dup = Dup ? 1 : 0;
-	hdr->mqtt_msg.retain = 0;
-	*hdr->mqtt_len = mqtt_encodeLen(siz - sizeof(struct mqtthdr));
-
-	mqtt_msgRealloc(buf, siz);
 	return siz;
 }
 
@@ -270,9 +292,9 @@ mqtt_msgUNSUBACK(mqtt_msg_t * __restrict buf, u_short 
  * @buf = Message buffer
  * @msgID = MessageID
  * @subscr = Subscriptions, must be free after use with mqtt_subFree()
- * return: NULL error or !=NULL MQTT fixed header
+ * return: -1 error or >-1 elements into subscr
  */
-struct mqtthdr *
+int
 mqtt_readSUBSCRIBE(mqtt_msg_t * __restrict buf, u_short *msgID, mqtt_subscr_t **subscr)
 {
 	register int i;
@@ -284,11 +306,11 @@ mqtt_readSUBSCRIBE(mqtt_msg_t * __restrict buf, u_shor
 	caddr_t pos;
 
 	if (!buf || !msgID || !subscr)
-		return NULL;
+		return -1;
 
 	hdr = _mqtt_readHEADER(buf, MQTT_TYPE_SUBSCRIBE, &ret, &len);
 	if (!hdr)
-		return NULL;
+		return -1;
 	pos = buf->msg_base + ret + 1;
 	v = (mqtt_len_t*) pos;
 
@@ -296,7 +318,7 @@ mqtt_readSUBSCRIBE(mqtt_msg_t * __restrict buf, u_shor
 	len -= sizeof(mqtt_len_t);
 	if (len < 0) {
 		mqtt_SetErr(EINVAL, "Short message length %d", len);
-		return NULL;
+		return -1;
 	} else {
 		*msgID = ntohs(v->val);
 		pos += sizeof(mqtt_len_t);
@@ -304,7 +326,7 @@ mqtt_readSUBSCRIBE(mqtt_msg_t * __restrict buf, u_shor
 
 	subs = mqtt_subAlloc(0);
 	if (!subs)
-		return NULL;
+		return -1;
 	else
 		*subscr = subs;
 
@@ -315,31 +337,32 @@ mqtt_readSUBSCRIBE(mqtt_msg_t * __restrict buf, u_shor
 		if (len < 0) {
 			mqtt_subFree(subscr);
 			mqtt_SetErr(EINVAL, "Short message length %d", len);
-			return NULL;
+			return -1;
 		}
-		subs = mqtt_subRealloc(subs, i + 1);
-		if (!subs) {
+		if (!mqtt_subRealloc(&subs, i + 1)) {
 			mqtt_subFree(subscr);
-			return NULL;
+			return -1;
 		} else
 			*subscr = subs;
 
 		memset(&subs[i], 0, sizeof subs[i]);
 		subs[i].sub_topic.msg_len = ntohs(var->var_sb.val);
-		subs[i].sub_topic.msg_base = malloc(subs[i].sub_topic.msg_len);
+		subs[i].sub_topic.msg_base = malloc(subs[i].sub_topic.msg_len + 1);
 		if (!subs[i].sub_topic.msg_base) {
 			LOGERR;
 			mqtt_subFree(subscr);
-			return NULL;
-		} else
+			return -1;
+		} else {
 			memcpy(subs[i].sub_topic.msg_base, var->var_data, subs[i].sub_topic.msg_len);
+			((char*) subs[i].sub_topic.msg_base)[subs[i].sub_topic.msg_len] = 0;
+		}
 		pos += MQTTHDR_VAR_SIZEOF(var);
 
 		subs[i].sub_ret = *pos;
 		pos++;
 	}
 
-	return hdr;
+	return i;
 }
 
 /*
@@ -394,9 +417,9 @@ mqtt_readSUBACK(mqtt_msg_t * __restrict buf, u_short *
  * @buf = Message buffer
  * @msgID = MessageID
  * @subscr = Subscriptions, must be free after use with mqtt_subFree()
- * return: NULL error or !=NULL MQTT fixed header
+ * return: -1 error or >-1 elements into subscr
  */
-struct mqtthdr *
+int
 mqtt_readUNSUBSCRIBE(mqtt_msg_t * __restrict buf, u_short *msgID, mqtt_subscr_t **subscr)
 {
 	register int i;
@@ -408,11 +431,11 @@ mqtt_readUNSUBSCRIBE(mqtt_msg_t * __restrict buf, u_sh
 	caddr_t pos;
 
 	if (!buf || !msgID || !subscr)
-		return NULL;
+		return -1;
 
 	hdr = _mqtt_readHEADER(buf, MQTT_TYPE_UNSUBSCRIBE, &ret, &len);
 	if (!hdr)
-		return NULL;
+		return -1;
 	pos = buf->msg_base + ret + 1;
 	v = (mqtt_len_t*) pos;
 
@@ -420,7 +443,7 @@ mqtt_readUNSUBSCRIBE(mqtt_msg_t * __restrict buf, u_sh
 	len -= sizeof(mqtt_len_t);
 	if (len < 0) {
 		mqtt_SetErr(EINVAL, "Short message length %d", len);
-		return NULL;
+		return -1;
 	} else {
 		*msgID = ntohs(v->val);
 		pos += sizeof(mqtt_len_t);
@@ -428,7 +451,7 @@ mqtt_readUNSUBSCRIBE(mqtt_msg_t * __restrict buf, u_sh
 
 	subs = mqtt_subAlloc(0);
 	if (!subs)
-		return NULL;
+		return -1;
 	else
 		*subscr = subs;
 
@@ -439,28 +462,29 @@ mqtt_readUNSUBSCRIBE(mqtt_msg_t * __restrict buf, u_sh
 		if (len < 0) {
 			mqtt_subFree(subscr);
 			mqtt_SetErr(EINVAL, "Short message length %d", len);
-			return NULL;
+			return -1;
 		}
-		subs = mqtt_subRealloc(subs, i + 1);
-		if (!subs) {
+		if (!mqtt_subRealloc(&subs, i + 1)) {
 			mqtt_subFree(subscr);
-			return NULL;
+			return -1;
 		} else
 			*subscr = subs;
 
 		memset(&subs[i], 0, sizeof subs[i]);
 		subs[i].sub_topic.msg_len = ntohs(var->var_sb.val);
-		subs[i].sub_topic.msg_base = malloc(subs[i].sub_topic.msg_len);
+		subs[i].sub_topic.msg_base = malloc(subs[i].sub_topic.msg_len + 1);
 		if (!subs[i].sub_topic.msg_base) {
 			LOGERR;
 			mqtt_subFree(subscr);
-			return NULL;
-		} else
+			return -1;
+		} else {
 			memcpy(subs[i].sub_topic.msg_base, var->var_data, subs[i].sub_topic.msg_len);
+			((char*) subs[i].sub_topic.msg_base)[subs[i].sub_topic.msg_len] = 0;
+		}
 		pos += MQTTHDR_VAR_SIZEOF(var);
 	}
 
-	return hdr;
+	return i;
 }
 
 /*