Vergil Cola
/
MQTTGateway2
Fork of my original MQTTGateway
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Tue Jul 12 2022 18:06:46 by 1.7.2