mbtk/libmbtk_lib/mqtt/MQTTPacket/MQTTPacket.c - T108_Public - Gitiles

 /*******************************************************************************
  * 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 "StackTrace.h"
 #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;

 	FUNC_ENTRY;
 	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);
 	FUNC_EXIT_RC(rc);
 	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

 	FUNC_ENTRY;
 	*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:
 	FUNC_EXIT_RC(len);
 	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;

 	FUNC_ENTRY;
 	/* 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;
 	FUNC_EXIT_RC(rc);
 	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;

 	FUNC_ENTRY;
 	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:
 	FUNC_EXIT_RC(rc);
 	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;
 }
	/*******************************************************************************
	* 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 "StackTrace.h"
	#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;

	FUNC_ENTRY;
	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);
	FUNC_EXIT_RC(rc);
	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

	FUNC_ENTRY;
	*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:
	FUNC_EXIT_RC(len);
	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;

	FUNC_ENTRY;
	/* 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;
	FUNC_EXIT_RC(rc);
	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;

	FUNC_ENTRY;
	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:
	FUNC_EXIT_RC(rc);
	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;
	}