mqttpacket.cpp
3.32 KB
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
#include "mqttpacket.h"
#include <cstring>
MqttPacket::MqttPacket(char *buf, size_t len, size_t fixed_header_length, Client *sender) :
bites(len),
fixed_header_length(fixed_header_length),
sender(sender)
{
unsigned char _packetType = buf[0] >> 4;
packetType = (PacketType)_packetType;
pos += fixed_header_length;
std::memcpy(&bites[0], buf, len);
variable_header_length = readTwoBytesToUInt16();
}
void MqttPacket::handle()
{
if (packetType == PacketType::CONNECT)
handleConnect();
}
void MqttPacket::handleConnect()
{
if (sender->hasConnectPacketSeen())
throw ProtocolError("Client already sent a CONNECT.");
if (variable_header_length == 4 || variable_header_length == 6)
{
char *c = readBytes(variable_header_length);
std::string magic_marker(c, variable_header_length);
char protocol_level = readByte();
if (magic_marker == "MQTT" && protocol_level == 0x04)
{
protocolVersion = ProtocolVersion::Mqtt311;
}
else if (magic_marker == "MQIsdp" && protocol_level == 0x03)
{
protocolVersion = ProtocolVersion::Mqtt31;
}
else
{
throw ProtocolError("Only MQTT 3.1 and 3.1.1 supported.");
}
char flagByte = readByte();
bool reserved = !!(flagByte & 0b00000001);
if (reserved)
throw ProtocolError("Protocol demands reserved flag in CONNECT is 0");
bool user_name_flag = !!(flagByte & 0b10000000);
bool password_flag = !!(flagByte & 0b01000000);
bool will_retain = !!(flagByte & 0b00100000);
char will_qos = (flagByte & 0b00011000) >> 3;
bool will_flag = !!(flagByte & 0b00000100);
bool clean_session = !!(flagByte & 0b00000010);
uint16_t keep_alive = readTwoBytesToUInt16();
uint16_t client_id_length = readTwoBytesToUInt16();
std::string client_id(readBytes(client_id_length), client_id_length);
std::string username;
std::string password;
if (will_flag)
{
}
if (user_name_flag)
{
uint16_t user_name_length = readTwoBytesToUInt16();
username = std::string(readBytes(user_name_length), user_name_length);
}
if (password_flag)
{
uint16_t password_length = readTwoBytesToUInt16();
password = std::string(readBytes(password_length), password_length);
}
// TODO: validate UTF8 encoded username/password.
sender->setClientProperties(client_id, username, true, keep_alive);
}
else
{
throw ProtocolError("Invalid variable header length. Garbage?");
}
}
char *MqttPacket::readBytes(size_t length)
{
if (pos + length > bites.size())
throw ProtocolError("Invalid packet: header specifies invalid length.");
char *b = &bites[pos];
pos += length;
return b;
}
char MqttPacket::readByte()
{
if (pos + 1 > bites.size())
throw ProtocolError("Invalid packet: header specifies invalid length.");
char b = bites[pos++];
return b;
}
uint16_t MqttPacket::readTwoBytesToUInt16()
{
if (pos + 2 > bites.size())
throw ProtocolError("Invalid packet: header specifies invalid length.");
uint16_t i = bites[pos] << 8 | bites[pos+1];
pos += 2;
return i;
}