UNPKG

12.4 kBPlain TextView Raw
1import socket
2import struct
3
4def 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
13def 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
29def 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
41def 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
58def 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 # Reserved
67 return "0x00"
68 elif cmd == 0x10:
69 # CONNECT
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 # CONNACK
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 # PUBLISH
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 # PUBACK
142 (cmd, rl, mid) = struct.unpack('!BBH', packet)
143 return "PUBACK, rl="+str(rl)+", mid="+str(mid)
144 elif cmd == 0x50:
145 # PUBREC
146 (cmd, rl, mid) = struct.unpack('!BBH', packet)
147 return "PUBREC, rl="+str(rl)+", mid="+str(mid)
148 elif cmd == 0x60:
149 # PUBREL
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 # PUBCOMP
155 (cmd, rl, mid) = struct.unpack('!BBH', packet)
156 return "PUBCOMP, rl="+str(rl)+", mid="+str(mid)
157 elif cmd == 0x80:
158 # SUBSCRIBE
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 # SUBACK
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 # UNSUBSCRIBE
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 # UNSUBACK
199 (cmd, rl, mid) = struct.unpack('!BBH', packet)
200 return "UNSUBACK, rl="+str(rl)+", mid="+str(mid)
201 elif cmd == 0xC0:
202 # PINGREQ
203 (cmd, rl) = struct.unpack('!BB', packet)
204 return "PINGREQ, rl="+str(rl)
205 elif cmd == 0xD0:
206 # PINGRESP
207 (cmd, rl) = struct.unpack('!BB', packet)
208 return "PINGRESP, rl="+str(rl)
209 elif cmd == 0xE0:
210 # DISCONNECT
211 (cmd, rl) = struct.unpack('!BB', packet)
212 return "DISCONNECT, rl="+str(rl)
213 elif cmd == 0xF0:
214 # Reserved
215 return "0xF0"
216
217def 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
268def gen_connack(resv=0, rc=0):
269 return struct.pack('!BBBB', 32, 2, resv, rc);
270
271def 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
295def gen_puback(mid):
296 return struct.pack('!BBH', 64, 2, mid)
297
298def gen_pubrec(mid):
299 return struct.pack('!BBH', 80, 2, mid)
300
301def 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
308def gen_pubcomp(mid):
309 return struct.pack('!BBH', 112, 2, mid)
310
311def 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
315def gen_suback(mid, qos):
316 return struct.pack('!BBHB', 144, 2+1, mid, qos)
317
318def 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
322def gen_unsuback(mid):
323 return struct.pack('!BBH', 176, 2, mid)
324
325def gen_pingreq():
326 return struct.pack('!BB', 192, 0)
327
328def gen_pingresp():
329 return struct.pack('!BB', 208, 0)
330
331def gen_disconnect():
332 return struct.pack('!BB', 224, 0)
333
334def pack_remaining_length(remaining_length):
335 s = ""
336 while True:
337 byte = remaining_length % 128
338 remaining_length = remaining_length // 128
339 # If there are more digits to encode, set the top bit of this digit
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