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  *******************************************************************************/
00016 
00017 #include "StackTrace.h"
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     FUNC_ENTRY;
00033     do
00034     {
00035         char d = length % 128;
00036         length /= 128;
00037         /* if there are more digits to encode, set the top bit of this digit */
00038         if (length > 0)
00039             d |= 0x80;
00040         buf[rc++] = d;
00041     } while (length > 0);
00042     FUNC_EXIT_RC(rc);
00043     return rc;
00044 }
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 int MQTTPacket_decode(int (*getcharfn)(unsigned char*, int), int* value)
00054 {
00055     unsigned char c;
00056     int multiplier = 1;
00057     int len = 0;
00058 #define MAX_NO_OF_REMAINING_LENGTH_BYTES 4
00059 
00060     FUNC_ENTRY;
00061     *value = 0;
00062     do
00063     {
00064         int rc = MQTTPACKET_READ_ERROR;
00065 
00066         if (++len > MAX_NO_OF_REMAINING_LENGTH_BYTES)
00067         {
00068             rc = MQTTPACKET_READ_ERROR; /* bad data */
00069             goto exit;
00070         }
00071         rc = (*getcharfn)(&c, 1);
00072         if (rc != 1)
00073             goto exit;
00074         *value += (c & 127) * multiplier;
00075         multiplier *= 128;
00076     } while ((c & 128) != 0);
00077 exit:
00078     FUNC_EXIT_RC(len);
00079     return len;
00080 }
00081 
00082 
00083 int MQTTPacket_len(int rem_len)
00084 {
00085     rem_len += 1; /* header byte */
00086 
00087     /* now remaining_length field */
00088     if (rem_len < 128)
00089         rem_len += 1;
00090     else if (rem_len < 16384)
00091         rem_len += 2;
00092     else if (rem_len < 2097151)
00093         rem_len += 3;
00094     else
00095         rem_len += 4;
00096     return rem_len;
00097 }
00098 
00099 
00100 static unsigned char* bufptr;
00101 
00102 int bufchar(unsigned char* c, int count)
00103 {
00104     int i;
00105 
00106     for (i = 0; i < count; ++i)
00107         *c = *bufptr++;
00108     return count;
00109 }
00110 
00111 
00112 int MQTTPacket_decodeBuf(unsigned char* buf, int* value)
00113 {
00114     bufptr = buf;
00115     return MQTTPacket_decode(bufchar, value);
00116 }
00117 
00118 
00119 /**
00120  * Calculates an integer from two bytes read from the input buffer
00121  * @param pptr pointer to the input buffer - incremented by the number of bytes used & returned
00122  * @return the integer value calculated
00123  */
00124 int readInt(unsigned char** pptr)
00125 {
00126     unsigned char* ptr = *pptr;
00127     int len = 256*(*ptr) + (*(ptr+1));
00128     *pptr += 2;
00129     return len;
00130 }
00131 
00132 
00133 /**
00134  * Reads one character from the input buffer.
00135  * @param pptr pointer to the input buffer - incremented by the number of bytes used & returned
00136  * @return the character read
00137  */
00138 char readChar(unsigned char** pptr)
00139 {
00140     char c = **pptr;
00141     (*pptr)++;
00142     return c;
00143 }
00144 
00145 
00146 /**
00147  * Writes one character to an output buffer.
00148  * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
00149  * @param c the character to write
00150  */
00151 void writeChar(unsigned char** pptr, char c)
00152 {
00153     **pptr = c;
00154     (*pptr)++;
00155 }
00156 
00157 
00158 /**
00159  * Writes an integer as 2 bytes to an output buffer.
00160  * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
00161  * @param anInt the integer to write
00162  */
00163 void writeInt(unsigned char** pptr, int anInt)
00164 {
00165     **pptr = (unsigned char)(anInt / 256);
00166     (*pptr)++;
00167     **pptr = (unsigned char)(anInt % 256);
00168     (*pptr)++;
00169 }
00170 
00171 
00172 /**
00173  * Writes a "UTF" string to an output buffer.  Converts C string to length-delimited.
00174  * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
00175  * @param string the C string to write
00176  */
00177 void writeCString(unsigned char** pptr, const char* string)
00178 {
00179     int len = strlen(string);
00180     writeInt(pptr, len);
00181     memcpy(*pptr, string, len);
00182     *pptr += len;
00183 }
00184 
00185 
00186 int getLenStringLen(char* ptr)
00187 {
00188     int len = 256*((unsigned char)(*ptr)) + (unsigned char)(*(ptr+1));
00189     return len;
00190 }
00191 
00192 
00193 void writeMQTTString(unsigned char** pptr, MQTTString mqttstring)
00194 {
00195     if (mqttstring.lenstring.len > 0)
00196     {
00197         writeInt(pptr, mqttstring.lenstring.len);
00198         memcpy(*pptr, mqttstring.lenstring.data, mqttstring.lenstring.len);
00199         *pptr += mqttstring.lenstring.len;
00200     }
00201     else if (mqttstring.cstring)
00202         writeCString(pptr, mqttstring.cstring);
00203     else
00204         writeInt(pptr, 0);
00205 }
00206 
00207 
00208 /**
00209  * @param mqttstring the MQTTString structure into which the data is to be read
00210  * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
00211  * @param enddata pointer to the end of the data: do not read beyond
00212  * @return 1 if successful, 0 if not
00213  */
00214 int readMQTTLenString(MQTTString* mqttstring, unsigned char** pptr, unsigned char* enddata)
00215 {
00216     int rc = 0;
00217 
00218     FUNC_ENTRY;
00219     /* the first two bytes are the length of the string */
00220     if (enddata - (*pptr) > 1) /* enough length to read the integer? */
00221     {
00222         mqttstring->lenstring.len = readInt(pptr); /* increments pptr to point past length */
00223         if (&(*pptr)[mqttstring->lenstring.len] <= enddata)
00224         {
00225             mqttstring->lenstring.data = (char*)*pptr;
00226             *pptr += mqttstring->lenstring.len;
00227             rc = 1;
00228         }
00229     }
00230     mqttstring->cstring = NULL;
00231     FUNC_EXIT_RC(rc);
00232     return rc;
00233 }
00234 
00235 
00236 /**
00237  * Return the length of the MQTTstring - C string if there is one, otherwise the length delimited string
00238  * @param mqttstring the string to return the length of
00239  * @return the length of the string
00240  */
00241 int MQTTstrlen(MQTTString mqttstring)
00242 {
00243     int rc = 0;
00244 
00245     if (mqttstring.cstring)
00246         rc = strlen(mqttstring.cstring);
00247     else
00248         rc = mqttstring.lenstring.len;
00249     return rc;
00250 }
00251 
00252 
00253 /**
00254  * Compares an MQTTString to a C string
00255  * @param a the MQTTString to compare
00256  * @param bptr the C string to compare
00257  * @return boolean - equal or not
00258  */
00259 int MQTTPacket_equals(MQTTString* a, char* bptr)
00260 {
00261     int alen = 0,
00262         blen = 0;
00263     char *aptr;
00264     
00265     if (a->cstring)
00266     {
00267         aptr = a->cstring;
00268         alen = strlen(a->cstring);
00269     }
00270     else
00271     {
00272         aptr = a->lenstring.data;
00273         alen = a->lenstring.len;
00274     }
00275     blen = strlen(bptr);
00276     
00277     return (alen == blen) && (strncmp(aptr, bptr, alen) == 0);
00278 }
00279 
00280 
00281 /**
00282  * Helper function to read packet data from some source into a buffer
00283  * @param buf the buffer into which the packet will be serialized
00284  * @param buflen the length in bytes of the supplied buffer
00285  * @param getfn pointer to a function which will read any number of bytes from the needed source
00286  * @return integer MQTT packet type, or -1 on error
00287  */
00288 int MQTTPacket_read(unsigned char* buf, int buflen, int (*getfn)(unsigned char*, int))
00289 {
00290     int rc = -1;
00291     MQTTHeader header = {0};
00292     int len = 0;
00293     int rem_len = 0;
00294 
00295     /* 1. read the header byte.  This has the packet type in it */
00296     if ((*getfn)(buf, 1) != 1)
00297         goto exit;
00298 
00299     len = 1;
00300     /* 2. read the remaining length.  This is variable in itself */
00301     MQTTPacket_decode(getfn, &rem_len);
00302     len += MQTTPacket_encode(buf + 1, rem_len); /* put the original remaining length back into the buffer */
00303 
00304     /* 3. read the rest of the buffer using a callback to supply the rest of the data */
00305     if ((*getfn)(buf + len, rem_len) != rem_len)
00306         goto exit;
00307 
00308     header.byte = buf[0];
00309     rc = header.bits.type;
00310 exit:
00311     return rc;
00312 }
00313 
00314 
00315 const char* MQTTPacket_names[] =
00316 {
00317     "RESERVED", "CONNECT", "CONNACK", "PUBLISH", "PUBACK", "PUBREC", "PUBREL",
00318     "PUBCOMP", "SUBSCRIBE", "SUBACK", "UNSUBSCRIBE", "UNSUBACK",
00319     "PINGREQ", "PINGRESP", "DISCONNECT"
00320 };
00321 
00322 
00323 char* MQTTPacket_toString(char* strbuf, int strbuflen, unsigned char* buf, int buflen)
00324 {
00325     int index = 0;
00326     int rem_length = 0;
00327     MQTTHeader header = {0};
00328     int strindex = 0;
00329 
00330     header.byte = buf[index++];
00331     index += MQTTPacket_decodeBuf(&buf[index], &rem_length);
00332 
00333     switch (header.bits.type)
00334     {
00335     case CONNECT:
00336     {
00337         MQTTPacket_connectData data;
00338         if (MQTTDeserialize_connect(&data, buf, buflen) == 1)
00339         {
00340             strindex = snprintf(strbuf, strbuflen,
00341                 "CONNECT MQTT version %d, client id %.*s, clean session %d, keep alive %hd",
00342                 (int)data.MQTTVersion, data.clientID.lenstring.len, data.clientID.lenstring.data,
00343                 (int)data.cleansession, data.keepAliveInterval);
00344             if (data.willFlag)
00345                 strindex += snprintf(&strbuf[strindex], strbuflen - strindex,
00346                 ", will QoS %d, will retain %d, will topic %.*s, will message %.*s",
00347                 data.will.qos, data.will.retained,
00348                 data.will.topicName.lenstring.len, data.will.topicName.lenstring.data,
00349                 data.will.message.lenstring.len, data.will.message.lenstring.data);
00350             if (data.username.lenstring.data && data.username.lenstring.len > 0)
00351             {
00352                 printf("user name\n");
00353                 strindex += snprintf(&strbuf[strindex], strbuflen - strindex,
00354                 ", user name %.*s", data.username.lenstring.len, data.username.lenstring.data);
00355             }
00356             if (data.password.lenstring.data && data.password.lenstring.len > 0)
00357                 strindex += snprintf(&strbuf[strindex], strbuflen - strindex,
00358                 ", password %.*s", data.password.lenstring.len, data.password.lenstring.data);
00359         }
00360     }
00361     break;
00362     case CONNACK:
00363     {
00364         unsigned char sessionPresent, connack_rc;
00365         if (MQTTDeserialize_connack(&sessionPresent, &connack_rc, buf, buflen) == 1)
00366             strindex = snprintf(strbuf, strbuflen,
00367             "CONNACK session present %d, rc %d", sessionPresent, connack_rc);
00368     }
00369     break;
00370     case PUBLISH:
00371     {
00372         unsigned char dup, retained, *payload;
00373         unsigned short packetid;
00374         int qos, payloadlen;
00375         MQTTString topicName = MQTTString_initializer;
00376         if (MQTTDeserialize_publish(&dup, &qos, &retained, &packetid, &topicName,
00377                 &payload, &payloadlen, buf, buflen) == 1)
00378             strindex = snprintf(strbuf, strbuflen,
00379                 "PUBLISH dup %d, QoS %d, retained %d, packet id %d, topic %.*s, payload length %d, payload %.*s",
00380                 dup, qos, retained, packetid,
00381                 (topicName.lenstring.len < 20) ? topicName.lenstring.len : 20, topicName.lenstring.data,
00382                 payloadlen, (payloadlen < 20) ? payloadlen : 20, payload);
00383     }
00384     break;
00385     case PUBACK:
00386     case PUBREC:
00387     case PUBREL:
00388     case PUBCOMP:
00389     {
00390         unsigned char packettype, dup;
00391         unsigned short packetid;
00392         if (MQTTDeserialize_ack(&packettype, &dup, &packetid, buf, buflen) == 1)
00393             strindex = snprintf(strbuf, strbuflen,
00394                 "%s dup %d, packet id %d",
00395                 MQTTPacket_names[packettype], dup, packetid);
00396     }
00397     break;
00398     case SUBSCRIBE:
00399     {
00400         unsigned char dup;
00401         unsigned short packetid;
00402         int maxcount = 1, count = 0;
00403         MQTTString topicFilters[1];
00404         int requestedQoSs[1];
00405         if (MQTTDeserialize_subscribe(&dup, &packetid, maxcount, &count,
00406                 topicFilters, requestedQoSs, buf, buflen) == 1)
00407             strindex = snprintf(strbuf, strbuflen,
00408                 "SUBSCRIBE dup %d, packet id %d count %d topic %.*s qos %d",
00409                 dup, packetid, count,
00410                 topicFilters[0].lenstring.len, topicFilters[0].lenstring.data,
00411                 requestedQoSs[0]);
00412     }
00413     break;
00414     case SUBACK:
00415     {
00416         unsigned short packetid;
00417         int maxcount = 1, count = 0;
00418         int grantedQoSs[1];
00419         if (MQTTDeserialize_suback(&packetid, maxcount, &count, grantedQoSs, buf, buflen) == 1)
00420             strindex = snprintf(strbuf, strbuflen,
00421                 "SUBACK packet id %d count %d granted qos %d",
00422                 packetid, count, grantedQoSs[0]);
00423     }
00424     break;
00425     case UNSUBSCRIBE:
00426     {
00427         unsigned char dup;
00428         unsigned short packetid;
00429         int maxcount = 1, count = 0;
00430         MQTTString topicFilters[1];
00431         if (MQTTDeserialize_unsubscribe(&dup, &packetid, maxcount, &count, topicFilters, buf, buflen) == 1)
00432             strindex = snprintf(strbuf, strbuflen,
00433                 "UNSUBSCRIBE dup %d, packet id %d count %d topic %.*s",
00434                 dup, packetid, count,
00435                 topicFilters[0].lenstring.len, topicFilters[0].lenstring.data);
00436     }
00437     break;
00438     case UNSUBACK:
00439     {
00440         unsigned short packetid;
00441         if (MQTTDeserialize_unsuback(&packetid, buf, buflen) == 1)
00442             strindex = snprintf(strbuf, strbuflen,
00443                 "UNSUBACK packet id %d", packetid);
00444     }
00445     break;
00446     case PINGREQ:
00447     case PINGRESP:
00448     case DISCONNECT:
00449         strindex = snprintf(strbuf, strbuflen, "%s", MQTTPacket_names[header.bits.type]);
00450         break;
00451     }
00452     return strbuf;
00453 }