MQTTSNGWPacket.cpp

00001 /**************************************************************************************
00002  * Copyright (c) 2016, Tomoaki Yamaguchi
00003  *
00004  * All rights reserved. This program and the accompanying materials
00005  * are made available under the terms of the Eclipse Public License v1.0
00006  * and Eclipse Distribution License v1.0 which accompany this distribution.
00007  *
00008  * The Eclipse Public License is available at
00009  *    http://www.eclipse.org/legal/epl-v10.html
00010  * and the Eclipse Distribution License is available at
00011  *   http://www.eclipse.org/org/documents/edl-v10.php.
00012  *
00013  * Contributors:
00014  *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
00015  **************************************************************************************/
00016 
00017 #include "MQTTSNGateway.h"
00018 #include "MQTTSNGWPacket.h"
00019 #include "MQTTSNPacket.h"
00020 #include "SensorNetwork.h"
00021 #include <stdio.h>
00022 #include <stdlib.h>
00023 #include <string.h>
00024 
00025 using namespace std;
00026 using namespace MQTTSNGW;
00027 int readInt(char** pptr);
00028 void writeInt(unsigned char** pptr, int msgId);
00029 
00030 MQTTSNPacket::MQTTSNPacket(void)
00031 {
00032     _buf = nullptr;
00033     _bufLen = 0;
00034 }
00035 
00036 MQTTSNPacket::MQTTSNPacket(MQTTSNPacket& packet)
00037 {
00038     _buf = (unsigned char*)malloc(packet._bufLen);
00039     if (_buf)
00040     {
00041         _bufLen = packet._bufLen;
00042         memcpy(_buf, packet._buf, _bufLen);
00043     }
00044     else
00045     {
00046         _buf = nullptr;
00047         _bufLen = 0;
00048     }
00049 }
00050 
00051 MQTTSNPacket::~MQTTSNPacket()
00052 {
00053     if (_buf)
00054     {
00055         free(_buf);
00056     }
00057 }
00058 
00059 int MQTTSNPacket::unicast(SensorNetwork* network, SensorNetAddress* sendTo)
00060 {
00061     return network->unicast(_buf, _bufLen, sendTo);
00062 }
00063 
00064 int MQTTSNPacket::broadcast(SensorNetwork* network)
00065 {
00066     return network->broadcast(_buf, _bufLen);
00067 }
00068 
00069 int MQTTSNPacket::serialize(uint8_t* buf)
00070 {
00071     buf = _buf;
00072     return _bufLen;
00073 }
00074 
00075 int MQTTSNPacket::desirialize(unsigned char* buf, unsigned short len)
00076 {
00077     if ( _buf )
00078     {
00079         free(_buf);
00080     }
00081 
00082     _buf = (unsigned char*)calloc(len, sizeof(unsigned char));
00083     if ( _buf )
00084     {
00085         memcpy(_buf, buf, len);
00086         _bufLen = len;
00087     }
00088     else
00089     {
00090         _bufLen = 0;
00091     }
00092     return _bufLen;
00093 }
00094 
00095 int MQTTSNPacket::recv(SensorNetwork* network)
00096 {
00097     uint8_t buf[MQTTSNGW_MAX_PACKET_SIZE];
00098     int len = network->read((uint8_t*) buf, MQTTSNGW_MAX_PACKET_SIZE);
00099     if (len > 1)
00100     {
00101         len = desirialize(buf, len);
00102     }
00103     else
00104     {
00105         len = 0;
00106     }
00107     return len;
00108 
00109 }
00110 
00111 int MQTTSNPacket::getType(void)
00112 {
00113     if ( _bufLen == 0 )
00114     {
00115         return 0;
00116     }
00117     int value = 0;
00118     int p = MQTTSNPacket_decode(_buf, _bufLen, &value);
00119     return _buf[p];
00120 }
00121 
00122 bool MQTTSNPacket::isQoSMinusPUBLISH(void)
00123 {
00124     if ( _bufLen == 0 )
00125     {
00126         return false;;
00127     }
00128     int value = 0;
00129     int p = MQTTSNPacket_decode(_buf, _bufLen, &value);
00130     return (  (_buf[p] == MQTTSN_PUBLISH) && ((_buf[p + 1] & 0x60 ) == 0x60 ));
00131 }
00132 
00133 unsigned char* MQTTSNPacket::getPacketData(void)
00134 {
00135     return _buf;
00136 }
00137 
00138 int MQTTSNPacket::getPacketLength(void)
00139 {
00140     return _bufLen;
00141 }
00142 
00143 const char* MQTTSNPacket::getName()
00144 {
00145     return MQTTSNPacket_name(getType());
00146 }
00147 
00148 int MQTTSNPacket::setADVERTISE(uint8_t gatewayid, uint16_t duration)
00149 {
00150     unsigned char buf[5];
00151     int buflen = sizeof(buf);
00152     int len = MQTTSNSerialize_advertise(buf, buflen, (unsigned char) gatewayid,
00153             (unsigned short) duration);
00154     return desirialize(buf, len);
00155 }
00156 
00157 int MQTTSNPacket::setGWINFO(uint8_t gatewayId)
00158 {
00159     unsigned char buf[3];
00160     int buflen = sizeof(buf);
00161     int len = MQTTSNSerialize_gwinfo(buf, buflen, (unsigned char) gatewayId, 0, 0);
00162     return desirialize(buf, len);
00163 }
00164 
00165 int MQTTSNPacket::setConnect(void)
00166 {
00167     unsigned char buf[40];
00168     int buflen = sizeof(buf);
00169     MQTTSNPacket_connectData data;
00170     data.clientID.cstring = (char*)"client01";
00171     int len = MQTTSNSerialize_connect(buf, buflen, &data);
00172     return desirialize(buf, len);
00173 }
00174 
00175 bool MQTTSNPacket::isAccepted(void)
00176 {
00177     return  ( getType() == MQTTSN_CONNACK)  && (_buf[2] == MQTTSN_RC_ACCEPTED);
00178 }
00179 
00180 int MQTTSNPacket::setCONNACK(uint8_t returnCode)
00181 {
00182     unsigned char buf[3];
00183     int buflen = sizeof(buf);
00184     int len = MQTTSNSerialize_connack(buf, buflen, (int) returnCode);
00185     return desirialize(buf, len);
00186 }
00187 
00188 int MQTTSNPacket::setWILLTOPICREQ(void)
00189 {
00190     unsigned char buf[2];
00191     int buflen = sizeof(buf);
00192     int len = MQTTSNSerialize_willtopicreq(buf, buflen);
00193     return desirialize(buf, len);
00194 }
00195 
00196 int MQTTSNPacket::setWILLMSGREQ(void)
00197 {
00198     unsigned char buf[2];
00199     int buflen = sizeof(buf);
00200     int len = MQTTSNSerialize_willmsgreq(buf, buflen);
00201     return desirialize(buf, len);
00202 }
00203 
00204 int MQTTSNPacket::setREGISTER(uint16_t topicId, uint16_t msgId, MQTTSNString* topicName)
00205 {
00206     unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
00207     int buflen = sizeof(buf);
00208     int len = MQTTSNSerialize_register(buf, buflen, (unsigned short) topicId, (unsigned short) msgId,
00209             topicName);
00210     return desirialize(buf, len);
00211 }
00212 
00213 int MQTTSNPacket::setREGACK(uint16_t topicId, uint16_t msgId, uint8_t returnCode)
00214 {
00215     unsigned char buf[7];
00216     int buflen = sizeof(buf);
00217     int len = MQTTSNSerialize_regack(buf, buflen, (unsigned short) topicId, (unsigned short) msgId,
00218             (unsigned char) returnCode);
00219     return desirialize(buf, len);
00220 }
00221 
00222 int MQTTSNPacket::setPUBLISH(uint8_t dup, int qos, uint8_t retained, uint16_t msgId, MQTTSN_topicid topic,
00223         uint8_t* payload, uint16_t payloadlen)
00224 {
00225     unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
00226     int buflen = sizeof(buf);
00227     int len = MQTTSNSerialize_publish(buf, buflen, (unsigned char) dup, qos, (unsigned char) retained,
00228             (unsigned short) msgId, topic, (unsigned char*) payload, (int) payloadlen);
00229     return desirialize(buf, len);
00230 }
00231 
00232 int MQTTSNPacket::setPUBACK(uint16_t topicId, uint16_t msgId, uint8_t returnCode)
00233 {
00234     unsigned char buf[7];
00235     int buflen = sizeof(buf);
00236     int len = MQTTSNSerialize_puback(buf, buflen, (unsigned short) topicId, (unsigned short) msgId,
00237             (unsigned char) returnCode);
00238     return desirialize(buf, len);
00239 }
00240 
00241 int MQTTSNPacket::setPUBREC(uint16_t msgId)
00242 {
00243     unsigned char buf[4];
00244     int buflen = sizeof(buf);
00245     int len = MQTTSNSerialize_pubrec(buf, buflen, (unsigned short) msgId);
00246     return desirialize(buf, len);
00247 }
00248 
00249 int MQTTSNPacket::setPUBREL(uint16_t msgId)
00250 {
00251     unsigned char buf[4];
00252     int buflen = sizeof(buf);
00253     int len = MQTTSNSerialize_pubrel(buf, buflen, (unsigned short) msgId);
00254     return desirialize(buf, len);
00255 }
00256 
00257 int MQTTSNPacket::setPUBCOMP(uint16_t msgId)
00258 {
00259     unsigned char buf[4];
00260     int buflen = sizeof(buf);
00261     int len = MQTTSNSerialize_pubcomp(buf, buflen, (unsigned short) msgId);
00262     return desirialize(buf, len);
00263 }
00264 
00265 int MQTTSNPacket::setSUBACK(int qos, uint16_t topicId, uint16_t msgId, uint8_t returnCode)
00266 {
00267     unsigned char buf[8];
00268     int buflen = sizeof(buf);
00269     int len = MQTTSNSerialize_suback(buf, buflen, qos, (unsigned short) topicId,
00270             (unsigned short) msgId, (unsigned char) returnCode);
00271     return desirialize(buf, len);
00272 }
00273 
00274 int MQTTSNPacket::setUNSUBACK(uint16_t msgId)
00275 {
00276     unsigned char buf[4];
00277     int buflen = sizeof(buf);
00278     int len = MQTTSNSerialize_unsuback(buf, buflen, (unsigned short) msgId);
00279     return desirialize(buf, len);
00280 }
00281 
00282 int MQTTSNPacket::setPINGRESP(void)
00283 {
00284     unsigned char buf[32];
00285     int buflen = sizeof(buf);
00286     int len = MQTTSNSerialize_pingresp(buf, buflen);
00287     return desirialize(buf, len);
00288 }
00289 
00290 int MQTTSNPacket::setDISCONNECT(uint16_t duration)
00291 {
00292     unsigned char buf[4];
00293     int buflen = sizeof(buf);
00294     int len = MQTTSNSerialize_disconnect(buf, buflen, (int) duration);
00295     return desirialize(buf, len);
00296 }
00297 
00298 int MQTTSNPacket::setWILLTOPICRESP(uint8_t returnCode)
00299 {
00300     unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
00301     int buflen = sizeof(buf);
00302     int len = MQTTSNSerialize_willtopicresp(buf, buflen, (int) returnCode);
00303     return desirialize(buf, len);
00304 }
00305 
00306 int MQTTSNPacket::setWILLMSGRESP(uint8_t returnCode)
00307 {
00308     unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
00309     int buflen = sizeof(buf);
00310     int len = MQTTSNSerialize_willmsgresp(buf, buflen, (int) returnCode);
00311     return desirialize(buf, len);
00312 }
00313 
00314 int MQTTSNPacket::setCONNECT(MQTTSNPacket_connectData* options)
00315 {
00316     unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
00317     int buflen = sizeof(buf);
00318     int len = MQTTSNSerialize_connect(buf, buflen, options);
00319     return desirialize(buf, len);
00320 }
00321 
00322 int MQTTSNPacket::setPINGREQ(MQTTSNString* clientId)
00323 {
00324     unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
00325     int buflen = sizeof(buf);
00326     int len = MQTTSNSerialize_pingreq( buf, buflen, *clientId);
00327     return desirialize(buf, len);
00328 }
00329 
00330 int MQTTSNPacket::getSERCHGW(uint8_t* radius)
00331 {
00332     return MQTTSNDeserialize_searchgw((unsigned char*) radius, (unsigned char*) _buf, _bufLen);
00333 }
00334 
00335 int MQTTSNPacket::getCONNECT(MQTTSNPacket_connectData* data)
00336 {
00337     return MQTTSNDeserialize_connect(data, _buf, _bufLen);
00338 }
00339 
00340 int MQTTSNPacket::getCONNACK(uint8_t* returnCode)
00341 {
00342     return MQTTSNSerialize_connack(_buf, _bufLen, (int) *returnCode);
00343 }
00344 
00345 int MQTTSNPacket::getWILLTOPIC(int* willQoS, uint8_t* willRetain, MQTTSNString* willTopic)
00346 {
00347     return MQTTSNDeserialize_willtopic((int*) willQoS, (unsigned char*) willRetain, willTopic, _buf, _bufLen);
00348 }
00349 
00350 int MQTTSNPacket::getWILLMSG(MQTTSNString* willmsg)
00351 {
00352     return MQTTSNDeserialize_willmsg(willmsg, _buf, _bufLen);
00353 }
00354 
00355 int MQTTSNPacket::getREGISTER(uint16_t* topicId, uint16_t* msgId, MQTTSNString* topicName)
00356 {
00357     return MQTTSNDeserialize_register((unsigned short*) topicId, (unsigned short*) msgId, topicName,
00358             _buf, _bufLen);
00359 }
00360 
00361 int MQTTSNPacket::getREGACK(uint16_t* topicId, uint16_t* msgId, uint8_t* returnCode)
00362 {
00363     return MQTTSNDeserialize_regack((unsigned short*) topicId, (unsigned short*) msgId, (unsigned char*) returnCode, _buf, _bufLen);
00364 }
00365 
00366 int MQTTSNPacket::getPUBLISH(uint8_t* dup, int* qos, uint8_t* retained, uint16_t* msgId, MQTTSN_topicid* topic,
00367         uint8_t** payload, int* payloadlen)
00368 {
00369     return MQTTSNDeserialize_publish((unsigned char*) dup, qos, (unsigned char*) retained, (unsigned short*) msgId,
00370             topic, (unsigned char**) payload, (int*) payloadlen, _buf, _bufLen);
00371 }
00372 
00373 int MQTTSNPacket::getPUBACK(uint16_t* topicId, uint16_t* msgId, uint8_t* returnCode)
00374 {
00375     return MQTTSNDeserialize_puback((unsigned short*) topicId, (unsigned short*) msgId, (unsigned char*) returnCode,
00376             _buf, _bufLen);
00377 }
00378 
00379 int MQTTSNPacket::getACK(uint16_t* msgId)
00380 {
00381     unsigned char type;
00382     return MQTTSNDeserialize_ack(&type, (unsigned short*) msgId, _buf, _bufLen);
00383 }
00384 
00385 int MQTTSNPacket::getSUBSCRIBE(uint8_t* dup, int* qos, uint16_t* msgId, MQTTSN_topicid* topicFilter)
00386 {
00387     return MQTTSNDeserialize_subscribe((unsigned char*) dup, qos, (unsigned short*) msgId, topicFilter, _buf, _bufLen);
00388 }
00389 
00390 int MQTTSNPacket::getUNSUBSCRIBE(uint16_t* msgId, MQTTSN_topicid* topicFilter)
00391 {
00392     return MQTTSNDeserialize_unsubscribe((unsigned short*) msgId, topicFilter, _buf, _bufLen);
00393 }
00394 
00395 int MQTTSNPacket::getPINGREQ(void)
00396 {
00397     if (getType() == MQTTSN_PINGRESP && _bufLen > 2 )
00398     {
00399         return _bufLen - 2;
00400     }
00401     return 0;
00402 }
00403 
00404 int MQTTSNPacket::getDISCONNECT(uint16_t* duration)
00405 {
00406     int dur = 0;
00407     int rc = MQTTSNDeserialize_disconnect(&dur, _buf, _bufLen);
00408     *duration = (uint16_t)dur;
00409     return rc;
00410 }
00411 
00412 int MQTTSNPacket::getWILLTOPICUPD(uint8_t* willQoS, uint8_t* willRetain, MQTTSNString* willTopic)
00413 {
00414     return MQTTSNDeserialize_willtopicupd((int*) willQoS, (unsigned char*) willRetain, willTopic, _buf, _bufLen);
00415 }
00416 
00417 int MQTTSNPacket::getWILLMSGUPD(MQTTSNString* willMsg)
00418 {
00419     return MQTTSNDeserialize_willmsgupd(willMsg, _buf, _bufLen);
00420 }
00421 
00422 char* MQTTSNPacket::print(char* pbuf)
00423 {
00424     char* ptr = pbuf;
00425     char** pptr = &pbuf;
00426     int size = _bufLen > SIZE_OF_LOG_PACKET ? SIZE_OF_LOG_PACKET : _bufLen;
00427 
00428     for (int i = 0; i < size; i++)
00429     {
00430         sprintf(*pptr, " %02X", *(_buf + i));
00431         *pptr += 3;
00432     }
00433     **pptr = 0;
00434     return ptr;
00435 }
00436 
00437 char* MQTTSNPacket::getMsgId(char* pbuf)
00438 {
00439     int value = 0;
00440     int p = 0;
00441 
00442     switch ( getType() )
00443     {
00444     case MQTTSN_PUBLISH:
00445         p = MQTTSNPacket_decode(_buf, _bufLen, &value);
00446         if ( _buf[p + 1] & 0x80 )
00447         {
00448             sprintf(pbuf, "+%02X%02X", _buf[p + 4], _buf[p + 5]);
00449         }
00450         else
00451         {
00452             sprintf(pbuf, " %02X%02X", _buf[p + 4], _buf[p + 5]);
00453         }
00454         break;
00455     case MQTTSN_PUBACK:
00456     case MQTTSN_REGISTER:
00457     case MQTTSN_REGACK:
00458         sprintf(pbuf, " %02X%02X", _buf[4], _buf[5]);
00459         break;
00460     case MQTTSN_PUBREC:
00461     case MQTTSN_PUBREL:
00462     case MQTTSN_PUBCOMP:
00463     case MQTTSN_UNSUBACK:
00464         sprintf(pbuf, " %02X%02X", _buf[2], _buf[3]);
00465         break;
00466     case MQTTSN_SUBSCRIBE:
00467     case MQTTSN_UNSUBSCRIBE:
00468         p = MQTTSNPacket_decode(_buf, _bufLen, &value);
00469         sprintf(pbuf, " %02X%02X", _buf[p + 2], _buf[p + 3]);
00470         break;
00471     case MQTTSN_SUBACK:
00472         sprintf(pbuf, " %02X%02X", _buf[5], _buf[6]);
00473         break;
00474     default:
00475         sprintf(pbuf, "    ");
00476         break;
00477     }
00478     if ( strcmp(pbuf, " 0000") == 0 )
00479     {
00480         sprintf(pbuf, "    ");
00481     }
00482     return pbuf;
00483 }
00484 
00485 int MQTTSNPacket::getMsgId(void)
00486 {
00487     int value = 0;
00488     int p = 0;
00489     int msgId = 0;
00490     char* ptr = 0;
00491 
00492     switch ( getType() )
00493     {
00494     case MQTTSN_PUBLISH:
00495         p = MQTTSNPacket_decode(_buf, _bufLen, &value);
00496         ptr = (char*)_buf + p + 4;
00497         msgId = readInt((char**)&ptr);
00498         break;
00499     case MQTTSN_PUBACK:
00500     case MQTTSN_REGISTER:
00501     case MQTTSN_REGACK:
00502         ptr = (char*)_buf + 4;
00503         msgId = readInt((char**)&ptr);
00504         break;
00505     case MQTTSN_PUBREC:
00506     case MQTTSN_PUBREL:
00507     case MQTTSN_PUBCOMP:
00508     case MQTTSN_UNSUBACK:
00509         ptr = (char*)_buf + 2;
00510         msgId = readInt((char**)&ptr);
00511         break;
00512     case MQTTSN_SUBSCRIBE:
00513     case MQTTSN_UNSUBSCRIBE:
00514         p = MQTTSNPacket_decode(_buf, _bufLen, &value);
00515         ptr = (char*)_buf + p + 2;
00516         msgId = readInt((char**)&ptr);
00517         break;
00518     case MQTTSN_SUBACK:
00519         ptr = (char*)_buf + 5;
00520         msgId = readInt((char**)&ptr);
00521         break;
00522     default:
00523         break;
00524     }
00525     return msgId;
00526 }
00527 
00528 void MQTTSNPacket::setMsgId(uint16_t msgId)
00529 {
00530     int value = 0;
00531     int p = 0;
00532     //unsigned char* ptr = 0;
00533 
00534     switch ( getType() )
00535     {
00536     case MQTTSN_PUBLISH:
00537         p = MQTTSNPacket_decode(_buf, _bufLen, &value);
00538         _buf[p + 4] = (unsigned char)(msgId / 256);
00539         _buf[p + 5] = (unsigned char)(msgId % 256);
00540         //ptr = _buf + p + 4;
00541         //writeInt(&ptr, msgId);
00542         break;
00543     case MQTTSN_PUBACK:
00544     case MQTTSN_REGISTER:
00545     case MQTTSN_REGACK:
00546         _buf[4] = (unsigned char)(msgId / 256);
00547         _buf[5] = (unsigned char)(msgId % 256);
00548         //ptr = _buf + 4;
00549         //writeInt(&ptr, msgId);
00550         break;
00551     case MQTTSN_PUBREC:
00552     case MQTTSN_PUBREL:
00553     case MQTTSN_PUBCOMP:
00554     case MQTTSN_UNSUBACK:
00555         _buf[2] = (unsigned char)(msgId / 256);
00556         _buf[3] = (unsigned char)(msgId % 256);
00557         //ptr = _buf + 2;
00558         //writeInt(&ptr, msgId);
00559         break;
00560     case MQTTSN_SUBSCRIBE:
00561     case MQTTSN_UNSUBSCRIBE:
00562         p = MQTTSNPacket_decode(_buf, _bufLen, &value);
00563         _buf[p + 2] = (unsigned char)(msgId / 256);
00564         _buf[p + 3] = (unsigned char)(msgId % 256);
00565         //ptr = _buf + p + 2;
00566         //writeInt(&ptr, msgId);
00567 break;
00568     case MQTTSN_SUBACK:
00569         _buf[5] = (unsigned char)(msgId / 256);
00570         _buf[6] = (unsigned char)(msgId % 256);
00571         //ptr = _buf + 5;
00572         //writeInt(&ptr, msgId);
00573 break;
00574     default:
00575         break;
00576     }
00577 }
00578 
00579 bool MQTTSNPacket::isDuplicate(void)
00580 {
00581     int value = 0;
00582     int p = MQTTSNPacket_decode(_buf, _bufLen, &value);
00583     return ( _buf[p + 1] & 0x80 );
00584 }