Diff: server_pkts.cpp

Revision:

0:547251f42a60

diff -r 000000000000 -r 547251f42a60 server_pkts.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/server_pkts.cpp	Sat Jun 06 13:29:08 2015 +0000
@@ -0,0 +1,1000 @@
+/******************************************************************************
+*
+*   Copyright (C) 2014 Texas Instruments Incorporated
+*
+*   All rights reserved. Property of Texas Instruments Incorporated.
+*   Restricted rights to use, duplicate or disclose this code are
+*   granted through contract.
+*
+*   The program may not be used without the written permission of
+*   Texas Instruments Incorporated or against the terms and conditions
+*   stipulated in the agreement under which this program has been supplied,
+*   and under no circumstances can it be used with non-TI connectivity device.
+*
+******************************************************************************/
+
+#include "server_pkts.h"
+
+namespace mbed_mqtt {
+
+/*-----------------------------------------------------------------------------
+ * Note: Do not create additional dependency of this file on any header other
+ * than server_pkts.h. Specifically, server_pkts.[hc] in conjunction with the
+ * mqtt_common.[hc] files must be facilitated to create a stand-alone library.
+ *-----------------------------------------------------------------------------
+ */
+
+static void  *mutex = NULL;
+static void (*mutex_lockin)(void*) = NULL;
+static void (*mutex_unlock)(void*) = NULL;
+
+#define MUTEX_LOCKIN() if(mutex_lockin) mutex_lockin(mutex);
+#define MUTEX_UNLOCK() if(mutex_unlock) mutex_unlock(mutex);
+
+static bool aux_dbg_enbl = false;
+static int32_t (*debug_printf)(const char *fmt, ...) = NULL;
+
+#define USR_INFO debug_printf
+#define DBG_INFO(I, ...) if(aux_dbg_enbl) debug_printf(I, ##__VA_ARGS__)
+
+static int32_t mqp_buf_rd_utf8(const uint8_t *buf, const uint8_t *end,
+                           struct utf8_string *utf8)
+{
+        const uint8_t *ref = buf; /* Reference */
+        uint16_t len = 0;         /* UTF8 Size */
+
+        if(end - buf < 2)
+                return -1; /* No valid buffer to hold UTF8 size */
+
+        buf += buf_rd_nbo_2B(buf, &len);
+        if(end - buf < len)
+                return -1; /* No valid buffer to hold UTF8 name */
+
+        utf8->length = len;
+        utf8->buffer = len? (char*)buf : NULL;
+
+        return buf + len - ref;
+}
+
+static struct mqtt_server_msg_cbs usr_obj, *usr_cbs = NULL;
+static const struct device_net_services *net_ops = NULL;
+
+#ifndef CFG_SR_MQTT_CTXS
+#define MAX_NWCONN 6
+#else
+#define MAX_NWCONN CFG_SR_MQTT_CTXS
+#endif
+
+static struct client_ctx contexts[MAX_NWCONN];
+
+static struct client_ctx *used_ctxs = NULL;
+static struct client_ctx *free_ctxs = NULL;
+
+
+#define NETWORK_CLOSE_FLAG         0x00200000
+#define NW_CONN_ERROR_FLAG         0x00400000
+#define RCVD_CONN_MSG_FLAG         0x00800000
+
+#define NEED_NET_CLOSE(cl_ctx) (cl_ctx->flags & NETWORK_CLOSE_FLAG)
+
+static void cl_ctx_init(void)
+{
+        int32_t i = 0;
+        for(i = 0; i < MAX_NWCONN; i++) {
+                struct client_ctx *cl_ctx = contexts + i;
+
+                cl_ctx_reset(cl_ctx);
+
+                cl_ctx->next = free_ctxs;
+                free_ctxs = cl_ctx;
+        }
+}
+
+static void cl_ctx_free(struct client_ctx *cl_ctx)
+{
+        cl_ctx_reset(cl_ctx);
+
+        cl_ctx->next = free_ctxs;
+        free_ctxs = cl_ctx;
+
+        return;
+}
+
+static struct client_ctx *cl_ctx_alloc(void)
+{
+        struct client_ctx *cl_ctx = free_ctxs;
+        if(cl_ctx) {
+                free_ctxs = cl_ctx->next;
+                cl_ctx->next = NULL;
+        } else
+                USR_INFO("S: fatal, no free cl_ctx\n\r");
+
+        return cl_ctx;
+}
+
+static inline bool had_rcvd_conn_msg(struct client_ctx *cl_ctx)
+{
+        return (cl_ctx->flags & RCVD_CONN_MSG_FLAG);
+}
+
+static inline void set_rcvd_conn_msg(struct client_ctx *cl_ctx)
+{
+        cl_ctx->flags |= RCVD_CONN_MSG_FLAG;
+}
+
+static void used_ctxs_insert(struct client_ctx *cl_ctx)
+{
+        cl_ctx_timeout_insert(&used_ctxs, cl_ctx);
+}
+
+static void used_ctxs_remove(struct client_ctx *cl_ctx)
+{
+        cl_ctx_remove(&used_ctxs, cl_ctx);
+}
+
+static int32_t loopb_net        = -1;
+static const uint8_t LOOP_DATA[] = {0x00, 0x01};
+#define LOOP_DLEN sizeof(LOOP_DATA)
+static uint16_t  loopback_port   = 0;
+static bool pending_trigs   = false;
+
+static int32_t loopb_trigger(void)
+{
+        uint8_t ip_addr[] = {127,0,0,1};
+        int32_t rv = 0;
+        
+        if((-1 != loopb_net) && (false == pending_trigs)) {
+                rv = net_ops->send_dest(loopb_net, LOOP_DATA, LOOP_DLEN,
+                                        loopback_port, ip_addr, 4);
+                if(0 == rv)
+                        pending_trigs = true;
+        }
+
+        return rv;
+}
+
+static void do_net_close_rx(struct client_ctx *cl_ctx, bool due2err)
+{
+        DBG_INFO("S: RX closing Net %d ...\n\r", (int32_t)cl_ctx->net);
+
+        net_ops->close(cl_ctx->net);
+        cl_ctx->net = -1;
+
+        if(cl_ctx->usr)
+                usr_cbs->on_cl_net_close(cl_ctx->usr, due2err);
+
+        used_ctxs_remove(cl_ctx);
+        cl_ctx_free(cl_ctx);
+}
+
+static void do_net_close_tx(struct client_ctx *cl_ctx, bool due2err)
+{
+        if(due2err)
+                cl_ctx->flags |= NW_CONN_ERROR_FLAG;
+
+        cl_ctx->flags |= NETWORK_CLOSE_FLAG;
+
+        loopb_trigger();
+}
+
+static int32_t cl_ctx_send(struct client_ctx *cl_ctx, uint8_t *buf, uint32_t len)
+{
+        int32_t rv = net_ops->send(cl_ctx->net, buf, len, NULL);
+        if(rv <= 0) {
+                do_net_close_tx(cl_ctx, true);
+                rv = MQP_ERR_NETWORK;
+        }
+
+        USR_INFO("S: FH-B1 0x%02x, len %u to net %d: %s\n\r",
+                 *buf, len, cl_ctx->net, rv? "Sent" : "Fail");
+        return rv;
+}
+
+static
+int32_t vh_msg_send(struct client_ctx *cl_ctx, uint8_t msg_type, enum mqtt_qos qos,
+                bool has_vh, uint16_t vh_data)
+{
+        uint8_t  buf[4];
+        uint32_t len = 2;
+
+        if(false == had_rcvd_conn_msg(cl_ctx))
+                return MQP_ERR_NOTCONN;
+
+        buf[0] = MAKE_FH_BYTE1(msg_type, MAKE_FH_FLAGS(false, qos, false));
+        buf[1] = has_vh ? 2 : 0;
+
+        if(has_vh)
+                len += buf_wr_nbo_2B(buf + 2, vh_data);
+
+        return cl_ctx_send(cl_ctx, buf, len);
+}
+
+static
+int32_t _mqtt_vh_msg_send(void *ctx_cl, uint8_t msg_type, enum mqtt_qos qos, bool has_vh,
+                      uint16_t vh_data)
+{
+        struct client_ctx *cl_ctx = (struct client_ctx*) ctx_cl;
+
+        return cl_ctx? vh_msg_send((client_ctx*)ctx_cl, msg_type, qos,
+                                   has_vh, vh_data) : -1;
+}
+
+int32_t  mqtt_vh_msg_send(void *ctx_cl, uint8_t msg_type, enum mqtt_qos qos, bool has_vh,
+                      uint16_t vh_data)
+{
+        return _mqtt_vh_msg_send(ctx_cl, msg_type, qos, has_vh, vh_data);
+}
+
+int32_t mqtt_vh_msg_send_locked(void *ctx_cl, uint8_t msg_type, enum mqtt_qos qos,
+                            bool has_vh, uint16_t vh_data)
+{
+        int32_t rv;
+
+        MUTEX_LOCKIN();
+        rv = _mqtt_vh_msg_send(ctx_cl, msg_type, qos, has_vh, vh_data);
+        MUTEX_UNLOCK();
+
+        return rv;
+}
+
+int32_t mqtt_connack_send(void *ctx_cl, uint8_t *vh_buf)
+{
+        struct client_ctx *cl_ctx = (struct client_ctx *) ctx_cl;
+
+        int32_t rv = vh_msg_send(cl_ctx, MQTT_CONNACK, MQTT_QOS0,
+                             true, (vh_buf[0] << 8) | vh_buf[1]);
+
+        if((rv > 0) && (0x00 != vh_buf[1]))
+                do_net_close_tx(cl_ctx, true);
+
+        return rv;
+}
+
+static
+int32_t _mqtt_server_pub_dispatch(void *ctx_cl, struct mqtt_packet *mqp, bool dup)
+{
+        int32_t rv = 0;
+        uint8_t *buf = MQP_FHEADER_BUF(mqp);
+
+        if(dup)
+                *buf |= DUP_FLAG_VAL(true);
+
+        rv = cl_ctx_send((struct client_ctx*)ctx_cl, buf, MQP_CONTENT_LEN(mqp));
+
+        *buf &= ~DUP_FLAG_VAL(true);
+
+        return rv;
+}
+
+int32_t mqtt_server_pub_dispatch(void *ctx_cl, struct mqtt_packet *mqp, bool dup)
+{
+        return _mqtt_server_pub_dispatch(ctx_cl, mqp, dup);
+}
+
+int32_t 
+mqtt_server_pub_dispatch_locked(void *ctx_cl, struct mqtt_packet *mqp, bool dup)
+{
+        int32_t rv;
+
+        MUTEX_LOCKIN();
+        rv = _mqtt_server_pub_dispatch(ctx_cl, mqp, dup);
+        MUTEX_UNLOCK();
+
+        return rv;
+}
+
+#define MQP_MAX_TOPICS      16
+#define MQP_SUBACK_PAY_OFS (MAX_FH_LEN + 2)
+
+static int32_t sub_ack_send(struct client_ctx *cl_ctx, uint8_t *buf, uint8_t pay_ofs,
+                        uint32_t pay_len, uint16_t msg_id)
+{
+        uint8_t *ref = buf += MAX_FH_LEN;
+
+        if(MQP_SUBACK_PAY_OFS != pay_ofs)
+                return MQP_ERR_PKT_LEN;
+
+        buf += buf_wr_nbo_2B(buf, msg_id);
+        ref -= mqp_buf_tail_wr_remlen(ref - MAX_REMLEN_BYTES, 
+                                      pay_len + buf - ref);
+
+        ref -= 1;
+        *ref = MAKE_FH_BYTE1(MQTT_SUBACK, 
+                             MAKE_FH_FLAGS(false, MQTT_QOS0, false));
+
+        return cl_ctx_send(cl_ctx, ref, pay_len + buf - ref);
+}
+
+static inline int32_t unsub_ack_send(struct client_ctx *cl_ctx, uint16_t msg_id)
+{
+        return vh_msg_send(cl_ctx, MQTT_UNSUBACK, MQTT_QOS0, true, msg_id);
+}
+
+/*----------------------------------------------------------------------------
+ * Receive Routines
+ *----------------------------------------------------------------------------
+ */
+
+/* Candidate to be moved to mqtt_common.c file */
+static bool mqp_proc_vh_msg_id_rx(struct mqtt_packet *mqp_raw)
+{
+        uint8_t *buf = MQP_VHEADER_BUF(mqp_raw);
+
+        if(mqp_raw->pl_len < 2)
+                return false;    /* Bytes for MSG ID not available */
+
+        buf += buf_rd_nbo_2B(buf, &mqp_raw->msg_id);
+        mqp_raw->vh_len += 2;
+        mqp_raw->pl_len -= 2;
+
+        return true;
+}
+
+#define BRK_IF_RD_ERR_UTF8(buf, end, utf8)      \
+        if(rd_buf_utf8(buf, end, utf8) < 0)     \
+                break;
+
+static int32_t buf_rd_utf8_qos(uint8_t *buf, uint8_t *end, struct utf8_strqos *utf8_qos)
+{
+        struct utf8_string utf8;
+        uint8_t *ref = buf;
+
+        buf += mqp_buf_rd_utf8(buf, end, &utf8);
+
+        /* Assess that UTF8 has been read and QOS can be read */
+        if((buf > ref) && (end > buf)) {
+                utf8_qos->buffer = utf8.buffer;
+                utf8_qos->length = utf8.length;
+                utf8_qos->qosreq = (enum mqtt_qos)*buf++;
+
+                return buf - ref;
+        }
+
+        return -1;
+}
+
+static bool _proc_sub_msg_rx(struct mqtt_packet *mqp_raw,
+                             struct utf8_strqos *qos_topics, uint32_t *n_topics)
+{
+        uint8_t *buf, *end;
+        uint32_t i = 0;
+
+        if(false == mqp_proc_vh_msg_id_rx(mqp_raw))
+                return false;   /* Problem in contents received from client */
+
+        buf = MQP_PAYLOAD_BUF(mqp_raw);
+        end = buf + mqp_raw->pl_len;
+
+        for(i = 0; (i < *n_topics) && (buf < end); i++) {
+                struct utf8_strqos *qos_top = qos_topics + i;
+                int32_t len = buf_rd_utf8_qos(buf, end, qos_top);
+                if(len < 0)
+                        break; /* Failed to read Topic */
+
+                buf += len;
+        }
+
+        *n_topics = i;
+
+        return ((0 == i) || (buf != end))? false : true;
+}
+
+static
+bool proc_sub_msg_rx(struct client_ctx *cl_ctx, struct mqtt_packet *mqp_raw)
+{
+        uint32_t n_topics = MQP_MAX_TOPICS;
+        uint16_t msg_id;
+
+        struct utf8_strqos qos_topics[MQP_MAX_TOPICS];
+        uint8_t ack[MQP_MAX_TOPICS  +  MQP_SUBACK_PAY_OFS];
+
+        if(false == _proc_sub_msg_rx(mqp_raw, qos_topics, &n_topics))
+                return false;
+
+        msg_id = mqp_raw->msg_id;
+
+        /* All topics have been now put in array, pass-on info to upper layer */
+        if(usr_cbs->sub_msg_rx(cl_ctx->usr, qos_topics, n_topics,
+                               msg_id, ack + MQP_SUBACK_PAY_OFS)) {
+
+                sub_ack_send(cl_ctx, ack, MQP_SUBACK_PAY_OFS, n_topics, msg_id);
+
+                return true;
+        }
+
+        return false;
+}
+
+
+static bool _proc_unsub_msg_rx(struct mqtt_packet *mqp_raw,
+                               struct utf8_string *topics, uint32_t *n_topics)
+{
+        uint8_t *buf, *end;
+        uint32_t i = 0;
+        
+        if(false == mqp_proc_vh_msg_id_rx(mqp_raw))
+                return false;   /* Problem in contents received from client */
+
+        buf = MQP_PAYLOAD_BUF(mqp_raw);
+        end = buf + mqp_raw->pl_len;
+
+        for(i = 0; (i < *n_topics) && (buf < end); i++) {
+                struct utf8_string *topic = topics + i;
+                int32_t len = mqp_buf_rd_utf8(buf, end, topic);
+                if(len < 0)
+                        break; /* Failed to read Topic */
+                
+                buf += len;
+        }
+
+        *n_topics = i;
+
+        return ((0 == i) || (buf != end))? false : true;
+}
+
+static
+bool proc_unsub_msg_rx(struct client_ctx *cl_ctx, struct mqtt_packet *mqp_raw)
+{
+        uint32_t n_topics = MQP_MAX_TOPICS;
+        uint16_t msg_id;
+
+        struct utf8_string topics[MQP_MAX_TOPICS];
+
+        if(false == _proc_unsub_msg_rx(mqp_raw, topics, &n_topics))
+                return false;
+
+        msg_id = mqp_raw->msg_id;
+
+        /* All topics have been now put in array, pass-on info to upper layer */
+        if(usr_cbs->un_sub_msg(cl_ctx->usr, topics, n_topics, msg_id)) {
+                unsub_ack_send(cl_ctx, msg_id);
+                return true;
+        }
+
+        return false;
+}
+
+static bool proc_pingreq_rx(struct client_ctx *cl_ctx)
+{
+        vh_msg_send(cl_ctx, MQTT_PINGRSP, MQTT_QOS0, false, 0x00);
+        return true;
+}
+
+static bool proc_disconn_rx(struct client_ctx *cl_ctx)
+{
+        do_net_close_rx(cl_ctx, false);
+        return true;
+}
+
+static
+bool proc_pub_msg_rx(struct client_ctx  *cl_ctx, struct mqtt_packet *mqp_raw)
+{
+        bool rv = mqp_proc_pub_rx(mqp_raw);
+        uint8_t B = mqp_raw->fh_byte1;
+        enum mqtt_qos qos = ENUM_QOS(B);
+        struct utf8_string topic;
+        uint16_t msg_id = 0;
+
+        if(false == rv)
+                return rv; /* Didn't get a good PUB Packet */
+
+        msg_id = mqp_raw->msg_id;
+
+        topic.buffer = (char*)MQP_PUB_TOP_BUF(mqp_raw);
+        topic.length =      MQP_PUB_TOP_LEN(mqp_raw);
+
+        rv = usr_cbs->pub_msg_rx(cl_ctx->usr, &topic, 
+                                 MQP_PUB_PAY_BUF(mqp_raw),
+                                 MQP_PUB_PAY_LEN(mqp_raw),
+                                 msg_id, BOOL_DUP(B), qos, 
+                                 BOOL_RETAIN(B));
+        if(false == rv)
+                return rv;
+
+        if(MQTT_QOS1 == qos)
+                vh_msg_send(cl_ctx, MQTT_PUBACK, MQTT_QOS0, true, msg_id);
+
+        if(MQTT_QOS2 == qos)
+                vh_msg_send(cl_ctx, MQTT_PUBREC, MQTT_QOS0, true, msg_id);
+
+        return rv;
+}
+
+static 
+bool proc_ack_msg_rx(struct client_ctx *cl_ctx, struct mqtt_packet *mqp_raw)
+{
+        if((false == mqp_proc_msg_id_ack_rx(mqp_raw, false)) ||
+           (false == usr_cbs->ack_notify(cl_ctx->usr,
+                                         mqp_raw->msg_type,
+                                         mqp_raw->msg_id)))
+                return false;
+
+        return true;
+}
+
+#define IO_MON_NO_TIMEOUT (0xFFFFFFFF) // TBD
+//#define KA_TIMEOUT_NONE   (0xFFFFFFFF)
+
+static void rx_timeout_update(struct client_ctx *cl_ctx)
+{
+        if(false == had_rcvd_conn_msg(cl_ctx))
+                return;
+
+        cl_ctx_timeout_update(cl_ctx, net_ops->time());
+
+        used_ctxs_remove(cl_ctx);
+        used_ctxs_insert(cl_ctx);
+
+        return;
+}
+
+static bool proc_protocol_info(struct utf8_string *utf8, uint8_t ver)
+{
+        const char *buf = utf8->buffer;
+
+        /* Check for protocol version 3.1.1 */
+        if((4 == utf8->length)  &&
+           (buf[0] == 'M')      &&
+           (buf[1] == 'Q')      &&
+           (buf[2] == 'T')      &&
+           (buf[3] == 'T')      &&
+           (0x04 == ver))
+                return true;
+
+        /* Check for protocol version 3.1   */
+        if((6 == utf8->length)  &&
+           (buf[0] == 'M')      &&
+           (buf[1] == 'Q')      &&
+           (buf[2] == 'I')      &&
+           (buf[3] == 's')      &&
+           (buf[4] == 'd')      &&
+           (buf[5] == 'p')      &&
+           (0x03 == ver))
+                return true;
+
+        return false;
+}
+
+static uint16_t proc_connect_vh_rx(struct mqtt_packet  *mqp_raw,
+                              uint8_t *conn_flags, uint16_t *ka_secs)
+{
+        struct utf8_string utf8;
+        uint8_t *buf = MQP_PAYLOAD_BUF(mqp_raw);
+        uint8_t *end = buf + mqp_raw->pl_len;
+        uint8_t *ref = buf;
+
+        buf += mqp_buf_rd_utf8(buf, end, &utf8);
+        if(end - buf < 1)
+                return CONNACK_RC_BAD_PROTOV; /* No proto ver */
+
+        if(false == proc_protocol_info(&utf8, *buf++))
+                return CONNACK_RC_BAD_PROTOV;
+
+        *conn_flags = *buf++;
+
+        if(end - buf < 2)
+                return 0xFF;        /* Bad packet composition */
+
+        *ka_secs  = (buf[0] << 8) | buf[1];
+        buf      += 2;
+        *ka_secs += *ka_secs >> 1;
+
+        mqp_raw->vh_len  = buf - ref;
+        mqp_raw->pl_len -= buf - ref;
+
+        return 0;
+}
+
+#define RET_IF_RD_CONN_ERROR(buf, end, utf8)                   \
+        {                                                      \
+                int32_t len = mqp_buf_rd_utf8(buf, end, utf8);     \
+                if(len < 0)                                    \
+                        return 0x00FF;                         \
+                                                               \
+                buf += len;                                    \
+        }
+
+uint16_t proc_connect_pl_rx(const uint8_t *buf, const uint8_t *end, uint8_t conn_flags,
+                       struct utf8_string *free_utf8s,
+                       struct utf8_string **used_refs)
+{
+        struct utf8_string *utf8;
+
+        utf8 = used_refs[0] = free_utf8s + 0;
+        RET_IF_RD_CONN_ERROR(buf, end, utf8);
+
+        if(conn_flags & WILL_CONFIG_VAL) {
+                utf8 = used_refs[1] = free_utf8s + 1;
+                RET_IF_RD_CONN_ERROR(buf, end, utf8);
+
+                utf8 = used_refs[2] = free_utf8s + 2;
+                RET_IF_RD_CONN_ERROR(buf, end, utf8);
+        }
+
+        if((0 == (conn_flags & USER_NAME_OPVAL))  &&
+           (0 != (conn_flags & PASS_WORD_OPVAL)))
+                return 0x00FF;  /* Bad combination */
+
+        if(conn_flags & USER_NAME_OPVAL) {
+                utf8 = used_refs[3] = free_utf8s + 3;
+                RET_IF_RD_CONN_ERROR(buf, end, utf8);
+        }
+
+        if(conn_flags & PASS_WORD_OPVAL) {
+                utf8 = used_refs[4] = free_utf8s + 4;
+                RET_IF_RD_CONN_ERROR(buf, end, utf8);
+        }
+
+        return 0;
+}
+
+static
+bool proc_connect_rx(struct client_ctx *cl_ctx, struct mqtt_packet *mqp_raw)
+{
+        struct utf8_string *used_refs[5] = {NULL, NULL, NULL, NULL, NULL};
+        struct utf8_string free_utf8s[5];
+        uint8_t conn_flags, *buf, *end;
+        bool clean_session = false;
+        uint16_t ack_vh16 = 0; /* Variable Header of CONNACK (response) Message */
+
+        set_rcvd_conn_msg(cl_ctx);
+
+        ack_vh16 = proc_connect_vh_rx(mqp_raw, &conn_flags, &cl_ctx->ka_secs);
+        if(ack_vh16) 
+                goto proc_connect_rx_exit1;
+
+        buf = MQP_PAYLOAD_BUF(mqp_raw);
+        end = buf + mqp_raw->pl_len;
+        ack_vh16 = proc_connect_pl_rx(buf,  end, conn_flags,
+                                      free_utf8s, used_refs);
+        if(ack_vh16)
+                goto proc_connect_rx_exit1;
+
+        clean_session = (conn_flags & CLEAN_START_VAL)? true : false;
+        ack_vh16 = (!used_refs[0]->length && !clean_session)?
+                   CONNACK_RC_CLI_REJECT : 0;   /* Validate 0 byte Client ID */
+        if(ack_vh16)
+                goto proc_connect_rx_exit1;
+
+        /* UTF8 Order: Client ID, Will Topic, Will Msg, User Name, Pass Word */
+        ack_vh16 = usr_cbs->connect_rx(cl_ctx, conn_flags, &used_refs[0],
+                                       &cl_ctx->usr);
+ proc_connect_rx_exit1:
+
+        DBG_INFO("S: CONNACK RC (16bits) is %u (%s)\n\r", ack_vh16,
+                 ack_vh16 & 0xFF? "error" : "good");
+
+        if(0xFF != (ack_vh16 & 0xFF))
+                vh_msg_send(cl_ctx, MQTT_CONNACK, MQTT_QOS0, true, ack_vh16);
+
+        if(CONNACK_RC_REQ_ACCEPT == (ack_vh16 & 0xFF)) {
+                rx_timeout_update(cl_ctx);
+                usr_cbs->on_connack_send(cl_ctx->usr, clean_session);
+        } else {
+                return false;
+        }
+
+        return true;
+}
+
+
+static void recv_hvec_load(int32_t *recv_hvec, uint32_t size, struct client_ctx *list)
+{
+        int32_t i = 0;
+
+        for(i = 0; (i < size) && (NULL != list); i++, list = list->next)
+                recv_hvec[i] = list->net;
+
+        recv_hvec[i] = -1;
+
+        return;
+}
+
+static bool process_recv(struct client_ctx *cl_ctx, struct mqtt_packet *mqp_raw)
+{
+        uint8_t msg_type = mqp_raw->msg_type;
+        bool rv = false;
+
+        USR_INFO("S: Rcvd msg Fix-Hdr (Byte1) 0x%02x from net %d\n\r",
+                 mqp_raw->fh_byte1, cl_ctx->net);
+
+        if((MQTT_CONNECT != msg_type) ^ had_rcvd_conn_msg(cl_ctx))
+                goto process_recv_exit1; /* Proto Violation */
+
+        rx_timeout_update(cl_ctx);
+
+        switch(msg_type) {
+
+        case MQTT_CONNECT:
+                rv = proc_connect_rx(cl_ctx, mqp_raw);
+                break;
+
+        case MQTT_DISCONNECT:
+                rv = proc_disconn_rx(cl_ctx);
+                break;
+
+        case MQTT_SUBSCRIBE:
+                rv = proc_sub_msg_rx(cl_ctx, mqp_raw);
+                break;
+
+        case MQTT_UNSUBSCRIBE:
+                rv = proc_unsub_msg_rx(cl_ctx, mqp_raw);
+                break;
+
+        case MQTT_PINGREQ:
+                rv = proc_pingreq_rx(cl_ctx);
+                break;
+
+        case MQTT_PUBLISH:
+                rv = proc_pub_msg_rx(cl_ctx, mqp_raw);
+                break;
+
+        case MQTT_PUBACK:
+        case MQTT_PUBREC:
+        case MQTT_PUBREL:
+        case MQTT_PUBCOMP:
+                rv = proc_ack_msg_rx(cl_ctx, mqp_raw);
+                break;
+
+        default:
+                break;
+        }
+
+ process_recv_exit1:
+        DBG_INFO("S: Processing of MSG ID 0x%02x: %s\n\r",
+                 mqp_raw->msg_id, rv? "Good" : "Fail");
+
+        return rv;
+}
+
+
+/* Terminate net connections which haven't received PKTs beyond expected time.
+   Caller must ensure atomic enviroment for execution of this routine.
+*/
+static void ka_sequence(uint32_t *secs2wait)
+{
+        struct client_ctx *cl_ctx = used_ctxs; /* Sorted for timeout (ascend) */
+        uint32_t now_secs = net_ops->time();
+
+        while(NULL != cl_ctx) {
+                struct client_ctx *next = cl_ctx->next;
+                if(NEED_NET_CLOSE(cl_ctx) || !(cl_ctx->timeout > now_secs)) {
+                        bool due2err = false;
+                        if(cl_ctx->flags & NW_CONN_ERROR_FLAG)
+                                due2err = true;
+
+                        cl_ctx->flags &= ~(NW_CONN_ERROR_FLAG |
+                                           NETWORK_CLOSE_FLAG);
+
+                        /* Close network: Timeout or TX err */
+                        do_net_close_rx(cl_ctx, due2err);
+                }
+
+                cl_ctx = next;
+        }
+
+        cl_ctx = used_ctxs;
+        if(((NULL != cl_ctx) && (KA_TIMEOUT_NONE == cl_ctx->timeout)) ||
+           ((NULL == cl_ctx)))
+                *secs2wait = IO_MON_NO_TIMEOUT;
+        else
+                *secs2wait = cl_ctx->timeout - now_secs;
+
+        return;
+}
+
+/* Put a new functiona name such as mk_new_ctx() or setup_ctx() and
+   processing to restrict limit number of connections.
+
+   Return value as well.
+*/
+static bool accept_ctx(int32_t net, uint32_t wait_secs)
+{
+        struct client_ctx *cl_ctx = cl_ctx_alloc();
+        if(NULL == cl_ctx)
+                return false;
+
+        cl_ctx->net = net_ops->accept(net, cl_ctx->remote_ip,
+                                      &cl_ctx->ip_length);
+        if(-1 == cl_ctx->net) {
+                cl_ctx_free(cl_ctx);
+                USR_INFO("S: failed to accept new NW connection\n\r");
+                return false;
+        }
+
+        DBG_INFO("Accepted new connection (fd) %d\n\r", (int32_t)cl_ctx->net);
+
+        /* Timeout to receive MQTT_CONNECT */
+        cl_ctx->timeout = wait_secs + net_ops->time(); 
+
+        used_ctxs_insert(cl_ctx);
+        return true;
+}
+
+static struct client_ctx *net_cl_ctx_find(int32_t net)
+{
+        struct client_ctx *cl_ctx = used_ctxs;
+        while(cl_ctx) {
+                if(net == cl_ctx->net)
+                        break;
+
+                cl_ctx = cl_ctx->next;
+        }
+
+        if(NULL == cl_ctx)
+                USR_INFO("Did not find ctx for net %d\n\r", net); 
+
+        return cl_ctx;
+}
+
+static int32_t recv_hvec[MAX_NWCONN + 1 + 1 + 1]; /* LISTEN + LOOPBACK + VEC END */
+static int32_t send_hvec  = -1;
+static int32_t rsvd_hvec  = -1;
+static int32_t listen_net = -1;
+
+static struct mqtt_packet rx_mqp;
+static uint8_t rxb[MQP_SERVER_RX_LEN];
+static uint16_t listener_port = 0;
+
+static inline
+int32_t net_recv(int32_t net, struct mqtt_packet *mqp, uint32_t wait_secs, bool *timed_out)
+{
+        int32_t rv = mqp_recv(net, net_ops, mqp, wait_secs, timed_out, NULL);
+        if(rv <= 0)
+                rv = MQP_ERR_NETWORK;
+
+        return rv;
+}
+
+static int32_t proc_loopback_recv(int32_t net)
+{
+        uint8_t buf[LOOP_DLEN];
+
+        /* Thanks for waking-up thread and do nothing in this routine */
+        int32_t rv = net_ops->recv_from(net, buf, LOOP_DLEN, NULL, NULL, 0);
+        pending_trigs = false;
+
+        if(rv <= 0) {
+                net_ops->close(net);
+                return MQP_ERR_LIBQUIT;
+        }
+
+        return rv;
+}
+
+static void proc_net_data_recv(int32_t net)
+{
+        struct client_ctx *cl_ctx = net_cl_ctx_find(net);
+        bool dummy;
+        int32_t rv;
+
+        mqp_reset(&rx_mqp); /* Start w/ a clean buffer */
+
+        rv = net_recv(net, &rx_mqp, 0, &dummy);
+        if(rv > 0)
+                /* Working Principle: Only RX processing errors should be
+                   reported as 'false'. Status of TX as a follow-up to RX
+                   messages need not be reported by the xyz_rx() routines.
+                   Error observed in TX is either dealt in next iteration
+                   of RX loop.
+                */
+                if(false == process_recv(cl_ctx, &rx_mqp))
+                        rv = MQP_ERR_CONTENT;
+
+        if(rv < 0)
+                do_net_close_rx(cl_ctx, rv);
+}
+
+static bool accept_and_recv_locked(int32_t *recv_hnds, int32_t n_hnds, uint32_t wait_secs)
+{
+        bool rv = true;
+        int32_t idx = 0;
+
+        MUTEX_LOCKIN();
+
+        for(idx = 0; (idx < n_hnds) && (rv == true); idx++) {
+                int32_t net = recv_hvec[idx];
+                if(net == listen_net) {
+                        rv = accept_ctx(listen_net, wait_secs);
+                } else if(loopback_port && (net == loopb_net)) {
+                        if(proc_loopback_recv(loopb_net) < 0)
+                                rv = false;
+                } else {
+                        proc_net_data_recv(net);
+                }
+        }
+
+        MUTEX_UNLOCK();
+
+        return rv;
+}
+
+int32_t mqtt_server_run(uint32_t wait_secs)   // TBD break into two functions
+{
+        uint32_t secs2wait = 0;
+        int32_t n_hnds = 0;
+
+        USR_INFO("S: MQTT Server Run invoked ....\n\r");
+
+        if(NULL == net_ops)
+                return MQP_ERR_NET_OPS;
+
+        if(loopback_port) {
+                loopb_net  = net_ops->open(DEV_NETCONN_OPT_UDP, NULL, 
+                                           loopback_port, NULL);
+                if(-1 == loopb_net)
+                        return MQP_ERR_LIBQUIT;
+        }
+
+        listen_net = net_ops->listen(0, listener_port, NULL);
+        if(-1 == listen_net)
+                return MQP_ERR_LIBQUIT;
+
+        do {
+                int32_t *r_hvec = recv_hvec + 0;
+
+                *r_hvec++ = listen_net;
+                if(loopback_port)
+                        *r_hvec++ = loopb_net;
+
+                /* MQTT Timeouts: close expired conns; get time to next expiry */
+                ka_sequence(&secs2wait); 
+
+                /* Prepare array of net handles. Must've atleast listen handle */
+                //                recv_hvec_load(&recv_hvec[2], MAX_NWCONN + 1,  used_ctxs);
+                recv_hvec_load(r_hvec, MAX_NWCONN + 1,  used_ctxs);
+
+                n_hnds = net_ops->io_mon(recv_hvec, &send_hvec,
+                                         &rsvd_hvec, secs2wait);
+                if(n_hnds < 0)
+                        return MQP_ERR_LIBQUIT;
+
+                if(false == accept_and_recv_locked(recv_hvec, n_hnds, wait_secs))
+                        return MQP_ERR_LIBQUIT;
+
+        } while(1);
+}
+
+int32_t mqtt_server_register_net_svc(const struct device_net_services *net)
+{
+        if(net && net->io_mon && net->close && net->send &&
+           net->recv && net->time && net->listen) {
+                net_ops = net;
+                return 0;
+        }
+
+        return -1;
+}
+
+int32_t mqtt_server_lib_init(const struct mqtt_server_lib_cfg *lib_cfg,
+                         const struct mqtt_server_msg_cbs *msg_cbs)
+{
+        cl_ctx_init();
+
+        if((NULL == lib_cfg)                ||
+           (0    == lib_cfg->listener_port) ||
+           (NULL == lib_cfg->debug_printf))
+                return -1;
+
+        debug_printf = lib_cfg->debug_printf; /* Facilitate debug */
+
+        loopback_port = lib_cfg->loopback_port;
+        listener_port = lib_cfg->listener_port;
+
+        mutex         = lib_cfg->mutex;
+        mutex_lockin  = lib_cfg->mutex_lockin;
+        mutex_unlock  = lib_cfg->mutex_unlock;
+
+        aux_dbg_enbl  = lib_cfg->aux_debug_en;
+        debug_printf  = lib_cfg->debug_printf;
+
+        usr_cbs = &usr_obj;
+
+        memcpy(usr_cbs, msg_cbs, sizeof(struct mqtt_server_msg_cbs));
+
+        mqp_buffer_attach(&rx_mqp, rxb, MQP_SERVER_RX_LEN, 0);
+
+        return 0;
+}
+
+}//namespace mbed_mqtt