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_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;
}
