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