--- libaitmqtt/src/pub.c	2012/01/26 13:07:33	1.1
+++ libaitmqtt/src/pub.c	2012/06/20 15:02:24	1.2
@@ -3,7 +3,7 @@
 *  by Michael Pounov <misho@openbsd-bg.org>
 *
 * $Author: misho $
-* $Id: pub.c,v 1.1 2012/01/26 13:07:33 misho Exp $
+* $Id: pub.c,v 1.2 2012/06/20 15:02:24 misho Exp $
 *
 **************************************************************************
 The ELWIX and AITNET software is distributed under the following
@@ -12,7 +12,7 @@ terms:
 All of the documentation and software included in the ELWIX and AITNET
 Releases is copyrighted by ELWIX - Sofia/Bulgaria <info@elwix.org>
 
-Copyright 2004, 2005, 2006, 2007, 2008, 2009, 2010, 2011
+Copyright 2004, 2005, 2006, 2007, 2008, 2009, 2010, 2011, 2012
 	by Michael Pounov <misho@elwix.org>.  All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -46,8 +46,6 @@ SUCH DAMAGE.
 #include "global.h"
 
 
-/* ------------------------------------------------------------------- */
-
 /*
  * mqtt_msgPUBLISH() Create PUBLISH message
  *
@@ -65,7 +63,8 @@ int
 mqtt_msgPUBLISH(mqtt_msg_t * __restrict buf, const char *csTopic, u_short msgID, 
 		u_char Dup, u_char QOS, u_char Retain, const void *pData, int datlen)
 {
-	int siz = 0;
+	int len, siz;
+	u_int n;
 	struct mqtthdr *hdr;
 	mqtthdr_var_t *topic;
 	mqtt_len_t *mid;
@@ -74,48 +73,55 @@ mqtt_msgPUBLISH(mqtt_msg_t * __restrict buf, const cha
 	if (!buf || !csTopic)
 		return -1;
 	if (QOS > MQTT_QOS_EXACTLY) {
-		mqtt_SetErr(EINVAL, "Error:: invalid QoS parameter");
+		mqtt_SetErr(EINVAL, "Invalid QoS parameter");
 		return -1;
 	}
 	if (!msgID && QOS != MQTT_QOS_ONCE) {
-		mqtt_SetErr(EINVAL, "Error:: invalid MessageID parameter must be >0");
+		mqtt_SetErr(EINVAL, "Invalid MessageID parameter must be >0");
 		return -1;
 	}
 
-	if (mqtt_msgRealloc(buf, MQTTMSG_MAX) == -1)
+	/* calculate message size */
+	len = sizeof(mqtt_len_t) + strlen(csTopic);	/* topic */
+	len += sizeof(mqtt_len_t);			/* msgid */
+	len += datlen;					/* data 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_PUBLISH;
+	hdr->mqtt_msg.qos = QOS;
+	hdr->mqtt_msg.dup = Dup ? 1 : 0;
+	hdr->mqtt_msg.retain = Retain ? 1 : 0;
+	*(u_int*) hdr->mqtt_len = n;
+	data += siz;
+
 	/* variable header */
-	topic = (mqtthdr_var_t*) (buf->msg_base + siz);
+	topic = (mqtthdr_var_t*) data;
 	topic->var_sb.val = htons(strlen(csTopic));
 	memcpy(topic->var_data, csTopic, ntohs(topic->var_sb.val));
-	siz += MQTTHDR_VAR_SIZEOF(topic);
+	data += MQTTHDR_VAR_SIZEOF(topic);
 
-	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);
 
 	/* load with data */
-	if (pData && datlen) {
-		data = buf->msg_base + siz;
+	if (pData && datlen)
 		memcpy(data, pData, datlen);
-		siz += datlen;
-	}
 
-	/* fixed header */
-	MQTTHDR_MSGINIT(hdr);
-	hdr->mqtt_msg.type = MQTT_TYPE_PUBLISH;
-	hdr->mqtt_msg.qos = QOS;
-	hdr->mqtt_msg.dup = Dup ? 1 : 0;
-	hdr->mqtt_msg.retain = Retain ? 1 : 0;
-	*hdr->mqtt_len = mqtt_encodeLen(siz - sizeof(struct mqtthdr));
-
-	mqtt_msgRealloc(buf, siz);
-	return siz;
+	return siz + len;
 }
 
 static int
@@ -210,13 +216,12 @@ mqtt_msgPUBCOMP(mqtt_msg_t * __restrict buf, u_short m
  * @psTopic = Topic
  * @topicLen = Topic length
  * @msgID = MessageID
- * @pData = Data buffer
- * @datLen = Data buffer length, if *datLen == 0 allocate memory for pData
- * return: NULL error or !=NULL MQTT fixed header
+ * @pData = Data buffer, may be NULL
+ * return: -1 error or !=-1 allocated data buffer length
  */
-struct mqtthdr *
+int
 mqtt_readPUBLISH(mqtt_msg_t * __restrict buf, char * __restrict psTopic, int topicLen, 
-		u_short *msgID, void * __restrict pData, int *datLen)
+		u_short *msgID, void ** __restrict pData)
 {
 	int len, ret;
 	struct mqtthdr *hdr;
@@ -224,20 +229,20 @@ mqtt_readPUBLISH(mqtt_msg_t * __restrict buf, char * _
 	mqtt_len_t *v;
 	caddr_t pos;
 
-	if (!buf || !psTopic || !msgID || !pData)
-		return NULL;
+	if (!buf || !psTopic || !msgID)
+		return -1;
 
 	hdr = _mqtt_readHEADER(buf, MQTT_TYPE_PUBLISH, &ret, &len);
 	if (!hdr)
-		return NULL;
+		return -1;
 	pos = buf->msg_base + ret + 1;
 	var = (mqtthdr_var_t*) pos;
 
 	/* topic */
 	len -= MQTTHDR_VAR_SIZEOF(var);
 	if (len < 0) {
-		mqtt_SetErr(EINVAL, "Error:: short message length %d", len);
-		return NULL;
+		mqtt_SetErr(EINVAL, "Short message length %d", len);
+		return -1;
 	} else {
 		memset(psTopic, 0, topicLen--);
 		memcpy(psTopic, var->var_data, ntohs(var->var_sb.val) > topicLen ? 
@@ -248,8 +253,8 @@ mqtt_readPUBLISH(mqtt_msg_t * __restrict buf, char * _
 
 	len -= sizeof(mqtt_len_t);
 	if (len < 0) {
-		mqtt_SetErr(EINVAL, "Error:: short message length %d", len);
-		return NULL;
+		mqtt_SetErr(EINVAL, "Short message length %d", len);
+		return -1;
 	} else {
 		*msgID = ntohs(v->val);
 		pos += sizeof(mqtt_len_t);
@@ -257,24 +262,19 @@ mqtt_readPUBLISH(mqtt_msg_t * __restrict buf, char * _
 
 	/* data */
 	if (len < 0) {
-		mqtt_SetErr(EINVAL, "Error:: short message length %d", len);
-		return NULL;
-	} else {
-		if (!*datLen) {
-			if (!(pData = malloc(len))) {
-				LOGERR;
-				return NULL;
-			} else
-				*datLen = len;
-		}
+		mqtt_SetErr(EINVAL, "Short message length %d", len);
+		return -1;
+	} else if (pData) {
+		if (!(*pData = malloc(len + 1))) {
+			LOGERR;
+			return -1;
+		} else
+			((char*) (*pData))[len] = 0;
 
-		memset(pData, 0, *datLen);
-		if (len < *datLen)
-			*datLen = len;
-		memcpy(pData, pos, *datLen);
+		memcpy(*pData, pos, len);
 	}
 
-	return hdr;
+	return len;
 }
 
 /*
@@ -295,7 +295,7 @@ mqtt_readPUBACK(mqtt_msg_t * __restrict buf)
 	if (!hdr)
 		return (u_short) -1;
 	if (len < sizeof(mqtt_len_t)) {
-		mqtt_SetErr(EINVAL, "Error:: short message length %d", len);
+		mqtt_SetErr(EINVAL, "Short message length %d", len);
 		return (u_short) -1;
 	} else {
 		pos = buf->msg_base + ret + 1;
@@ -323,7 +323,7 @@ mqtt_readPUBREC(mqtt_msg_t * __restrict buf)
 	if (!hdr)
 		return (u_short) -1;
 	if (len < sizeof(mqtt_len_t)) {
-		mqtt_SetErr(EINVAL, "Error:: short message length %d", len);
+		mqtt_SetErr(EINVAL, "Short message length %d", len);
 		return (u_short) -1;
 	} else {
 		pos = buf->msg_base + ret + 1;
@@ -351,7 +351,7 @@ mqtt_readPUBREL(mqtt_msg_t * __restrict buf)
 	if (!hdr)
 		return (u_short) -1;
 	if (len < sizeof(mqtt_len_t)) {
-		mqtt_SetErr(EINVAL, "Error:: short message length %d", len);
+		mqtt_SetErr(EINVAL, "Short message length %d", len);
 		return (u_short) -1;
 	} else {
 		pos = buf->msg_base + ret + 1;
@@ -379,7 +379,7 @@ mqtt_readPUBCOMP(mqtt_msg_t * __restrict buf)
 	if (!hdr)
 		return (u_short) -1;
 	if (len < sizeof(mqtt_len_t)) {
-		mqtt_SetErr(EINVAL, "Error:: short message length %d", len);
+		mqtt_SetErr(EINVAL, "Short message length %d", len);
 		return (u_short) -1;
 	} else {
 		pos = buf->msg_base + ret + 1;