Zoltan Hudak / Mbed OS MQTT_Hello

MQTT client to test the ENC28J60-EMAC on NUCLEO-F446RE.

Dependencies:   ENC28J60-EMAC

MQTT/MQTTPacket/MQTTPacket.c@5:d9570dbf2f82, 2021-03-29 (annotated)

Committer:
hudakz
Date:
Mon Mar 29 09:32:44 2021 +0000
Revision:
5:d9570dbf2f82
Parent:
0:238f0d0c0ba3 
MQTT client to test the ENC28J60-EMAC on NUCLEO-F446RE.

Who changed what in which revision?

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