MQTTPacket library (clone of https://os.mbed.com/teams/mqtt/code/MQTTPacket/)
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 * 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 }
Generated on Thu Jul 14 2022 11:12:53 by 1.7.2