mirror of
https://github.com/eclipse/paho.mqtt-sn.embedded-c.git
synced 2025-12-13 15:36:51 +01:00
First Commit of MQTT-SN Gateway
Add new SensorNetwork XBee Add a sensor network type in a start message. Update MQTTSNClient to avoid build warning. Update WiringPi's functions to my original ones. BugFix check msgId before adding waitdTopicId table. BugFix Process termination procedures Update print curent time in millseconds. update move currentDateTime() to linux directory. Bugfix: blink blue lightiIndicator. Bugfix: Register returns wrong id. change a status of the client to Disconnected. change client status procedure Update README BugFix: change Network Disconnect procedures. Signed-off-by: tomoaki <tomoaki@tomy-tech.com>
This commit is contained in:
tomoaki
530
MQTTSNGateway/src/MQTTGWPacket.cpp
Normal file
530
MQTTSNGateway/src/MQTTGWPacket.cpp
Normal file
@@ -0,0 +1,530 @@
|
||||
/**************************************************************************************
|
||||
* Copyright (c) 2009, 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
|
||||
* Tomoaki Yamaguchi - modify codes for MATT-SN Gateway
|
||||
**************************************************************************************/
|
||||
|
||||
#include "MQTTGWPacket.h"
|
||||
#include <string>
|
||||
|
||||
using namespace MQTTSNGW;
|
||||
|
||||
#define MAX_NO_OF_REMAINING_LENGTH_BYTES 3
|
||||
/**
|
||||
* List of the predefined MQTT v3 packet names.
|
||||
*/
|
||||
static const char* mqtt_packet_names[] =
|
||||
{ "RESERVED", "CONNECT", "CONNACK", "PUBLISH", "PUBACK", "PUBREC", "PUBREL", "PUBCOMP", "SUBSCRIBE", "SUBACK",
|
||||
"UNSUBSCRIBE", "UNSUBACK", "PINGREQ", "PINGRESP", "DISCONNECT" };
|
||||
|
||||
/**
|
||||
* 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(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;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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(char** pptr)
|
||||
{
|
||||
char* ptr = *pptr;
|
||||
int len = 256 * ((unsigned char) (*ptr)) + (unsigned char) (*(ptr + 1));
|
||||
*pptr += 2;
|
||||
return len;
|
||||
}
|
||||
|
||||
/**
|
||||
* Reads a "UTF" string from the input buffer. UTF as in the MQTT v3 spec which really means
|
||||
* a length delimited string. So it reads the two byte length then the data according to
|
||||
* that length. The end of the buffer is provided too, so we can prevent buffer overruns caused
|
||||
* by an incorrect length.
|
||||
* @param pptr pointer to the input buffer - incremented by the number of bytes used & returned
|
||||
* @param enddata pointer to the end of the buffer not to be read beyond
|
||||
* @param len returns the calculcated value of the length bytes read
|
||||
* @return an allocated C string holding the characters read, or NULL if the length read would
|
||||
* have caused an overrun.
|
||||
*
|
||||
*/
|
||||
char* readUTFlen(char** pptr, char* enddata, int* len)
|
||||
{
|
||||
char* string = NULL;
|
||||
|
||||
if (enddata - (*pptr) > 1) /* enough length to read the integer? */
|
||||
{
|
||||
*len = readInt(pptr);
|
||||
if (&(*pptr)[*len] <= enddata)
|
||||
{
|
||||
string = (char*)calloc(*len + 1, 1);
|
||||
memcpy(string, *pptr, (size_t)*len);
|
||||
string[*len] = '\0';
|
||||
*pptr += *len;
|
||||
}
|
||||
}
|
||||
return string;
|
||||
}
|
||||
|
||||
/**
|
||||
* Reads a "UTF" string from the input buffer. UTF as in the MQTT v3 spec which really means
|
||||
* a length delimited string. So it reads the two byte length then the data according to
|
||||
* that length. The end of the buffer is provided too, so we can prevent buffer overruns caused
|
||||
* by an incorrect length.
|
||||
* @param pptr pointer to the input buffer - incremented by the number of bytes used & returned
|
||||
* @param enddata pointer to the end of the buffer not to be read beyond
|
||||
* @return an allocated C string holding the characters read, or NULL if the length read would
|
||||
* have caused an overrun.
|
||||
*/
|
||||
char* readUTF(char** pptr, char* enddata)
|
||||
{
|
||||
int len;
|
||||
return readUTFlen(pptr, enddata, &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
|
||||
*/
|
||||
unsigned char readChar(char** pptr)
|
||||
{
|
||||
unsigned 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 writeUTF(unsigned char** pptr, const char* string)
|
||||
{
|
||||
int len = (int)strlen(string);
|
||||
writeInt(pptr, len);
|
||||
memcpy(*pptr, string, (size_t)len);
|
||||
*pptr += len;
|
||||
}
|
||||
|
||||
/**
|
||||
* Lapper class of MQTTPacket
|
||||
*
|
||||
*/
|
||||
MQTTGWPacket::MQTTGWPacket()
|
||||
{
|
||||
_data = 0;
|
||||
_header.byte = 0;
|
||||
_remainingLength = 0;
|
||||
}
|
||||
|
||||
MQTTGWPacket::~MQTTGWPacket()
|
||||
{
|
||||
if (_data)
|
||||
{
|
||||
free(_data);
|
||||
}
|
||||
}
|
||||
|
||||
int MQTTGWPacket::recv(Network* network)
|
||||
{
|
||||
int len = 0;
|
||||
int multiplier = 1;
|
||||
unsigned char c;
|
||||
|
||||
/* read First Byte of Packet */
|
||||
if (network->recv((unsigned char*)&_header.byte, 1) == -1)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
/* read RemainingLength */
|
||||
do
|
||||
{
|
||||
if (++len > MAX_NO_OF_REMAINING_LENGTH_BYTES)
|
||||
{
|
||||
return -2;
|
||||
}
|
||||
if (network->recv(&c, 1) == -1)
|
||||
{
|
||||
return -2;
|
||||
}
|
||||
_remainingLength += (c & 127) * multiplier;
|
||||
multiplier *= 128;
|
||||
} while ((c & 128) != 0);
|
||||
|
||||
/* allocate buffer */
|
||||
_data = (unsigned char*)calloc(_remainingLength, 1);
|
||||
if ( !_data )
|
||||
{
|
||||
return -3;
|
||||
}
|
||||
|
||||
/* read Payload */
|
||||
int remlen = network->recv(_data, _remainingLength);
|
||||
|
||||
if (remlen == -1 || remlen != _remainingLength )
|
||||
{
|
||||
return -2;
|
||||
}
|
||||
return 1 + len + _remainingLength;
|
||||
}
|
||||
|
||||
int MQTTGWPacket::send(Network* network)
|
||||
{
|
||||
unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
|
||||
memset(buf, 0, MQTTSNGW_MAX_PACKET_SIZE);
|
||||
int len = getPacketData(buf);
|
||||
return network->send(buf, len);
|
||||
|
||||
}
|
||||
|
||||
int MQTTGWPacket::getAck(Ack* ack)
|
||||
{
|
||||
if (PUBACK != _header.bits.type && PUBREC != _header.bits.type && PUBREL != _header.bits.type
|
||||
&& PUBCOMP != _header.bits.type && UNSUBACK != _header.bits.type)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
char* ptr = (char*) _data;
|
||||
ack->header.byte = _header.byte;
|
||||
ack->msgId = readInt((char**) &ptr);
|
||||
return 1;
|
||||
}
|
||||
|
||||
int MQTTGWPacket::getCONNACK(Connack* resp)
|
||||
{
|
||||
if (_header.bits.type != CONNACK)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
char* ptr = (char*) _data;
|
||||
resp->header.byte = _header.byte;
|
||||
resp->flags.all = *ptr++;
|
||||
resp->rc = readChar(&ptr);
|
||||
return 1;
|
||||
}
|
||||
|
||||
int MQTTGWPacket::getSUBACK(unsigned short* msgId, unsigned char* rc)
|
||||
{
|
||||
if (_header.bits.type != SUBACK)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
char *ptr = (char*) _data;
|
||||
*msgId = readInt((char**) &ptr);
|
||||
*rc = readChar(&ptr);
|
||||
return 1;
|
||||
}
|
||||
|
||||
int MQTTGWPacket::getPUBLISH(Publish* pub)
|
||||
{
|
||||
if (_header.bits.type != PUBLISH)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
char* ptr = (char*) _data;
|
||||
pub->header = _header;
|
||||
pub->topiclen = readInt((char**) &ptr);
|
||||
pub->topic = (char*) _data + 2;
|
||||
ptr += pub->topiclen;
|
||||
pub->msgId = readInt(&ptr);
|
||||
pub->payload = ptr;
|
||||
pub->payloadlen = _remainingLength - pub->topiclen - 4;
|
||||
return 1;
|
||||
}
|
||||
|
||||
int MQTTGWPacket::setCONNECT(Connect* connect, unsigned char* username, unsigned char* password)
|
||||
{
|
||||
clearData();
|
||||
_header = connect->header;
|
||||
|
||||
_remainingLength = ((connect->version == 3) ? 12 : 10) + (int)strlen(connect->clientID) + 2;
|
||||
if (connect->flags.bits.will)
|
||||
{
|
||||
_remainingLength += (int)strlen(connect->willTopic) + 2 + (int)strlen(connect->willMsg) + 2;
|
||||
}
|
||||
if ( connect->flags.bits.username )
|
||||
{
|
||||
_remainingLength += (int)strlen((char*) username) + 2;
|
||||
}
|
||||
if (connect->flags.bits.password)
|
||||
{
|
||||
_remainingLength += (int)strlen((char*) password) + 2;
|
||||
}
|
||||
|
||||
_data = (unsigned char*)calloc(_remainingLength, 1);
|
||||
unsigned char* ptr = _data;
|
||||
|
||||
if (connect->version == 3)
|
||||
{
|
||||
writeUTF(&ptr, "MQIsdp");
|
||||
writeChar(&ptr, (char) 3);
|
||||
}
|
||||
else if (connect->version == 4)
|
||||
{
|
||||
writeUTF(&ptr, "MQTT");
|
||||
writeChar(&ptr, (char) 4);
|
||||
}
|
||||
else
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
writeChar(&ptr, connect->flags.all);
|
||||
writeInt(&ptr, connect->keepAliveTimer);
|
||||
writeUTF(&ptr, connect->clientID);
|
||||
if (connect->flags.bits.will)
|
||||
{
|
||||
writeUTF(&ptr, connect->willTopic);
|
||||
writeUTF(&ptr, connect->willMsg);
|
||||
}
|
||||
|
||||
if (connect->flags.bits.username)
|
||||
{
|
||||
writeUTF(&ptr, (const char*) username);
|
||||
}
|
||||
if (connect->flags.bits.password)
|
||||
{
|
||||
writeUTF(&ptr, (const char*) password);
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
int MQTTGWPacket::setSUBSCRIBE(const char* topic, unsigned char qos, unsigned short msgId)
|
||||
{
|
||||
clearData();
|
||||
_header.byte = 0;
|
||||
_header.bits.type = SUBSCRIBE;
|
||||
_header.bits.qos = 1; // Reserved
|
||||
_remainingLength = (int)strlen(topic) + 5;
|
||||
_data = (unsigned char*)calloc(_remainingLength, 1);
|
||||
if (_data)
|
||||
{
|
||||
unsigned char* ptr = _data;
|
||||
writeInt(&ptr, msgId);
|
||||
writeUTF(&ptr, topic);
|
||||
writeChar(&ptr, (char) qos);
|
||||
return 1;
|
||||
}
|
||||
clearData();
|
||||
return 0;
|
||||
}
|
||||
|
||||
int MQTTGWPacket::setUNSUBSCRIBE(const char* topic, unsigned short msgid)
|
||||
{
|
||||
clearData();
|
||||
_header.byte = 0;
|
||||
_header.bits.type = UNSUBSCRIBE;
|
||||
_header.bits.qos = 1;
|
||||
_remainingLength = (int)strlen(topic) + 4;
|
||||
_data = (unsigned char*)calloc(_remainingLength, 1);
|
||||
if (_data)
|
||||
{
|
||||
unsigned char* ptr = _data;
|
||||
writeInt(&ptr, msgid);
|
||||
writeUTF(&ptr, topic);
|
||||
return 1;
|
||||
}
|
||||
clearData();
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
int MQTTGWPacket::setPUBLISH(Publish* pub)
|
||||
{
|
||||
clearData();
|
||||
_header.byte = pub->header.byte;
|
||||
_header.bits.type = PUBLISH;
|
||||
_remainingLength = 4 + pub->topiclen + pub->payloadlen;
|
||||
_data = (unsigned char*)calloc(_remainingLength, 1);
|
||||
if (_data)
|
||||
{
|
||||
unsigned char* ptr = _data;
|
||||
writeInt(&ptr, pub->topiclen);
|
||||
memcpy(ptr, pub->topic, pub->topiclen);
|
||||
ptr += pub->topiclen;
|
||||
writeInt(&ptr, pub->msgId);
|
||||
memcpy(ptr, pub->payload, pub->payloadlen);
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
clearData();
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
int MQTTGWPacket::setAck(unsigned char msgType, unsigned short msgid)
|
||||
{
|
||||
clearData();
|
||||
_remainingLength = 2;
|
||||
_header.bits.type = msgType;
|
||||
_header.bits.qos = (msgType == PUBREL) ? 1 : 0;
|
||||
|
||||
_data = (unsigned char*)calloc(_remainingLength, 1);
|
||||
if (_data)
|
||||
{
|
||||
unsigned char* data = _data;
|
||||
writeInt(&data, msgid);
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int MQTTGWPacket::setHeader(unsigned char msgType)
|
||||
{
|
||||
clearData();
|
||||
if (msgType < CONNECT || msgType > DISCONNECT)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
_header.bits.type = msgType;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int MQTTGWPacket::getType(void)
|
||||
{
|
||||
return _header.bits.type;
|
||||
}
|
||||
|
||||
const char* MQTTGWPacket::getName(void)
|
||||
{
|
||||
return getType() > DISCONNECT ? "UNKNOWN" : mqtt_packet_names[getType()];
|
||||
}
|
||||
|
||||
int MQTTGWPacket::getPacketData(unsigned char* buf)
|
||||
{
|
||||
unsigned char* ptr = buf;
|
||||
*ptr++ = _header.byte;
|
||||
int len = MQTTPacket_encode((char*)ptr, _remainingLength);
|
||||
ptr += len;
|
||||
memcpy(ptr, _data, _remainingLength);
|
||||
return 1 + len + _remainingLength;
|
||||
}
|
||||
|
||||
int MQTTGWPacket::getPacketLength(void)
|
||||
{
|
||||
char buf[4];
|
||||
return 1 + MQTTPacket_encode(buf, _remainingLength) + _remainingLength;
|
||||
}
|
||||
|
||||
void MQTTGWPacket::clearData(void)
|
||||
{
|
||||
if (_data)
|
||||
{
|
||||
free(_data);
|
||||
}
|
||||
_header.byte = 0;
|
||||
_remainingLength = 0;
|
||||
}
|
||||
|
||||
char* MQTTGWPacket::getMsgId(char* pbuf)
|
||||
{
|
||||
int type = getType();
|
||||
|
||||
switch ( type )
|
||||
{
|
||||
case PUBLISH:
|
||||
Publish pub;
|
||||
getPUBLISH(&pub);
|
||||
if ( _header.bits.dup )
|
||||
{
|
||||
sprintf(pbuf, "+%04X", pub.msgId);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(pbuf, " %04X", pub.msgId);
|
||||
}
|
||||
break;
|
||||
case PUBACK:
|
||||
case PUBREC:
|
||||
case PUBREL:
|
||||
case PUBCOMP:
|
||||
case SUBSCRIBE:
|
||||
case UNSUBSCRIBE:
|
||||
case SUBACK:
|
||||
case UNSUBACK:
|
||||
sprintf(pbuf, " %02X%02X", _data[0], _data[1]);
|
||||
break;
|
||||
default:
|
||||
sprintf(pbuf, " ");
|
||||
break;
|
||||
}
|
||||
return pbuf;
|
||||
}
|
||||
|
||||
char* MQTTGWPacket::print(char* pbuf)
|
||||
{
|
||||
char* ptr = pbuf;
|
||||
char** pptr = &pbuf;
|
||||
char digit[4];
|
||||
|
||||
sprintf(*pptr, " %02X",(const unsigned char)_header.byte);
|
||||
*pptr += 3;
|
||||
int len = MQTTPacket_encode((char*) digit, _remainingLength);
|
||||
for (int i = 0; i < len; i++)
|
||||
{
|
||||
sprintf(*pptr, " %02X", digit[i]);
|
||||
*pptr += 3;
|
||||
}
|
||||
|
||||
int size = _remainingLength > SIZEOF_LOG_PACKET ? SIZEOF_LOG_PACKET : _remainingLength;
|
||||
for (int i = 0; i < size; i++)
|
||||
{
|
||||
sprintf(*pptr, " %02X", *(_data + i));
|
||||
*pptr += 3;
|
||||
}
|
||||
**pptr = 0;
|
||||
return ptr;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user