--- libaitmqtt/src/pub.c	2012/06/11 08:37:41	1.1.1.1.2.5
+++ libaitmqtt/src/pub.c	2013/05/30 09:18:33	1.4
@@ -3,7 +3,7 @@
 *  by Michael Pounov <misho@openbsd-bg.org>
 *
 * $Author: misho $
-* $Id: pub.c,v 1.1.1.1.2.5 2012/06/11 08:37:41 misho Exp $
+* $Id: pub.c,v 1.4 2013/05/30 09:18:33 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, 2012
+Copyright 2004, 2005, 2006, 2007, 2008, 2009, 2010, 2011, 2012, 2013
 	by Michael Pounov <misho@elwix.org>.  All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -63,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, *l;
 	struct mqtthdr *hdr;
 	mqtthdr_var_t *topic;
 	mqtt_len_t *mid;
@@ -80,45 +81,54 @@ mqtt_msgPUBLISH(mqtt_msg_t * __restrict buf, const cha
 		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;
+	l = (u_int*) hdr->mqtt_len;
+	*l = 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));
-
-	return siz;
+	return siz + len;
 }
 
 static int
 _mqtt_msgPUB_(mqtt_msg_t * __restrict buf, u_char cmd, u_short msgID)
 {
-	int siz = 0;
+	int siz = sizeof(struct mqtthdr);
 	struct mqtthdr *hdr;
 	mqtt_len_t *v;
 
@@ -127,12 +137,8 @@ _mqtt_msgPUB_(mqtt_msg_t * __restrict buf, u_char cmd,
 
 	if (mqtt_msgRealloc(buf, sizeof(struct mqtthdr) + sizeof(mqtt_len_t)) == -1)
 		return -1;
-	else {
-		hdr = (struct mqtthdr *) (buf->msg_base + siz);
-		siz += sizeof(struct mqtthdr);
-		v = (mqtt_len_t*) (buf->msg_base + siz);
-		siz += sizeof(mqtt_len_t);
-	}
+	else
+		hdr = (struct mqtthdr *) buf->msg_base;
 
 	/* fixed header */
 	MQTTHDR_MSGINIT(hdr);
@@ -140,7 +146,9 @@ _mqtt_msgPUB_(mqtt_msg_t * __restrict buf, u_char cmd,
 	*hdr->mqtt_len = sizeof(mqtt_len_t);
 
 	/* MessageID */
+	v = (mqtt_len_t*) (buf->msg_base + siz);
 	v->val = htons(msgID);
+	siz += sizeof(mqtt_len_t);
 
 	return siz;
 }
@@ -152,7 +160,7 @@ _mqtt_msgPUB_(mqtt_msg_t * __restrict buf, u_char cmd,
  * @msgID = MessageID
  * return: -1 error or >-1 message size for send
  */
-inline int
+int
 mqtt_msgPUBACK(mqtt_msg_t * __restrict buf, u_short msgID)
 {
 	return _mqtt_msgPUB_(buf, MQTT_TYPE_PUBACK, msgID);
@@ -165,7 +173,7 @@ mqtt_msgPUBACK(mqtt_msg_t * __restrict buf, u_short ms
  * @msgID = MessageID
  * return: -1 error or >-1 message size for send
  */
-inline int
+int
 mqtt_msgPUBREC(mqtt_msg_t * __restrict buf, u_short msgID)
 {
 	return _mqtt_msgPUB_(buf, MQTT_TYPE_PUBREC, msgID);
@@ -178,7 +186,7 @@ mqtt_msgPUBREC(mqtt_msg_t * __restrict buf, u_short ms
  * @msgID = MessageID
  * return: -1 error or >-1 message size for send
  */
-inline int
+int
 mqtt_msgPUBREL(mqtt_msg_t * __restrict buf, u_short msgID)
 {
 	return _mqtt_msgPUB_(buf, MQTT_TYPE_PUBREL, msgID);
@@ -191,7 +199,7 @@ mqtt_msgPUBREL(mqtt_msg_t * __restrict buf, u_short ms
  * @msgID = MessageID
  * return: -1 error or >-1 message size for send
  */
-inline int
+int
 mqtt_msgPUBCOMP(mqtt_msg_t * __restrict buf, u_short msgID)
 {
 	return _mqtt_msgPUB_(buf, MQTT_TYPE_PUBCOMP, msgID);