MQTTPacket.c

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 int MQTTPacket_encode(unsigned char* buf, int length)
00030 {
00031     int rc = 0;
00032 
00033     FUNC_ENTRY;
00034     do
00035     {
00036         char d = length % 128;
00037         length /= 128;
00038         /* if there are more digits to encode, set the top bit of this digit */
00039         if (length > 0)
00040             d |= 0x80;
00041         buf[rc++] = d;
00042     } while (length > 0);
00043     FUNC_EXIT_RC(rc);
00044     return rc;
00045 }
00046 
00047 
00048 /**
00049  * Decodes the message length according to the MQTT algorithm
00050  * @param getcharfn pointer to function to read the next character from the data source
00051  * @param value the decoded length returned
00052  * @return the number of bytes read from the socket
00053  */
00054 int MQTTPacket_decode(int (*getcharfn)(unsigned char*, int), int* value)
00055 {
00056     unsigned char c;
00057     int multiplier = 1;
00058     int len = 0;
00059 #define MAX_NO_OF_REMAINING_LENGTH_BYTES 4
00060 
00061     FUNC_ENTRY;
00062     *value = 0;
00063     do
00064     {
00065         int rc = MQTTPACKET_READ_ERROR;
00066 
00067         if (++len > MAX_NO_OF_REMAINING_LENGTH_BYTES)
00068         {
00069             rc = MQTTPACKET_READ_ERROR; /* bad data */
00070             goto exit;
00071         }
00072         rc = (*getcharfn)(&c, 1);
00073         if (rc != 1)
00074             goto exit;
00075         *value += (c & 127) * multiplier;
00076         multiplier *= 128;
00077     } while ((c & 128) != 0);
00078 exit:
00079     FUNC_EXIT_RC(len);
00080     return len;
00081 }
00082 
00083 
00084 int MQTTPacket_len(int rem_len)
00085 {
00086     rem_len += 1; /* header byte */
00087 
00088     /* now remaining_length field */
00089     if (rem_len < 128)
00090         rem_len += 1;
00091     else if (rem_len < 16384)
00092         rem_len += 2;
00093     else if (rem_len < 2097151)
00094         rem_len += 3;
00095     else
00096         rem_len += 4;
00097     return rem_len;
00098 }
00099 
00100 
00101 static unsigned char* bufptr;
00102 
00103 int bufchar(unsigned char* c, int count)
00104 {
00105     int i;
00106 
00107     for (i = 0; i < count; ++i)
00108         *c = *bufptr++;
00109     return count;
00110 }
00111 
00112 
00113 int MQTTPacket_decodeBuf(unsigned char* buf, int* value)
00114 {
00115     bufptr = buf;
00116     return MQTTPacket_decode(bufchar, value);
00117 }
00118 
00119 
00120 /**
00121  * Calculates an integer from two bytes read from the input buffer
00122  * @param pptr pointer to the input buffer - incremented by the number of bytes used & returned
00123  * @return the integer value calculated
00124  */
00125 int readInt(unsigned char** pptr)
00126 {
00127     unsigned char* ptr = *pptr;
00128     int len = 256*(*ptr) + (*(ptr+1));
00129     *pptr += 2;
00130     return len;
00131 }
00132 
00133 
00134 /**
00135  * Reads one character from the input buffer.
00136  * @param pptr pointer to the input buffer - incremented by the number of bytes used & returned
00137  * @return the character read
00138  */
00139 char readChar(unsigned char** pptr)
00140 {
00141     char c = **pptr;
00142     (*pptr)++;
00143     return c;
00144 }
00145 
00146 
00147 /**
00148  * Writes one character to an output buffer.
00149  * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
00150  * @param c the character to write
00151  */
00152 void writeChar(unsigned char** pptr, char c)
00153 {
00154     **pptr = c;
00155     (*pptr)++;
00156 }
00157 
00158 
00159 /**
00160  * Writes an integer as 2 bytes to an output buffer.
00161  * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
00162  * @param anInt the integer to write
00163  */
00164 void writeInt(unsigned char** pptr, int anInt)
00165 {
00166     **pptr = (unsigned char)(anInt / 256);
00167     (*pptr)++;
00168     **pptr = (unsigned char)(anInt % 256);
00169     (*pptr)++;
00170 }
00171 
00172 
00173 /**
00174  * Writes a "UTF" string to an output buffer.  Converts C string to length-delimited.
00175  * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
00176  * @param string the C string to write
00177  */
00178 void writeCString(unsigned char** pptr, const char* string)
00179 {
00180     int len = strlen(string);
00181     writeInt(pptr, len);
00182     memcpy(*pptr, string, len);
00183     *pptr += len;
00184 }
00185 
00186 
00187 int getLenStringLen(char* ptr)
00188 {
00189     int len = 256*((unsigned char)(*ptr)) + (unsigned char)(*(ptr+1));
00190     return len;
00191 }
00192 
00193 
00194 void writeMQTTString(unsigned char** pptr, MQTTString mqttstring)
00195 {
00196     if (mqttstring.lenstring.len > 0)
00197     {
00198         writeInt(pptr, mqttstring.lenstring.len);
00199         memcpy(*pptr, mqttstring.lenstring.data, mqttstring.lenstring.len);
00200         *pptr += mqttstring.lenstring.len;
00201     }
00202     else if (mqttstring.cstring)
00203         writeCString(pptr, mqttstring.cstring);
00204     else
00205         writeInt(pptr, 0);
00206 }
00207 
00208 
00209 /**
00210  * @param mqttstring the MQTTString structure into which the data is to be read
00211  * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
00212  * @param enddata pointer to the end of the data: do not read beyond
00213  * @return 1 if successful, 0 if not
00214  */
00215 int readMQTTLenString(MQTTString* mqttstring, unsigned char** pptr, unsigned char* enddata)
00216 {
00217     int rc = 0;
00218 
00219     FUNC_ENTRY;
00220     /* the first two bytes are the length of the string */
00221     if (enddata - (*pptr) > 1) /* enough length to read the integer? */
00222     {
00223         mqttstring->lenstring.len = readInt(pptr); /* increments pptr to point past length */
00224         if (&(*pptr)[mqttstring->lenstring.len] <= enddata)
00225         {
00226             mqttstring->lenstring.data = (char*)*pptr;
00227             *pptr += mqttstring->lenstring.len;
00228             rc = 1;
00229         }
00230     }
00231     mqttstring->cstring = NULL;
00232     FUNC_EXIT_RC(rc);
00233     return rc;
00234 }
00235 
00236 
00237 /**
00238  * Return the length of the MQTTstring - C string if there is one, otherwise the length delimited string
00239  * @param mqttstring the string to return the length of
00240  * @return the length of the string
00241  */
00242 int MQTTstrlen(MQTTString mqttstring)
00243 {
00244     int rc = 0;
00245 
00246     if (mqttstring.cstring)
00247         rc = strlen(mqttstring.cstring);
00248     else
00249         rc = mqttstring.lenstring.len;
00250     return rc;
00251 }
00252 
00253 
00254 /**
00255  * Compares an MQTTString to a C string
00256  * @param a the MQTTString to compare
00257  * @param bptr the C string to compare
00258  * @return boolean - equal or not
00259  */
00260 int MQTTPacket_equals(MQTTString* a, char* bptr)
00261 {
00262     int alen = 0,
00263         blen = 0;
00264     char *aptr;
00265     
00266     if (a->cstring)
00267     {
00268         aptr = a->cstring;
00269         alen = strlen(a->cstring);
00270     }
00271     else
00272     {
00273         aptr = a->lenstring.data;
00274         alen = a->lenstring.len;
00275     }
00276     blen = strlen(bptr);
00277     
00278     return (alen == blen) && (strncmp(aptr, bptr, alen) == 0);
00279 }
00280 
00281 
00282 /**
00283  * Helper function to read packet data from some source into a buffer
00284  * @param buf the buffer into which the packet will be serialized
00285  * @param buflen the length in bytes of the supplied buffer
00286  * @param getfn pointer to a function which will read any number of bytes from the needed source
00287  * @return integer MQTT packet type, or -1 on error
00288  * @note  the whole message must fit into the caller's buffer
00289  */
00290 int MQTTPacket_read(unsigned char* buf, int buflen, int (*getfn)(unsigned char*, int))
00291 {
00292     int rc = -1;
00293     MQTTHeader header = {0};
00294     int len = 0;
00295     int rem_len = 0;
00296 
00297     /* 1. read the header byte.  This has the packet type in it */
00298     if ((*getfn)(buf, 1) != 1)
00299         goto exit;
00300 
00301     len = 1;
00302     /* 2. read the remaining length.  This is variable in itself */
00303     MQTTPacket_decode(getfn, &rem_len);
00304     len += MQTTPacket_encode(buf + 1, rem_len); /* put the original remaining length back into the buffer */
00305 
00306     /* 3. read the rest of the buffer using a callback to supply the rest of the data */
00307     if((rem_len + len) > buflen)
00308         goto exit;
00309     if (rem_len && ((*getfn)(buf + len, rem_len) != rem_len))
00310         goto exit;
00311 
00312     header.byte = buf[0];
00313     rc = header.bits.type;
00314 exit:
00315     return rc;
00316 }
00317 
00318 /**
00319  * Decodes the message length according to the MQTT algorithm, non-blocking
00320  * @param trp pointer to a transport structure holding what is needed to solve getting data from it
00321  * @param value the decoded length returned
00322  * @return integer the number of bytes read from the socket, 0 for call again, or -1 on error
00323  */
00324 static int MQTTPacket_decodenb(MQTTTransport *trp)
00325 {
00326     unsigned char c;
00327     int rc = MQTTPACKET_READ_ERROR;
00328 
00329     FUNC_ENTRY;
00330     if(trp->len == 0){      /* initialize on first call */
00331         trp->multiplier = 1;
00332         trp->rem_len = 0;
00333     }
00334     do {
00335         int frc;
00336         if (trp->len >= MAX_NO_OF_REMAINING_LENGTH_BYTES)
00337             goto exit;
00338         if ((frc=(*trp->getfn)(trp->sck, &c, 1)) == -1)
00339             goto exit;
00340         if (frc == 0){
00341             rc = 0;
00342             goto exit;
00343         }
00344         ++(trp->len);
00345         trp->rem_len += (c & 127) * trp->multiplier;
00346         trp->multiplier *= 128;
00347     } while ((c & 128) != 0);
00348     rc = trp->len;
00349 exit:
00350     FUNC_EXIT_RC(rc);
00351     return rc;
00352 }
00353 
00354 /**
00355  * Helper function to read packet data from some source into a buffer, non-blocking
00356  * @param buf the buffer into which the packet will be serialized
00357  * @param buflen the length in bytes of the supplied buffer
00358  * @param trp pointer to a transport structure holding what is needed to solve getting data from it
00359  * @return integer MQTT packet type, 0 for call again, or -1 on error
00360  * @note  the whole message must fit into the caller's buffer
00361  */
00362 int MQTTPacket_readnb(unsigned char* buf, int buflen, MQTTTransport *trp)
00363 {
00364     int rc = -1, frc;
00365     MQTTHeader header = {0};
00366 
00367     switch(trp->state){
00368     default:
00369         trp->state = 0;
00370         /*FALLTHROUGH*/
00371     case 0:
00372         /* read the header byte.  This has the packet type in it */
00373         if ((frc=(*trp->getfn)(trp->sck, buf, 1)) == -1)
00374             goto exit;
00375         if (frc == 0)
00376             return 0;
00377         trp->len = 0;
00378         ++trp->state;
00379         /*FALLTHROUGH*/
00380         /* read the remaining length.  This is variable in itself */
00381     case 1:
00382         if((frc=MQTTPacket_decodenb(trp)) == MQTTPACKET_READ_ERROR)
00383             goto exit;
00384         if(frc == 0)
00385             return 0;
00386         trp->len = 1 + MQTTPacket_encode(buf + 1, trp->rem_len); /* put the original remaining length back into the buffer */
00387         if((trp->rem_len + trp->len) > buflen)
00388             goto exit;
00389         ++trp->state;
00390         /*FALLTHROUGH*/
00391     case 2:
00392         if(trp->rem_len){
00393             /* read the rest of the buffer using a callback to supply the rest of the data */
00394             if ((frc=(*trp->getfn)(trp->sck, buf + trp->len, trp->rem_len)) == -1)
00395                 goto exit;
00396             if (frc == 0)
00397                 return 0;
00398             trp->rem_len -= frc;
00399             trp->len += frc;
00400             if(trp->rem_len)
00401                 return 0;
00402         }
00403         header.byte = buf[0];
00404         rc = header.bits.type;
00405         break;
00406     }
00407 
00408 exit:
00409     trp->state = 0;
00410     return rc;
00411 }