410 lines
10 KiB
C
410 lines
10 KiB
C
|
/*******************************************************************************
|
||
|
|
* 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 <espressif/esp_common.h>
|
||
|
|
#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;
|
||
|
|
}
|
||
|
|
|