/*
 * Copyright (c) 2016 Intel Corporation.
 *
 * SPDX-License-Identifier: Apache-2.0
 */

/**
 * @file	mqtt_pkt.c
 *
 * @brief	MQTT v3.1.1 packet library, see:
 *		http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/mqtt-v3.1.1.html
 *
 *		   This file is part of the Zephyr Project
 *			http://www.zephyrproject.org
 */

#include "mqtt_pkt.h"
#include <net/net_ip.h>			/* for htons/ntohs */
#include <string.h>
#include <errno.h>

#define PACKET_TYPE_SIZE		1
#define REM_LEN_MIN_SIZE		1
#define ENCLENBUF_MAX_SIZE		4
#define CONNECT_VARIABLE_HDR_SIZE	10	/* See MQTT 3.1.1	*/
#define CONNECT_MIN_SIZE		(PACKET_TYPE_SIZE + REM_LEN_MIN_SIZE +\
					 CONNECT_VARIABLE_HDR_SIZE)
#define CONNACK_SIZE			4
#define CONNACK_REMLEN			2	/* See MQTT 3.2.1	*/
#define MSG_PKTID_ONLY_SIZE		4

/* Fixed header, reserved bits */
#define PUBREL_RESERVED			2	/* See MQTT 3.6.1	*/
#define PUBACK_RESERVED			0
#define PUBREC_RESERVED			0
#define PUBCOMP_RESERVED		0
#define UNSUBACK_RESERVED		0
#define UNSUBSCRIBE_RESERVED		0

#define INT_SIZE			2	/* See MQTT 1.5.2	*/
#define KEEP_ALIVE_SIZE			2	/* See MQTT 3.1.2.10	*/
#define PACKET_ID_SIZE			2	/* See MQTT 2.3.1	*/
#define QoS_SIZE			1
#define FLAGS_SIZE			1
#define SUBSCRIBE_RESERVED		0x02	/* See MQTT 3.8.1	*/
#define MSG_ZEROLEN_SIZE		2

#define TOPIC_STR_MIN_SIZE		1
#define TOPIC_MIN_SIZE			(INT_SIZE + TOPIC_STR_MIN_SIZE +\
					 QoS_SIZE)

/**
 * Enhanced strlen function
 *
 * @details This function is introduced to allow developers to pass null strings
 * to functions that compute the length of strings. strlen returns random values
 * for null arguments, so mqtt_strlen(NULL) is quite useful when optional
 * strings are allowed by mqtt-related functions. For example: username in the
 * MQTT CONNECT message is an optional parameter, so
 * connect(..., username = NULL, ...) will work fine without passing an
 * additional parameter like:
 *	connect(.., is_username_present, username, ...) or
 *	connect(.., username, username_len, ...).
 *
 * @param str C-string or NULL
 *
 * @retval 0 for NULL
 * @retval strlen otherwise
 */
static inline
uint16_t mqtt_strlen(const char *str)
{
	if (str) {
		return (uint16_t)strlen(str);
	}

	return 0;
}

/**
 * Computes the amount of bytes needed to codify the value stored in len.
 *
 * @param [out] size Amount of bytes required to codify the value stored in len
 * @param [in] len Value to be codified
 *
 * @retval 0 on success
 * @retval -EINVAL
 */
static
int compute_rlen_size(uint16_t *size, uint32_t len)
{
	if (len <= 127) {
		*size = 1;
	} else if (len >= 128 && len <= 16383) {
		*size = 2;
	} else if (len >= 16384 && len <= 2097151) {
		*size = 3;
	} else if (len >= 2097152 && len <= 268435455) {
		*size = 4;
	} else {
		return -EINVAL;
	}

	return 0;
}

/**
 * Remaining Length encoding algorithm. See MQTT 2.2.3 Remaining Length
 *
 * @param [out] buf Buffer where the encoded value will be stored
 * @param [in] len Value to encode
 *
 * @retval 0 always
 */
static int rlen_encode(uint8_t *buf, int len)
{
	uint8_t encoded;
	uint8_t i;

	i = 0;
	do {
		encoded = len % 128;
		len = len / 128;
		/* if there are more data to encode,
		 * set the top bit of this byte
		 */
		if (len > 0) {
			encoded = encoded | 128;
		}
		buf[i++] = encoded;
	} while (len > 0);

	return 0;
}

/**
 * Remaining Length decoding algorithm. See MQTT 2.2.3 Remaining Length
 *
 * @param [out] rlen Remaining Length (decoded)
 * @param [out] rlen_size Number of bytes required to codify rlen's value
 * @param [in] buf Buffer where the codified Remaining Length is codified
 * @param [in] size Buffer size
 *
 * @retval 0 on success
 * @retval -ENOMEM if size < 4
 */
static int rlen_decode(uint16_t *rlen, uint16_t *rlen_size,
		       uint8_t *buf, uint16_t size)
{
	uint16_t value = 0;
	uint16_t mult = 1;
	uint16_t i = 0;
	uint8_t encoded;

	do {
		if (i >= ENCLENBUF_MAX_SIZE || i >= size) {
			return -ENOMEM;
		}

		encoded = buf[i++];
		value += (encoded & 127) * mult;
		mult *= 128;
	} while ((encoded & 128) != 0);

	*rlen = value;
	*rlen_size = i;

	return 0;
}

int mqtt_pack_connack(uint8_t *buf, uint16_t *length, uint16_t size,
		      uint8_t session_present, uint8_t ret_code)
{
	if (size < CONNACK_SIZE) {
		return -ENOMEM;
	}

	buf[0] = MQTT_CONNACK << 4;
	buf[1] = CONNACK_REMLEN;
	buf[2] = (session_present ? 0x01 : 0x00);
	buf[3] = ret_code & 0xFF;
	*length = CONNACK_SIZE;

	return 0;
}

/**
 * Packs a message that only contains the Packet Identifier as payload.
 *
 * @details Many MQTT messages only codify the packet type, reserved flags and
 * the Packet Identifier as payload, so this function is used by those MQTT
 * messages. The total size of this message is always 4 bytes, with a payload
 * of only 2 bytes to codify the identifier.
 *
 * @param [out] buf Buffer where the resultant message is stored
 * @param [out] length Number of bytes required to codify the message
 * @param [in] size Buffer's size
 * @param [in] type MQTT Control Packet type
 * @param [in] reserved	Control Packet Reserved Flags. See MQTT 2.2.2 Flags
 * @param [in] pkt_id Packet Identifier. See MQTT 2.3.1 Packet Identifier
 *
 * @retval 0 on success
 * @retval -ENOMEM if size < 4
 */
static
int pack_pkt_id(uint8_t *buf, uint16_t *length, uint16_t size,
		enum mqtt_packet type, uint8_t reserved, uint16_t pkt_id)
{
	if (size < MSG_PKTID_ONLY_SIZE) {
		return -ENOMEM;
	}

	buf[0] = (type << 4) + (reserved & 0x0F);
	buf[1] = PACKET_ID_SIZE;
	UNALIGNED_PUT(htons(pkt_id), (uint16_t *)(buf + PACKET_ID_SIZE));
	*length = MSG_PKTID_ONLY_SIZE;

	return 0;
}

int mqtt_pack_puback(uint8_t *buf, uint16_t *length, uint16_t size,
		     uint16_t pkt_id)
{
	return pack_pkt_id(buf, length, size, MQTT_PUBACK, 0, pkt_id);
}

int mqtt_pack_pubrec(uint8_t *buf, uint16_t *length, uint16_t size,
		     uint16_t pkt_id)
{
	return pack_pkt_id(buf, length, size, MQTT_PUBREC, 0, pkt_id);
}

int mqtt_pack_pubrel(uint8_t *buf, uint16_t *length, uint16_t size,
		     uint16_t pkt_id)
{
	return pack_pkt_id(buf, length, size, MQTT_PUBREL, PUBREL_RESERVED,
			   pkt_id);
}

int mqtt_pack_pubcomp(uint8_t *buf, uint16_t *length, uint16_t size,
		      uint16_t pkt_id)
{
	return pack_pkt_id(buf, length, size, MQTT_PUBCOMP, 0, pkt_id);
}

int mqtt_pack_unsuback(uint8_t *buf, uint16_t *length, uint16_t size,
		       uint16_t pkt_id)
{
	return pack_pkt_id(buf, length, size, MQTT_UNSUBACK, 0, pkt_id);
}

int mqtt_pack_suback(uint8_t *buf, uint16_t *length, uint16_t size,
		     uint16_t pkt_id, uint8_t elements,
		     enum mqtt_qos granted_qos[])
{
	uint16_t rlen_size;
	uint16_t offset;
	uint16_t rlen;
	uint8_t i;
	int rc;


	rlen = PACKET_ID_SIZE + QoS_SIZE*elements;
	rc = compute_rlen_size(&rlen_size, rlen);
	if (rc != 0) {
		return -EINVAL;
	}

	*length = PACKET_TYPE_SIZE + rlen_size + rlen;
	if (*length > size) {
		return -ENOMEM;
	}

	buf[0] = MQTT_SUBACK << 4;

	rlen_encode(buf + PACKET_TYPE_SIZE, rlen);
	offset = PACKET_TYPE_SIZE + rlen_size;

	UNALIGNED_PUT(htons(pkt_id), (uint16_t *)(buf + offset));
	offset += PACKET_ID_SIZE;

	for (i = 0; i < elements; i++) {
		buf[offset + i] = granted_qos[i];
	}

	return 0;
}

int mqtt_pack_connect(uint8_t *buf, uint16_t *length, uint16_t size,
		      struct mqtt_connect_msg *msg)
{
	uint16_t total_buf_size;
	uint16_t rlen_size;
	uint16_t pkt_size;
	uint16_t offset;
	int rc;

	/* ----------- Payload size ----------- */

	/* client_id				*/
	pkt_size = INT_SIZE;
	msg->client_id_len = mqtt_strlen(msg->client_id);
	pkt_size += msg->client_id_len;

	/* will flag - optional			*/
	if (msg->will_flag) {
		/* will topic			*/
		pkt_size += INT_SIZE;
		msg->will_topic_len = mqtt_strlen(msg->will_topic);
		pkt_size += msg->will_topic_len;
		/* will message - binary	*/
		pkt_size += INT_SIZE;
		pkt_size += msg->will_msg_len;
	}

	/* user_name - UTF-8 - optional		*/
	if (msg->user_name) {
		pkt_size += INT_SIZE;
		msg->user_name_len = mqtt_strlen(msg->user_name);
		pkt_size += msg->user_name_len;
	}

	/* password - binary - optional		*/
	if (msg->password) {
		pkt_size += INT_SIZE;
		pkt_size += msg->password_len;
	}

	/* Variable Header size			*/
	pkt_size += CONNECT_VARIABLE_HDR_SIZE;

	rc = compute_rlen_size(&rlen_size, pkt_size);
	if (rc != 0) {
		return -EINVAL;
	}

	/* 1 Byte for the MQTT Control Packet Type
	 * + Remaining length field size
	 * + Variable Header Size + Payload size
	 */
	total_buf_size = PACKET_TYPE_SIZE + rlen_size + pkt_size;
	if (total_buf_size > size) {
		return -ENOMEM;
	}

	buf[0] = MQTT_CONNECT << 4;

	rlen_encode(buf + PACKET_TYPE_SIZE, pkt_size);
	offset = PACKET_TYPE_SIZE + rlen_size;

	/* Variable Header size for MQTT CONNECT is 10 bytes.
	 * The following magic numbers are specified by the standard.
	 * See MQTT 3.1.2 Variable Header.
	 */
	buf[offset + 0] = 0x00;
	buf[offset + 1] = 0x04;
	buf[offset + 2] = 'M';
	buf[offset + 3] = 'Q';
	buf[offset + 4] = 'T';
	buf[offset + 5] = 'T';
	buf[offset + 6] = 0x04;
	/* connect flags */
	buf[offset + 7] = (msg->user_name ? 1 << 7 : 0) |
			  (msg->password_len ? 1 << 6 : 0) |
			  (msg->will_retain ? 1 << 5 : 0) |
			  ((msg->will_qos & 0x03) << 3) |
			  (msg->will_flag ? 1 << 2 : 0) |
			  (msg->clean_session ? 1 << 1 : 0);

	UNALIGNED_PUT(htons(msg->keep_alive), (uint16_t *)(buf + offset + 8));
	offset += 8 + INT_SIZE;
	/* end of the CONNECT's Variable Header */

	/* Payload */
	UNALIGNED_PUT(htons(msg->client_id_len),
		      (uint16_t *)(buf + offset));
	offset += INT_SIZE;
	memcpy(buf + offset, msg->client_id, msg->client_id_len);
	offset += msg->client_id_len;

	if (msg->will_flag) {
		UNALIGNED_PUT(htons(msg->will_topic_len),
			      (uint16_t *)(buf + offset));
		offset += INT_SIZE;
		memcpy(buf + offset, msg->will_topic,
		       msg->will_topic_len);
		offset += msg->will_topic_len;

		UNALIGNED_PUT(htons(msg->will_msg_len),
			      (uint16_t *)(buf + offset));
		offset += INT_SIZE;
		memcpy(buf + offset, msg->will_msg, msg->will_msg_len);
		offset += msg->will_msg_len;
	}

	if (msg->user_name) {
		UNALIGNED_PUT(htons(msg->user_name_len),
			      (uint16_t *)(buf + offset));
		offset += INT_SIZE;
		memcpy(buf + offset, msg->user_name, msg->user_name_len);
		offset += msg->user_name_len;
	}

	if (msg->password) {
		UNALIGNED_PUT(htons(msg->password_len),
			      (uint16_t *)(buf + offset));
		offset += INT_SIZE;
		memcpy(buf + offset, msg->password, msg->password_len);
		offset += msg->password_len;
	}

	*length = total_buf_size;

	return 0;
}

/**
 * Recovers the length and sets val to point to the beginning of the value
 *
 * @param [in] buf Buffer where the length and value are stored
 * @param [in] length Buffer's length
 * @param [out] val Pointer to the beginning of the value
 * @param [out] val_len Recovered value's length
 *
 * @retval 0 on success
 * @retval -EINVAL
 */
static
int recover_value_len(uint8_t *buf, uint16_t length, uint8_t **val,
		      uint16_t *val_len)
{
	uint16_t val_u16;

	if (length < INT_SIZE) {
		return -EINVAL;
	}

	val_u16 = UNALIGNED_GET((uint16_t *)buf);
	*val_len = ntohs(val_u16);

	/* malformed packet: avoid buffer overflows */
	if ((INT_SIZE + *val_len) > length) {
		return -EINVAL;
	}
	*val = buf + INT_SIZE;

	return 0;
}

int mqtt_unpack_connect(uint8_t *buf, uint16_t length,
			struct mqtt_connect_msg *msg)
{
	uint8_t user_name_flag;
	uint8_t password_flag;
	uint16_t payload_len;
	uint16_t rlen_size;
	uint16_t val_u16;
	uint8_t offset;
	int rc;

	memset(msg, 0x00, sizeof(struct mqtt_connect_msg));

	/* MQTT CONNECT packet min size, assuming no payload:
	 * packet type + min rem length size + var size len
	 *      1      +       1             +     10
	 */
	if (length < CONNECT_MIN_SIZE) {
		return -EINVAL;
	}

	if (buf[0] != (MQTT_CONNECT << 4)) {
		return -EINVAL;
	}

	rc = rlen_decode(&payload_len, &rlen_size, buf + PACKET_TYPE_SIZE,
			 length - PACKET_TYPE_SIZE);
	if (rc != 0) {
		return rc;
	}

	/* offset points to the protocol name length */
	offset = PACKET_TYPE_SIZE + rlen_size;

	/* protocol name length, fixed value */
	if (buf[offset + 0] != 0x00 || buf[offset + 1] != 0x04) {
		return -EINVAL;
	}

	offset += INT_SIZE;

	if (buf[offset + 0] != 'M' || buf[offset + 1] != 'Q' ||
	    buf[offset + 2] != 'T' || buf[offset + 3] != 'T' ||
	    buf[offset + 4] != 0x04) {
		return -EINVAL;
	}

	/* MQTT string size (4) + Protocol Level size (1) */
	offset += 5;

	/* validating "Connect Flag" bit 0, it must be 0! */
	if ((buf[offset] & 0x01) != 0) {
		return -EINVAL;
	}

	/* connection flags */
	user_name_flag = (buf[offset] & 0x80) ? 1 : 0;
	password_flag = (buf[offset] & 0x40) ? 1 : 0;
	msg->will_retain = (buf[offset] & 0x20) ? 1 : 0;
	msg->will_qos = (buf[offset] & 0x18) >> 3;
	msg->will_flag = (buf[offset] & 0x04) ? 1 : 0;
	msg->clean_session = (buf[offset] & 0x02) ? 1 : 0;

	offset += FLAGS_SIZE;

	val_u16 = UNALIGNED_GET((uint16_t *)(buf + offset));
	msg->keep_alive = ntohs(val_u16);
	offset += KEEP_ALIVE_SIZE;

	rc = recover_value_len(buf + offset, length - offset,
			       (uint8_t **)&msg->client_id,
			       &msg->client_id_len);
	if (rc != 0) {
		return -EINVAL;
	}

	offset += INT_SIZE + msg->client_id_len;

	if (msg->will_flag) {
		rc = recover_value_len(buf + offset, length - offset,
				       (uint8_t **)&msg->will_topic,
				       &msg->will_topic_len);
		if (rc != 0) {
			return -EINVAL;
		}

		offset += INT_SIZE + msg->will_topic_len;

		rc = recover_value_len(buf + offset, length - offset,
				       &msg->will_msg,
				       &msg->will_msg_len);
		if (rc != 0) {
			return -EINVAL;
		}

		offset += INT_SIZE + msg->will_msg_len;
	}

	if (user_name_flag) {
		rc = recover_value_len(buf + offset, length - offset,
				       (uint8_t **)&msg->user_name,
				       &msg->user_name_len);
		if (rc != 0) {
			return -EINVAL;
		}

		offset += INT_SIZE + msg->user_name_len;
	}

	if (password_flag) {
		rc = recover_value_len(buf + offset, length - offset,
				       &msg->password, &msg->password_len);
		if (rc != 0) {
			return -EINVAL;
		}
	}

	return 0;
}

/**
 * Computes the packet size for the SUBSCRIBE and UNSUBSCRIBE messages
 *
 * This routine does not consider the packet type field size (1 byte)
 *
 * @param rlen_size Remaining length size
 * @param payload_size SUBSCRIBE or UNSUBSCRIBE payload size
 * @param items Number of topics
 * @param topics Array of C-strings containing the topics to subscribe to
 * @param with_qos 0 for UNSUBSCRIBE, != 0 for SUBSCRIBE
 *
 * @retval 0 on success
 * @retval -EINVAL on error
 */
static
int subscribe_size(uint16_t *rlen_size, uint16_t *payload_size, uint8_t items,
		   const char *topics[], enum mqtt_qos with_qos)
{
	uint8_t i;
	int rc;

	*payload_size = PACKET_ID_SIZE;

	for (i = 0; i < items; i++) {
		/* topic length (as string) +1 byte for its QoS		*/
		*payload_size += mqtt_strlen(topics[i]) +
				 (with_qos ? QoS_SIZE : 0);
	}

	/* we add two bytes per topic to codify the topic's length	*/
	*payload_size += items * INT_SIZE;

	rc = compute_rlen_size(rlen_size, *payload_size);
	if (rc != 0) {
		return -EINVAL;
	}

	return 0;
}

/**
 * Packs the SUBSCRIBE and UNSUBSCRIBE messages
 *
 * @param buf Buffer where the message will be stored
 * @param pkt_id Packet Identifier
 * @param items Number of topics
 * @param topics Array of C-strings containing the topics to subscribe to
 * @param qos Array of QoS' values, qos[i] is the QoS of topic[i]
 * @param type MQTT_SUBSCRIBE or MQTT_UNSUBSCRIBE
 *
 * @retval 0 on success
 * @retval -EINVAL
 * @retval -ENOMEM
 */
static int mqtt_pack_subscribe_unsubscribe(uint8_t *buf, uint16_t *length,
					   uint16_t size, uint16_t pkt_id,
					   uint8_t items, const char *topics[],
					   const enum mqtt_qos qos[],
					   enum mqtt_packet type)
{
	uint16_t rlen_size;
	uint16_t payload;
	uint16_t offset;
	uint8_t i;
	int rc;

	if (items <= 0) {
		return -EINVAL;
	}

	if (type != MQTT_SUBSCRIBE && type != MQTT_UNSUBSCRIBE) {
		return -EINVAL;
	}

	rc = subscribe_size(&rlen_size, &payload, items, topics,
			    type == MQTT_SUBSCRIBE ? 1 : 0);
	if (rc != 0) {
		return -EINVAL;
	}

	/* full packet size is:
	 * rem len size + payload + 1 byte for the packet type field size
	 */
	if ((rlen_size + payload + 1) > size) {
		return -ENOMEM;
	}

	/* after data length validations, we are ready...		*/
	buf[0] = (type << 4) + 0x02;

	/* compute and set the packet length, return code not evaluated
	 * because we previously called compute_rlen_size
	 */
	rlen_encode(buf + PACKET_TYPE_SIZE, payload);

	offset = PACKET_TYPE_SIZE + rlen_size;

	UNALIGNED_PUT(htons(pkt_id), (uint16_t *)(buf + offset));
	offset += PACKET_ID_SIZE;

	for (i = 0; i < items; i++) {
		uint16_t topic_len = mqtt_strlen(topics[i]);

		UNALIGNED_PUT(htons(topic_len), (uint16_t *)(buf + offset));
		offset += INT_SIZE;

		memcpy(buf + offset, topics[i], topic_len);
		offset += topic_len;

		if (type == MQTT_SUBSCRIBE) {
			buf[offset] = qos[i] & 0x03;
			offset += QoS_SIZE;
		}
	}

	*length = offset;

	return 0;
}

int mqtt_pack_subscribe(uint8_t *buf, uint16_t *length, uint16_t size,
			uint16_t pkt_id, uint8_t items, const char *topics[],
			const enum mqtt_qos qos[])
{

	return mqtt_pack_subscribe_unsubscribe(buf, length, size, pkt_id,
					       items, topics, qos,
					       MQTT_SUBSCRIBE);
}

int mqtt_pack_unsubscribe(uint8_t *buf, uint16_t *length, uint16_t size,
			  uint16_t pkt_id, uint8_t items, const char *topics[])
{
	return mqtt_pack_subscribe_unsubscribe(buf, length, size, pkt_id,
					       items, topics, NULL,
					       MQTT_UNSUBSCRIBE);
}

int mqtt_unpack_subscribe(uint8_t *buf, uint16_t length, uint16_t *pkt_id,
			  uint8_t *items, uint8_t elements, char *topics[],
			  uint16_t topic_len[], enum mqtt_qos qos[])
{
	uint16_t rmlen_size;
	uint16_t val_u16;
	uint16_t rmlen;
	uint16_t offset;
	uint8_t i;
	int rc;

	rc = rlen_decode(&rmlen, &rmlen_size, buf + PACKET_TYPE_SIZE,
			 length - PACKET_TYPE_SIZE);

	if (rc != 0) {
		return -EINVAL;
	}

	/* avoid buffer overflow due to malformed message		*/
	if ((PACKET_TYPE_SIZE + rmlen_size + rmlen) > length) {
		return -EINVAL;
	}

	/* MQTT-3.8.2 and MQTT-3.8.3-3: pkt_id and one topic filter	*/
	if (PACKET_ID_SIZE + TOPIC_MIN_SIZE > rmlen) {
		return -EINVAL;
	}

	/* MQTT-3.8.1:	SUBSCRIBE Fixed Header				*/
	if (buf[0] != (MQTT_SUBSCRIBE << 4 | SUBSCRIBE_RESERVED)) {
		return -EINVAL;
	}

	offset = PACKET_TYPE_SIZE + rmlen_size;

	val_u16 = UNALIGNED_GET((uint16_t *)(buf + offset));
	*pkt_id = ntohs(val_u16);
	offset += PACKET_ID_SIZE;

	*items = 0;
	for (i = 0; i < elements; i++) {
		if ((offset + TOPIC_MIN_SIZE) > length) {
			return -EINVAL;
		}

		val_u16 = UNALIGNED_GET((uint16_t *)(buf + offset));
		topic_len[i] = ntohs(val_u16);
		offset += INT_SIZE;
		/* invalid topic length found: malformed message	*/
		if ((offset + topic_len[i] + QoS_SIZE) > length) {
			return -EINVAL;
		}

		topics[i] = (char *)(buf + offset);
		offset += topic_len[i];
		qos[i] = *(buf + offset);
		offset += QoS_SIZE;

		*items += 1;

		if (offset == length) {
			return 0;
		}
	}

	return 0;
}

int mqtt_unpack_suback(uint8_t *buf, uint16_t length, uint16_t *pkt_id,
		       uint8_t *items, uint8_t elements,
		       enum mqtt_qos granted_qos[])
{
	uint16_t rlen_size;
	enum mqtt_qos qos;
	uint16_t val_u16;
	uint16_t rlen;
	uint16_t offset;
	uint8_t i;
	int rc;

	*pkt_id = 0;
	*items = 0;

	if (elements <= 0) {
		return -EINVAL;
	}

	if (buf[0] != MQTT_SUBACK << 4) {
		return -EINVAL;
	}

	rc = rlen_decode(&rlen, &rlen_size, buf + PACKET_TYPE_SIZE,
			 length - PACKET_TYPE_SIZE);
	if (rc != 0) {
		return -EINVAL;
	}

	/* header size + remaining length value + rm length size	*/
	if (PACKET_TYPE_SIZE + rlen + rlen_size > length) {
		return -EINVAL;
	}

	offset = PACKET_TYPE_SIZE + rlen_size;

	val_u16 = UNALIGNED_GET((uint16_t *)(buf + offset));
	*pkt_id = ntohs(val_u16);
	offset += PACKET_ID_SIZE;

	*items = length - offset;

	/* no enough space to store the QoS				*/
	if (*items > elements) {
		return -EINVAL;
	}

	for (i = 0; i < *items; i++) {
		qos = *(buf + offset);
		if (qos < MQTT_QoS0 || qos > MQTT_QoS2) {
			return -EINVAL;
		}

		granted_qos[i] = qos;
		offset += QoS_SIZE;
	}

	return 0;
}

int mqtt_pack_publish(uint8_t *buf, uint16_t *length, uint16_t size,
		      struct mqtt_publish_msg *msg)
{
	uint16_t offset;
	uint16_t rlen_size;
	uint16_t payload;
	int rc;

	if (msg->qos < MQTT_QoS0 || msg->qos > MQTT_QoS2) {
		return -EINVAL;
	}

	msg->topic_len = mqtt_strlen(msg->topic);
	/* Packet Identifier is only included if QoS > QoS0. See MQTT 3.3.2.2
	 * So, payload size is:
	 * topic length size + topic length + packet id + msg's size
	 */
	payload = INT_SIZE + msg->topic_len +
		  (msg->qos > MQTT_QoS0 ? PACKET_ID_SIZE : 0) + msg->msg_len;

	rc = compute_rlen_size(&rlen_size, payload);
	if (rc != 0) {
		return -EINVAL;
	}

	/* full packet size is:
	 * 1 byte for the packet type field size + rem len size + payload
	 */
	if (PACKET_TYPE_SIZE + rlen_size + payload > size) {
		return -ENOMEM;
	}

	buf[0] = (MQTT_PUBLISH << 4) | ((msg->dup ? 1 : 0) << 3) |
		 (msg->qos << 1) | (msg->retain ? 1 : 0);

	/* compute and set the packet length, return code not evaluated
	 * because we previously called compute_rlen_size
	 */
	rlen_encode(buf + PACKET_TYPE_SIZE, payload);

	offset = PACKET_TYPE_SIZE + rlen_size;
	UNALIGNED_PUT(htons(msg->topic_len), (uint16_t *)(buf + offset));
	offset += INT_SIZE;

	memcpy(buf + offset, msg->topic, msg->topic_len);
	offset += msg->topic_len;

	/* packet id is only present for QoS 1 and 2			*/
	if (msg->qos > MQTT_QoS0) {
		UNALIGNED_PUT(htons(msg->pkt_id), (uint16_t *)(buf + offset));
		offset += PACKET_ID_SIZE;
	}

	memcpy(buf + offset, msg->msg, msg->msg_len);
	offset += msg->msg_len;

	*length = offset;

	return 0;
}

int mqtt_unpack_publish(uint8_t *buf, uint16_t length,
			struct mqtt_publish_msg *msg)
{
	uint16_t rmlen_size;
	uint16_t val_u16;
	uint16_t rmlen;
	uint16_t offset;
	int rc;

	if (buf[0] >> 4 != MQTT_PUBLISH) {
		return -EINVAL;
	}

	msg->dup = (buf[0] & 0x08) >> 3;
	msg->qos = (buf[0] & 0x06) >> 1;
	msg->retain = buf[0] & 0x01;

	rc = rlen_decode(&rmlen, &rmlen_size, buf + PACKET_TYPE_SIZE,
			 length - PACKET_TYPE_SIZE);
	if (rc != 0) {
		return -EINVAL;
	}

	if ((PACKET_TYPE_SIZE + rmlen_size + rmlen) > length) {
		return -EINVAL;
	}

	offset = PACKET_TYPE_SIZE + rmlen_size;
	val_u16 = UNALIGNED_GET((uint16_t *)(buf + offset));
	msg->topic_len = ntohs(val_u16);

	offset += INT_SIZE;
	if (offset + msg->topic_len > length) {
		return -EINVAL;
	}

	msg->topic = (char *)(buf + offset);
	offset += msg->topic_len;

	val_u16 = UNALIGNED_GET((uint16_t *)(buf + offset));
	if (msg->qos == MQTT_QoS1 || msg->qos == MQTT_QoS2) {
		msg->pkt_id = ntohs(val_u16);
		offset += PACKET_ID_SIZE;
	} else {
		msg->pkt_id = 0;
	}

	msg->msg_len = length - offset;
	msg->msg = buf + offset;

	return 0;
}

int mqtt_unpack_connack(uint8_t *buf, uint16_t length, uint8_t *session,
			uint8_t *connect_rc)
{
	if (length < CONNACK_SIZE) {
		return -EINVAL;
	}

	if (buf[0] != (MQTT_CONNACK << 4) || buf[1] != 2) {
		return -EINVAL;
	}

	if (buf[2] > 1) {
		return -EINVAL;
	}

	*session = buf[2];
	*connect_rc = buf[3];

	return 0;
}

/**
 * Packs a zero length message
 *
 * @details This function packs MQTT messages with no payload. No validations
 * are made about the input arguments, besides 'size' that must be at least
 * 2 bytes
 *
 * @param [out] buf Buffer where the resultant message is stored
 * @param [out] length Number of bytes required to codify the message
 * @param [in] size Buffer size
 * @param [in] pkt_type MQTT Control Packet Type. See MQTT 2.2.1
 * @param [in] reserved Reserved bits. See MQTT 2.2
 *
 * @retval 0 on success
 * @retval -ENOMEM
 */
static
int pack_zerolen(uint8_t *buf, uint16_t *length, uint16_t size,
		 enum mqtt_packet pkt_type, uint8_t reserved)
{
	if (size < MSG_ZEROLEN_SIZE) {
		return -ENOMEM;
	}

	buf[0] = (pkt_type << 4) + (reserved & 0x0F);
	buf[1] = 0x00;
	*length = MSG_ZEROLEN_SIZE;

	return 0;
}

int mqtt_pack_pingreq(uint8_t *buf, uint16_t *length, uint16_t size)
{
	return pack_zerolen(buf, length, size, MQTT_PINGREQ, 0x00);
}

int mqtt_pack_pingresp(uint8_t *buf, uint16_t *length, uint16_t size)
{
	return pack_zerolen(buf, length, size, MQTT_PINGRESP, 0x00);
}

int mqtt_pack_disconnect(uint8_t *buf, uint16_t *length, uint16_t size)
{
	return pack_zerolen(buf, length, size, MQTT_DISCONNECT, 0x00);
}

/**
 * Unpacks a MQTT message with a Packet Id as payload
 *
 * @param [in] buf Buffer where the message is stored
 * @param [in] length Message's length
 * @param [out] type MQTT Control Packet type
 * @param [out] reserved Reserved flags
 * @param [out] pkt_id Packet Identifier
 *
 * @retval 0 on success
 * @retval -EINVAL
 */
static
int unpack_pktid(uint8_t *buf, uint16_t length, enum mqtt_packet *type,
		 uint8_t *reserved, uint16_t *pkt_id)
{
	if (length < MSG_PKTID_ONLY_SIZE) {
		return -EINVAL;
	}

	if (buf[1] != PACKET_ID_SIZE) {
		return -EINVAL;
	}

	*type = buf[0] >> 4;
	*reserved = buf[0] & 0x0F;
	*pkt_id = ntohs(*(uint16_t *)(buf + 2));

	return 0;
}

/**
 * Unpacks and validates a MQTT message containing a Packet Identifier
 *
 * @details The message codified in buf must contain a 1) packet type,
 * 2) reserved flags and 3) packet identifier. The user must provide the
 * expected packet type and expected reserved flags. See MQTT 2.2.2 Flags.
 * If the message contains different values for type and reserved flags
 * than the ones passed as arguments, the function will return -EINVAL
 *
 * @param [in] buf Buffer where the message is stored
 * @param [in] length Message's length
 * @param [out] pkt_id Packet Identifier
 * @param [in] expected_type Expected MQTT Control Packet type
 * @param [in] expected_reserv Expected Reserved Flags
 *
 * @retval 0 on success
 * @retval -EINVAL
 */
static
int unpack_pktid_validate(uint8_t *buf, uint16_t length, uint16_t *pkt_id,
			  uint8_t expected_type, uint8_t expected_reserv)
{
	enum mqtt_packet type;
	uint8_t reserved;
	int rc;

	rc = unpack_pktid(buf, length, &type, &reserved, pkt_id);
	if (rc != 0) {
		return rc;
	}

	if (type != expected_type || reserved != expected_reserv) {
		return -EINVAL;
	}

	return 0;
}

int mqtt_unpack_puback(uint8_t *buf, uint16_t length, uint16_t *pkt_id)
{
	return unpack_pktid_validate(buf, length, pkt_id, MQTT_PUBACK,
				     PUBACK_RESERVED);
}

int mqtt_unpack_pubrec(uint8_t *buf, uint16_t length, uint16_t *pkt_id)
{
	return unpack_pktid_validate(buf, length, pkt_id, MQTT_PUBREC,
				     PUBREC_RESERVED);
}

int mqtt_unpack_pubrel(uint8_t *buf, uint16_t length, uint16_t *pkt_id)
{
	return unpack_pktid_validate(buf, length, pkt_id, MQTT_PUBREL,
				     PUBREL_RESERVED);
}

int mqtt_unpack_pubcomp(uint8_t *buf, uint16_t length, uint16_t *pkt_id)
{
	return unpack_pktid_validate(buf, length, pkt_id, MQTT_PUBCOMP,
				     PUBCOMP_RESERVED);
}

int mqtt_unpack_unsuback(uint8_t *buf, uint16_t length, uint16_t *pkt_id)
{
	return unpack_pktid_validate(buf, length, pkt_id, MQTT_UNSUBACK,
				     UNSUBACK_RESERVED);
}

/**
 * Unpacks a zero-length MQTT message
 *
 * @param [in] buf Buffer where the message is stored
 * @param [in] length Message's length
 * @param [out] pkt_type MQTT Control Packet type
 * @param [out] reserved Reserved flags
 *
 * @retval 0 on success
 * @retval -EINVAL
 */
static
int unpack_zerolen(uint8_t *buf, uint16_t length, enum mqtt_packet *pkt_type,
		   uint8_t *reserved)
{
	if (length < MSG_ZEROLEN_SIZE) {
		return -EINVAL;
	}

	*pkt_type = buf[0] >> 4;
	*reserved = buf[0] & 0x0F;

	if (buf[1] != 0) {
		return -EINVAL;
	}

	return 0;
}

/**
 * Unpacks and validates a zero-len MQTT message
 *
 * @param [in] buf Buffer where the message is stored
 * @param [in] length Message's length
 * @param expected_type Expected MQTT Control Packet type
 * @param expected_reserved Expected Reserved Flags
 *
 * @retval 0 on success
 * @retval -EINVAL
 */
static
int unpack_zerolen_validate(uint8_t *buf, uint16_t length,
			    enum mqtt_packet expected_type,
			    uint8_t expected_reserved)
{
	enum mqtt_packet pkt_type;
	uint8_t reserved;
	int rc;

	rc = unpack_zerolen(buf, length, &pkt_type, &reserved);
	if (rc != 0) {
		return rc;
	}

	if (pkt_type != expected_type || reserved != expected_reserved) {
		return -EINVAL;
	}

	return 0;
}

int mqtt_unpack_pingreq(uint8_t *buf, uint16_t length)
{
	return unpack_zerolen_validate(buf, length, MQTT_PINGREQ, 0x00);
}

int mqtt_unpack_pingresp(uint8_t *buf, uint16_t length)
{
	return unpack_zerolen_validate(buf, length, MQTT_PINGRESP, 0x00);
}

int mqtt_unpack_disconnect(uint8_t *buf, uint16_t length)
{
	return unpack_zerolen_validate(buf, length, MQTT_DISCONNECT, 0x00);
}