authorNathael Pajani <nathael.pajani@ed3l.fr>
Sat, 16 Mar 2019 17:51:40 +0000 (18:51 +0100)
committerNathael Pajani <nathael.pajani@ed3l.fr>
Sat, 16 Mar 2019 17:51:40 +0000 (18:51 +0100)
include/lib/protocols/mqtt.h [new file with mode: 0644]
lib/protocols/mqtt.c [new file with mode: 0644]

diff --git a/include/lib/protocols/mqtt.h b/include/lib/protocols/mqtt.h
new file mode 100644 (file)
index 0000000..dc995a1
--- /dev/null
@@ -0,0 +1,335 @@
+/****************************************************************************
+ *   lib/protocols/mqtt/mqtt.h
+ *
+ * Copyright 2019 Nathael Pajani <nathael.pajani@ed3l.fr>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *************************************************************************** */
+
+/* 
+ * MQTT client implementation for embeded microcontrollers.
+ *
+ * For protocol defiition, refer to 
+ * http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/os/mqtt-v3.1.1-os.html
+ *
+ * This code is the implementation of the MQTT protocol part of the communication.
+ * The MQTT protocol requires a lossless, ordered transport protocol layer with an address mechanism
+ * which is not part of the code found in mqtt.c and mqtt.h, making it possible to use any underlying
+ * communication layer which fulfills the above requirements.
+ *
+ * This code only handles the packet encoding and decoding according to the MQTT specification and
+ * provides usage information in order to respect the protocol flow, but does not provide the communication
+ * parts of the protocol, which should be application specific.
+ * This lets the application designer choose between any of the possible MQTT flow available, from single
+ * "in-flight" message window to a single server to multiple servers and complex message queues.
+ * 
+ */
+
+#ifndef MQTT_H
+#define MQTT_H
+
+#include "core/system.h"
+
+/***************************************************************************** */
+
+
+/* Protocol identifiers for MQTT v3.1.1. */
+#define MQTT_PROTOCOL_LEVEL 0x04
+#define MQTT_PROTOCOL_NAME   "MQTT"
+
+
+/* MQTT control packet types. */
+enum MQTT_message_types {
+       MQTT_CONTROL_CONNECT = 1,
+       MQTT_CONTROL_CONNACK,
+       MQTT_CONTROL_PUBLISH,
+       MQTT_CONTROL_PUBACK,
+       MQTT_CONTROL_PUBREC,
+       MQTT_CONTROL_PUBREL,
+       MQTT_CONTROL_PUBCOMP,
+       MQTT_CONTROL_SUBSCRIBE,
+       MQTT_CONTROL_SUBACK,
+       MQTT_CONTROL_UNSUBSCRIBE,
+       MQTT_CONTROL_UNSUBACK,
+       MQTT_CONTROL_PINGREQ,
+       MQTT_CONTROL_PINGRESP,
+       MQTT_CONTROL_DISCONNECT
+};
+
+/* MQTT QoS levels */
+#define MQTT_QoS_0  (0)
+#define MQTT_QoS_1  (1)
+#define MQTT_QoS_2  (2)
+
+
+struct mqtt_topic {
+       char* name;
+       uint8_t QoS; /* 0, 1 or 2 */
+};
+
+
+/***************************************************************************** */
+/* Connect and Connack packets */
+
+/* CONNECT packet flags */
+#define MQTT_CONNECT_FLAG_CLEAN_SESSION (0x01 << 1)
+#define MQTT_CONNECT_FLAG_WILL_FLAG  (0x01 << 2)
+#define MQTT_CONNECT_FLAG_WILL_QoS(x)  (((x) & 0x03) << 3)
+#define MQTT_CONNECT_FLAG_WILL_RETAIN  (0x01 << 5)
+#define MQTT_CONNECT_FLAG_PASSWORD  (0x01 << 6)
+#define MQTT_CONNECT_FLAG_USER_NAME  (0x01 << 7)
+
+struct mqtt_connect_pkt_fixed_payload {
+       uint16_t proto_name_len; /* network endian */
+       uint8_t protocol_name[4]; /* "MQTT" : use MQTT_PROTOCOL_NAME */
+       uint8_t protocol_level;  /* use MQTT_PROTOCOL_LEVEL */
+       uint8_t connect_flags;
+       uint16_t keep_alive_seconds; /* network endian */
+};
+
+#define MQTT_SESSION_RESUME 0x00
+#define MQTT_SESSION_NEW    0x01
+struct mqtt_connect_pkt {
+       uint16_t keep_alive_seconds;
+       uint8_t clean_session_flag; /* Either MQTT_SESSION_RESUME or MQTT_SESSION_NEW */
+       char* client_id;
+       struct mqtt_topic will_topic;
+       uint8_t will_retain; /* 0 or 1 */
+       uint16_t will_msg_size; /* stored in host endianness */
+       uint8_t* will_msg;
+       char* username;
+       char* password;
+};
+
+/* Create MQTT connect packet
+ * This function must be called in order to create the connect MQTT packet used to connect to the
+ * server.
+ * The client must send a connect packet whenever the server does not acknoledge a publish,
+ * subscribe, unsubscribe, or ping packet, or when the server explicitely closes the connection.
+ *
+ * Caller must provide a buffer "buf" big enougth to receive the whole packet and indicate it's size
+ * in "buf_size".
+ * Returns the used buffer size (packet size) on success.
+ * Return value is negative on error :
+ *   -EINVAL if either buf or pkt is NULL,
+ *   -EINVAL if client_id string provided in mqtt_connect_pkt struct is NULL (it's length may be 0,
+ *     but pointer cannot bu NULL).
+ *   -E2BIG if buffer is not big enough for packet.
+ */
+int mqtt_pack_connect_packet(const struct mqtt_connect_pkt* pkt, uint8_t* buf, uint32_t buf_size);
+
+
+/* Connack return codes returned in a CONNACK packet */
+enum MQTT_connack_return_codes {
+       MQTT_CONNACK_ACCEPTED = 0,
+       MQTT_CONNACK_REFUSED_PROTOCOL_VERSION = 1,
+       MQTT_CONNACK_REFUSED_IDENTIFIER_REJECTED = 2,
+       MQTT_CONNACK_REFUSED_SERVER_UNAVAILABLE = 3,
+       MQTT_CONNACK_REFUSED_BAD_USER_NAME_OR_PASSWORD = 4,
+       MQTT_CONNACK_REFUSED_NOT_AUTHORIZED = 5,
+       MQTT_CONNACK_MAX,
+};
+
+/* MQTT connack packet */
+struct mqtt_connack_reply_pkt {
+       uint8_t control; /* Paquet type : must be (MQTT_CONTROL_CONNACK << 4) */
+       uint8_t rem_len; /* Remaining length : must be 0x02 */
+       uint8_t flags;  /* Connect acknowledge flags */
+       uint8_t ret_code; /* Connect return code */
+} __attribute__ ((__packed__));
+
+/* Connect acknowledge flags bit 0 set to 1 only if connect with Clean session flag was set to 0 and
+ * the server accepts the connection and has a stored session for this client id */
+#define MQTT_CONNACK_SESSION_PRESENT  (0x01 << 0)
+
+/* Check MQTT connack packet
+ * This function may get called to check a supposed connect acknowledge packet.
+ * The function checks for conformance to the MQTT protocol and returns 0 if the packet is valid and
+ * the server accepted the connection.
+ * The function returns -EINVAL if pkt is NULL, -EPROTO in case of invalid connack packet or
+ *  -EREMOTEIO in case of connection refused by the server (ret_code is then valid in the packet).
+ */
+int mqtt_check_connack_reply_pkt(const struct mqtt_connack_reply_pkt* pkt);
+
+/***************************************************************************** */
+/* publish and puback packets */
+
+#define MQTT_PUBLISH_DUP  (0x01 << 3)
+#define MQTT_PUBLISH_QoS(x)  (((x) & 0x03) << 1)
+#define MQTT_PUBLISH_RETAIN  (0x01 << 0)
+
+/* MQTT publish paquet
+ * A publish control packet is sent from a Client to a Server or from Server to a Client to transport
+ * an Application Message. */
+struct mqtt_publish_pkt {
+       struct mqtt_topic topic;
+       uint16_t packet_id; /* Packet identifier is required for publish if QoS > 0 */
+       uint8_t dup_flag; /* Accept 1 or MQTT_PUBLISH_DUP as flag set */
+       uint8_t retain_flag;
+       uint32_t message_size;
+       uint8_t* application_message;
+};
+
+/* Create MQTT publish packet
+ * This function must be called in order to create a publish MQTT packet used to publish data on a
+ * topic (send data to the server).
+ * The application message size can be 0, in which case the application_message pointer in the
+ * mqtt_publish_pkt struct may be NULL
+ *
+ * Caller must provide a buffer "buf" big enougth to receive the whole packet and indicate it's size
+ * in "buf_size".
+ * Returns the used buffer size (packet size) on success.
+ * Return value is negative on error :
+ *   -EINVAL if either buf or pkt is NULL,
+ *   -EINVAL if client_id string provided in mqtt_connect_pkt struct is NULL (it's length may be 0,
+ *     but pointer cannot bu NULL).
+ *   -E2BIG if buffer is not big enough for packet.
+ */
+int mqtt_pack_publish_packet(const struct mqtt_publish_pkt* pkt, uint8_t* buf, uint32_t buf_size);
+
+
+/* Unpack MQTT publish packet
+ * This function must be called in order to transform a received publish MQTT packet to a
+ * mqtt_publish_pkt structure.
+ * The function also checks the validity of the packet.
+ * All returned pointers within the struct will point to parts of the provided buffer, so the buffer
+ * must not be discarded after the call.
+ * if the return value is positive, it is the topic string length.
+ */
+int mqtt_unpack_publish_packet(struct mqtt_publish_pkt* pkt, uint8_t* buf, uint32_t size);
+
+
+#define MQTT_PUBREL_FLAG  (0x01 << 1)
+
+/* MQTT publish replies packet, used for puback, pubrec, pubrel and pubcomp */
+/* control paquet type must be either (MQTT_CONTROL_PUBACK << 4) if QoS = 1 (no further reply required)
+ * or (MQTT_CONTROL_PUBREC << 4) if QoS = 2 and then a publish release must be received or sent
+ * (MQTT_CONTROL_PUBREL << 4), answered by a publish complete packet (MQTT_CONTROL_PUBCOMP << 4) */
+struct mqtt_publish_reply_pkt {
+       uint8_t control; /* Packet type */
+       uint8_t rem_len; /* Remaining length : must be 0x02 */
+       uint16_t acked_pkt_id; /* Id of packet that is being acknowledged, in network endianness */
+} __attribute__ ((__packed__));
+
+/* Build MQTT puback, pubrec, pubrel or pubcomp packet, used in the publish acknowledge one-way or
+ * two-way hand-check mechanism.
+ * The provided buf must be big enough to hold 4 bytes.
+ * Note that buf may safely be cast to or from a mqtt_publish_reply_pkt struct pointer.
+ * type is one of MQTT_CONTROL_PUBACK, MQTT_CONTROL_PUBREC, MQTT_CONTROL_PUBREL or MQTT_CONTROL_PUBCOMP.
+ * acked_pkt_id is the ID of the concerned publish packet, in host endianness.
+ * Returns the used buffer size (4 bytes) on success, or -EINVAL in case of a NULL buf pointer.
+ */
+int mqtt_pack_publish_reply_pkt(uint8_t* buf, uint16_t acked_pkt_id, uint8_t type);
+
+/* Check MQTT puback, pubrec, pubrel or pubcomp packet
+ * This function may get called to check a supposed publish acknowledge packet in either one-way or
+ * two-way hand-check mechanism.
+ * type is either MQTT_CONTROL_PUBACK, MQTT_CONTROL_PUBREC, MQTT_CONTROL_PUBREL or MQTT_CONTROL_PUBCOMP.
+ * The function checks for conformance to the MQTT protocol and returns 0 if the packet is valid.
+ * The function returns -EPROTO in case of protocol error or -EINVAL in case of a NULL pkt pointer.
+ */
+int mqtt_check_publish_reply_pkt(struct mqtt_publish_reply_pkt* pkt, uint8_t type);
+
+
+
+/***************************************************************************** */
+/* subsribe, unsubsribe, suback and unsuback packets */
+
+#define MQTT_SUBSCRIBE_FLAG  (0x01 << 1)
+#define MQTT_UNSUBSCRIBE_FLAG  MQTT_SUBSCRIBE_FLAG
+
+/* MQTT subscribe or unsubsribe packet */
+struct mqtt_sub_pkt {
+       uint16_t packet_id; /* Packet identifier */
+       uint8_t nb_topics; /* Number of topics in the topics table. Limited to 125 */
+       struct mqtt_topic* topics; /* Table of topics */
+};
+
+/* Build MQTT subscribe packet
+ * This function must be called in order to create a subscribe MQTT packet used to
+ * subscibe on a topic (or multiple topics) in order to receive data published on this
+ * or these topics.
+ * We limit the number of subscriptions sent at once to 125 in order to get a fixed size
+ * subscription acknoledgement packet.
+ */
+int mqtt_pack_subscribe_pkt(const struct mqtt_sub_pkt* pkt, uint8_t* buf, uint32_t buf_size);
+
+/* Build MQTT unsubscribe packet
+ * This function must be called in order to create an unsubscribe MQTT packet used to unsubscibe
+ * from a topic (or multiple topics) in order to stop receiving data published on this or these
+ * topics.
+ */
+int mqtt_pack_unsubscribe_pkt(const struct mqtt_sub_pkt* pkt, uint8_t* buf, uint32_t buf_size);
+
+
+
+/* MQTT subscribe or unsubscribe reply packet */
+struct mqtt_sub_reply_pkt {
+       uint8_t control; /* Packet type */
+       uint8_t rem_len; /* Remaining length : this is valid for up to 125 subscriptions sent at once ... */
+       uint16_t acked_pkt_id; /* Id of packet that is being acknowledged, in network endianness */
+} __attribute__ ((__packed__));
+
+#define MQTT_SUBSCRIBE_FAILURE   (0x80)
+
+/* Check MQTT suback packet
+ * This function may get called to check a supposed subscribe acknowledge packet.
+ * The function checks for conformance to the MQTT protocol and returns 0 if the packet is valid,
+ * regardless of the return codes received.
+ * len must be the length of the full packet received, which includes the mqtt_subscribe_reply_pkt
+ * structure and all the return codes received.
+ */
+int mqtt_check_suback_reply_pkt(const struct mqtt_sub_reply_pkt* pkt, uint8_t len);
+
+/* Check MQTT unsuback packet
+ * This function may get called to check a supposed unsubscribe acknowledge packet.
+ * The function checks for conformance to the MQTT protocol and returns 0 if the packet is valid.
+ */
+int mqtt_check_unsuback_reply_pkt(const struct mqtt_sub_reply_pkt* pkt);
+
+
+
+
+/***************************************************************************** */
+/* MQTT ping request and reply packet */
+struct mqtt_ping_pkt {
+       uint8_t control; /* Packet type : either (MQTT_CONTROL_PINGREQ << 4) or (MQTT_CONTROL_PINGRESP << 4) */
+       uint8_t rem_len; /* Remaining length : must be 0x00 */
+} __attribute__ ((__packed__));
+
+/* Build MQTT ping packet
+ * This one is a fixed packet, easy.
+ */
+int mqtt_pack_ping_pkt(uint8_t* buf);
+
+/* Check MQTT ping reply packet */
+int mqtt_check_ping_reply_pkt(const struct mqtt_ping_pkt* pkt);
+
+
+
+/***************************************************************************** */
+/* MQTT disconnect */
+/* This one may not be very useful in most applications, but it's defined by the MQTT standard and
+ * including the definition here takes no room in either code or data ...
+ */
+struct mqtt_disconnect_pkt {
+       uint8_t control; /* Packet type : (MQTT_CONTROL_DISCONNECT << 4) */
+       uint8_t rem_len; /* Remaining length : must be 0x00 */
+} __attribute__ ((__packed__));
+
+
+
+#endif /* MQTT_H */
+
diff --git a/lib/protocols/mqtt.c b/lib/protocols/mqtt.c
new file mode 100644 (file)
index 0000000..cec954c
--- /dev/null
@@ -0,0 +1,610 @@
+/****************************************************************************
+ *   lib/protocols/mqtt/mqtt.c
+ *
+ * Copyright 2019 Nathael Pajani <nathael.pajani@ed3l.fr>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *************************************************************************** */
+
+#include "core/system.h"
+#include "lib/utils.h"
+#include "lib/string.h"
+#include "lib/errno.h"
+#include "lib/protocols/mqtt.h"
+
+/* 
+ * MQTT client implementation for embeded microcontrollers.
+ *
+ * For protocol defiition, refer to 
+ * http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/os/mqtt-v3.1.1-os.html
+ *
+ * This implementation has a limitation on the subscription mechanism : the protocol allows for
+ * multiple subscriptions / unsubscriptions using a single subscribe packet, which is not supported
+ * by this code for code simplicity, memory usage limitation, and packets size limitation.
+ * All other features of the protocol should be fully supported.
+ *
+ * This code is the implementation of the MQTT protocol part of the communication.
+ * The MQTT protocol requires a lossless, ordered transport protocol layer with an address mechanism
+ * which is not part of the code found in mqtt.c and mqtt.h, making it possible to use any underlying
+ * communication layer which fulfills the above requirements.
+ *
+ * This code only handles the packet encoding and decoding according to the MQTT specification and
+ * provides usage information in order to respect the protocol flow, but does not provide the communication
+ * parts of the protocol, which should be application specific.
+ *
+ */
+
+
+
+/***************************************************************************** */
+/* MQTT helper functions */
+
+/* Encode remaining length
+ * Remaining length is encoded on 1 to four bytes. The least significant seven bits of each byte encode the
+ * data, and the most significant bit is used to indicate that there are following bytes in the representation.
+ * The size is encoded in little endianness (least significant 7 bits on first byte and so on).
+ * Refer to section 2.2.3 of mqtt-v3.1.1-os for translation of remaining_length on wire.
+ */
+static int encode_remaining_length(uint8_t* buf, uint32_t length)
+{
+       int idx = 0;
+
+       do {
+               buf[idx] = length & 0x7F;
+               length >>= 7;
+               if (length > 0) {
+                       buf[idx] |= 0x80;
+               }
+               idx++;
+       } while (length > 0);
+
+       return idx;
+}
+/* Decode remaining length */
+static int decode_remaining_length(uint8_t* buf, uint32_t* length)
+{
+       int idx = 0;
+
+       do {
+               *length += (buf[idx] & 0x7F) << (7 * idx);
+       } while ((buf[idx++] & 0x80) && (idx <= 3));
+
+       return idx;
+}
+
+
+/* Pack a string to MQTT strings format.
+ * The string must already be UTF8 encoded.
+ */
+static int mqtt_pack_str(uint8_t *buf, char* str, uint16_t len)
+{
+       uint16_t tmp = htons(len);
+       memcpy(buf, &tmp, 2);
+       memcpy((buf + 2), str, len);
+       return len + 2;
+}
+
+/* Pack fixed header */
+static int mqtt_pack_fixed_header(uint8_t *buf, uint8_t control, uint8_t flags, uint32_t length)
+{
+       *buf = (control << 4) | flags;
+       return 1 + encode_remaining_length((buf + 1), length);
+}
+
+
+
+/***************************************************************************** */
+/* Create MQTT connect packet
+ * This function must be called in order to create the connect MQTT packet used to
+ * connect to the server.
+ */
+int mqtt_pack_connect_packet(const struct mqtt_connect_pkt* pkt, uint8_t* buf, uint32_t buf_size)
+{
+       struct mqtt_connect_pkt_fixed_payload fxpl;
+       uint32_t remaining_length = 0;
+       uint32_t len = 0;
+       uint16_t client_id_len = 0, will_topic_len = 0, username_len = 0, password_len = 0;
+
+       if ((pkt == NULL) || (buf == NULL)) {
+               return -EINVAL;
+       }
+       /* Fixed payload part */
+       memset(&fxpl, 0, sizeof(struct mqtt_connect_pkt_fixed_payload));
+       fxpl.proto_name_len = htons(4);
+       memcpy(&(fxpl.protocol_name), MQTT_PROTOCOL_NAME, 4);
+       fxpl.protocol_level = MQTT_PROTOCOL_LEVEL;
+       fxpl.keep_alive_seconds = htons(pkt->keep_alive_seconds);
+       remaining_length = sizeof(struct mqtt_connect_pkt_fixed_payload);
+
+       /* Client id is (almost) mandatory */
+       if (pkt->client_id != NULL) {
+               client_id_len = strlen(pkt->client_id);
+               /* Update packet payload size */
+               remaining_length += client_id_len + 2;
+       } else {
+               return -EINVAL;
+       }
+
+       /* Set connect flags and compute message size for optionnal parts */
+       if (pkt->clean_session_flag) {
+               fxpl.connect_flags |= MQTT_CONNECT_FLAG_CLEAN_SESSION;
+       }
+       /* Optionnal will message */
+       if ((pkt->will_msg_size != 0) && (pkt->will_topic.name != NULL) && (pkt->will_msg != NULL)) {
+               fxpl.connect_flags |= MQTT_CONNECT_FLAG_WILL_FLAG;
+               fxpl.connect_flags |= MQTT_CONNECT_FLAG_WILL_QoS(pkt->will_topic.QoS);
+               if (pkt->will_retain != 0) {
+                       fxpl.connect_flags |= MQTT_CONNECT_FLAG_WILL_RETAIN;
+               }
+               will_topic_len = strlen(pkt->will_topic.name);
+               /* Update packet payload size */
+               remaining_length += will_topic_len + 2;
+               remaining_length += pkt->will_msg_size + 2;
+       }
+       /* Optionnal username and password */
+       if (pkt->username != NULL) {
+               username_len = strlen(pkt->username);
+               fxpl.connect_flags |= MQTT_CONNECT_FLAG_USER_NAME;
+               /* Update packet payload size */
+               remaining_length += username_len + 2;
+               if (pkt->password != NULL) {
+                       password_len = strlen(pkt->password);
+                       fxpl.connect_flags |= MQTT_CONNECT_FLAG_PASSWORD;
+                       /* Update packet payload size */
+                       remaining_length += password_len + 2;
+               }
+       }
+
+       /* Build MQTT Connect packet */
+       /* Fixed header */
+       len = mqtt_pack_fixed_header(buf, MQTT_CONTROL_CONNECT, 0, remaining_length);
+       if (remaining_length + len > buf_size) {
+               return -E2BIG;
+       }
+       /* Fixed payload */
+       memcpy((buf + len), &fxpl, sizeof(struct mqtt_connect_pkt_fixed_payload));
+       len += sizeof(struct mqtt_connect_pkt_fixed_payload);
+
+       /* Client ID is mandatory, even if length of ID string is 0 */
+       len += mqtt_pack_str((buf + len), pkt->client_id, client_id_len);
+
+       /* Add optionnal Will message */
+       if ((pkt->will_msg_size != 0) && (pkt->will_topic.name != NULL) && (pkt->will_msg != NULL)) {
+               uint16_t tmp = htons(pkt->will_msg_size);
+               len += mqtt_pack_str((buf + len), pkt->will_topic.name, will_topic_len);
+               memcpy((buf + len), &tmp, 2);
+               memcpy((buf + len + 2), pkt->will_msg, pkt->will_msg_size);
+               len += pkt->will_msg_size + 2;
+       }
+       /* Add optionnal username and password */
+       if (pkt->username != NULL) {
+               len += mqtt_pack_str((buf + len), pkt->username, username_len);
+               /* Add password too ? */
+               if (pkt->password != NULL) {
+                       len += mqtt_pack_str((buf + len), pkt->password, password_len);
+               }
+       }
+
+       return len;
+}
+
+
+/***************************************************************************** */
+/* Check MQTT connack packet
+ * This function may get called to check a supposed connect acknowledge packet.
+ * The function checks for conformance to the MQTT protocol and returns 0 if the packet is valid,
+ * regardless of the retrun code value.
+ */
+int mqtt_check_connack_reply_pkt(const struct mqtt_connack_reply_pkt* pkt)
+{
+       if (pkt == NULL) {
+               return -EINVAL;
+       }
+       if (pkt->control != (MQTT_CONTROL_CONNACK << 4)) {
+               return -EPROTO;
+       }
+       if (pkt->rem_len != 2) {
+               return -EPROTO;
+       }
+       if (pkt->flags & 0xFE) {
+               return -EPROTO;
+       }
+       if (pkt->ret_code >= MQTT_CONNACK_MAX) {
+               return -EPROTO;
+       }
+       if (pkt->ret_code != 0) {
+               return -EREMOTEIO;
+       }
+       return 0;
+}
+
+
+
+/***************************************************************************** */
+/* Create MQTT publish packet
+ * This function must be called in order to create a publish MQTT packet used to
+ * publish data on a topic (send data to the server).
+ */
+int mqtt_pack_publish_packet(const struct mqtt_publish_pkt* pkt, uint8_t* buf, uint32_t buf_size)
+{
+       uint32_t remaining_length = 0;
+       uint32_t len = 0;
+       uint16_t topic_len = 0;
+       uint16_t tmp_packet_id = 0;
+       uint8_t publish_flags = 0;
+
+       if ((pkt == NULL) || (buf == NULL)) {
+               return -EINVAL;
+       }
+       /* Check packet consistency */
+       if ((pkt->message_size != 0) && (pkt->application_message == NULL)) {
+               return -ENODATA;
+       }
+       /* Compute message size
+        * There is no "message size" field in the publish packet, as the message size can be computed by
+        * substracting the topic string size and packet id size from the packet "remaining length" field.
+        */
+       remaining_length = pkt->message_size;
+       /* Packet ID is 2 bytes long. If QoS is 0, then the packet must not include a packet ID */
+       if (pkt->topic.QoS != 0) {
+               remaining_length += 2;
+       }
+       /* Topic is mandatory */
+       if (pkt->topic.name == NULL) {
+               return -EINVAL;
+       }
+       topic_len = strlen(pkt->topic.name);
+       /* Update packet payload size */
+       remaining_length += topic_len + 2;
+
+       /* Set publish flags */
+       publish_flags = MQTT_PUBLISH_QoS(pkt->topic.QoS);
+       if (pkt->dup_flag) {
+               publish_flags |= MQTT_CONNECT_FLAG_CLEAN_SESSION;
+       }
+       if (pkt->retain_flag) {
+               publish_flags |= MQTT_PUBLISH_RETAIN;
+       }
+
+       /* Build MQTT Publish packet */
+       /* Fixed header */
+       len = mqtt_pack_fixed_header(buf, MQTT_CONTROL_PUBLISH, publish_flags, remaining_length);
+       if (remaining_length + len > buf_size) {
+               return -E2BIG;
+       }
+       /* Topic is mandatory */
+       len += mqtt_pack_str((buf + len), pkt->topic.name, topic_len);
+       /* Packet ID */
+       if (pkt->topic.QoS != 0) {
+               tmp_packet_id = htons(pkt->packet_id);
+               memcpy((buf + len), &tmp_packet_id, 2);
+               len += 2;
+       }
+       /* Add optionnal application message */
+       if (pkt->message_size != 0) {
+               memcpy((buf + len), pkt->application_message, pkt->message_size);
+               len += pkt->message_size;
+       }
+
+       return len;
+}
+
+
+/***************************************************************************** */
+/* Unpack MQTT publish packet
+ * This function must be called in order to transform a received publish MQTT packet to a
+ * mqtt_publish_pkt structure.
+ * The function also checks the validity of the packet.
+ * All returned pointers within the struct will point to parts of the provided buffer, so the buffer
+ * must not be discarded after the call.
+ * if the return value is positive, it is the topic string length.
+ */
+int mqtt_unpack_publish_packet(struct mqtt_publish_pkt* pkt, uint8_t* buf, uint32_t size)
+{
+       int ret = 0;
+       uint32_t idx = 0, pkt_len = 0;
+       uint16_t tmp = 0, topic_len = 0;
+       if ((pkt == NULL) || (buf == NULL)) {
+               return -EINVAL;
+       }
+       /* Check type */
+       if ((buf[0] & 0xF0) != (MQTT_CONTROL_PUBLISH << 4)) {
+               return -EPROTO;
+       }
+       /* Check packet size */
+       ret = decode_remaining_length((buf + 1), &pkt_len);
+       if ((ret > 4) || ((ret + pkt_len + 1) != size)) {
+               return -EPROTO;
+       }
+       idx = 1 + ret;
+       /* Decode flags */
+       pkt->topic.QoS = (buf[0] >> 1) & 0x03;
+       pkt->dup_flag = (buf[0] & MQTT_PUBLISH_DUP);
+       pkt->retain_flag = (buf[0] & MQTT_PUBLISH_RETAIN);
+       if (pkt->topic.QoS > 2) {
+               return -EPROTO;
+       }
+       /* Decode topic string */
+       memcpy(&tmp, (buf + idx), 2);
+       topic_len = ntohs(tmp);
+       pkt->topic.name = (char*)(buf + idx + 2);
+       idx += topic_len + 2;
+       /* Does it fit in the remaining length ? */
+       tmp = idx;
+       if (pkt->topic.QoS != 0) {
+               tmp += 2;
+       }
+       if (tmp > size) {
+               return -EPROTO;
+       }
+       /* Decode packet ID */
+       if (pkt->topic.QoS != 0) {
+               memcpy(&tmp, (buf + idx), 2);
+               pkt->packet_id = ntohs(tmp);
+               idx += 2;
+       }
+       /* Get application message */
+       pkt->message_size = size - idx;
+       if (pkt->message_size != 0) {
+               pkt->application_message = (buf + idx);
+       }
+       return topic_len;
+}
+
+/***************************************************************************** */
+/* Build MQTT puback, pubrec, pubrel or pubcomp packet, used in the publish acknowledge one-way or
+ * two-way hand-check mechanism.
+ */
+int mqtt_pack_publish_reply_pkt(uint8_t* buf, uint16_t acked_pkt_id, uint8_t type)
+{
+       uint16_t tmp_acked_pkt_id = 0;
+       if (buf == NULL) {
+               return -EINVAL;
+       }
+       buf[0] = (type << 4);
+       if (type == MQTT_CONTROL_PUBREL) {
+               buf[0] |= MQTT_PUBREL_FLAG;
+       }
+       buf[1] = 0x02;
+       tmp_acked_pkt_id = htons(acked_pkt_id);
+       memcpy((buf + 2), &tmp_acked_pkt_id, 2);
+
+       return 4;
+}
+
+/***************************************************************************** */
+/* Check MQTT puback, pubrec, pubrel or pubcomp packet
+ * This function may get called to check a supposed publish acknowledge packet in either one-way or
+ * two-way hand-check mechanism.
+ */
+int mqtt_check_publish_reply_pkt(struct mqtt_publish_reply_pkt* pkt, uint8_t type)
+{
+       uint8_t awaited_control = 0;
+
+       if (pkt == NULL) {
+               return -EINVAL;
+       }
+       /* Check control byte */
+       awaited_control = (type << 4);
+       /* The pubrel packet must also have a "pubrel" flag ... the protocol provides no indication about the
+        * reason behind this */
+       if (type == MQTT_CONTROL_PUBREL) {
+               awaited_control |= MQTT_PUBREL_FLAG;
+       }
+       if (pkt->control != awaited_control) {
+               return -EPROTO;
+       }
+       /* Check size */
+       if (pkt->rem_len != 2) {
+               return -EPROTO;
+       }
+       /* Valid packet */
+       return 0;
+}
+
+
+/* Subscribe and unsubscribe packets only differ by the addition of the QoS byte after each topic. */
+static int mqtt_pack_sub_unsub(const struct mqtt_sub_pkt* pkt,
+                                                               uint8_t* buf, uint32_t buf_size, uint8_t type)
+{
+       uint32_t remaining_length = 0;
+       uint32_t len = 0, i = 0;
+       uint16_t topic_len = 0;
+       uint16_t tmp_packet_id = 0;
+
+       if ((pkt == NULL) || (buf == NULL)) {
+               return -EINVAL;
+       }
+       if ((type != MQTT_CONTROL_SUBSCRIBE) && (type != MQTT_CONTROL_UNSUBSCRIBE)) {
+               return -EINVAL;
+       }
+       /* Check packet consistency */
+       if ((pkt->nb_topics == 0) || (pkt->topics == NULL)) {
+               return -ENODATA;
+       }
+       /* Limit the number of topics to 125 in orer to keep the variable length field on one byte in
+        * the subscribe acknowledge packet. */
+       if (pkt->nb_topics > 125) {
+               return -EINVAL;
+       }
+       /* Compute message size
+        * Packet ID is 2 bytes long. */
+       remaining_length = 2;
+       for (i = 0; i < pkt->nb_topics; i++) {
+               if (pkt->topics[i].name == NULL) {
+                       return -EINVAL;
+               }
+               topic_len = strlen(pkt->topics[i].name);
+               /* Update packet payload size. */
+               remaining_length += topic_len + 2;
+               if (type == MQTT_CONTROL_SUBSCRIBE) {
+                       /* Add one for the associated QoS for each topic */
+                       remaining_length += 1;
+               }
+       }
+
+       /* Build MQTT Publish packet */
+       /* Fixed header */
+       len = mqtt_pack_fixed_header(buf, type, MQTT_SUBSCRIBE_FLAG, remaining_length);
+       if (remaining_length + len > buf_size) {
+               return -E2BIG;
+       }
+       /* Packet ID */
+       tmp_packet_id = htons(pkt->packet_id);
+       memcpy((buf + len), &tmp_packet_id, 2);
+       len += 2;
+       /* Topic(s) */
+       for (i = 0; i < pkt->nb_topics; i++) {
+               topic_len = strlen(pkt->topics[i].name);
+               len += mqtt_pack_str((buf + len), pkt->topics[i].name, topic_len);
+               if (type == MQTT_CONTROL_SUBSCRIBE) {
+                       /* Add the associated QoS */
+                       buf[len++] = pkt->topics[i].QoS & 0x03;
+               }
+       }
+
+       return len;
+}
+
+/***************************************************************************** */
+/* Build MQTT subscribe packet
+ * This function must be called in order to create a subscribe MQTT packet used to
+ * subscibe on a topic (or multiple topics) in order to receive data published on this
+ * or these topics.
+ * We limit the number of subscriptions sent at once to 125 in order to get a fixed size
+ * subscription acknoledgement packet.
+ */
+int mqtt_pack_subscribe_pkt(const struct mqtt_sub_pkt* pkt, uint8_t* buf, uint32_t buf_size)
+{
+       return mqtt_pack_sub_unsub(pkt, buf, buf_size, MQTT_CONTROL_SUBSCRIBE);
+}
+
+/***************************************************************************** */
+/* Build MQTT unsubscribe packet
+ * This function must be called in order to create an unsubscribe MQTT packet used to unsubscibe
+ * from a topic (or multiple topics) in order to stop receiving data published on this or these
+ * topics.
+ */
+int mqtt_pack_unsubscribe_pkt(const struct mqtt_sub_pkt* pkt, uint8_t* buf, uint32_t buf_size)
+{
+       return mqtt_pack_sub_unsub(pkt, buf, buf_size, MQTT_CONTROL_UNSUBSCRIBE);
+}
+
+
+/***************************************************************************** */
+/* Check MQTT suback packet
+ * This function may get called to check a supposed subscribe acknowledge packet.
+ * The function checks for conformance to the MQTT protocol and returns 0 if the packet is valid,
+ * regardless of the return codes received.
+ * len must be the length of the full packet received, which includes the mqtt_subscribe_reply_pkt
+ * structure and all the return codes received.
+ */
+int mqtt_check_suback_reply_pkt(const struct mqtt_sub_reply_pkt* pkt, uint8_t len)
+{
+       int i = 0;
+       uint8_t* ret_codes = NULL;
+       if (pkt == NULL) {
+               return -EINVAL;
+       }
+       if (pkt->control != (MQTT_CONTROL_SUBACK << 4)) {
+               return -EPROTO;
+       }
+       if ((pkt->rem_len > 127) || (pkt->rem_len != (len - 2))) {
+               return -EPROTO;
+       }
+       ret_codes = (uint8_t*)pkt + 4;
+       for (i = 0; i < (pkt->rem_len - 2); i++) {
+               if ((ret_codes[i] != 0x80) && (ret_codes[i] > MQTT_QoS_2)) {
+                       return -EREMOTEIO;
+               }
+       }
+       /* Valid packet */
+       return 0;
+}
+
+
+/***************************************************************************** */
+/* Check MQTT unsuback packet
+ * This function may get called to check a supposed unsubscribe acknowledge packet.
+ * The function checks for conformance to the MQTT protocol and returns 0 if the packet is valid.
+ */
+int mqtt_check_unsuback_reply_pkt(const struct mqtt_sub_reply_pkt* pkt)
+{
+       if (pkt == NULL) {
+               return -EINVAL;
+       }
+       if (pkt->control != (MQTT_CONTROL_UNSUBACK << 4)) {
+               return -EPROTO;
+       }
+       if (pkt->rem_len != 2) {
+               return -EPROTO;
+       }
+       /* Valid packet */
+       return 0;
+}
+
+
+/***************************************************************************** */
+/* Build MQTT ping packet
+ * This one is a fixed packet, easy.
+ */
+int mqtt_pack_ping_pkt(uint8_t* buf)
+{
+       struct mqtt_ping_pkt pkt = {
+               .control = (MQTT_CONTROL_PINGREQ << 4),
+               .rem_len = 0,
+       };
+       if (buf == NULL) {
+               return -EINVAL;
+       }
+       memcpy(buf, &pkt, sizeof(struct mqtt_ping_pkt));
+       return sizeof(struct mqtt_ping_pkt);
+}
+
+
+/***************************************************************************** */
+/* Check MQTT ping reply packet */
+int mqtt_check_ping_reply_pkt(const struct mqtt_ping_pkt* pkt)
+{
+       if (pkt == NULL) {
+               return -EINVAL;
+       }
+       if (pkt->control != (MQTT_CONTROL_PINGRESP << 4)) {
+               return -EPROTO;
+       }
+       if (pkt->rem_len != 0) {
+               return -EPROTO;
+       }
+       return 0;
+}
+
+
+/***************************************************************************** */
+/* Build MQTT disconnect packet
+ * This one is a fixed packet, easy.
+ */
+int mqtt_pack_disconnect_pkt(uint8_t* buf)
+{
+       struct mqtt_disconnect_pkt pkt = {
+               .control = (MQTT_CONTROL_DISCONNECT << 4),
+               .rem_len = 0,
+       };
+       if (buf == NULL) {
+               return -EINVAL;
+       }
+       memcpy(buf, &pkt, sizeof(struct mqtt_disconnect_pkt));
+       return sizeof(struct mqtt_disconnect_pkt);
+}
+