--- libaitmqtt/src/cmds.c	2012/05/08 09:07:31	1.1.2.3
+++ libaitmqtt/src/cmds.c	2012/06/29 15:23:10	1.2.6.1
@@ -76,3 +76,54 @@ end:
 	free(msg.msg_base);
 	return ret;
 }
+
+/*
+ * mqtt_WillMessage() - Publish WILL message
+ *
+ * @sock = connected socket
+ * @ka = keep alive timeout
+ * @topic = will topic
+ * @data = will message
+ * return: -1 error, 1 timeout, 2 not ack or 0 ok
+ */
+int
+mqtt_WillMessage(int sock, u_short ka, const char *topic, const char *data)
+{
+	int ret = 0;
+	mqtt_msg_t msg = { NULL, 0 };
+
+	if (!topic)
+		return -1;	/* error */
+
+	/* will message */
+	if ((ret = mqtt_wait4data(sock, ka, POLLOUT)))
+		return ret;
+	ret = mqtt_msgPUBLISH(&msg, topic, 0xDEAD, 0, 1, 0, data, data ? strlen(data) : 0);
+	if (ret == -1)
+		return -1;	/* error */
+	if ((ret = send(sock, msg.msg_base, ret, MSG_NOSIGNAL)) == -1) {
+		LOGERR;
+		free(msg.msg_base);
+		return -1;	/* error */
+	} else
+		memset(msg.msg_base, 0, msg.msg_len);
+
+	/* will ack */
+	if ((ret = mqtt_wait4data(sock, ka, POLLIN | POLLPRI))) {
+		free(msg.msg_base);
+		return ret;
+	}
+	/* receive & decode packet */
+	if ((ret = recv(sock, msg.msg_base, msg.msg_len, 0)) == -1) {
+		LOGERR;
+		free(msg.msg_base);
+		return -1;	/* error */
+	}
+	if (mqtt_readPUBACK(&msg))
+		ret = 0;	/* ok */
+	else
+		ret = 2;	/* semi-error */
+
+	free(msg.msg_base);
+	return ret;
+}