1 /*******************************************************************************
2 * Copyright (c) 2014 IBM Corp.
3 *
4 * All rights reserved. This program and the accompanying materials
5 * are made available under the terms of the Eclipse Public License v1.0
6 * and Eclipse Distribution License v1.0 which accompany this distribution.
7 *
8 * The Eclipse Public License is available at
9 * http://www.eclipse.org/legal/epl-v10.html
10 * and the Eclipse Distribution License is available at
11 * http://www.eclipse.org/org/documents/edl-v10.php.
12 *
13 * Contributors:
14 * Ian Craggs - initial API and implementation and/or initial documentation
15 * Sergio R. Caprile - non-blocking packet read functions for stream transport
16 *******************************************************************************/
17
18 #include "StackTrace.h"
19 #include "MQTTPacket.h"
20
21 #include <string.h>
22
23 /**
24 * Encodes the message length according to the MQTT algorithm
25 * @param buf the buffer into which the encoded data is written
26 * @param length the length to be encoded
27 * @return the number of bytes written to buffer
28 */
MQTTPacket_encode(unsigned char * buf,int length)29 int MQTTPacket_encode(unsigned char* buf, int length)
30 {
31 int rc = 0;
32
33 FUNC_ENTRY;
34 do
35 {
36 char d = length % 128;
37 length /= 128;
38 /* if there are more digits to encode, set the top bit of this digit */
39 if (length > 0)
40 d |= 0x80;
41 buf[rc++] = d;
42 } while (length > 0);
43 FUNC_EXIT_RC(rc);
44 return rc;
45 }
46
47
48 /**
49 * Decodes the message length according to the MQTT algorithm
50 * @param getcharfn pointer to function to read the next character from the data source
51 * @param value the decoded length returned
52 * @return the number of bytes read from the socket
53 */
MQTTPacket_decode(int (* getcharfn)(unsigned char *,int),int * value)54 int MQTTPacket_decode(int (*getcharfn)(unsigned char*, int), int* value)
55 {
56 unsigned char c;
57 int multiplier = 1;
58 int len = 0;
59 #define MAX_NO_OF_REMAINING_LENGTH_BYTES 4
60
61 FUNC_ENTRY;
62 *value = 0;
63 do
64 {
65 int rc = MQTTPACKET_READ_ERROR;
66
67 if (++len > MAX_NO_OF_REMAINING_LENGTH_BYTES)
68 {
69 rc = MQTTPACKET_READ_ERROR; /* bad data */
70 goto exit;
71 }
72 rc = (*getcharfn)(&c, 1);
73 if (rc != 1)
74 goto exit;
75 *value += (c & 127) * multiplier;
76 multiplier *= 128;
77 } while ((c & 128) != 0);
78 exit:
79 FUNC_EXIT_RC(len);
80 return len;
81 }
82
83
MQTTPacket_len(int rem_len)84 int MQTTPacket_len(int rem_len)
85 {
86 rem_len += 1; /* header byte */
87
88 /* now remaining_length field */
89 if (rem_len < 128)
90 rem_len += 1;
91 else if (rem_len < 16384)
92 rem_len += 2;
93 else if (rem_len < 2097151)
94 rem_len += 3;
95 else
96 rem_len += 4;
97 return rem_len;
98 }
99
100
101 static unsigned char* bufptr;
102
bufchar(unsigned char * c,int count)103 int bufchar(unsigned char* c, int count)
104 {
105 int i;
106
107 for (i = 0; i < count; ++i)
108 *c = *bufptr++;
109 return count;
110 }
111
112
MQTTPacket_decodeBuf(unsigned char * buf,int * value)113 int MQTTPacket_decodeBuf(unsigned char* buf, int* value)
114 {
115 bufptr = buf;
116 return MQTTPacket_decode(bufchar, value);
117 }
118
119
120 /**
121 * Calculates an integer from two bytes read from the input buffer
122 * @param pptr pointer to the input buffer - incremented by the number of bytes used & returned
123 * @return the integer value calculated
124 */
readInt(unsigned char ** pptr)125 int readInt(unsigned char** pptr)
126 {
127 unsigned char* ptr = *pptr;
128 int len = 256*(*ptr) + (*(ptr+1));
129 *pptr += 2;
130 return len;
131 }
132
133
134 /**
135 * Reads one character from the input buffer.
136 * @param pptr pointer to the input buffer - incremented by the number of bytes used & returned
137 * @return the character read
138 */
readChar(unsigned char ** pptr)139 char readChar(unsigned char** pptr)
140 {
141 char c = **pptr;
142 (*pptr)++;
143 return c;
144 }
145
146
147 /**
148 * Writes one character to an output buffer.
149 * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
150 * @param c the character to write
151 */
writeChar(unsigned char ** pptr,char c)152 void writeChar(unsigned char** pptr, char c)
153 {
154 **pptr = c;
155 (*pptr)++;
156 }
157
158
159 /**
160 * Writes an integer as 2 bytes to an output buffer.
161 * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
162 * @param anInt the integer to write
163 */
writeInt(unsigned char ** pptr,int anInt)164 void writeInt(unsigned char** pptr, int anInt)
165 {
166 **pptr = (unsigned char)(anInt / 256);
167 (*pptr)++;
168 **pptr = (unsigned char)(anInt % 256);
169 (*pptr)++;
170 }
171
172
173 /**
174 * Writes a "UTF" string to an output buffer. Converts C string to length-delimited.
175 * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
176 * @param string the C string to write
177 */
writeCString(unsigned char ** pptr,const char * string)178 void writeCString(unsigned char** pptr, const char* string)
179 {
180 int len = strlen(string);
181 writeInt(pptr, len);
182 memcpy(*pptr, string, len);
183 *pptr += len;
184 }
185
186
getLenStringLen(char * ptr)187 int getLenStringLen(char* ptr)
188 {
189 int len = 256*((unsigned char)(*ptr)) + (unsigned char)(*(ptr+1));
190 return len;
191 }
192
193
writeMQTTString(unsigned char ** pptr,MQTTString mqttstring)194 void writeMQTTString(unsigned char** pptr, MQTTString mqttstring)
195 {
196 if (mqttstring.lenstring.len > 0)
197 {
198 writeInt(pptr, mqttstring.lenstring.len);
199 memcpy(*pptr, mqttstring.lenstring.data, mqttstring.lenstring.len);
200 *pptr += mqttstring.lenstring.len;
201 }
202 else if (mqttstring.cstring)
203 writeCString(pptr, mqttstring.cstring);
204 else
205 writeInt(pptr, 0);
206 }
207
208
209 /**
210 * @param mqttstring the MQTTString structure into which the data is to be read
211 * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
212 * @param enddata pointer to the end of the data: do not read beyond
213 * @return 1 if successful, 0 if not
214 */
readMQTTLenString(MQTTString * mqttstring,unsigned char ** pptr,unsigned char * enddata)215 int readMQTTLenString(MQTTString* mqttstring, unsigned char** pptr, unsigned char* enddata)
216 {
217 int rc = 0;
218
219 FUNC_ENTRY;
220 /* the first two bytes are the length of the string */
221 if (enddata - (*pptr) > 1) /* enough length to read the integer? */
222 {
223 mqttstring->lenstring.len = readInt(pptr); /* increments pptr to point past length */
224 if (&(*pptr)[mqttstring->lenstring.len] <= enddata)
225 {
226 mqttstring->lenstring.data = (char*)*pptr;
227 *pptr += mqttstring->lenstring.len;
228 rc = 1;
229 }
230 }
231 mqttstring->cstring = NULL;
232 FUNC_EXIT_RC(rc);
233 return rc;
234 }
235
236
237 /**
238 * Return the length of the MQTTstring - C string if there is one, otherwise the length delimited string
239 * @param mqttstring the string to return the length of
240 * @return the length of the string
241 */
MQTTstrlen(MQTTString mqttstring)242 int MQTTstrlen(MQTTString mqttstring)
243 {
244 int rc = 0;
245
246 if (mqttstring.cstring)
247 rc = strlen(mqttstring.cstring);
248 else
249 rc = mqttstring.lenstring.len;
250 return rc;
251 }
252
253
254 /**
255 * Compares an MQTTString to a C string
256 * @param a the MQTTString to compare
257 * @param bptr the C string to compare
258 * @return boolean - equal or not
259 */
MQTTPacket_equals(MQTTString * a,char * bptr)260 int MQTTPacket_equals(MQTTString* a, char* bptr)
261 {
262 int alen = 0,
263 blen = 0;
264 char *aptr;
265
266 if (a->cstring)
267 {
268 aptr = a->cstring;
269 alen = strlen(a->cstring);
270 }
271 else
272 {
273 aptr = a->lenstring.data;
274 alen = a->lenstring.len;
275 }
276 blen = strlen(bptr);
277
278 return (alen == blen) && (strncmp(aptr, bptr, alen) == 0);
279 }
280
281
282 /**
283 * Helper function to read packet data from some source into a buffer
284 * @param buf the buffer into which the packet will be serialized
285 * @param buflen the length in bytes of the supplied buffer
286 * @param getfn pointer to a function which will read any number of bytes from the needed source
287 * @return integer MQTT packet type, or -1 on error
288 * @note the whole message must fit into the caller's buffer
289 */
MQTTPacket_read(unsigned char * buf,int buflen,int (* getfn)(unsigned char *,int))290 int MQTTPacket_read(unsigned char* buf, int buflen, int (*getfn)(unsigned char*, int))
291 {
292 int rc = -1;
293 MQTTHeader header = {0};
294 int len = 0;
295 int rem_len = 0;
296
297 /* 1. read the header byte. This has the packet type in it */
298 if ((*getfn)(buf, 1) != 1)
299 goto exit;
300
301 len = 1;
302 /* 2. read the remaining length. This is variable in itself */
303 MQTTPacket_decode(getfn, &rem_len);
304 len += MQTTPacket_encode(buf + 1, rem_len); /* put the original remaining length back into the buffer */
305
306 /* 3. read the rest of the buffer using a callback to supply the rest of the data */
307 if((rem_len + len) > buflen)
308 goto exit;
309 if (rem_len && ((*getfn)(buf + len, rem_len) != rem_len))
310 goto exit;
311
312 header.byte = buf[0];
313 rc = header.bits.type;
314 exit:
315 return rc;
316 }
317
318 /**
319 * Decodes the message length according to the MQTT algorithm, non-blocking
320 * @param trp pointer to a transport structure holding what is needed to solve getting data from it
321 * @param value the decoded length returned
322 * @return integer the number of bytes read from the socket, 0 for call again, or -1 on error
323 */
MQTTPacket_decodenb(MQTTTransport * trp)324 static int MQTTPacket_decodenb(MQTTTransport *trp)
325 {
326 unsigned char c;
327 int rc = MQTTPACKET_READ_ERROR;
328
329 FUNC_ENTRY;
330 if(trp->len == 0){ /* initialize on first call */
331 trp->multiplier = 1;
332 trp->rem_len = 0;
333 }
334 do {
335 int frc;
336 if (trp->len >= MAX_NO_OF_REMAINING_LENGTH_BYTES)
337 goto exit;
338 if ((frc=(*trp->getfn)(trp->sck, &c, 1)) == -1)
339 goto exit;
340 if (frc == 0){
341 rc = 0;
342 goto exit;
343 }
344 ++(trp->len);
345 trp->rem_len += (c & 127) * trp->multiplier;
346 trp->multiplier *= 128;
347 } while ((c & 128) != 0);
348 rc = trp->len;
349 exit:
350 FUNC_EXIT_RC(rc);
351 return rc;
352 }
353
354 /**
355 * Helper function to read packet data from some source into a buffer, non-blocking
356 * @param buf the buffer into which the packet will be serialized
357 * @param buflen the length in bytes of the supplied buffer
358 * @param trp pointer to a transport structure holding what is needed to solve getting data from it
359 * @return integer MQTT packet type, 0 for call again, or -1 on error
360 * @note the whole message must fit into the caller's buffer
361 */
MQTTPacket_readnb(unsigned char * buf,int buflen,MQTTTransport * trp)362 int MQTTPacket_readnb(unsigned char* buf, int buflen, MQTTTransport *trp)
363 {
364 int rc = -1, frc;
365 MQTTHeader header = {0};
366
367 switch(trp->state){
368 default:
369 trp->state = 0;
370 /*FALLTHROUGH*/
371 case 0:
372 /* read the header byte. This has the packet type in it */
373 if ((frc=(*trp->getfn)(trp->sck, buf, 1)) == -1)
374 goto exit;
375 if (frc == 0)
376 return 0;
377 trp->len = 0;
378 ++trp->state;
379 /*FALLTHROUGH*/
380 /* read the remaining length. This is variable in itself */
381 case 1:
382 if((frc=MQTTPacket_decodenb(trp)) == MQTTPACKET_READ_ERROR)
383 goto exit;
384 if(frc == 0)
385 return 0;
386 trp->len = 1 + MQTTPacket_encode(buf + 1, trp->rem_len); /* put the original remaining length back into the buffer */
387 if((trp->rem_len + trp->len) > buflen)
388 goto exit;
389 ++trp->state;
390 /*FALLTHROUGH*/
391 case 2:
392 if(trp->rem_len){
393 /* read the rest of the buffer using a callback to supply the rest of the data */
394 if ((frc=(*trp->getfn)(trp->sck, buf + trp->len, trp->rem_len)) == -1)
395 goto exit;
396 if (frc == 0)
397 return 0;
398 trp->rem_len -= frc;
399 trp->len += frc;
400 if(trp->rem_len)
401 return 0;
402 }
403 header.byte = buf[0];
404 rc = header.bits.type;
405 break;
406 }
407
408 exit:
409 trp->state = 0;
410 return rc;
411 }
412
413