Files
paho.mqtt-sn.embedded-c/MQTTSNGateway/src/MQTTSNGWPacket.cpp
tomoaki 5fb4312aad This branch for debugging DTLS #90, #150, #195, #227
The purpose of this branch is to share work in process.
Change sellect() of UDP to poll()
Rewrite UDP6 for DTLS6

Known bug: can't reconnect DTLS

Signed-off-by: tomoaki <tomoaki@tomy-tech.com>
2021-08-01 19:45:58 +09:00

577 lines
15 KiB
C++

/**************************************************************************************
* Copyright (c) 2016, Tomoaki Yamaguchi
*
* 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:
* Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
**************************************************************************************/
#include "MQTTSNGateway.h"
#include "MQTTSNGWPacket.h"
#include "MQTTSNPacket.h"
#include "SensorNetwork.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
using namespace std;
using namespace MQTTSNGW;
int readInt(char** pptr);
void writeInt(unsigned char** pptr, int msgId);
MQTTSNPacket::MQTTSNPacket(void)
{
_buf = nullptr;
_bufLen = 0;
}
MQTTSNPacket::MQTTSNPacket(MQTTSNPacket& packet)
{
_buf = (unsigned char*) malloc(packet._bufLen);
if (_buf)
{
_bufLen = packet._bufLen;
memcpy(_buf, packet._buf, _bufLen);
}
else
{
_buf = nullptr;
_bufLen = 0;
}
}
MQTTSNPacket::~MQTTSNPacket()
{
if (_buf)
{
free(_buf);
}
}
int MQTTSNPacket::unicast(SensorNetwork* network, SensorNetAddress* sendTo)
{
return network->unicast(_buf, _bufLen, sendTo);
}
int MQTTSNPacket::broadcast(SensorNetwork* network)
{
return network->broadcast(_buf, _bufLen);
}
int MQTTSNPacket::serialize(uint8_t* buf)
{
buf = _buf;
return _bufLen;
}
int MQTTSNPacket::desirialize(unsigned char* buf, unsigned short len)
{
if (_buf)
{
free(_buf);
}
_buf = (unsigned char*) calloc(len, sizeof(unsigned char));
if (_buf)
{
memcpy(_buf, buf, len);
_bufLen = len;
}
else
{
_bufLen = 0;
}
return _bufLen;
}
int MQTTSNPacket::recv(SensorNetwork* network)
{
uint8_t buf[MQTTSNGW_MAX_PACKET_SIZE];
int len = network->read((uint8_t*) buf, MQTTSNGW_MAX_PACKET_SIZE);
if (len > 1)
{
len = desirialize(buf, len);
}
return len;
}
int MQTTSNPacket::getType(void)
{
if (_bufLen == 0)
{
return 0;
}
int value = 0;
int p = MQTTSNPacket_decode(_buf, _bufLen, &value);
return _buf[p];
}
bool MQTTSNPacket::isQoSMinusPUBLISH(void)
{
if (_bufLen == 0)
{
return false;;
}
int value = 0;
int p = MQTTSNPacket_decode(_buf, _bufLen, &value);
return ((_buf[p] == MQTTSN_PUBLISH) && ((_buf[p + 1] & 0x60) == 0x60));
}
unsigned char* MQTTSNPacket::getPacketData(void)
{
return _buf;
}
int MQTTSNPacket::getPacketLength(void)
{
return _bufLen;
}
const char* MQTTSNPacket::getName()
{
return MQTTSNPacket_name(getType());
}
int MQTTSNPacket::setADVERTISE(uint8_t gatewayid, uint16_t duration)
{
unsigned char buf[5];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_advertise(buf, buflen, (unsigned char) gatewayid, (unsigned short) duration);
return desirialize(buf, len);
}
int MQTTSNPacket::setGWINFO(uint8_t gatewayId)
{
unsigned char buf[3];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_gwinfo(buf, buflen, (unsigned char) gatewayId, 0, 0);
return desirialize(buf, len);
}
int MQTTSNPacket::setConnect(void)
{
unsigned char buf[40];
int buflen = sizeof(buf);
MQTTSNPacket_connectData data;
data.clientID.cstring = (char*) "client01";
int len = MQTTSNSerialize_connect(buf, buflen, &data);
return desirialize(buf, len);
}
bool MQTTSNPacket::isAccepted(void)
{
return (getType() == MQTTSN_CONNACK) && (_buf[2] == MQTTSN_RC_ACCEPTED);
}
int MQTTSNPacket::setCONNACK(uint8_t returnCode)
{
unsigned char buf[3];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_connack(buf, buflen, (int) returnCode);
return desirialize(buf, len);
}
int MQTTSNPacket::setWILLTOPICREQ(void)
{
unsigned char buf[2];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_willtopicreq(buf, buflen);
return desirialize(buf, len);
}
int MQTTSNPacket::setWILLMSGREQ(void)
{
unsigned char buf[2];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_willmsgreq(buf, buflen);
return desirialize(buf, len);
}
int MQTTSNPacket::setREGISTER(uint16_t topicId, uint16_t msgId, MQTTSNString* topicName)
{
unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_register(buf, buflen, (unsigned short) topicId, (unsigned short) msgId, topicName);
return desirialize(buf, len);
}
int MQTTSNPacket::setREGACK(uint16_t topicId, uint16_t msgId, uint8_t returnCode)
{
unsigned char buf[7];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_regack(buf, buflen, (unsigned short) topicId, (unsigned short) msgId, (unsigned char) returnCode);
return desirialize(buf, len);
}
int MQTTSNPacket::setPUBLISH(uint8_t dup, int qos, uint8_t retained, uint16_t msgId, MQTTSN_topicid topic, uint8_t* payload,
uint16_t payloadlen)
{
unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_publish(buf, buflen, (unsigned char) dup, qos, (unsigned char) retained, (unsigned short) msgId,
topic, (unsigned char*) payload, (int) payloadlen);
return desirialize(buf, len);
}
int MQTTSNPacket::setPUBACK(uint16_t topicId, uint16_t msgId, uint8_t returnCode)
{
unsigned char buf[7];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_puback(buf, buflen, (unsigned short) topicId, (unsigned short) msgId, (unsigned char) returnCode);
return desirialize(buf, len);
}
int MQTTSNPacket::setPUBREC(uint16_t msgId)
{
unsigned char buf[4];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_pubrec(buf, buflen, (unsigned short) msgId);
return desirialize(buf, len);
}
int MQTTSNPacket::setPUBREL(uint16_t msgId)
{
unsigned char buf[4];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_pubrel(buf, buflen, (unsigned short) msgId);
return desirialize(buf, len);
}
int MQTTSNPacket::setPUBCOMP(uint16_t msgId)
{
unsigned char buf[4];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_pubcomp(buf, buflen, (unsigned short) msgId);
return desirialize(buf, len);
}
int MQTTSNPacket::setSUBACK(int qos, uint16_t topicId, uint16_t msgId, uint8_t returnCode)
{
unsigned char buf[8];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_suback(buf, buflen, qos, (unsigned short) topicId, (unsigned short) msgId,
(unsigned char) returnCode);
return desirialize(buf, len);
}
int MQTTSNPacket::setUNSUBACK(uint16_t msgId)
{
unsigned char buf[4];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_unsuback(buf, buflen, (unsigned short) msgId);
return desirialize(buf, len);
}
int MQTTSNPacket::setPINGRESP(void)
{
unsigned char buf[32];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_pingresp(buf, buflen);
return desirialize(buf, len);
}
int MQTTSNPacket::setDISCONNECT(uint16_t duration)
{
unsigned char buf[4];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_disconnect(buf, buflen, (int) duration);
return desirialize(buf, len);
}
int MQTTSNPacket::setWILLTOPICRESP(uint8_t returnCode)
{
unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_willtopicresp(buf, buflen, (int) returnCode);
return desirialize(buf, len);
}
int MQTTSNPacket::setWILLMSGRESP(uint8_t returnCode)
{
unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_willmsgresp(buf, buflen, (int) returnCode);
return desirialize(buf, len);
}
int MQTTSNPacket::setCONNECT(MQTTSNPacket_connectData* options)
{
unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_connect(buf, buflen, options);
return desirialize(buf, len);
}
int MQTTSNPacket::setPINGREQ(MQTTSNString* clientId)
{
unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
int buflen = sizeof(buf);
int len = MQTTSNSerialize_pingreq(buf, buflen, *clientId);
return desirialize(buf, len);
}
int MQTTSNPacket::getSERCHGW(uint8_t* radius)
{
return MQTTSNDeserialize_searchgw((unsigned char*) radius, (unsigned char*) _buf, _bufLen);
}
int MQTTSNPacket::getCONNECT(MQTTSNPacket_connectData* data)
{
return MQTTSNDeserialize_connect(data, _buf, _bufLen);
}
int MQTTSNPacket::getCONNACK(uint8_t* returnCode)
{
return MQTTSNSerialize_connack(_buf, _bufLen, (int) *returnCode);
}
int MQTTSNPacket::getWILLTOPIC(int* willQoS, uint8_t* willRetain, MQTTSNString* willTopic)
{
return MQTTSNDeserialize_willtopic((int*) willQoS, (unsigned char*) willRetain, willTopic, _buf, _bufLen);
}
int MQTTSNPacket::getWILLMSG(MQTTSNString* willmsg)
{
return MQTTSNDeserialize_willmsg(willmsg, _buf, _bufLen);
}
int MQTTSNPacket::getREGISTER(uint16_t* topicId, uint16_t* msgId, MQTTSNString* topicName)
{
return MQTTSNDeserialize_register((unsigned short*) topicId, (unsigned short*) msgId, topicName, _buf, _bufLen);
}
int MQTTSNPacket::getREGACK(uint16_t* topicId, uint16_t* msgId, uint8_t* returnCode)
{
return MQTTSNDeserialize_regack((unsigned short*) topicId, (unsigned short*) msgId, (unsigned char*) returnCode, _buf,
_bufLen);
}
int MQTTSNPacket::getPUBLISH(uint8_t* dup, int* qos, uint8_t* retained, uint16_t* msgId, MQTTSN_topicid* topic,
uint8_t** payload, int* payloadlen)
{
return MQTTSNDeserialize_publish((unsigned char*) dup, qos, (unsigned char*) retained, (unsigned short*) msgId, topic,
(unsigned char**) payload, (int*) payloadlen, _buf, _bufLen);
}
int MQTTSNPacket::getPUBACK(uint16_t* topicId, uint16_t* msgId, uint8_t* returnCode)
{
return MQTTSNDeserialize_puback((unsigned short*) topicId, (unsigned short*) msgId, (unsigned char*) returnCode, _buf,
_bufLen);
}
int MQTTSNPacket::getACK(uint16_t* msgId)
{
unsigned char type;
return MQTTSNDeserialize_ack(&type, (unsigned short*) msgId, _buf, _bufLen);
}
int MQTTSNPacket::getSUBSCRIBE(uint8_t* dup, int* qos, uint16_t* msgId, MQTTSN_topicid* topicFilter)
{
return MQTTSNDeserialize_subscribe((unsigned char*) dup, qos, (unsigned short*) msgId, topicFilter, _buf, _bufLen);
}
int MQTTSNPacket::getUNSUBSCRIBE(uint16_t* msgId, MQTTSN_topicid* topicFilter)
{
return MQTTSNDeserialize_unsubscribe((unsigned short*) msgId, topicFilter, _buf, _bufLen);
}
int MQTTSNPacket::getPINGREQ(void)
{
if (getType() == MQTTSN_PINGRESP && _bufLen > 2)
{
return _bufLen - 2;
}
return 0;
}
int MQTTSNPacket::getDISCONNECT(uint16_t* duration)
{
int dur = 0;
int rc = MQTTSNDeserialize_disconnect(&dur, _buf, _bufLen);
*duration = (uint16_t) dur;
return rc;
}
int MQTTSNPacket::getWILLTOPICUPD(uint8_t* willQoS, uint8_t* willRetain, MQTTSNString* willTopic)
{
return MQTTSNDeserialize_willtopicupd((int*) willQoS, (unsigned char*) willRetain, willTopic, _buf, _bufLen);
}
int MQTTSNPacket::getWILLMSGUPD(MQTTSNString* willMsg)
{
return MQTTSNDeserialize_willmsgupd(willMsg, _buf, _bufLen);
}
char* MQTTSNPacket::print(char* pbuf)
{
char* ptr = pbuf;
char** pptr = &pbuf;
int size = _bufLen > SIZE_OF_LOG_PACKET ? SIZE_OF_LOG_PACKET : _bufLen;
for (int i = 0; i < size; i++)
{
sprintf(*pptr, " %02X", *(_buf + i));
*pptr += 3;
}
**pptr = 0;
return ptr;
}
char* MQTTSNPacket::getMsgId(char* pbuf)
{
int value = 0;
int p = 0;
switch (getType())
{
case MQTTSN_PUBLISH:
p = MQTTSNPacket_decode(_buf, _bufLen, &value);
if (_buf[p + 1] & 0x80)
{
sprintf(pbuf, "+%02X%02X", _buf[p + 4], _buf[p + 5]);
}
else
{
sprintf(pbuf, " %02X%02X", _buf[p + 4], _buf[p + 5]);
}
break;
case MQTTSN_PUBACK:
case MQTTSN_REGISTER:
case MQTTSN_REGACK:
sprintf(pbuf, " %02X%02X", _buf[4], _buf[5]);
break;
case MQTTSN_PUBREC:
case MQTTSN_PUBREL:
case MQTTSN_PUBCOMP:
case MQTTSN_UNSUBACK:
sprintf(pbuf, " %02X%02X", _buf[2], _buf[3]);
break;
case MQTTSN_SUBSCRIBE:
case MQTTSN_UNSUBSCRIBE:
p = MQTTSNPacket_decode(_buf, _bufLen, &value);
sprintf(pbuf, " %02X%02X", _buf[p + 2], _buf[p + 3]);
break;
case MQTTSN_SUBACK:
sprintf(pbuf, " %02X%02X", _buf[5], _buf[6]);
break;
default:
sprintf(pbuf, " ");
break;
}
if (strcmp(pbuf, " 0000") == 0)
{
sprintf(pbuf, " ");
}
return pbuf;
}
int MQTTSNPacket::getMsgId(void)
{
int value = 0;
int p = 0;
int msgId = 0;
char* ptr = 0;
switch (getType())
{
case MQTTSN_PUBLISH:
p = MQTTSNPacket_decode(_buf, _bufLen, &value);
ptr = (char*) _buf + p + 4;
msgId = readInt((char**) &ptr);
break;
case MQTTSN_PUBACK:
case MQTTSN_REGISTER:
case MQTTSN_REGACK:
ptr = (char*) _buf + 4;
msgId = readInt((char**) &ptr);
break;
case MQTTSN_PUBREC:
case MQTTSN_PUBREL:
case MQTTSN_PUBCOMP:
case MQTTSN_UNSUBACK:
ptr = (char*) _buf + 2;
msgId = readInt((char**) &ptr);
break;
case MQTTSN_SUBSCRIBE:
case MQTTSN_UNSUBSCRIBE:
p = MQTTSNPacket_decode(_buf, _bufLen, &value);
ptr = (char*) _buf + p + 2;
msgId = readInt((char**) &ptr);
break;
case MQTTSN_SUBACK:
ptr = (char*) _buf + 5;
msgId = readInt((char**) &ptr);
break;
default:
break;
}
return msgId;
}
void MQTTSNPacket::setMsgId(uint16_t msgId)
{
int value = 0;
int p = 0;
//unsigned char* ptr = 0;
switch (getType())
{
case MQTTSN_PUBLISH:
p = MQTTSNPacket_decode(_buf, _bufLen, &value);
_buf[p + 4] = (unsigned char) (msgId / 256);
_buf[p + 5] = (unsigned char) (msgId % 256);
//ptr = _buf + p + 4;
//writeInt(&ptr, msgId);
break;
case MQTTSN_PUBACK:
case MQTTSN_REGISTER:
case MQTTSN_REGACK:
_buf[4] = (unsigned char) (msgId / 256);
_buf[5] = (unsigned char) (msgId % 256);
//ptr = _buf + 4;
//writeInt(&ptr, msgId);
break;
case MQTTSN_PUBREC:
case MQTTSN_PUBREL:
case MQTTSN_PUBCOMP:
case MQTTSN_UNSUBACK:
_buf[2] = (unsigned char) (msgId / 256);
_buf[3] = (unsigned char) (msgId % 256);
//ptr = _buf + 2;
//writeInt(&ptr, msgId);
break;
case MQTTSN_SUBSCRIBE:
case MQTTSN_UNSUBSCRIBE:
p = MQTTSNPacket_decode(_buf, _bufLen, &value);
_buf[p + 2] = (unsigned char) (msgId / 256);
_buf[p + 3] = (unsigned char) (msgId % 256);
//ptr = _buf + p + 2;
//writeInt(&ptr, msgId);
break;
case MQTTSN_SUBACK:
_buf[5] = (unsigned char) (msgId / 256);
_buf[6] = (unsigned char) (msgId % 256);
//ptr = _buf + 5;
//writeInt(&ptr, msgId);
break;
default:
break;
}
}
bool MQTTSNPacket::isDuplicate(void)
{
int value = 0;
int p = MQTTSNPacket_decode(_buf, _bufLen, &value);
return (_buf[p + 1] & 0x80);
}