+/****************************************************************************
+ * Host app for drzportail
+ *
+ *
+ * Copyright 2020 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 <arpa/inet.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+
+#include "mqtt.h"
+
+/*
+ * MQTT client implementation - helpers for cimmunication part
+ *
+ * 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 packets part of MQTT 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.
+ * Return the number of length bytes. length value is updated.
+ */
+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, will_msg_size = 0;
+ uint16_t 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_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);
+ will_msg_size = strlen(pkt->will_msg);
+ /* Update packet payload size */
+ remaining_length += will_topic_len + 2;
+ remaining_length += 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 - NULL pointers checks already made above */
+ if (will_msg_size != 0) {
+ uint16_t tmp = htons(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, will_msg_size);
+ len += 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 Subscribe or Unsubscribe 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] != MQTT_SUBSCRIBE_FAILURE) && (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);
+}
+
