--- mqtt/src/mqtt_subs.c	2012/06/26 08:05:58	1.2.2.17
+++ mqtt/src/mqtt_subs.c	2012/07/03 12:46:00	1.4
@@ -1,3 +1,48 @@
+/*************************************************************************
+* (C) 2011 AITNET ltd - Sofia/Bulgaria - <misho@aitbg.com>
+*  by Michael Pounov <misho@openbsd-bg.org>
+*
+* $Author: misho $
+* $Id: mqtt_subs.c,v 1.4 2012/07/03 12:46:00 misho Exp $
+*
+**************************************************************************
+The ELWIX and AITNET software is distributed under the following
+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
+	by Michael Pounov <misho@elwix.org>.  All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+1. Redistributions of source code must retain the above copyright
+   notice, this list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright
+   notice, this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+3. All advertising materials mentioning features or use of this software
+   must display the following acknowledgement:
+This product includes software developed by Michael Pounov <misho@elwix.org>
+ELWIX - Embedded LightWeight unIX and its contributors.
+4. Neither the name of AITNET nor the names of its contributors
+   may be used to endorse or promote products derived from this software
+   without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY AITNET AND CONTRIBUTORS ``AS IS'' AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+SUCH DAMAGE.
+*/
 #include "global.h"
 #include "rtlm.h"
 #include "mqtt.h"
@@ -126,7 +171,7 @@ execProc(sched_task_t *task)
 	char szLine[MAXPATHLEN + BUFSIZ] = { 0 };
 
 	snprintf(szLine, sizeof szLine, "%s '%s' %hu %u", AIT_GET_STR(&args->Value), 
-			(char*) TASK_ARG(task), (u_short) TASK_VAL(task), TASK_DATLEN(task));
+			(char*) TASK_ARG(task), (u_short) TASK_VAL(task), (u_int) TASK_DATLEN(task));
 	if (TASK_ARG(task))
 		io_free(TASK_ARG(task));
 
@@ -148,6 +193,7 @@ pubRX(sched_task_t *task)
 	void *data = NULL;
 	u_short mid;
 	time_t tim;
+	struct mqtthdr *hdr;
 
 	rlen = RecvFrom(TASK_FD(task));
 	if (rlen == -1)
@@ -156,27 +202,61 @@ pubRX(sched_task_t *task)
 		Kill++;
 		return NULL;
 	}
-	siz = mqtt_readPUBLISH(args->msg, szTopic, sizeof szTopic, &mid, &data);
-	if (siz == -1)
-		goto end;
 
-	/* send to output */
-	tim = time(NULL);
-	strftime(szTime, sizeof szTime, "%Y-%m-%d %H:%M:%S", localtime(&tim));
-	fprintf(TASK_ARG(task), "\n[%s] Message ID: %04hu, Length: %u, Topic: %s\n", 
-			szTime, mid, siz, szTopic);
+	while (rlen > 0) {
+		hdr = (struct mqtthdr*) args->msg->msg_base;
 
-	if (data) {
-		fputs((const char*) data, TASK_ARG(task));
-		free(data);
-	}
+		switch (hdr->mqtt_msg.type) {
+			case MQTT_TYPE_PUBLISH:
+				siz = mqtt_readPUBLISH(args->msg, szTopic, sizeof szTopic, &mid, &data);
+				if (siz == -1)
+					goto end;
+				else {
+					siz = mqtt_pktLen(hdr);
+					rlen -= siz;
+					ioVERBOSE(4) printf("Remains %d bytes, packet %d bytes\n", 
+							rlen, siz);
+				}
 
-	fprintf(TASK_ARG(task), "\n");
-	fflush(TASK_ARG(task));
+				/* send to output */
+				tim = time(NULL);
+				strftime(szTime, sizeof szTime, "%Y-%m-%d %H:%M:%S", localtime(&tim));
+				fprintf(TASK_ARG(task), "\n[%s] Message ID: %04hu, QoS: %hhu, "
+						"Length: %u, Topic: %s\n", szTime, mid, hdr->mqtt_msg.qos, 
+						siz, szTopic);
 
-	/* if exists exec script */
-	if (!AIT_ISEMPTY(&args->Value))
-		schedEvent(root, execProc, io_strdup(szTopic), mid, data, siz);
+				if (data) {
+					fputs((const char*) data, TASK_ARG(task));
+					free(data);
+				}
+
+				fprintf(TASK_ARG(task), "\n");
+				fflush(TASK_ARG(task));
+
+				/* if exists exec script */
+				if (!AIT_ISEMPTY(&args->Value))
+					schedEvent(root, execProc, io_strdup(szTopic), mid, data, siz);
+
+				memmove(args->msg->msg_base, args->msg->msg_base + siz, rlen);
+				break;
+			case MQTT_TYPE_PINGREQ:
+				siz = mqtt_msgPINGRESP(args->msg);
+				if (siz == -1)
+					goto end;
+				else
+					rlen -= siz;
+
+				/* send ping reply */
+				if (SendTo(TASK_FD(task), siz) == -1)
+					goto end;
+
+				memmove(args->msg->msg_base, args->msg->msg_base + siz, rlen);
+				break;
+			default:
+				ioVERBOSE(1) printf("Unwanted message type #%d ...\n", hdr->mqtt_msg.type);
+				goto end;
+		}
+	}
 end:
 	schedReadSelf(task);
 	return NULL;