--- libaitmqtt/src/cliside.c	2012/05/08 11:29:56	1.1.2.5
+++ libaitmqtt/src/cliside.c	2012/06/28 09:01:42	1.2.2.1
@@ -3,7 +3,7 @@
 *  by Michael Pounov <misho@openbsd-bg.org>
 *
 * $Author: misho $
-* $Id: cliside.c,v 1.1.2.5 2012/05/08 11:29:56 misho Exp $
+* $Id: cliside.c,v 1.2.2.1 2012/06/28 09:01:42 misho Exp $
 *
 **************************************************************************
 The ELWIX and AITNET software is distributed under the following
@@ -152,7 +152,8 @@ mqtt_cli_Subscribe(mqtt_cli_t * __restrict cli, mqtt_s
 	if (siz == -1) {
 		LOGERR;
 		return NULL;
-	}
+	} else
+		memset(cli->buf->msg_base, 0, cli->buf->msg_len);
 
 	if ((siz = mqtt_wait4data(cli->sock, cli->timeout, POLLIN | POLLPRI)) == -1) {
 		return NULL;
@@ -170,7 +171,7 @@ mqtt_cli_Subscribe(mqtt_cli_t * __restrict cli, mqtt_s
 		return NULL;
 	if (msgID != mid) {
 		free(qoses);
-		mqtt_SetErr(EBADMSG, "Receive different message ID %hu != %hu", msgID, mid);
+		mqtt_SetErr(ECANCELED, "Receive different message ID %hu != %hu", msgID, mid);
 		return NULL;
 	}
 
@@ -192,7 +193,6 @@ mqtt_cli_Unsubscribe(mqtt_cli_t * __restrict cli, mqtt
 		u_short msgID, u_char Dup, u_char QoS)
 {
 	int siz = 0;
-	u_short mid = 0;
 
 	if (!cli || !Topics)
 		return -1;
@@ -205,7 +205,8 @@ mqtt_cli_Unsubscribe(mqtt_cli_t * __restrict cli, mqtt
 	if (siz == -1) {
 		LOGERR;
 		return -1;
-	}
+	} else
+		memset(cli->buf->msg_base, 0, cli->buf->msg_len);
 
 	if ((siz = mqtt_wait4data(cli->sock, cli->timeout, POLLIN | POLLPRI)) == -1) {
 		return -1;
@@ -222,9 +223,122 @@ mqtt_cli_Unsubscribe(mqtt_cli_t * __restrict cli, mqtt
 	if (siz == -1)
 		return -1;
 	if (msgID != siz) {
-		mqtt_SetErr(EBADMSG, "Receive different message ID %hu != %hu", msgID, mid);
+		mqtt_SetErr(ECANCELED, "Receive different message ID %hu != %hu", msgID, siz);
 		return -1;
 	}
 
 	return 0;
+}
+
+/*
+ * mqtt_cli_Publish() - Publish message to broker
+ *
+ * @cli = connected client
+ * @msgID = Message ID
+ * @Dup = Duplicated request
+ * @QoS = Message QoS
+ * @Retain = Retain message
+ * @csTopic = Topic
+ * @pData = Data
+ * @datLen = Data length
+ * return: -1 error or > -1 sended bytes
+ */
+int
+mqtt_cli_Publish(mqtt_cli_t * __restrict cli, u_short msgID, u_char Dup, u_char QoS, u_char Retain, 
+		const char *csTopic, const void *pData, int datLen)
+{
+	int wlen = 0, siz = 0;
+
+	if (!cli || !csTopic)
+		return -1;
+
+	/* send publish */
+	siz = mqtt_msgPUBLISH(cli->buf, csTopic, msgID, Dup, QoS, Retain, pData, datLen);
+	if (siz == -1)
+		return -1;
+	siz = send(cli->sock, cli->buf->msg_base, siz, MSG_NOSIGNAL);
+	if (siz == -1) {
+		LOGERR;
+		return -1;
+	} else {
+		wlen = siz;
+		memset(cli->buf->msg_base, 0, cli->buf->msg_len);
+	}
+
+	if (QoS == MQTT_QOS_ONCE)	/* no reply */
+		goto end;
+
+	if ((siz = mqtt_wait4data(cli->sock, cli->timeout, POLLIN | POLLPRI)) == -1) {
+		return -1;
+	} else if (siz && mqtt_KeepAlive(cli->sock, cli->timeout, 1))
+		return -1;
+
+	/* receive PUBxxx */
+	siz = recv(cli->sock, cli->buf->msg_base, cli->buf->msg_len, 0);
+	if (siz == -1) {
+		LOGERR;
+		return -1;
+	}
+
+	if (QoS == MQTT_QOS_ACK) {	/* reply with PUBACK */
+		siz = mqtt_readPUBACK(cli->buf);
+		if (siz == -1)
+			return -1;
+		if (msgID != siz) {
+			mqtt_SetErr(ECANCELED, "Receive different message ID %hu != %hu", msgID, siz);
+			return -1;
+		}
+		goto end;
+	} else {			/* reply with PUBREC */
+		siz = mqtt_readPUBREC(cli->buf);
+		if (siz == -1)
+			return -1;
+		if (msgID != siz) {
+			mqtt_SetErr(ECANCELED, "Receive different message ID %hu != %hu", msgID, siz);
+			return -1;
+		}
+	}
+
+	do {
+		/* send publish release QoS == 2 */
+		siz = mqtt_msgPUBREL(cli->buf, msgID);
+		if (siz == -1)
+			return -1;
+		siz = send(cli->sock, cli->buf->msg_base, siz, MSG_NOSIGNAL);
+		if (siz == -1) {
+			LOGERR;
+			return -1;
+		} else
+			memset(cli->buf->msg_base, 0, cli->buf->msg_len);
+
+		if ((siz = mqtt_wait4data(cli->sock, cli->timeout, POLLIN | POLLPRI)) == -1) {
+			return -1;
+		} else if (siz && mqtt_KeepAlive(cli->sock, cli->timeout, 1)) {
+			if (Dup++ > 1)
+				return -1;
+			else
+				continue;
+		}
+
+		/* receive PUBCOMP */
+		siz = recv(cli->sock, cli->buf->msg_base, cli->buf->msg_len, 0);
+		if (siz == -1) {
+			LOGERR;
+			return -1;
+		}
+
+		siz = mqtt_readPUBCOMP(cli->buf);
+		if (siz == -1)
+			return -1;
+		if (msgID != siz) {
+			mqtt_SetErr(ECANCELED, "Receive different message ID %hu != %hu", msgID, siz);
+			if (Dup++ > 1)
+				return -1;
+			else
+				continue;
+		}
+	} while (0);
+
+end:
+	return wlen;
 }