1 /*
2 * Copyright (c) 2018 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 /** @file mqtt_encoder.c
8 *
9 * @brief Encoding functions needed to create packet to be sent to the broker.
10 */
11
12 #include <zephyr/logging/log.h>
13 LOG_MODULE_REGISTER(net_mqtt_enc, CONFIG_MQTT_LOG_LEVEL);
14
15 #include "mqtt_internal.h"
16 #include "mqtt_os.h"
17
18 static const struct mqtt_utf8 mqtt_3_1_0_proto_desc =
19 MQTT_UTF8_LITERAL("MQIsdp");
20
21 static const struct mqtt_utf8 mqtt_3_1_1_proto_desc =
22 MQTT_UTF8_LITERAL("MQTT");
23
24 /** Never changing ping request, needed for Keep Alive. */
25 static const uint8_t ping_packet[MQTT_FIXED_HEADER_MIN_SIZE] = {
26 MQTT_PKT_TYPE_PINGREQ,
27 0x00
28 };
29
30 /** Never changing disconnect request. */
31 static const uint8_t disc_packet[MQTT_FIXED_HEADER_MIN_SIZE] = {
32 MQTT_PKT_TYPE_DISCONNECT,
33 0x00
34 };
35
36 /**
37 * @brief Packs unsigned 8 bit value to the buffer at the offset requested.
38 *
39 * @param[in] val Value to be packed.
40 * @param[inout] buf A pointer to the buf_ctx structure containing current
41 * buffer position.
42 *
43 * @retval 0 if procedure is successful.
44 * @retval -ENOMEM if there is no place in the buffer to store the value.
45 */
pack_uint8(uint8_t val,struct buf_ctx * buf)46 static int pack_uint8(uint8_t val, struct buf_ctx *buf)
47 {
48 uint8_t *cur = buf->cur;
49 uint8_t *end = buf->end;
50
51 if ((end - cur) < sizeof(uint8_t)) {
52 return -ENOMEM;
53 }
54
55 NET_DBG(">> val:%02x cur:%p, end:%p", val, (void *)cur, (void *)end);
56
57 /* Pack value. */
58 cur[0] = val;
59 buf->cur = (cur + sizeof(uint8_t));
60
61 return 0;
62 }
63
64 /**
65 * @brief Packs unsigned 16 bit value to the buffer at the offset requested.
66 *
67 * @param[in] val Value to be packed.
68 * @param[inout] buf A pointer to the buf_ctx structure containing current
69 * buffer position.
70 *
71 * @retval 0 if the procedure is successful.
72 * @retval -ENOMEM if there is no place in the buffer to store the value.
73 */
pack_uint16(uint16_t val,struct buf_ctx * buf)74 static int pack_uint16(uint16_t val, struct buf_ctx *buf)
75 {
76 uint8_t *cur = buf->cur;
77 uint8_t *end = buf->end;
78
79 if ((end - cur) < sizeof(uint16_t)) {
80 return -ENOMEM;
81 }
82
83 NET_DBG(">> val:%04x cur:%p, end:%p", val, (void *)cur, (void *)end);
84
85 /* Pack value. */
86 sys_put_be16(val, cur);
87 buf->cur = (cur + sizeof(uint16_t));
88
89 return 0;
90 }
91
92 /**
93 * @brief Packs utf8 string to the buffer at the offset requested.
94 *
95 * @param[in] str UTF-8 string and its length to be packed.
96 * @param[inout] buf A pointer to the buf_ctx structure containing current
97 * buffer position.
98 *
99 * @retval 0 if the procedure is successful.
100 * @retval -ENOMEM if there is no place in the buffer to store the string.
101 */
pack_utf8_str(const struct mqtt_utf8 * str,struct buf_ctx * buf)102 static int pack_utf8_str(const struct mqtt_utf8 *str, struct buf_ctx *buf)
103 {
104 if ((buf->end - buf->cur) < GET_UT8STR_BUFFER_SIZE(str)) {
105 return -ENOMEM;
106 }
107
108 NET_DBG(">> str_size:%08x cur:%p, end:%p",
109 (uint32_t)GET_UT8STR_BUFFER_SIZE(str), (void *)buf->cur, (void *)buf->end);
110
111 /* Pack length followed by string. */
112 (void)pack_uint16(str->size, buf);
113
114 memcpy(buf->cur, str->utf8, str->size);
115 buf->cur += str->size;
116
117 return 0;
118 }
119
120 /**
121 * @brief Computes and encodes length for the MQTT fixed header.
122 *
123 * @note The remaining length is not packed as a fixed unsigned 32 bit integer.
124 * Instead it is packed on algorithm below:
125 *
126 * @code
127 * do
128 * encodedByte = X MOD 128
129 * X = X DIV 128
130 * // if there are more data to encode, set the top bit of this byte
131 * if ( X > 0 )
132 * encodedByte = encodedByte OR 128
133 * endif
134 * 'output' encodedByte
135 * while ( X > 0 )
136 * @endcode
137 *
138 * @param[in] length Length of variable header and payload in the MQTT message.
139 * @param[inout] buf A pointer to the buf_ctx structure containing current
140 * buffer position. May be NULL (in this case function will
141 * only calculate number of bytes needed).
142 *
143 * @return Number of bytes needed to encode length value.
144 */
packet_length_encode(uint32_t length,struct buf_ctx * buf)145 static uint8_t packet_length_encode(uint32_t length, struct buf_ctx *buf)
146 {
147 uint8_t encoded_bytes = 0U;
148
149 NET_DBG(">> length:0x%08x cur:%p, end:%p", length,
150 (buf == NULL) ? 0 : (void *)buf->cur, (buf == NULL) ? 0 : (void *)buf->end);
151
152 do {
153 encoded_bytes++;
154
155 if (buf != NULL) {
156 *(buf->cur) = length & MQTT_LENGTH_VALUE_MASK;
157 }
158
159 length >>= MQTT_LENGTH_SHIFT;
160
161 if (buf != NULL) {
162 if (length > 0) {
163 *(buf->cur) |= MQTT_LENGTH_CONTINUATION_BIT;
164 }
165 buf->cur++;
166 }
167 } while (length > 0);
168
169 return encoded_bytes;
170 }
171
172 /**
173 * @brief Encodes fixed header for the MQTT message and provides pointer to
174 * start of the header.
175 *
176 * @param[in] message_type Message type containing packet type and the flags.
177 * Use @ref MQTT_MESSAGES_OPTIONS to construct the
178 * message_type.
179 * @param[in] start Pointer to the start of the variable header.
180 * @param[inout] buf Buffer context used to encode the frame.
181 * The 5 bytes before the start of the message are assumed
182 * by the routine to be available to pack the fixed header.
183 * However, since the fixed header length is variable
184 * length, the pointer to the start of the MQTT message
185 * along with encoded fixed header is supplied as output
186 * parameter if the procedure was successful.
187 * As output, the pointers will point to beginning and the end
188 * of the frame.
189 *
190 * @retval 0 if the procedure is successful.
191 * @retval -EMSGSIZE if the message is too big for MQTT.
192 */
mqtt_encode_fixed_header(uint8_t message_type,uint8_t * start,struct buf_ctx * buf)193 static uint32_t mqtt_encode_fixed_header(uint8_t message_type, uint8_t *start,
194 struct buf_ctx *buf)
195 {
196 uint32_t length = buf->cur - start;
197 uint8_t fixed_header_length;
198
199 if (length > MQTT_MAX_PAYLOAD_SIZE) {
200 return -EMSGSIZE;
201 }
202
203 NET_DBG("<< msg type:0x%02x length:0x%08x", message_type, length);
204
205 fixed_header_length = packet_length_encode(length, NULL);
206 fixed_header_length += sizeof(uint8_t);
207
208 NET_DBG("Fixed header length = %02x", fixed_header_length);
209
210 /* Set the pointer at the start of the frame before encoding. */
211 buf->cur = start - fixed_header_length;
212
213 (void)pack_uint8(message_type, buf);
214 (void)packet_length_encode(length, buf);
215
216 /* Set the cur pointer back at the start of the frame,
217 * and end pointer to the end of the frame.
218 */
219 buf->cur = buf->cur - fixed_header_length;
220 buf->end = buf->cur + length + fixed_header_length;
221
222 return 0;
223 }
224
225 /**
226 * @brief Encodes a string of a zero length.
227 *
228 * @param[in] buffer_len Total size of the buffer on which string will be
229 * encoded. This shall not be zero.
230 * @param[inout] buf A pointer to the buf_ctx structure containing current
231 * buffer position.
232 *
233 * @retval 0 if the procedure is successful.
234 * @retval -ENOMEM if there is no place in the buffer to store the binary
235 * string.
236 */
zero_len_str_encode(struct buf_ctx * buf)237 static int zero_len_str_encode(struct buf_ctx *buf)
238 {
239 return pack_uint16(0x0000, buf);
240 }
241
242 /**
243 * @brief Encodes and sends messages that contain only message id in
244 * the variable header.
245 *
246 * @param[in] message_type Message type and reserved bit fields.
247 * @param[in] message_id Message id to be encoded in the variable header.
248 * @param[inout] buf_ctx Pointer to the buffer context structure,
249 * containing buffer for the encoded message.
250 *
251 * @retval 0 or an error code indicating a reason for failure.
252 */
mqtt_message_id_only_enc(uint8_t message_type,uint16_t message_id,struct buf_ctx * buf)253 static int mqtt_message_id_only_enc(uint8_t message_type, uint16_t message_id,
254 struct buf_ctx *buf)
255 {
256 int err_code;
257 uint8_t *start;
258
259 /* Message id zero is not permitted by spec. */
260 if (message_id == 0U) {
261 return -EINVAL;
262 }
263
264 /* Reserve space for fixed header. */
265 buf->cur += MQTT_FIXED_HEADER_MAX_SIZE;
266 start = buf->cur;
267
268 err_code = pack_uint16(message_id, buf);
269 if (err_code != 0) {
270 return err_code;
271 }
272
273 return mqtt_encode_fixed_header(message_type, start, buf);
274 }
275
connect_request_encode(const struct mqtt_client * client,struct buf_ctx * buf)276 int connect_request_encode(const struct mqtt_client *client,
277 struct buf_ctx *buf)
278 {
279 uint8_t connect_flags = client->clean_session << 1;
280 const uint8_t message_type =
281 MQTT_MESSAGES_OPTIONS(MQTT_PKT_TYPE_CONNECT, 0, 0, 0);
282 const struct mqtt_utf8 *mqtt_proto_desc;
283 uint8_t *connect_flags_pos;
284 int err_code;
285 uint8_t *start;
286
287 if (client->protocol_version == MQTT_VERSION_3_1_1) {
288 mqtt_proto_desc = &mqtt_3_1_1_proto_desc;
289 } else {
290 mqtt_proto_desc = &mqtt_3_1_0_proto_desc;
291 }
292
293 /* Reserve space for fixed header. */
294 buf->cur += MQTT_FIXED_HEADER_MAX_SIZE;
295 start = buf->cur;
296
297 NET_HEXDUMP_DBG(mqtt_proto_desc->utf8, mqtt_proto_desc->size,
298 "Encoding Protocol Description.");
299
300 err_code = pack_utf8_str(mqtt_proto_desc, buf);
301 if (err_code != 0) {
302 return err_code;
303 }
304
305 NET_DBG("Encoding Protocol Version %02x.", client->protocol_version);
306 err_code = pack_uint8(client->protocol_version, buf);
307 if (err_code != 0) {
308 return err_code;
309 }
310
311 /* Remember position of connect flag and leave one byte for it to
312 * be packed once we determine its value.
313 */
314 connect_flags_pos = buf->cur;
315
316 err_code = pack_uint8(0, buf);
317 if (err_code != 0) {
318 return err_code;
319 }
320
321 NET_DBG("Encoding Keep Alive Time %04x.", client->keepalive);
322 err_code = pack_uint16(client->keepalive, buf);
323 if (err_code != 0) {
324 return err_code;
325 }
326
327 NET_HEXDUMP_DBG(client->client_id.utf8, client->client_id.size,
328 "Encoding Client Id.");
329 err_code = pack_utf8_str(&client->client_id, buf);
330 if (err_code != 0) {
331 return err_code;
332 }
333
334 /* Pack will topic and QoS */
335 if (client->will_topic != NULL) {
336 connect_flags |= MQTT_CONNECT_FLAG_WILL_TOPIC;
337 /* QoS is always 1 as of now. */
338 connect_flags |= ((client->will_topic->qos & 0x03) << 3);
339 connect_flags |= client->will_retain << 5;
340
341 NET_HEXDUMP_DBG(client->will_topic->topic.utf8,
342 client->will_topic->topic.size,
343 "Encoding Will Topic.");
344 err_code = pack_utf8_str(&client->will_topic->topic, buf);
345 if (err_code != 0) {
346 return err_code;
347 }
348
349 if (client->will_message != NULL) {
350 NET_HEXDUMP_DBG(client->will_message->utf8,
351 client->will_message->size,
352 "Encoding Will Message.");
353 err_code = pack_utf8_str(client->will_message, buf);
354 if (err_code != 0) {
355 return err_code;
356 }
357 } else {
358 NET_DBG("Encoding Zero Length Will Message.");
359 err_code = zero_len_str_encode(buf);
360 if (err_code != 0) {
361 return err_code;
362 }
363 }
364 }
365
366 /* Pack Username if any. */
367 if (client->user_name != NULL) {
368 connect_flags |= MQTT_CONNECT_FLAG_USERNAME;
369
370 NET_HEXDUMP_DBG(client->user_name->utf8,
371 client->user_name->size,
372 "Encoding Username.");
373 err_code = pack_utf8_str(client->user_name, buf);
374 if (err_code != 0) {
375 return err_code;
376 }
377 }
378
379 /* Pack Password if any. */
380 if (client->password != NULL) {
381 connect_flags |= MQTT_CONNECT_FLAG_PASSWORD;
382
383 NET_HEXDUMP_DBG(client->password->utf8,
384 client->password->size,
385 "Encoding Password.");
386 err_code = pack_utf8_str(client->password, buf);
387 if (err_code != 0) {
388 return err_code;
389 }
390 }
391
392 /* Write the flags the connect flags. */
393 *connect_flags_pos = connect_flags;
394
395 return mqtt_encode_fixed_header(message_type, start, buf);
396 }
397
publish_encode(const struct mqtt_publish_param * param,struct buf_ctx * buf)398 int publish_encode(const struct mqtt_publish_param *param, struct buf_ctx *buf)
399 {
400 const uint8_t message_type = MQTT_MESSAGES_OPTIONS(
401 MQTT_PKT_TYPE_PUBLISH, param->dup_flag,
402 param->message.topic.qos, param->retain_flag);
403 int err_code;
404 uint8_t *start;
405
406 /* Message id zero is not permitted by spec. */
407 if ((param->message.topic.qos) && (param->message_id == 0U)) {
408 return -EINVAL;
409 }
410
411 /* Reserve space for fixed header. */
412 buf->cur += MQTT_FIXED_HEADER_MAX_SIZE;
413 start = buf->cur;
414
415 err_code = pack_utf8_str(¶m->message.topic.topic, buf);
416 if (err_code != 0) {
417 return err_code;
418 }
419
420 if (param->message.topic.qos) {
421 err_code = pack_uint16(param->message_id, buf);
422 if (err_code != 0) {
423 return err_code;
424 }
425 }
426
427 /* Do not copy payload. We move the buffer pointer to ensure that
428 * message length in fixed header is encoded correctly.
429 */
430 buf->cur += param->message.payload.len;
431
432 err_code = mqtt_encode_fixed_header(message_type, start, buf);
433 if (err_code != 0) {
434 return err_code;
435 }
436
437 buf->end -= param->message.payload.len;
438
439 return 0;
440 }
441
publish_ack_encode(const struct mqtt_puback_param * param,struct buf_ctx * buf)442 int publish_ack_encode(const struct mqtt_puback_param *param,
443 struct buf_ctx *buf)
444 {
445 const uint8_t message_type =
446 MQTT_MESSAGES_OPTIONS(MQTT_PKT_TYPE_PUBACK, 0, 0, 0);
447
448 return mqtt_message_id_only_enc(message_type, param->message_id, buf);
449 }
450
publish_receive_encode(const struct mqtt_pubrec_param * param,struct buf_ctx * buf)451 int publish_receive_encode(const struct mqtt_pubrec_param *param,
452 struct buf_ctx *buf)
453 {
454 const uint8_t message_type =
455 MQTT_MESSAGES_OPTIONS(MQTT_PKT_TYPE_PUBREC, 0, 0, 0);
456
457 return mqtt_message_id_only_enc(message_type, param->message_id, buf);
458 }
459
publish_release_encode(const struct mqtt_pubrel_param * param,struct buf_ctx * buf)460 int publish_release_encode(const struct mqtt_pubrel_param *param,
461 struct buf_ctx *buf)
462 {
463 const uint8_t message_type =
464 MQTT_MESSAGES_OPTIONS(MQTT_PKT_TYPE_PUBREL, 0, 1, 0);
465
466 return mqtt_message_id_only_enc(message_type, param->message_id, buf);
467 }
468
publish_complete_encode(const struct mqtt_pubcomp_param * param,struct buf_ctx * buf)469 int publish_complete_encode(const struct mqtt_pubcomp_param *param,
470 struct buf_ctx *buf)
471 {
472 const uint8_t message_type =
473 MQTT_MESSAGES_OPTIONS(MQTT_PKT_TYPE_PUBCOMP, 0, 0, 0);
474
475 return mqtt_message_id_only_enc(message_type, param->message_id, buf);
476 }
477
disconnect_encode(struct buf_ctx * buf)478 int disconnect_encode(struct buf_ctx *buf)
479 {
480 uint8_t *cur = buf->cur;
481 uint8_t *end = buf->end;
482
483 if ((end - cur) < sizeof(disc_packet)) {
484 return -ENOMEM;
485 }
486
487 memcpy(cur, disc_packet, sizeof(disc_packet));
488 buf->end = (cur + sizeof(disc_packet));
489
490 return 0;
491 }
492
subscribe_encode(const struct mqtt_subscription_list * param,struct buf_ctx * buf)493 int subscribe_encode(const struct mqtt_subscription_list *param,
494 struct buf_ctx *buf)
495 {
496 const uint8_t message_type = MQTT_MESSAGES_OPTIONS(
497 MQTT_PKT_TYPE_SUBSCRIBE, 0, 1, 0);
498 int err_code, i;
499 uint8_t *start;
500
501 /* Message id zero is not permitted by spec. */
502 if (param->message_id == 0U) {
503 return -EINVAL;
504 }
505
506 /* Reserve space for fixed header. */
507 buf->cur += MQTT_FIXED_HEADER_MAX_SIZE;
508 start = buf->cur;
509
510 err_code = pack_uint16(param->message_id, buf);
511 if (err_code != 0) {
512 return err_code;
513 }
514
515 for (i = 0; i < param->list_count; i++) {
516 err_code = pack_utf8_str(¶m->list[i].topic, buf);
517 if (err_code != 0) {
518 return err_code;
519 }
520
521 err_code = pack_uint8(param->list[i].qos, buf);
522 if (err_code != 0) {
523 return err_code;
524 }
525 }
526
527 return mqtt_encode_fixed_header(message_type, start, buf);
528 }
529
unsubscribe_encode(const struct mqtt_subscription_list * param,struct buf_ctx * buf)530 int unsubscribe_encode(const struct mqtt_subscription_list *param,
531 struct buf_ctx *buf)
532 {
533 const uint8_t message_type = MQTT_MESSAGES_OPTIONS(
534 MQTT_PKT_TYPE_UNSUBSCRIBE, 0, MQTT_QOS_1_AT_LEAST_ONCE, 0);
535 int err_code, i;
536 uint8_t *start;
537
538 /* Reserve space for fixed header. */
539 buf->cur += MQTT_FIXED_HEADER_MAX_SIZE;
540 start = buf->cur;
541
542 err_code = pack_uint16(param->message_id, buf);
543 if (err_code != 0) {
544 return err_code;
545 }
546
547 for (i = 0; i < param->list_count; i++) {
548 err_code = pack_utf8_str(¶m->list[i].topic, buf);
549 if (err_code != 0) {
550 return err_code;
551 }
552 }
553
554 return mqtt_encode_fixed_header(message_type, start, buf);
555 }
556
ping_request_encode(struct buf_ctx * buf)557 int ping_request_encode(struct buf_ctx *buf)
558 {
559 uint8_t *cur = buf->cur;
560 uint8_t *end = buf->end;
561
562 if ((end - cur) < sizeof(ping_packet)) {
563 return -ENOMEM;
564 }
565
566 memcpy(cur, ping_packet, sizeof(ping_packet));
567 buf->end = (cur + sizeof(ping_packet));
568
569 return 0;
570 }
571
