Linux preempt-rt

Check our new training course

Real-Time Linux with PREEMPT_RT

Check our new training course
with Creative Commons CC-BY-SA
lecture and lab materials

Bootlin logo

Elixir Cross Referencer

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
/*
 * Copyright (c) 2018 Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: Apache-2.0
 */

/** @file mqtt_encoder.c
 *
 * @brief Encoding functions needed to create packet to be sent to the broker.
 */

#include <logging/log.h>
LOG_MODULE_REGISTER(net_mqtt_enc, CONFIG_MQTT_LOG_LEVEL);

#include "mqtt_internal.h"
#include "mqtt_os.h"

static const struct mqtt_utf8 mqtt_3_1_0_proto_desc =
	MQTT_UTF8_LITERAL("MQIsdp");

static const struct mqtt_utf8 mqtt_3_1_1_proto_desc =
	MQTT_UTF8_LITERAL("MQTT");

/** Never changing ping request, needed for Keep Alive. */
static const uint8_t ping_packet[MQTT_FIXED_HEADER_MIN_SIZE] = {
	MQTT_PKT_TYPE_PINGREQ,
	0x00
};

/** Never changing disconnect request. */
static const uint8_t disc_packet[MQTT_FIXED_HEADER_MIN_SIZE] = {
	MQTT_PKT_TYPE_DISCONNECT,
	0x00
};

/**
 * @brief Packs unsigned 8 bit value to the buffer at the offset requested.
 *
 * @param[in] val Value to be packed.
 * @param[inout] buf A pointer to the buf_ctx structure containing current
 *                   buffer position.
 *
 * @retval 0 if procedure is successful.
 * @retval -ENOMEM if there is no place in the buffer to store the value.
 */
static int pack_uint8(uint8_t val, struct buf_ctx *buf)
{
	if ((buf->end - buf->cur) < sizeof(uint8_t)) {
		return -ENOMEM;
	}

	MQTT_TRC(">> val:%02x cur:%p, end:%p", val, buf->cur, buf->end);

	/* Pack value. */
	*(buf->cur++) = val;

	return 0;
}

/**
 * @brief Packs unsigned 16 bit value to the buffer at the offset requested.
 *
 * @param[in] val Value to be packed.
 * @param[inout] buf A pointer to the buf_ctx structure containing current
 *                   buffer position.
 *
 * @retval 0 if the procedure is successful.
 * @retval -ENOMEM if there is no place in the buffer to store the value.
 */
static int pack_uint16(uint16_t val, struct buf_ctx *buf)
{
	if ((buf->end - buf->cur) < sizeof(uint16_t)) {
		return -ENOMEM;
	}

	MQTT_TRC(">> val:%04x cur:%p, end:%p", val, buf->cur, buf->end);

	/* Pack value. */
	*(buf->cur++) = (val >> 8) & 0xFF;
	*(buf->cur++) = val & 0xFF;

	return 0;
}

/**
 * @brief Packs utf8 string to the buffer at the offset requested.
 *
 * @param[in] str UTF-8 string and its length to be packed.
 * @param[inout] buf A pointer to the buf_ctx structure containing current
 *                   buffer position.
 *
 * @retval 0 if the procedure is successful.
 * @retval -ENOMEM if there is no place in the buffer to store the string.
 */
static int pack_utf8_str(const struct mqtt_utf8 *str, struct buf_ctx *buf)
{
	if ((buf->end - buf->cur) < GET_UT8STR_BUFFER_SIZE(str)) {
		return -ENOMEM;
	}

	MQTT_TRC(">> str_size:%08x cur:%p, end:%p",
		 (uint32_t)GET_UT8STR_BUFFER_SIZE(str), buf->cur, buf->end);

	/* Pack length followed by string. */
	(void)pack_uint16(str->size, buf);

	memcpy(buf->cur, str->utf8, str->size);
	buf->cur += str->size;

	return 0;
}

/**
 * @brief Computes and encodes length for the MQTT fixed header.
 *
 * @note The remaining length is not packed as a fixed unsigned 32 bit integer.
 *       Instead it is packed on algorithm below:
 *
 * @code
 * do
 *            encodedByte = X MOD 128
 *            X = X DIV 128
 *            // if there are more data to encode, set the top bit of this byte
 *            if ( X > 0 )
 *                encodedByte = encodedByte OR 128
 *            endif
 *                'output' encodedByte
 *       while ( X > 0 )
 * @endcode
 *
 * @param[in] length Length of variable header and payload in the MQTT message.
 * @param[inout] buf A pointer to the buf_ctx structure containing current
 *                   buffer position. May be NULL (in this case function will
 *                   only calculate number of bytes needed).
 *
 * @return Number of bytes needed to encode length value.
 */
static uint8_t packet_length_encode(uint32_t length, struct buf_ctx *buf)
{
	uint8_t encoded_bytes = 0U;

	MQTT_TRC(">> length:0x%08x cur:%p, end:%p", length,
		 (buf == NULL) ? 0 : buf->cur, (buf == NULL) ? 0 : buf->end);

	do {
		encoded_bytes++;

		if (buf != NULL) {
			*(buf->cur) = length & MQTT_LENGTH_VALUE_MASK;
		}

		length >>= MQTT_LENGTH_SHIFT;

		if (buf != NULL) {
			if (length > 0) {
				*(buf->cur) |= MQTT_LENGTH_CONTINUATION_BIT;
			}
			buf->cur++;
		}
	} while (length > 0);

	return encoded_bytes;
}

/**
 * @brief Encodes fixed header for the MQTT message and provides pointer to
 *        start of the header.
 *
 * @param[in] message_type Message type containing packet type and the flags.
 *                         Use @ref MQTT_MESSAGES_OPTIONS to construct the
 *                         message_type.
 * @param[in] start  Pointer to the start of the variable header.
 * @param[inout] buf Buffer context used to encode the frame.
 *                   The 5 bytes before the start of the message are assumed
 *                   by the routine to be available to pack the fixed header.
 *                   However, since the fixed header length is variable
 *                   length, the pointer to the start of the MQTT message
 *                   along with encoded fixed header is supplied as output
 *                   parameter if the procedure was successful.
 *                   As output, the pointers will point to beginning and the end
 *                   of the frame.
 *
 * @retval 0 if the procedure is successful.
 * @retval -EMSGSIZE if the message is too big for MQTT.
 */
static uint32_t mqtt_encode_fixed_header(uint8_t message_type, uint8_t *start,
				      struct buf_ctx *buf)
{
	uint32_t length = buf->cur - start;
	uint8_t fixed_header_length;

	if (length > MQTT_MAX_PAYLOAD_SIZE) {
		return -EMSGSIZE;
	}

	MQTT_TRC("<< msg type:0x%02x length:0x%08x", message_type, length);

	fixed_header_length = packet_length_encode(length, NULL);
	fixed_header_length += sizeof(uint8_t);

	MQTT_TRC("Fixed header length = %02x", fixed_header_length);

	/* Set the pointer at the start of the frame before encoding. */
	buf->cur = start - fixed_header_length;

	(void)pack_uint8(message_type, buf);
	(void)packet_length_encode(length, buf);

	/* Set the cur pointer back at the start of the frame,
	 * and end pointer to the end of the frame.
	 */
	buf->cur = buf->cur - fixed_header_length;
	buf->end = buf->cur + length + fixed_header_length;

	return 0;
}

/**
 * @brief Encodes a string of a zero length.
 *
 * @param[in] buffer_len Total size of the buffer on which string will be
 *                       encoded. This shall not be zero.
 * @param[inout] buf A pointer to the buf_ctx structure containing current
 *                   buffer position.
 *
 * @retval 0 if the procedure is successful.
 * @retval -ENOMEM if there is no place in the buffer to store the binary
 *                 string.
 */
static int zero_len_str_encode(struct buf_ctx *buf)
{
	return pack_uint16(0x0000, buf);
}

/**
 * @brief Encodes and sends messages that contain only message id in
 *        the variable header.
 *
 * @param[in] message_type Message type and reserved bit fields.
 * @param[in] message_id Message id to be encoded in the variable header.
 * @param[inout] buf_ctx Pointer to the buffer context structure,
 *                       containing buffer for the encoded message.
 *
 * @retval 0 or an error code indicating a reason for failure.
 */
static int mqtt_message_id_only_enc(uint8_t message_type, uint16_t message_id,
				    struct buf_ctx *buf)
{
	int err_code;
	uint8_t *start;

	/* Message id zero is not permitted by spec. */
	if (message_id == 0U) {
		return -EINVAL;
	}

	/* Reserve space for fixed header. */
	buf->cur += MQTT_FIXED_HEADER_MAX_SIZE;
	start = buf->cur;

	err_code = pack_uint16(message_id, buf);
	if (err_code != 0) {
		return err_code;
	}

	return mqtt_encode_fixed_header(message_type, start, buf);
}

int connect_request_encode(const struct mqtt_client *client,
			   struct buf_ctx *buf)
{
	uint8_t connect_flags = client->clean_session << 1;
	const uint8_t message_type =
			MQTT_MESSAGES_OPTIONS(MQTT_PKT_TYPE_CONNECT, 0, 0, 0);
	const struct mqtt_utf8 *mqtt_proto_desc;
	uint8_t *connect_flags_pos;
	int err_code;
	uint8_t *start;

	if (client->protocol_version == MQTT_VERSION_3_1_1) {
		mqtt_proto_desc = &mqtt_3_1_1_proto_desc;
	} else {
		mqtt_proto_desc = &mqtt_3_1_0_proto_desc;
	}

	/* Reserve space for fixed header. */
	buf->cur += MQTT_FIXED_HEADER_MAX_SIZE;
	start = buf->cur;

	MQTT_HEXDUMP_TRC(mqtt_proto_desc->utf8, mqtt_proto_desc->size,
			 "Encoding Protocol Description.");

	err_code = pack_utf8_str(mqtt_proto_desc, buf);
	if (err_code != 0) {
		return err_code;
	}

	MQTT_TRC("Encoding Protocol Version %02x.", client->protocol_version);
	err_code = pack_uint8(client->protocol_version, buf);
	if (err_code != 0) {
		return err_code;
	}

	/* Remember position of connect flag and leave one byte for it to
	 * be packed once we determine its value.
	 */
	connect_flags_pos = buf->cur;

	err_code = pack_uint8(0, buf);
	if (err_code != 0) {
		return err_code;
	}

	MQTT_TRC("Encoding Keep Alive Time %04x.", client->keepalive);
	err_code = pack_uint16(client->keepalive, buf);
	if (err_code != 0) {
		return err_code;
	}

	MQTT_HEXDUMP_TRC(client->client_id.utf8, client->client_id.size,
			 "Encoding Client Id.");
	err_code = pack_utf8_str(&client->client_id, buf);
	if (err_code != 0) {
		return err_code;
	}

	/* Pack will topic and QoS */
	if (client->will_topic != NULL) {
		connect_flags |= MQTT_CONNECT_FLAG_WILL_TOPIC;
		/* QoS is always 1 as of now. */
		connect_flags |= ((client->will_topic->qos & 0x03) << 3);
		connect_flags |= client->will_retain << 5;

		MQTT_HEXDUMP_TRC(client->will_topic->topic.utf8,
				 client->will_topic->topic.size,
				 "Encoding Will Topic.");
		err_code = pack_utf8_str(&client->will_topic->topic, buf);
		if (err_code != 0) {
			return err_code;
		}

		if (client->will_message != NULL) {
			MQTT_HEXDUMP_TRC(client->will_message->utf8,
					 client->will_message->size,
					 "Encoding Will Message.");
			err_code = pack_utf8_str(client->will_message, buf);
			if (err_code != 0) {
				return err_code;
			}
		} else {
			MQTT_TRC("Encoding Zero Length Will Message.");
			err_code = zero_len_str_encode(buf);
			if (err_code != 0) {
				return err_code;
			}
		}
	}

	/* Pack Username if any. */
	if (client->user_name != NULL) {
		connect_flags |= MQTT_CONNECT_FLAG_USERNAME;

		MQTT_HEXDUMP_TRC(client->user_name->utf8,
				 client->user_name->size,
				 "Encoding Username.");
		err_code = pack_utf8_str(client->user_name, buf);
		if (err_code != 0) {
			return err_code;
		}
	}

	/* Pack Password if any. */
	if (client->password != NULL) {
		connect_flags |= MQTT_CONNECT_FLAG_PASSWORD;

		MQTT_HEXDUMP_TRC(client->password->utf8,
				 client->password->size,
				 "Encoding Password.");
		err_code = pack_utf8_str(client->password, buf);
		if (err_code != 0) {
			return err_code;
		}
	}

	/* Write the flags the connect flags. */
	*connect_flags_pos = connect_flags;

	return mqtt_encode_fixed_header(message_type, start, buf);
}

int publish_encode(const struct mqtt_publish_param *param, struct buf_ctx *buf)
{
	const uint8_t message_type = MQTT_MESSAGES_OPTIONS(
			MQTT_PKT_TYPE_PUBLISH, param->dup_flag,
			param->message.topic.qos, param->retain_flag);
	int err_code;
	uint8_t *start;

	/* Message id zero is not permitted by spec. */
	if ((param->message.topic.qos) && (param->message_id == 0U)) {
		return -EINVAL;
	}

	/* Reserve space for fixed header. */
	buf->cur += MQTT_FIXED_HEADER_MAX_SIZE;
	start = buf->cur;

	err_code = pack_utf8_str(&param->message.topic.topic, buf);
	if (err_code != 0) {
		return err_code;
	}

	if (param->message.topic.qos) {
		err_code = pack_uint16(param->message_id, buf);
		if (err_code != 0) {
			return err_code;
		}
	}

	/* Do not copy payload. We move the buffer pointer to ensure that
	 * message length in fixed header is encoded correctly.
	 */
	buf->cur += param->message.payload.len;

	err_code = mqtt_encode_fixed_header(message_type, start, buf);
	if (err_code != 0) {
		return err_code;
	}

	buf->end -= param->message.payload.len;

	return 0;
}

int publish_ack_encode(const struct mqtt_puback_param *param,
		       struct buf_ctx *buf)
{
	const uint8_t message_type =
		MQTT_MESSAGES_OPTIONS(MQTT_PKT_TYPE_PUBACK, 0, 0, 0);

	return mqtt_message_id_only_enc(message_type, param->message_id, buf);
}

int publish_receive_encode(const struct mqtt_pubrec_param *param,
			   struct buf_ctx *buf)
{
	const uint8_t message_type =
		MQTT_MESSAGES_OPTIONS(MQTT_PKT_TYPE_PUBREC, 0, 0, 0);

	return mqtt_message_id_only_enc(message_type, param->message_id, buf);
}

int publish_release_encode(const struct mqtt_pubrel_param *param,
			   struct buf_ctx *buf)
{
	const uint8_t message_type =
		MQTT_MESSAGES_OPTIONS(MQTT_PKT_TYPE_PUBREL, 0, 1, 0);

	return mqtt_message_id_only_enc(message_type, param->message_id, buf);
}

int publish_complete_encode(const struct mqtt_pubcomp_param *param,
			    struct buf_ctx *buf)
{
	const uint8_t message_type =
		MQTT_MESSAGES_OPTIONS(MQTT_PKT_TYPE_PUBCOMP, 0, 0, 0);

	return mqtt_message_id_only_enc(message_type, param->message_id, buf);
}

int disconnect_encode(struct buf_ctx *buf)
{
	if (buf->end - buf->cur < sizeof(disc_packet)) {
		return -ENOMEM;
	}

	memcpy(buf->cur, disc_packet, sizeof(disc_packet));
	buf->end = buf->cur + sizeof(disc_packet);

	return 0;
}

int subscribe_encode(const struct mqtt_subscription_list *param,
		     struct buf_ctx *buf)
{
	const uint8_t message_type = MQTT_MESSAGES_OPTIONS(
			MQTT_PKT_TYPE_SUBSCRIBE, 0, 1, 0);
	int err_code, i;
	uint8_t *start;

	/* Message id zero is not permitted by spec. */
	if (param->message_id == 0U) {
		return -EINVAL;
	}

	/* Reserve space for fixed header. */
	buf->cur += MQTT_FIXED_HEADER_MAX_SIZE;
	start = buf->cur;

	err_code = pack_uint16(param->message_id, buf);
	if (err_code != 0) {
		return err_code;
	}

	for (i = 0; i < param->list_count; i++) {
		err_code = pack_utf8_str(&param->list[i].topic, buf);
		if (err_code != 0) {
			return err_code;
		}

		err_code = pack_uint8(param->list[i].qos, buf);
		if (err_code != 0) {
			return err_code;
		}
	}

	return mqtt_encode_fixed_header(message_type, start, buf);
}

int unsubscribe_encode(const struct mqtt_subscription_list *param,
		       struct buf_ctx *buf)
{
	const uint8_t message_type = MQTT_MESSAGES_OPTIONS(
		MQTT_PKT_TYPE_UNSUBSCRIBE, 0, MQTT_QOS_1_AT_LEAST_ONCE, 0);
	int err_code, i;
	uint8_t *start;

	/* Reserve space for fixed header. */
	buf->cur += MQTT_FIXED_HEADER_MAX_SIZE;
	start = buf->cur;

	err_code = pack_uint16(param->message_id, buf);
	if (err_code != 0) {
		return err_code;
	}

	for (i = 0; i < param->list_count; i++) {
		err_code = pack_utf8_str(&param->list[i].topic, buf);
		if (err_code != 0) {
			return err_code;
		}
	}

	return mqtt_encode_fixed_header(message_type, start, buf);
}

int ping_request_encode(struct buf_ctx *buf)
{
	if (buf->end - buf->cur < sizeof(ping_packet)) {
		return -ENOMEM;
	}

	memcpy(buf->cur, ping_packet, sizeof(ping_packet));
	buf->end = buf->cur + sizeof(ping_packet);

	return 0;
}