--- libaitmqtt/src/srvside.c	2012/01/27 08:28:09	1.1.2.1
+++ libaitmqtt/src/srvside.c	2012/01/27 15:12:27	1.1.2.2
@@ -1,3 +1,77 @@
 #include "global.h"
 
 
+
+/*
+ * mqtt_srv_Init() Init MQTT server side support
+ *
+ * @sock = Client socket from accept()
+ * @buf = Message buffer
+ * return: NULL error or !=NULL allocated server handle
+ */
+mqtt_srv_t *
+mqtt_srv_Init(int sock, mqtt_msg_t * __restrict buf)
+{
+	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->msg_base = buf->msg_base;
+	srv->buf->msg_len = buf->msg_len;
+
+	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
+ * @arg = Argument pass to command callback
+ * return: -1 error or 0 ok
+ */
+int
+mqtt_srv_Dispatch(mqtt_srv_t * __restrict srv, 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)) {
+		mqtt_SetErr(EINVAL, "Error:: message is too small ...");
+		return -1;
+	}
+	hdr = (struct mqtthdr*) srv->buf->msg_base;
+	if (hdr->mqtt_msg.type > MQTT_TYPE_MAX) {
+		mqtt_SetErr(ENOSYS, "Error:: 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, arg);
+
+	mqtt_SetErr(ENOTSUP, "Error:: Unsupported message type %d", hdr->mqtt_msg.type);
+	return -1;
+}