mqtt_msg.c 14 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502
  1. /*
  2. * Copyright (c) 2014, Stephen Robinson
  3. * All rights reserved.
  4. *
  5. * Redistribution and use in source and binary forms, with or without
  6. * modification, are permitted provided that the following conditions
  7. * are met:
  8. *
  9. * 1. Redistributions of source code must retain the above copyright
  10. * notice, this list of conditions and the following disclaimer.
  11. * 2. Redistributions in binary form must reproduce the above copyright
  12. * notice, this list of conditions and the following disclaimer in the
  13. * documentation and/or other materials provided with the distribution.
  14. * 3. Neither the name of the copyright holder nor the names of its
  15. * contributors may be used to endorse or promote products derived
  16. * from this software without specific prior written permission.
  17. *
  18. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  19. * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  20. * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  21. * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
  22. * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  23. * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  24. * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  25. * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  26. * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  27. * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  28. * POSSIBILITY OF SUCH DAMAGE.
  29. *
  30. */
  31. #include "c_string.h"
  32. #include "mqtt_msg.h"
  33. #define MQTT_MAX_FIXED_HEADER_SIZE 3
  34. enum mqtt_connect_flag
  35. {
  36. MQTT_CONNECT_FLAG_USERNAME = 1 << 7,
  37. MQTT_CONNECT_FLAG_PASSWORD = 1 << 6,
  38. MQTT_CONNECT_FLAG_WILL_RETAIN = 1 << 5,
  39. MQTT_CONNECT_FLAG_WILL = 1 << 2,
  40. MQTT_CONNECT_FLAG_CLEAN_SESSION = 1 << 1
  41. };
  42. struct __attribute((__packed__)) mqtt_connect_variable_header
  43. {
  44. uint8_t lengthMsb;
  45. uint8_t lengthLsb;
  46. uint8_t magic[4];
  47. uint8_t version;
  48. uint8_t flags;
  49. uint8_t keepaliveMsb;
  50. uint8_t keepaliveLsb;
  51. };
  52. static int append_string(mqtt_connection_t* connection, const char* string, int len)
  53. {
  54. if(connection->message.length + len + 2 > connection->buffer_length)
  55. return -1;
  56. connection->buffer[connection->message.length++] = len >> 8;
  57. connection->buffer[connection->message.length++] = len & 0xff;
  58. c_memcpy(connection->buffer + connection->message.length, string, len);
  59. connection->message.length += len;
  60. return len + 2;
  61. }
  62. static uint16_t append_message_id(mqtt_connection_t* connection, uint16_t message_id)
  63. {
  64. // If message_id is zero then we should assign one, otherwise
  65. // we'll use the one supplied by the caller
  66. while(message_id == 0)
  67. message_id = ++connection->message_id;
  68. if(connection->message.length + 2 > connection->buffer_length)
  69. return 0;
  70. connection->buffer[connection->message.length++] = message_id >> 8;
  71. connection->buffer[connection->message.length++] = message_id & 0xff;
  72. return message_id;
  73. }
  74. static int init_message(mqtt_connection_t* connection)
  75. {
  76. connection->message.length = MQTT_MAX_FIXED_HEADER_SIZE;
  77. return MQTT_MAX_FIXED_HEADER_SIZE;
  78. }
  79. static mqtt_message_t* fail_message(mqtt_connection_t* connection)
  80. {
  81. connection->message.data = connection->buffer;
  82. connection->message.length = 0;
  83. return &connection->message;
  84. }
  85. static mqtt_message_t* fini_message(mqtt_connection_t* connection, int type, int dup, int qos, int retain)
  86. {
  87. int remaining_length = connection->message.length - MQTT_MAX_FIXED_HEADER_SIZE;
  88. if(remaining_length > 127)
  89. {
  90. connection->buffer[0] = ((type & 0x0f) << 4) | ((dup & 1) << 3) | ((qos & 3) << 1) | (retain & 1);
  91. connection->buffer[1] = 0x80 | (remaining_length % 128);
  92. connection->buffer[2] = remaining_length / 128;
  93. connection->message.length = remaining_length + 3;
  94. connection->message.data = connection->buffer;
  95. }
  96. else
  97. {
  98. connection->buffer[1] = ((type & 0x0f) << 4) | ((dup & 1) << 3) | ((qos & 3) << 1) | (retain & 1);
  99. connection->buffer[2] = remaining_length;
  100. connection->message.length = remaining_length + 2;
  101. connection->message.data = connection->buffer + 1;
  102. }
  103. return &connection->message;
  104. }
  105. void mqtt_msg_init(mqtt_connection_t* connection, uint8_t* buffer, uint16_t buffer_length)
  106. {
  107. c_memset(connection, 0, sizeof(connection));
  108. connection->buffer = buffer;
  109. connection->buffer_length = buffer_length;
  110. }
  111. int mqtt_get_total_length(uint8_t* buffer, uint16_t length)
  112. {
  113. int i;
  114. int totlen = 0;
  115. for(i = 1; i < length; ++i)
  116. {
  117. totlen += (buffer[i] & 0x7f) << (7 * (i - 1));
  118. if((buffer[i] & 0x80) == 0)
  119. {
  120. ++i;
  121. break;
  122. }
  123. }
  124. totlen += i;
  125. return totlen;
  126. }
  127. const char* mqtt_get_publish_topic(uint8_t* buffer, uint16_t* length)
  128. {
  129. int i;
  130. int totlen = 0;
  131. int topiclen;
  132. for(i = 1; i < *length; ++i)
  133. {
  134. totlen += (buffer[i] & 0x7f) << (7 * (i -1));
  135. if((buffer[i] & 0x80) == 0)
  136. {
  137. ++i;
  138. break;
  139. }
  140. }
  141. totlen += i;
  142. if(i + 2 >= *length)
  143. return NULL;
  144. topiclen = buffer[i++] << 8;
  145. topiclen |= buffer[i++];
  146. if(i + topiclen > *length)
  147. return NULL;
  148. *length = topiclen;
  149. return (const char*)(buffer + i);
  150. }
  151. const char* mqtt_get_publish_data(uint8_t* buffer, uint16_t* length)
  152. {
  153. int i;
  154. int totlen = 0;
  155. int topiclen;
  156. for(i = 1; i < *length; ++i)
  157. {
  158. totlen += (buffer[i] & 0x7f) << (7 * (i - 1));
  159. if((buffer[i] & 0x80) == 0)
  160. {
  161. ++i;
  162. break;
  163. }
  164. }
  165. totlen += i;
  166. if(i + 2 >= *length)
  167. return NULL;
  168. topiclen = buffer[i++] << 8;
  169. topiclen |= buffer[i++];
  170. if(i + topiclen >= *length){
  171. *length = 0;
  172. return NULL;
  173. }
  174. i += topiclen;
  175. if(mqtt_get_qos(buffer) > 0)
  176. {
  177. if(i + 2 >= *length)
  178. return NULL;
  179. i += 2;
  180. }
  181. if(totlen < i)
  182. return NULL;
  183. if(totlen <= *length)
  184. *length = totlen - i;
  185. else
  186. *length = *length - i;
  187. return (const char*)(buffer + i);
  188. }
  189. uint16_t mqtt_get_id(uint8_t* buffer, uint16_t length)
  190. {
  191. if(length < 1)
  192. return 0;
  193. switch(mqtt_get_type(buffer))
  194. {
  195. case MQTT_MSG_TYPE_PUBLISH:
  196. {
  197. int i;
  198. int topiclen;
  199. for(i = 1; i < length; ++i)
  200. {
  201. if((buffer[i] & 0x80) == 0)
  202. {
  203. ++i;
  204. break;
  205. }
  206. }
  207. if(i + 2 >= length)
  208. return 0;
  209. topiclen = buffer[i++] << 8;
  210. topiclen |= buffer[i++];
  211. if(i + topiclen >= length)
  212. return 0;
  213. i += topiclen;
  214. if(mqtt_get_qos(buffer) > 0)
  215. {
  216. if(i + 2 >= length)
  217. return 0;
  218. //i += 2;
  219. } else {
  220. return 0;
  221. }
  222. return (buffer[i] << 8) | buffer[i + 1];
  223. }
  224. case MQTT_MSG_TYPE_PUBACK:
  225. case MQTT_MSG_TYPE_PUBREC:
  226. case MQTT_MSG_TYPE_PUBREL:
  227. case MQTT_MSG_TYPE_PUBCOMP:
  228. case MQTT_MSG_TYPE_SUBACK:
  229. case MQTT_MSG_TYPE_UNSUBACK:
  230. case MQTT_MSG_TYPE_SUBSCRIBE:
  231. {
  232. // This requires the remaining length to be encoded in 1 byte,
  233. // which it should be.
  234. if(length >= 4 && (buffer[1] & 0x80) == 0)
  235. return (buffer[2] << 8) | buffer[3];
  236. else
  237. return 0;
  238. }
  239. default:
  240. return 0;
  241. }
  242. }
  243. mqtt_message_t* mqtt_msg_connect(mqtt_connection_t* connection, mqtt_connect_info_t* info)
  244. {
  245. struct mqtt_connect_variable_header* variable_header;
  246. init_message(connection);
  247. if(connection->message.length + sizeof(*variable_header) > connection->buffer_length)
  248. return fail_message(connection);
  249. variable_header = (void*)(connection->buffer + connection->message.length);
  250. connection->message.length += sizeof(*variable_header);
  251. variable_header->lengthMsb = 0;
  252. variable_header->lengthLsb = 4;
  253. c_memcpy(variable_header->magic, "MQTT", 4);
  254. variable_header->version = 4;
  255. variable_header->flags = 0;
  256. variable_header->keepaliveMsb = info->keepalive >> 8;
  257. variable_header->keepaliveLsb = info->keepalive & 0xff;
  258. if(info->clean_session)
  259. variable_header->flags |= MQTT_CONNECT_FLAG_CLEAN_SESSION;
  260. if(info->client_id != NULL && info->client_id[0] != '\0')
  261. {
  262. if(append_string(connection, info->client_id, c_strlen(info->client_id)) < 0)
  263. return fail_message(connection);
  264. }
  265. else
  266. return fail_message(connection);
  267. if(info->will_topic != NULL && info->will_topic[0] != '\0')
  268. {
  269. if(append_string(connection, info->will_topic, c_strlen(info->will_topic)) < 0)
  270. return fail_message(connection);
  271. if(append_string(connection, info->will_message, c_strlen(info->will_message)) < 0)
  272. return fail_message(connection);
  273. variable_header->flags |= MQTT_CONNECT_FLAG_WILL;
  274. if(info->will_retain)
  275. variable_header->flags |= MQTT_CONNECT_FLAG_WILL_RETAIN;
  276. variable_header->flags |= (info->will_qos & 3) << 3;
  277. }
  278. if(info->username != NULL && info->username[0] != '\0')
  279. {
  280. if(append_string(connection, info->username, c_strlen(info->username)) < 0)
  281. return fail_message(connection);
  282. variable_header->flags |= MQTT_CONNECT_FLAG_USERNAME;
  283. }
  284. if(info->password != NULL && info->password[0] != '\0')
  285. {
  286. if(append_string(connection, info->password, c_strlen(info->password)) < 0)
  287. return fail_message(connection);
  288. variable_header->flags |= MQTT_CONNECT_FLAG_PASSWORD;
  289. }
  290. return fini_message(connection, MQTT_MSG_TYPE_CONNECT, 0, 0, 0);
  291. }
  292. mqtt_message_t* mqtt_msg_publish(mqtt_connection_t* connection, const char* topic, const char* data, int data_length, int qos, int retain, uint16_t* message_id)
  293. {
  294. init_message(connection);
  295. if(topic == NULL || topic[0] == '\0')
  296. return fail_message(connection);
  297. if(append_string(connection, topic, c_strlen(topic)) < 0)
  298. return fail_message(connection);
  299. if(qos > 0)
  300. {
  301. if((*message_id = append_message_id(connection, 0)) == 0)
  302. return fail_message(connection);
  303. }
  304. else
  305. *message_id = 0;
  306. if(connection->message.length + data_length > connection->buffer_length)
  307. return fail_message(connection);
  308. c_memcpy(connection->buffer + connection->message.length, data, data_length);
  309. connection->message.length += data_length;
  310. return fini_message(connection, MQTT_MSG_TYPE_PUBLISH, 0, qos, retain);
  311. }
  312. mqtt_message_t* mqtt_msg_puback(mqtt_connection_t* connection, uint16_t message_id)
  313. {
  314. init_message(connection);
  315. if(append_message_id(connection, message_id) == 0)
  316. return fail_message(connection);
  317. return fini_message(connection, MQTT_MSG_TYPE_PUBACK, 0, 0, 0);
  318. }
  319. mqtt_message_t* mqtt_msg_pubrec(mqtt_connection_t* connection, uint16_t message_id)
  320. {
  321. init_message(connection);
  322. if(append_message_id(connection, message_id) == 0)
  323. return fail_message(connection);
  324. return fini_message(connection, MQTT_MSG_TYPE_PUBREC, 0, 0, 0);
  325. }
  326. mqtt_message_t* mqtt_msg_pubrel(mqtt_connection_t* connection, uint16_t message_id)
  327. {
  328. init_message(connection);
  329. if(append_message_id(connection, message_id) == 0)
  330. return fail_message(connection);
  331. return fini_message(connection, MQTT_MSG_TYPE_PUBREL, 0, 1, 0);
  332. }
  333. mqtt_message_t* mqtt_msg_pubcomp(mqtt_connection_t* connection, uint16_t message_id)
  334. {
  335. init_message(connection);
  336. if(append_message_id(connection, message_id) == 0)
  337. return fail_message(connection);
  338. return fini_message(connection, MQTT_MSG_TYPE_PUBCOMP, 0, 0, 0);
  339. }
  340. mqtt_message_t* mqtt_msg_subscribe_init(mqtt_connection_t* connection, uint16_t *message_id)
  341. {
  342. init_message(connection);
  343. if((*message_id = append_message_id(connection, 0)) == 0)
  344. return fail_message(connection);
  345. return &connection->message;
  346. }
  347. mqtt_message_t* mqtt_msg_subscribe_topic(mqtt_connection_t* connection, const char* topic, int qos)
  348. {
  349. if(topic == NULL || topic[0] == '\0')
  350. return fail_message(connection);
  351. if(append_string(connection, topic, c_strlen(topic)) < 0)
  352. return fail_message(connection);
  353. if(connection->message.length + 1 > connection->buffer_length)
  354. return fail_message(connection);
  355. connection->buffer[connection->message.length++] = qos;
  356. return &connection->message;
  357. }
  358. mqtt_message_t* mqtt_msg_subscribe_fini(mqtt_connection_t* connection)
  359. {
  360. return fini_message(connection, MQTT_MSG_TYPE_SUBSCRIBE, 0, 1, 0);
  361. }
  362. mqtt_message_t* mqtt_msg_subscribe(mqtt_connection_t* connection, const char* topic, int qos, uint16_t* message_id)
  363. {
  364. mqtt_message_t* result;
  365. result = mqtt_msg_subscribe_init(connection, message_id);
  366. if (result->length != 0) {
  367. result = mqtt_msg_subscribe_topic(connection, topic, qos);
  368. }
  369. if (result->length != 0) {
  370. result = mqtt_msg_subscribe_fini(connection);
  371. }
  372. return result;
  373. }
  374. mqtt_message_t* mqtt_msg_unsubscribe_init(mqtt_connection_t* connection, uint16_t *message_id)
  375. {
  376. return mqtt_msg_subscribe_init(connection, message_id);
  377. }
  378. mqtt_message_t* mqtt_msg_unsubscribe_topic(mqtt_connection_t* connection, const char* topic)
  379. {
  380. if(topic == NULL || topic[0] == '\0')
  381. return fail_message(connection);
  382. if(append_string(connection, topic, c_strlen(topic)) < 0)
  383. return fail_message(connection);
  384. return &connection->message;
  385. }
  386. mqtt_message_t* mqtt_msg_unsubscribe_fini(mqtt_connection_t* connection)
  387. {
  388. return fini_message(connection, MQTT_MSG_TYPE_UNSUBSCRIBE, 0, 1, 0);
  389. }
  390. mqtt_message_t* mqtt_msg_unsubscribe(mqtt_connection_t* connection, const char* topic, uint16_t* message_id)
  391. {
  392. mqtt_message_t* result;
  393. result = mqtt_msg_unsubscribe_init(connection, message_id);
  394. if (result->length != 0) {
  395. result = mqtt_msg_unsubscribe_topic(connection, topic);
  396. }
  397. if (result->length != 0) {
  398. result = mqtt_msg_unsubscribe_fini(connection);
  399. }
  400. return result;
  401. }
  402. mqtt_message_t* mqtt_msg_pingreq(mqtt_connection_t* connection)
  403. {
  404. init_message(connection);
  405. return fini_message(connection, MQTT_MSG_TYPE_PINGREQ, 0, 0, 0);
  406. }
  407. mqtt_message_t* mqtt_msg_pingresp(mqtt_connection_t* connection)
  408. {
  409. init_message(connection);
  410. return fini_message(connection, MQTT_MSG_TYPE_PINGRESP, 0, 0, 0);
  411. }
  412. mqtt_message_t* mqtt_msg_disconnect(mqtt_connection_t* connection)
  413. {
  414. init_message(connection);
  415. return fini_message(connection, MQTT_MSG_TYPE_DISCONNECT, 0, 0, 0);
  416. }