paho.mqtt-sn.embedded-c/MQTTSNGateway/src/MQTTSNGWEncapsulatedPacket.cpp

/**************************************************************************************
 * Copyright (c) 2018, 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 "MQTTSNGWPacket.h"
#include "MQTTSNGWEncapsulatedPacket.h"
#include "MQTTSNPacket.h"
#include <string.h>

using namespace MQTTSNGW;
using namespace std;

WirelessNodeId::WirelessNodeId() :
        _len { 0 }, _nodeId { 0 }
{

}

WirelessNodeId::~WirelessNodeId()
{
    if (_nodeId)
    {
        free(_nodeId);
    }
}

void WirelessNodeId::setId(uint8_t* id, uint8_t len)
{
    if (_nodeId)
    {
        free(_nodeId);
    }
    uint8_t* buf = (uint8_t*) malloc(len);
    if (buf)
    {
        memcpy(buf, id, len);
        _len = len;
        _nodeId = buf;
    }
    else
    {
        _nodeId = nullptr;
        _len = 0;
    }
}

void WirelessNodeId::setId(WirelessNodeId* id)
{
    setId(id->_nodeId, id->_len);
}

bool WirelessNodeId::operator ==(WirelessNodeId& id)
{
    if (_len == id._len)
    {
        return memcmp(_nodeId, id._nodeId, _len) == 0;
    }
    else
    {
        return false;
    }
}

/*
 *    Class MQTTSNGWEncapsulatedPacket
 */
MQTTSNGWEncapsulatedPacket::MQTTSNGWEncapsulatedPacket() :
        _mqttsn { 0 }, _ctrl { 0 }
{

}

MQTTSNGWEncapsulatedPacket::MQTTSNGWEncapsulatedPacket(MQTTSNPacket* packet) :
        _mqttsn { packet }, _ctrl { 0 }
{

}

MQTTSNGWEncapsulatedPacket::~MQTTSNGWEncapsulatedPacket()
{
    /*  Do not delete the MQTTSNPacket.  MQTTSNPacket is deleted by delete Event */
}

int MQTTSNGWEncapsulatedPacket::unicast(SensorNetwork* network, SensorNetAddress* sendTo)
{
    uint8_t buf[MQTTSNGW_MAX_PACKET_SIZE];
    int len = serialize(buf);
    return network->unicast(buf, len, sendTo);
}

int MQTTSNGWEncapsulatedPacket::serialize(uint8_t* buf)
{
    int len = 0;
    buf[0] = _id._len + 3;
    buf[1] = MQTTSN_ENCAPSULATED;
    buf[2] = _ctrl;
    memcpy(buf + 3, _id._nodeId, _id._len);
    if (_mqttsn)
    {
        len = _mqttsn->getPacketLength();
        memcpy(buf + buf[0], _mqttsn->getPacketData(), len);
    }
    return buf[0] + len;
}

int MQTTSNGWEncapsulatedPacket::desirialize(unsigned char* buf, unsigned short len)
{
    if (_mqttsn)
    {
        delete _mqttsn;
        _mqttsn = nullptr;
    }

    _ctrl = buf[2];
    _id.setId(buf + 3, buf[0] - 3);

    _mqttsn = new MQTTSNPacket;
    _mqttsn->desirialize(buf + buf[0], len - buf[0]);
    return buf[0];
}

int MQTTSNGWEncapsulatedPacket::getType(void)
{
    return MQTTSN_ENCAPSULATED;
}

const char* MQTTSNGWEncapsulatedPacket::getName()
{
    return MQTTSNPacket_name(MQTTSN_ENCAPSULATED);
}

MQTTSNPacket* MQTTSNGWEncapsulatedPacket::getMQTTSNPacket(void)
{
    return _mqttsn;
}

WirelessNodeId* MQTTSNGWEncapsulatedPacket::getWirelessNodeId(void)
{
    return &_id;
}

void MQTTSNGWEncapsulatedPacket::setWirelessNodeId(WirelessNodeId* id)
{
    _id.setId(id);
}

char* MQTTSNGWEncapsulatedPacket::print(char* pbuf)
{
    char* ptr = pbuf;
    char** pptr = &pbuf;

    uint8_t buf[MQTTSNGW_MAX_PACKET_SIZE];
    int len = serialize(buf);
    int size = len > SIZE_OF_LOG_PACKET ? SIZE_OF_LOG_PACKET : len;

    for (int i = 1; i < size; i++)
    {
        sprintf(*pptr, " %02X", *(buf + i));
        *pptr += 3;
    }
    **pptr = 0;
    return ptr;
}