1
+#define _GNU_SOURCE
2
+
3
+#include <stdint.h>
4
+#include <stdlib.h>
5
+#include <string.h>
6
+#include <pthread.h>
7
+#include <inttypes.h>
8
+
9
+#include "c_rhash.h"
10
+
11
+#include "common_internal.h"
12
+#include "mqtt_constants.h"
13
+#include "mqtt_wss_log.h"
14
+#include "mqtt_ng.h"
15
+
16
+#define UNIT_LOG_PREFIX "mqtt_client: "
17
+#define FATAL(fmt, ...) mws_fatal(client->log, UNIT_LOG_PREFIX fmt, ##__VA_ARGS__)
18
+#define ERROR(fmt, ...) mws_error(client->log, UNIT_LOG_PREFIX fmt, ##__VA_ARGS__)
19
+#define WARN(fmt, ...) mws_warn (client->log, UNIT_LOG_PREFIX fmt, ##__VA_ARGS__)
20
+#define INFO(fmt, ...) mws_info (client->log, UNIT_LOG_PREFIX fmt, ##__VA_ARGS__)
21
+#define DEBUG(fmt, ...) mws_debug(client->log, UNIT_LOG_PREFIX fmt, ##__VA_ARGS__)
22
+
23
+#define SMALL_STRING_DONT_FRAGMENT_LIMIT 128
24
+
25
+#define MIN(a,b) (((a)<(b))?(a):(b))
26
+
27
+#define LOCK_HDR_BUFFER(buffer) pthread_mutex_lock(&((buffer)->mutex))
28
+#define UNLOCK_HDR_BUFFER(buffer) pthread_mutex_unlock(&((buffer)->mutex))
29
+
30
+#define BUFFER_FRAG_GARBAGE_COLLECT 0x01
31
+// some packets can be marked for garbage collection
32
+// immediately when they are sent (e.g. sent PUBACK on QoS1)
33
+#define BUFFER_FRAG_GARBAGE_COLLECT_ON_SEND 0x02
34
+// as buffer fragment can point to both
35
+// external data and data in the same buffer
36
+// we mark the former case with BUFFER_FRAG_DATA_EXTERNAL
37
+#define BUFFER_FRAG_DATA_EXTERNAL 0x04
38
+// as single MQTT Packet can be stored into multiple
39
+// buffer fragments (depending on copy requirements)
40
+// this marks this fragment to be the first/last
41
+#define BUFFER_FRAG_MQTT_PACKET_HEAD 0x10
42
+#define BUFFER_FRAG_MQTT_PACKET_TAIL 0x20
43
+
44
+typedef uint16_t buffer_frag_flag_t;
45
+struct buffer_fragment {
46
+ size_t len;
47
+ size_t sent;
48
+ buffer_frag_flag_t flags;
49
+ void (*free_fnc)(void *ptr);
50
+ char *data;
51
+
52
+ uint16_t packet_id;
53
+
54
+ struct buffer_fragment *next;
55
+};
56
+
57
+typedef struct buffer_fragment *mqtt_msg_data;
58
+
59
+// buffer used for MQTT headers only
60
+// not for actual data sent
61
+struct header_buffer {
62
+ size_t size;
63
+ char *data;
64
+ char *tail;
65
+ struct buffer_fragment *tail_frag;
66
+};
67
+
68
+struct transaction_buffer {
69
+ struct header_buffer hdr_buffer;
70
+ // used while building new message
71
+ // to be able to revert state easily
72
+ // in case of error mid processing
73
+ struct header_buffer state_backup;
74
+ pthread_mutex_t mutex;
75
+ struct buffer_fragment *sending_frag;
76
+};
77
+
78
+enum mqtt_client_state {
79
+ RAW = 0,
80
+ CONNECT_PENDING,
81
+ CONNECTING,
82
+ CONNECTED,
83
+ ERROR,
84
+ DISCONNECTED
85
+};
86
+
87
+enum parser_state {
88
+ MQTT_PARSE_FIXED_HEADER_PACKET_TYPE = 0,
89
+ MQTT_PARSE_FIXED_HEADER_LEN,
90
+ MQTT_PARSE_VARIABLE_HEADER,
91
+ MQTT_PARSE_MQTT_PACKET_DONE
92
+};
93
+
94
+enum varhdr_parser_state {
95
+ MQTT_PARSE_VARHDR_INITIAL = 0,
96
+ MQTT_PARSE_VARHDR_OPTIONAL_REASON_CODE,
97
+ MQTT_PARSE_VARHDR_PROPS,
98
+ MQTT_PARSE_VARHDR_TOPICNAME,
99
+ MQTT_PARSE_VARHDR_POST_TOPICNAME,
100
+ MQTT_PARSE_VARHDR_PACKET_ID,
101
+ MQTT_PARSE_REASONCODES,
102
+ MQTT_PARSE_PAYLOAD
103
+};
104
+
105
+struct mqtt_vbi_parser_ctx {
106
+ char data[MQTT_VBI_MAXBYTES];
107
+ uint8_t bytes;
108
+ uint32_t result;
109
+};
110
+
111
+enum mqtt_datatype {
112
+ MQTT_TYPE_UNKNOWN = 0,
113
+ MQTT_TYPE_UINT_8,
114
+ MQTT_TYPE_UINT_16,
115
+ MQTT_TYPE_UINT_32,
116
+ MQTT_TYPE_VBI,
117
+ MQTT_TYPE_STR,
118
+ MQTT_TYPE_STR_PAIR,
119
+ MQTT_TYPE_BIN
120
+};
121
+
122
+struct mqtt_property {
123
+ uint8_t id;
124
+ enum mqtt_datatype type;
125
+ union {
126
+ char *strings[2];
127
+ void *bindata;
128
+ uint8_t uint8;
129
+ uint16_t uint16;
130
+ uint32_t uint32;
131
+ } data;
132
+ size_t bindata_len;
133
+ struct mqtt_property *next;
134
+};
135
+
136
+enum mqtt_properties_parser_state {
137
+ PROPERTIES_LENGTH = 0,
138
+ PROPERTY_CREATE,
139
+ PROPERTY_ID,
140
+ PROPERTY_TYPE_UINT8,
141
+ PROPERTY_TYPE_UINT16,
142
+ PROPERTY_TYPE_UINT32,
143
+ PROPERTY_TYPE_STR_BIN_LEN,
144
+ PROPERTY_TYPE_STR,
145
+ PROPERTY_TYPE_BIN,
146
+ PROPERTY_TYPE_VBI,
147
+ PROPERTY_NEXT
148
+};
149
+
150
+struct mqtt_properties_parser_ctx {
151
+ enum mqtt_properties_parser_state state;
152
+ struct mqtt_property *head;
153
+ struct mqtt_property *tail;
154
+ uint32_t properties_length;
155
+ uint32_t vbi_length;
156
+ struct mqtt_vbi_parser_ctx vbi_parser_ctx;
157
+ size_t bytes_consumed;
158
+ int str_idx;
159
+};
160
+
161
+struct mqtt_connack {
162
+ uint8_t flags;
163
+ uint8_t reason_code;
164
+};
165
+struct mqtt_puback {
166
+ uint16_t packet_id;
167
+ uint8_t reason_code;
168
+};
169
+
170
+struct mqtt_suback {
171
+ uint16_t packet_id;
172
+ uint8_t *reason_codes;
173
+ uint8_t reason_code_count;
174
+ uint8_t reason_codes_pending;
175
+};
176
+
177
+struct mqtt_publish {
178
+ uint16_t topic_len;
179
+ char *topic;
180
+ uint16_t packet_id;
181
+ size_t data_len;
182
+ char *data;
183
+ uint8_t qos;
184
+};
185
+
186
+struct mqtt_disconnect {
187
+ uint8_t reason_code;
188
+};
189
+
190
+struct mqtt_ng_parser {
191
+ rbuf_t received_data;
192
+
193
+ uint8_t mqtt_control_packet_type;
194
+ uint32_t mqtt_fixed_hdr_remaining_length;
195
+ size_t mqtt_parsed_len;
196
+
197
+ struct mqtt_vbi_parser_ctx vbi_parser;
198
+ struct mqtt_properties_parser_ctx properties_parser;
199
+
200
+ enum parser_state state;
201
+ enum varhdr_parser_state varhdr_state;
202
+
203
+ struct mqtt_property *varhdr_properties;
204
+
205
+ union {
206
+ struct mqtt_connack connack;
207
+ struct mqtt_puback puback;
208
+ struct mqtt_suback suback;
209
+ struct mqtt_publish publish;
210
+ struct mqtt_disconnect disconnect;
211
+ } mqtt_packet;
212
+};
213
+
214
+struct topic_alias_data {
215
+ uint16_t idx;
216
+ uint32_t usage_count;
217
+};
218
+
219
+struct topic_aliases_data {
220
+ c_rhash stoi_dict;
221
+ uint32_t idx_max;
222
+ uint32_t idx_assigned;
223
+ pthread_rwlock_t rwlock;
224
+};
225
+
226
+struct mqtt_ng_client {
227
+ struct transaction_buffer main_buffer;
228
+
229
+ enum mqtt_client_state client_state;
230
+
231
+ mqtt_msg_data connect_msg;
232
+
233
+ mqtt_wss_log_ctx_t log;
234
+
235
+ mqtt_ng_send_fnc_t send_fnc_ptr;
236
+ void *user_ctx;
237
+
238
+ // time when last fragment of MQTT message was sent
239
+ time_t time_of_last_send;
240
+
241
+ struct mqtt_ng_parser parser;
242
+
243
+ size_t max_mem_bytes;
244
+
245
+ void (*puback_callback)(uint16_t packet_id);
246
+ void (*connack_callback)(void* user_ctx, int connack_reply);
247
+ void (*msg_callback)(const char *topic, const void *msg, size_t msglen, int qos);
248
+
249
+ unsigned int ping_pending:1;
250
+
251
+ struct mqtt_ng_stats stats;
252
+ pthread_mutex_t stats_mutex;
253
+
254
+ struct topic_aliases_data tx_topic_aliases;
255
+ c_rhash rx_aliases;
256
+
257
+ size_t max_msg_size;
258
+};
259
+
260
+char pingreq[] = { MQTT_CPT_PINGREQ << 4, 0x00 };
261
+
262
+struct buffer_fragment ping_frag = {
263
+ .data = pingreq,
264
+ .flags = BUFFER_FRAG_MQTT_PACKET_HEAD | BUFFER_FRAG_MQTT_PACKET_TAIL,
265
+ .free_fnc = NULL,
266
+ .len = sizeof(pingreq),
267
+ .next = NULL,
268
+ .sent = 0,
269
+ .packet_id = 0
270
+};
271
+
272
+int uint32_to_mqtt_vbi(uint32_t input, char *output) {
273
+ int i = 1;
274
+ *output = 0;
275
+
276
+ /* MQTT 5 specs allows max 4 bytes of output
277
+ making it 0xFF, 0xFF, 0xFF, 0x7F
278
+ representing number 268435455 decimal
279
+ see 1.5.5. Variable Byte Integer */
280
+ if(input >= 256 * 1024 * 1024)
281
+ return 0;
282
+
283
+ if(!input) {
284
+ *output = 0;
285
+ return 1;
286
+ }
287
+
288
+ while(input) {
289
+ output[i-1] = input & MQTT_VBI_DATA_MASK;
290
+ input >>= 7;
291
+ if (input)
292
+ output[i-1] |= MQTT_VBI_CONTINUATION_FLAG;
293
+ i++;
294
+ }
295
+ return i - 1;
296
+}
297
+
298
+int mqtt_vbi_to_uint32(char *input, uint32_t *output) {
299
+ // dont want to operate directly on output
300
+ // as I want it to be possible for input and output
301
+ // pointer to be the same
302
+ uint32_t result = 0;
303
+ uint32_t multiplier = 1;
304
+
305
+ do {
306
+ result += (uint32_t)(*input & MQTT_VBI_DATA_MASK) * multiplier;
307
+ if (multiplier > 128*128*128)
308
+ return 1;
309
+ multiplier <<= 7;
310
+ } while (*input++ & MQTT_VBI_CONTINUATION_FLAG);
311
+ *output = result;
312
+ return 0;
313
+}
314
+
315
+#ifdef TESTS
316
+#include <stdio.h>
317
+#define MQTT_VBI_MAXLEN 4
318
+// we add extra byte to check we dont write out of bounds
319
+// in case where 4 bytes are supposed to be written
320
+static const char _mqtt_vbi_0[MQTT_VBI_MAXLEN + 1] = { 0x00, 0x00, 0x00, 0x00, 0x00 };
321
+static const char _mqtt_vbi_127[MQTT_VBI_MAXLEN + 1] = { 0x7F, 0x00, 0x00, 0x00, 0x00 };
322
+static const char _mqtt_vbi_128[MQTT_VBI_MAXLEN + 1] = { 0x80, 0x01, 0x00, 0x00, 0x00 };
323
+static const char _mqtt_vbi_16383[MQTT_VBI_MAXLEN + 1] = { 0xFF, 0x7F, 0x00, 0x00, 0x00 };
324
+static const char _mqtt_vbi_16384[MQTT_VBI_MAXLEN + 1] = { 0x80, 0x80, 0x01, 0x00, 0x00 };
325
+static const char _mqtt_vbi_2097151[MQTT_VBI_MAXLEN + 1] = { 0xFF, 0xFF, 0x7F, 0x00, 0x00 };
326
+static const char _mqtt_vbi_2097152[MQTT_VBI_MAXLEN + 1] = { 0x80, 0x80, 0x80, 0x01, 0x00 };
327
+static const char _mqtt_vbi_268435455[MQTT_VBI_MAXLEN + 1] = { 0xFF, 0xFF, 0xFF, 0x7F, 0x00 };
328
+static const char _mqtt_vbi_999999999[MQTT_VBI_MAXLEN + 1] = { 0x80, 0x80, 0x80, 0x80, 0x01 };
329
+
330
+#define MQTT_VBI_TESTCASE(case, expected_len) \
331
+ { \
332
+ memset(buf, 0, MQTT_VBI_MAXLEN + 1); \
333
+ int len; \
334
+ if ((len=uint32_to_mqtt_vbi(case, buf)) != expected_len) { \
335
+ fprintf(stderr, "uint32_to_mqtt_vbi(case:%d, line:%d): Incorrect length returned. Expected %d, Got %d\n", case, __LINE__, expected_len, len); \
336
+ return 1; \
337
+ } \
338
+ if (memcmp(buf, _mqtt_vbi_ ## case, MQTT_VBI_MAXLEN + 1 )) { \
339
+ fprintf(stderr, "uint32_to_mqtt_vbi(case:%d, line:%d): Wrong output\n", case, __LINE__); \
340
+ return 1; \
341
+ } }
342
+
343
+
344
+int test_uint32_mqtt_vbi() {
345
+ char buf[MQTT_VBI_MAXLEN + 1];
346
+
347
+ MQTT_VBI_TESTCASE(0, 1)
348
+ MQTT_VBI_TESTCASE(127, 1)
349
+ MQTT_VBI_TESTCASE(128, 2)
350
+ MQTT_VBI_TESTCASE(16383, 2)
351
+ MQTT_VBI_TESTCASE(16384, 3)
352
+ MQTT_VBI_TESTCASE(2097151, 3)
353
+ MQTT_VBI_TESTCASE(2097152, 4)
354
+ MQTT_VBI_TESTCASE(268435455, 4)
355
+
356
+ memset(buf, 0, MQTT_VBI_MAXLEN + 1);
357
+ int len;
358
+ if ((len=uint32_to_mqtt_vbi(268435456, buf)) != 0) {
359
+ fprintf(stderr, "uint32_to_mqtt_vbi(case:268435456, line:%d): Incorrect length returned. Expected 0, Got %d\n", __LINE__, len);
360
+ return 1;
361
+ }
362
+
363
+ return 0;
364
+}
365
+
366
+#define MQTT_VBI2UINT_TESTCASE(case, expected_error) \
367
+ { \
368
+ uint32_t result; \
369
+ int ret = mqtt_vbi_to_uint32(_mqtt_vbi_ ## case, &result); \
370
+ if (ret && !(expected_error)) { \
371
+ fprintf(stderr, "mqtt_vbi_to_uint(case:%d, line:%d): Unexpectedly Errored\n", (case), __LINE__); \
372
+ return 1; \
373
+ } \
374
+ if (!ret && (expected_error)) { \
375
+ fprintf(stderr, "mqtt_vbi_to_uint(case:%d, line:%d): Should return error but didnt\n", (case), __LINE__); \
376
+ return 1; \
377
+ } \
378
+ if (!ret && result != (case)) { \
379
+ fprintf(stderr, "mqtt_vbi_to_uint(case:%d, line:%d): Returned wrong result %d\n", (case), __LINE__, result); \
380
+ return 1; \
381
+ }}
382
+
383
+
384
+int test_mqtt_vbi_to_uint32() {
385
+ MQTT_VBI2UINT_TESTCASE(0, 0)
386
+ MQTT_VBI2UINT_TESTCASE(127, 0)
387
+ MQTT_VBI2UINT_TESTCASE(128, 0)
388
+ MQTT_VBI2UINT_TESTCASE(16383, 0)
389
+ MQTT_VBI2UINT_TESTCASE(16384, 0)
390
+ MQTT_VBI2UINT_TESTCASE(2097151, 0)
391
+ MQTT_VBI2UINT_TESTCASE(2097152, 0)
392
+ MQTT_VBI2UINT_TESTCASE(268435455, 0)
393
+ MQTT_VBI2UINT_TESTCASE(999999999, 1)
394
+ return 0;
395
+}
396
+#endif /* TESTS */
397
+
398
+// this helps with switch statements
399
+// as they have to use integer type (not pointer)
400
+enum memory_mode {
401
+ MEMCPY,
402
+ EXTERNAL_FREE_AFTER_USE,
403
+ CALLER_RESPONSIBLE
404
+};
405
+
406
+static inline enum memory_mode ptr2memory_mode(void * ptr) {
407
+ if (ptr == NULL)
408
+ return MEMCPY;
409
+ if (ptr == CALLER_RESPONSIBILITY)
410
+ return CALLER_RESPONSIBLE;
411
+ return EXTERNAL_FREE_AFTER_USE;
412
+}
413
+
414
+#define frag_is_marked_for_gc(frag) ((frag->flags & BUFFER_FRAG_GARBAGE_COLLECT) || ((frag->flags & BUFFER_FRAG_GARBAGE_COLLECT_ON_SEND) && frag->sent == frag->len))
415
+#define FRAG_SIZE_IN_BUFFER(frag) (sizeof(struct buffer_fragment) + ((frag->flags & BUFFER_FRAG_DATA_EXTERNAL) ? 0 : frag->len))
416
+
417
+static void buffer_frag_free_data(struct buffer_fragment *frag)
418
+{
419
+ if ( frag->flags & BUFFER_FRAG_DATA_EXTERNAL && frag->data != NULL) {
420
+ switch (ptr2memory_mode(frag->free_fnc)) {
421
+ case MEMCPY:
422
+ mw_free(frag->data);
423
+ break;
424
+ case EXTERNAL_FREE_AFTER_USE:
425
+ frag->free_fnc(frag->data);
426
+ break;
427
+ case CALLER_RESPONSIBLE:
428
+ break;
429
+ }
430
+ frag->data = NULL;
431
+ }
432
+}
433
+
434
+#define HEADER_BUFFER_SIZE 1024*1024
435
+#define GROWTH_FACTOR 1.25
436
+
437
+#define BUFFER_BYTES_USED(buf) ((size_t)((buf)->tail - (buf)->data))
438
+#define BUFFER_BYTES_AVAILABLE(buf) ((buf)->size - BUFFER_BYTES_USED(buf))
439
+#define BUFFER_FIRST_FRAG(buf) ((struct buffer_fragment *)((buf)->tail_frag ? (buf)->data : NULL))
440
+static void buffer_purge(struct header_buffer *buf) {
441
+ struct buffer_fragment *frag = BUFFER_FIRST_FRAG(buf);
442
+ while (frag) {
443
+ buffer_frag_free_data(frag);
444
+ frag = frag->next;
445
+ }
446
+ buf->tail = buf->data;
447
+ buf->tail_frag = NULL;
448
+}
449
+
450
+#define FRAG_PADDING(addr) ((MQTT_WSS_FRAG_MEMALIGN - ((uintptr_t)addr % MQTT_WSS_FRAG_MEMALIGN)) % MQTT_WSS_FRAG_MEMALIGN)
451
+static struct buffer_fragment *buffer_new_frag(struct header_buffer *buf, buffer_frag_flag_t flags)
452
+{
453
+ uint8_t padding = FRAG_PADDING(buf->tail);
454
+
455
+ if (BUFFER_BYTES_AVAILABLE(buf) < sizeof(struct buffer_fragment) + padding)
456
+ return NULL;
457
+
458
+ struct buffer_fragment *frag = (struct buffer_fragment *)(buf->tail + padding);
459
+
460
+ memset(frag, 0, sizeof(*frag));
461
+ buf->tail += sizeof(*frag) + padding;
462
+
463
+ if (/*!((frag)->flags & BUFFER_FRAG_MQTT_PACKET_HEAD) &&*/ buf->tail_frag)
464
+ buf->tail_frag->next = frag;
465
+
466
+ buf->tail_frag = frag;
467
+
468
+ frag->data = buf->tail;
469
+
470
+ frag->flags = flags;
471
+
472
+ return frag;
473
+}
474
+
475
+static void buffer_rebuild(struct header_buffer *buf)
476
+{
477
+ struct buffer_fragment *frag = (struct buffer_fragment*)buf->data;
478
+ do {
479
+ buf->tail = (char*)frag + sizeof(struct buffer_fragment);
480
+ buf->tail_frag = frag;
481
+ if (!(frag->flags & BUFFER_FRAG_DATA_EXTERNAL)) {
482
+ buf->tail_frag->data = buf->tail;
483
+ buf->tail += frag->len;
484
+ }
485
+ if (frag->next != NULL)
486
+ frag->next = (struct buffer_fragment*)(buf->tail + FRAG_PADDING(buf->tail));
487
+ frag = frag->next;
488
+ } while(frag);
489
+}
490
+
491
+static void buffer_garbage_collect(struct header_buffer *buf, mqtt_wss_log_ctx_t log_ctx)
492
+{
493
+#if !defined(MQTT_DEBUG_VERBOSE) && !defined(ADDITIONAL_CHECKS)
494
+ (void) log_ctx;
495
+#endif
496
+#ifdef MQTT_DEBUG_VERBOSE
497
+ mws_debug(log_ctx, "Buffer Garbage Collection!");
498
+#endif
499
+
500
+ struct buffer_fragment *frag = BUFFER_FIRST_FRAG(buf);
501
+ while (frag) {
502
+ if (!frag_is_marked_for_gc(frag))
503
+ break;
504
+
505
+ buffer_frag_free_data(frag);
506
+
507
+ frag = frag->next;
508
+ }
509
+
510
+ if (frag == BUFFER_FIRST_FRAG(buf)) {
511
+#ifdef MQTT_DEBUG_VERBOSE
512
+ mws_debug(log_ctx, "Buffer Garbage Collection! No Space Reclaimed!");
513
+#endif
514
+ return;
515
+ }
516
+
517
+ if (!frag) {
518
+ buf->tail_frag = NULL;
519
+ buf->tail = buf->data;
520
+ return;
521
+ }
522
+
523
+#ifdef ADDITIONAL_CHECKS
524
+ if (!(frag->flags & BUFFER_FRAG_MQTT_PACKET_HEAD)) {
525
+ mws_error(log_ctx, "Expected to find end of buffer (NULL) or next packet head!");
526
+ return;
527
+ }
528
+#endif
529
+
530
+ memmove(buf->data, frag, buf->tail - (char*)frag);
531
+ buffer_rebuild(buf);
532
+}
533
+
534
+static void transaction_buffer_garbage_collect(struct transaction_buffer *buf, mqtt_wss_log_ctx_t log_ctx)
535
+{
536
+#ifdef MQTT_DEBUG_VERBOSE
537
+ mws_debug(log_ctx, "Transaction Buffer Garbage Collection! %s", buf->sending_frag == NULL ? "NULL" : "in flight message");
538
+#endif
539
+
540
+ // Invalidate the cached sending fragment
541
+ // as we will move data around
542
+ if (buf->sending_frag != &ping_frag)
543
+ buf->sending_frag = NULL;
544
+
545
+ buffer_garbage_collect(&buf->hdr_buffer, log_ctx);
546
+}
547
+
548
+static int transaction_buffer_grow(struct transaction_buffer *buf, mqtt_wss_log_ctx_t log_ctx, float rate, size_t max)
549
+{
550
+ if (buf->hdr_buffer.size >= max)
551
+ return 0;
552
+
553
+ // Invalidate the cached sending fragment
554
+ // as we will move data around
555
+ if (buf->sending_frag != &ping_frag)
556
+ buf->sending_frag = NULL;
557
+
558
+ buf->hdr_buffer.size *= rate;
559
+ if (buf->hdr_buffer.size > max)
560
+ buf->hdr_buffer.size = max;
561
+
562
+ void *ret = mw_realloc(buf->hdr_buffer.data, buf->hdr_buffer.size);
563
+ if (ret == NULL) {
564
+ mws_warn(log_ctx, "Buffer growth failed (realloc)");
565
+ return 1;
566
+ }
567
+
568
+ mws_debug(log_ctx, "Message metadata buffer was grown");
569
+
570
+ buf->hdr_buffer.data = ret;
571
+ buffer_rebuild(&buf->hdr_buffer);
572
+ return 0;
573
+}
574
+
575
+inline static int transaction_buffer_init(struct transaction_buffer *to_init, size_t size)
576
+{
577
+ pthread_mutex_init(&to_init->mutex, NULL);
578
+
579
+ to_init->hdr_buffer.size = size;
580
+ to_init->hdr_buffer.data = mw_malloc(size);
581
+ if (to_init->hdr_buffer.data == NULL)
582
+ return 1;
583
+
584
+ to_init->hdr_buffer.tail = to_init->hdr_buffer.data;
585
+ to_init->hdr_buffer.tail_frag = NULL;
586
+ return 0;
587
+}
588
+
589
+static void transaction_buffer_destroy(struct transaction_buffer *to_init)
590
+{
591
+ buffer_purge(&to_init->hdr_buffer);
592
+ pthread_mutex_destroy(&to_init->mutex);
593
+ mw_free(to_init->hdr_buffer.data);
594
+}
595
+
596
+// Creates transaction
597
+// saves state of buffer before any operation was done
598
+// allowing for rollback if things go wrong
599
+#define transaction_buffer_transaction_start(buf) \
600
+ { LOCK_HDR_BUFFER(buf); \
601
+ memcpy(&(buf)->state_backup, &(buf)->hdr_buffer, sizeof((buf)->hdr_buffer)); }
602
+
603
+#define transaction_buffer_transaction_commit(buf) UNLOCK_HDR_BUFFER(buf);
604
+
605
+void transaction_buffer_transaction_rollback(struct transaction_buffer *buf, struct buffer_fragment *frag)
606
+{
607
+ memcpy(&buf->hdr_buffer, &buf->state_backup, sizeof(buf->hdr_buffer));
608
+ if (buf->hdr_buffer.tail_frag != NULL)
609
+ buf->hdr_buffer.tail_frag->next = NULL;
610
+
611
+ while(frag) {
612
+ buffer_frag_free_data(frag);
613
+ // we are not actually freeing the structure itself
614
+ // just the data it manages
615
+ // structure itself is in permanent buffer
616
+ // which is locked by HDR_BUFFER lock
617
+ frag = frag->next;
618
+ }
619
+
620
+ UNLOCK_HDR_BUFFER(buf);
621
+}
622
+
623
+#define TX_ALIASES_INITIALIZE() c_rhash_new(0)
624
+#define RX_ALIASES_INITIALIZE() c_rhash_new(UINT16_MAX >> 8)
625
+struct mqtt_ng_client *mqtt_ng_init(struct mqtt_ng_init *settings)
626
+{
627
+ struct mqtt_ng_client *client = mw_calloc(1, sizeof(struct mqtt_ng_client));
628
+ if (client == NULL)
629
+ return NULL;
630
+
631
+ if (transaction_buffer_init(&client->main_buffer, HEADER_BUFFER_SIZE))
632
+ goto err_free_client;
633
+
634
+ client->rx_aliases = RX_ALIASES_INITIALIZE();
635
+ if (client->rx_aliases == NULL)
636
+ goto err_free_trx_buf;
637
+
638
+ if (pthread_mutex_init(&client->stats_mutex, NULL))
639
+ goto err_free_rx_alias;
640
+
641
+ client->tx_topic_aliases.stoi_dict = TX_ALIASES_INITIALIZE();
642
+ if (client->tx_topic_aliases.stoi_dict == NULL)
643
+ goto err_free_stats_mutex;
644
+ client->tx_topic_aliases.idx_max = UINT16_MAX;
645
+
646
+ if (pthread_rwlock_init(&client->tx_topic_aliases.rwlock, NULL))
647
+ goto err_free_tx_alias;
648
+
649
+ // TODO just embed the struct into mqtt_ng_client
650
+ client->parser.received_data = settings->data_in;
651
+ client->send_fnc_ptr = settings->data_out_fnc;
652
+ client->user_ctx = settings->user_ctx;
653
+
654
+ client->log = settings->log;
655
+
656
+ client->puback_callback = settings->puback_callback;
657
+ client->connack_callback = settings->connack_callback;
658
+ client->msg_callback = settings->msg_callback;
659
+
660
+ return client;
661
+
662
+err_free_tx_alias:
663
+ c_rhash_destroy(client->tx_topic_aliases.stoi_dict);
664
+err_free_stats_mutex:
665
+ pthread_mutex_destroy(&client->stats_mutex);
666
+err_free_rx_alias:
667
+ c_rhash_destroy(client->rx_aliases);
668
+err_free_trx_buf:
669
+ transaction_buffer_destroy(&client->main_buffer);
670
+err_free_client:
671
+ mw_free(client);
672
+ return NULL;
673
+}
674
+
675
+static inline uint8_t get_control_packet_type(uint8_t first_hdr_byte)
676
+{
677
+ return first_hdr_byte >> 4;
678
+}
679
+
680
+static void mqtt_ng_destroy_rx_alias_hash(c_rhash hash)
681
+{
682
+ c_rhash_iter_t i = C_RHASH_ITER_T_INITIALIZER;
683
+ uint64_t stored_key;
684
+ void *to_free;
685
+ while(!c_rhash_iter_uint64_keys(hash, &i, &stored_key)) {
686
+ c_rhash_get_ptr_by_uint64(hash, stored_key, &to_free);
687
+ mw_free(to_free);
688
+ }
689
+ c_rhash_destroy(hash);
690
+}
691
+
692
+static void mqtt_ng_destroy_tx_alias_hash(c_rhash hash)
693
+{
694
+ c_rhash_iter_t i = C_RHASH_ITER_T_INITIALIZER;
695
+ const char *stored_key;
696
+ void *to_free;
697
+ while(!c_rhash_iter_str_keys(hash, &i, &stored_key)) {
698
+ c_rhash_get_ptr_by_str(hash, stored_key, &to_free);
699
+ mw_free(to_free);
700
+ }
701
+ c_rhash_destroy(hash);
702
+}
703
+
704
+void mqtt_ng_destroy(struct mqtt_ng_client *client)
705
+{
706
+ transaction_buffer_destroy(&client->main_buffer);
707
+ pthread_mutex_destroy(&client->stats_mutex);
708
+
709
+ mqtt_ng_destroy_tx_alias_hash(client->tx_topic_aliases.stoi_dict);
710
+ pthread_rwlock_destroy(&client->tx_topic_aliases.rwlock);
711
+ mqtt_ng_destroy_rx_alias_hash(client->rx_aliases);
712
+
713
+ mw_free(client);
714
+}
715
+
716
+int frag_set_external_data(mqtt_wss_log_ctx_t log, struct buffer_fragment *frag, void *data, size_t data_len, free_fnc_t data_free_fnc)
717
+{
718
+ if (frag->len) {
719
+ // TODO?: This could potentially be done in future if we set rule
720
+ // external data always follows in buffer data
721
+ // could help reduce fragmentation in some messages but
722
+ // currently not worth it considering time is tight
723
+ mws_fatal(log, UNIT_LOG_PREFIX "INTERNAL ERROR: Cannot set external data to fragment already containing in buffer data!");
724
+ return 1;
725
+ }
726
+
727
+ switch (ptr2memory_mode(data_free_fnc)) {
728
+ case MEMCPY:
729
+ frag->data = mw_malloc(data_len);
730
+ if (frag->data == NULL) {
731
+ mws_error(log, UNIT_LOG_PREFIX "OOM while malloc @_optimized_add");
732
+ return 1;
733
+ }
734
+ memcpy(frag->data, data, data_len);
735
+ break;
736
+ case EXTERNAL_FREE_AFTER_USE:
737
+ case CALLER_RESPONSIBLE:
738
+ frag->data = data;
739
+ break;
740
+ }
741
+ frag->free_fnc = data_free_fnc;
742
+ frag->len = data_len;
743
+
744
+ frag->flags |= BUFFER_FRAG_DATA_EXTERNAL;
745
+ return 0;
746
+ }
747
+
748
+// this is fixed part of variable header for connect packet
749
+// mqtt-v5.0-cs1, 3.1.2.1, 2.1.2.2
750
+static const char mqtt_protocol_name_frag[] =
751
+ { 0x00, 0x04, 'M', 'Q', 'T', 'T', MQTT_VERSION_5_0 };
752
+
753
+#define MQTT_UTF8_STRING_SIZE(string) (2 + strlen(string))
754
+
755
+// see 1.5.5
756
+#define MQTT_VARSIZE_INT_BYTES(value) ( value > 2097152 ? 4 : ( value > 16384 ? 3 : ( value > 128 ? 2 : 1 ) ) )
757
+
758
+static size_t mqtt_ng_connect_size(struct mqtt_auth_properties *auth,
759
+ struct mqtt_lwt_properties *lwt)
760
+{
761
+ // First get the size of payload + variable header
762
+ size_t size =
763
+ + sizeof(mqtt_protocol_name_frag) /* Proto Name and Version */
764
+ + 1 /* Connect Flags */
765
+ + 2 /* Keep Alive */
766
+ + 4 /* 3.1.2.11.1 Property Length - for now fixed to only Topic Alias Maximum, TODO TODO*/;
767
+
768
+ // CONNECT payload. 3.1.3
769
+ if (auth->client_id)
770
+ size += MQTT_UTF8_STRING_SIZE(auth->client_id);
771
+
772
+ if (lwt) {
773
+ // 3.1.3.2 will properties TODO TODO
774
+ size += 1;
775
+
776
+ // 3.1.3.3
777
+ if (lwt->will_topic)
778
+ size += MQTT_UTF8_STRING_SIZE(lwt->will_topic);
779
+
780
+ // 3.1.3.4 will payload
781
+ if (lwt->will_message) {
782
+ size += 2 + lwt->will_message_size;
783
+ }
784
+ }
785
+
786
+ // 3.1.3.5
787
+ if (auth->username)
788
+ size += MQTT_UTF8_STRING_SIZE(auth->username);
789
+
790
+ // 3.1.3.6
791
+ if (auth->password)
792
+ size += MQTT_UTF8_STRING_SIZE(auth->password);
793
+
794
+ return size;
795
+}
796
+
797
+#define BUFFER_TRANSACTION_NEW_FRAG(buf, flags, frag, on_fail) \
798
+ { if(frag==NULL) { \
799
+ frag = buffer_new_frag(buf, (flags)); } \
800
+ if(frag==NULL) { on_fail; }}
801
+
802
+#define CHECK_BYTES_AVAILABLE(buf, needed, fail) \
803
+ { if (BUFFER_BYTES_AVAILABLE(buf) < (size_t)needed) { \
804
+ fail; } }
805
+
806
+#define DATA_ADVANCE(buf, bytes, frag) { size_t b = (bytes); (buf)->tail += b; (frag)->len += b; }
807
+
808
+// TODO maybe just user client->buf.tail?
809
+#define WRITE_POS(frag) (&(frag->data[frag->len]))
810
+
811
+// [MQTT-1.5.2] Two Byte Integer
812
+#define PACK_2B_INT(buffer, integer, frag) { *(uint16_t *)WRITE_POS(frag) = htobe16((integer)); \
813
+ DATA_ADVANCE(buffer, sizeof(uint16_t), frag); }
814
+
815
+static int _optimized_add(struct header_buffer *buf, mqtt_wss_log_ctx_t log_ctx, void *data, size_t data_len, free_fnc_t data_free_fnc, struct buffer_fragment **frag)
816
+{
817
+ if (data_len > SMALL_STRING_DONT_FRAGMENT_LIMIT) {
818
+ buffer_frag_flag_t flags = BUFFER_FRAG_DATA_EXTERNAL;
819
+ if ((*frag)->flags & BUFFER_FRAG_GARBAGE_COLLECT_ON_SEND)
820
+ flags |= BUFFER_FRAG_GARBAGE_COLLECT_ON_SEND;
821
+ if( (*frag = buffer_new_frag(buf, flags)) == NULL ) {
822
+ mws_error(log_ctx, "Out of buffer space while generating the message");
823
+ return 1;
824
+ }
825
+ if (frag_set_external_data(log_ctx, *frag, data, data_len, data_free_fnc)) {
826
+ mws_error(log_ctx, "Error adding external data to newly created fragment");
827
+ return 1;
828
+ }
829
+ // we dont want to write to this fragment anymore
830
+ *frag = NULL;
831
+ } else if (data_len) {
832
+ // if the data are small dont bother creating new fragments
833
+ // store in buffer directly
834
+ CHECK_BYTES_AVAILABLE(buf, data_len, return 1);
835
+ memcpy(buf->tail, data, data_len);
836
+ DATA_ADVANCE(buf, data_len, *frag);
837
+ }
838
+ return 0;
839
+}
840
+
841
+#define TRY_GENERATE_MESSAGE(generator_function, client, ...) \
842
+ int rc = generator_function(&client->main_buffer, client->log, ##__VA_ARGS__); \
843
+ if (rc == MQTT_NG_MSGGEN_BUFFER_OOM) { \
844
+ LOCK_HDR_BUFFER(&client->main_buffer); \
845
+ transaction_buffer_garbage_collect((&client->main_buffer), client->log); \
846
+ UNLOCK_HDR_BUFFER(&client->main_buffer); \
847
+ rc = generator_function(&client->main_buffer, client->log, ##__VA_ARGS__); \
848
+ if (rc == MQTT_NG_MSGGEN_BUFFER_OOM && client->max_mem_bytes) { \
849
+ LOCK_HDR_BUFFER(&client->main_buffer); \
850
+ transaction_buffer_grow((&client->main_buffer), client->log, GROWTH_FACTOR, client->max_mem_bytes); \
851
+ UNLOCK_HDR_BUFFER(&client->main_buffer); \
852
+ rc = generator_function(&client->main_buffer, client->log, ##__VA_ARGS__); \
853
+ } \
854
+ if (rc == MQTT_NG_MSGGEN_BUFFER_OOM) \
855
+ mws_error(client->log, "%s failed to generate message due to insufficient buffer space (line %d)", __FUNCTION__, __LINE__); \
856
+ } \
857
+ if (rc == MQTT_NG_MSGGEN_OK) { \
858
+ pthread_mutex_lock(&client->stats_mutex); \
859
+ client->stats.tx_messages_queued++; \
860
+ pthread_mutex_unlock(&client->stats_mutex); \
861
+ } \
862
+ return rc;
863
+
864
+mqtt_msg_data mqtt_ng_generate_connect(struct transaction_buffer *trx_buf,
865
+ mqtt_wss_log_ctx_t log_ctx,
866
+ struct mqtt_auth_properties *auth,
867
+ struct mqtt_lwt_properties *lwt,
868
+ uint8_t clean_start,
869
+ uint16_t keep_alive)
870
+{
871
+ // Sanity Checks First (are given parameters correct and up to MQTT spec)
872
+ if (!auth->client_id) {
873
+ mws_error(log_ctx, "ClientID must be set. [MQTT-3.1.3-3]");
874
+ return NULL;
875
+ }
876
+
877
+ size_t len = strlen(auth->client_id);
878
+ if (!len) {
879
+ // [MQTT-3.1.3-6] server MAY allow empty client_id and treat it
880
+ // as specific client_id (not same as client_id not given)
881
+ // however server MUST allow ClientIDs between 1-23 bytes [MQTT-3.1.3-5]
882
+ // so we will warn client server might not like this and he is using it
883
+ // at his own risk!
884
+ mws_warn(log_ctx, "client_id provided is empty string. This might not be allowed by server [MQTT-3.1.3-6]");
885
+ }
886
+ if(len > MQTT_MAX_CLIENT_ID) {
887
+ // [MQTT-3.1.3-5] server MUST allow client_id length 1-32
888
+ // server MAY allow longer client_id, if user provides longer client_id
889
+ // warn them he is doing so at his own risk!
890
+ mws_warn(log_ctx, "client_id provided is longer than 23 bytes, server might not allow that [MQTT-3.1.3-5]");
891
+ }
892
+
893
+ if (lwt) {
894
+ if (lwt->will_message && lwt->will_message_size > 65535) {
895
+ mws_error(log_ctx, "Will message cannot be longer than 65535 bytes due to MQTT protocol limitations [MQTT-3.1.3-4] and [MQTT-1.5.6]");
896
+ return NULL;
897
+ }
898
+
899
+ if (!lwt->will_topic) { //TODO topic given with strlen==0 ? check specs
900
+ mws_error(log_ctx, "If will message is given will topic must also be given [MQTT-3.1.3.3]");
901
+ return NULL;
902
+ }
903
+
904
+ if (lwt->will_qos > MQTT_MAX_QOS) {
905
+ // refer to [MQTT-3-1.2-12]
906
+ mws_error(log_ctx, "QOS for LWT message is bigger than max");
907
+ return NULL;
908
+ }
909
+ }
910
+
911
+ // >> START THE RODEO <<
912
+ transaction_buffer_transaction_start(trx_buf);
913
+
914
+ // Calculate the resulting message size sans fixed MQTT header
915
+ size_t size = mqtt_ng_connect_size(auth, lwt);
916
+
917
+ // Start generating the message
918
+ struct buffer_fragment *frag = NULL;
919
+ mqtt_msg_data ret = NULL;
920
+
921
+ BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, BUFFER_FRAG_MQTT_PACKET_HEAD, frag, goto fail_rollback );
922
+ ret = frag;
923
+
924
+ // MQTT Fixed Header
925
+ size_t needed_bytes = 1 /* Packet type */ + MQTT_VARSIZE_INT_BYTES(size) + sizeof(mqtt_protocol_name_frag) + 1 /* CONNECT FLAGS */ + 2 /* keepalive */ + 1 /* Properties TODO now fixed 0*/;
926
+ CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, needed_bytes, goto fail_rollback);
927
+
928
+ *WRITE_POS(frag) = MQTT_CPT_CONNECT << 4;
929
+ DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag);
930
+ DATA_ADVANCE(&trx_buf->hdr_buffer, uint32_to_mqtt_vbi(size, WRITE_POS(frag)), frag);
931
+
932
+ memcpy(WRITE_POS(frag), mqtt_protocol_name_frag, sizeof(mqtt_protocol_name_frag));
933
+ DATA_ADVANCE(&trx_buf->hdr_buffer, sizeof(mqtt_protocol_name_frag), frag);
934
+
935
+ // [MQTT-3.1.2.3] Connect flags
936
+ char *connect_flags = WRITE_POS(frag);
937
+ *connect_flags = 0;
938
+ if (auth->username)
939
+ *connect_flags |= MQTT_CONNECT_FLAG_USERNAME;
940
+ if (auth->password)
941
+ *connect_flags |= MQTT_CONNECT_FLAG_PASSWORD;
942
+ if (lwt) {
943
+ *connect_flags |= MQTT_CONNECT_FLAG_LWT;
944
+ *connect_flags |= lwt->will_qos << MQTT_CONNECT_FLAG_QOS_BITSHIFT;
945
+ if (lwt->will_retain)
946
+ *connect_flags |= MQTT_CONNECT_FLAG_LWT_RETAIN;
947
+ }
948
+ if (clean_start)
949
+ *connect_flags |= MQTT_CONNECT_FLAG_CLEAN_START;
950
+
951
+ DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag);
952
+
953
+ PACK_2B_INT(&trx_buf->hdr_buffer, keep_alive, frag);
954
+
955
+ // TODO Property Length [MQTT-3.1.3.2.1] temporary fixed to 3 (one property topic alias max)
956
+ DATA_ADVANCE(&trx_buf->hdr_buffer, uint32_to_mqtt_vbi(3, WRITE_POS(frag)), frag);
957
+ *WRITE_POS(frag) = MQTT_PROP_TOPIC_ALIAS_MAX;
958
+ DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag);
959
+
960
+ PACK_2B_INT(&trx_buf->hdr_buffer, 65535, frag);
961
+
962
+ // [MQTT-3.1.3.1] Client identifier
963
+ CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, 2, goto fail_rollback);
964
+ PACK_2B_INT(&trx_buf->hdr_buffer, strlen(auth->client_id), frag);
965
+ if (_optimized_add(&trx_buf->hdr_buffer, log_ctx, auth->client_id, strlen(auth->client_id), auth->client_id_free, &frag))
966
+ goto fail_rollback;
967
+
968
+ if (lwt != NULL) {
969
+ // Will Properties [MQTT-3.1.3.2]
970
+ // TODO for now fixed 0
971
+ BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, 0, frag, goto fail_rollback);
972
+ CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, 1, goto fail_rollback);
973
+ *WRITE_POS(frag) = 0;
974
+ DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag);
975
+
976
+ // Will Topic [MQTT-3.1.3.3]
977
+ CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, 2, goto fail_rollback);
978
+ PACK_2B_INT(&trx_buf->hdr_buffer, strlen(lwt->will_topic), frag);
979
+ if (_optimized_add(&trx_buf->hdr_buffer, log_ctx, lwt->will_topic, strlen(lwt->will_topic), lwt->will_topic_free, &frag))
980
+ goto fail_rollback;
981
+
982
+ // Will Payload [MQTT-3.1.3.4]
983
+ if (lwt->will_message_size) {
984
+ BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, 0, frag, goto fail_rollback);
985
+ CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, 2, goto fail_rollback);
986
+ PACK_2B_INT(&trx_buf->hdr_buffer, lwt->will_message_size, frag);
987
+ if (_optimized_add(&trx_buf->hdr_buffer, log_ctx, lwt->will_message, lwt->will_message_size, lwt->will_topic_free, &frag))
988
+ goto fail_rollback;
989
+ }
990
+ }
991
+
992
+ // [MQTT-3.1.3.5]
993
+ if (auth->username) {
994
+ BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, 0, frag, goto fail_rollback);
995
+ CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, 2, goto fail_rollback);
996
+ PACK_2B_INT(&trx_buf->hdr_buffer, strlen(auth->username), frag);
997
+ if (_optimized_add(&trx_buf->hdr_buffer, log_ctx, auth->username, strlen(auth->username), auth->username_free, &frag))
998
+ goto fail_rollback;
999
+ }
1000
+
1001
+ // [MQTT-3.1.3.6]
1002
+ if (auth->password) {
1003
+ BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, 0, frag, goto fail_rollback);
1004
+ CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, 2, goto fail_rollback);
1005
+ PACK_2B_INT(&trx_buf->hdr_buffer, strlen(auth->password), frag);
1006
+ if (_optimized_add(&trx_buf->hdr_buffer, log_ctx, auth->password, strlen(auth->password), auth->password_free, &frag))
1007
+ goto fail_rollback;
1008
+ }
1009
+ trx_buf->hdr_buffer.tail_frag->flags |= BUFFER_FRAG_MQTT_PACKET_TAIL;
1010
+ transaction_buffer_transaction_commit(trx_buf);
1011
+ return ret;
1012
+fail_rollback:
1013
+ transaction_buffer_transaction_rollback(trx_buf, ret);
1014
+ return NULL;
1015
+}
1016
+
1017
+int mqtt_ng_connect(struct mqtt_ng_client *client,
1018
+ struct mqtt_auth_properties *auth,
1019
+ struct mqtt_lwt_properties *lwt,
1020
+ uint8_t clean_start,
1021
+ uint16_t keep_alive)
1022
+{
1023
+ client->client_state = RAW;
1024
+ client->parser.state = MQTT_PARSE_FIXED_HEADER_PACKET_TYPE;
1025
+
1026
+ LOCK_HDR_BUFFER(&client->main_buffer);
1027
+ client->main_buffer.sending_frag = NULL;
1028
+ if (clean_start)
1029
+ buffer_purge(&client->main_buffer.hdr_buffer);
1030
+ UNLOCK_HDR_BUFFER(&client->main_buffer);
1031
+
1032
+ pthread_rwlock_wrlock(&client->tx_topic_aliases.rwlock);
1033
+ // according to MQTT spec topic aliases should not be persisted
1034
+ // even if clean session is true
1035
+ mqtt_ng_destroy_tx_alias_hash(client->tx_topic_aliases.stoi_dict);
1036
+ client->tx_topic_aliases.stoi_dict = TX_ALIASES_INITIALIZE();
1037
+ if (client->tx_topic_aliases.stoi_dict == NULL) {
1038
+ pthread_rwlock_unlock(&client->tx_topic_aliases.rwlock);
1039
+ return 1;
1040
+ }
1041
+ client->tx_topic_aliases.idx_assigned = 0;
1042
+ pthread_rwlock_unlock(&client->tx_topic_aliases.rwlock);
1043
+
1044
+ mqtt_ng_destroy_rx_alias_hash(client->rx_aliases);
1045
+ client->rx_aliases = RX_ALIASES_INITIALIZE();
1046
+ if (client->rx_aliases == NULL)
1047
+ return 1;
1048
+
1049
+ client->connect_msg = mqtt_ng_generate_connect(&client->main_buffer, client->log, auth, lwt, clean_start, keep_alive);
1050
+ if (client->connect_msg == NULL)
1051
+ return 1;
1052
+
1053
+ pthread_mutex_lock(&client->stats_mutex);
1054
+ if (clean_start)
1055
+ client->stats.tx_messages_queued = 1;
1056
+ else
1057
+ client->stats.tx_messages_queued++;
1058
+
1059
+ client->stats.tx_messages_sent = 0;
1060
+ client->stats.rx_messages_rcvd = 0;
1061
+ pthread_mutex_unlock(&client->stats_mutex);
1062
+
1063
+ client->client_state = CONNECT_PENDING;
1064
+ return 0;
1065
+}
1066
+
1067
+uint16_t get_unused_packet_id() {
1068
+ static uint16_t packet_id = 0;
1069
+ packet_id++;
1070
+ return packet_id ? packet_id : ++packet_id;
1071
+}
1072
+
1073
+static inline size_t mqtt_ng_publish_size(const char *topic,
1074
+ size_t msg_len,
1075
+ uint16_t topic_id)
1076
+{
1077
+ size_t retval = 2 /* Topic Name Length */
1078
+ + (topic == NULL ? 0 : strlen(topic))
1079
+ + 2 /* Packet identifier */
1080
+ + 1 /* Properties Length TODO for now fixed to 1 property */
1081
+ + msg_len;
1082
+
1083
+ if (topic_id)
1084
+ retval += 3;
1085
+
1086
+ return retval;
1087
+}
1088
+
1089
+int mqtt_ng_generate_publish(struct transaction_buffer *trx_buf,
1090
+ mqtt_wss_log_ctx_t log_ctx,
1091
+ char *topic,
1092
+ free_fnc_t topic_free,
1093
+ void *msg,
1094
+ free_fnc_t msg_free,
1095
+ size_t msg_len,
1096
+ uint8_t publish_flags,
1097
+ uint16_t *packet_id,
1098
+ uint16_t topic_alias)
1099
+{
1100
+ // >> START THE RODEO <<
1101
+ transaction_buffer_transaction_start(trx_buf);
1102
+
1103
+ // Calculate the resulting message size sans fixed MQTT header
1104
+ size_t size = mqtt_ng_publish_size(topic, msg_len, topic_alias);
1105
+
1106
+ // Start generating the message
1107
+ struct buffer_fragment *frag = NULL;
1108
+ mqtt_msg_data mqtt_msg = NULL;
1109
+
1110
+ BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, BUFFER_FRAG_MQTT_PACKET_HEAD, frag, goto fail_rollback );
1111
+ // in case of QOS 0 we can garbage collect immediatelly after sending
1112
+ uint8_t qos = (publish_flags >> 1) & 0x03;
1113
+ if (!qos)
1114
+ frag->flags |= BUFFER_FRAG_GARBAGE_COLLECT_ON_SEND;
1115
+ mqtt_msg = frag;
1116
+
1117
+ // MQTT Fixed Header
1118
+ size_t needed_bytes = 1 /* Packet type */ + MQTT_VARSIZE_INT_BYTES(size) + size - msg_len;
1119
+ CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, needed_bytes, goto fail_rollback);
1120
+
1121
+ *WRITE_POS(frag) = (MQTT_CPT_PUBLISH << 4) | (publish_flags & 0xF);
1122
+ DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag);
1123
+ DATA_ADVANCE(&trx_buf->hdr_buffer, uint32_to_mqtt_vbi(size, WRITE_POS(frag)), frag);
1124
+
1125
+ // MQTT Variable Header
1126
+ // [MQTT-3.3.2.1]
1127
+ PACK_2B_INT(&trx_buf->hdr_buffer, topic == NULL ? 0 : strlen(topic), frag);
1128
+ if (topic != NULL) {
1129
+ if (_optimized_add(&trx_buf->hdr_buffer, log_ctx, topic, strlen(topic), topic_free, &frag))
1130
+ goto fail_rollback;
1131
+ BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, 0, frag, goto fail_rollback);
1132
+ }
1133
+
1134
+ // [MQTT-3.3.2.2]
1135
+ mqtt_msg->packet_id = get_unused_packet_id();
1136
+ *packet_id = mqtt_msg->packet_id;
1137
+ PACK_2B_INT(&trx_buf->hdr_buffer, mqtt_msg->packet_id, frag);
1138
+
1139
+ // [MQTT-3.3.2.3.1] TODO Property Length for now fixed 0
1140
+ *WRITE_POS(frag) = topic_alias ? 3 : 0;
1141
+ DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag);
1142
+
1143
+ if(topic_alias) {
1144
+ *WRITE_POS(frag) = MQTT_PROP_TOPIC_ALIAS;
1145
+ DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag);
1146
+
1147
+ PACK_2B_INT(&trx_buf->hdr_buffer, topic_alias, frag);
1148
+ }
1149
+
1150
+ if( (frag = buffer_new_frag(&trx_buf->hdr_buffer, BUFFER_FRAG_DATA_EXTERNAL)) == NULL )
1151
+ goto fail_rollback;
1152
+
1153
+ if (frag_set_external_data(log_ctx, frag, msg, msg_len, msg_free))
1154
+ goto fail_rollback;
1155
+
1156
+ trx_buf->hdr_buffer.tail_frag->flags |= BUFFER_FRAG_MQTT_PACKET_TAIL;
1157
+ if (!qos)
1158
+ trx_buf->hdr_buffer.tail_frag->flags |= BUFFER_FRAG_GARBAGE_COLLECT_ON_SEND;
1159
+ transaction_buffer_transaction_commit(trx_buf);
1160
+ return MQTT_NG_MSGGEN_OK;
1161
+fail_rollback:
1162
+ transaction_buffer_transaction_rollback(trx_buf, mqtt_msg);
1163
+ return MQTT_NG_MSGGEN_BUFFER_OOM;
1164
+}
1165
+
1166
+#define PUBLISH_SP_SIZE 64
1167
+int mqtt_ng_publish(struct mqtt_ng_client *client,
1168
+ char *topic,
1169
+ free_fnc_t topic_free,
1170
+ void *msg,
1171
+ free_fnc_t msg_free,
1172
+ size_t msg_len,
1173
+ uint8_t publish_flags,
1174
+ uint16_t *packet_id)
1175
+{
1176
+ struct topic_alias_data *alias = NULL;
1177
+ pthread_rwlock_rdlock(&client->tx_topic_aliases.rwlock);
1178
+ c_rhash_get_ptr_by_str(client->tx_topic_aliases.stoi_dict, topic, (void**)&alias);
1179
+ pthread_rwlock_unlock(&client->tx_topic_aliases.rwlock);
1180
+
1181
+ uint16_t topic_id = 0;
1182
+
1183
+ if (alias != NULL) {
1184
+ topic_id = alias->idx;
1185
+ uint32_t cnt = __atomic_fetch_add(&alias->usage_count, 1, __ATOMIC_SEQ_CST);
1186
+ if (cnt) {
1187
+ topic = NULL;
1188
+ topic_free = NULL;
1189
+ }
1190
+ }
1191
+
1192
+ if (client->max_msg_size && PUBLISH_SP_SIZE + mqtt_ng_publish_size(topic, msg_len, topic_id) > client->max_msg_size) {
1193
+ mws_error(client->log, "Message too big for server: %zu", msg_len);
1194
+ return MQTT_NG_MSGGEN_MSG_TOO_BIG;
1195
+ }
1196
+
1197
+ TRY_GENERATE_MESSAGE(mqtt_ng_generate_publish, client, topic, topic_free, msg, msg_free, msg_len, publish_flags, packet_id, topic_id);
1198
+}
1199
+
1200
+static inline size_t mqtt_ng_subscribe_size(struct mqtt_sub *subs, size_t sub_count)
1201
+{
1202
+ size_t len = 2 /* Packet Identifier */ + 1 /* Properties Length TODO for now fixed 0 */;
1203
+ len += sub_count * (2 /* topic filter string length */ + 1 /* [MQTT-3.8.3.1] Subscription Options Byte */);
1204
+
1205
+ for (size_t i = 0; i < sub_count; i++) {
1206
+ len += strlen(subs[i].topic);
1207
+ }
1208
+ return len;
1209
+}
1210
+
1211
+int mqtt_ng_generate_subscribe(struct transaction_buffer *trx_buf, mqtt_wss_log_ctx_t log_ctx, struct mqtt_sub *subs, size_t sub_count)
1212
+{
1213
+ // >> START THE RODEO <<
1214
+ transaction_buffer_transaction_start(trx_buf);
1215
+
1216
+ // Calculate the resulting message size sans fixed MQTT header
1217
+ size_t size = mqtt_ng_subscribe_size(subs, sub_count);
1218
+
1219
+ // Start generating the message
1220
+ struct buffer_fragment *frag = NULL;
1221
+ mqtt_msg_data ret = NULL;
1222
+
1223
+ BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, BUFFER_FRAG_MQTT_PACKET_HEAD, frag, goto fail_rollback);
1224
+ ret = frag;
1225
+
1226
+ // MQTT Fixed Header
1227
+ size_t needed_bytes = 1 /* Packet type */ + MQTT_VARSIZE_INT_BYTES(size) + 3 /*Packet ID + Property Length*/;
1228
+ CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, needed_bytes, goto fail_rollback);
1229
+
1230
+ *WRITE_POS(frag) = (MQTT_CPT_SUBSCRIBE << 4) | 0x2 /* [MQTT-3.8.1-1] */;
1231
+ DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag);
1232
+ DATA_ADVANCE(&trx_buf->hdr_buffer, uint32_to_mqtt_vbi(size, WRITE_POS(frag)), frag);
1233
+
1234
+ // MQTT Variable Header
1235
+ // [MQTT-3.8.2] PacketID
1236
+ ret->packet_id = get_unused_packet_id();
1237
+ PACK_2B_INT(&trx_buf->hdr_buffer, ret->packet_id, frag);
1238
+
1239
+ // [MQTT-3.8.2.1.1] Property Length // TODO for now fixed 0
1240
+ *WRITE_POS(frag) = 0;
1241
+ DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag);
1242
+
1243
+ for (size_t i = 0; i < sub_count; i++) {
1244
+ BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, 0, frag, goto fail_rollback);
1245
+ PACK_2B_INT(&trx_buf->hdr_buffer, strlen(subs[i].topic), frag);
1246
+ if (_optimized_add(&trx_buf->hdr_buffer, log_ctx, subs[i].topic, strlen(subs[i].topic), subs[i].topic_free, &frag))
1247
+ goto fail_rollback;
1248
+ BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, 0, frag, goto fail_rollback);
1249
+ *WRITE_POS(frag) = subs[i].options;
1250
+ DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag);
1251
+ }
1252
+
1253
+ trx_buf->hdr_buffer.tail_frag->flags |= BUFFER_FRAG_MQTT_PACKET_TAIL;
1254
+ transaction_buffer_transaction_commit(trx_buf);
1255
+ return MQTT_NG_MSGGEN_OK;
1256
+fail_rollback:
1257
+ transaction_buffer_transaction_rollback(trx_buf, ret);
1258
+ return MQTT_NG_MSGGEN_BUFFER_OOM;
1259
+}
1260
+
1261
+int mqtt_ng_subscribe(struct mqtt_ng_client *client, struct mqtt_sub *subs, size_t sub_count)
1262
+{
1263
+ TRY_GENERATE_MESSAGE(mqtt_ng_generate_subscribe, client, subs, sub_count);
1264
+}
1265
+
1266
+int mqtt_ng_generate_disconnect(struct transaction_buffer *trx_buf, mqtt_wss_log_ctx_t log_ctx, uint8_t reason_code)
1267
+{
1268
+ (void) log_ctx;
1269
+ // >> START THE RODEO <<
1270
+ transaction_buffer_transaction_start(trx_buf);
1271
+
1272
+ // Calculate the resulting message size sans fixed MQTT header
1273
+ size_t size = reason_code ? 1 : 0;
1274
+
1275
+ // Start generating the message
1276
+ struct buffer_fragment *frag = NULL;
1277
+ mqtt_msg_data ret = NULL;
1278
+
1279
+ BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, BUFFER_FRAG_MQTT_PACKET_HEAD, frag, goto fail_rollback);
1280
+ ret = frag;
1281
+
1282
+ // MQTT Fixed Header
1283
+ size_t needed_bytes = 1 /* Packet type */ + MQTT_VARSIZE_INT_BYTES(size) + (reason_code ? 1 : 0);
1284
+ CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, needed_bytes, goto fail_rollback);
1285
+
1286
+ *WRITE_POS(frag) = MQTT_CPT_DISCONNECT << 4;
1287
+ DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag);
1288
+ DATA_ADVANCE(&trx_buf->hdr_buffer, uint32_to_mqtt_vbi(size, WRITE_POS(frag)), frag);
1289
+
1290
+ if (reason_code) {
1291
+ // MQTT Variable Header
1292
+ // [MQTT-3.14.2.1] PacketID
1293
+ *WRITE_POS(frag) = reason_code;
1294
+ DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag);
1295
+ }
1296
+
1297
+ trx_buf->hdr_buffer.tail_frag->flags |= BUFFER_FRAG_MQTT_PACKET_TAIL;
1298
+ transaction_buffer_transaction_commit(trx_buf);
1299
+ return MQTT_NG_MSGGEN_OK;
1300
+fail_rollback:
1301
+ transaction_buffer_transaction_rollback(trx_buf, ret);
1302
+ return MQTT_NG_MSGGEN_BUFFER_OOM;
1303
+}
1304
+
1305
+int mqtt_ng_disconnect(struct mqtt_ng_client *client, uint8_t reason_code)
1306
+{
1307
+ TRY_GENERATE_MESSAGE(mqtt_ng_generate_disconnect, client, reason_code);
1308
+}
1309
+
1310
+static int mqtt_generate_puback(struct transaction_buffer *trx_buf, mqtt_wss_log_ctx_t log_ctx, uint16_t packet_id, uint8_t reason_code)
1311
+{
1312
+ (void) log_ctx;
1313
+ // >> START THE RODEO <<
1314
+ transaction_buffer_transaction_start(trx_buf);
1315
+
1316
+ // Calculate the resulting message size sans fixed MQTT header
1317
+ size_t size = 2 /* Packet ID */ + (reason_code ? 1 : 0) /* reason code */;
1318
+
1319
+ // Start generating the message
1320
+ struct buffer_fragment *frag = NULL;
1321
+
1322
+ BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, BUFFER_FRAG_MQTT_PACKET_HEAD | BUFFER_FRAG_GARBAGE_COLLECT_ON_SEND, frag, goto fail_rollback);
1323
+
1324
+ // MQTT Fixed Header
1325
+ size_t needed_bytes = 1 /* Packet type */ + MQTT_VARSIZE_INT_BYTES(size) + size;
1326
+ CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, needed_bytes, goto fail_rollback);
1327
+
1328
+ *WRITE_POS(frag) = MQTT_CPT_PUBACK << 4;
1329
+ DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag);
1330
+ DATA_ADVANCE(&trx_buf->hdr_buffer, uint32_to_mqtt_vbi(size, WRITE_POS(frag)), frag);
1331
+
1332
+ // MQTT Variable Header
1333
+ PACK_2B_INT(&trx_buf->hdr_buffer, packet_id, frag);
1334
+
1335
+ if (reason_code) {
1336
+ // MQTT Variable Header
1337
+ // [MQTT-3.14.2.1] PacketID
1338
+ *WRITE_POS(frag) = reason_code;
1339
+ DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag);
1340
+ }
1341
+
1342
+ trx_buf->hdr_buffer.tail_frag->flags |= BUFFER_FRAG_MQTT_PACKET_TAIL;
1343
+ transaction_buffer_transaction_commit(trx_buf);
1344
+ return MQTT_NG_MSGGEN_OK;
1345
+fail_rollback:
1346
+ transaction_buffer_transaction_rollback(trx_buf, frag);
1347
+ return MQTT_NG_MSGGEN_BUFFER_OOM;
1348
+}
1349
+
1350
+static int mqtt_ng_puback(struct mqtt_ng_client *client, uint16_t packet_id, uint8_t reason_code)
1351
+{
1352
+ TRY_GENERATE_MESSAGE(mqtt_generate_puback, client, packet_id, reason_code);
1353
+}
1354
+
1355
+int mqtt_ng_ping(struct mqtt_ng_client *client)
1356
+{
1357
+ client->ping_pending = 1;
1358
+ return MQTT_NG_MSGGEN_OK;
1359
+}
1360
+
1361
+#define MQTT_NG_CLIENT_NEED_MORE_BYTES 0x10
1362
+#define MQTT_NG_CLIENT_MQTT_PACKET_DONE 0x11
1363
+#define MQTT_NG_CLIENT_PARSE_DONE 0x12
1364
+#define MQTT_NG_CLIENT_WANT_WRITE 0x13
1365
+#define MQTT_NG_CLIENT_OK_CALL_AGAIN 0
1366
+#define MQTT_NG_CLIENT_PROTOCOL_ERROR -1
1367
+#define MQTT_NG_CLIENT_SERVER_RETURNED_ERROR -2
1368
+#define MQTT_NG_CLIENT_NOT_IMPL_YET -3
1369
+#define MQTT_NG_CLIENT_OOM -4
1370
+#define MQTT_NG_CLIENT_INTERNAL_ERROR -5
1371
+
1372
+#define BUF_READ_CHECK_AT_LEAST(buf, x) \
1373
+ if (rbuf_bytes_available(buf) < (x)) \
1374
+ return MQTT_NG_CLIENT_NEED_MORE_BYTES;
1375
+
1376
+#define vbi_parser_reset_ctx(ctx) memset(ctx, 0, sizeof(struct mqtt_vbi_parser_ctx))
1377
+
1378
+static int vbi_parser_parse(struct mqtt_vbi_parser_ctx *ctx, rbuf_t data, mqtt_wss_log_ctx_t log)
1379
+{
1380
+ if (ctx->bytes > MQTT_VBI_MAXBYTES - 1) {
1381
+ mws_error(log, "MQTT Variable Byte Integer can't be longer than %d bytes", MQTT_VBI_MAXBYTES);
1382
+ return MQTT_NG_CLIENT_PROTOCOL_ERROR;
1383
+ }
1384
+ if (!ctx->bytes || ctx->data[ctx->bytes-1] & MQTT_VBI_CONTINUATION_FLAG) {
1385
+ BUF_READ_CHECK_AT_LEAST(data, 1);
1386
+ ctx->bytes++;
1387
+ rbuf_pop(data, &ctx->data[ctx->bytes-1], 1);
1388
+ if ( ctx->data[ctx->bytes-1] & MQTT_VBI_CONTINUATION_FLAG )
1389
+ return MQTT_NG_CLIENT_OK_CALL_AGAIN;
1390
+ }
1391
+
1392
+ if (mqtt_vbi_to_uint32(ctx->data, &ctx->result)) {
1393
+ mws_error(log, "MQTT Variable Byte Integer failed to be parsed.");
1394
+ return MQTT_NG_CLIENT_PROTOCOL_ERROR;
1395
+ }
1396
+
1397
+ return MQTT_NG_CLIENT_PARSE_DONE;
1398
+}
1399
+
1400
+static void mqtt_properties_parser_ctx_reset(struct mqtt_properties_parser_ctx *ctx)
1401
+{
1402
+ ctx->state = PROPERTIES_LENGTH;
1403
+ while (ctx->head) {
1404
+ struct mqtt_property *f = ctx->head;
1405
+ ctx->head = ctx->head->next;
1406
+ if (f->type == MQTT_TYPE_STR || f->type == MQTT_TYPE_STR_PAIR)
1407
+ mw_free(f->data.strings[0]);
1408
+ if (f->type == MQTT_TYPE_STR_PAIR)
1409
+ mw_free(f->data.strings[1]);
1410
+ if (f->type == MQTT_TYPE_BIN)
1411
+ mw_free(f->data.bindata);
1412
+ mw_free(f);
1413
+ }
1414
+ ctx->tail = NULL;
1415
+ ctx->properties_length = 0;
1416
+ ctx->bytes_consumed = 0;
1417
+ vbi_parser_reset_ctx(&ctx->vbi_parser_ctx);
1418
+}
1419
+
1420
+struct mqtt_property_type {
1421
+ uint8_t id;
1422
+ enum mqtt_datatype datatype;
1423
+ const char* name;
1424
+};
1425
+
1426
+const struct mqtt_property_type mqtt_property_types[] = {
1427
+ { .id = MQTT_PROP_TOPIC_ALIAS, .name = MQTT_PROP_TOPIC_ALIAS_NAME, .datatype = MQTT_TYPE_UINT_16 },
1428
+
1429
+ { .id = MQTT_PROP_PAYLOAD_FMT_INDICATOR, .name = MQTT_PROP_PAYLOAD_FMT_INDICATOR_NAME, .datatype = MQTT_TYPE_UINT_8 },
1430
+ { .id = MQTT_PROP_MSG_EXPIRY_INTERVAL, .name = MQTT_PROP_MSG_EXPIRY_INTERVAL_NAME, .datatype = MQTT_TYPE_UINT_32 },
1431
+ { .id = MQTT_PROP_CONTENT_TYPE, .name = MQTT_PROP_CONTENT_TYPE_NAME, .datatype = MQTT_TYPE_STR },
1432
+ { .id = MQTT_PROP_RESPONSE_TOPIC, .name = MQTT_PROP_RESPONSE_TOPIC_NAME, .datatype = MQTT_TYPE_STR },
1433
+ { .id = MQTT_PROP_CORRELATION_DATA, .name = MQTT_PROP_CORRELATION_DATA_NAME, .datatype = MQTT_TYPE_BIN },
1434
+ { .id = MQTT_PROP_SUB_IDENTIFIER, .name = MQTT_PROP_SUB_IDENTIFIER_NAME, .datatype = MQTT_TYPE_VBI },
1435
+ { .id = MQTT_PROP_SESSION_EXPIRY_INTERVAL, .name = MQTT_PROP_SESSION_EXPIRY_INTERVAL_NAME, .datatype = MQTT_TYPE_UINT_32 },
1436
+ { .id = MQTT_PROP_ASSIGNED_CLIENT_ID, .name = MQTT_PROP_ASSIGNED_CLIENT_ID_NAME, .datatype = MQTT_TYPE_STR },
1437
+ { .id = MQTT_PROP_SERVER_KEEP_ALIVE, .name = MQTT_PROP_SERVER_KEEP_ALIVE_NAME, .datatype = MQTT_TYPE_UINT_16 },
1438
+ { .id = MQTT_PROP_AUTH_METHOD, .name = MQTT_PROP_AUTH_METHOD_NAME, .datatype = MQTT_TYPE_STR },
1439
+ { .id = MQTT_PROP_AUTH_DATA, .name = MQTT_PROP_AUTH_DATA_NAME, .datatype = MQTT_TYPE_BIN },
1440
+ { .id = MQTT_PROP_REQ_PROBLEM_INFO, .name = MQTT_PROP_REQ_PROBLEM_INFO_NAME, .datatype = MQTT_TYPE_UINT_8 },
1441
+ { .id = MQTT_PROP_WILL_DELAY_INTERVAL, .name = MQTT_PROP_WIIL_DELAY_INTERVAL_NAME, .datatype = MQTT_TYPE_UINT_32 },
1442
+ { .id = MQTT_PROP_REQ_RESP_INFORMATION, .name = MQTT_PROP_REQ_RESP_INFORMATION_NAME, .datatype = MQTT_TYPE_UINT_8 },
1443
+ { .id = MQTT_PROP_RESP_INFORMATION, .name = MQTT_PROP_RESP_INFORMATION_NAME, .datatype = MQTT_TYPE_STR },
1444
+ { .id = MQTT_PROP_SERVER_REF, .name = MQTT_PROP_SERVER_REF_NAME, .datatype = MQTT_TYPE_STR },
1445
+ { .id = MQTT_PROP_REASON_STR, .name = MQTT_PROP_REASON_STR_NAME, .datatype = MQTT_TYPE_STR },
1446
+ { .id = MQTT_PROP_RECEIVE_MAX, .name = MQTT_PROP_RECEIVE_MAX_NAME, .datatype = MQTT_TYPE_UINT_16 },
1447
+ { .id = MQTT_PROP_TOPIC_ALIAS_MAX, .name = MQTT_PROP_TOPIC_ALIAS_MAX_NAME, .datatype = MQTT_TYPE_UINT_16 },
1448
+ // MQTT_PROP_TOPIC_ALIAS is first as it is most often used
1449
+ { .id = MQTT_PROP_MAX_QOS, .name = MQTT_PROP_MAX_QOS_NAME, .datatype = MQTT_TYPE_UINT_8 },
1450
+ { .id = MQTT_PROP_RETAIN_AVAIL, .name = MQTT_PROP_RETAIN_AVAIL_NAME, .datatype = MQTT_TYPE_UINT_8 },
1451
+ { .id = MQTT_PROP_USR, .name = MQTT_PROP_USR_NAME, .datatype = MQTT_TYPE_STR_PAIR },
1452
+ { .id = MQTT_PROP_MAX_PKT_SIZE, .name = MQTT_PROP_MAX_PKT_SIZE_NAME, .datatype = MQTT_TYPE_UINT_32 },
1453
+ { .id = MQTT_PROP_WILDCARD_SUB_AVAIL, .name = MQTT_PROP_WILDCARD_SUB_AVAIL_NAME, .datatype = MQTT_TYPE_UINT_8 },
1454
+ { .id = MQTT_PROP_SUB_ID_AVAIL, .name = MQTT_PROP_SUB_ID_AVAIL_NAME, .datatype = MQTT_TYPE_UINT_8 },
1455
+ { .id = MQTT_PROP_SHARED_SUB_AVAIL, .name = MQTT_PROP_SHARED_SUB_AVAIL_NAME, .datatype = MQTT_TYPE_UINT_8 },
1456
+ { .id = 0, .name = NULL, .datatype = MQTT_TYPE_UNKNOWN }
1457
+};
1458
+
1459
+static int get_property_type_by_id(uint8_t property_id) {
1460
+ for (int i = 0; mqtt_property_types[i].datatype != MQTT_TYPE_UNKNOWN; i++) {
1461
+ if (mqtt_property_types[i].id == property_id)
1462
+ return mqtt_property_types[i].datatype;
1463
+ }
1464
+ return MQTT_TYPE_UNKNOWN;
1465
+}
1466
+
1467
+struct mqtt_property *get_property_by_id(struct mqtt_property *props, uint8_t property_id)
1468
+{
1469
+ while (props) {
1470
+ if (props->id == property_id) {
1471
+ return props;
1472
+ }
1473
+ props = props->next;
1474
+ }
1475
+ return NULL;
1476
+}
1477
+
1478
+// Parses [MQTT-2.2.2]
1479
+static int parse_properties_array(struct mqtt_properties_parser_ctx *ctx, rbuf_t data, mqtt_wss_log_ctx_t log)
1480
+{
1481
+ int rc;
1482
+ switch (ctx->state) {
1483
+ case PROPERTIES_LENGTH:
1484
+ rc = vbi_parser_parse(&ctx->vbi_parser_ctx, data, log);
1485
+ if (rc == MQTT_NG_CLIENT_PARSE_DONE) {
1486
+ ctx->properties_length = ctx->vbi_parser_ctx.result;
1487
+ ctx->bytes_consumed += ctx->vbi_parser_ctx.bytes;
1488
+ ctx->vbi_length = ctx->vbi_parser_ctx.bytes;
1489
+ if (!ctx->properties_length)
1490
+ return MQTT_NG_CLIENT_PARSE_DONE;
1491
+ ctx->state = PROPERTY_CREATE;
1492
+ break;
1493
+ }
1494
+ return rc;
1495
+ case PROPERTY_CREATE:
1496
+ BUF_READ_CHECK_AT_LEAST(data, 1);
1497
+ struct mqtt_property *prop = mw_calloc(1, sizeof(struct mqtt_property));
1498
+ if (ctx->head == NULL) {
1499
+ ctx->head = prop;
1500
+ ctx->tail = prop;
1501
+ } else {
1502
+ ctx->tail->next = prop;
1503
+ ctx->tail = ctx->tail->next;
1504
+ }
1505
+ ctx->state = PROPERTY_ID;
1506
+ /* FALLTHROUGH */
1507
+ case PROPERTY_ID:
1508
+ rbuf_pop(data, (char*)&ctx->tail->id, 1);
1509
+ ctx->bytes_consumed += 1;
1510
+ ctx->tail->type = get_property_type_by_id(ctx->tail->id);
1511
+ switch (ctx->tail->type) {
1512
+ case MQTT_TYPE_UINT_16:
1513
+ ctx->state = PROPERTY_TYPE_UINT16;
1514
+ break;
1515
+ case MQTT_TYPE_UINT_32:
1516
+ ctx->state = PROPERTY_TYPE_UINT32;
1517
+ break;
1518
+ case MQTT_TYPE_UINT_8:
1519
+ ctx->state = PROPERTY_TYPE_UINT8;
1520
+ break;
1521
+ case MQTT_TYPE_VBI:
1522
+ ctx->state = PROPERTY_TYPE_VBI;
1523
+ vbi_parser_reset_ctx(&ctx->vbi_parser_ctx);
1524
+ break;
1525
+ case MQTT_TYPE_STR:
1526
+ case MQTT_TYPE_STR_PAIR:
1527
+ ctx->str_idx = 0;
1528
+ /* FALLTHROUGH */
1529
+ case MQTT_TYPE_BIN:
1530
+ ctx->state = PROPERTY_TYPE_STR_BIN_LEN;
1531
+ break;
1532
+ default:
1533
+ mws_error(log, "Unsupported property type %d for property id %d.", (int)ctx->tail->type, (int)ctx->tail->id);
1534
+ return MQTT_NG_CLIENT_PROTOCOL_ERROR;
1535
+ }
1536
+ break;
1537
+ case PROPERTY_TYPE_STR_BIN_LEN:
1538
+ BUF_READ_CHECK_AT_LEAST(data, sizeof(uint16_t));
1539
+ rbuf_pop(data, (char*)&ctx->tail->bindata_len, sizeof(uint16_t));
1540
+ ctx->tail->bindata_len = be16toh(ctx->tail->bindata_len);
1541
+ ctx->bytes_consumed += 2;
1542
+ switch (ctx->tail->type) {
1543
+ case MQTT_TYPE_BIN:
1544
+ ctx->state = PROPERTY_TYPE_BIN;
1545
+ break;
1546
+ case MQTT_TYPE_STR:
1547
+ case MQTT_TYPE_STR_PAIR:
1548
+ ctx->state = PROPERTY_TYPE_STR;
1549
+ break;
1550
+ default:
1551
+ mws_error(log, "Unexpected datatype in PROPERTY_TYPE_STR_BIN_LEN %d", (int)ctx->tail->type);
1552
+ return MQTT_NG_CLIENT_INTERNAL_ERROR;
1553
+ }
1554
+ break;
1555
+ case PROPERTY_TYPE_STR:
1556
+ BUF_READ_CHECK_AT_LEAST(data, ctx->tail->bindata_len);
1557
+ ctx->tail->data.strings[ctx->str_idx] = mw_malloc(ctx->tail->bindata_len + 1);
1558
+ rbuf_pop(data, ctx->tail->data.strings[ctx->str_idx], ctx->tail->bindata_len);
1559
+ ctx->tail->data.strings[ctx->str_idx][ctx->tail->bindata_len] = 0;
1560
+ ctx->str_idx++;
1561
+ ctx->bytes_consumed += ctx->tail->bindata_len;
1562
+ if (ctx->tail->type == MQTT_TYPE_STR_PAIR && ctx->str_idx < 2) {
1563
+ ctx->state = PROPERTY_TYPE_STR_BIN_LEN;
1564
+ break;
1565
+ }
1566
+ ctx->state = PROPERTY_NEXT;
1567
+ break;
1568
+ case PROPERTY_TYPE_BIN:
1569
+ BUF_READ_CHECK_AT_LEAST(data, ctx->tail->bindata_len);
1570
+ ctx->tail->data.bindata = mw_malloc(ctx->tail->bindata_len);
1571
+ rbuf_pop(data, ctx->tail->data.bindata, ctx->tail->bindata_len);
1572
+ ctx->bytes_consumed += ctx->tail->bindata_len;
1573
+ ctx->state = PROPERTY_NEXT;
1574
+ break;
1575
+ case PROPERTY_TYPE_VBI:
1576
+ rc = vbi_parser_parse(&ctx->vbi_parser_ctx, data, log);
1577
+ if (rc == MQTT_NG_CLIENT_PARSE_DONE) {
1578
+ ctx->tail->data.uint32 = ctx->vbi_parser_ctx.result;
1579
+ ctx->bytes_consumed += ctx->vbi_parser_ctx.bytes;
1580
+ ctx->state = PROPERTY_NEXT;
1581
+ break;
1582
+ }
1583
+ return rc;
1584
+ case PROPERTY_TYPE_UINT8:
1585
+ BUF_READ_CHECK_AT_LEAST(data, sizeof(uint8_t));
1586
+ rbuf_pop(data, (char*)&ctx->tail->data.uint8, sizeof(uint8_t));
1587
+ ctx->bytes_consumed += sizeof(uint8_t);
1588
+ ctx->state = PROPERTY_NEXT;
1589
+ break;
1590
+ case PROPERTY_TYPE_UINT32:
1591
+ BUF_READ_CHECK_AT_LEAST(data, sizeof(uint32_t));
1592
+ rbuf_pop(data, (char*)&ctx->tail->data.uint32, sizeof(uint32_t));
1593
+ ctx->tail->data.uint32 = be32toh(ctx->tail->data.uint32);
1594
+ ctx->bytes_consumed += sizeof(uint32_t);
1595
+ ctx->state = PROPERTY_NEXT;
1596
+ break;
1597
+ case PROPERTY_TYPE_UINT16:
1598
+ BUF_READ_CHECK_AT_LEAST(data, sizeof(uint16_t));
1599
+ rbuf_pop(data, (char*)&ctx->tail->data.uint16, sizeof(uint16_t));
1600
+ ctx->tail->data.uint16 = be16toh(ctx->tail->data.uint16);
1601
+ ctx->bytes_consumed += sizeof(uint16_t);
1602
+ ctx->state = PROPERTY_NEXT;
1603
+ /* FALLTHROUGH */
1604
+ case PROPERTY_NEXT:
1605
+ if (ctx->properties_length > ctx->bytes_consumed - ctx->vbi_length) {
1606
+ ctx->state = PROPERTY_CREATE;
1607
+ break;
1608
+ } else
1609
+ return MQTT_NG_CLIENT_PARSE_DONE;
1610
+ }
1611
+ return MQTT_NG_CLIENT_OK_CALL_AGAIN;
1612
+}
1613
+
1614
+static int parse_connack_varhdr(struct mqtt_ng_client *client)
1615
+{
1616
+ struct mqtt_ng_parser *parser = &client->parser;
1617
+ switch (parser->varhdr_state) {
1618
+ case MQTT_PARSE_VARHDR_INITIAL:
1619
+ BUF_READ_CHECK_AT_LEAST(parser->received_data, 2);
1620
+ rbuf_pop(parser->received_data, (char*)&parser->mqtt_packet.connack.flags, 1);
1621
+ rbuf_pop(parser->received_data, (char*)&parser->mqtt_packet.connack.reason_code, 1);
1622
+ parser->varhdr_state = MQTT_PARSE_VARHDR_PROPS;
1623
+ mqtt_properties_parser_ctx_reset(&parser->properties_parser);
1624
+ break;
1625
+ case MQTT_PARSE_VARHDR_PROPS:
1626
+ return parse_properties_array(&parser->properties_parser, parser->received_data, client->log);
1627
+ default:
1628
+ ERROR("invalid state for connack varhdr parser");
1629
+ return MQTT_NG_CLIENT_INTERNAL_ERROR;
1630
+ }
1631
+ return MQTT_NG_CLIENT_OK_CALL_AGAIN;
1632
+}
1633
+
1634
+static int parse_disconnect_varhdr(struct mqtt_ng_client *client)
1635
+{
1636
+ struct mqtt_ng_parser *parser = &client->parser;
1637
+ switch (parser->varhdr_state) {
1638
+ case MQTT_PARSE_VARHDR_INITIAL:
1639
+ if (!parser->mqtt_fixed_hdr_remaining_length) {
1640
+ // [MQTT-3.14.2.1] if reason code omitted act same as == 0
1641
+ parser->mqtt_packet.disconnect.reason_code = 0;
1642
+ return MQTT_NG_CLIENT_PARSE_DONE;
1643
+ }
1644
+ BUF_READ_CHECK_AT_LEAST(parser->received_data, 1);
1645
+ rbuf_pop(parser->received_data, (char*)&parser->mqtt_packet.connack.reason_code, 1);
1646
+ if (parser->mqtt_fixed_hdr_remaining_length == 1)
1647
+ return MQTT_NG_CLIENT_PARSE_DONE;
1648
+ parser->varhdr_state = MQTT_PARSE_VARHDR_PROPS;
1649
+ mqtt_properties_parser_ctx_reset(&parser->properties_parser);
1650
+ break;
1651
+ case MQTT_PARSE_VARHDR_PROPS:
1652
+ return parse_properties_array(&parser->properties_parser, parser->received_data, client->log);
1653
+ default:
1654
+ ERROR("invalid state for connack varhdr parser");
1655
+ return MQTT_NG_CLIENT_INTERNAL_ERROR;
1656
+ }
1657
+ return MQTT_NG_CLIENT_OK_CALL_AGAIN;
1658
+}
1659
+
1660
+static int parse_puback_varhdr(struct mqtt_ng_client *client)
1661
+{
1662
+ struct mqtt_ng_parser *parser = &client->parser;
1663
+ switch (parser->varhdr_state) {
1664
+ case MQTT_PARSE_VARHDR_INITIAL:
1665
+ BUF_READ_CHECK_AT_LEAST(parser->received_data, 2);
1666
+ rbuf_pop(parser->received_data, (char*)&parser->mqtt_packet.puback.packet_id, 2);
1667
+ parser->mqtt_packet.puback.packet_id = be16toh(parser->mqtt_packet.puback.packet_id);
1668
+ if (parser->mqtt_fixed_hdr_remaining_length < 3) {
1669
+ // [MQTT-3.4.2.1] if length is not big enough for reason code
1670
+ // it is omitted and handled same as if it was present and == 0
1671
+ // initially missed this detail and was wondering WTF is going on (sigh)
1672
+ parser->mqtt_packet.puback.reason_code = 0;
1673
+ return MQTT_NG_CLIENT_PARSE_DONE;
1674
+ }
1675
+ parser->varhdr_state = MQTT_PARSE_VARHDR_OPTIONAL_REASON_CODE;
1676
+ /* FALLTHROUGH */
1677
+ case MQTT_PARSE_VARHDR_OPTIONAL_REASON_CODE:
1678
+ BUF_READ_CHECK_AT_LEAST(parser->received_data, 1);
1679
+ rbuf_pop(parser->received_data, (char*)&parser->mqtt_packet.puback.reason_code, 1);
1680
+ // LOL so in CONNACK you have to have 0 byte to
1681
+ // signify empty properties list
1682
+ // but in PUBACK it can be omitted if remaining length doesn't allow it (sigh)
1683
+ if (parser->mqtt_fixed_hdr_remaining_length < 4)
1684
+ return MQTT_NG_CLIENT_PARSE_DONE;
1685
+
1686
+ parser->varhdr_state = MQTT_PARSE_VARHDR_PROPS;
1687
+ mqtt_properties_parser_ctx_reset(&parser->properties_parser);
1688
+ /* FALLTHROUGH */
1689
+ case MQTT_PARSE_VARHDR_PROPS:
1690
+ return parse_properties_array(&parser->properties_parser, parser->received_data, client->log);
1691
+ default:
1692
+ ERROR("invalid state for puback varhdr parser");
1693
+ return MQTT_NG_CLIENT_INTERNAL_ERROR;
1694
+ }
1695
+ return MQTT_NG_CLIENT_OK_CALL_AGAIN;
1696
+}
1697
+
1698
+static int parse_suback_varhdr(struct mqtt_ng_client *client)
1699
+{
1700
+ int rc;
1701
+ size_t avail;
1702
+ struct mqtt_ng_parser *parser = &client->parser;
1703
+ struct mqtt_suback *suback = &client->parser.mqtt_packet.suback;
1704
+ switch (parser->varhdr_state) {
1705
+ case MQTT_PARSE_VARHDR_INITIAL:
1706
+ suback->reason_codes = NULL;
1707
+ BUF_READ_CHECK_AT_LEAST(parser->received_data, 2);
1708
+ rbuf_pop(parser->received_data, (char*)&suback->packet_id, 2);
1709
+ suback->packet_id = be16toh(suback->packet_id);
1710
+ parser->varhdr_state = MQTT_PARSE_VARHDR_PROPS;
1711
+ parser->mqtt_parsed_len = 2;
1712
+ mqtt_properties_parser_ctx_reset(&parser->properties_parser);
1713
+ /* FALLTHROUGH */
1714
+ case MQTT_PARSE_VARHDR_PROPS:
1715
+ rc = parse_properties_array(&parser->properties_parser, parser->received_data, client->log);
1716
+ if (rc != MQTT_NG_CLIENT_PARSE_DONE)
1717
+ return rc;
1718
+ parser->mqtt_parsed_len += parser->properties_parser.bytes_consumed;
1719
+ suback->reason_code_count = parser->mqtt_fixed_hdr_remaining_length - parser->mqtt_parsed_len;
1720
+ suback->reason_codes = mw_calloc(suback->reason_code_count, sizeof(*suback->reason_codes));
1721
+ suback->reason_codes_pending = suback->reason_code_count;
1722
+ parser->varhdr_state = MQTT_PARSE_REASONCODES;
1723
+ /* FALLTHROUGH */
1724
+ case MQTT_PARSE_REASONCODES:
1725
+ avail = rbuf_bytes_available(parser->received_data);
1726
+ if (avail < 1)
1727
+ return MQTT_NG_CLIENT_NEED_MORE_BYTES;
1728
+
1729
+ suback->reason_codes_pending -= rbuf_pop(parser->received_data, (char*)suback->reason_codes, MIN(suback->reason_codes_pending, avail));
1730
+
1731
+ if (!suback->reason_codes_pending)
1732
+ return MQTT_NG_CLIENT_PARSE_DONE;
1733
+
1734
+ return MQTT_NG_CLIENT_NEED_MORE_BYTES;
1735
+ default:
1736
+ ERROR("invalid state for suback varhdr parser");
1737
+ return MQTT_NG_CLIENT_INTERNAL_ERROR;
1738
+ }
1739
+ return MQTT_NG_CLIENT_OK_CALL_AGAIN;
1740
+}
1741
+
1742
+static int parse_publish_varhdr(struct mqtt_ng_client *client)
1743
+{
1744
+ int rc;
1745
+ struct mqtt_ng_parser *parser = &client->parser;
1746
+ struct mqtt_publish *publish = &client->parser.mqtt_packet.publish;
1747
+ switch (parser->varhdr_state) {
1748
+ case MQTT_PARSE_VARHDR_INITIAL:
1749
+ BUF_READ_CHECK_AT_LEAST(parser->received_data, 2);
1750
+ publish->topic = NULL;
1751
+ publish->qos = ((parser->mqtt_control_packet_type >> 1) & 0x03);
1752
+ rbuf_pop(parser->received_data, (char*)&publish->topic_len, 2);
1753
+ publish->topic_len = be16toh(publish->topic_len);
1754
+ parser->mqtt_parsed_len = 2;
1755
+ if (!publish->topic_len) {
1756
+ parser->varhdr_state = MQTT_PARSE_VARHDR_POST_TOPICNAME;
1757
+ break;
1758
+ }
1759
+ publish->topic = mw_calloc(1, publish->topic_len + 1 /* add 0x00 */);
1760
+ if (publish->topic == NULL)
1761
+ return MQTT_NG_CLIENT_OOM;
1762
+ parser->varhdr_state = MQTT_PARSE_VARHDR_TOPICNAME;
1763
+ /* FALLTHROUGH */
1764
+ case MQTT_PARSE_VARHDR_TOPICNAME:
1765
+ // TODO check empty topic can be valid? In which case we have to skip this step
1766
+ BUF_READ_CHECK_AT_LEAST(parser->received_data, publish->topic_len);
1767
+ rbuf_pop(parser->received_data, publish->topic, publish->topic_len);
1768
+ parser->mqtt_parsed_len += publish->topic_len;
1769
+ parser->varhdr_state = MQTT_PARSE_VARHDR_POST_TOPICNAME;
1770
+ /* FALLTHROUGH */
1771
+ case MQTT_PARSE_VARHDR_POST_TOPICNAME:
1772
+ mqtt_properties_parser_ctx_reset(&parser->properties_parser);
1773
+ if (!publish->qos) { // PacketID present only for QOS > 0 [MQTT-3.3.2.2]
1774
+ parser->varhdr_state = MQTT_PARSE_VARHDR_PROPS;
1775
+ break;
1776
+ }
1777
+ parser->varhdr_state = MQTT_PARSE_VARHDR_PACKET_ID;
1778
+ /* FALLTHROUGH */
1779
+ case MQTT_PARSE_VARHDR_PACKET_ID:
1780
+ BUF_READ_CHECK_AT_LEAST(parser->received_data, 2);
1781
+ rbuf_pop(parser->received_data, (char*)&publish->packet_id, 2);
1782
+ publish->packet_id = be16toh(publish->packet_id);
1783
+ parser->varhdr_state = MQTT_PARSE_VARHDR_PROPS;
1784
+ parser->mqtt_parsed_len += 2;
1785
+ /* FALLTHROUGH */
1786
+ case MQTT_PARSE_VARHDR_PROPS:
1787
+ rc = parse_properties_array(&parser->properties_parser, parser->received_data, client->log);
1788
+ if (rc != MQTT_NG_CLIENT_PARSE_DONE)
1789
+ return rc;
1790
+ parser->mqtt_parsed_len += parser->properties_parser.bytes_consumed;
1791
+ parser->varhdr_state = MQTT_PARSE_PAYLOAD;
1792
+ /* FALLTHROUGH */
1793
+ case MQTT_PARSE_PAYLOAD:
1794
+ if (parser->mqtt_fixed_hdr_remaining_length < parser->mqtt_parsed_len) {
1795
+ mw_free(publish->topic);
1796
+ publish->topic = NULL;
1797
+ ERROR("Error parsing PUBLISH message");
1798
+ return MQTT_NG_CLIENT_PROTOCOL_ERROR;
1799
+ }
1800
+ publish->data_len = parser->mqtt_fixed_hdr_remaining_length - parser->mqtt_parsed_len;
1801
+ if (!publish->data_len) {
1802
+ publish->data = NULL;
1803
+ return MQTT_NG_CLIENT_PARSE_DONE; // 0 length payload is OK [MQTT-3.3.3]
1804
+ }
1805
+ BUF_READ_CHECK_AT_LEAST(parser->received_data, publish->data_len);
1806
+
1807
+ publish->data = mw_malloc(publish->data_len);
1808
+ if (publish->data == NULL) {
1809
+ mw_free(publish->topic);
1810
+ publish->topic = NULL;
1811
+ return MQTT_NG_CLIENT_OOM;
1812
+ }
1813
+
1814
+ rbuf_pop(parser->received_data, publish->data, publish->data_len);
1815
+ parser->mqtt_parsed_len += publish->data_len;
1816
+
1817
+ return MQTT_NG_CLIENT_PARSE_DONE;
1818
+ default:
1819
+ ERROR("invalid state for publish varhdr parser");
1820
+ return MQTT_NG_CLIENT_INTERNAL_ERROR;
1821
+ }
1822
+ return MQTT_NG_CLIENT_OK_CALL_AGAIN;
1823
+}
1824
+
1825
+// TODO move to separate file, dont send whole client pointer just to be able
1826
+// to access LOG context send parser only which should include log
1827
+static int parse_data(struct mqtt_ng_client *client)
1828
+{
1829
+ int rc;
1830
+ struct mqtt_ng_parser *parser = &client->parser;
1831
+ switch(parser->state) {
1832
+ case MQTT_PARSE_FIXED_HEADER_PACKET_TYPE:
1833
+ BUF_READ_CHECK_AT_LEAST(parser->received_data, 1);
1834
+ rbuf_pop(parser->received_data, (char*)&parser->mqtt_control_packet_type, 1);
1835
+ vbi_parser_reset_ctx(&parser->vbi_parser);
1836
+ parser->state = MQTT_PARSE_FIXED_HEADER_LEN;
1837
+ break;
1838
+ case MQTT_PARSE_FIXED_HEADER_LEN:
1839
+ rc = vbi_parser_parse(&parser->vbi_parser, parser->received_data, client->log);
1840
+ if (rc == MQTT_NG_CLIENT_PARSE_DONE) {
1841
+ parser->mqtt_fixed_hdr_remaining_length = parser->vbi_parser.result;
1842
+ parser->state = MQTT_PARSE_VARIABLE_HEADER;
1843
+ parser->varhdr_state = MQTT_PARSE_VARHDR_INITIAL;
1844
+ break;
1845
+ }
1846
+ return rc;
1847
+ case MQTT_PARSE_VARIABLE_HEADER:
1848
+ switch (get_control_packet_type(parser->mqtt_control_packet_type)) {
1849
+ case MQTT_CPT_CONNACK:
1850
+ rc = parse_connack_varhdr(client);
1851
+ if (rc == MQTT_NG_CLIENT_PARSE_DONE) {
1852
+ parser->state = MQTT_PARSE_MQTT_PACKET_DONE;
1853
+ break;
1854
+ }
1855
+ return rc;
1856
+ case MQTT_CPT_PUBACK:
1857
+ rc = parse_puback_varhdr(client);
1858
+ if (rc == MQTT_NG_CLIENT_PARSE_DONE) {
1859
+ parser->state = MQTT_PARSE_MQTT_PACKET_DONE;
1860
+ break;
1861
+ }
1862
+ return rc;
1863
+ case MQTT_CPT_SUBACK:
1864
+ rc = parse_suback_varhdr(client);
1865
+ if (rc != MQTT_NG_CLIENT_NEED_MORE_BYTES && rc != MQTT_NG_CLIENT_OK_CALL_AGAIN) {
1866
+ mw_free(parser->mqtt_packet.suback.reason_codes);
1867
+ }
1868
+ if (rc == MQTT_NG_CLIENT_PARSE_DONE) {
1869
+ parser->state = MQTT_PARSE_MQTT_PACKET_DONE;
1870
+ break;
1871
+ }
1872
+ return rc;
1873
+ case MQTT_CPT_PUBLISH:
1874
+ rc = parse_publish_varhdr(client);
1875
+ if (rc == MQTT_NG_CLIENT_PARSE_DONE) {
1876
+ parser->state = MQTT_PARSE_MQTT_PACKET_DONE;
1877
+ break;
1878
+ }
1879
+ return rc;
1880
+ case MQTT_CPT_PINGRESP:
1881
+ if (parser->mqtt_fixed_hdr_remaining_length) {
1882
+ ERROR ("PINGRESP has to be 0 Remaining Length."); // [MQTT-3.13.1]
1883
+ return MQTT_NG_CLIENT_PROTOCOL_ERROR;
1884
+ }
1885
+ parser->state = MQTT_PARSE_MQTT_PACKET_DONE;
1886
+ break;
1887
+ case MQTT_CPT_DISCONNECT:
1888
+ rc = parse_disconnect_varhdr(client);
1889
+ if (rc == MQTT_NG_CLIENT_PARSE_DONE) {
1890
+ parser->state = MQTT_PARSE_MQTT_PACKET_DONE;
1891
+ break;
1892
+ }
1893
+ return rc;
1894
+ default:
1895
+ ERROR("Parsing Control Packet Type %" PRIu8 " not implemented yet.", get_control_packet_type(parser->mqtt_control_packet_type));
1896
+ rbuf_bump_tail(parser->received_data, parser->mqtt_fixed_hdr_remaining_length);
1897
+ parser->state = MQTT_PARSE_MQTT_PACKET_DONE;
1898
+ return MQTT_NG_CLIENT_NOT_IMPL_YET;
1899
+ }
1900
+ // we could also return MQTT_NG_CLIENT_OK_CALL_AGAIN
1901
+ // and be called again later
1902
+ /* FALLTHROUGH */
1903
+ case MQTT_PARSE_MQTT_PACKET_DONE:
1904
+ parser->state = MQTT_PARSE_FIXED_HEADER_PACKET_TYPE;
1905
+ return MQTT_NG_CLIENT_MQTT_PACKET_DONE;
1906
+ }
1907
+ return MQTT_NG_CLIENT_OK_CALL_AGAIN;
1908
+}
1909
+
1910
+// set next MQTT fragment to send
1911
+// return 1 if nothing to send
1912
+// return -1 on error
1913
+// return 0 if there is fragment set
1914
+static int mqtt_ng_next_to_send(struct mqtt_ng_client *client) {
1915
+ if (client->client_state == CONNECT_PENDING) {
1916
+ client->main_buffer.sending_frag = client->connect_msg;
1917
+ client->client_state = CONNECTING;
1918
+ return 0;
1919
+ }
1920
+ if (client->client_state != CONNECTED)
1921
+ return -1;
1922
+
1923
+ struct buffer_fragment *frag = BUFFER_FIRST_FRAG(&client->main_buffer.hdr_buffer);
1924
+ while (frag) {
1925
+ if ( frag->sent != frag->len )
1926
+ break;
1927
+ frag = frag->next;
1928
+ }
1929
+
1930
+ if ( client->ping_pending && (!frag || (frag->flags & BUFFER_FRAG_MQTT_PACKET_HEAD && frag->sent == 0)) ) {
1931
+ client->ping_pending = 0;
1932
+ ping_frag.sent = 0;
1933
+ client->main_buffer.sending_frag = &ping_frag;
1934
+ return 0;
1935
+ }
1936
+
1937
+ client->main_buffer.sending_frag = frag;
1938
+ return frag == NULL ? 1 : 0;
1939
+}
1940
+
1941
+// send current fragment
1942
+// return 0 if whole remaining length could be sent as a whole
1943
+// return -1 if send buffer was filled and
1944
+// nothing could be written anymore
1945
+// return 1 if last fragment of a message was fully sent
1946
+static int send_fragment(struct mqtt_ng_client *client) {
1947
+ struct buffer_fragment *frag = client->main_buffer.sending_frag;
1948
+
1949
+ // for readability
1950
+ char *ptr = frag->data + frag->sent;
1951
+ size_t bytes = frag->len - frag->sent;
1952
+
1953
+ size_t processed = 0;
1954
+
1955
+ if (bytes)
1956
+ processed = client->send_fnc_ptr(client->user_ctx, ptr, bytes);
1957
+ else
1958
+ WARN("This fragment was fully sent already. This should not happen!");
1959
+
1960
+ frag->sent += processed;
1961
+ if (frag->sent != frag->len)
1962
+ return -1;
1963
+
1964
+ if (frag->flags & BUFFER_FRAG_MQTT_PACKET_TAIL) {
1965
+ client->time_of_last_send = time(NULL);
1966
+ pthread_mutex_lock(&client->stats_mutex);
1967
+ if (client->main_buffer.sending_frag != &ping_frag)
1968
+ client->stats.tx_messages_queued--;
1969
+ client->stats.tx_messages_sent++;
1970
+ pthread_mutex_unlock(&client->stats_mutex);
1971
+ client->main_buffer.sending_frag = NULL;
1972
+ return 1;
1973
+ }
1974
+
1975
+ client->main_buffer.sending_frag = frag->next;
1976
+
1977
+ return 0;
1978
+}
1979
+
1980
+// attempt sending all fragments of current single MQTT packet
1981
+static int send_all_message_fragments(struct mqtt_ng_client *client) {
1982
+ int rc;
1983
+ while ( !(rc = send_fragment(client)) );
1984
+ return rc;
1985
+}
1986
+
1987
+static void try_send_all(struct mqtt_ng_client *client) {
1988
+ do {
1989
+ if (client->main_buffer.sending_frag == NULL && mqtt_ng_next_to_send(client))
1990
+ return;
1991
+ } while(send_all_message_fragments(client) >= 0);
1992
+}
1993
+
1994
+static inline void mark_message_for_gc(struct buffer_fragment *frag)
1995
+{
1996
+ while (frag) {
1997
+ frag->flags |= BUFFER_FRAG_GARBAGE_COLLECT;
1998
+ buffer_frag_free_data(frag);
1999
+ if (frag->flags & BUFFER_FRAG_MQTT_PACKET_TAIL)
2000
+ return;
2001
+ frag = frag->next;
2002
+ }
2003
+}
2004
+
2005
+static int mark_packet_acked(struct mqtt_ng_client *client, uint16_t packet_id)
2006
+{
2007
+ LOCK_HDR_BUFFER(&client->main_buffer);
2008
+ struct buffer_fragment *frag = BUFFER_FIRST_FRAG(&client->main_buffer.hdr_buffer);
2009
+ while (frag) {
2010
+ if ( (frag->flags & BUFFER_FRAG_MQTT_PACKET_HEAD) && frag->packet_id == packet_id) {
2011
+ if (!frag->sent) {
2012
+ ERROR("Received packet_id (%" PRIu16 ") belongs to MQTT packet which was not yet sent!", packet_id);
2013
+ UNLOCK_HDR_BUFFER(&client->main_buffer);
2014
+ return 1;
2015
+ }
2016
+ mark_message_for_gc(frag);
2017
+ UNLOCK_HDR_BUFFER(&client->main_buffer);
2018
+ return 0;
2019
+ }
2020
+ frag = frag->next;
2021
+ }
2022
+ ERROR("Received packet_id (%" PRIu16 ") is unknown!", packet_id);
2023
+ UNLOCK_HDR_BUFFER(&client->main_buffer);
2024
+ return 1;
2025
+}
2026
+
2027
+int handle_incoming_traffic(struct mqtt_ng_client *client)
2028
+{
2029
+ int rc;
2030
+ struct mqtt_publish *pub;
2031
+ while( (rc = parse_data(client)) == MQTT_NG_CLIENT_OK_CALL_AGAIN );
2032
+ if ( rc == MQTT_NG_CLIENT_MQTT_PACKET_DONE ) {
2033
+ struct mqtt_property *prop;
2034
+#ifdef MQTT_DEBUG_VERBOSE
2035
+ DEBUG("MQTT Packet Parsed Successfully!");
2036
+#endif
2037
+ pthread_mutex_lock(&client->stats_mutex);
2038
+ client->stats.rx_messages_rcvd++;
2039
+ pthread_mutex_unlock(&client->stats_mutex);
2040
+
2041
+ switch (get_control_packet_type(client->parser.mqtt_control_packet_type)) {
2042
+ case MQTT_CPT_CONNACK:
2043
+#ifdef MQTT_DEBUG_VERBOSE
2044
+ DEBUG("Received CONNACK");
2045
+#endif
2046
+ LOCK_HDR_BUFFER(&client->main_buffer);
2047
+ mark_message_for_gc(client->connect_msg);
2048
+ UNLOCK_HDR_BUFFER(&client->main_buffer);
2049
+ client->connect_msg = NULL;
2050
+ if (client->client_state != CONNECTING) {
2051
+ ERROR("Received unexpected CONNACK");
2052
+ client->client_state = ERROR;
2053
+ return MQTT_NG_CLIENT_PROTOCOL_ERROR;
2054
+ }
2055
+ if ((prop = get_property_by_id(client->parser.properties_parser.head, MQTT_PROP_MAX_PKT_SIZE)) != NULL) {
2056
+ INFO("MQTT server limits message size to %" PRIu32, prop->data.uint32);
2057
+ client->max_msg_size = prop->data.uint32;
2058
+ }
2059
+ if (client->connack_callback)
2060
+ client->connack_callback(client->user_ctx, client->parser.mqtt_packet.connack.reason_code);
2061
+ if (!client->parser.mqtt_packet.connack.reason_code) {
2062
+ INFO("MQTT Connection Accepted By Server");
2063
+ client->client_state = CONNECTED;
2064
+ break;
2065
+ }
2066
+ client->client_state = ERROR;
2067
+ return MQTT_NG_CLIENT_SERVER_RETURNED_ERROR;
2068
+ case MQTT_CPT_PUBACK:
2069
+#ifdef MQTT_DEBUG_VERBOSE
2070
+ DEBUG("Received PUBACK %" PRIu16, client->parser.mqtt_packet.puback.packet_id);
2071
+#endif
2072
+ if (mark_packet_acked(client, client->parser.mqtt_packet.puback.packet_id))
2073
+ return MQTT_NG_CLIENT_PROTOCOL_ERROR;
2074
+ if (client->puback_callback)
2075
+ client->puback_callback(client->parser.mqtt_packet.puback.packet_id);
2076
+ break;
2077
+ case MQTT_CPT_PINGRESP:
2078
+#ifdef MQTT_DEBUG_VERBOSE
2079
+ DEBUG("Received PINGRESP");
2080
+#endif
2081
+ break;
2082
+ case MQTT_CPT_SUBACK:
2083
+#ifdef MQTT_DEBUG_VERBOSE
2084
+ DEBUG("Received SUBACK %" PRIu16, client->parser.mqtt_packet.suback.packet_id);
2085
+#endif
2086
+ if (mark_packet_acked(client, client->parser.mqtt_packet.suback.packet_id))
2087
+ return MQTT_NG_CLIENT_PROTOCOL_ERROR;
2088
+ break;
2089
+ case MQTT_CPT_PUBLISH:
2090
+#ifdef MQTT_DEBUG_VERBOSE
2091
+ DEBUG("Recevied PUBLISH");
2092
+#endif
2093
+ pub = &client->parser.mqtt_packet.publish;
2094
+ if (pub->qos > 1) {
2095
+ mw_free(pub->topic);
2096
+ mw_free(pub->data);
2097
+ return MQTT_NG_CLIENT_NOT_IMPL_YET;
2098
+ }
2099
+ if ( pub->qos == 1 && (rc = mqtt_ng_puback(client, pub->packet_id, 0)) ) {
2100
+ client->client_state = ERROR;
2101
+ ERROR("Error generating PUBACK reply for PUBLISH");
2102
+ return rc;
2103
+ }
2104
+ if ( (prop = get_property_by_id(client->parser.properties_parser.head, MQTT_PROP_TOPIC_ALIAS)) != NULL ) {
2105
+ // Topic Alias property was sent from server
2106
+ void *topic_ptr;
2107
+ if (!c_rhash_get_ptr_by_uint64(client->rx_aliases, prop->data.uint8, &topic_ptr)) {
2108
+ if (pub->topic != NULL) {
2109
+ ERROR("We do not yet support topic alias reassignment");
2110
+ return MQTT_NG_CLIENT_NOT_IMPL_YET;
2111
+ }
2112
+ pub->topic = topic_ptr;
2113
+ } else {
2114
+ if (pub->topic == NULL) {
2115
+ ERROR("Topic alias with id %d unknown and topic not set by server!", prop->data.uint8);
2116
+ return MQTT_NG_CLIENT_PROTOCOL_ERROR;
2117
+ }
2118
+ c_rhash_insert_uint64_ptr(client->rx_aliases, prop->data.uint8, pub->topic);
2119
+ }
2120
+ }
2121
+ if (client->msg_callback)
2122
+ client->msg_callback(pub->topic, pub->data, pub->data_len, pub->qos);
2123
+ // in case we have property topic alias and we have topic we take over the string
2124
+ // and add pointer to it into topic alias list
2125
+ if (prop == NULL)
2126
+ mw_free(pub->topic);
2127
+ mw_free(pub->data);
2128
+ return MQTT_NG_CLIENT_WANT_WRITE;
2129
+ case MQTT_CPT_DISCONNECT:
2130
+ INFO ("Got MQTT DISCONNECT control packet from server. Reason code: %d", (int)client->parser.mqtt_packet.disconnect.reason_code);
2131
+ client->client_state = DISCONNECTED;
2132
+ break;
2133
+ }
2134
+ }
2135
+
2136
+ return rc;
2137
+}
2138
+
2139
+int mqtt_ng_sync(struct mqtt_ng_client *client)
2140
+{
2141
+ if (client->client_state == RAW || client->client_state == DISCONNECTED)
2142
+ return 0;
2143
+
2144
+ if (client->client_state == ERROR)
2145
+ return 1;
2146
+
2147
+ LOCK_HDR_BUFFER(&client->main_buffer);
2148
+ try_send_all(client);
2149
+ UNLOCK_HDR_BUFFER(&client->main_buffer);
2150
+
2151
+ int rc;
2152
+
2153
+ while ((rc = handle_incoming_traffic(client)) != MQTT_NG_CLIENT_NEED_MORE_BYTES) {
2154
+ if (rc < 0)
2155
+ break;
2156
+ if (rc == MQTT_NG_CLIENT_WANT_WRITE) {
2157
+ LOCK_HDR_BUFFER(&client->main_buffer);
2158
+ try_send_all(client);
2159
+ UNLOCK_HDR_BUFFER(&client->main_buffer);
2160
+ }
2161
+ }
2162
+
2163
+ if (rc < 0)
2164
+ return rc;
2165
+
2166
+ return 0;
2167
+}
2168
+
2169
+time_t mqtt_ng_last_send_time(struct mqtt_ng_client *client)
2170
+{
2171
+ return client->time_of_last_send;
2172
+}
2173
+
2174
+void mqtt_ng_set_max_mem(struct mqtt_ng_client *client, size_t bytes)
2175
+{
2176
+ client->max_mem_bytes = bytes;
2177
+}
2178
+
2179
+void mqtt_ng_get_stats(struct mqtt_ng_client *client, struct mqtt_ng_stats *stats)
2180
+{
2181
+ pthread_mutex_lock(&client->stats_mutex);
2182
+ memcpy(stats, &client->stats, sizeof(struct mqtt_ng_stats));
2183
+ pthread_mutex_unlock(&client->stats_mutex);
2184
+
2185
+ stats->tx_bytes_queued = 0;
2186
+ stats->tx_buffer_reclaimable = 0;
2187
+
2188
+ LOCK_HDR_BUFFER(&client->main_buffer);
2189
+ stats->tx_buffer_used = BUFFER_BYTES_USED(&client->main_buffer.hdr_buffer);
2190
+ stats->tx_buffer_free = BUFFER_BYTES_AVAILABLE(&client->main_buffer.hdr_buffer);
2191
+ stats->tx_buffer_size = client->main_buffer.hdr_buffer.size;
2192
+ struct buffer_fragment *frag = BUFFER_FIRST_FRAG(&client->main_buffer.hdr_buffer);
2193
+ while (frag) {
2194
+ stats->tx_bytes_queued += frag->len - frag->sent;
2195
+ if (frag_is_marked_for_gc(frag))
2196
+ stats->tx_buffer_reclaimable += FRAG_SIZE_IN_BUFFER(frag);
2197
+
2198
+ frag = frag->next;
2199
+ }
2200
+ UNLOCK_HDR_BUFFER(&client->main_buffer);
2201
+}
2202
+
2203
+int mqtt_ng_set_topic_alias(struct mqtt_ng_client *client, const char *topic)
2204
+{
2205
+ uint16_t idx;
2206
+ pthread_rwlock_wrlock(&client->tx_topic_aliases.rwlock);
2207
+
2208
+ if (client->tx_topic_aliases.idx_assigned >= client->tx_topic_aliases.idx_max) {
2209
+ pthread_rwlock_unlock(&client->tx_topic_aliases.rwlock);
2210
+ mws_error(client->log, "Tx topic alias indexes were exhausted (current version of the library doesn't support reassigning yet. Feel free to contribute.");
2211
+ return 0; //0 is not a valid topic alias
2212
+ }
2213
+
2214
+ struct topic_alias_data *alias;
2215
+ if (!c_rhash_get_ptr_by_str(client->tx_topic_aliases.stoi_dict, topic, (void**)&alias)) {
2216
+ // this is not a problem for library but might be helpful to warn user
2217
+ // as it might indicate bug in their program (but also might be expected)
2218
+ idx = alias->idx;
2219
+ pthread_rwlock_unlock(&client->tx_topic_aliases.rwlock);
2220
+ mws_debug(client->log, "%s topic \"%s\" already has alias set. Ignoring.", __FUNCTION__, topic);
2221
+ return idx;
2222
+ }
2223
+
2224
+ alias = mw_malloc(sizeof(struct topic_alias_data));
2225
+ idx = ++client->tx_topic_aliases.idx_assigned;
2226
+ alias->idx = idx;
2227
+ __atomic_store_n(&alias->usage_count, 0, __ATOMIC_SEQ_CST);
2228
+
2229
+ c_rhash_insert_str_ptr(client->tx_topic_aliases.stoi_dict, topic, (void*)alias);
2230
+
2231
+ pthread_rwlock_unlock(&client->tx_topic_aliases.rwlock);
2232
+ return idx;
2233
+}