--- libaitmqtt/src/srvside.c	2012/01/27 08:28:09	1.1
+++ libaitmqtt/src/srvside.c	2012/06/20 15:02:24	1.2
@@ -0,0 +1,123 @@
+/*************************************************************************
+* (C) 2011 AITNET ltd - Sofia/Bulgaria - <misho@aitbg.com>
+*  by Michael Pounov <misho@openbsd-bg.org>
+*
+* $Author: misho $
+* $Id: srvside.c,v 1.2 2012/06/20 15:02:24 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"
+
+
+/*
+ * mqtt_srv_Init() Init MQTT server side support
+ *
+ * @sock = Client socket from accept()
+ * @buf = Message buffer
+ * @timeout = timeout
+ * return: NULL error or !=NULL allocated server handle
+ */
+mqtt_srv_t *
+mqtt_srv_Init(int sock, mqtt_msg_t * __restrict buf, u_short timeout)
+{
+	mqtt_srv_t *srv = NULL;
+
+	if (!buf || !buf->msg_base || !buf->msg_len)
+		return srv;
+
+	srv = malloc(sizeof(mqtt_srv_t));
+	if (!srv) {
+		LOGERR;
+		return srv;
+	} else
+		memset(srv, 0, sizeof(mqtt_srv_t));
+
+	srv->sock = sock;
+	srv->buf = buf;
+	srv->timeout = timeout;
+
+	return srv;
+}
+
+/*
+ * mqtt_srv_Fini() Finish MQTT server side support
+ *
+ * @psrv = Server handle
+ * return: none
+ */
+void
+mqtt_srv_Fini(mqtt_srv_t ** __restrict psrv)
+{
+	if (psrv && *psrv) {
+		free(*psrv);
+		*psrv = NULL;
+	}
+}
+
+/*
+ * mqtt_srv_Dispatch() MQTT server dispatcher
+ *
+ * @srv = Server handle
+ * @rlen = Ready bytes to parse
+ * @arg = Argument pass to command callback
+ * return: -1 error or 0 ok
+ */
+int
+mqtt_srv_Dispatch(mqtt_srv_t * __restrict srv, int rlen, void *arg)
+{
+	struct mqtthdr *hdr;
+
+	if (!srv || !srv->buf->msg_base || !srv->buf->msg_len)
+		return -1;
+	if (srv->buf->msg_len < sizeof(struct mqtthdr) || rlen < sizeof(struct mqtthdr)) {
+		mqtt_SetErr(EINVAL, "Message is too short ...");
+		return -1;
+	}
+	hdr = (struct mqtthdr*) srv->buf->msg_base;
+	if (hdr->mqtt_msg.type > MQTT_TYPE_MAX) {
+		mqtt_SetErr(ENOSYS, "Message type %d not implemented", hdr->mqtt_msg.type);
+		return -1;
+	}
+	if (srv->cmds[hdr->mqtt_msg.type])
+		return srv->cmds[hdr->mqtt_msg.type]((void*) srv, rlen, arg);
+
+	mqtt_SetErr(ENOTSUP, "Unsupported message type %d", hdr->mqtt_msg.type);
+	return -1;
+}