Demo application for using the AT&T IoT Starter Kit Powered by AWS.
Dependencies: SDFileSystem
Fork of ATT_AWS_IoT_demo by
MQTTPacket.cpp
00001 /******************************************************************************* 00002 * Copyright (c) 2014 IBM Corp. 00003 * 00004 * All rights reserved. This program and the accompanying materials 00005 * are made available under the terms of the Eclipse Public License v1.0 00006 * and Eclipse Distribution License v1.0 which accompany this distribution. 00007 * 00008 * The Eclipse Public License is available at 00009 * http://www.eclipse.org/legal/epl-v10.html 00010 * and the Eclipse Distribution License is available at 00011 * http://www.eclipse.org/org/documents/edl-v10.php. 00012 * 00013 * Contributors: 00014 * Ian Craggs - initial API and implementation and/or initial documentation 00015 * Sergio R. Caprile - non-blocking packet read functions for stream transport 00016 *******************************************************************************/ 00017 00018 #include "StackTrace.h" 00019 #include "MQTTPacket.h" 00020 00021 #include <string.h> 00022 00023 /** 00024 * Encodes the message length according to the MQTT algorithm 00025 * @param buf the buffer into which the encoded data is written 00026 * @param length the length to be encoded 00027 * @return the number of bytes written to buffer 00028 */ 00029 uint32_t MQTTPacket_encode(unsigned char *buf, size_t length) { 00030 uint32_t outLen = 0; 00031 00032 FUNC_ENTRY; 00033 do { 00034 int16_t d = length % 128; 00035 length /= 128; 00036 /* if there are more digits to encode, set the top bit of this digit */ 00037 if(length > 0) { 00038 d |= 0x80; 00039 } 00040 buf[outLen++] = (unsigned char)d; 00041 }while(length > 0); 00042 00043 FUNC_EXIT_RC(outLen); 00044 return outLen; 00045 } 00046 00047 /** 00048 * Decodes the message length according to the MQTT algorithm 00049 * @param getcharfn pointer to function to read the next character from the data source 00050 * @param value the decoded length returned 00051 * @return the number of bytes read from the socket 00052 */ 00053 MQTTReturnCode MQTTPacket_decode(uint32_t (*getcharfn)(unsigned char *, uint32_t), uint32_t *value, uint32_t *readBytesLen) { 00054 unsigned char c; 00055 uint32_t multiplier = 1; 00056 uint32_t len = 0; 00057 uint32_t getLen = 0; 00058 #define MAX_NO_OF_REMAINING_LENGTH_BYTES 4 00059 00060 FUNC_ENTRY; 00061 *value = 0; 00062 do { 00063 if(++len > MAX_NO_OF_REMAINING_LENGTH_BYTES) { 00064 /* bad data */ 00065 FUNC_EXIT_RC(MQTTPACKET_READ_ERROR); 00066 return MQTTPACKET_READ_ERROR; 00067 } 00068 getLen = (*getcharfn)(&c, 1); 00069 if(1 != getLen) { 00070 FUNC_EXIT_RC(FAILURE); 00071 return FAILURE; 00072 } 00073 *value += (c & 127) * multiplier; 00074 multiplier *= 128; 00075 }while((c & 128) != 0); 00076 00077 *readBytesLen = len; 00078 00079 FUNC_EXIT_RC(SUCCESS); 00080 return SUCCESS; 00081 } 00082 00083 size_t MQTTPacket_len(size_t rem_len) { 00084 rem_len += 1; /* header byte */ 00085 00086 /* now remaining_length field */ 00087 if(rem_len < 128) { 00088 rem_len += 1; 00089 } else if (rem_len < 16384) { 00090 rem_len += 2; 00091 } else if (rem_len < 2097151) { 00092 rem_len += 3; 00093 } else { 00094 rem_len += 4; 00095 } 00096 00097 return rem_len; 00098 } 00099 00100 static unsigned char *bufptr; 00101 00102 uint32_t bufchar(unsigned char *c, uint32_t count) { 00103 uint32_t i; 00104 00105 for(i = 0; i < count; ++i) { 00106 *c = *bufptr++; 00107 } 00108 00109 return count; 00110 } 00111 00112 MQTTReturnCode MQTTPacket_decodeBuf(unsigned char *buf, uint32_t *value, uint32_t *readBytesLen) { 00113 bufptr = buf; 00114 return MQTTPacket_decode(bufchar, value, readBytesLen); 00115 } 00116 00117 /** 00118 * Calculates an integer from two bytes read from the input buffer 00119 * @param pptr pointer to the input buffer - incremented by the number of bytes used & returned 00120 * @return the integer value calculated 00121 */ 00122 int32_t readInt(unsigned char **pptr) { 00123 unsigned char *ptr = *pptr; 00124 int32_t len = 256*(*ptr) + (*(ptr+1)); 00125 *pptr += 2; 00126 return len; 00127 } 00128 00129 /** 00130 * Calculates an integer from two bytes read from the input buffer 00131 * @param pptr pointer to the input buffer - incremented by the number of bytes used & returned 00132 * @return the integer value calculated 00133 */ 00134 size_t readSizeT(unsigned char **pptr) { 00135 unsigned char *ptr = *pptr; 00136 size_t firstByte = (size_t)(*ptr); 00137 size_t secondByte = (size_t)(*(ptr+1)); 00138 size_t size = 256 * firstByte + secondByte; 00139 *pptr += 2; 00140 return size; 00141 } 00142 00143 /** 00144 * Calculates uint16 packet id from two bytes read from the input buffer 00145 * @param pptr pointer to the input buffer - incremented by the number of bytes used & returned 00146 * @return the value calculated 00147 */ 00148 uint16_t readPacketId(unsigned char **pptr) { 00149 unsigned char *ptr = *pptr; 00150 uint8_t firstByte = (uint8_t)(*ptr); 00151 uint8_t secondByte = (uint8_t)(*(ptr + 1)); 00152 uint16_t len = (uint16_t)(secondByte + (256 * firstByte)); 00153 *pptr += 2; 00154 return len; 00155 } 00156 00157 /** 00158 * Reads one character from the input buffer. 00159 * @param pptr pointer to the input buffer - incremented by the number of bytes used & returned 00160 * @return the character read 00161 */ 00162 unsigned char readChar(unsigned char **pptr) { 00163 unsigned char c = **pptr; 00164 (*pptr)++; 00165 return c; 00166 } 00167 00168 /** 00169 * Writes one character to an output buffer. 00170 * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned 00171 * @param c the character to write 00172 */ 00173 void writeChar(unsigned char **pptr, unsigned char c) { 00174 **pptr = c; 00175 (*pptr)++; 00176 } 00177 00178 /** 00179 * Writes an integer as 2 bytes to an output buffer. 00180 * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned 00181 * @param anInt the integer to write 00182 */ 00183 void writePacketId(unsigned char** pptr, uint16_t anInt) { 00184 **pptr = (unsigned char)(anInt / 256); 00185 (*pptr)++; 00186 **pptr = (unsigned char)(anInt % 256); 00187 (*pptr)++; 00188 } 00189 00190 /** 00191 * Writes an integer as 2 bytes to an output buffer. 00192 * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned 00193 * @param anInt the integer to write 00194 */ 00195 void writeInt(unsigned char **pptr, int32_t anInt) { 00196 **pptr = (unsigned char)(anInt / 256); 00197 (*pptr)++; 00198 **pptr = (unsigned char)(anInt % 256); 00199 (*pptr)++; 00200 } 00201 00202 /** 00203 * Writes size as 2 bytes to an output buffer. 00204 * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned 00205 * @param anInt the integer to write 00206 */ 00207 void writeSizeT(unsigned char **pptr, size_t size) { 00208 **pptr = (unsigned char)(size / 256); 00209 (*pptr)++; 00210 **pptr = (unsigned char)(size % 256); 00211 (*pptr)++; 00212 } 00213 00214 /** 00215 * Writes a "UTF" string to an output buffer. Converts C string to length-delimited. 00216 * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned 00217 * @param string the C string to write 00218 */ 00219 void writeCString(unsigned char **pptr, const char *string) { 00220 size_t len = strlen(string); 00221 writeSizeT(pptr, len); 00222 memcpy(*pptr, string, len); 00223 *pptr += len; 00224 } 00225 00226 void writeMQTTString(unsigned char **pptr, MQTTString mqttstring) { 00227 if(mqttstring.lenstring.len > 0) { 00228 writeSizeT(pptr, mqttstring.lenstring.len); 00229 memcpy(*pptr, mqttstring.lenstring.data, mqttstring.lenstring.len); 00230 *pptr += mqttstring.lenstring.len; 00231 } else if (mqttstring.cstring) { 00232 writeCString(pptr, mqttstring.cstring); 00233 } else { 00234 writeInt(pptr, 0); 00235 } 00236 } 00237 00238 /** 00239 * @param mqttstring the MQTTString structure into which the data is to be read 00240 * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned 00241 * @param enddata pointer to the end of the data: do not read beyond 00242 * @return SUCCESS if successful, FAILURE if not 00243 */ 00244 MQTTReturnCode readMQTTLenString(MQTTString *mqttstring, unsigned char **pptr, unsigned char *enddata) { 00245 MQTTReturnCode rc = FAILURE; 00246 00247 FUNC_ENTRY; 00248 /* the first two bytes are the length of the string */ 00249 /* enough length to read the integer? */ 00250 if(enddata - (*pptr) > 1) { 00251 mqttstring->lenstring.len = readSizeT(pptr); /* increments pptr to point past length */ 00252 if(&(*pptr)[mqttstring->lenstring.len] <= enddata) { 00253 mqttstring->lenstring.data = (char*)*pptr; 00254 *pptr += mqttstring->lenstring.len; 00255 rc = SUCCESS; 00256 } 00257 } 00258 mqttstring->cstring = NULL; 00259 00260 FUNC_EXIT_RC(rc); 00261 return rc; 00262 } 00263 00264 /** 00265 * Return the length of the MQTTstring - C string if there is one, otherwise the length delimited string 00266 * @param mqttstring the string to return the length of 00267 * @return the length of the string 00268 */ 00269 size_t MQTTstrlen(MQTTString mqttstring) { 00270 size_t len = 0; 00271 00272 if(mqttstring.cstring) { 00273 len = strlen(mqttstring.cstring); 00274 } else { 00275 len = mqttstring.lenstring.len; 00276 } 00277 00278 return len; 00279 } 00280 00281 /** 00282 * Compares an MQTTString to a C string 00283 * @param a the MQTTString to compare 00284 * @param bptr the C string to compare 00285 * @return boolean - equal or not 00286 */ 00287 uint8_t MQTTPacket_equals(MQTTString *a, char *bptr) { 00288 size_t alen = 0; 00289 size_t blen = 0; 00290 char *aptr; 00291 00292 if(a->cstring) { 00293 aptr = a->cstring; 00294 alen = strlen(a->cstring); 00295 } else { 00296 aptr = a->lenstring.data; 00297 alen = a->lenstring.len; 00298 } 00299 blen = strlen(bptr); 00300 00301 return (alen == blen) && (strncmp(aptr, bptr, alen) == 0); 00302 } 00303 00304 /** 00305 * Initialize the MQTTHeader structure. Used to ensure that Header bits are 00306 * always initialized using the proper mappings. No Endianness issues here since 00307 * the individual fields are all less than a byte. Also generates no warnings since 00308 * all fields are initialized using hex constants 00309 */ 00310 MQTTReturnCode MQTTPacket_InitHeader(MQTTHeader *header, MessageTypes message_type, 00311 QoS qos, uint8_t dup, uint8_t retained) { 00312 if(NULL == header) { 00313 return MQTT_NULL_VALUE_ERROR; 00314 } 00315 00316 /* Set all bits to zero */ 00317 header->byte = 0; 00318 switch(message_type) { 00319 case UNKNOWN: 00320 /* Should never happen */ 00321 return MQTT_UNKNOWN_ERROR; 00322 case CONNECT: 00323 header->bits.type = 0x01; 00324 break; 00325 case CONNACK: 00326 header->bits.type = 0x02; 00327 break; 00328 case PUBLISH: 00329 header->bits.type = 0x03; 00330 break; 00331 case PUBACK: 00332 header->bits.type = 0x04; 00333 break; 00334 case PUBREC: 00335 header->bits.type = 0x05; 00336 break; 00337 case PUBREL: 00338 header->bits.type = 0x06; 00339 break; 00340 case PUBCOMP: 00341 header->bits.type = 0x07; 00342 break; 00343 case SUBSCRIBE: 00344 header->bits.type = 0x08; 00345 break; 00346 case SUBACK: 00347 header->bits.type = 0x09; 00348 break; 00349 case UNSUBSCRIBE: 00350 header->bits.type = 0x0A; 00351 break; 00352 case UNSUBACK: 00353 header->bits.type = 0x0B; 00354 break; 00355 case PINGREQ: 00356 header->bits.type = 0x0C; 00357 break; 00358 case PINGRESP: 00359 header->bits.type = 0x0D; 00360 break; 00361 case DISCONNECT: 00362 header->bits.type = 0x0E; 00363 break; 00364 default: 00365 /* Should never happen */ 00366 return MQTT_UNKNOWN_ERROR; 00367 } 00368 00369 header->bits.dup = (1 == dup) ? 0x01 : 0x00; 00370 switch(qos) { 00371 case QOS0: 00372 header->bits.qos = 0x00; 00373 break; 00374 case QOS1: 00375 header->bits.qos = 0x01; 00376 break; 00377 case QOS2: 00378 header->bits.qos = 0x02; 00379 break; 00380 default: 00381 /* Using QOS0 as default */ 00382 header->bits.qos = 0x00; 00383 break; 00384 } 00385 00386 header->bits.retain = (1 == retained) ? 0x01 : 0x00; 00387 00388 return SUCCESS; 00389 } 00390 00391
Generated on Tue Jul 12 2022 22:13:21 by 1.7.2