ME910 support
Dependents: 5_Dragonfly_Cayenne_Sprint_IKS01A1
Fork of Cayenne-MQTT-mbed-MTSAS by
src/MQTTCommon/MQTTPacket.c
- Committer:
- jburhenn
- Date:
- 2016-10-07
- Revision:
- 0:09ef59d2d0f7
- Child:
- 22:0dbabcc6e7b2
File content as of revision 0:09ef59d2d0f7:
/******************************************************************************* * Copyright (c) 2014 IBM Corp. * * All rights reserved. This program and the accompanying materials * are made available under the terms of the Eclipse Public License v1.0 * and Eclipse Distribution License v1.0 which accompany this distribution. * * The Eclipse Public License is available at * http://www.eclipse.org/legal/epl-v10.html * and the Eclipse Distribution License is available at * http://www.eclipse.org/org/documents/edl-v10.php. * * Contributors: * Ian Craggs - initial API and implementation and/or initial documentation * Sergio R. Caprile - non-blocking packet read functions for stream transport *******************************************************************************/ #include "MQTTPacket.h" #include <string.h> /** * Encodes the message length according to the MQTT algorithm * @param buf the buffer into which the encoded data is written * @param length the length to be encoded * @return the number of bytes written to buffer */ int MQTTPacket_encode(unsigned char* buf, int length) { int rc = 0; do { char d = length % 128; length /= 128; /* if there are more digits to encode, set the top bit of this digit */ if (length > 0) d |= 0x80; buf[rc++] = d; } while (length > 0); return rc; } /** * Decodes the message length according to the MQTT algorithm * @param getcharfn pointer to function to read the next character from the data source * @param value the decoded length returned * @return the number of bytes read from the socket */ int MQTTPacket_decode(int (*getcharfn)(unsigned char*, int), int* value) { unsigned char c; int multiplier = 1; int len = 0; #define MAX_NO_OF_REMAINING_LENGTH_BYTES 4 *value = 0; do { int rc = MQTTPACKET_READ_ERROR; if (++len > MAX_NO_OF_REMAINING_LENGTH_BYTES) { rc = MQTTPACKET_READ_ERROR; /* bad data */ goto exit; } rc = (*getcharfn)(&c, 1); if (rc != 1) goto exit; *value += (c & 127) * multiplier; multiplier *= 128; } while ((c & 128) != 0); exit: return len; } int MQTTPacket_len(int rem_len) { rem_len += 1; /* header byte */ /* now remaining_length field */ if (rem_len < 128) rem_len += 1; else if (rem_len < 16384) rem_len += 2; else if (rem_len < 2097151) rem_len += 3; else rem_len += 4; return rem_len; } static unsigned char* bufptr; int bufchar(unsigned char* c, int count) { int i; for (i = 0; i < count; ++i) *c = *bufptr++; return count; } int MQTTPacket_decodeBuf(unsigned char* buf, int* value) { bufptr = buf; return MQTTPacket_decode(bufchar, value); } /** * Calculates an integer from two bytes read from the input buffer * @param pptr pointer to the input buffer - incremented by the number of bytes used & returned * @return the integer value calculated */ int readInt(unsigned char** pptr) { unsigned char* ptr = *pptr; int len = 256*(*ptr) + (*(ptr+1)); *pptr += 2; return len; } /** * Reads one character from the input buffer. * @param pptr pointer to the input buffer - incremented by the number of bytes used & returned * @return the character read */ char readChar(unsigned char** pptr) { char c = **pptr; (*pptr)++; return c; } /** * Writes one character to an output buffer. * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned * @param c the character to write */ void writeChar(unsigned char** pptr, char c) { **pptr = c; (*pptr)++; } /** * Writes an integer as 2 bytes to an output buffer. * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned * @param anInt the integer to write */ void writeInt(unsigned char** pptr, int anInt) { **pptr = (unsigned char)(anInt / 256); (*pptr)++; **pptr = (unsigned char)(anInt % 256); (*pptr)++; } /** * Writes a "UTF" string to an output buffer. Converts C string to length-delimited. * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned * @param string the C string to write */ void writeCString(unsigned char** pptr, const char* string) { int len = strlen(string); writeInt(pptr, len); memcpy(*pptr, string, len); *pptr += len; } int getLenStringLen(char* ptr) { int len = 256*((unsigned char)(*ptr)) + (unsigned char)(*(ptr+1)); return len; } void writeMQTTString(unsigned char** pptr, MQTTString mqttstring) { if (mqttstring.lenstring.len > 0) { writeInt(pptr, mqttstring.lenstring.len); memcpy(*pptr, mqttstring.lenstring.data, mqttstring.lenstring.len); *pptr += mqttstring.lenstring.len; } else if (mqttstring.cstring) writeCString(pptr, mqttstring.cstring); else writeInt(pptr, 0); } /** * @param mqttstring the MQTTString structure into which the data is to be read * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned * @param enddata pointer to the end of the data: do not read beyond * @return 1 if successful, 0 if not */ int readMQTTLenString(MQTTString* mqttstring, unsigned char** pptr, unsigned char* enddata) { int rc = 0; /* the first two bytes are the length of the string */ if (enddata - (*pptr) > 1) /* enough length to read the integer? */ { mqttstring->lenstring.len = readInt(pptr); /* increments pptr to point past length */ if (&(*pptr)[mqttstring->lenstring.len] <= enddata) { mqttstring->lenstring.data = (char*)*pptr; *pptr += mqttstring->lenstring.len; rc = 1; } } mqttstring->cstring = NULL; return rc; } /** * Return the length of the MQTTstring - C string if there is one, otherwise the length delimited string * @param mqttstring the string to return the length of * @return the length of the string */ int MQTTstrlen(MQTTString mqttstring) { int rc = 0; if (mqttstring.cstring) rc = strlen(mqttstring.cstring); else rc = mqttstring.lenstring.len; return rc; } /** * Compares an MQTTString to a C string * @param a the MQTTString to compare * @param bptr the C string to compare * @return boolean - equal or not */ int MQTTPacket_equals(MQTTString* a, char* bptr) { int alen = 0, blen = 0; char *aptr; if (a->cstring) { aptr = a->cstring; alen = strlen(a->cstring); } else { aptr = a->lenstring.data; alen = a->lenstring.len; } blen = strlen(bptr); return (alen == blen) && (strncmp(aptr, bptr, alen) == 0); } /** * Helper function to read packet data from some source into a buffer * @param buf the buffer into which the packet will be serialized * @param buflen the length in bytes of the supplied buffer * @param getfn pointer to a function which will read any number of bytes from the needed source * @return integer MQTT packet type, or -1 on error * @note the whole message must fit into the caller's buffer */ int MQTTPacket_read(unsigned char* buf, int buflen, int (*getfn)(unsigned char*, int)) { int rc = -1; MQTTHeader header = {0}; int len = 0; int rem_len = 0; /* 1. read the header byte. This has the packet type in it */ if ((*getfn)(buf, 1) != 1) goto exit; len = 1; /* 2. read the remaining length. This is variable in itself */ MQTTPacket_decode(getfn, &rem_len); len += MQTTPacket_encode(buf + 1, rem_len); /* put the original remaining length back into the buffer */ /* 3. read the rest of the buffer using a callback to supply the rest of the data */ if((rem_len + len) > buflen) goto exit; if ((*getfn)(buf + len, rem_len) != rem_len) goto exit; header.byte = buf[0]; rc = header.bits.type; exit: return rc; } /** * Decodes the message length according to the MQTT algorithm, non-blocking * @param trp pointer to a transport structure holding what is needed to solve getting data from it * @param value the decoded length returned * @return integer the number of bytes read from the socket, 0 for call again, or -1 on error */ static int MQTTPacket_decodenb(MQTTTransport *trp) { unsigned char c; int rc = MQTTPACKET_READ_ERROR; if(trp->len == 0){ /* initialize on first call */ trp->multiplier = 1; trp->rem_len = 0; } do { int frc; if (++(trp->len) > MAX_NO_OF_REMAINING_LENGTH_BYTES) goto exit; if ((frc=(*trp->getfn)(trp->sck, &c, 1)) == -1) goto exit; if (frc == 0){ rc = 0; goto exit; } trp->rem_len += (c & 127) * trp->multiplier; trp->multiplier *= 128; } while ((c & 128) != 0); rc = trp->len; exit: return rc; } /** * Helper function to read packet data from some source into a buffer, non-blocking * @param buf the buffer into which the packet will be serialized * @param buflen the length in bytes of the supplied buffer * @param trp pointer to a transport structure holding what is needed to solve getting data from it * @return integer MQTT packet type, 0 for call again, or -1 on error * @note the whole message must fit into the caller's buffer */ int MQTTPacket_readnb(unsigned char* buf, int buflen, MQTTTransport *trp) { int rc = -1, frc; MQTTHeader header = {0}; switch(trp->state){ default: trp->state = 0; /*FALLTHROUGH*/ case 0: /* read the header byte. This has the packet type in it */ if ((frc=(*trp->getfn)(trp->sck, buf, 1)) == -1) goto exit; if (frc == 0) return 0; trp->len = 0; ++trp->state; /*FALLTHROUGH*/ /* read the remaining length. This is variable in itself */ case 1: if((frc=MQTTPacket_decodenb(trp)) == MQTTPACKET_READ_ERROR) goto exit; if(frc == 0) return 0; trp->len = 1 + MQTTPacket_encode(buf + 1, trp->rem_len); /* put the original remaining length back into the buffer */ if((trp->rem_len + trp->len) > buflen) goto exit; ++trp->state; /*FALLTHROUGH*/ case 2: /* read the rest of the buffer using a callback to supply the rest of the data */ if ((frc=(*trp->getfn)(trp->sck, buf + trp->len, trp->rem_len)) == -1) goto exit; if (frc == 0) return 0; trp->rem_len -= frc; trp->len += frc; if(trp->rem_len) return 0; header.byte = buf[0]; rc = header.bits.type; break; } exit: trp->state = 0; return rc; }