1 | import socket
|
2 | import struct
|
3 |
|
4 | def expect_packet(sock, name, expected):
|
5 | if len(expected) > 0:
|
6 | rlen = len(expected)
|
7 | else:
|
8 | rlen = 1
|
9 |
|
10 | packet_recvd = sock.recv(rlen)
|
11 | return packet_matches(name, packet_recvd, expected)
|
12 |
|
13 | def packet_matches(name, recvd, expected):
|
14 | if recvd != expected:
|
15 | print("FAIL: Received incorrect "+name+".")
|
16 | try:
|
17 | print("Received: "+to_string(recvd))
|
18 | except struct.error:
|
19 | print("Received (not decoded): "+recvd)
|
20 | try:
|
21 | print("Expected: "+to_string(expected))
|
22 | except struct.error:
|
23 | print("Expected (not decoded): "+expected)
|
24 |
|
25 | return 0
|
26 | else:
|
27 | return 1
|
28 |
|
29 | def do_client_connect(connect_packet, connack_packet, hostname="localhost", port=1888, timeout=60, connack_error="connack"):
|
30 | sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
31 | sock.settimeout(timeout)
|
32 | sock.connect((hostname, port))
|
33 | sock.send(connect_packet)
|
34 |
|
35 | if expect_packet(sock, connack_error, connack_packet):
|
36 | return sock
|
37 | else:
|
38 | sock.close()
|
39 | raise ValueError
|
40 |
|
41 | def remaining_length(packet):
|
42 | l = min(5, len(packet))
|
43 | all_bytes = struct.unpack("!"+"B"*l, packet[:l])
|
44 | mult = 1
|
45 | rl = 0
|
46 | for i in range(1,l-1):
|
47 | byte = all_bytes[i]
|
48 |
|
49 | rl += (byte & 127) * mult
|
50 | mult *= 128
|
51 | if byte & 128 == 0:
|
52 | packet = packet[i+1:]
|
53 | break
|
54 |
|
55 | return (packet, rl)
|
56 |
|
57 |
|
58 | def to_string(packet):
|
59 | if len(packet) == 0:
|
60 | return ""
|
61 |
|
62 | packet0 = struct.unpack("!B", packet[0])
|
63 | packet0 = packet0[0]
|
64 | cmd = packet0 & 0xF0
|
65 | if cmd == 0x00:
|
66 |
|
67 | return "0x00"
|
68 | elif cmd == 0x10:
|
69 |
|
70 | (packet, rl) = remaining_length(packet)
|
71 | pack_format = "!H" + str(len(packet)-2) + 's'
|
72 | (slen, packet) = struct.unpack(pack_format, packet)
|
73 | pack_format = "!" + str(slen)+'sBBH' + str(len(packet)-slen-4) + 's'
|
74 | (protocol, proto_ver, flags, keepalive, packet) = struct.unpack(pack_format, packet)
|
75 | s = "CONNECT, proto="+protocol+str(proto_ver)+", keepalive="+str(keepalive)
|
76 | if flags&2:
|
77 | s = s+", clean-session"
|
78 | else:
|
79 | s = s+", durable"
|
80 |
|
81 | pack_format = "!H" + str(len(packet)-2) + 's'
|
82 | (slen, packet) = struct.unpack(pack_format, packet)
|
83 | pack_format = "!" + str(slen)+'s' + str(len(packet)-slen) + 's'
|
84 | (client_id, packet) = struct.unpack(pack_format, packet)
|
85 | s = s+", id="+client_id
|
86 |
|
87 | if flags&4:
|
88 | pack_format = "!H" + str(len(packet)-2) + 's'
|
89 | (slen, packet) = struct.unpack(pack_format, packet)
|
90 | pack_format = "!" + str(slen)+'s' + str(len(packet)-slen) + 's'
|
91 | (will_topic, packet) = struct.unpack(pack_format, packet)
|
92 | s = s+", will-topic="+will_topic
|
93 |
|
94 | pack_format = "!H" + str(len(packet)-2) + 's'
|
95 | (slen, packet) = struct.unpack(pack_format, packet)
|
96 | pack_format = "!" + str(slen)+'s' + str(len(packet)-slen) + 's'
|
97 | (will_message, packet) = struct.unpack(pack_format, packet)
|
98 | s = s+", will-message="+will_message
|
99 |
|
100 | s = s+", will-qos="+str((flags&24)>>3)
|
101 | s = s+", will-retain="+str((flags&32)>>5)
|
102 |
|
103 | if flags&128:
|
104 | pack_format = "!H" + str(len(packet)-2) + 's'
|
105 | (slen, packet) = struct.unpack(pack_format, packet)
|
106 | pack_format = "!" + str(slen)+'s' + str(len(packet)-slen) + 's'
|
107 | (username, packet) = struct.unpack(pack_format, packet)
|
108 | s = s+", username="+username
|
109 |
|
110 | if flags&64:
|
111 | pack_format = "!H" + str(len(packet)-2) + 's'
|
112 | (slen, packet) = struct.unpack(pack_format, packet)
|
113 | pack_format = "!" + str(slen)+'s' + str(len(packet)-slen) + 's'
|
114 | (password, packet) = struct.unpack(pack_format, packet)
|
115 | s = s+", password="+password
|
116 |
|
117 | return s
|
118 | elif cmd == 0x20:
|
119 |
|
120 | (cmd, rl, resv, rc) = struct.unpack('!BBBB', packet)
|
121 | return "CONNACK, rl="+str(rl)+", res="+str(resv)+", rc="+str(rc)
|
122 | elif cmd == 0x30:
|
123 |
|
124 | dup = (packet0 & 0x08)>>3
|
125 | qos = (packet0 & 0x06)>>1
|
126 | retain = (packet0 & 0x01)
|
127 | (packet, rl) = remaining_length(packet)
|
128 | pack_format = "!H" + str(len(packet)-2) + 's'
|
129 | (tlen, packet) = struct.unpack(pack_format, packet)
|
130 | pack_format = "!" + str(tlen)+'s' + str(len(packet)-tlen) + 's'
|
131 | (topic, packet) = struct.unpack(pack_format, packet)
|
132 | s = "PUBLISH, rl="+str(rl)+", topic="+topic+", qos="+str(qos)+", retain="+str(retain)+", dup="+str(dup)
|
133 | if qos > 0:
|
134 | pack_format = "!H" + str(len(packet)-2) + 's'
|
135 | (mid, packet) = struct.unpack(pack_format, packet)
|
136 | s = s + ", mid="+str(mid)
|
137 |
|
138 | s = s + ", payload="+packet
|
139 | return s
|
140 | elif cmd == 0x40:
|
141 |
|
142 | (cmd, rl, mid) = struct.unpack('!BBH', packet)
|
143 | return "PUBACK, rl="+str(rl)+", mid="+str(mid)
|
144 | elif cmd == 0x50:
|
145 |
|
146 | (cmd, rl, mid) = struct.unpack('!BBH', packet)
|
147 | return "PUBREC, rl="+str(rl)+", mid="+str(mid)
|
148 | elif cmd == 0x60:
|
149 |
|
150 | dup = (packet0 & 0x08)>>3
|
151 | (cmd, rl, mid) = struct.unpack('!BBH', packet)
|
152 | return "PUBREL, rl="+str(rl)+", mid="+str(mid)+", dup="+str(dup)
|
153 | elif cmd == 0x70:
|
154 |
|
155 | (cmd, rl, mid) = struct.unpack('!BBH', packet)
|
156 | return "PUBCOMP, rl="+str(rl)+", mid="+str(mid)
|
157 | elif cmd == 0x80:
|
158 |
|
159 | (packet, rl) = remaining_length(packet)
|
160 | pack_format = "!H" + str(len(packet)-2) + 's'
|
161 | (mid, packet) = struct.unpack(pack_format, packet)
|
162 | s = "SUBSCRIBE, rl="+str(rl)+", mid="+str(mid)
|
163 | topic_index = 0
|
164 | while len(packet) > 0:
|
165 | pack_format = "!H" + str(len(packet)-2) + 's'
|
166 | (tlen, packet) = struct.unpack(pack_format, packet)
|
167 | pack_format = "!" + str(tlen)+'sB' + str(len(packet)-tlen-1) + 's'
|
168 | (topic, qos, packet) = struct.unpack(pack_format, packet)
|
169 | s = s + ", topic"+str(topic_index)+"="+topic+","+str(qos)
|
170 | return s
|
171 | elif cmd == 0x90:
|
172 |
|
173 | (packet, rl) = remaining_length(packet)
|
174 | pack_format = "!H" + str(len(packet)-2) + 's'
|
175 | (mid, packet) = struct.unpack(pack_format, packet)
|
176 | pack_format = "!" + "B"*len(packet)
|
177 | granted_qos = struct.unpack(pack_format, packet)
|
178 |
|
179 | s = "SUBACK, rl="+str(rl)+", mid="+str(mid)+", granted_qos="+str(granted_qos[0])
|
180 | for i in range(1, len(granted_qos)-1):
|
181 | s = s+", "+str(granted_qos[i])
|
182 | return s
|
183 | elif cmd == 0xA0:
|
184 |
|
185 | (packet, rl) = remaining_length(packet)
|
186 | pack_format = "!H" + str(len(packet)-2) + 's'
|
187 | (mid, packet) = struct.unpack(pack_format, packet)
|
188 | s = "UNSUBSCRIBE, rl="+str(rl)+", mid="+str(mid)
|
189 | topic_index = 0
|
190 | while len(packet) > 0:
|
191 | pack_format = "!H" + str(len(packet)-2) + 's'
|
192 | (tlen, packet) = struct.unpack(pack_format, packet)
|
193 | pack_format = "!" + str(tlen)+'s' + str(len(packet)-tlen) + 's'
|
194 | (topic, packet) = struct.unpack(pack_format, packet)
|
195 | s = s + ", topic"+str(topic_index)+"="+topic
|
196 | return s
|
197 | elif cmd == 0xB0:
|
198 |
|
199 | (cmd, rl, mid) = struct.unpack('!BBH', packet)
|
200 | return "UNSUBACK, rl="+str(rl)+", mid="+str(mid)
|
201 | elif cmd == 0xC0:
|
202 |
|
203 | (cmd, rl) = struct.unpack('!BB', packet)
|
204 | return "PINGREQ, rl="+str(rl)
|
205 | elif cmd == 0xD0:
|
206 |
|
207 | (cmd, rl) = struct.unpack('!BB', packet)
|
208 | return "PINGRESP, rl="+str(rl)
|
209 | elif cmd == 0xE0:
|
210 |
|
211 | (cmd, rl) = struct.unpack('!BB', packet)
|
212 | return "DISCONNECT, rl="+str(rl)
|
213 | elif cmd == 0xF0:
|
214 |
|
215 | return "0xF0"
|
216 |
|
217 | def gen_connect(client_id, clean_session=True, keepalive=60, username=None, password=None, will_topic=None, will_qos=0, will_retain=False, will_payload="", proto_ver=3):
|
218 | if (proto_ver&0x7F) == 3 or proto_ver == 0:
|
219 | remaining_length = 12
|
220 | elif (proto_ver&0x7F) == 4:
|
221 | remaining_length = 10
|
222 | else:
|
223 | raise ValueError
|
224 |
|
225 | if client_id != None:
|
226 | remaining_length = remaining_length + 2+len(client_id)
|
227 |
|
228 | connect_flags = 0
|
229 | if clean_session:
|
230 | connect_flags = connect_flags | 0x02
|
231 |
|
232 | if will_topic != None:
|
233 | remaining_length = remaining_length + 2+len(will_topic) + 2+len(will_payload)
|
234 | connect_flags = connect_flags | 0x04 | ((will_qos&0x03) << 3)
|
235 | if will_retain:
|
236 | connect_flags = connect_flags | 32
|
237 |
|
238 | if username != None:
|
239 | remaining_length = remaining_length + 2+len(username)
|
240 | connect_flags = connect_flags | 0x80
|
241 | if password != None:
|
242 | connect_flags = connect_flags | 0x40
|
243 | remaining_length = remaining_length + 2+len(password)
|
244 |
|
245 | rl = pack_remaining_length(remaining_length)
|
246 | packet = struct.pack("!B"+str(len(rl))+"s", 0x10, rl)
|
247 | if (proto_ver&0x7F) == 3 or proto_ver == 0:
|
248 | packet = packet + struct.pack("!H6sBBH", len("MQIsdp"), "MQIsdp", proto_ver, connect_flags, keepalive)
|
249 | elif (proto_ver&0x7F) == 4:
|
250 | packet = packet + struct.pack("!H4sBBH", len("MQTT"), "MQTT", proto_ver, connect_flags, keepalive)
|
251 |
|
252 | if client_id != None:
|
253 | packet = packet + struct.pack("!H"+str(len(client_id))+"s", len(client_id), client_id)
|
254 |
|
255 | if will_topic != None:
|
256 | packet = packet + struct.pack("!H"+str(len(will_topic))+"s", len(will_topic), will_topic)
|
257 | if len(will_payload) > 0:
|
258 | packet = packet + struct.pack("!H"+str(len(will_payload))+"s", len(will_payload), will_payload)
|
259 | else:
|
260 | packet = packet + struct.pack("!H", 0)
|
261 |
|
262 | if username != None:
|
263 | packet = packet + struct.pack("!H"+str(len(username))+"s", len(username), username)
|
264 | if password != None:
|
265 | packet = packet + struct.pack("!H"+str(len(password))+"s", len(password), password)
|
266 | return packet
|
267 |
|
268 | def gen_connack(resv=0, rc=0):
|
269 | return struct.pack('!BBBB', 32, 2, resv, rc);
|
270 |
|
271 | def gen_publish(topic, qos, payload=None, retain=False, dup=False, mid=0):
|
272 | rl = 2+len(topic)
|
273 | pack_format = "!BBH"+str(len(topic))+"s"
|
274 | if qos > 0:
|
275 | rl = rl + 2
|
276 | pack_format = pack_format + "H"
|
277 | if payload != None:
|
278 | rl = rl + len(payload)
|
279 | pack_format = pack_format + str(len(payload))+"s"
|
280 | else:
|
281 | payload = ""
|
282 | pack_format = pack_format + "0s"
|
283 |
|
284 | cmd = 48 | (qos<<1)
|
285 | if retain:
|
286 | cmd = cmd + 1
|
287 | if dup:
|
288 | cmd = cmd + 8
|
289 |
|
290 | if qos > 0:
|
291 | return struct.pack(pack_format, cmd, rl, len(topic), topic, mid, payload)
|
292 | else:
|
293 | return struct.pack(pack_format, cmd, rl, len(topic), topic, payload)
|
294 |
|
295 | def gen_puback(mid):
|
296 | return struct.pack('!BBH', 64, 2, mid)
|
297 |
|
298 | def gen_pubrec(mid):
|
299 | return struct.pack('!BBH', 80, 2, mid)
|
300 |
|
301 | def gen_pubrel(mid, dup=False):
|
302 | if dup:
|
303 | cmd = 96+8+2
|
304 | else:
|
305 | cmd = 96+2
|
306 | return struct.pack('!BBH', cmd, 2, mid)
|
307 |
|
308 | def gen_pubcomp(mid):
|
309 | return struct.pack('!BBH', 112, 2, mid)
|
310 |
|
311 | def gen_subscribe(mid, topic, qos):
|
312 | pack_format = "!BBHH"+str(len(topic))+"sB"
|
313 | return struct.pack(pack_format, 130, 2+2+len(topic)+1, mid, len(topic), topic, qos)
|
314 |
|
315 | def gen_suback(mid, qos):
|
316 | return struct.pack('!BBHB', 144, 2+1, mid, qos)
|
317 |
|
318 | def gen_unsubscribe(mid, topic):
|
319 | pack_format = "!BBHH"+str(len(topic))+"s"
|
320 | return struct.pack(pack_format, 162, 2+2+len(topic), mid, len(topic), topic)
|
321 |
|
322 | def gen_unsuback(mid):
|
323 | return struct.pack('!BBH', 176, 2, mid)
|
324 |
|
325 | def gen_pingreq():
|
326 | return struct.pack('!BB', 192, 0)
|
327 |
|
328 | def gen_pingresp():
|
329 | return struct.pack('!BB', 208, 0)
|
330 |
|
331 | def gen_disconnect():
|
332 | return struct.pack('!BB', 224, 0)
|
333 |
|
334 | def pack_remaining_length(remaining_length):
|
335 | s = ""
|
336 | while True:
|
337 | byte = remaining_length % 128
|
338 | remaining_length = remaining_length // 128
|
339 |
|
340 | if remaining_length > 0:
|
341 | byte = byte | 0x80
|
342 |
|
343 | s = s + struct.pack("!B", byte)
|
344 | if remaining_length == 0:
|
345 | return s
|