Ian Craggs / MQTTPacket

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of MQTTPacket by MQTT

MQTTPacket.c@14:c2052aee81de, 2014-08-01 (annotated)

Committer:
Ian Craggs
Date:
Fri Aug 01 15:34:04 2014 +0100
Revision:
14:c2052aee81de
Parent:
8:b97b9873af52
Child:
17:c5bd28cc139a
Change C types to better fit MQTT types, and match MQTT-SN library

Who changed what in which revision?

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