lwmqtt_err_t lwmqtt_encode_unsubscribe(uint8_t *buf, size_t buf_len, size_t *len, uint16_t packet_id, int count, lwmqtt_string_t *topic_filters) { // prepare pointer uint8_t *buf_ptr = buf; uint8_t *buf_end = buf + buf_len; // calculate remaining length uint32_t rem_len = 2; for (int i = 0; i < count; i++) { rem_len += 2 + topic_filters[i].len; } // check remaining length length int rem_len_len; lwmqtt_err_t err = lwmqtt_varnum_length(rem_len, &rem_len_len); if (err == LWMQTT_VARNUM_OVERFLOW) { return LWMQTT_REMAINING_LENGTH_OVERFLOW; } // prepare header uint8_t header = 0; // set packet type lwmqtt_write_bits(&header, LWMQTT_UNSUBSCRIBE_PACKET, 4, 4); // set qos lwmqtt_write_bits(&header, LWMQTT_QOS1, 1, 2); // write header err = lwmqtt_write_byte(&buf_ptr, buf_end, header); if (err != LWMQTT_SUCCESS) { return err; } // write remaining length err = lwmqtt_write_varnum(&buf_ptr, buf_end, rem_len); if (err != LWMQTT_SUCCESS) { return err; } // write packet id err = lwmqtt_write_num(&buf_ptr, buf_end, packet_id); if (err != LWMQTT_SUCCESS) { return err; } // write topics for (int i = 0; i < count; i++) { err = lwmqtt_write_string(&buf_ptr, buf_end, topic_filters[i]); if (err != LWMQTT_SUCCESS) { return err; } } // set length *len = buf_ptr - buf; return LWMQTT_SUCCESS; }
lwmqtt_err_t lwmqtt_write_string(uint8_t **buf, const uint8_t *buf_end, lwmqtt_string_t str) { // write string length lwmqtt_err_t err = lwmqtt_write_num(buf, buf_end, str.len); if (err != LWMQTT_SUCCESS) { return err; } // write data err = lwmqtt_write_data(buf, buf_end, (uint8_t *)str.data, str.len); if (err != LWMQTT_SUCCESS) { return err; } return LWMQTT_SUCCESS; }
lwmqtt_err_t lwmqtt_encode_ack(uint8_t *buf, size_t buf_len, size_t *len, lwmqtt_packet_type_t packet_type, bool dup, uint16_t packet_id) { // prepare pointer uint8_t *buf_ptr = buf; uint8_t *buf_end = buf + buf_len; // prepare header uint8_t header = 0; // set packet type lwmqtt_write_bits(&header, packet_type, 4, 4); // set dup lwmqtt_write_bits(&header, (uint8_t)(dup), 3, 1); // set qos lwmqtt_write_bits(&header, (uint8_t)(packet_type == LWMQTT_PUBREL_PACKET ? LWMQTT_QOS1 : LWMQTT_QOS0), 1, 2); // write header lwmqtt_err_t err = lwmqtt_write_byte(&buf_ptr, buf_end, header); if (err != LWMQTT_SUCCESS) { return err; } // write remaining length err = lwmqtt_write_varnum(&buf_ptr, buf_end, 2); if (err != LWMQTT_SUCCESS) { return err; } // write packet id err = lwmqtt_write_num(&buf_ptr, buf_end, packet_id); if (err != LWMQTT_SUCCESS) { return err; } // set written length *len = buf_ptr - buf; return LWMQTT_SUCCESS; }
lwmqtt_err_t lwmqtt_encode_connect(uint8_t *buf, size_t buf_len, size_t *len, lwmqtt_options_t options, lwmqtt_will_t *will) { // prepare pointers uint8_t *buf_ptr = buf; uint8_t *buf_end = buf + buf_len; // fixed header is 10 uint32_t rem_len = 10; // add client id to remaining length rem_len += options.client_id.len + 2; // add will if present to remaining length if (will != NULL) { rem_len += will->topic.len + 2 + will->payload.len + 2; } // add username if present to remaining length if (options.username.len > 0) { rem_len += options.username.len + 2; // add password if present to remaining length if (options.password.len > 0) { rem_len += options.password.len + 2; } } // check remaining length length int rem_len_len; lwmqtt_err_t err = lwmqtt_varnum_length(rem_len, &rem_len_len); if (err == LWMQTT_VARNUM_OVERFLOW) { return LWMQTT_REMAINING_LENGTH_OVERFLOW; } // prepare header uint8_t header = 0; lwmqtt_write_bits(&header, LWMQTT_CONNECT_PACKET, 4, 4); // write header err = lwmqtt_write_byte(&buf_ptr, buf_end, header); if (err != LWMQTT_SUCCESS) { return err; } // write remaining length err = lwmqtt_write_varnum(&buf_ptr, buf_end, rem_len); if (err != LWMQTT_SUCCESS) { return err; } // write version string err = lwmqtt_write_string(&buf_ptr, buf_end, lwmqtt_string("MQTT")); if (err != LWMQTT_SUCCESS) { return err; } // write version number err = lwmqtt_write_byte(&buf_ptr, buf_end, 4); if (err != LWMQTT_SUCCESS) { return err; } // prepare flags uint8_t flags = 0; // set clean session lwmqtt_write_bits(&flags, (uint8_t)(options.clean_session), 1, 1); // set will flags if present if (will != NULL) { lwmqtt_write_bits(&flags, 1, 2, 1); lwmqtt_write_bits(&flags, will->qos, 3, 2); lwmqtt_write_bits(&flags, (uint8_t)(will->retained), 5, 1); } // set username flag if present if (options.username.len > 0) { lwmqtt_write_bits(&flags, 1, 7, 1); // set password flag if present if (options.password.len > 0) { lwmqtt_write_bits(&flags, 1, 6, 1); } } // write flags err = lwmqtt_write_byte(&buf_ptr, buf_end, flags); if (err != LWMQTT_SUCCESS) { return err; } // write keep alive err = lwmqtt_write_num(&buf_ptr, buf_end, options.keep_alive); if (err != LWMQTT_SUCCESS) { return err; } // write client id err = lwmqtt_write_string(&buf_ptr, buf_end, options.client_id); if (err != LWMQTT_SUCCESS) { return err; } // write will if present if (will != NULL) { // write topic err = lwmqtt_write_string(&buf_ptr, buf_end, will->topic); if (err != LWMQTT_SUCCESS) { return err; } // write payload length err = lwmqtt_write_num(&buf_ptr, buf_end, (uint16_t)will->payload.len); if (err != LWMQTT_SUCCESS) { return err; } // write payload err = lwmqtt_write_data(&buf_ptr, buf_end, (uint8_t *)will->payload.data, will->payload.len); if (err != LWMQTT_SUCCESS) { return err; } } // write username if present if (options.username.len > 0) { err = lwmqtt_write_string(&buf_ptr, buf_end, options.username); if (err != LWMQTT_SUCCESS) { return err; } } // write password if present if (options.username.len > 0 && options.password.len > 0) { err = lwmqtt_write_string(&buf_ptr, buf_end, options.password); if (err != LWMQTT_SUCCESS) { return err; } } // set written length *len = buf_ptr - buf; return LWMQTT_SUCCESS; }
lwmqtt_err_t lwmqtt_encode_publish(uint8_t *buf, size_t buf_len, size_t *len, bool dup, uint16_t packet_id, lwmqtt_string_t topic, lwmqtt_message_t msg) { // prepare pointer uint8_t *buf_ptr = buf; uint8_t *buf_end = buf + buf_len; // calculate remaining length uint32_t rem_len = 2 + topic.len + (uint32_t)msg.payload_len; if (msg.qos > 0) { rem_len += 2; } // check remaining length length int rem_len_len; lwmqtt_err_t err = lwmqtt_varnum_length(rem_len, &rem_len_len); if (err == LWMQTT_VARNUM_OVERFLOW) { return LWMQTT_REMAINING_LENGTH_OVERFLOW; } // prepare header uint8_t header = 0; // set packet type lwmqtt_write_bits(&header, LWMQTT_PUBLISH_PACKET, 4, 4); // set dup lwmqtt_write_bits(&header, (uint8_t)(dup), 3, 1); // set qos lwmqtt_write_bits(&header, msg.qos, 1, 2); // set retained lwmqtt_write_bits(&header, (uint8_t)(msg.retained), 0, 1); // write header err = lwmqtt_write_byte(&buf_ptr, buf_end, header); if (err != LWMQTT_SUCCESS) { return err; } // write remaining length err = lwmqtt_write_varnum(&buf_ptr, buf_end, rem_len); if (err != LWMQTT_SUCCESS) { return err; } // write topic err = lwmqtt_write_string(&buf_ptr, buf_end, topic); if (err != LWMQTT_SUCCESS) { return err; } // write packet id if qos is at least 1 if (msg.qos > 0) { err = lwmqtt_write_num(&buf_ptr, buf_end, packet_id); if (err != LWMQTT_SUCCESS) { return err; } } // write payload err = lwmqtt_write_data(&buf_ptr, buf_end, msg.payload, msg.payload_len); if (err != LWMQTT_SUCCESS) { return err; } // set length *len = buf_ptr - buf; return LWMQTT_SUCCESS; }