MQTTPacket.c 10 KB

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