Peter Ferland / Cayenne-MQTT-mbed-M1

ME910 support

Dependents:   5_Dragonfly_Cayenne_Sprint_IKS01A1

Fork of Cayenne-MQTT-mbed-MTSAS by Peter Ferland

src/MQTTCommon/MQTTPacket.c@0:09ef59d2d0f7, 2016-10-07 (annotated)

Committer:
jburhenn
Date:
Fri Oct 07 17:21:45 2016 +0000
Revision:
0:09ef59d2d0f7
Child:
22:0dbabcc6e7b2
Initial commit.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jburhenn 0:09ef59d2d0f7 1 /*******************************************************************************
jburhenn 0:09ef59d2d0f7 2 * Copyright (c) 2014 IBM Corp.
jburhenn 0:09ef59d2d0f7 3 *
jburhenn 0:09ef59d2d0f7 4 * All rights reserved. This program and the accompanying materials
jburhenn 0:09ef59d2d0f7 5 * are made available under the terms of the Eclipse Public License v1.0
jburhenn 0:09ef59d2d0f7 6 * and Eclipse Distribution License v1.0 which accompany this distribution.
jburhenn 0:09ef59d2d0f7 7 *
jburhenn 0:09ef59d2d0f7 8 * The Eclipse Public License is available at
jburhenn 0:09ef59d2d0f7 9 * http://www.eclipse.org/legal/epl-v10.html
jburhenn 0:09ef59d2d0f7 10 * and the Eclipse Distribution License is available at
jburhenn 0:09ef59d2d0f7 11 * http://www.eclipse.org/org/documents/edl-v10.php.
jburhenn 0:09ef59d2d0f7 12 *
jburhenn 0:09ef59d2d0f7 13 * Contributors:
jburhenn 0:09ef59d2d0f7 14 * Ian Craggs - initial API and implementation and/or initial documentation
jburhenn 0:09ef59d2d0f7 15 * Sergio R. Caprile - non-blocking packet read functions for stream transport
jburhenn 0:09ef59d2d0f7 16 *******************************************************************************/
jburhenn 0:09ef59d2d0f7 17
jburhenn 0:09ef59d2d0f7 18 #include "MQTTPacket.h"
jburhenn 0:09ef59d2d0f7 19
jburhenn 0:09ef59d2d0f7 20 #include <string.h>
jburhenn 0:09ef59d2d0f7 21
jburhenn 0:09ef59d2d0f7 22 /**
jburhenn 0:09ef59d2d0f7 23 * Encodes the message length according to the MQTT algorithm
jburhenn 0:09ef59d2d0f7 24 * @param buf the buffer into which the encoded data is written
jburhenn 0:09ef59d2d0f7 25 * @param length the length to be encoded
jburhenn 0:09ef59d2d0f7 26 * @return the number of bytes written to buffer
jburhenn 0:09ef59d2d0f7 27 */
jburhenn 0:09ef59d2d0f7 28 int MQTTPacket_encode(unsigned char* buf, int length)
jburhenn 0:09ef59d2d0f7 29 {
jburhenn 0:09ef59d2d0f7 30 int rc = 0;
jburhenn 0:09ef59d2d0f7 31
jburhenn 0:09ef59d2d0f7 32 do
jburhenn 0:09ef59d2d0f7 33 {
jburhenn 0:09ef59d2d0f7 34 char d = length % 128;
jburhenn 0:09ef59d2d0f7 35 length /= 128;
jburhenn 0:09ef59d2d0f7 36 /* if there are more digits to encode, set the top bit of this digit */
jburhenn 0:09ef59d2d0f7 37 if (length > 0)
jburhenn 0:09ef59d2d0f7 38 d |= 0x80;
jburhenn 0:09ef59d2d0f7 39 buf[rc++] = d;
jburhenn 0:09ef59d2d0f7 40 } while (length > 0);
jburhenn 0:09ef59d2d0f7 41 return rc;
jburhenn 0:09ef59d2d0f7 42 }
jburhenn 0:09ef59d2d0f7 43
jburhenn 0:09ef59d2d0f7 44
jburhenn 0:09ef59d2d0f7 45 /**
jburhenn 0:09ef59d2d0f7 46 * Decodes the message length according to the MQTT algorithm
jburhenn 0:09ef59d2d0f7 47 * @param getcharfn pointer to function to read the next character from the data source
jburhenn 0:09ef59d2d0f7 48 * @param value the decoded length returned
jburhenn 0:09ef59d2d0f7 49 * @return the number of bytes read from the socket
jburhenn 0:09ef59d2d0f7 50 */
jburhenn 0:09ef59d2d0f7 51 int MQTTPacket_decode(int (*getcharfn)(unsigned char*, int), int* value)
jburhenn 0:09ef59d2d0f7 52 {
jburhenn 0:09ef59d2d0f7 53 unsigned char c;
jburhenn 0:09ef59d2d0f7 54 int multiplier = 1;
jburhenn 0:09ef59d2d0f7 55 int len = 0;
jburhenn 0:09ef59d2d0f7 56 #define MAX_NO_OF_REMAINING_LENGTH_BYTES 4
jburhenn 0:09ef59d2d0f7 57
jburhenn 0:09ef59d2d0f7 58 *value = 0;
jburhenn 0:09ef59d2d0f7 59 do
jburhenn 0:09ef59d2d0f7 60 {
jburhenn 0:09ef59d2d0f7 61 int rc = MQTTPACKET_READ_ERROR;
jburhenn 0:09ef59d2d0f7 62
jburhenn 0:09ef59d2d0f7 63 if (++len > MAX_NO_OF_REMAINING_LENGTH_BYTES)
jburhenn 0:09ef59d2d0f7 64 {
jburhenn 0:09ef59d2d0f7 65 rc = MQTTPACKET_READ_ERROR; /* bad data */
jburhenn 0:09ef59d2d0f7 66 goto exit;
jburhenn 0:09ef59d2d0f7 67 }
jburhenn 0:09ef59d2d0f7 68 rc = (*getcharfn)(&c, 1);
jburhenn 0:09ef59d2d0f7 69 if (rc != 1)
jburhenn 0:09ef59d2d0f7 70 goto exit;
jburhenn 0:09ef59d2d0f7 71 *value += (c & 127) * multiplier;
jburhenn 0:09ef59d2d0f7 72 multiplier *= 128;
jburhenn 0:09ef59d2d0f7 73 } while ((c & 128) != 0);
jburhenn 0:09ef59d2d0f7 74 exit:
jburhenn 0:09ef59d2d0f7 75 return len;
jburhenn 0:09ef59d2d0f7 76 }
jburhenn 0:09ef59d2d0f7 77
jburhenn 0:09ef59d2d0f7 78
jburhenn 0:09ef59d2d0f7 79 int MQTTPacket_len(int rem_len)
jburhenn 0:09ef59d2d0f7 80 {
jburhenn 0:09ef59d2d0f7 81 rem_len += 1; /* header byte */
jburhenn 0:09ef59d2d0f7 82
jburhenn 0:09ef59d2d0f7 83 /* now remaining_length field */
jburhenn 0:09ef59d2d0f7 84 if (rem_len < 128)
jburhenn 0:09ef59d2d0f7 85 rem_len += 1;
jburhenn 0:09ef59d2d0f7 86 else if (rem_len < 16384)
jburhenn 0:09ef59d2d0f7 87 rem_len += 2;
jburhenn 0:09ef59d2d0f7 88 else if (rem_len < 2097151)
jburhenn 0:09ef59d2d0f7 89 rem_len += 3;
jburhenn 0:09ef59d2d0f7 90 else
jburhenn 0:09ef59d2d0f7 91 rem_len += 4;
jburhenn 0:09ef59d2d0f7 92 return rem_len;
jburhenn 0:09ef59d2d0f7 93 }
jburhenn 0:09ef59d2d0f7 94
jburhenn 0:09ef59d2d0f7 95
jburhenn 0:09ef59d2d0f7 96 static unsigned char* bufptr;
jburhenn 0:09ef59d2d0f7 97
jburhenn 0:09ef59d2d0f7 98 int bufchar(unsigned char* c, int count)
jburhenn 0:09ef59d2d0f7 99 {
jburhenn 0:09ef59d2d0f7 100 int i;
jburhenn 0:09ef59d2d0f7 101
jburhenn 0:09ef59d2d0f7 102 for (i = 0; i < count; ++i)
jburhenn 0:09ef59d2d0f7 103 *c = *bufptr++;
jburhenn 0:09ef59d2d0f7 104 return count;
jburhenn 0:09ef59d2d0f7 105 }
jburhenn 0:09ef59d2d0f7 106
jburhenn 0:09ef59d2d0f7 107
jburhenn 0:09ef59d2d0f7 108 int MQTTPacket_decodeBuf(unsigned char* buf, int* value)
jburhenn 0:09ef59d2d0f7 109 {
jburhenn 0:09ef59d2d0f7 110 bufptr = buf;
jburhenn 0:09ef59d2d0f7 111 return MQTTPacket_decode(bufchar, value);
jburhenn 0:09ef59d2d0f7 112 }
jburhenn 0:09ef59d2d0f7 113
jburhenn 0:09ef59d2d0f7 114
jburhenn 0:09ef59d2d0f7 115 /**
jburhenn 0:09ef59d2d0f7 116 * Calculates an integer from two bytes read from the input buffer
jburhenn 0:09ef59d2d0f7 117 * @param pptr pointer to the input buffer - incremented by the number of bytes used & returned
jburhenn 0:09ef59d2d0f7 118 * @return the integer value calculated
jburhenn 0:09ef59d2d0f7 119 */
jburhenn 0:09ef59d2d0f7 120 int readInt(unsigned char** pptr)
jburhenn 0:09ef59d2d0f7 121 {
jburhenn 0:09ef59d2d0f7 122 unsigned char* ptr = *pptr;
jburhenn 0:09ef59d2d0f7 123 int len = 256*(*ptr) + (*(ptr+1));
jburhenn 0:09ef59d2d0f7 124 *pptr += 2;
jburhenn 0:09ef59d2d0f7 125 return len;
jburhenn 0:09ef59d2d0f7 126 }
jburhenn 0:09ef59d2d0f7 127
jburhenn 0:09ef59d2d0f7 128
jburhenn 0:09ef59d2d0f7 129 /**
jburhenn 0:09ef59d2d0f7 130 * Reads one character from the input buffer.
jburhenn 0:09ef59d2d0f7 131 * @param pptr pointer to the input buffer - incremented by the number of bytes used & returned
jburhenn 0:09ef59d2d0f7 132 * @return the character read
jburhenn 0:09ef59d2d0f7 133 */
jburhenn 0:09ef59d2d0f7 134 char readChar(unsigned char** pptr)
jburhenn 0:09ef59d2d0f7 135 {
jburhenn 0:09ef59d2d0f7 136 char c = **pptr;
jburhenn 0:09ef59d2d0f7 137 (*pptr)++;
jburhenn 0:09ef59d2d0f7 138 return c;
jburhenn 0:09ef59d2d0f7 139 }
jburhenn 0:09ef59d2d0f7 140
jburhenn 0:09ef59d2d0f7 141
jburhenn 0:09ef59d2d0f7 142 /**
jburhenn 0:09ef59d2d0f7 143 * Writes one character to an output buffer.
jburhenn 0:09ef59d2d0f7 144 * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
jburhenn 0:09ef59d2d0f7 145 * @param c the character to write
jburhenn 0:09ef59d2d0f7 146 */
jburhenn 0:09ef59d2d0f7 147 void writeChar(unsigned char** pptr, char c)
jburhenn 0:09ef59d2d0f7 148 {
jburhenn 0:09ef59d2d0f7 149 **pptr = c;
jburhenn 0:09ef59d2d0f7 150 (*pptr)++;
jburhenn 0:09ef59d2d0f7 151 }
jburhenn 0:09ef59d2d0f7 152
jburhenn 0:09ef59d2d0f7 153
jburhenn 0:09ef59d2d0f7 154 /**
jburhenn 0:09ef59d2d0f7 155 * Writes an integer as 2 bytes to an output buffer.
jburhenn 0:09ef59d2d0f7 156 * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
jburhenn 0:09ef59d2d0f7 157 * @param anInt the integer to write
jburhenn 0:09ef59d2d0f7 158 */
jburhenn 0:09ef59d2d0f7 159 void writeInt(unsigned char** pptr, int anInt)
jburhenn 0:09ef59d2d0f7 160 {
jburhenn 0:09ef59d2d0f7 161 **pptr = (unsigned char)(anInt / 256);
jburhenn 0:09ef59d2d0f7 162 (*pptr)++;
jburhenn 0:09ef59d2d0f7 163 **pptr = (unsigned char)(anInt % 256);
jburhenn 0:09ef59d2d0f7 164 (*pptr)++;
jburhenn 0:09ef59d2d0f7 165 }
jburhenn 0:09ef59d2d0f7 166
jburhenn 0:09ef59d2d0f7 167
jburhenn 0:09ef59d2d0f7 168 /**
jburhenn 0:09ef59d2d0f7 169 * Writes a "UTF" string to an output buffer. Converts C string to length-delimited.
jburhenn 0:09ef59d2d0f7 170 * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
jburhenn 0:09ef59d2d0f7 171 * @param string the C string to write
jburhenn 0:09ef59d2d0f7 172 */
jburhenn 0:09ef59d2d0f7 173 void writeCString(unsigned char** pptr, const char* string)
jburhenn 0:09ef59d2d0f7 174 {
jburhenn 0:09ef59d2d0f7 175 int len = strlen(string);
jburhenn 0:09ef59d2d0f7 176 writeInt(pptr, len);
jburhenn 0:09ef59d2d0f7 177 memcpy(*pptr, string, len);
jburhenn 0:09ef59d2d0f7 178 *pptr += len;
jburhenn 0:09ef59d2d0f7 179 }
jburhenn 0:09ef59d2d0f7 180
jburhenn 0:09ef59d2d0f7 181
jburhenn 0:09ef59d2d0f7 182 int getLenStringLen(char* ptr)
jburhenn 0:09ef59d2d0f7 183 {
jburhenn 0:09ef59d2d0f7 184 int len = 256*((unsigned char)(*ptr)) + (unsigned char)(*(ptr+1));
jburhenn 0:09ef59d2d0f7 185 return len;
jburhenn 0:09ef59d2d0f7 186 }
jburhenn 0:09ef59d2d0f7 187
jburhenn 0:09ef59d2d0f7 188
jburhenn 0:09ef59d2d0f7 189 void writeMQTTString(unsigned char** pptr, MQTTString mqttstring)
jburhenn 0:09ef59d2d0f7 190 {
jburhenn 0:09ef59d2d0f7 191 if (mqttstring.lenstring.len > 0)
jburhenn 0:09ef59d2d0f7 192 {
jburhenn 0:09ef59d2d0f7 193 writeInt(pptr, mqttstring.lenstring.len);
jburhenn 0:09ef59d2d0f7 194 memcpy(*pptr, mqttstring.lenstring.data, mqttstring.lenstring.len);
jburhenn 0:09ef59d2d0f7 195 *pptr += mqttstring.lenstring.len;
jburhenn 0:09ef59d2d0f7 196 }
jburhenn 0:09ef59d2d0f7 197 else if (mqttstring.cstring)
jburhenn 0:09ef59d2d0f7 198 writeCString(pptr, mqttstring.cstring);
jburhenn 0:09ef59d2d0f7 199 else
jburhenn 0:09ef59d2d0f7 200 writeInt(pptr, 0);
jburhenn 0:09ef59d2d0f7 201 }
jburhenn 0:09ef59d2d0f7 202
jburhenn 0:09ef59d2d0f7 203
jburhenn 0:09ef59d2d0f7 204 /**
jburhenn 0:09ef59d2d0f7 205 * @param mqttstring the MQTTString structure into which the data is to be read
jburhenn 0:09ef59d2d0f7 206 * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
jburhenn 0:09ef59d2d0f7 207 * @param enddata pointer to the end of the data: do not read beyond
jburhenn 0:09ef59d2d0f7 208 * @return 1 if successful, 0 if not
jburhenn 0:09ef59d2d0f7 209 */
jburhenn 0:09ef59d2d0f7 210 int readMQTTLenString(MQTTString* mqttstring, unsigned char** pptr, unsigned char* enddata)
jburhenn 0:09ef59d2d0f7 211 {
jburhenn 0:09ef59d2d0f7 212 int rc = 0;
jburhenn 0:09ef59d2d0f7 213
jburhenn 0:09ef59d2d0f7 214 /* the first two bytes are the length of the string */
jburhenn 0:09ef59d2d0f7 215 if (enddata - (*pptr) > 1) /* enough length to read the integer? */
jburhenn 0:09ef59d2d0f7 216 {
jburhenn 0:09ef59d2d0f7 217 mqttstring->lenstring.len = readInt(pptr); /* increments pptr to point past length */
jburhenn 0:09ef59d2d0f7 218 if (&(*pptr)[mqttstring->lenstring.len] <= enddata)
jburhenn 0:09ef59d2d0f7 219 {
jburhenn 0:09ef59d2d0f7 220 mqttstring->lenstring.data = (char*)*pptr;
jburhenn 0:09ef59d2d0f7 221 *pptr += mqttstring->lenstring.len;
jburhenn 0:09ef59d2d0f7 222 rc = 1;
jburhenn 0:09ef59d2d0f7 223 }
jburhenn 0:09ef59d2d0f7 224 }
jburhenn 0:09ef59d2d0f7 225 mqttstring->cstring = NULL;
jburhenn 0:09ef59d2d0f7 226 return rc;
jburhenn 0:09ef59d2d0f7 227 }
jburhenn 0:09ef59d2d0f7 228
jburhenn 0:09ef59d2d0f7 229
jburhenn 0:09ef59d2d0f7 230 /**
jburhenn 0:09ef59d2d0f7 231 * Return the length of the MQTTstring - C string if there is one, otherwise the length delimited string
jburhenn 0:09ef59d2d0f7 232 * @param mqttstring the string to return the length of
jburhenn 0:09ef59d2d0f7 233 * @return the length of the string
jburhenn 0:09ef59d2d0f7 234 */
jburhenn 0:09ef59d2d0f7 235 int MQTTstrlen(MQTTString mqttstring)
jburhenn 0:09ef59d2d0f7 236 {
jburhenn 0:09ef59d2d0f7 237 int rc = 0;
jburhenn 0:09ef59d2d0f7 238
jburhenn 0:09ef59d2d0f7 239 if (mqttstring.cstring)
jburhenn 0:09ef59d2d0f7 240 rc = strlen(mqttstring.cstring);
jburhenn 0:09ef59d2d0f7 241 else
jburhenn 0:09ef59d2d0f7 242 rc = mqttstring.lenstring.len;
jburhenn 0:09ef59d2d0f7 243 return rc;
jburhenn 0:09ef59d2d0f7 244 }
jburhenn 0:09ef59d2d0f7 245
jburhenn 0:09ef59d2d0f7 246
jburhenn 0:09ef59d2d0f7 247 /**
jburhenn 0:09ef59d2d0f7 248 * Compares an MQTTString to a C string
jburhenn 0:09ef59d2d0f7 249 * @param a the MQTTString to compare
jburhenn 0:09ef59d2d0f7 250 * @param bptr the C string to compare
jburhenn 0:09ef59d2d0f7 251 * @return boolean - equal or not
jburhenn 0:09ef59d2d0f7 252 */
jburhenn 0:09ef59d2d0f7 253 int MQTTPacket_equals(MQTTString* a, char* bptr)
jburhenn 0:09ef59d2d0f7 254 {
jburhenn 0:09ef59d2d0f7 255 int alen = 0,
jburhenn 0:09ef59d2d0f7 256 blen = 0;
jburhenn 0:09ef59d2d0f7 257 char *aptr;
jburhenn 0:09ef59d2d0f7 258
jburhenn 0:09ef59d2d0f7 259 if (a->cstring)
jburhenn 0:09ef59d2d0f7 260 {
jburhenn 0:09ef59d2d0f7 261 aptr = a->cstring;
jburhenn 0:09ef59d2d0f7 262 alen = strlen(a->cstring);
jburhenn 0:09ef59d2d0f7 263 }
jburhenn 0:09ef59d2d0f7 264 else
jburhenn 0:09ef59d2d0f7 265 {
jburhenn 0:09ef59d2d0f7 266 aptr = a->lenstring.data;
jburhenn 0:09ef59d2d0f7 267 alen = a->lenstring.len;
jburhenn 0:09ef59d2d0f7 268 }
jburhenn 0:09ef59d2d0f7 269 blen = strlen(bptr);
jburhenn 0:09ef59d2d0f7 270
jburhenn 0:09ef59d2d0f7 271 return (alen == blen) && (strncmp(aptr, bptr, alen) == 0);
jburhenn 0:09ef59d2d0f7 272 }
jburhenn 0:09ef59d2d0f7 273
jburhenn 0:09ef59d2d0f7 274
jburhenn 0:09ef59d2d0f7 275 /**
jburhenn 0:09ef59d2d0f7 276 * Helper function to read packet data from some source into a buffer
jburhenn 0:09ef59d2d0f7 277 * @param buf the buffer into which the packet will be serialized
jburhenn 0:09ef59d2d0f7 278 * @param buflen the length in bytes of the supplied buffer
jburhenn 0:09ef59d2d0f7 279 * @param getfn pointer to a function which will read any number of bytes from the needed source
jburhenn 0:09ef59d2d0f7 280 * @return integer MQTT packet type, or -1 on error
jburhenn 0:09ef59d2d0f7 281 * @note the whole message must fit into the caller's buffer
jburhenn 0:09ef59d2d0f7 282 */
jburhenn 0:09ef59d2d0f7 283 int MQTTPacket_read(unsigned char* buf, int buflen, int (*getfn)(unsigned char*, int))
jburhenn 0:09ef59d2d0f7 284 {
jburhenn 0:09ef59d2d0f7 285 int rc = -1;
jburhenn 0:09ef59d2d0f7 286 MQTTHeader header = {0};
jburhenn 0:09ef59d2d0f7 287 int len = 0;
jburhenn 0:09ef59d2d0f7 288 int rem_len = 0;
jburhenn 0:09ef59d2d0f7 289
jburhenn 0:09ef59d2d0f7 290 /* 1. read the header byte. This has the packet type in it */
jburhenn 0:09ef59d2d0f7 291 if ((*getfn)(buf, 1) != 1)
jburhenn 0:09ef59d2d0f7 292 goto exit;
jburhenn 0:09ef59d2d0f7 293
jburhenn 0:09ef59d2d0f7 294 len = 1;
jburhenn 0:09ef59d2d0f7 295 /* 2. read the remaining length. This is variable in itself */
jburhenn 0:09ef59d2d0f7 296 MQTTPacket_decode(getfn, &rem_len);
jburhenn 0:09ef59d2d0f7 297 len += MQTTPacket_encode(buf + 1, rem_len); /* put the original remaining length back into the buffer */
jburhenn 0:09ef59d2d0f7 298
jburhenn 0:09ef59d2d0f7 299 /* 3. read the rest of the buffer using a callback to supply the rest of the data */
jburhenn 0:09ef59d2d0f7 300 if((rem_len + len) > buflen)
jburhenn 0:09ef59d2d0f7 301 goto exit;
jburhenn 0:09ef59d2d0f7 302 if ((*getfn)(buf + len, rem_len) != rem_len)
jburhenn 0:09ef59d2d0f7 303 goto exit;
jburhenn 0:09ef59d2d0f7 304
jburhenn 0:09ef59d2d0f7 305 header.byte = buf[0];
jburhenn 0:09ef59d2d0f7 306 rc = header.bits.type;
jburhenn 0:09ef59d2d0f7 307 exit:
jburhenn 0:09ef59d2d0f7 308 return rc;
jburhenn 0:09ef59d2d0f7 309 }
jburhenn 0:09ef59d2d0f7 310
jburhenn 0:09ef59d2d0f7 311 /**
jburhenn 0:09ef59d2d0f7 312 * Decodes the message length according to the MQTT algorithm, non-blocking
jburhenn 0:09ef59d2d0f7 313 * @param trp pointer to a transport structure holding what is needed to solve getting data from it
jburhenn 0:09ef59d2d0f7 314 * @param value the decoded length returned
jburhenn 0:09ef59d2d0f7 315 * @return integer the number of bytes read from the socket, 0 for call again, or -1 on error
jburhenn 0:09ef59d2d0f7 316 */
jburhenn 0:09ef59d2d0f7 317 static int MQTTPacket_decodenb(MQTTTransport *trp)
jburhenn 0:09ef59d2d0f7 318 {
jburhenn 0:09ef59d2d0f7 319 unsigned char c;
jburhenn 0:09ef59d2d0f7 320 int rc = MQTTPACKET_READ_ERROR;
jburhenn 0:09ef59d2d0f7 321
jburhenn 0:09ef59d2d0f7 322 if(trp->len == 0){ /* initialize on first call */
jburhenn 0:09ef59d2d0f7 323 trp->multiplier = 1;
jburhenn 0:09ef59d2d0f7 324 trp->rem_len = 0;
jburhenn 0:09ef59d2d0f7 325 }
jburhenn 0:09ef59d2d0f7 326 do {
jburhenn 0:09ef59d2d0f7 327 int frc;
jburhenn 0:09ef59d2d0f7 328 if (++(trp->len) > MAX_NO_OF_REMAINING_LENGTH_BYTES)
jburhenn 0:09ef59d2d0f7 329 goto exit;
jburhenn 0:09ef59d2d0f7 330 if ((frc=(*trp->getfn)(trp->sck, &c, 1)) == -1)
jburhenn 0:09ef59d2d0f7 331 goto exit;
jburhenn 0:09ef59d2d0f7 332 if (frc == 0){
jburhenn 0:09ef59d2d0f7 333 rc = 0;
jburhenn 0:09ef59d2d0f7 334 goto exit;
jburhenn 0:09ef59d2d0f7 335 }
jburhenn 0:09ef59d2d0f7 336 trp->rem_len += (c & 127) * trp->multiplier;
jburhenn 0:09ef59d2d0f7 337 trp->multiplier *= 128;
jburhenn 0:09ef59d2d0f7 338 } while ((c & 128) != 0);
jburhenn 0:09ef59d2d0f7 339 rc = trp->len;
jburhenn 0:09ef59d2d0f7 340 exit:
jburhenn 0:09ef59d2d0f7 341 return rc;
jburhenn 0:09ef59d2d0f7 342 }
jburhenn 0:09ef59d2d0f7 343
jburhenn 0:09ef59d2d0f7 344 /**
jburhenn 0:09ef59d2d0f7 345 * Helper function to read packet data from some source into a buffer, non-blocking
jburhenn 0:09ef59d2d0f7 346 * @param buf the buffer into which the packet will be serialized
jburhenn 0:09ef59d2d0f7 347 * @param buflen the length in bytes of the supplied buffer
jburhenn 0:09ef59d2d0f7 348 * @param trp pointer to a transport structure holding what is needed to solve getting data from it
jburhenn 0:09ef59d2d0f7 349 * @return integer MQTT packet type, 0 for call again, or -1 on error
jburhenn 0:09ef59d2d0f7 350 * @note the whole message must fit into the caller's buffer
jburhenn 0:09ef59d2d0f7 351 */
jburhenn 0:09ef59d2d0f7 352 int MQTTPacket_readnb(unsigned char* buf, int buflen, MQTTTransport *trp)
jburhenn 0:09ef59d2d0f7 353 {
jburhenn 0:09ef59d2d0f7 354 int rc = -1, frc;
jburhenn 0:09ef59d2d0f7 355 MQTTHeader header = {0};
jburhenn 0:09ef59d2d0f7 356
jburhenn 0:09ef59d2d0f7 357 switch(trp->state){
jburhenn 0:09ef59d2d0f7 358 default:
jburhenn 0:09ef59d2d0f7 359 trp->state = 0;
jburhenn 0:09ef59d2d0f7 360 /*FALLTHROUGH*/
jburhenn 0:09ef59d2d0f7 361 case 0:
jburhenn 0:09ef59d2d0f7 362 /* read the header byte. This has the packet type in it */
jburhenn 0:09ef59d2d0f7 363 if ((frc=(*trp->getfn)(trp->sck, buf, 1)) == -1)
jburhenn 0:09ef59d2d0f7 364 goto exit;
jburhenn 0:09ef59d2d0f7 365 if (frc == 0)
jburhenn 0:09ef59d2d0f7 366 return 0;
jburhenn 0:09ef59d2d0f7 367 trp->len = 0;
jburhenn 0:09ef59d2d0f7 368 ++trp->state;
jburhenn 0:09ef59d2d0f7 369 /*FALLTHROUGH*/
jburhenn 0:09ef59d2d0f7 370 /* read the remaining length. This is variable in itself */
jburhenn 0:09ef59d2d0f7 371 case 1:
jburhenn 0:09ef59d2d0f7 372 if((frc=MQTTPacket_decodenb(trp)) == MQTTPACKET_READ_ERROR)
jburhenn 0:09ef59d2d0f7 373 goto exit;
jburhenn 0:09ef59d2d0f7 374 if(frc == 0)
jburhenn 0:09ef59d2d0f7 375 return 0;
jburhenn 0:09ef59d2d0f7 376 trp->len = 1 + MQTTPacket_encode(buf + 1, trp->rem_len); /* put the original remaining length back into the buffer */
jburhenn 0:09ef59d2d0f7 377 if((trp->rem_len + trp->len) > buflen)
jburhenn 0:09ef59d2d0f7 378 goto exit;
jburhenn 0:09ef59d2d0f7 379 ++trp->state;
jburhenn 0:09ef59d2d0f7 380 /*FALLTHROUGH*/
jburhenn 0:09ef59d2d0f7 381 case 2:
jburhenn 0:09ef59d2d0f7 382 /* read the rest of the buffer using a callback to supply the rest of the data */
jburhenn 0:09ef59d2d0f7 383 if ((frc=(*trp->getfn)(trp->sck, buf + trp->len, trp->rem_len)) == -1)
jburhenn 0:09ef59d2d0f7 384 goto exit;
jburhenn 0:09ef59d2d0f7 385 if (frc == 0)
jburhenn 0:09ef59d2d0f7 386 return 0;
jburhenn 0:09ef59d2d0f7 387 trp->rem_len -= frc;
jburhenn 0:09ef59d2d0f7 388 trp->len += frc;
jburhenn 0:09ef59d2d0f7 389 if(trp->rem_len)
jburhenn 0:09ef59d2d0f7 390 return 0;
jburhenn 0:09ef59d2d0f7 391
jburhenn 0:09ef59d2d0f7 392 header.byte = buf[0];
jburhenn 0:09ef59d2d0f7 393 rc = header.bits.type;
jburhenn 0:09ef59d2d0f7 394 break;
jburhenn 0:09ef59d2d0f7 395 }
jburhenn 0:09ef59d2d0f7 396
jburhenn 0:09ef59d2d0f7 397 exit:
jburhenn 0:09ef59d2d0f7 398 trp->state = 0;
jburhenn 0:09ef59d2d0f7 399 return rc;
jburhenn 0:09ef59d2d0f7 400 }
jburhenn 0:09ef59d2d0f7 401