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