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
|
#include "mqtt_parser.h"
#include <assert.h>
#include <setjmp.h>
#include <stdint.h>
struct BufferView {
const uint8_t* data;
size_t size;
};
struct ConnectAck {
uint8_t packet_type;
uint8_t message_length;
uint8_t connack_flags;
uint8_t return_code;
} __attribute__((packed));
struct SubscribeAck {
uint8_t packet_type;
uint8_t message_length;
uint16_t packet_identifier;
uint8_t return_code;
} __attribute__((packed));
static_assert(sizeof(struct ConnectAck) == 4, "Unexpected connect ack size");
static_assert(sizeof(struct SubscribeAck) == 5,
"Unexpected subscribe ack size");
static enum MqttParserResult ParseConnectAck(
const void* buffer, size_t buffer_size,
const struct MqttParserCallbacks* callbacks, void* user) {
if (buffer_size < sizeof(struct ConnectAck)) return kMqttParserResultWantMore;
const struct ConnectAck* connect_ack = buffer;
if (connect_ack->message_length != 2) return kMqttParserResultError;
callbacks->on_connect_ack(user, !connect_ack->return_code);
callbacks->on_finished(user, sizeof(struct ConnectAck));
return kMqttParserResultFinished;
}
static enum MqttParserResult ReadVarint(struct BufferView* buffer_view,
size_t* result) {
*result = 0;
for (size_t counter = 0; counter < 4; counter++) {
if (!buffer_view->size) return kMqttParserResultWantMore;
uint8_t byte = *buffer_view->data++;
buffer_view->size--;
*result |= (byte & 0x7full) << (7ull * counter);
if (~byte & 0x80) return kMqttParserResultFinished;
}
return kMqttParserResultError;
}
static enum MqttParserResult ParsePublish(
const void* buffer, size_t buffer_size,
const struct MqttParserCallbacks* callbacks, void* user) {
size_t message_length;
const uint8_t* data = buffer;
struct BufferView publish = {.data = data + 1, .size = buffer_size - 1};
enum MqttParserResult result = ReadVarint(&publish, &message_length);
if (result != kMqttParserResultFinished) return result;
if (publish.size < message_length) return kMqttParserResultWantMore;
publish.size = message_length;
uint16_t topic_size = (publish.data[0] << 8 | publish.data[1]) & 0xffff;
const char* topic = (const char*)&publish.data[2];
const uint8_t* payload = publish.data + 2 + topic_size;
size_t payload_size = publish.size - 2 - topic_size;
callbacks->on_publish(user, topic, topic_size, payload, payload_size);
callbacks->on_finished(user, (size_t)(publish.data + publish.size - data));
return kMqttParserResultFinished;
}
static enum MqttParserResult ParseSubscribeAck(
const void* buffer, size_t buffer_size,
const struct MqttParserCallbacks* callbacks, void* user) {
if (buffer_size < sizeof(struct SubscribeAck))
return kMqttParserResultWantMore;
const struct SubscribeAck* subscribe_ack = buffer;
if (subscribe_ack->message_length != 3) return kMqttParserResultError;
callbacks->on_subscribe_ack(user, subscribe_ack->return_code < 3);
callbacks->on_finished(user, sizeof(struct SubscribeAck));
return kMqttParserResultFinished;
}
enum MqttParserResult MqttParserParse(
const void* buffer, size_t buffer_size,
const struct MqttParserCallbacks* callbacks, void* user) {
if (!buffer_size) return kMqttParserResultWantMore;
const uint8_t* data = buffer;
switch (data[0] & 0xf0) {
case 0x20:
return ParseConnectAck(buffer, buffer_size, callbacks, user);
case 0x30:
return ParsePublish(buffer, buffer_size, callbacks, user);
case 0x90:
return ParseSubscribeAck(buffer, buffer_size, callbacks, user);
default:
return kMqttParserResultError;
}
}
|
