Full Code of ty4tw/MQTT-SN for AI

master 6f471e225221 cached
65 files
535.4 KB
151.8k tokens
238 symbols
1 requests
Download .txt
Showing preview only (560K chars total). Download the full file or copy to clipboard to get everything.
Repository: ty4tw/MQTT-SN
Branch: master
Commit: 6f471e225221
Files: 65
Total size: 535.4 KB

Directory structure:
gitextract_nt1a49t7/

├── Client/
│   ├── Makefile
│   ├── README.md
│   └── src/
│       ├── LinuxClientSample.cpp
│       ├── SoilMoistureClientSample.ino
│       ├── lib/
│       │   ├── MQTTSN_Application.h
│       │   ├── Network.h
│       │   ├── mqUtil.cpp
│       │   ├── mqUtil.h
│       │   ├── mqttsn.cpp
│       │   ├── mqttsn.h
│       │   ├── mqttsnClient.cpp
│       │   ├── mqttsnClient.h
│       │   ├── mqttsnClientAppFw4Arduino.cpp
│       │   ├── mqttsnClientAppFw4Arduino.h
│       │   ├── mqttsnClientAppFw4Linux.cpp
│       │   ├── mqttsnClientAppFw4Linux.h
│       │   ├── mqttsnClientAppFw4mbed.cpp
│       │   ├── mqttsnClientAppFw4mbed.h
│       │   ├── udpStack.cpp
│       │   ├── udpStack.h
│       │   ├── zbeeStack.cpp
│       │   └── zbeeStack.h
│       └── mbedClientSample.cpp
├── Gateway/
│   ├── Makefile
│   ├── README.md
│   └── src/
│       ├── BrokerRecvTask.cpp
│       ├── BrokerRecvTask.h
│       ├── BrokerSendTask.cpp
│       ├── BrokerSendTask.h
│       ├── ClientRecvTask.cpp
│       ├── ClientRecvTask.h
│       ├── ClientSendTask.cpp
│       ├── ClientSendTask.h
│       ├── ErrorMessage.h
│       ├── GatewayControlTask.cpp
│       ├── GatewayControlTask.h
│       ├── GatewayDefines.h
│       ├── GatewayResourcesProvider.cpp
│       ├── GatewayResourcesProvider.h
│       ├── TomyGateway.cpp
│       └── lib/
│           ├── Defines.h
│           ├── Messages.cpp
│           ├── Messages.h
│           ├── ProcessFramework.cpp
│           ├── ProcessFramework.h
│           ├── TCPStack.cpp
│           ├── TCPStack.h
│           ├── TLSStack.cpp
│           ├── TLSStack.h
│           ├── Topics.cpp
│           ├── Topics.h
│           ├── UDPStack.cpp
│           ├── UDPStack.h
│           ├── XXXXXStack.cpp
│           ├── XXXXXStack.h
│           ├── ZBStack.cpp
│           └── ZBStack.h
├── LogMonitor/
│   ├── Makefile
│   └── src/
│       ├── LogMonitor.cpp
│       ├── LogMonitor.h
│       ├── LogMonitorApp.cpp
│       └── lib/
│           ├── Defines.h
│           ├── ProcessFramework.cpp
│           └── ProcessFramework.h
└── README.md

================================================
FILE CONTENTS
================================================

================================================
FILE: Client/Makefile
================================================
PROGNAME := TomyClient
SRCDIR := src
SUBDIR := src/lib

SRCS := $(SRCDIR)/LinuxClientSample.cpp \
$(SUBDIR)/mqttsnClientAppFw4Linux.cpp \
$(SUBDIR)/mqttsn.cpp \
$(SUBDIR)/mqttsnClient.cpp \
$(SUBDIR)/zbeeStack.cpp \
$(SUBDIR)/udpStack.cpp \
$(SUBDIR)/mqUtil.cpp 

CXX := g++
CPPFLAGS += 
DEFS :=
LDFLAGS += 
LIBS +=

CXXFLAGS := -Wall -O3

OUTDIR := Build

PROG := $(OUTDIR)/$(PROGNAME)
OBJS := $(SRCS:%.cpp=$(OUTDIR)/%.o)
DEPS := $(SRCS:%.cpp=$(OUTDIR)/%.d)

.PHONY: install clean distclean

all: $(PROG)

-include $(DEPS)

$(PROG): $(OBJS)
	$(CXX) $(LDFLAGS) -o $@ $^ $(LIBS)

$(OUTDIR)/%.o:%.cpp
	@if [ ! -e `dirname $@` ]; then mkdir -p `dirname $@`; fi
	$(CXX) $(CXXFLAGS) $(CPPFLAGS) $(DEFS) -o $@ -c -MMD -MP -MF $(@:%.o=%.d) $<

clean:
	rm -rf $(OUTDIR)

distclean:
	rm -rf Build
	
install:
	cp -p $(PROG) ../../
	


================================================
FILE: Client/README.md
================================================
MQTT-SN Client
======
  MQTT-SN Client over XBee (running on linux, Arduino and mbed)    
  MQTT-SN Client over UDP  (running on linux and Arduino)  
  
  
Supported functions
-------------------

   QOS Level 0 and 1
*  SEARCHGW, GWINFO
*  CONNECT, WILLTOPICREQ, WILLTOPIC, WILLMSGREQ, WILLMSG
*  PINGREQ, PINGRESP
*  CONNACK, REGISTER, REGACK, SUBACK, PUBACK, UNSUBACK 
*  SUBSCRIBE, PUBLISH, UNSUBSCRIBE, DISCONNECT

Implemented control flows:  
   Application program executes publish() function,   
   Message flow as berrow is conducted automaticaly.  


                 Client              Gateway               Broker
                    |                   |                    |      
       PUBLISH() -->| --- SERCHGW ----> |                    |  
                    | <-- GWINFO  ----- |                    |  
                    | --- CONNECT ----> |                    |  
                    | <--WILLTOPICREQ-- |                    |  
                    | --- WILLTOPIC --> |                    |  
                    | <-- WILLMSGREQ -- |                    |  
                    | --- WILLMSG ----> | ---- CONNECT ----> |(accepted)     
                    | <-- CONNACK ----- | <--- CONNACK ----- |   
                    | --- PUBLISH ----> |                    |  
                    | <-- PUBACK  ----- | (invalid TopicId)  |  
                    | --- REGISTER ---> |                    |  
                    | <-- REGACK  ----- |                    |  
                    | --- PUBLISH ----> | ---- PUBLISH ----> |(accepted)  
                    | <-- PUBACK  ----- | <---- PUBACK ----- |    
                    |                   |                    |    
                    //                  //                   //      
                    |                   |                    |          
     SUBSCRIBE() -->| --- SUBSCRIBE --> | ---- SUBSCRIBE --> |     
     [set Callback] | <-- SUBACK ------ | <--- SUBACK ------ |    
                    |                   |                    |    
                    //                  //                   //    
                    |                   |                    |    
                    | <-- REGISTER ---- | <--- PUBLISH ----- |<-- PUBLISH  
    [exec Callback] | <-- PUBLISH  ---- |                    |  
                    | --- PUBACK   ---> | ---- PUBACK  ----> |--> PUBACK  
                    |                   |                    |  
                

Usage
------
####Minimum requirements
  Over XBee    
  Three XBee S2 devices,  a coordinator, a gateway and a client.    
  or two XBee S1 with digimesh firmware, gateway and client.    
  
  Over UDP    
  Arduino with Ethernet shield or Arduino Ether.
  or linux client.

  MQTT Broker
  TomyGateway   

####1) Start Gateway  

  see MQTT-SN/Gateway    
    
    
  
####2) Start Client   (-d parameter is a device which XBee dongle connected.)  
    
  1. XBee client    

    $ TomyClient  -i ClientID  -d /dev/ttyUSB0  -b 9600    

  2. UDP Client (Linux only, Arduino is embeded parameters defined by UDP_APP_CONFIG)    

    $ TomyClient  -i ClientID  -g  225.1.1.1  -p 1883   -c  -t WillTopic  -m WillMessage -k 300 

     	-g : Multicast address    
        -p : Multicast port        
        -c : Clean session        
        -k : Keep alive    
  
    
####3) XBee configurations 
    
  Serial interfacing  of Clients and gateway.    
  Coordinator is default setting.  
  XBee Firmware version is 2xA7.  
  
    [BD] 0-7   Arduino Clients : 3 (9600bps)  Linux Clients : 5 (38400bps)  
    [D7] 1  
    [D6] 0 or 1  
    [AP] 2  
    [SP]  Coordinator, Router Device : AF0  End device :20  

  Other values are defaults. Baudrate is used by  mqtts.begin(device, _baudrate_) or mqtts.begin(_baudrate_) function.   
  In case of LINUX, if you set D6 to 1, uncomment a line //#define XBEE_FLOWCTL_CRTSCTS in Mqtts_Defines.h
  

How to Build (Requiered source code list)
-----------
####1) Linux Client
_copy src/lib/*  and src/LinuxClientSample.cpp_  

     $ make    
     $ make install    

####2) Arduino Client
_Copy src/lib into Aruino Librally directory._
_Copy SoilMoistureClientSample.ino into Aruino sketch directory._

  
_in IPAddress.h, change raw_address() tp public from private._  
_Add beginMulticast() to EthernetUDP.cpp_   
see http://forum.arduino.cc/index.php?topic=150006.0    

####3) mbed Client
_copy src/lib/*  and src/mbedClientSample.cpp_  

Module descriptions
-------------------  
####1) MqttsClientApp.cpp  
Client application sample which is used for debug.

  
####2) MqttsClientFwApp.ino
  MqttsClient sample application for Arduino. 
    
###Modules in mqttslib

####1) MqttsClient.cpp
  MQTT-S Client Engine class. This Class is used by  a application.  
  Usages are shown as follows.
  
    MqttsClient mqtts = MqttsClient();  // Declare the client object
    mqtts.begin(argv[1], B9600);        // argv[1] is a serial device for XBee. ex) /dev/ttyUSB0 
    mqtts.init("Node-02");              // Get XBee's address64, short address and set XBee Node ID, 
    mqtts.setWillTopic(willtopic);      // set WILLTOPIC.   
    mqtts.setWillMessage(willmsg);      // set WILLMSG  those are sent automatically. 
    mqtts.setKeepAlive(60000);          // PINGREQ interval time

    mqtts.subscribe(topic, callback,qos);   // Execute the callback, when the subscribed topic's data is published. 
    mqtts.publish(topic, payload, payload_length,qos); // publish the data, topic is converted into ID automatically.
    mqtts.publish(topic, MQString* payload,qos);  
    mqtts.unsubscribe(topic);  
    mqtts.disconnect();

    above APIs are primitive one.     
    In the Application, use PUBLISE(), SUBSCRIBE(), UNSUBSCRIBE(), DISCONNECT().     
    see Sample.cpp    

    
####2) MqttsClientAppFw4Arduino.cpp
  Application framework for Arduino.
  Interupt and  watch dog timer are supported.
  set MAC address of your Ethernet sheild to the UDP_APP_CONFIG structure.
      
####3) MqttsClientAppFw4Linux.cpp
  Application framework for Linux.
  watch dog timer are supported.    

####4) MqttsClientAppFw4mbed.cpp
  Application framework for mbed.
  watch dog timer are supported.    
  UDP is not supported.

####5) MQTTS.cpp 
  MQTT-S messages classes and some classes for client and Gateway.
    
####6) ZBeeStack.cpp
  XBee control classes

####7) udpStack.cpp
  UDP control classes
    
####8) MQTTSN_Application.h
  Default setting is Linux and UDP.  
  select the system and uncoment it.
    
    //#define LINUX 
    //#define MBED
    
  select the NetworkStack and uncomment it.
   
   //#define NETWORK_XBEE     
   //#define NETWORK_UDP     
  
  



================================================
FILE: Client/src/LinuxClientSample.cpp
================================================
/*
 * LinuxClientSample.cpp
 *
 *                      The BSD License
 *
 *           Copyright (c) 2014, tomoaki@tomy-tech.com
 *                    All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: 2014/06/01
 *    Modified: 2014/09/05
 *      Author: Tomoaki YAMAGUCHI
 *     Version: 1.0.0
 */

#include "lib/MQTTSN_Application.h"

#ifdef LINUX
#include "lib/mqttsnClientAppFw4Linux.h"
#include <stdio.h>
#include <string.h>

using namespace std;
using namespace tomyClient;

/*============================================
 *
 *   MQTT-SN Client Application for Linux
 *
 *===========================================*/

/*------------------------------------------------------
 *             Create Topic
 *------------------------------------------------------*/

MQString* tp1 = new MQString("topic/01");
MQString* tp2 = new MQString("topic/02");
MQString* tp3 = new MQString("topic/03");
MQString* tp4 = new MQString("topic/04");
MQString* tp5 = new MQString("topic/05");

MQString* tp_request_01 = new MQString("topic/req/01");
MQString* tp_response_01 = new MQString("topic/rsp/01");

MQString* tp_request_02 = new MQString("topic/req/02");
MQString* tp_response_02 = new MQString("topic/rsp/02");

/*------------------------------------------------------
 *             Tasks invoked by Timer
 *------------------------------------------------------*/

int f_publish_all(){
  PUBLISH(tp1,"topic/01", 8,QOS1);
  PUBLISH(tp2,"topic/02", 8,QOS1);
  PUBLISH(tp3,"topic/03", 8,QOS1);
  PUBLISH(tp4,"topic/04", 8,QOS1);
  PUBLISH(tp5,"topic/05", 8,QOS1);

  Payload pl = Payload(40);
  pl.set_uint32( 200);
  pl.set_uint32( 10);
  pl.set_uint32( 50000);
  pl.set_uint32( 70000);
  pl.set_int32( -300);
  pl.set_int32(-70000);
  pl.set_float((float)1000.01);
  pl.set_str("abcdef");
  PUBLISH(tp_request_01,&pl,1);

  return 0;
}


/*---------------  List of task invoked by Timer ------------*/

TASK_LIST = {  //{ MQString* topic, executing duration in second},
  {f_publish_all, 10},
  END_OF_TASK_LIST
};


/*------------------------------------------------------
 *       Tasks invoked by PUBLISH command Packet
 *------------------------------------------------------*/
/*----- Topic list used in callback functions -----*/
TOPICS_IN_CALLBACK = {
	tp_response_01,
	tp_response_02,
	END_OF_TOPICS
};

int f_onRequest_01(MqttsnPublish* msg){
	printf("inside f_onRequest_01()\n");
	Payload pl;
	pl.getPayload(msg);
	pl.print();
	printf("uint8: %d\n",pl.get_uint32(0));
	printf("uint16: %d\n",pl.get_uint32(1));
	printf("uint32: %d\n",pl.get_uint32(2));
	printf("int8: %d\n",pl.get_uint32(3));
	printf("int16: %d\n",pl.get_int32(4));
	printf("int32: %d\n",pl.get_int32(5));
	printf("float: %g \n",pl.get_float(6));
	uint16_t len;
	printf("str: %s  \n",pl.get_str(7,&len));

  PUBLISH(tp_response_01,&pl,QOS1);
  return 0;
}

int f_onRequest_02(MqttsnPublish* msg){
	printf("inside f_onRequest_02()\n");
  PUBLISH(tp_response_02,"topic/rsp/02", 12,1);
  return 0;
}

/*-------------- List of Task invoked by PUBLISH  -----------*/

SUBSCRIBE_LIST = { //{ MQString* topic, on_publish1, QOS },
  {tp_request_01, f_onRequest_01, QOS1},
  {tp_request_02, f_onRequest_02, QOS1},
  END_OF_SUBSCRIBE_LIST
};


/*==================================================
 *      Application setup
 *=================================================*/
 void setup(){
    printf("Client start\n");
 }
/*==============    End of Program    ==============*/

#endif  // LINUX


================================================
FILE: Client/src/SoilMoistureClientSample.ino
================================================
/*
 * SoilMoisture.ino
 *
 *                      The BSD License
 *
 *           Copyright (c) 2014, tomoaki@tomy-tech.com
 *                    All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: 2014/06/01
 *    Modified: 2014/09/05
 *      Author: Tomoaki YAMAGUCHI
 *     Version: 1.0.0
 */

#include <MQTTSN_Application.h>
#include <mqttsnClientAppFw4Arduino.h>
#include <SPI.h>
#include <Ethernet.h>
#include <EthernetUdp.h>

/*============================================
 *
 *   MQTT-SN Client Application for Arduino
 *
 *===========================================*/
 #ifdef NETWORK_XBEE
 XBEE_APP_CONFIG = {
    {
        9600,           //Baudrate
        0,              //Serial PortNo (for Arduino App)
        0               //Device (for linux App)
    }, 
    {             
        "ARD02",       //ClientId
        300,            //KeepAlive
        true,           //Clean session
        false,           //EndDevice
        "willTopic",    //WillTopic   or 0   DO NOT USE NULL STRING "" !
        "willMessage"   //WillMessage or 0   DO NOT USE NULL STRING "" !
    }
 };
#endif

#ifdef NETWORK_UDP
UDP_APP_CONFIG = {
    { 
        {225,1,1,1},         // Multicast group IP
        1883,                // Multicast group Port
        {192,168,11,18},     // Local IP     (for Arduino App)
        12001,               // Local PortNo
        {0x90,0xa2,0xda,0x0f,0x53,0xa5}       // MAC address  (for Arduino App)
    },
    { 
        "ARD02",       //ClientId
        300,            //KeepAlive
        true,           //Clean session
        false,           //EndDevice
        "willTopic",    //WillTopic   or 0   DO NOT USE NULL STRING "" !
        "willMessage"   //WillMessage or 0   DO NOT USE NULL STRING "" !
    } 
};
#endif

/*------------------------------------------------------
 *             Create Topic
 *------------------------------------------------------*/
MQString* topic1 = new MQString("ty4tw/tp1");
MQString* topic2 = new MQString("ty4tw/tp2");
MQString* tpMeasure = new MQString("ty4tw/soilReg");
/*------------------------------------------------------
 *             Tasks invoked by Timer
 *------------------------------------------------------*/
#define PIN5  5    // 3.3V supply port 
#define MCOUNT 10  // measurement count
#define PIN0  0    // measurement port
#define RP   20    // resistance [K ohom] 

int measure(){
  int val = 0;
  pinMode(PIN5,OUTPUT);
  digitalWrite(PIN5,1);
  
  for(uint8_t cnt = 0; cnt < MCOUNT; cnt++){
    delay(50);
    val += analogRead(PIN0);
  }
  digitalWrite(PIN5,0);
  int soilR = 1023 * RP / (val /MCOUNT) - RP;
  if(soilR < 0){
    soilR = 9999;
  }
  Payload pl(30);
  pl.set_array(3);
  pl.set_uint32(GETUTC());
  pl.set_int32(soilR);
  pl.set_str("Kohom");
  return PUBLISH(tpMeasure,&pl,QOS1);
}


int task1(){
  Payload pl = Payload(36);
  pl.set_array(9);
  pl.set_int32(30);
  pl.set_int32(255);
  pl.set_int32(70000);
  pl.set_str("abcdef");
  pl.set_int32(-16);
  pl.set_int32(-60);
  pl.set_int32(-300);
  pl.set_int32(-70000);
  pl.set_float(1000.01);
  return PUBLISH(topic1,&pl,QOS1);
}

/*---------------  List of task invoked by Timer ------------*/

TASK_LIST = {  //{ MQString* topic, executing duration in second},
           {measure, 40},
           {task1,6},
           //{task2,6},
END_OF_TASK_LIST};


/*------------------------------------------------------
 *       Tasks invoked by PUBLISH command Packet
 *------------------------------------------------------*/
 TOPICS_IN_CALLBACK = {

   END_OF_TOPICS
 };
 

int on_publish2(MqttsnPublish* msg){
    theApplication->indicatorOff();
    return 0;
}

/*------------ Link Callback to Topic -------------*/

SUBSCRIBE_LIST = {
                    //{topic1, on_publish1, QOS1},
                    //{topic2, on_publish2, QOS1},

END_OF_SUBSCRIBE_LIST};

/*------------------------------------------------------
 *            Tasks invoked by INT0 interuption 
 *------------------------------------------------------*/
void interruptCallback(){
  //NOP
}

/*------------------------------------------------------
 *            Arduino setup() function 
 *------------------------------------------------------*/
 void setup(){

 }




================================================
FILE: Client/src/lib/MQTTSN_Application.h
================================================
/*
 * MQTTSN_Application.h
 *
 *                    The BSD License
 *
 *           Copyright (c) 2014, tomoaki@tomy-tech.com
 *                   All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: 2014/06/01
 *    Modified: 2014/09/05
 *      Author: Tomoaki YAMAGUCHI
 *     Version: 1.0.0
 */

#ifndef MATTSNAPPLICATION_H_
#define MATTSNAPPLICATION_H_

/****************************************
      Select Platform and Network
*****************************************/

/*----------- Select Platform  ---------*/
#ifndef ARDUINO
	#define LINUX
	//#define MBED
#endif

/*--------- Select byte order ----------*/
#define CPU_LITTLEENDIANN
//#define CPU_BIGENDIANN


/*-------- Select Network  -------------*/
#define NETWORK_XBEE
//#define NETWORK_UDP


/*--- XBee Buffer Flow Control --*/
#ifdef NETWORK_XBEE
	//#define XBEE_FLOWCTL_CRTSCTS
#endif

/*======================================
 *         Debug Flag
 ======================================*/
//#define NW_DEBUG
//#define MQTTSN_DEBUG
#define DEBUG


/****************************************
      MQTT-SN Packet length
*****************************************/
#ifdef ARDUINO
	#define MQTTSN_MAX_FRAME_SIZE               70
#else
	#ifdef NETWORK_XBEE
		#define MQTTSN_MAX_FRAME_SIZE           70
	#else
		#ifdef NETWORK_UDP
			#define MQTTSN_MAX_FRAME_SIZE     1024
		#endif
	#endif
#endif
/****************************************
      Application config structures
*****************************************/
#ifdef LINUX
typedef unsigned char  uint8_t;
typedef unsigned short uint16_t;
typedef unsigned int   uint32_t;
typedef signed char    int8_t;
typedef signed short   int16_t;
typedef signed int     int32_t;
#endif

#ifdef MBED
	#include "mbed.h"
#endif

#ifdef ARDUINO
	#include <Arduino.h>
#endif

typedef struct {
	const char* nodeId;
	uint16_t keepAlive;
	bool     cleanSession;
	bool     endDevice;
	const char* willTopic;
	const char* willMsg;
}MqttsnConfig;

typedef struct {
	long baudrate;
	uint8_t  portNo;
	char* device;
}XBeeConfig;

typedef struct {
	XBeeConfig netCfg;
	MqttsnConfig mqttsnCfg;
}XBeeAppConfig;

typedef struct {
	uint8_t  ipAddress[4];
	uint16_t gPortNo;
	uint8_t  ipLocal[4];
	uint16_t uPortNo;
	uint8_t  macAddr[6];
}UdpConfig;

typedef struct {
	UdpConfig netCfg;
	MqttsnConfig mqttsnCfg;
}UdpAppConfig;

/*======================================
      MACROs for Application
=======================================*/
#ifdef NETWORK_XBEE
    #define XBEE_APP_CONFIG       XBeeAppConfig  theAppConfig
	#define APP_CONFIG            XBeeAppConfig
	#define NETWORK_CONFIG        XBeeConfig
#endif

#ifdef NETWORK_UDP
    #define UDP_APP_CONFIG        UdpAppConfig   theAppConfig
	#define APP_CONFIG            UdpAppConfig
	#define NETWORK_CONFIG        UdpConfig
#endif

#define TASK_LIST         TaskList theTaskList[]
#define END_OF_TASK_LIST  {0,0}
#define SUBSCRIBE_LIST   OnPublishList theOnPublishList[]
#define END_OF_SUBSCRIBE_LIST {0,0,0}
#define TOPICS_IN_CALLBACK MQString* theTopics[]
#define END_OF_TOPICS    0

#define PUBLISH(...)     theApplication->publish(__VA_ARGS__)
#define SUBSCRIBE(...)   theApplication->subscribe(__VA_ARGS__)
#define UNSUBSCRIBE(...) theApplication->unsubscribe(__VA_ARGS__)
#define DISCONNECT()     theApplication->disconnect()
#define SETRETAIN(...)   theApplication->setRetain(__VA_ARGS__)

/*======================================
      MACROs for debugging
========================================*/
#ifdef ARDUINO
	#ifdef NW_DEBUG
		#define D_NWSTACK(...)    debug.print(__VA_ARGS__)
		#define D_NWSTACKLN(... ) debug.println(__VA_ARGS__)
		#define D_NWSTACKW(...)   debug.print(__VA_ARGS__)
		#define D_NWSTACKF(...)
	#else
		#define D_NWSTACK(...)
		#define D_NWSTACKLN(...)
		#define D_NWSTACKW(...)
		#define D_NWSTACKF(...)
	#endif

	#ifdef MQTTSN_DEBUG
		#define D_MQTT(...)    debug.print(__VA_ARGS__)
		#define D_MQTTW(...)   debug.print(__VA_ARGS__)
		#define D_MQTTLN(...)  debug.println(__VA_ARGS__)
		#define D_MQTTF(...)
	#else
		#define D_MQTT(...)
		#define D_MQTTLN(...)
		#define D_MQTTW(...)
		#define D_MQTTF(...)
	#endif
#else
	#ifdef NW_DEBUG
		#define D_NWSTACKF(...)    printf(__VA_ARGS__)
		#define D_NWSTACKW(...)   printf(__VA_ARGS__)
		#define D_NWSTACKLN(...)
		#define D_NWSTACK(...)
	#else
		#define D_NWSTACK(...)
		#define D_NWSTACKLN(...)
		#define D_NWSTACKW(...)
		#define D_NWSTACKF(...)
	#endif
	#ifdef MQTTSN_DEBUG
		#define D_MQTTF(...)    printf(__VA_ARGS__)
		#define D_MQTTW(...)   printf("%s",__VA_ARGS__)
		#define D_MQTTLN(...)
		#define D_MQTT(...)
	#else
		#define D_MQTT(...)
		#define D_MQTTLN(...)
		#define D_MQTTW(...)
		#define D_MQTTF(...)
		#endif
#endif

#endif /* MATTSNAPPLICATION_H_ */



================================================
FILE: Client/src/lib/Network.h
================================================
/*
 * Network.cpp
 *
 *                      The BSD License
 *
 *           Copyright (c) 2014, tomoaki@tomy-tech.com
 *                    All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: 2014/06/01
 *    Modified: 2014/09/05
 *      Author: Tomoaki YAMAGUCHI
 *     Version: 1.0.0
 */

#ifndef  NEWORK_H_
#define  NEWORK_H_

#ifndef ARDUINO
    #include "MQTTSN_Application.h"
#else
	#include <MQTTSN_Application.h>
#endif

/***********************************
 *     Send Request Type
 ***********************************/
enum SendReqType{
    NoReq = 0,
    UcastReq,
    BcastReq
};

/*=================================
 *    Network stacks
 ==================================*/
#ifdef NETWORK_XBEE
	#ifdef ARDUINO
		#include <zbeeStack.h>
	#else
		#include "zbeeStack.h"
	#endif
#endif

#ifdef NETWORK_UDP
	#ifdef ARDUINO
		#include <udpStack.h>
	#else
		#include "udpStack.h"
	#endif
#endif


namespace tomyClient {
/***********************************
 *     MQTT-SN  Client's state
 ***********************************/
enum NodeStatus {
    NdDisconnected = 0,
    NdActive,
    NdAsleep,
    NdAwaik,
    NdLost
};

#define PACKET_SENDED           0
#define PACKET_ERROR_RESPONSE  -1
#define PACKET_ERROR_UNKOWN    -2
#define PACKET_ERROR_NODATA    -3
#define PACKET_MODEM_STATUS    -4

}   /* end of namespace */

#endif  /* NEWORK_H_ */


================================================
FILE: Client/src/lib/mqUtil.cpp
================================================
/*
 * mqUtil.cpp
 *
 *                      The BSD License
 *
 *           Copyright (c) 2014, tomoaki@tomy-tech.com
 *                    All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: 2014/06/01
 *    Modified: 2014/09/05
 *      Author: Tomoaki YAMAGUCHI
 *     Version: 1.0.0
 */

#ifndef ARDUINO
        #include "MQTTSN_Application.h"
        #include "mqUtil.h"
		#include "stdio.h"
		#include "string.h"
#else
        #include <MQTTSN_Application.h>
        #include <mqUtil.h>
		#include <stdio.h>
		#include <string.h>
#endif  /* ARDUINO */

#ifdef MBED
        #include "mbed.h"
#endif /* MBED */


using namespace std;
using namespace tomyClient;

/*=====================================
        Global functions
 ======================================*/

#ifdef CPU_LITTLEENDIANN

/*--- For Little endianness ---*/

uint16_t getUint16(uint8_t* pos){
	uint16_t val = ((uint16_t)*pos++ << 8);
	return val += *pos;
}

void setUint16(uint8_t* pos, uint16_t val){
    *pos++ = (val >> 8) & 0xff;
	*pos   = val & 0xff;
}

uint32_t getUint32(uint8_t* pos){
    uint32_t val = uint32_t(*pos++) <<  24;
	val += uint32_t(*pos++) << 16;
	val += uint32_t(*pos++) <<  8;
	return val += *pos++;
}

void setUint32(uint8_t* pos, uint32_t val){
	*pos++ = (val >> 24) & 0xff;
	*pos++ = (val >> 16) & 0xff;
	*pos++ = (val >>  8) & 0xff;
	*pos   =  val & 0xff;
}

float getFloat32(uint8_t* pos){
	union{
		float flt;
		uint8_t d[4];
	}val;
    val.d[3] = *pos++;
	val.d[2] = *pos++;
	val.d[1] = *pos++;
	val.d[0] = *pos;
	return val.flt;
}

void setFloat32(uint8_t* pos, float flt){
	union{
		float flt;
		uint8_t d[4];
	}val;
	val.flt = flt;
    *pos++ = val.d[3];
    *pos++ = val.d[2];
    *pos++ = val.d[1];
    *pos   = val.d[0];
}

#endif  // CPU_LITTLEENDIANN

/*--- For Big endianness ---*/
#ifdef CPU_BIGENDIANN

uint16_t getUint16(uint8_t* pos){
  uint16_t val = *pos++;
  return val += ((uint16_t)*pos++ << 8);
}

void setUint16(uint8_t* pos, uint16_t val){
	*pos++ =  val & 0xff;
	*pos   = (val >>  8) & 0xff;
}

uint32_t getUint32(uint8_t* pos){
    long val = uint32_t(*(pos + 3)) << 24;
    val += uint32_t(*(pos + 2)) << 16;
	val += uint32_t(*(pos + 1)) <<  8;
	return val += *pos;
}

void setUint32(uint8_t* pos, uint32_t val){
    *pos++ =  val & 0xff;
    *pos++ = (val >>  8) & 0xff;
    *pos++ = (val >> 16) & 0xff;
    *pos   = (val >> 24) & 0xff;
}

float getFloat32(uint8_t* pos){
	union{
		float flt;
		uint8_t d[4];
	}val;

    val.d[0] = *pos++;
	val.d[1] = *pos++;
	val.d[2] = *pos++;
	val.d[3] = *pos;
	return val.flt;
}

void setFloat32(uint8_t* pos, float flt){
	union{
		float flt;
		uint8_t d[4];
	}val;
	val.flt = flt;
    *pos++ = val.d[0];
    *pos++ = val.d[1];
    *pos++ = val.d[2];
    *pos   = val.d[3];
}

#endif  // CPU_BIGENDIANN

/*=========================================
             Class XBeeTimer
 =========================================*/

#ifdef ARDUINO
/**
 *   for Arduino
 */
uint32_t XTimer::_unixTime;
uint32_t XTimer::_epochTime;
uint32_t XTimer::_timerStopTimeAccum;
bool    XTimer::_utcFlag;


XTimer::XTimer(){
    stop();
}

void XTimer::initialize(){
	_unixTime = 0;
	_epochTime = 0;
	_timerStopTimeAccum = 0;
	_utcFlag = false;
}

void XTimer::start(uint32_t msec){
    _startTime = millis();
    _millis = msec;
    _currentTime = 0;
	_timeupUnixTime =  getUnixTime() + msec / 1000;
}

bool XTimer::isTimeUp(){
    return isTimeUp(_millis);
}

bool XTimer::isTimeUp(uint32_t msec){
	if(_utcFlag){
		_utcFlag = false;
		if(_timeupUnixTime > 1000000000 && _unixTime){
			return (getUnixTime() >= _timeupUnixTime);
		}else{
			return false;
		}
	}else{
		if ( _startTime){
			_currentTime = millis();
			if ( _currentTime < _startTime){
				return (0xffffffff - _startTime + _currentTime > msec);
			}else{
				return (_currentTime - _startTime > msec);
			}
		}else{
			return false;
		}
	}
}

void XTimer::stop(){
    _startTime = 0;
    _millis = 0;
    _currentTime = 0;
    _timeupUnixTime = 0;
}

void XTimer::setUnixTime(uint32_t utc){
    _epochTime = millis();
    _timerStopTimeAccum = 0;
    _unixTime = utc;
}

uint32_t XTimer::getUnixTime(){
    uint32_t tm = _timerStopTimeAccum + millis();
    if (_epochTime > tm ){
        return _unixTime + (uint32_t)((0xffffffff - tm - _epochTime) / 1000);
    }else{
        return _unixTime + (uint32_t)((tm - _epochTime) / 1000);
    }
}

void XTimer::setStopTimeDuration(uint32_t msec){
	_timerStopTimeAccum += msec;
}

void XTimer::changeUTC(){
	_utcFlag = true;
}

#endif


#ifdef MBED
/**
 *   for MBED
 */
XTimer::XTimer(){
    stop();
}

void XTimer::start(uint32_t msec){
    _timer.start();
    _millis = msec;
    _timeupTime = time(0) + (uint32_t)(msec/1000UL);
}

bool XTimer::isTimeUp(){
    return isTimeUp(_millis);
}

bool XTimer::isTimeUp(uint32_t msec){
	if(_utcFlg){
		if(_timeupTime > 100000000){
			return( time(0) > _timeupTime);
		}else{
			return false;
		}
	}else{
		return _timer.read_ms() > msec;
	}
}

void XTimer::stop(){
    _timer.stop();
    _millis = 0;
    _timeupTime = 0;
}

void XTimer::changeUTC(){
	_utcFlg = true;
}

#endif

#ifdef LINUX
/**
 *   for LINUX
 */
XTimer::XTimer(){
	_startTime.tv_sec = 0;
	_millis = 0;
}

XTimer::~XTimer(){

}

void XTimer::start(uint32_t msec){
  gettimeofday(&_startTime, 0);
  _millis = msec;
}

bool XTimer::isTimeUp(void){
  return isTimeUp(_millis);
}

bool XTimer::isTimeUp(uint32_t msec){
    struct timeval curTime;
    uint32_t secs, usecs;
    if (_startTime.tv_sec == 0){
        return false;
    }else{
        gettimeofday(&curTime, 0);
        secs  = (curTime.tv_sec  - _startTime.tv_sec) * 1000;
        usecs = (curTime.tv_usec - _startTime.tv_usec) / 1000.0;
        return ((secs + usecs) > (uint32_t)msec);
    }
}

void XTimer::stop(){
  _startTime.tv_sec = 0;
  _millis = 0;
}

#endif


================================================
FILE: Client/src/lib/mqUtil.h
================================================
/*
 * mqUtil.h
 *
 *                      The BSD License
 *
 *           Copyright (c) 2014, tomoaki@tomy-tech.com
 *                    All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: 2014/06/01
 *    Modified: 2014/09/05
 *      Author: Tomoaki YAMAGUCHI
 *     Version: 1.0.0
 */

#ifndef XTIMER_H_
#define XTIMER_H_


#if defined(ARDUINO)
	#include <MQTTSN_Application.h>
    #if ARDUINO >= 100
        #include "Arduino.h"
        #include <inttypes.h>
    #else
        #if ARDUINO < 100
            #include "WProgram.h"
            #include <inttypes.h>
        #endif
    #endif

	#define XB_CTS_PIN   3   // XBee CTS
	#define XB_SLEEP_PIN 4   // XBee Pinhybernate
#else
	#include "MQTTSN_Application.h"
#endif /* ARDUINO */


#ifdef LINUX
    #include <sys/time.h>
#endif

#ifdef MBED
    #include "mbed.h"
#endif

namespace tomyClient {


#ifdef ARDUINO
/*============================================
       XBeeTimer for Arduino
 ============================================*/
class XTimer{
public:
    XTimer();
    ~XTimer(){};
    void start(uint32_t msec = 0);
    bool isTimeUp(uint32_t msec);
    bool isTimeUp(void);
    void stop();
    static uint32_t getUnixTime();
    static void setUnixTime(uint32_t utc);
	static void setStopTimeDuration(uint32_t msec);
    static void initialize();
    static void changeUTC();

private:
    uint32_t _startTime;
    uint32_t _currentTime;
    uint32_t _millis;
	uint32_t _timeupUnixTime;
	static uint32_t _unixTime;
	static uint32_t _epochTime;
	static uint32_t _timerStopTimeAccum;
	static bool _utcFlag;

};


#endif

#ifdef MBED
/*============================================
    XBeeTimer  for MBED
 ============================================*/
class XTimer{
public:
    XTimer();
    ~XTimer(){};
    void start(uint32_t msec = 0);
    bool isTimeUp(uint32_t msec);
    bool isTimeUp(void);
    void stop(void);
    void changeUTC(void);

private:
    Timer _timer;
    time_t _startTime;
    uint32_t _millis;
    time_t _timeupTime;
    bool _utcFlg;
};

#endif


#ifdef LINUX
/*============================================
                XBeeTimer
 ============================================*/
class XTimer{
public:
    XTimer();
    ~XTimer();
    void start(uint32_t msec = 0);
    bool isTimeUp(uint32_t msec);
    bool isTimeUp(void);
    void stop(void);
    void changeUTC(void){};
private:
    struct timeval _startTime;
    uint32_t _millis;
};

#endif
}

#endif  /* XTIMER_H_ */


================================================
FILE: Client/src/lib/mqttsn.cpp
================================================
/*
 * mqttsn.cpp
 *
 *                      The BSD License
 *
 *           Copyright (c) 2014, tomoaki@tomy-tech.com
 *                    All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: 2014/06/01
 *    Modified: 2014/09/05
 *      Author: Tomoaki YAMAGUCHI
 *     Version: 1.0.0
 */


#ifndef ARDUINO
  	  #include "MQTTSN_Application.h"
#else
  	  #include <MQTTSN_Application.h>
#endif


#ifdef ARDUINO
  #include <mqttsn.h>
  #include <Network.h>

  #if defined(MQTTSN_DEBUG) || defined(ZBEE_DEBUG)
    #include <SoftwareSerial.h>
    extern SoftwareSerial debug;
  #endif

#endif  /* ARDUINO */

#ifdef MBED
  #include "mbed.h"
  #include "Network.h"
  #include "mqttsn.h"
#endif  /* MBED */

#ifdef LINUX
  #include "Network.h"
  #include "mqttsn.h"
  #include <stdio.h>
  #include <sys/time.h>
  #include <sys/types.h>
  #include <sys/stat.h>
  #include <unistd.h>
  #include <stdlib.h>
  #include <string.h>
  #include <fcntl.h>
  #include <errno.h>
  #include <termios.h>
#endif /* LINUX */


using namespace std;
using namespace tomyClient;

extern uint16_t getUint16(uint8_t* pos);
extern uint32_t getUint32(uint8_t* pos);
extern float    getFloat32(uint8_t* pos);

extern void setUint16(uint8_t* pos, uint16_t val);
extern void setUint32(uint8_t* pos, uint32_t val);
extern void setFloat32(uint8_t* pos, float val);


/*=====================================
        Class MQString
  =====================================*/
MQString::MQString(){
    //_length = 0;
    _constStr = 0;
    _str = 0;
}

MQString::MQString(const char* str){
    if (strlen(str)){
        _constStr = str;
    }else{
        _constStr = 0;
    }
    _str = 0;
}

MQString::MQString(char* str){
    if (strlen(str)){
        _str = str;
    }else{
        _str = 0;
    }
    _constStr = 0;
}

MQString::~MQString(){
    if (_str){
        free(_str);
    }
}

uint16_t MQString::getCharLength(){
    if(_str){
    	return strlen(_str);
    }
    return strlen(_constStr);
}

int MQString::comp(MQString* str){
    if (_str){
    	if(str->getStr()){
    		return (strcmp(_str, str->getStr()));
    	}else if(str->getConstStr()){
    		return (strcmp(_str, str->getConstStr()));
    	}
    }else if(_constStr){
    	if(str->getStr()){
    		return (strcmp(_constStr, str->getStr()));
    	}else if(str->getConstStr()){
    		return (strcmp(_constStr, str->getConstStr()));
    	}
    }
    return 1;
}


int MQString::comp(const char* str){
    if(_str){
        return (strcmp(_str, str));
    }else if (_constStr){
        return (strcmp(_constStr, str));
    }
    return 1;
}


int MQString::ncomp(MQString* str, uint16_t len){
    if (_str){
        return strncmp(_str, str->getStr(), (long)len);
    }else if (_constStr){
        return strncmp(_constStr, str->getConstStr(), (long)len);
    }
    return 1;
}


bool MQString::operator==(MQString &str){
	return (comp(&str) == 0);
}

bool MQString::operator!=(MQString &str){
	return (comp(&str) != 0);
}


void MQString::copy(MQString* str){
    if (str->isConst()){
        _constStr = getConstStr();
    }else{
        _str = str->getStr();
    }
}

MQString* MQString::create(){
    char* newStr = (char*)calloc(getCharLength()+ 1, sizeof(char));
    memcpy(newStr, getConstStr(), getCharLength());
    MQString* newPtr = new MQString((const char*)newStr);
    return newPtr;
}

void MQString::copy(const char* str){
    _constStr = str;
    _str = 0;
    freeStr();
}

void MQString::copy(char* str){
    freeStr();
    _str = (char*)calloc(strlen(str) + 1, sizeof(char));
    _constStr = 0;
    strcpy(_str, str);
}

void MQString::copy(uint8_t* str, uint8_t len){
    freeStr();
    _str = (char*)calloc(len + 1, sizeof(char));
    _constStr = 0;
    memcpy(_str, str, len);
}

void MQString::writeBuf(uint8_t* buf){
    if (_str){
        memcpy(buf, _str, strlen(_str));
    }else if (_constStr){
        memcpy(buf, _constStr, strlen(_constStr));
    }
}

void MQString::readBuf(uint8_t* buf){
    _str = 0;
    _constStr = (const char*)buf;
}

uint8_t MQString::getChar(uint8_t index){
    if (_str){
        return (index < strlen(_str) ? _str[index]: 0);
    }else if (_constStr){
        return (index < strlen(_constStr) ? _constStr[index]: 0);
    }
    return 0;
}

char* MQString::getStr(){
    return (_str ? _str : 0);
}

const char* MQString::getConstStr(){
    return (_constStr ? _constStr : 0);
}


bool MQString::isConst(){
  return (_constStr ? true : false);
}

void MQString::freeStr(){
    if (_str){
        free(_str);
        _str = 0;
    }
}


/*=====================================
        Class MqttsnMessage
 ======================================*/
MqttsnMessage::MqttsnMessage(){
    _msgBuff = 0;    _flags = 0;
    _flags = 0;
    _length = 0;
    _status = 0;
    _type = 0;
}
MqttsnMessage::~MqttsnMessage(){
    if (_msgBuff != 0){
        delete(_msgBuff);
    }
}

void MqttsnMessage::reset(){
    _msgBuff = 0;
    _length = 0;
    _status = 0;
    _type = 0;
}

void MqttsnMessage::setType(uint8_t type){
    _type = type;
    if ( _msgBuff != 0){
    	if(_length > 255){
    		_msgBuff[3] = type;
    	}else{
    		_msgBuff[1] = type;
    	}
    }
}

void MqttsnMessage::setFlags(uint8_t flg){
    _flags = flg;
    if ( _msgBuff != 0){
    	if(_length > 255){
    		_msgBuff[4] = flg;
    	}else{
    		_msgBuff[2] = flg;
    	}
    }
}

void MqttsnMessage::setLength(uint16_t length){
    _length = length;
    if ( _msgBuff != 0){
    	if(_length > 255){
    		_msgBuff[0] = 0x01;
			setUint16(_msgBuff + 1, _length);
		}else{
			 _msgBuff[0] = length;
		}
    }
}

void MqttsnMessage::setQos(uint8_t qos){
	_flags &= 0x9f;
	if(qos == QOS1){
		_flags |= 0x20;
	}else if(qos == QOS2){
		_flags |= 0x40;
	}
	setFlags(_flags);
}

bool MqttsnMessage::setBody(uint8_t* body){
    if (allocateBody()) {
    	if(_length > 255){
    		_msgBuff[0] = 0x01;
			setUint16(_msgBuff + 1, _length);
			_msgBuff[3] = _type;
			memcpy(_msgBuff + 4, body, _length - 4);
    	}else{
			_msgBuff[0] = _length;
			_msgBuff[1] = _type;
			memcpy(_msgBuff + 2, body, _length - MQTTSN_HEADER_SIZE);
    	}
        return true;
    }else{
        return false;
    }
}

bool MqttsnMessage::allocateBody(){
    if ( _length > 0 ) {
        if (_msgBuff){
              free(_msgBuff);
        }

        _msgBuff = (uint8_t*)calloc(_length + 1, sizeof(uint8_t));
        if ( _msgBuff){
        	if(_length > 255){
        		_msgBuff[0] = 0x01;
        		setUint16(_msgBuff + 1, _length);
        		_msgBuff[3] = _type;
                _msgBuff[4] = _flags;
        	}else{
				_msgBuff[0] = _length;
				_msgBuff[1] = _type;
		        _msgBuff[2] = _flags;
        	}
            return true;
        }else{
            return false;
        }
    }else{
        return false;
    }
}

void MqttsnMessage::setDup(){
	if(_msgBuff && (_type == MQTTSN_TYPE_PUBLISH || _type == MQTTSN_TYPE_SUBSCRIBE)){
		_flags |= 0x80;
		getBody()[0] = _flags ;
	}
}

void MqttsnMessage::setStatus(uint8_t stat){
    _status = stat;
}

uint8_t MqttsnMessage::getQos(){
    return (_flags & (MQTTSN_FLAG_QOS_1 | MQTTSN_FLAG_QOS_2)) >> 5;
}

uint8_t MqttsnMessage::getLength(){
    return _length;
}

uint8_t MqttsnMessage::getType(){
    return _type;
}

uint8_t MqttsnMessage::getFlags(){
    return _flags;
}

uint8_t MqttsnMessage::getStatus(){
    return _status;
}

uint8_t* MqttsnMessage::getBody(){
	if(_length > 255){
		return _msgBuff + 4;
	}else{
		return _msgBuff + MQTTSN_HEADER_SIZE;
	}
}

uint16_t MqttsnMessage::getBodyLength(){
    if(_length > 255){
    	return _length - 4;
    }else{
    	return _length - MQTTSN_HEADER_SIZE;
    }
}

uint16_t MqttsnMessage::getFrameLength(){
    return _length;
}

uint8_t* MqttsnMessage::getMsgBuff(){
    return _msgBuff;
}

void MqttsnMessage::setMsgBuff(uint8_t* msgBuff){
    _msgBuff = msgBuff;
}

bool MqttsnMessage::copy(MqttsnMessage* src){
    setLength(src->getLength());
    setType(src->getType());
    setFlags(src->getFlags());
    setStatus(src->getStatus());
    _msgBuff = src->_msgBuff;
    src->setMsgBuff(0);
    if (_msgBuff == 0){
        return false;
    }
    return true;
}

const char* MqttsnMessage::getMsgTypeName(){
	switch(_type){
	case 0x00:
		return "ADVERTIZE";
	case 0x01:
		return "SEARCGW";
	case 0x02:
		return "GWINFO";
	case 0x04:
		return "CONNECT";
	case 0x05:
		return "CONNACK";
	case 0x06:
		return "WILLTOPICREQ";
	case 0x07:
		return "WILLTOPIC";
	case 0x08:
		return "WILLMSGREQ";
	case 0x09:
		return "WILLMSG";
	case 0x0a:
		return "REGISTER";
	case 0x0b:
		return "REGACK";
	case 0x0c:
		return "PUBLISH";
	case 0x0d:
		return "PUBACK";
	case 0x0e:
		return "PUBCOMP";
	case 0x0f:
		return "PUBREC";
	case 0x10:
		return "PUBREL";
	case 0x12:
		return "SUBSCRIBE";
	case 0x13:
		return "SUBACK";
	case 0x14:
		return "UNSUBSCRIBE";
	case 0x15:
		return "UNSUBACK";
	case 0x16:
		return "PINGREQ";
	case 0x17:
		return "PINGRESP";
	case 0x18:
		return "DISCONNECT";
	default:
		return "";
	}
}


/*=====================================
        Class MqttsnAdvrtise
 ======================================*/
MqttsnAdvertise::MqttsnAdvertise():MqttsnMessage(){
    setLength(5);
    setType(MQTTSN_TYPE_ADVERTISE);
    allocateBody();
}

MqttsnAdvertise::~MqttsnAdvertise(){

}

void MqttsnAdvertise::setGwId(uint8_t id){
    getBody()[0] = id;
}

void MqttsnAdvertise::MqttsnAdvertise::setDuration(uint16_t duration){
    uint8_t* pos = getBody() + 1;
    setUint16(pos, duration);
}

uint8_t MqttsnAdvertise::getGwId(){
    return getBody()[0];
}

uint16_t MqttsnAdvertise::getDuration(){
  uint8_t* pos = getBody() + 1;
    return getUint16(pos);
}

/*=====================================
        Class MqttsnSearchgw
 ======================================*/
MqttsnSearchGw::MqttsnSearchGw():MqttsnMessage(){
    setLength(3);
    setType(MQTTSN_TYPE_SEARCHGW);
    allocateBody();
}

MqttsnSearchGw::~MqttsnSearchGw(){

}

void MqttsnSearchGw::setRadius(uint8_t radius){
  getBody()[0] = radius;
}

uint8_t MqttsnSearchGw::getRadius(){
  return getBody()[0];
}

/*=====================================
        Class MqttsnGwinfo
 ======================================*/
MqttsnGwInfo::MqttsnGwInfo():MqttsnMessage(){
  setLength(3);
  setType(MQTTSN_TYPE_GWINFO);
  allocateBody();
}

MqttsnGwInfo::~MqttsnGwInfo(){

}

uint8_t MqttsnGwInfo::getGwId(){
    return getBody()[0];
}

void MqttsnGwInfo::setGwId(uint8_t id){
    getBody()[0] = id;
}

/*=====================================
         Class MqttsnConnect
  ======================================*/
MqttsnConnect::MqttsnConnect(MQString* id):MqttsnMessage(){
    setLength(id->getCharLength() + 6);
    allocateBody();
    setType(MQTTSN_TYPE_CONNECT);
    getBody()[1] = MQTTSN_PROTOCOL_ID;
    id->writeBuf(getBody() + 4);
}

MqttsnConnect::~MqttsnConnect(){

}

void MqttsnConnect::setDuration(uint16_t msec){
    setUint16((uint8_t*)getBody() + 2, msec);
}

uint16_t MqttsnConnect::getDuration(){
    return getUint16((uint8_t*)getBody() + 2);
}

void MqttsnConnect::setClientId(MQString* id){
    id->writeBuf(getBody() + 4);
    setLength(id->getCharLength() + 6);
}

uint8_t* MqttsnConnect::getClientId(){
    return getBody() + 6;
}

void MqttsnConnect::setFrame(uint8_t* data, uint8_t len){
    setLength(len + MQTTSN_HEADER_SIZE);
    allocateBody();
    memcpy(getBody(), data, len);
    getBody()[2] = getUint16(data + 4);
    getBody()[0] = *(data + 2);
}

/*=====================================
        Class MqttsnConnack
 ======================================*/
MqttsnConnack::MqttsnConnack():MqttsnMessage(){
    setLength(3);
    setType(MQTTSN_TYPE_CONNACK);
    allocateBody();
}

MqttsnConnack::~MqttsnConnack(){

}

void MqttsnConnack::setReturnCode(uint8_t rc){
    getBody()[0] = rc;
}

uint8_t MqttsnConnack::getReturnCode(){
    return getBody()[0];
}

/*=====================================
       Class MqttsnWillTopicReq
======================================*/
MqttsnWillTopicReq::MqttsnWillTopicReq():MqttsnMessage(){
    setLength(2);
    setType(MQTTSN_TYPE_WILLTOPICREQ);
    allocateBody();
}

MqttsnWillTopicReq::~MqttsnWillTopicReq(){

}

/*=====================================
         Class MqttsnWillTopic
  ======================================*/
MqttsnWillTopic::MqttsnWillTopic():MqttsnMessage(){
    setLength(3);
    setType(MQTTSN_TYPE_WILLTOPIC);
    allocateBody();
    _flags = 0;
}

MqttsnWillTopic::~MqttsnWillTopic(){

}


void MqttsnWillTopic::setWillTopic(MQString* topic){
    setLength(topic->getCharLength() + 3);
    allocateBody();
    topic->writeBuf(getBody() + 1);
    _msgBuff[2] = _flags;
    _ustring.copy(topic);
}

MQString* MqttsnWillTopic::getWillTopic(){
  if (_msgBuff){
        return &_ustring;
    }else{
        return 0;
    }
}

bool MqttsnWillTopic::isWillRequired(){
    return getBody()[0] && MQTTSN_FLAG_WILL;
}

/*=====================================
         Class MqttsnWillMsgReq
  ======================================*/
MqttsnWillMsgReq::MqttsnWillMsgReq():MqttsnMessage(){
    setLength(2);
    setType(MQTTSN_TYPE_WILLMSGREQ);
    allocateBody();

}

MqttsnWillMsgReq::~MqttsnWillMsgReq(){

}

/*=====================================
         Class MqttsnWillMsg
  ======================================*/
MqttsnWillMsg::MqttsnWillMsg():MqttsnMessage(){
    setLength(2);
    setType(MQTTSN_TYPE_WILLMSG);
    allocateBody();
}

MqttsnWillMsg::~MqttsnWillMsg(){

}

void MqttsnWillMsg::setWillMsg(MQString* msg){
    setLength(2 + msg->getCharLength());
    allocateBody();
    msg->writeBuf(getBody());
}

char* MqttsnWillMsg::getWillMsg(){
    if (_msgBuff){
            return (char*)getBody();
    }else{
            return 0;
    }
}

/*=====================================
         Class MqttsnRegister
  ======================================*/
MqttsnRegister::MqttsnRegister():MqttsnMessage(){
    setLength(6);
    setType(MQTTSN_TYPE_REGISTER);
    allocateBody();
    _topicId = 0;
    _msgId = 0;
}

MqttsnRegister::~MqttsnRegister(){

}

void MqttsnRegister::setTopicId(uint16_t topicId){
  if (_msgBuff){
            setUint16(getBody(), topicId);
    }
    _topicId = topicId;
}
uint16_t MqttsnRegister::getTopicId(){
    return _topicId;
}
void MqttsnRegister::setMsgId(uint16_t msgId){
    if (_msgBuff){
            setUint16(getBody() + 2, msgId);
    }
    _msgId = msgId;
}
uint16_t MqttsnRegister::getMsgId(){
    return _msgId;

}
void MqttsnRegister::setTopicName(MQString* topicName){
    setLength(6 + topicName->getCharLength());
    allocateBody();
    topicName->writeBuf(getBody() + 4);
    setTopicId(_topicId);
    setMsgId(_msgId);
}

void MqttsnRegister::setFrame(uint8_t* data, uint8_t len){
    setLength(len + MQTTSN_HEADER_SIZE);
    allocateBody();
    memcpy(getBody(), data, len);
    _topicId = getUint16(data);
    _msgId = getUint16(data + 2);
    _ustring.readBuf(getBody() + 4);
}

void MqttsnRegister::setFrame(NWResponse* resp){
    setFrame(resp->getPayload() + MQTTSN_HEADER_SIZE, resp->getPayload(0) - MQTTSN_HEADER_SIZE);
}

MQString* MqttsnRegister::getTopicName(){
    return &_ustring;
}

/*=====================================
         Class MqttsnRegAck
  ======================================*/
MqttsnRegAck::MqttsnRegAck():MqttsnMessage(){
    setLength(7);
    setType(MQTTSN_TYPE_REGACK);
    allocateBody();
}
MqttsnRegAck::~MqttsnRegAck(){

}
void MqttsnRegAck::setTopicId(uint16_t topicId){
    setUint16((uint8_t*)getBody(), topicId);
}
uint16_t MqttsnRegAck::getTopicId(){
    return getUint16((unsigned char*)getBody());
}
void MqttsnRegAck::setMsgId(uint16_t msgId){
    setUint16(getBody()+ 2,msgId);
}
uint16_t MqttsnRegAck::getMsgId(){
    return getUint16((unsigned char*)getBody()+ 2);
}
void MqttsnRegAck::setReturnCode(uint8_t rc){
    getBody()[4] = rc;
}
uint8_t MqttsnRegAck::getReturnCode(){
    return (uint8_t)getBody()[4];
}

/*=====================================
         Class MqttsnPublish
  ======================================*/
MqttsnPublish::MqttsnPublish():MqttsnMessage(){
    setLength(7);
    setType(MQTTSN_TYPE_PUBLISH);
    allocateBody();
    _topicId = 0;
    _topic = 0;
    _msgId = 0;
}

MqttsnPublish::~MqttsnPublish(){

}

void MqttsnPublish::setTopicId(uint16_t id){
    setUint16((uint8_t*)(getBody() + 1), id);
    _topicId = id;
}

uint16_t MqttsnPublish::getTopicId(){
    return _topicId;
}

MQString* MqttsnPublish::getTopic(MQString* topic){
	char tp[3];
	tp[0] = (char)*(getBody() + 1);
	tp[1] = (char)*(getBody() + 2);
	tp[2] = 0;
	topic->copy(tp);
    return topic;
}

void MqttsnPublish::setTopic(MQString* topic){
	if(topic->getCharLength() == 2){
		topic->writeBuf((uint8_t*)(getBody() + 1));
		memcpy((void*)&_topicId,(getBody() + 1), 2);
		_flags &= 0xfc;
		_flags |= MQTTSN_TOPIC_TYPE_SHORT;
	}
}

void MqttsnPublish::setMsgId(uint16_t msgId){
    setUint16((uint8_t*)(getBody() + 3), msgId);
    _msgId = msgId;
}

uint16_t MqttsnPublish::getMsgId(){
    return _msgId;
}


void MqttsnPublish::setData(uint8_t* data, uint16_t len){
    setLength(7 + len);
    allocateBody();
    memcpy(getBody() + 5, data, len);
    setTopicId(_topicId);
    setMsgId(_msgId);
}

void MqttsnPublish::setData(MQString* str){
	setLength(7 + str->getCharLength());
	allocateBody();
	setTopicId(_topicId);
	setMsgId(_msgId);
	str->writeBuf(getBody() + 5);
}

uint8_t*  MqttsnPublish::getData(){
    return (uint8_t*)(getBody() + 5);
}

uint16_t  MqttsnPublish::getDataLength(){
    return  this->getBodyLength() -  5;
}

void MqttsnPublish::setFrame(uint8_t* data, uint16_t len){
	if(len > 255){
		setLength(len + MQTTSN_HEADER_SIZE + 2);
	}else{
		setLength(len + MQTTSN_HEADER_SIZE);
	}
    allocateBody();
    memcpy(getBody(), data, len);
    _topicId = getUint16(data + 1);
    _msgId = getUint16(data + 3);
    _flags = *data;
}

void MqttsnPublish::setFrame(NWResponse* resp){
	setFrame(resp->getBody(), resp->getBodyLength());
}

void MqttsnPublish::setPayload(Payload* payload){

}
/*=====================================
         Class MqttsnPubAck
 ======================================*/
MqttsnPubAck::MqttsnPubAck():MqttsnMessage(){
    setLength(7);
    setType(MQTTSN_TYPE_PUBACK);
    allocateBody();
}
MqttsnPubAck::~MqttsnPubAck(){

}
void MqttsnPubAck::setTopicId(uint16_t topicId){
    setUint16((uint8_t*)getBody(), topicId);
}
uint16_t MqttsnPubAck::getTopicId(){
    return getUint16((unsigned char*)getBody());
}
void MqttsnPubAck::setMsgId(uint16_t msgId){
    setUint16(getBody()+ 2,msgId);
}
uint16_t MqttsnPubAck::getMsgId(){
    return getUint16((unsigned char*)getBody()+ 2);
}
void MqttsnPubAck::setReturnCode(uint8_t rc){
    getBody()[4] = rc;
}
uint8_t MqttsnPubAck::getReturnCode(){
    return (uint8_t)getBody()[4];
}

/*=====================================
         Class MqttsnPubRec
 ======================================*/
MqttsnPubRec::MqttsnPubRec(){
    setLength(4);
    setType(MQTTSN_TYPE_PUBREC);
    allocateBody();
}
MqttsnPubRec::~MqttsnPubRec(){

}

uint16_t MqttsnPubRec::getMsgId(){
    return getUint16(getBody());
}

void MqttsnPubRec::setMsgId(uint16_t msgId){
    setUint16(getBody(),msgId);
}

/*=====================================
         Class MqttsnPubRel
 ======================================*/
MqttsnPubRel::MqttsnPubRel(){
    setLength(4);
    setType(MQTTSN_TYPE_PUBREL);
    allocateBody();
}

MqttsnPubRel::~MqttsnPubRel(){

}

/*=====================================
         Class MqttsnPubComp
 ======================================*/
MqttsnPubComp::MqttsnPubComp(){
    setLength(4);
    setType(MQTTSN_TYPE_PUBCOMP);
    allocateBody();
}

MqttsnPubComp::~MqttsnPubComp(){

}

 /*=====================================
         Class MqttsnSubscribe
  ======================================*/
MqttsnSubscribe::MqttsnSubscribe(){
    setLength(5);
    setType(MQTTSN_TYPE_SUBSCRIBE);
    allocateBody();
    _topicId = 0;
    _msgId = 0;
    _flags = 0;
}

MqttsnSubscribe::~MqttsnSubscribe(){

}

void MqttsnSubscribe::setTopicId(uint16_t predefinedId){
    setLength(7);
    allocateBody();
    setMsgId(_msgId);
    setUint16((uint8_t*)(getBody() + 3), predefinedId);
    setFlags(_flags | MQTTSN_TOPIC_TYPE_PREDEFINED);
    _topicId = predefinedId;
}

uint16_t MqttsnSubscribe::getTopicId(){
    if (_msgBuff){
        _topicId = getUint16(getBody() +3);
    }
    return _topicId;
}
void MqttsnSubscribe::setMsgId(uint16_t msgId){
    _msgId = msgId;
    if (_msgBuff){
       setUint16((uint8_t*)(getBody() + 1), msgId);
    }
}

uint16_t MqttsnSubscribe::getMsgId(){
    if (_msgBuff){
        _msgId = getUint16(getBody() + 1);
    }
    return _msgId;
}

void MqttsnSubscribe::setTopicName(MQString* data){
    setLength(5 + data->getCharLength());
    allocateBody();
    data->writeBuf(getBody() + 3);
    setMsgId(_msgId);
    setFlags((_flags & 0xe0) | MQTTSN_TOPIC_TYPE_NORMAL);
    _ustring.copy(data);
}


MQString*  MqttsnSubscribe::getTopicName(){
    return &_ustring;
}

void MqttsnSubscribe::setFrame(uint8_t* data, uint8_t len){
    setLength(len + MQTTSN_HEADER_SIZE);
    allocateBody();
    memcpy(getBody(), data, len);
    _msgId = getUint16(data + 1);
    _flags = *data;
    if ((_flags & MQTTSN_TOPIC_TYPE) == MQTTSN_TOPIC_TYPE_PREDEFINED){
    	_topicId = getUint16(data + 3);
    }else{
    	_topicId = 0;
		_ustring.readBuf(data + 3);
    }
}

void MqttsnSubscribe::setFrame(NWResponse* resp){
	setFrame(resp->getPayload() + MQTTSN_HEADER_SIZE, resp->getPayload(0) - MQTTSN_HEADER_SIZE);
}

/*=====================================
         Class MqttsnSubAck
  ======================================*/
MqttsnSubAck::MqttsnSubAck():MqttsnMessage(){
    setLength(8);
    setType(MQTTSN_TYPE_SUBACK);
    allocateBody();
}

MqttsnSubAck::~MqttsnSubAck(){

}

void MqttsnSubAck::setTopicId(uint16_t id){
    setUint16((uint8_t*)(getBody() + 1), id);
}

uint16_t MqttsnSubAck::getTopicId(){
    return getUint16(getBody() + 1);
}
void MqttsnSubAck::setMsgId(uint16_t msgId){
   setUint16((uint8_t*)(getBody() + 3), msgId);
}

uint16_t MqttsnSubAck::getMsgId(){
    return getUint16(getBody() + 3);
}
void MqttsnSubAck::setReturnCode(uint8_t rc){
    getBody()[6] = rc;
}
uint8_t  MqttsnSubAck::getReturnCode(){
    return getBody()[5];
}


 /*=====================================
         Class MqttsnUnsubscribe
  ======================================*/
MqttsnUnsubscribe::MqttsnUnsubscribe() : MqttsnSubscribe(){
    setType(MQTTSN_TYPE_UNSUBSCRIBE);
}
MqttsnUnsubscribe::~MqttsnUnsubscribe(){

}

/*=====================================
         Class MqttsnUnSubAck
  ======================================*/
MqttsnUnSubAck::MqttsnUnSubAck():MqttsnMessage(){
    setLength(4);
    setType(MQTTSN_TYPE_UNSUBACK);
    allocateBody();
}

MqttsnUnSubAck::~MqttsnUnSubAck(){

}

void MqttsnUnSubAck::setMsgId(uint16_t msgId){
    setUint16((uint8_t*)getBody(), msgId);
}

uint16_t MqttsnUnSubAck::getMsgId(){
    return getUint16(getBody());
}

/*=====================================
        Class MqttsnPingReq
 ======================================*/
MqttsnPingReq::MqttsnPingReq(MQString* id):MqttsnMessage(){
  setLength(id->getCharLength() + 2);
  setType(MQTTSN_TYPE_PINGREQ);
  allocateBody();
  id->writeBuf(getBody());

}
MqttsnPingReq::~MqttsnPingReq(){

}

char* MqttsnPingReq::getClientId(){
    return (char*)getBody();
}

/*=====================================
        Class MqttsnPingResp
 ======================================*/
MqttsnPingResp::MqttsnPingResp():MqttsnMessage(){
    setLength(2);
    setType(MQTTSN_TYPE_PINGRESP);
    allocateBody();
}
MqttsnPingResp::~MqttsnPingResp(){

}

 /*=====================================
         Class MqttsnDisconnect
  ======================================*/
MqttsnDisconnect::MqttsnDisconnect():MqttsnMessage(){
    setLength(4);
    setType(MQTTSN_TYPE_DISCONNECT);
    allocateBody();

}
MqttsnDisconnect::~MqttsnDisconnect(){

}
void MqttsnDisconnect::setDuration(uint16_t duration){
    setUint16((uint8_t*)getBody(), duration);
}
uint16_t MqttsnDisconnect::getDuration(){
    return getUint16((uint8_t*)getBody());
}


/*=====================================
        Class Topic
 ======================================*/
Topic::Topic(){
    _topicStr = 0;
    _callback = 0;
    _topicId = 0;
    _status = 0;
}

Topic::~Topic(){
    if (_topicStr){
        delete _topicStr;
    }
}

uint8_t Topic::getStatus(){
    return _status;
}

uint16_t Topic::getTopicId(){
    return _topicId;
}

MQString* Topic::getTopicName(){
    return _topicStr;
}

uint8_t Topic::getTopicLength(){
    return _topicStr->getCharLength();
}

TopicCallback Topic::getCallback(){
    return _callback;
}

void Topic::setTopicId(uint16_t id){
    _topicId = id;
}

void Topic::setStatus(uint8_t stat){
    _topicId = stat;
}


void Topic::setTopicName(MQString* topic){
    _topicStr = topic;
}

void Topic::setCallback(TopicCallback callback){
    _callback = callback;
}

int Topic::execCallback(MqttsnPublish* msg){
    if(_callback != 0){
        return _callback(msg);
    }
    return 0;
}

void Topic::copy(Topic* src){
    setTopicId(src->getTopicId());
    setStatus(src->getStatus());
    setCallback(src->getCallback());
    setCallback(_callback);
    _topicStr = src->getTopicName();
}

uint8_t Topic::isWildCard(){
    if (getTopicName()->getChar(getTopicName()->getCharLength() - 1) == MQTTSN_TOPIC_SINGLE_WILDCARD){
        return MQTTSN_TOPIC_SINGLE_WILDCARD;
    }else if (getTopicName()->getChar(getTopicName()->getCharLength() - 1) == MQTTSN_TOPIC_MULTI_WILDCARD){
        return MQTTSN_TOPIC_MULTI_WILDCARD;
    }
    return 0;
}

bool Topic::isMatch(Topic* wildCard){
    uint8_t pos = wildCard->getTopicName()->getCharLength() - 1;
    if (wildCard->isWildCard() == MQTTSN_TOPIC_SINGLE_WILDCARD &&
        getTopicName()->ncomp(wildCard->getTopicName(), (long)pos) == 0 ){
        for(; pos < getTopicName()->getCharLength(); pos++){
            if (getTopicName()->getChar(pos) == '/'){
                return false;
            }
        }
        return true;
    }else if(wildCard->isWildCard() == MQTTSN_TOPIC_MULTI_WILDCARD &&
        getTopicName()->ncomp(wildCard->getTopicName(), (long)pos) == 0 ){
        return true;
    }
    return false;
}

/*=====================================
        Class Topics
 ======================================*/
Topics::Topics(){
    _sizeMax = 0;
    _elmCnt = 0;
    _topics = 0;
}

Topics::~Topics() {

}

bool Topics::allocate(uint8_t size){
    _elmCnt = 0;
      _topics = (Topic*)calloc(size, sizeof(Topic));
    if (_topics == 0){
        _sizeMax = 0;
        return false;
    }else{
        _sizeMax = size;
        return true;
    }
}


uint16_t Topics::getTopicId(MQString* topic){
    Topic *p = getTopic(topic);
    if ( p != 0) {
        return p->getTopicId();
    }
    return 0;
}


Topic* Topics::getTopic(MQString* topic) {
    for (int i = 0; i < _elmCnt; i++) {
		if ( topic->comp(_topics[i].getTopicName()) == 0) {
            return &_topics[i];
        }
    }
    return 0;
}

Topic* Topics::getTopic(uint16_t id) {
    for (int i = 0; i < _elmCnt; i++) {
        if ( _topics[i].getTopicId() == id) {
            return &_topics[i];
        }
    }
      return 0;
}

bool Topics::setTopicId(MQString* topic, uint16_t id){
    Topic* p = getTopic(topic);
    if ( p != 0) {
        p->setTopicId(id);
        return true;
    }else{
        return false;
    }
}

bool Topics::setCallback(MQString* topic, TopicCallback callback){
    Topic* p = getTopic(topic);
    if ( p != 0) {
        p->setCallback(callback);
        return true;
    }else{
        return false;
    }
}
bool Topics::setCallback(uint16_t topicId, TopicCallback callback){
    Topic* p = getTopic(topicId);
    if ( p != 0) {
        p->setCallback(callback);
        return true;
    }else{
        return false;
    }
}
int Topics::execCallback(uint16_t topicId, MqttsnPublish* msg){
    Topic* p = getTopic(topicId);
    if ( p != 0) {
        return p->execCallback(msg);
    }
    return 0;
}

int Topics::execCallback(MQString* topic, MqttsnPublish* msg){
    Topic* p = getTopic(topic);
    if ( p != 0) {
        return p->execCallback(msg);
    }
    return 0;
}

void Topics::addTopic(MQString* topic){
    if (getTopic(topic) == 0){
        if ( _elmCnt < _sizeMax){
            _topics[_elmCnt].setTopicName(topic);
            _elmCnt++;
        }else{
            Topic* saveTopics = _topics;
            Topic* newTopics = (Topic*)calloc(_sizeMax += MQTTSN_MAX_TOPICS + 1, sizeof(Topic));
            if (newTopics != 0){
                _topics = newTopics;
                for(int i = 0; i < _elmCnt; i++){
                    _topics[i].copy(&saveTopics[i]);
                    saveTopics[i].setTopicName((MQString*)0);
                }

                _topics[_elmCnt].setTopicName(topic);
                _elmCnt++;
                if (saveTopics){
                    delete saveTopics;
                }
            }
        }
    }
}

Topic* Topics::match(MQString* topic){
    for ( int i = 0; i< _elmCnt; i++){
        if (_topics[i].isWildCard()){
            if (getTopic(topic)->isMatch(&_topics[i])){
               return &_topics[i];
            }
        }
    }
    return 0;
}

void Topics::setSize(uint8_t size){
    _sizeMax = size;
}

void Topics::clearTopic(void){
	for( int i = 0; i < _elmCnt; i++){
		if ( _topics[i].getTopicId() >= MQTTSN_TOPICID_NORMAL) {
			_elmCnt = i + 1;
			break;
		}
	}
}


/*=====================================
        Class Payload
  =====================================*/
Payload::Payload(){
	_buff = _pos = 0;
	_len = 0;
	_elmCnt = 0;
	_memDlt = 0;
}

Payload::Payload(uint16_t len){
	_buff = (uint8_t*)calloc(len, sizeof(uint8_t));
	if(_buff == 0){
		exit(-1);
	}
	_pos = _buff;
	_elmCnt = 0;
	_len = len;
	_memDlt = 1;
}

Payload::~Payload(){
	if(_memDlt){
		free(_buff);
	}
}

void Payload::init(){
	_pos = _buff;
	_elmCnt = 0;
}


void Payload::getPayload(MqttsnPublish* msg){
	if(_memDlt){
			free(_buff);
			_memDlt = 0;
	}
	_buff = msg->getData();
	_len = msg->getDataLength();
	_pos = _buff + _len;
	/*
	for(uint8_t i; i < MSGPACK_MAX_ELEMENTS; i++){
		if(getBufferPos(i) != 0){
			_elmCnt++;
		}else{
			break;
		}
	}
	*/
	_elmCnt = 6;
}

uint16_t Payload::getAvailableLength(){
	return _len - (_pos - _buff);
}

uint16_t Payload::getLen(){
	return _pos - _buff;
}

uint8_t* Payload::getBuf(){
	return _buff;
}

/*======================
 *     setter
 ======================*/
int8_t Payload::set_uint32(uint32_t val){
	if(getAvailableLength() < 6){
		return -1;
	}
	if(val < 128){
		*_pos++ = (uint8_t)val;
	}else if(val < 256){
		*_pos++ = MSGPACK_UINT8;
		*_pos++ = (uint8_t)val;
	}else if(val < 65536){
		*_pos++ = MSGPACK_UINT16;
		setUint16(_pos,(uint16_t) val);
		_pos += 2;
	}else{
		*_pos++ = MSGPACK_UINT32;
		setUint32(_pos, val);
		_pos += 4;
	}
	_elmCnt++;
	return 0;
}

int8_t Payload::set_int32(int32_t val){
	if(getAvailableLength() < 6){
			return -1;
	}
	if((val > -32) && (val < 0)){
		*_pos++ = val | MSGPACK_NEGINT;
	}else if((val >= 0) && (val < 128)){
		*_pos++ = val;
	}else if(val > -128 && val < 128){
		*_pos++ = MSGPACK_INT8;
		*_pos++ = (uint8_t)val;
	}else if(val > -32768 && val < 32768){
		*_pos++ = MSGPACK_INT16;
		setUint16(_pos, (uint16_t)val);
		_pos += 2;
	}else{
		*_pos++ = MSGPACK_INT32;
		setUint32(_pos, (uint32_t)val);
		_pos += 4;
	}
	_elmCnt++;
	return 0;
}

int8_t Payload::set_float(float val){
	if(getAvailableLength() < 6){
			return -1;
	}
	*_pos++ = MSGPACK_FLOAT32;
	setFloat32(_pos, val);
	_pos += 4;
	_elmCnt++;
	return 0;
}

int8_t Payload::set_str(char* val){
	return set_str((const char*) val);
}

int8_t Payload::set_str(const char* val){
	if(getAvailableLength() < strlen(val) + 3){
		return -1;
	}else if(strlen(val) < 32){
		*_pos++ = (uint8_t)strlen(val) | MSGPACK_FIXSTR;
	}else if(strlen(val) < 256){
		*_pos++ = MSGPACK_STR8;
		*_pos++ = (uint8_t)strlen(val);
	}else if(strlen(val) < 65536){
		*_pos++ = MSGPACK_STR16;
		setUint16(_pos, (uint16_t)strlen(val));
		_pos += 2;
	}
	memcpy(_pos, val, strlen(val));
	_pos += strlen(val);
	return 0;
}

int8_t Payload::set_array(uint8_t val){
	if(getAvailableLength() < (uint16_t)val+ 1){
		return -1;
	}
	if(val < 16){
		*_pos++ = MSGPACK_ARRAY15 | val;
	}else{
		*_pos++ = MSGPACK_ARRAY16;
		setUint16(_pos,(uint16_t)val);
		_pos += 2;
	}
	_elmCnt++;
	return 0;
}

/*======================
 *     getter
 ======================*/
uint8_t Payload::getArray(uint8_t index){
	uint8_t rc = 0;
	uint8_t* val = getBufferPos(index);
	if(val != 0){
		if(*val == MSGPACK_ARRAY15){
			rc = *val & 0x0F;
		}else if(*val == MSGPACK_ARRAY16){
			rc = (uint8_t)getUint16(val + 1);
		}
	}
	return rc;
}

uint32_t Payload::get_uint32(uint8_t index){
	uint32_t rc = 0;
	uint8_t* val = getBufferPos(index);
	if(val != 0){
		if(*val == MSGPACK_UINT32){
			rc = getUint32(val + 1);
		}else if(*val == MSGPACK_UINT16){
			rc = (uint32_t)getUint16(val + 1);
		}else if(*val == MSGPACK_UINT8){
			rc = (uint32_t)*(val + 1);
		}else if(*val < 128){
			rc = (uint32_t)*val;
		}
	}
	return rc;
}

int32_t Payload::get_int32(uint8_t index){
	int32_t rc = 0;
	uint8_t* val = getBufferPos(index);
	if(val != 0){
		if(*val == MSGPACK_INT32){
			rc = (int32_t) getUint32(val + 1);
		}else if(*val == MSGPACK_INT16){
			uint16_t d16 = getUint16(val + 1);
			if(d16 >= 32768){
				rc = d16 - 65536;
			}else{
				rc = (int32_t)d16;
			}
		}else if(*val == MSGPACK_INT8){
			rc = (int32_t)*(val + 1);
		}else if((*val & MSGPACK_NEGINT) == MSGPACK_NEGINT){
			*val &= ~MSGPACK_NEGINT;
			rc = ((int32_t)*val) * -1;
		}else{
			rc = (int32_t) *val;
		}
	}
	return rc;
}

float Payload::get_float(uint8_t index){
	uint8_t* val = getBufferPos(index);
	if(val != 0){
		if(*val == MSGPACK_FLOAT32){
			return getFloat32(val + 1);
		}
	}
	return 0;
}

const char* Payload::get_str(uint8_t index, uint16_t* len){
	uint8_t* val = getBufferPos(index);
	if(val != 0){
		if(*val == MSGPACK_STR16){
			*len = getUint16(val + 1);
			return (const char*)(val + 3);
		}else if(*val == MSGPACK_STR8){
			*len = *(val + 1);
			return (const char*)(val + 2);
		}else if(*val & MSGPACK_FIXSTR){
			*len = *val & (~MSGPACK_FIXSTR);
			return (const char*)(val + 1);
		}
	}
	*len = 0;
	return (const char*) 0;

}


uint8_t* Payload::getBufferPos(uint8_t index){
	uint8_t* bpos = 0;
	uint8_t* pos = _buff;

	for(uint8_t i = 0; i <= index; i++){
		bpos = pos;
		switch(*pos){
		case MSGPACK_FALSE:
		case MSGPACK_TRUE:
			pos++;
			break;
		case MSGPACK_UINT8:
		case MSGPACK_INT8:
			pos += 2;
			break;
		case MSGPACK_UINT16:
		case MSGPACK_INT16:
		case MSGPACK_ARRAY16:
			pos += 3;
			break;
		case MSGPACK_UINT32:
		case MSGPACK_INT32:
		case MSGPACK_FLOAT32:
			pos += 5;
			break;
		case MSGPACK_STR8:
			pos += *(pos + 1) + 2;
			break;
		case MSGPACK_STR16:
			pos += getUint16(pos + 1) + 3;
			break;
		default:
			if((*pos < MSGPACK_POSINT) ||
				((*pos & MSGPACK_NEGINT) == MSGPACK_NEGINT) ||
				((*pos & MSGPACK_ARRAY15) == MSGPACK_ARRAY15)) {
				pos++;
			}else if((*pos & MSGPACK_FIXSTR) == MSGPACK_FIXSTR){
				pos += *pos & (~MSGPACK_FIXSTR);
			}
		}
		/*
		if((pos - _buff) >= _len){
			return 0;
		}
		*/
	}
	return bpos;
}

void Payload::print(){
	for(uint8_t* pos = _buff; pos < _pos; pos++){
		printf(" 0x%x", *pos);
	}
	printf("\n");
}

/*=====================================
        Class PublishHandller
 ======================================*/
PublishHandller::PublishHandller(){

}
PublishHandller::~PublishHandller(){

}
int PublishHandller::exec(MqttsnPublish* msg, Topics* topics){
	MQString tp = MQString();
	if((msg->getFlags() && MQTTSN_TOPIC_TYPE) == MQTTSN_TOPIC_TYPE_SHORT){
		if (topics->getTopic(msg->getTopic(&tp))){
			return topics->getTopic(msg->getTopic(&tp))->execCallback(msg);
		}
	}else{
		if (topics->getTopic(msg->getTopicId())){
			return topics->getTopic(msg->getTopicId())->execCallback(msg);
		}
	}
    return 0;
}



/////////////////// End of File ///////////////


================================================
FILE: Client/src/lib/mqttsn.h
================================================
/*
 * mqttsn.h
 *
 *                      The BSD License
 *
 *           Copyright (c) 2014, tomoaki@tomy-tech.com
 *                    All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: 2014/06/01
 *    Modified: 2014/09/05
 *      Author: Tomoaki YAMAGUCHI
 *     Version: 1.0.0
 */

#ifndef MQTTS_H_
#define MQTTS_H_

#ifdef ARDUINO
        #include <MQTTSN_Application.h>
#else
        #include "MQTTSN_Application.h"
#endif

#if defined(ARDUINO) && ARDUINO >= 100
        #include "Arduino.h"
        #include <inttypes.h>
        #include <Network.h>
#endif

#if defined(ARDUINO) && ARDUINO < 100
        #include "WProgram.h"
        #include <inttypes.h>
        #include <Network.h>
#endif

#ifdef LINUX
        #include <sys/time.h>
        #include <iostream>
        #include "Network.h"
#endif

#ifdef MBED
        #include "mbed.h"
        #include "Network.h"
#endif


                              /* [sec] */
#define MQTTSN_DEFAULT_KEEPALIVE 3600     // 1H
#define MQTTSN_DEFAULT_DURATION   900     // 15min
#define MQTTSN_TIME_SEARCHGW        3
#define MQTTSN_TIME_RETRY          10
#define MQTTSN_TIME_WAIT          300     //  5min
#define MQTTSN_RETRY_COUNT          5

#define MQTTSN_MAX_TOPICS         15
#define MQTTSN_MAX_PACKET_LENGTH  60

#define MQTTSN_TYPE_ADVERTISE     0x00
#define MQTTSN_TYPE_SEARCHGW      0x01
#define MQTTSN_TYPE_GWINFO        0x02
#define MQTTSN_TYPE_CONNECT       0x04
#define MQTTSN_TYPE_CONNACK       0x05
#define MQTTSN_TYPE_WILLTOPICREQ  0x06
#define MQTTSN_TYPE_WILLTOPIC     0x07
#define MQTTSN_TYPE_WILLMSGREQ    0x08
#define MQTTSN_TYPE_WILLMSG       0x09
#define MQTTSN_TYPE_REGISTER      0x0A
#define MQTTSN_TYPE_REGACK        0x0B
#define MQTTSN_TYPE_PUBLISH       0x0C
#define MQTTSN_TYPE_PUBACK        0x0D
#define MQTTSN_TYPE_PUBCOMP       0x0E
#define MQTTSN_TYPE_PUBREC        0x0F
#define MQTTSN_TYPE_PUBREL        0x10
#define MQTTSN_TYPE_SUBSCRIBE     0x12
#define MQTTSN_TYPE_SUBACK        0x13
#define MQTTSN_TYPE_UNSUBSCRIBE   0x14
#define MQTTSN_TYPE_UNSUBACK      0x15
#define MQTTSN_TYPE_PINGREQ       0x16
#define MQTTSN_TYPE_PINGRESP      0x17
#define MQTTSN_TYPE_DISCONNECT    0x18
#define MQTTSN_TYPE_WILLTOPICUPD  0x1A
#define MQTTSN_TYPE_WILLTOPICRESP 0x1B
#define MQTTSN_TYPE_WILLMSGUPD    0x1C
#define MQTTSN_TYPE_WILLMSGRESP   0x1D

#define MQTTSN_TOPIC_TYPE_NORMAL     0x00
#define MQTTSN_TOPIC_TYPE_PREDEFINED 0x01
#define MQTTSN_TOPIC_TYPE_SHORT      0x02
#define MQTTSN_TOPIC_TYPE            0x03

#define MQTTSN_FLAG_DUP     0x80
#define MQTTSN_FLAG_QOS_0   0x0
#define MQTTSN_FLAG_QOS_1   0x20
#define MQTTSN_FLAG_QOS_2   0x40
#define MQTTSN_FLAG_QOS_N1  0xc0
#define MQTTSN_FLAG_RETAIN  0x10
#define MQTTSN_FLAG_WILL    0x08
#define MQTTSN_FLAG_CLEAN   0x04

#define MQTTSN_PROTOCOL_ID  0x01
#define MQTTSN_HEADER_SIZE  2

#define MQTTSN_RC_ACCEPTED                  0x00
#define MQTTSN_RC_REJECTED_CONGESTION       0x01
#define MQTTSN_RC_REJECTED_INVALID_TOPIC_ID 0x02
#define MQTTSN_RC_REJECTED_NOT_SUPPORTED    0x03

#define MQTTSN_MSG_REQUEST     1
#define MQTTSN_MSG_RESEND_REQ  2
#define MQTTSN_MSG_WAIT_ACK    3
#define MQTTSN_MSG_COMPLETE    4
#define MQTTSN_MSG_REJECTED    5


#define MQTTSN_GW_INIT         0
#define MQTTSN_GW_SEARCHING    1
#define MQTTSN_GW_FOUND        2
#define MQTTSN_GW_CONNECTED    3
#define MQTTSN_GW_DISCONNECTED 4
#define MQTTSN_GW_LOST         5

#define MQTTSN_ERR_NO_ERROR            0
#define MQTTSN_ERR_NOT_CONNECTED      -11
#define MQTTSN_ERR_RETRY_OVER         -12
#define MQTTSN_ERR_GATEWAY_LOST       -13
#define MQTTSN_ERR_CANNOT_ADD_REQUEST -14
#define MQTTSN_ERR_NO_TOPICID         -15
#define MQTTSN_ERR_REJECTED           -16
#define MQTTSN_ERR_WAIT_GWINFO        -17
#define MQTTSN_ERR_OUT_OF_MEMORY      -18
#define MQTTSN_ERR_PING_REQUIRED      -19
#define MQTTSN_ERR_ACK_TIMEOUT        -20
#define MQTTSN_ERR_PINGRESP_TIMEOUT   -21
#define MQTTSN_ERR_INVALID_TOPICID    -22
#define MQTTSN_ERR_NO_DATA            -23
#define MQTTSN_ERR_REBOOT_REQUIRED    -24
#define MQTTSN_READ_RESP_ONCE_MORE    -25

#define MQTTSN_TOPIC_MULTI_WILDCARD   '#'
#define MQTTSN_TOPIC_SINGLE_WILDCARD  '+'

#define MQTTSN_TOPICID_NORMAL 256
#define MQTTSN_TOPICID_PREDEFINED_TIME   0x0001
#define MQTTSN_TOPIC_PREDEFINED_TIME     ("$GW/01")

#define QOS0  0
#define QOS1  1
#define QOS2  2

#define MSGPACK_FALSE    0xc2
#define MSGPACK_TRUE     0xc3
#define MSGPACK_POSINT   0x80
#define MSGPACK_NEGINT   0xe0
#define MSGPACK_UINT8    0xcc
#define MSGPACK_UINT16   0xcd
#define MSGPACK_UINT32   0xce
#define MSGPACK_INT8     0xd0
#define MSGPACK_INT16    0xd1
#define MSGPACK_INT32    0xd2
#define MSGPACK_FLOAT32  0xca
#define MSGPACK_FIXSTR   0xa0
#define MSGPACK_STR8     0xd9
#define MSGPACK_STR16    0xda
#define MSGPACK_ARRAY15  0x90
#define MSGPACK_ARRAY16  0xdc
#define MSGPACK_MAX_ELEMENTS   50   // Less than 256

using namespace tomyClient;

/*=====================================
        Class MQString
 =====================================*/
class MQString{
public:
    MQString();
    MQString(const char*);
    MQString(char*);
    ~MQString();
    uint16_t getCharLength();
    int     comp(MQString* str);
    int     comp(const char* str);
    int     ncomp(MQString* str, uint16_t n);
    void    copy(MQString* str);
    void    copy(const char* str);
    void    copy(char* str);
    void    copy(uint8_t* str, uint8_t len);
    MQString* create();
    void    writeBuf(uint8_t* buf);
    void    readBuf(uint8_t* buf);
    uint8_t getChar(uint8_t index);
    char*  getStr();
    const char* getConstStr();
    bool    isConst();
    bool operator==(MQString&);
    bool operator!=(MQString&);
private:
    void    freeStr();
    char*    _str;
    const char* _constStr;
};

/*=====================================
        Class MqttsnMessage
  =====================================*/
class MqttsnMessage {
public:
    MqttsnMessage();
    ~MqttsnMessage();
    void   setLength(uint16_t length);
    void   setType(uint8_t type);
    bool   setBody(uint8_t* body);
    bool   allocateBody();
    void   setStatus(uint8_t stat);
    void   setDup();
    void   setQos(uint8_t qos);
    void   setRetain(bool flg);
    void   setFlags(uint8_t flag);
    uint8_t getLength();
    uint8_t getType();
    uint8_t getFlags();
    uint8_t getStatus();
    uint8_t getQos();
    uint8_t* getBody();
    uint8_t* getMsgBuff();
    uint16_t getFrameLength();
    uint16_t getBodyLength();
    bool    isDup();
    bool    copy(MqttsnMessage* src);
    void    reset();
    void    setMsgBuff(uint8_t* buff);
    const char* getMsgTypeName();
protected:
    uint8_t* _msgBuff;
    uint8_t  _flags;
private:
    uint8_t  _status; // 1:request 2:sending 3:resending 4:waitingAck  5:complite
    uint8_t  _type;
    uint16_t  _length;
};

/*=====================================
        Class MqttsnAdvertize
 ======================================*/
class MqttsnAdvertise : public MqttsnMessage {
public:
  MqttsnAdvertise();
  ~MqttsnAdvertise();
  void setGwId(uint8_t id);
  void setDuration(uint16_t duration);
  uint8_t getGwId();
  uint16_t getDuration();

private:
};

/*=====================================
        Class MqttsnSearchGw
 ======================================*/
class MqttsnSearchGw : public MqttsnMessage {
public:
    MqttsnSearchGw();
  ~MqttsnSearchGw();
  void setRadius(uint8_t radius);
  uint8_t getRadius();

private:
};

/*=====================================
        Class MqttsnGwinfo
 ======================================*/
class MqttsnGwInfo : public MqttsnMessage {
public:
  MqttsnGwInfo();
  ~MqttsnGwInfo();
  void setGwId(uint8_t id);
  uint8_t getGwId();

private:
};

/*=====================================
         Class MqttsnConnect
  ======================================*/
class MqttsnConnect : public MqttsnMessage {
public:
    MqttsnConnect(MQString* id);
    ~MqttsnConnect();
    void setDuration(uint16_t msec);
    void setClientId(MQString* id);
    uint8_t* getClientId();
    uint16_t getDuration();
    void setFrame(uint8_t* data, uint8_t len);
private:
 };

/*=====================================
        Class MqttsnConnack
 ======================================*/
class MqttsnConnack : public MqttsnMessage  {
public:
    MqttsnConnack();
    ~MqttsnConnack();
    void setReturnCode(uint8_t rc);
    uint8_t getReturnCode();

private:

};

/*=====================================
         Class MqttsnWillTopicReq
  ======================================*/
class MqttsnWillTopicReq : public MqttsnMessage  {
public:
    MqttsnWillTopicReq();
    ~MqttsnWillTopicReq();

private:

 };

/*=====================================
         Class MqttsnWillTopic
  ======================================*/
class MqttsnWillTopic : public MqttsnMessage  {
public:
    MqttsnWillTopic();
    ~MqttsnWillTopic();
    void setWillTopic(MQString* topic);
    MQString* getWillTopic();
    bool isWillRequired();

private:
    MQString _ustring;
 };

/*=====================================
         Class MqttsnWillMsgReq
  ======================================*/
class MqttsnWillMsgReq : public MqttsnMessage  {
public:
    MqttsnWillMsgReq();
    ~MqttsnWillMsgReq();

private:

 };

/*=====================================
         Class MqttsnWillMsg
  ======================================*/
class MqttsnWillMsg : public MqttsnMessage  {
public:
    MqttsnWillMsg();
    ~MqttsnWillMsg();
    void setWillMsg(MQString* msg);
    char* getWillMsg();

private:

 };

/*=====================================
         Class MqttsnRegister
  ======================================*/
class MqttsnRegister : public MqttsnMessage  {
public:
    MqttsnRegister();
    ~MqttsnRegister();
    void setTopicId(uint16_t topicId);
    uint16_t getTopicId();
    void setMsgId(uint16_t msgId);
    uint16_t getMsgId();
    void setTopicName(MQString* topicName);
    void setFrame(uint8_t* data, uint8_t len);
    void setFrame(NWResponse* resp);
    MQString* getTopicName();

private:
    uint16_t _topicId;
    uint16_t _msgId;
    MQString _ustring;

 };

/*=====================================
         Class MqttsnRegAck
  ======================================*/
class MqttsnRegAck : public MqttsnMessage  {
public:
    MqttsnRegAck();
    ~MqttsnRegAck();
    void setTopicId(uint16_t topicId);
    uint16_t getTopicId();
    void setMsgId(uint16_t msgId);
    uint16_t getMsgId();
    void setReturnCode(uint8_t rc);
    uint8_t getReturnCode();

private:

 };

 /*=====================================
         Class MqttsnPublish
  ======================================*/
class Payload;

class MqttsnPublish : public MqttsnMessage  {
public:
    MqttsnPublish();
    ~MqttsnPublish();
    void setTopicId(uint16_t id);
    void setTopic(MQString* topic);
    void setMsgId(uint16_t msgId);
    void setData(uint8_t* data, uint16_t len);
    void setData(MQString* str);
    void setFrame(uint8_t* data, uint16_t len);
    void setFrame(NWResponse* resp);
    void setPayload(Payload* payload);
    MQString* getTopic(MQString* topic);
    uint16_t getMsgId();
    uint16_t getTopicId();

    uint8_t* getData();
    uint16_t getDataLength();

private:
    uint16_t _topicId;
    uint16_t _msgId;
    MQString* _topic;
 };

/*=====================================
         Class MqttsnPubAck
  ======================================*/
class MqttsnPubAck : public MqttsnMessage  {
public:
    MqttsnPubAck();
    ~MqttsnPubAck();
    void setTopicId(uint16_t id);
    uint16_t getTopicId();
    void setMsgId(uint16_t msgId);
    uint16_t getMsgId();
    void setReturnCode(uint8_t rc);
    uint8_t getReturnCode();


private:

 };

/*=====================================
         Class MqttsnPubRec
  ======================================*/
class MqttsnPubRec : public MqttsnMessage  {
public:
	MqttsnPubRec();
    ~MqttsnPubRec();
    void setMsgId(uint16_t msgId);
    uint16_t getMsgId();

private:

 };

/*=====================================
         Class MqttsnPubRel
  ======================================*/
class MqttsnPubRel : public MqttsnPubRec  {
public:
	MqttsnPubRel();
    ~MqttsnPubRel();
private:

 };

/*=====================================
         Class MqttsnPubComp
  ======================================*/
class MqttsnPubComp : public MqttsnPubRec  {
public:
	MqttsnPubComp();
    ~MqttsnPubComp();
private:

 };

 /*=====================================
         Class MqttsnSubscribe
  ======================================*/
class MqttsnSubscribe : public MqttsnMessage  {
public:
    MqttsnSubscribe();
    ~MqttsnSubscribe();
    void setMsgId(uint16_t msgId);
    uint16_t getMsgId();
    uint16_t getTopicId();
    MQString* getTopicName();
    void setTopicId(uint16_t predefinedId);
	void setFrame(uint8_t* data, uint8_t len);
	void setFrame(NWResponse* resp);
    void setTopicName(MQString* topicName);
protected:
    uint16_t _topicId;
    uint16_t _msgId;
    MQString _ustring;
 };

/*=====================================
         Class MqttsnSubAck
  ======================================*/
class MqttsnSubAck : public MqttsnMessage  {
public:
    MqttsnSubAck();
    ~MqttsnSubAck();
    void setMsgId(uint16_t msgId);
    uint16_t getMsgId();
    void setTopicId(uint16_t topicId);
    uint16_t getTopicId();
    void setReturnCode(uint8_t rc);
    uint8_t getReturnCode();

private:

 };

 /*=====================================
         Class MqttsnUnsubscribe
  ======================================*/
class MqttsnUnsubscribe : public MqttsnSubscribe  {
public:
    MqttsnUnsubscribe();
    ~MqttsnUnsubscribe();
private:

 };

/*=====================================
         Class MqttsnUnSubAck
  ======================================*/
class MqttsnUnSubAck : public MqttsnMessage  {
public:
    MqttsnUnSubAck();
    ~MqttsnUnSubAck();
    void setMsgId(uint16_t msgId);
    uint16_t getMsgId();

private:

 };

/*=====================================
        Class MqttsnPingReq
 ======================================*/
class MqttsnPingReq : public MqttsnMessage  {
public:
    MqttsnPingReq(MQString* id);
    ~MqttsnPingReq();
    char* getClientId();
private:

};

/*=====================================
        Class MqttsnPingResp
 ======================================*/
class MqttsnPingResp : public MqttsnMessage  {
public:
    MqttsnPingResp();
    ~MqttsnPingResp();
private:

};

 /*=====================================
         Class MqttsnDisconnect
  ======================================*/
class MqttsnDisconnect : public MqttsnMessage  {
public:
    MqttsnDisconnect();
    ~MqttsnDisconnect();
    void setDuration(uint16_t duration);
    uint16_t getDuration();
private:

 };

/*=====================================
        Class Topic
 ======================================*/
typedef int (*TopicCallback)(MqttsnPublish*);

class Topic {
public:
    Topic();
    ~Topic();
    uint8_t   getStatus();
    uint16_t  getTopicId();
    MQString*  getTopicName();
    uint8_t   getTopicLength();
    uint8_t   getTopicType();
    TopicCallback getCallback();
    void     setTopicId(uint16_t id);
    void     setTopicName(MQString* topic);
    void     setStatus(uint8_t stat);
    int      execCallback(MqttsnPublish* msg);
    void     copy(Topic* src);
    void     setCallback(TopicCallback callback);
    uint8_t   isWildCard();
    bool     isMatch(Topic* wildCard);
private:
    uint16_t  _topicId;
    uint8_t   _status;
    MQString*  _topicStr;
    TopicCallback  _callback;
};

/*=====================================
        Class Topics
 ======================================*/
class Topics {
public:
      Topics();
      ~Topics();
      bool     allocate(uint8_t topicsSize);
      uint16_t  getTopicId(MQString* topic);
      Topic*    getTopic(MQString* topic);
      Topic*    getTopic(uint16_t topicId);
      bool     setTopicId(MQString* topic, uint16_t id);
      bool     setCallback(MQString* topic, TopicCallback callback);
      bool     setCallback(uint16_t topicId, TopicCallback callback);
      int      execCallback(uint16_t  topicId, MqttsnPublish* msg);
      int      execCallback(MQString* topic, MqttsnPublish* msg);
      void     addTopic(MQString* topic);
      Topic*    match(MQString* topic);
      void     setSize(uint8_t size);
      void     clearTopic(void);

private:

    uint8_t   _sizeMax;
    uint8_t   _elmCnt;
    Topic*  _topics;

};

/*=====================================
        Class Payload
  =====================================*/
class Payload{
public:
	Payload();
	Payload(uint16_t len);
	~Payload();
	void init(void);
	int8_t set_uint32(uint32_t val);
	int8_t set_int32(int32_t val);
	int8_t set_float(float val);
	int8_t set_str(char* val);
	int8_t set_str(const char* val);
	int8_t set_array(uint8_t val);

	uint8_t getArray(uint8_t index);
	uint32_t get_uint32(uint8_t index);
	int32_t  get_int32(uint8_t index);
    float    get_float(uint8_t index);
    const char* get_str(uint8_t index, uint16_t* len);

	void 	 getPayload(MqttsnPublish* msg);
	uint16_t getAvailableLength();
	uint16_t getLen();
	uint8_t* getBuf();

	void print();
private:
	uint8_t* getBufferPos(uint8_t index);
	uint8_t* _buff;
	uint16_t _len;
	uint8_t  _elmCnt;
	uint8_t* _pos;
	uint8_t  _memDlt;
};

/*=====================================
        Class Publish Handler
 ======================================*/
class PublishHandller {
public:
    PublishHandller();
    ~PublishHandller();
    int exec(MqttsnPublish* msg, Topics* topics);

};


#endif  /* MQTTS_H_ */


================================================
FILE: Client/src/lib/mqttsnClient.cpp
================================================
/*
 * mqttsnClient.cpp
 *
 *                      The BSD License
 *
 *           Copyright (c) 2014, tomoaki@tomy-tech.com
 *                    All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: 2014/06/01
 *    Modified: 2014/11/25
 *      Author: Tomoaki YAMAGUCHI
 *     Version: 1.1.1
 */

#ifdef ARDUINO

#endif  /* ARDUINO */

#ifdef ARDUINO
	#include <MQTTSN_Application.h>
	#include <mqUtil.h>
	#include <mqttsnClient.h>
	#if defined(MQTTSN_DEBUG) || defined(NW_DEBUG) || defined(DEBUG)
		#include <SoftwareSerial.h>
		extern SoftwareSerial debug;
	#endif
#else
	#include "MQTTSN_Application.h"
	#include "mqUtil.h"
	#include "mqttsnClient.h"
#endif

#ifdef MBED
  #include "mbed.h"
#endif  /* MBED */

#ifdef LINUX
  #include <stdio.h>
  #include <sys/time.h>
  #include <sys/types.h>
  #include <sys/stat.h>
  #include <unistd.h>
  #include <stdlib.h>
  #include <string.h>
  #include <fcntl.h>
  #include <errno.h>
  #include <termios.h>
#endif /* LINUX */


using namespace std;
using namespace tomyClient;

extern OnPublishList theOnPublishList[];
extern APP_CONFIG theAppConfig;
extern MQString* theTopics[];

extern uint16_t getUint16(uint8_t* pos);
extern uint32_t getUint32(uint8_t* pos);
extern float    getFloat32(uint8_t* pos);

extern void setUint16(uint8_t* pos, uint16_t val);
extern void setUint32(uint8_t* pos, uint32_t val);
extern void setFloat32(uint8_t* pos, float val);

static MqttsnClient*  theMqttsn;

/*=================================================================

        Class MqttsnnClient

 ================================================================*/
void ResponseHandler(NWResponse* resp, int* returnCode){
        theMqttsn->recieveMessageHandler(resp, returnCode);
}

MqttsnClient::MqttsnClient(){
    _network = new Network();
    _network->setRxHandler(ResponseHandler);
    _sendQ = new SendQue();
    _duration = 0;
    _clientId = new MQString();
    _clientFlg = 0;
    _nRetry = MQTTSN_RETRY_COUNT;
    _nRetryCnt = 0;
    _tRetry = 0;
    _willTopic = _willMessage = 0;
    _clientStatus.setKeepAlive(MQTTSN_DEFAULT_KEEPALIVE);
    _msgId = 0;
    _topics.allocate(MQTTSN_MAX_TOPICS + 1);
    _sendFlg = false;
    _subscribingFlg = false;
    theMqttsn = this;
}

MqttsnClient::~MqttsnClient(){
  _sendQ->deleteAllRequest();
  delete _network;
}


int MqttsnClient::initialize(APP_CONFIG config){
    _clientId->copy(config.mqttsnCfg.nodeId);
    MQString* pre1 = new MQString(MQTTSN_TOPIC_PREDEFINED_TIME);
    _topics.addTopic(pre1);
    _topics.setTopicId(pre1,MQTTSN_TOPICID_PREDEFINED_TIME);
    return _network->initialize(config.netCfg);
}

void MqttsnClient::subscribe(){
	for(int i = 0; theOnPublishList[i].pubCallback; i++){
		subscribe(theOnPublishList[i].topic, theOnPublishList[i].pubCallback, theOnPublishList[i].qos);
	}
}

void MqttsnClient::createTopics(){
	for(int i = 0; theTopics[i]; i++){
		registerTopic(theTopics[i]);
	}
}

void MqttsnClient::setSubscribing(bool flg){
	_clientStatus.setSubscribing(flg);
}

int MqttsnClient::readPacket(){
	return _network->readPacket(1); // XBee: Modem Staus, UDP: neglect
}

Topics* MqttsnClient::getTopics(){
    return &_topics;
}

void MqttsnClient::setKeepAlive(uint16_t sec){
	_clientStatus.setKeepAlive(sec);
}

void MqttsnClient::setWillTopic(MQString* topic){
    _willTopic = topic;
    _clientFlg |= MQTTSN_FLAG_WILL;
}

void MqttsnClient::setWillMessage(MQString* msg){
    _willMessage = msg;
    _clientFlg |= MQTTSN_FLAG_WILL;
}


void MqttsnClient::setRetain(bool retain){
	if(retain){
		_clientFlg |= MQTTSN_FLAG_RETAIN;
	}else{
		_clientFlg &= (~MQTTSN_FLAG_RETAIN);
	}
}

void MqttsnClient::setSleepMode(){
	_clientStatus.setModeSleep();
}

void MqttsnClient::setClean(bool clean){
	if(clean){
		_clientFlg |= MQTTSN_FLAG_CLEAN;
	}else{
		_clientFlg ^= MQTTSN_FLAG_CLEAN;
	}
}

bool MqttsnClient::isCleanSession(){
	return _clientFlg && MQTTSN_FLAG_CLEAN;
}

void MqttsnClient::setRetryMax(uint8_t cnt){
    _nRetry = cnt;
}


void MqttsnClient::setGwAddress(){
	_network->setGwAddress();
}

MQString* MqttsnClient::getClientId(){
    return _clientId;
}

ClientStatus* MqttsnClient::getClientStatus(){
	return &_clientStatus;
}


uint16_t MqttsnClient::getNextMsgId(){
    _msgId++;
    if (_msgId == 0){
        _msgId = 1;
    }
    return _msgId;
}

uint8_t MqttsnClient::getMsgRequestType(){
    if (_sendQ->getMessage(0)){
        return _sendQ->getMessage(0)->getType();
    }else{
        return 0;
    }
}
uint8_t MqttsnClient::getMsgRequestStatus(){
    return _sendQ->getStatus(0);
}

uint8_t MqttsnClient::getMsgRequestCount(){
  return _sendQ->getCount();
}

void MqttsnClient::setMsgRequestStatus(uint8_t stat){
    _sendQ->setStatus(0,stat);
}

void MqttsnClient::clearMsgRequest(){
    _sendQ->deleteRequest(0);
}

void MqttsnClient::createTopic(MQString* topic, TopicCallback callback){
    _topics.addTopic(topic);
    _topics.setCallback(topic, callback);
}

void MqttsnClient::delayTime(uint16_t maxTime){
#ifdef ARDUINO
    srand((uint32_t)millis( ));
    uint32_t tm = rand() % (maxTime * 1000);
#else
    srand((uint32_t)time( 0 ));
    uint32_t tm = (rand() % (maxTime * 1000));
#endif
    XTimer delayTimer;
    delayTimer.start(tm);
    while(!delayTimer.isTimeUp()){
       _network->readPacket();
    }
}

void MqttsnClient::copyMsg(MqttsnMessage* msg, NWResponse* recvMsg){
    memcpy(msg->getMsgBuff(), recvMsg->getPayload(), recvMsg->getPayload(0));
}



/*========================================================
    Send a MQTT-S Message (add the send request)
==========================================================*/
int MqttsnClient::requestSendMsg(MqttsnMessage* mqttsMsgPtr){
    int index = _sendQ->addRequest((MqttsnMessage*)mqttsMsgPtr);
	_sendQ->setStatus(index, MQTTSN_MSG_REQUEST);
    return MQTTSN_ERR_NO_ERROR;
}

/*========================================================
  Send a MQTT-S Message (add to the top of the send request)
==========================================================*/
int MqttsnClient::requestPrioritySendMsg(MqttsnMessage* mqttsMsgPtr){
    _sendQ->addPriorityRequest((MqttsnMessage*)mqttsMsgPtr);
    _sendQ->setStatus(0, MQTTSN_MSG_REQUEST);
    return MQTTSN_ERR_NO_ERROR;
}

/*========================================================
  Execute sending a MQTT-S Message
==========================================================*/

/*-------------  send Message once -----------------*/
int MqttsnClient::exec(){
    int rc;

    if (!_clientStatus.isGatewayAlive()){
		D_MQTTW("Gateway is Dead.\r\n");
		_clientStatus.init();
	}
/*
    if(_sendFlg){
    	return MQTTSN_ERR_NO_ERROR;
    }else{
    	//_sendFlg = true;
    }
*/
    while(true){
        rc = sendRecvMsg();

        if (rc == MQTTSN_ERR_RETRY_OVER){
			if (getMsgRequestType() == MQTTSN_TYPE_WILLTOPIC    ||
			    getMsgRequestType() == MQTTSN_TYPE_WILLMSG      ||
			  	getMsgRequestType() == MQTTSN_TYPE_PINGREQ      ||
				getMsgRequestType() == MQTTSN_TYPE_PUBLISH      ||
				getMsgRequestType() == MQTTSN_TYPE_REGISTER     ||
				getMsgRequestType() == MQTTSN_TYPE_SUBSCRIBE    ||
				getMsgRequestType() == MQTTSN_TYPE_CONNECT      ||
				getMsgRequestType() == MQTTSN_TYPE_UNSUBSCRIBE  ||
				getMsgRequestType() == MQTTSN_TYPE_PUBREC       ||
				getMsgRequestType() == MQTTSN_TYPE_SEARCHGW     ||
				getMsgRequestType() == MQTTSN_TYPE_PUBREL) {
				_clientStatus.init();
			}
        }else if(rc == MQTTSN_ERR_REBOOT_REQUIRED){
			_clientStatus.init();
		}else if(rc != MQTTSN_ERR_NO_ERROR && rc != MQTTSN_ERR_INVALID_TOPICID){
			continue;
		}
		break;
	}
    clearMsgRequest();
//    _sendFlg = false;
    return rc;
}

/*=============================
 *   Send or Receive Message
 ==============================*/
int MqttsnClient::sendRecvMsg(){
    int rc = MQTTSN_ERR_NO_ERROR;

    if (!_clientStatus.isGatewayAlive()){
		D_MQTTW("Gateway is Dead.\r\n");
		_clientStatus.init();
	}

	/*======= Establish Connection ===========*/
	if (_clientStatus.isLost() ||_clientStatus.isSearching() ){
		/*------------ Send SEARCHGW --------------*/
		if (getMsgRequestType() != MQTTSN_TYPE_SEARCHGW){
			searchGw(0);   //ZB_BROADCAST_RADIUS_MAX_HOPS
		}
		_clientStatus.sendSEARCHGW();
		_network->resetGwAddress();
		rc = broadcast(MQTTSN_TIME_RETRY);
		if ( rc != MQTTSN_ERR_NO_ERROR){
			return rc;
		}
	}

	if (!_clientStatus.isConnected() && !_clientStatus.isSearching()){
		/*-----------  Send CONNECT ----------*/
		if (getMsgRequestType() != MQTTSN_TYPE_CONNECT){
			connect();
		}
		rc = unicast(MQTTSN_TIME_RETRY);
		if ( rc != MQTTSN_ERR_NO_ERROR){
			return rc;
		}
	}

	if(_clientStatus.isOnConnected()){
		/*----------- ready to send  ----------*/
		_clientStatus.setAvailableToSend();
		/*----------- in case of clean session ----------*/
		if(isCleanSession() && !_clientStatus.isSubscribing()){
			_clientStatus.setSubscribing(true);  // re-entrant control
			subscribe();
			_clientStatus.setSubscribing(false); // re-entrant control
		}
	}

	if (getMsgRequestStatus() == MQTTSN_MSG_REQUEST || getMsgRequestStatus() == MQTTSN_MSG_RESEND_REQ){
        /*======  Send Message =======*/
        if (_clientStatus.isAvailableToSend()){
			rc = unicast(MQTTSN_TIME_RETRY);
		}else{
			rc = MQTTSN_ERR_NOT_CONNECTED;
		}
    }

	/*======= Receive Message ===========*/
	_network->readPacket();  //  Receive MQTT-S Message

	if (_clientStatus.isPINGREQRequired()){
		/*-------- Send PINGREQ -----------*/
		if(getMsgRequestType() != MQTTSN_TYPE_PINGREQ){
			pingReq(_clientId);
		}
		rc = unicast(MQTTSN_TIME_RETRY);
	}
	return rc;
}

/*------------------------------------
 *   Broad cast the MQTT-S Message
 -------------------------------------*/
int MqttsnClient::broadcast(uint16_t packetReadTimeout){
    int retry = 0;
    while(retry < _nRetry){
        D_MQTTW("Bcast ");
		D_MQTTLN(_sendQ->getMessage(0)->getMsgTypeName());
		D_MQTTF("%s\r\n", _sendQ->getMessage(0)->getMsgTypeName());

#if defined(DEBUG) && defined(ARDUINO)
		debug.print("Bcast ");
		debug.println(_sendQ->getMessage(0)->getMsgTypeName());
#endif
        _network->send(_sendQ->getMessage(0)->getMsgBuff(), _sendQ->getMessage(0)->getLength(),BcastReq);

        _respTimer.start(packetReadTimeout * 1000);
        setMsgRequestStatus(MQTTSN_MSG_WAIT_ACK);

        while(!_respTimer.isTimeUp()){
        	if(_network->readPacket() != PACKET_ERROR_NODATA){
				if (getMsgRequestStatus() == MQTTSN_MSG_COMPLETE){
					clearMsgRequest();
					return MQTTSN_ERR_NO_ERROR;
				}else if(getMsgRequestStatus() == MQTTSN_MSG_REJECTED){
					//clearMsgRequest();
					return MQTTSN_ERR_REBOOT_REQUIRED;
				}
        	}
        }
        setMsgRequestStatus(MQTTSN_MSG_REQUEST);
        retry++;
    }
    //clearMsgRequest();
    return MQTTSN_ERR_RETRY_OVER;
}

/*------------------------------------
 *   Unicast the MQTT-S Message
 -------------------------------------*/
int MqttsnClient::unicast(uint16_t packetReadTimeout){
    int retry = 0;

    while(retry < _nRetry){
    	if (getMsgRequestStatus() == MQTTSN_MSG_RESEND_REQ){
			/* ------  Re send Time delay -------*/
			#ifdef ARDUINO
			  delay(MQTTSN_TIME_WAIT * 1000UL);
			#else
				#ifdef MBED
					wait_ms(MQTTSN_TIME_WAIT * 1000);
				#else
					usleep(MQTTSN_TIME_WAIT * 1000000);
				#endif
			#endif
			setMsgRequestStatus(MQTTSN_MSG_REQUEST);
		}

    	/*------ Send Top message in SendQue -----*/
    	if (getMsgRequestStatus() != MQTTSN_MSG_REQUEST){
    		return MQTTSN_ERR_NO_ERROR;
    	}

        D_MQTTW("Ucast ");
        D_MQTTLN(_sendQ->getMessage(0)->getMsgTypeName());
        D_MQTTF("%s\r\n", _sendQ->getMessage(0)->getMsgTypeName());
#if defined(DEBUG) && defined(ARDUINO)
		debug.print("Ucast ");
		debug.println(_sendQ->getMessage(0)->getMsgTypeName());
#endif

        _network->send(_sendQ->getMessage(0)->getMsgBuff(), _sendQ->getMessage(0)->getLength(), UcastReq);
        _clientStatus.setLastSendTime();

        _sendQ->getMessage(0)->setDup();
        _respTimer.start(packetReadTimeout * 1000UL);
        setMsgRequestStatus(MQTTSN_MSG_WAIT_ACK);

        while(!_respTimer.isTimeUp()){
            if ((getMsgRequestType() == MQTTSN_TYPE_PUBLISH &&
            		  _sendQ->getMessage(0)->getQos() == 0 )  ||
            	getMsgRequestType() == MQTTSN_TYPE_PUBACK     ||
				getMsgRequestType() == MQTTSN_TYPE_REGACK     ||
				getMsgRequestType() == MQTTSN_TYPE_PUBCOMP    ||
				getMsgRequestStatus() == MQTTSN_MSG_COMPLETE ){
            	//clearMsgRequest();
                return MQTTSN_ERR_NO_ERROR;
            }else if(getMsgRequestType() == MQTTSN_TYPE_DISCONNECT ){
            	_clientStatus.recvDISCONNECT();
            	//clearMsgRequest();
				return MQTTSN_ERR_NO_ERROR;
        	}else if (getMsgRequestStatus() == MQTTSN_MSG_REJECTED){
        		//clearMsgRequest();
				return MQTTSN_ERR_REJECTED;
            }
            /*----- Read response  ----*/
			int rc = _network->readPacket();
			if(rc == MQTTSN_READ_RESP_ONCE_MORE){
				rc = _network->readPacket();
			}
            if(rc == MQTTSN_ERR_INVALID_TOPICID){
            	//clearMsgRequest();
            	return rc;
            }else if(rc == PACKET_MODEM_STATUS ){
				break;
			}

            /*---- WILLTOPICREQ ,WILLMESSAGEREQ PUBREC are received ---*/
            if (getMsgRequestStatus() == MQTTSN_MSG_REQUEST &&
               (getMsgRequestType() == MQTTSN_TYPE_WILLTOPIC ||
                getMsgRequestType() == MQTTSN_TYPE_WILLMSG  ||
                getMsgRequestType() == MQTTSN_TYPE_PUBREL)){
            	retry = 0;
                break;
            }

        }
		setMsgRequestStatus(MQTTSN_MSG_REQUEST);
        retry++;
    }
    return MQTTSN_ERR_RETRY_OVER;
}

/*========================================
 *   Create & send the MQTT-S Messages
 =========================================*/
#ifdef MQTTSN_DEBUG
const char* gateWayLost = " Gateway lost\r\n";
#endif

/*--------- REGISTER ------*/
int MqttsnClient::registerTopic(MQString* topic){
    MqttsnRegister mqttsMsg = MqttsnRegister();
    mqttsMsg.setTopicName(topic);
    mqttsMsg.setMsgId(getNextMsgId());
    _topics.addTopic(topic);

    D_MQTTW("\nREGISTER SEND Topic = ");
	D_MQTTLN(topic->getConstStr());
	D_MQTTF("%s\r\n", topic->getConstStr());

    requestPrioritySendMsg((MqttsnMessage*)&mqttsMsg);
    return exec();
}

/*--------- PUBLISH ------*/
int MqttsnClient::publish(MQString* topic, const char* data, int dataLength, uint8_t qos){
	if(!_clientStatus.isAvailableToSend()){
		return MQTTSN_ERR_NOT_CONNECTED;
	}

	MqttsnPublish mqttsMsg = MqttsnPublish();
	uint16_t topicId = _topics.getTopicId(topic);

    if (topic->getCharLength() == 2 && topicId == 0){
		_topics.addTopic(topic);
		if(topic->getStr()){
			topicId = getUint16((uint8_t*)topic->getStr());
		}else{
			topicId = getUint16((uint8_t*)topic->getConstStr());
		}
		_topics.setTopicId(topic, topicId);
		mqttsMsg.setFlags(_clientFlg | MQTTSN_TOPIC_TYPE_SHORT);
		mqttsMsg.setTopicId(topicId);
	}else{
		mqttsMsg.setFlags(_clientFlg | MQTTSN_TOPIC_TYPE_NORMAL);
		if(topicId){
			mqttsMsg.setTopicId(topicId);
		}else{
			registerTopic(topic);
			mqttsMsg.setTopicId(_topics.getTopicId(topic));
		}
	}
    mqttsMsg.setQos(qos);
	mqttsMsg.setData((uint8_t*)data, (uint8_t)dataLength);
	if (qos){
		mqttsMsg.setMsgId(getNextMsgId());
	}
	requestSendMsg((MqttsnMessage*)&mqttsMsg);

	D_MQTTW("PUBLISH SEND msgID = ");
	D_MQTTLN(mqttsMsg.getMsgId(),DEC);
	D_MQTTF("%d\r\n", mqttsMsg.getMsgId());

	int rc = exec();
	if (rc == MQTTSN_ERR_INVALID_TOPICID){
		registerTopic(topic);
	}
	return rc;
}

/*--------- PUBLISH ------*/
int MqttsnClient::publish(MQString* topic, MQString* data, uint8_t qos){
	if(data->getStr()){
		return publish(topic, (const char*)data->getStr(),data->getCharLength(), qos);
	}else if(data->getConstStr()){
		return publish(topic, data->getConstStr(),data->getCharLength(), qos);
	}
	return MQTTSN_ERR_NO_DATA;
}

/*--------- PUBLISH ------*/
int MqttsnClient::publish(MQString* topic, Payload* payload, uint8_t qos){
	return publish(topic, (const char*)(payload->getBuf()), payload->getLen(), qos);
}
/*--------- PUBLISH ------*/
int MqttsnClient::publish(uint16_t predefinedId, const char* data, int dataLength, uint8_t qos){
	MqttsnPublish mqttsMsg = MqttsnPublish();
    mqttsMsg.setFlags(_clientFlg | MQTTSN_TOPIC_TYPE_PREDEFINED);
    mqttsMsg.setTopicId(predefinedId);
    mqttsMsg.setQos(qos);
    mqttsMsg.setData((uint8_t*)data, (uint8_t)dataLength);
    if (qos){
        mqttsMsg.setMsgId(getNextMsgId());
    }
    requestSendMsg((MqttsnMessage*)&mqttsMsg);
    return exec();
}

/*--------- SUBSCRIBE ------*/
int MqttsnClient::subscribe(MQString* topic, TopicCallback callback, uint8_t qos){
	MqttsnSubscribe mqttsMsg = MqttsnSubscribe();
    uint16_t topicId = _topics.getTopicId(topic);
    if (topic->getCharLength() == 2 && topicId == 0){
    	if(topic->getStr()){
    		topicId = getUint16((uint8_t*)topic->getStr());
    	}else{
    		topicId = getUint16((uint8_t*)topic->getConstStr());
    	}
		_topics.addTopic(topic);
		_topics.setTopicId(topic, topicId);
		_topics.setCallback(topic, callback);
		mqttsMsg.setTopicName(topic);
		mqttsMsg.setFlags(_clientFlg | MQTTSN_TOPIC_TYPE_SHORT);
    }else{
        mqttsMsg.setTopicName(topic);
        mqttsMsg.setFlags(_clientFlg | MQTTSN_TOPIC_TYPE_NORMAL);
        _topics.addTopic(topic);
        _topics.setCallback(topic, callback);
    }
    mqttsMsg.setMsgId(getNextMsgId());
    mqttsMsg.setQos(qos);
    requestSendMsg((MqttsnMessage*)&mqttsMsg);
    return exec();
}

/*--------- SUBSCRIBE ------*/
int MqttsnClient::subscribe(uint16_t predefinedId, TopicCallback callback, uint8_t qos){
	MqttsnSubscribe mqttsMsg = MqttsnSubscribe();
    mqttsMsg.setTopicId(predefinedId);
    mqttsMsg.setFlags(_clientFlg | MQTTSN_TOPIC_TYPE_PREDEFINED);
    mqttsMsg.setMsgId(getNextMsgId());
    _topics.setCallback(predefinedId, callback);
    requestSendMsg((MqttsnMessage*)&mqttsMsg);
    return exec();
}

/*--------- UNSUBSCRIBE ------*/
int MqttsnClient::unsubscribe(MQString* topic){
    MqttsnUnsubscribe mqttsMsg = MqttsnUnsubscribe();
    uint16_t topicId = _topics.getTopicId(topic);
    if (topic->getCharLength() == 2 && topicId > 0){
        mqttsMsg.setTopicName(topic);
        mqttsMsg.setFlags(_clientFlg | MQTTSN_TOPIC_TYPE_SHORT);
    }else{
        mqttsMsg.setTopicId(topicId);
		mqttsMsg.setFlags(_clientFlg | MQTTSN_TOPIC_TYPE_NORMAL);
    }
    mqttsMsg.setMsgId(getNextMsgId());
    requestSendMsg((MqttsnMessage*)&mqttsMsg);
    return exec();
}

/*--------- UNSUBSCRIBE ------*/
int MqttsnClient::unsubscribe(uint16_t predefinedId){
    MqttsnUnsubscribe mqttsMsg = MqttsnUnsubscribe();
    mqttsMsg.setTopicId(predefinedId);
    mqttsMsg.setMsgId(getNextMsgId());
    mqttsMsg.setFlags(_clientFlg | MQTTSN_TOPIC_TYPE_PREDEFINED);
    requestSendMsg((MqttsnMessage*)&mqttsMsg);
    return exec();
}

/*--------- DISCONNECT ------*/
int MqttsnClient::disconnect(uint16_t duration){
    MqttsnDisconnect mqttsMsg = MqttsnDisconnect();
    if (duration){
        mqttsMsg.setDuration(duration);
    }
    requestSendMsg((MqttsnMessage*)&mqttsMsg);
    return exec();
}

/*====  private Messages ====*/

/*--------- SEARCHGW ------*/
int  MqttsnClient::searchGw(uint8_t radius){
    MqttsnSearchGw mqttsMsg = MqttsnSearchGw();
    mqttsMsg.setRadius(radius);
    delayTime(MQTTSN_TIME_SEARCHGW);
    return requestPrioritySendMsg((MqttsnMessage*)&mqttsMsg);
}

/*--------- CONNECT ------*/
int MqttsnClient::connect(){
    MqttsnConnect mqttsMsg = MqttsnConnect(_clientId);
    mqttsMsg.setDuration(_clientStatus.getKeepAlive());
    mqttsMsg.setFlags(_clientFlg);
    return requestPrioritySendMsg((MqttsnMessage*)&mqttsMsg);
}

/*--------- WILLTOPIC ------*/
int MqttsnClient::willTopic(){
    MqttsnWillTopic mqttsMsg = MqttsnWillTopic();
    mqttsMsg.setWillTopic(_willTopic);
    mqttsMsg.setFlags(_clientFlg & 0x70);
    return requestSendMsg((MqttsnMessage*)&mqttsMsg);
}

/*--------- WILLMSG ------*/
int MqttsnClient::willMsg(){
    MqttsnWillMsg mqttsMsg = MqttsnWillMsg();
    mqttsMsg.setWillMsg(_willMessage);
    return requestSendMsg((MqttsnMessage*)&mqttsMsg);
}

/*--------- PUBACK ------*/
int MqttsnClient::pubAck(uint16_t topicId, uint16_t msgId, uint8_t rc){
    MqttsnPubAck mqttsMsg = MqttsnPubAck();
    mqttsMsg.setTopicId(topicId);
    mqttsMsg.setMsgId(msgId);
    mqttsMsg.setReturnCode(rc);

    D_MQTTW("\nPUBACK SEND MsgId = ");
	D_MQTTLN(msgId,DEC);
	D_MQTTF("%d", msgId);

    return requestPrioritySendMsg((MqttsnMessage*)&mqttsMsg);
    //return requestSendMsg((MqttsnMessage*)&mqttsMsg);
}

/*--------- PUBREC ------*/
int MqttsnClient::pubRec(uint16_t msgId){
	MqttsnPubRec mqttsMsg = MqttsnPubRec();
    mqttsMsg.setMsgId(msgId);
    return requestPrioritySendMsg((MqttsnMessage*)&mqttsMsg);
}

/*--------- PUBREL ------*/
int MqttsnClient::pubRel(uint16_t msgId){
    MqttsnPubRel mqttsMsg = MqttsnPubRel();
    mqttsMsg.setMsgId(msgId);
    return requestPrioritySendMsg((MqttsnMessage*)&mqttsMsg);
}

/*--------- PUBCOMP ------*/
int MqttsnClient::pubComp(uint16_t msgId){
    MqttsnPubComp mqttsMsg = MqttsnPubComp();
    mqttsMsg.setMsgId(msgId);
    return requestPrioritySendMsg((MqttsnMessage*)&mqttsMsg);
}

/*--------- REGACK ------*/
int MqttsnClient::regAck(uint16_t topicId, uint16_t msgId, uint8_t rc){
    MqttsnRegAck mqttsMsg = MqttsnRegAck();
    mqttsMsg.setTopicId(topicId);
    mqttsMsg.setMsgId(msgId);
    mqttsMsg.setReturnCode(rc);
    return requestSendMsg((MqttsnMessage*)&mqttsMsg);
}

/*--------- PINGREQ ------*/
int  MqttsnClient::pingReq(MQString* clientId){
    MqttsnPingReq mqttsMsg = MqttsnPingReq(clientId);
    return requestSendMsg((MqttsnMessage*)&mqttsMsg);
}

/* ===================================================
          Procedures for  Received Messages
 =====================================================*/
void MqttsnClient::recieveMessageHandler(NWResponse* recvMsg, int* returnCode){
	uint8_t msgType = recvMsg->getType();
#if defined(DEBUG) && defined(ARDUINO)
		debug.print("Receive Msg=0x");
		debug.println(msgType,HEX);
#endif
    if ( (_clientStatus.isSearching() && msgType != MQTTSN_TYPE_GWINFO) ||
    	 (_clientStatus.isSubscribing() && msgType == MQTTSN_TYPE_PUBLISH ) ||
		 (getMsgRequestType() == MQTTSN_TYPE_PUBLISH &&
		  getMsgRequestStatus() == MQTTSN_MSG_WAIT_ACK &&
		  msgType == MQTTSN_TYPE_PUBLISH ) )
    {
    	D_MQTTW("Ignore Received Message\r\n");

#if defined(DEBUG) && defined(ARDUINO)
		debug.println("Ignore Received Message");
#endif
/*---------  GWINFO  ----------*/
	}else if (msgType == MQTTSN_TYPE_GWINFO && recvMsg->getPayloadLength() == 3){
		D_MQTTW("GWINFO recv\r\n");
		MqttsnGwInfo mqMsg = MqttsnGwInfo();
		copyMsg(&mqMsg, recvMsg);
		if (getMsgRequestType() == MQTTSN_TYPE_SEARCHGW){
			if(_clientStatus.recvGWINFO(&mqMsg)){
				setMsgRequestStatus(MQTTSN_MSG_COMPLETE);
			}else{
				setMsgRequestStatus(MQTTSN_MSG_REJECTED);
			}
			if(isCleanSession()){
				_topics.clearTopic();
			}
			_network->setGwAddress();
		}
/*---------  REGISTER  ----------*/
	}else if (msgType == MQTTSN_TYPE_REGISTER){

		if(_clientStatus.isAvailableToSend()){
			D_MQTTW("REGISTER recv\r\n");
			MqttsnRegister mqMsg = MqttsnRegister();

			mqMsg.setFrame(recvMsg);
			uint16_t topicId = _topics.getTopicId(mqMsg.getTopicName());
			if (topicId == 0){
				if (_topics.match(mqMsg.getTopicName())){
					MQString* mqStr = mqMsg.getTopicName()->create();
					_topics.addTopic(mqStr);
					_topics.setTopicId(mqStr,mqMsg.getTopicId());
					_topics.setCallback(mqMsg.getTopicId(),_topics.match(mqStr)->getCallback());
				}
			}

			if (mqMsg.getMsgId() != 0){
				regAck(mqMsg.getTopicId(), mqMsg.getMsgId(), MQTTSN_RC_ACCEPTED);
				if(getMsgRequestStatus() == MQTTSN_MSG_WAIT_ACK){
					*returnCode = MQTTSN_READ_RESP_ONCE_MORE;
				}
			}else{
				D_MQTTW("RegAck is not required\r\n");
			}
		}else{
			D_MQTTW("REGISTER recv. Client is not Active\r\n");
		}
/*---------  PUBLISH  --------*/
    }else if (msgType == MQTTSN_TYPE_PUBLISH){

    	if(_clientStatus.isAvailableToSend()){
    		_clientStatus.setSubscribing(true);
			MqttsnPublish mqMsg = MqttsnPublish();
			mqMsg.setFrame(recvMsg);

    		D_MQTTW("PUBLISH RECV msgID = ");
    		D_MQTTLN(mqMsg.getMsgId(),DEC);
    		D_MQTTF("%d\r\n", mqMsg.getMsgId());

			if (mqMsg.getQos() == QOS1){
				pubAck(mqMsg.getTopicId(), mqMsg.getMsgId(), MQTTSN_RC_ACCEPTED);
				unicast(MQTTSN_TIME_RETRY);
			}else if(mqMsg.getQos() == QOS2){
				pubRec(mqMsg.getMsgId());
				unicast(MQTTSN_TIME_RETRY);
			}

			if(getMsgRequestStatus() == MQTTSN_MSG_WAIT_ACK){
				*returnCode = MQTTSN_READ_RESP_ONCE_MORE;
			}
			_pubHdl.exec(&mqMsg,&_topics);   //  Execute Callback routine
    		_clientStatus.setSubscribing(false);
    	}else{
    		D_MQTTW("PUBLISH recv Client is not Active\r\n");
    	}

/*===========  Response  =========*/

/*---------  PUBACK  ----------*/
    }else if (msgType == MQTTSN_TYPE_PUBACK &&
    		(getMsgRequestStatus() == MQTTSN_MSG_WAIT_ACK &&
    		 getMsgRequestType() == MQTTSN_TYPE_PUBLISH)){
        MqttsnPubAck mqMsg = MqttsnPubAck();
        copyMsg(&mqMsg, recvMsg);

        D_MQTTW("\nPUBACK RECV MsgId = ");
		D_MQTTLN(mqMsg.getMsgId(),DEC);
		D_MQTTF("%d", mqMsg.getMsgId());
        D_MQTTW(" RC=");
        D_MQTTLN(mqMsg.getReturnCode(),DEC);
        D_MQTTF("%d\r\n", mqMsg.getReturnCode());

        if (mqMsg.getMsgId() == getUint16(_sendQ->getMessage(0)->getBody() + 3)){
            if (mqMsg.getReturnCode() == MQTTSN_RC_ACCEPTED){
                setMsgRequestStatus(MQTTSN_MSG_COMPLETE);

            }else if (mqMsg.getReturnCode() == MQTTSN_RC_REJECTED_CONGESTION){
                  setMsgRequestStatus(MQTTSN_MSG_RESEND_REQ);

            }else if (mqMsg.getReturnCode() == MQTTSN_RC_REJECTED_INVALID_TOPIC_ID){
                *returnCode = MQTTSN_ERR_INVALID_TOPICID;
                setMsgRequestStatus(MQTTSN_MSG_REJECTED);
            }
        }else{
        	//D_MQTTW("MsgId dosn't match.\r\n");
        }

/*---------  PINGRESP  ----------*/
    }else if (msgType == MQTTSN_TYPE_PINGRESP){
        D_MQTTW("PINGRESP recv\r\n");

        if (getMsgRequestType() == MQTTSN_TYPE_PINGREQ){
        	_clientStatus.recvPINGRESP();
            setMsgRequestStatus(MQTTSN_MSG_COMPLETE);
        }

/*---------  ADVERTISE  ----------*/
    }else if (msgType == MQTTSN_TYPE_ADVERTISE){
        D_MQTTW("ADVERTISE recv\r\n");

        MqttsnAdvertise mqMsg = MqttsnAdvertise();
        copyMsg(&mqMsg, recvMsg);
        _clientStatus.recvADVERTISE(&mqMsg);
        if(getMsgRequestStatus() == MQTTSN_MSG_WAIT_ACK){
        	*returnCode = MQTTSN_READ_RESP_ONCE_MORE;
        }

/*---------  CONNACK  ----------*/
    }else if (msgType == MQTTSN_TYPE_CONNACK){
        if ((getMsgRequestType() == MQTTSN_TYPE_CONNECT || getMsgRequestType() == MQTTSN_TYPE_WILLMSG)){
            MqttsnConnack mqMsg = MqttsnConnack();
            copyMsg(&mqMsg, recvMsg);

            D_MQTTW("CONNACK recv RC=");
			D_MQTTLN(mqMsg.getReturnCode(),DEC);
			D_MQTTF("%d\r\n", mqMsg.getReturnCode());

            if (mqMsg.getReturnCode() == MQTTSN_RC_ACCEPTED){
                setMsgRequestStatus(MQTTSN_MSG_COMPLETE);
                _clientStatus.recvCONNACK();

            }else{
               setMsgRequestStatus(MQTTSN_MSG_REJECTED);
               *returnCode = MQTTSN_ERR_REJECTED;
				clearMsgRequest();
               _clientStatus.recvDISCONNECT();
            }
        }
        D_MQTTW("\r\n");

/*---------  REGACK  ----------*/
    }else if (msgType == MQTTSN_TYPE_REGACK){
         D_MQTTW("REGACK recv\r\n");

        if (getMsgRequestStatus() == MQTTSN_MSG_WAIT_ACK &&
            getMsgRequestType() == MQTTSN_TYPE_REGISTER){
            MqttsnRegAck mqMsg = MqttsnRegAck();
            copyMsg(&mqMsg, recvMsg);
            if (mqMsg.getMsgId() == getUint16(_sendQ->getMessage(0)->getBody() + 2)){
				if (getUint16((uint8_t*)_sendQ->getMessage(0)->getBody()+4)){
					if (mqMsg.getReturnCode() == MQTTSN_RC_ACCEPTED){
						setMsgRequestStatus(MQTTSN_MSG_COMPLETE);
						MQString topic;
						topic.readBuf(_sendQ->getMessage(0)->getBody() + 4);
						_topics.setTopicId(&topic, mqMsg.getTopicId());
					}else if (mqMsg.getReturnCode() == MQTTSN_RC_REJECTED_CONGESTION){
					  setMsgRequestStatus(MQTTSN_MSG_RESEND_REQ);
					}else{
						*returnCode = MQTTSN_ERR_REJECTED;
					}
				}
            }
        }

/*---------  SUBACK  ----------*/
    }else if (msgType == MQTTSN_TYPE_SUBACK && getMsgRequestStatus() == MQTTSN_MSG_WAIT_ACK){
        MqttsnSubAck mqMsg = MqttsnSubAck();
        copyMsg(&mqMsg, recvMsg);

        D_MQTT("\nSUBACK RC=");
        D_MQTTLN(mqMsg.getReturnCode(),HEX);
        D_MQTTF("\nSUBACK RC=%d\r\n", mqMsg.getReturnCode());

        if (mqMsg.getMsgId() == getUint16(_sendQ->getMessage(0)->getBody() + 1)){
            if (mqMsg.getReturnCode() == MQTTSN_RC_ACCEPTED){
                setMsgRequestStatus(MQTTSN_MSG_COMPLETE);
                if (_sendQ->getMessage(0)->getBodyLength() > 5){ // TopicName is not Id
                    MQString topic;
                    topic.copy(_sendQ->getMessage(0)->getBody() + 3,
                    		_sendQ->getMessage(0)->getBodyLength() - 3);
                    _topics.setTopicId(&topic, mqMsg.getTopicId());
                }
            }else if (mqMsg.getReturnCode() == MQTTSN_RC_REJECTED_CONGESTION){
                setMsgRequestStatus(MQTTSN_MSG_RESEND_REQ);
            }else{
                *returnCode = MQTTSN_ERR_REJECTED;       // Return Code
            }
        }

/*---------  UNSUBACK  ----------*/
    }else if (msgType == MQTTSN_TYPE_UNSUBACK && getMsgRequestStatus() == MQTTSN_MSG_WAIT_ACK){
        D_MQTTW("UNSUBACK recv\r\n");
        MqttsnUnSubAck mqMsg = MqttsnUnSubAck();
        copyMsg(&mqMsg, recvMsg);
        if (mqMsg.getMsgId() == getUint16(_sendQ->getMessage(0)->getBody() + 1)){
              setMsgRequestStatus(MQTTSN_MSG_COMPLETE);
        }

/*---------  DISCONNECT  ----------*/
    }else if (msgType == MQTTSN_TYPE_DISCONNECT){
         D_MQTTW("DISCONNECT recv\r\n");
         setMsgRequestStatus(MQTTSN_MSG_COMPLETE);
         _clientStatus.recvDISCONNECT();


/*---------  WILLTOPICREQ  ----------*/
    }else if (msgType == MQTTSN_TYPE_WILLTOPICREQ){
        D_MQTTW("WILLTOPICREQ recv\r\n");
        if (getMsgRequestType() == MQTTSN_TYPE_CONNECT){
            clearMsgRequest();
        	//setMsgRequestStatus(MQTTSN_MSG_COMPLETE);
            MqttsnWillTopic mqMsg = MqttsnWillTopic();
            mqMsg.setFlags(0);                               // ToDo:  add  WillQoS, WillRetain to appConfig
            mqMsg.setWillTopic(_willTopic);
            requestPrioritySendMsg((MqttsnMessage*)&mqMsg);
        }

/*---------  WILLMSGREQ  -----------*/
    }else if (msgType == MQTTSN_TYPE_WILLMSGREQ){
        D_MQTTW("WILLMSGREQ recv\r\n");
        if (getMsgRequestType() == MQTTSN_TYPE_WILLTOPIC){
            clearMsgRequest();
        	//setMsgRequestStatus(MQTTSN_MSG_COMPLETE);
            MqttsnWillMsg mqMsg = MqttsnWillMsg();
            mqMsg.setWillMsg(_willMessage);
            requestPrioritySendMsg((MqttsnMessage*)&mqMsg);
        }

/*---------  PUBREC  ----------*/
	}else if (msgType == MQTTSN_TYPE_PUBREC &&
			(getMsgRequestStatus() == MQTTSN_MSG_WAIT_ACK &&
			 getMsgRequestType() == MQTTSN_TYPE_PUBLISH)){
		MqttsnPubRec mqMsg = MqttsnPubRec();
		copyMsg(&mqMsg, recvMsg);

		D_MQTTW("\nPUBREC recv\r\n");

		if (mqMsg.getMsgId() == getUint16(_sendQ->getMessage(0)->getBody() + 3)){
			MqttsnPubRel mqrMsg = MqttsnPubRel();
			mqrMsg.setMsgId(mqMsg.getMsgId());
			requestPrioritySendMsg((MqttsnMessage*)&mqrMsg);
		}

/*---------  PUBREL  ----------*/
	}else if (msgType == MQTTSN_TYPE_PUBREL &&
			(getMsgRequestStatus() == MQTTSN_MSG_WAIT_ACK &&
			 getMsgRequestType() == MQTTSN_TYPE_PUBREC)){
		MqttsnPubRel mqMsg = MqttsnPubRel();
		copyMsg(&mqMsg, recvMsg);

		D_MQTTW("\nPUBREL recv\r\n");

		if (mqMsg.getMsgId() == getUint16(_sendQ->getMessage(0)->getBody())){
			clearMsgRequest();  // delete PUBREC
			MqttsnPubComp mqrMsg = MqttsnPubComp();
			mqrMsg.setMsgId(mqMsg.getMsgId());
			requestPrioritySendMsg((MqttsnMessage*)&mqMsg);
		}

/*---------  PUBCOMP  ----------*/
	}else if (msgType == MQTTSN_TYPE_PUBCOMP &&
			(getMsgRequestStatus() == MQTTSN_MSG_WAIT_ACK &&
			 getMsgRequestType() == MQTTSN_TYPE_PUBREL)){
		MqttsnPubComp mqMsg = MqttsnPubComp();
		copyMsg(&mqMsg, recvMsg);

		D_MQTTW("\nPUBCOMP recv\r\n");

		if (mqMsg.getMsgId() == getUint16(_sendQ->getMessage(0)->getBody())){
			clearMsgRequest();    // delete request of PUBREL
			setMsgRequestStatus(MQTTSN_MSG_COMPLETE);  // PUBLISH complete
		}
    }else{
		*returnCode = PACKET_ERROR_NODATA;
	}
}


/*=====================================
        Class SendQue
 ======================================*/
SendQue::SendQue(){
    _queCnt = 0;
    _queSize = SENDQ_SIZE;
}
SendQue::~SendQue(){
    for( int i = 0; i < _queCnt; i++){
        delete _msg[i];
    }
}

int SendQue::addRequest(MqttsnMessage* msg){
    if ( _queCnt < _queSize){

		D_MQTTW("\nAdd SendQue size = ");
		D_MQTT(_queCnt + 1, DEC);
		D_MQTT(" Msg = 0x");
		D_MQTTLN(msg->getMsgTypeName());
		D_MQTTF("%d  Msg = 0x%x\r\n", _queCnt + 1, msg->getType());

#if defined(DEBUG) && defined(ARDUINO)
		debug.print("\nAdd SendQue size = ");
		debug.print(_queCnt + 1, DEC);
		debug.print(" Msg=");
		debug.println(msg->getMsgTypeName());
#endif
        _msg[_queCnt] =new MqttsnMessage();
        _msg[_queCnt++]->copy(msg);
        return _queCnt - 1;
    }
    return MQTTSN_ERR_CANNOT_ADD_REQUEST; // Over Que size
}

int SendQue::addPriorityRequest(MqttsnMessage* msg){
    if ( _queCnt < _queSize){
	_queCnt++;
    }

	D_MQTTW("\nAdd SendQue Top Size = ");
	D_MQTT(_queCnt, DEC);
	D_MQTT("  Msg = 0x");
	D_MQTTLN(msg->getType(), HEX);
	D_MQTTF("%d  Msg = 0x%x", _queCnt, msg->getType());
#if defined(DEBUG) && defined(ARDUINO)
		debug.print("\nAdd SendQue Top size = ");
		debug.print(_queCnt, DEC);
		debug.print(" Msg=");
		debug.print(msg->getMsgTypeName());
#endif

    for(int i = _queCnt-1; i > 0; i--){
        _msg[i] = _msg[i - 1];
    }
    _msg[0] = new MqttsnMessage();
    _msg[0]->copy(msg);


        for(int i = 1; i < _queCnt; i++){
     	    D_MQTT( "  Msg = 0x");
			D_MQTT(_msg[i]->getType(), HEX);
			D_MQTTF("  Msg = 0x%x ", _msg[i]->getType());
#if defined(DEBUG) && defined(ARDUINO)
			debug.print("  Msg=");
			debug.print(_msg[i]->getMsgTypeName());
#endif
        }
        D_MQTTW("\r\n");
#if defined(DEBUG) && defined(ARDUINO)
		debug.println();
#endif

    return 0;
}

int SendQue::deleteRequest(uint8_t index){

	if ( index < _queCnt){

        delete _msg[index];
        _queCnt--;

    	D_MQTTW("\nDelete SendQue  Size = ");
    	D_MQTT(_queCnt, DEC);
    	D_MQTTF("%d", _queCnt);
#if defined(DEBUG) && defined(ARDUINO)
		debug.print("\nDelete SendQue  Size = ");
		debug.print(_queCnt, DEC);
#endif

        for(int i = index; i < _queCnt; i++){
            _msg[i] = _msg[i + 1];

            D_MQTT( "  Msg = 0x");
			D_MQTT(_msg[i]->getType(), HEX);
			D_MQTTF("  Msg = 0x%x ", _msg[i]->getType());
#if defined(DEBUG) && defined(ARDUINO)
			debug.print("  Msg=");
			debug.print(_msg[i]->getMsgTypeName());
#endif
        }

        D_MQTTW("\r\n");
#if defined(DEBUG) && defined(ARDUINO)
		debug.println();
#endif
        for(int i = _queCnt; i < _queSize; i++){
            _msg[i] = 0;
        }

        return 0;
    }
    return -2;
}

void   SendQue::deleteAllRequest(){
    while ( _queCnt > 0){
        deleteRequest(0);
    }
}

void SendQue::setStatus(uint8_t index, uint8_t status){
    if ( index < _queCnt){
        _msg[index]->setStatus(status);
    }
}

void SendQue::setQueSize(uint8_t sz){
  _queSize = sz;
}

MqttsnMessage* SendQue::getMessage(uint8_t index){
  if ( index < _queCnt){
      return _msg[index];
  }
  return 0;
}

int SendQue::getStatus(uint8_t index){
  if ( index < _queCnt){
      return _msg[index]->getStatus();
  }
  return -1;
}

uint8_t SendQue::getCount(){
   return _queCnt;
}


/*=====================================
        Class ClientStatus
 ======================================*/
ClientStatus::ClientStatus(){
	init();
	_gwId = 0;
	_sleepModeFlg = false;
	_onConnectFlg = false;
	_subscribingFlg = false;
}

ClientStatus::~ClientStatus(){
}

void ClientStatus::init(){
	_gwStat = GW_LOST;
	_clStat = CL_DISCONNECTED;

	_keepAliveDuration = MQTTSN_DEFAULT_DURATION;
	_advertiseDuration = MQTTSN_DEFAULT_KEEPALIVE;
	_advertiseTimer.stop();
	_keepAliveTimer.stop();
	_onConnectFlg = false;
	_subscribingFlg = false;
}


void ClientStatus::changeUTC(){
	_keepAliveTimer.changeUTC();   // changeUTC() is static
}

bool ClientStatus::isLost(){
	if(_gwStat == GW_LOST){
		return true;
	}else{
		return false;
	}
}

bool ClientStatus::isSearching(){
	if(_gwStat == GW_SEARCHING){
		return true;
	}else{
		return false;
	}
}


bool ClientStatus::isConnected(){
	if(_gwStat == GW_FIND && _clStat != CL_DISCONNECTED){
		return true;
	}else{
		return false;
	}
}

bool ClientStatus::isOnConnected(){
	return _onConnectFlg;
}

bool ClientStatus::isSubscribing(){
	return _subscribingFlg;
}

bool ClientStatus::isAvailableToSend(){
	if((_gwStat == GW_FIND) &&
		((_clStat == CL_ACTIVE) || (_clStat == CL_ASLEEP))){
		return true;
	}else{
		return false;
	}
}

bool ClientStatus::isPINGREQRequired(){
	if(_sleepModeFlg){
		_keepAliveTimer.changeUTC();
	}
	return (_keepAliveTimer.isTimeUp() && isConnected());
}

bool ClientStatus::isGatewayAlive(){
	if(_sleepModeFlg){
		_advertiseTimer.changeUTC();
	}
	return (_advertiseTimer.isTimeUp() ? false : true);
}

uint16_t ClientStatus::getKeepAlive(){
	return _keepAliveDuration;
}

void ClientStatus::setKeepAlive(uint16_t sec){
	_keepAliveDuration = sec;
}

void ClientStatus::sendSEARCHGW(){
	_gwStat = GW_SEARCHING;
}

bool ClientStatus::recvGWINFO(MqttsnGwInfo* gwi){
	bool rc = true;
	if (_gwStat == GW_SEARCHING){
		_gwStat = GW_FIND;
		if(_gwId != gwi->getGwId() && _gwId != 0){
			rc = false;
		}
		_gwId = gwi->getGwId();
	}
	return rc;
}

void ClientStatus::recvADVERTISE(MqttsnAdvertise* adv){
	if ( adv->getGwId() == _gwId ){
		_advertiseDuration = (adv->getDuration() > 60 ?
				adv->getDuration() + adv->getDuration() / 10 :
				adv->getDuration() + adv->getDuration() / 2);
		_advertiseTimer.start((uint32_t)(_advertiseDuration * 1000UL));
	}
}

void ClientStatus::recvCONNACK(){
	_onConnectFlg = true;
}

void ClientStatus::recvDISCONNECT(){
	_clStat = CL_DISCONNECTED;
	_advertiseTimer.stop();
}

void ClientStatus::setLastSendTime(){
	_keepAliveTimer.start((uint32_t)(_keepAliveDuration * 1000UL));
}


void ClientStatus::recvPINGRESP(){
	setLastSendTime();
}

void ClientStatus::setAvailableToSend(){
	_clStat = CL_ACTIVE;
	_onConnectFlg = false;
}

void ClientStatus::setSubscribing(bool stat){
	_subscribingFlg = stat;
}

void ClientStatus::setModeSleep(){
	_sleepModeFlg = true;
}


/*===================  End of file ====================*/


================================================
FILE: Client/src/lib/mqttsnClient.h
================================================
/*
 * mqttsnClient.h
 *
 *                      The BSD License
 *
 *           Copyright (c) 2014, tomoaki@tomy-tech.com
 *                    All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: 2014/06/01
 *    Modified: 2014/09/05
 *      Author: Tomoaki YAMAGUCHI
 *     Version: 1.0.0
 */

#ifndef MQTTSCLIENT_H_
#define MQTTSCLIENT_H_

#ifdef LINUX
		#define SENDQ_SIZE   20
#else
		#define SENDQ_SIZE    5
#endif

#if defined(ARDUINO) && ARDUINO >= 100
        #include "Arduino.h"
        #include <inttypes.h>
		#include <MQTTSN_Application.h>
		#include <Network.h>
        #include <mqttsn.h>
		#include <mqUtil.h>
#else
        #if defined(ARDUINO) && ARDUINO < 100
                #include "WProgram.h"
                #include <inttypes.h>
                #include <mqttsn.h>
				#include <Network.h>
				#include <mqUtil.h>
        #else
                #ifdef LINUX
                    #include <sys/time.h>
                #endif
                #include <iostream>
				#include "MQTTSN_Application.h"
				#include "Network.h"
				#include "mqUtil.h"
                #include "mqttsn.h"
        #endif
#endif


#define GW_LOST      0
#define GW_SEARCHING 1
#define GW_FIND      2

#define CL_DISCONNECTED  0
#define CL_ACTIVE        1
#define CL_ASLEEP        2
#define CL_AWAKE         3


using namespace tomyClient;

typedef struct {
	int (*callback)(void);
	uint32_t sec;
}TaskList;

typedef struct {
	MQString* topic;
	int (*pubCallback)(MqttsnPublish*);
	uint8_t qos;
}OnPublishList;

typedef struct {
	uint32_t prevTime;
	uint32_t interval;
	int (*callback)(void);
}MQ_TimerTbl;

/*=====================================
        Class ClientStatus
 ======================================*/
class ClientStatus{
public:
	ClientStatus();
	~ClientStatus();

	bool isLost();
	bool isSearching();
	bool isConnected();
	bool isOnConnected();
	bool isSubscribing();
	bool isAvailableToSend();
	bool isPINGREQRequired();
	bool isGatewayAlive();

	uint16_t getKeepAlive();
	void setKeepAlive(uint16_t sec);
	void sendSEARCHGW();
	bool recvGWINFO(MqttsnGwInfo* gwi);
	void recvADVERTISE(MqttsnAdvertise* adv);
	void recvCONNACK();
	void recvDISCONNECT();
	void recvPINGRESP();
	void setAvailableToSend();
	void setSubscribing(bool);
	void setLastSendTime();
	void setModeSleep();
	void init();


private:
	void changeUTC();

	uint8_t _gwId;
	uint8_t _gwStat;
	uint8_t _clStat;
	bool   _onConnectFlg;
	bool   _subscribingFlg;
	uint16_t _keepAliveDuration; // PINGREQ interval
	uint16_t _advertiseDuration; // Gateway heart beat
	XTimer  _keepAliveTimer;
	XTimer  _advertiseTimer;
	bool _sleepModeFlg;
};

/*=====================================
        Class SendQue  (FIFO)
 ======================================*/
class SendQue {
public:
    SendQue();
    ~SendQue();
    int addRequest(MqttsnMessage* msg);
    int addPriorityRequest(MqttsnMessage* msg);
    void setStatus(uint8_t index, uint8_t status);
    MqttsnMessage* getMessage(uint8_t index);
    int  getStatus(uint8_t index);
    uint8_t getCount();
    int deleteRequest(uint8_t index);
    void   deleteAllRequest();
    void setQueSize(uint8_t sz);
private:
    uint8_t   _queSize;
    uint8_t   _queCnt;
    MqttsnMessage*  _msg[SENDQ_SIZE];
};


/*=====================================
        Class MqttsnClient
 ======================================*/
class MqttsnClient {
public:
    MqttsnClient();
    ~MqttsnClient();

    Topics* getTopics();

    int initialize(APP_CONFIG config);
    void subscribe();
    void setSubscribing(bool);
    void setKeepAlive(uint16_t sec);
    void setWillTopic(MQString* topic);
    void setWillMessage(MQString* msg);
    void setSleepMode(void);
    void setRetain(bool retain);
    void setClean(bool clean);
    void setRetryMax(uint8_t cnt);
    void setGwAddress();
    MQString* getClientId();
    ClientStatus* getClientStatus();
    bool isCleanSession();


    int  publish(MQString* topic, const char* data, int dataLength, uint8_t qos = 1);
    int  publish(MQString* topic, MQString* data, uint8_t qos = 1);
    int  publish(uint16_t predifinedId,  const char* data, int dataLength, uint8_t qos = 1);
    int  publish(MQString* topic, Payload* payload, uint8_t qos = 1);
    int  registerTopic(MQString* topic);
    int  subscribe(MQString* topic, TopicCallback callback, uint8_t qos = 1);
    int  subscribe(uint16_t predefinedId, TopicCallback callback, uint8_t qos);
    int  unsubscribe(MQString* topic);
    int  unsubscribe(uint16_t predefinedId);
    int  disconnect(uint16_t duration = 0);
    void createTopics();

    void recieveMessageHandler(NWResponse* msg, int* returnCode);
    void publishHdl(MqttsnPublish* msg);
    void recvMsg(uint16_t msec);
    int  exec();
    int readPacket();
    uint8_t getMsgRequestCount();

private:
    int  sendRecvMsg();
    void clearMsgRequest();
    int  requestSendMsg(MqttsnMessage* msg);
    int  requestPrioritySendMsg(MqttsnMessage* mqttsMsgPtr);
    int  broadcast(uint16_t packetReadTimeout);
    int  unicast(uint16_t packetReadTimeout);

    int  searchGw(uint8_t radius);
    int  connect();
    int  pingReq(MQString* clietnId);
    int  willTopic();
    int  willMsg();
    int  pubAck(uint16_t topicId, uint16_t msgId, uint8_t rc);
    int  pubRec(uint16_t msgId);
    int  pubRel(uint16_t msgId);
    int  regAck(uint16_t topicId, uint16_t msgId, uint8_t rc);
    int  pubComp(uint16_t msgId);

    uint8_t getMsgRequestType();
    uint8_t getMsgRequestStatus();
    void   setMsgRequestStatus(uint8_t stat);
    void createTopic(MQString* topic, TopicCallback callback);

    void delayTime(uint16_t baseTime);
    void copyMsg(MqttsnMessage* msg, NWResponse* recvMsg);
    uint16_t getNextMsgId();

    Network*         _network;
    Topics           _topics;
    SendQue*         _sendQ;
    XTimer           _respTimer;
    PublishHandller  _pubHdl;

    uint16_t         _duration;
    MQString*        _clientId;
    uint8_t          _clientFlg;
    uint8_t          _nRetry;
    uint8_t          _nRetryCnt;
    uint16_t         _tRetry;
    MQString*         _willTopic;
    MQString*         _willMessage;
    uint16_t         _msgId;
    ClientStatus     _clientStatus;
    bool             _sendFlg;
    bool             _subscribingFlg;
};



#endif /* MQTTSCLIENT_H_ */


================================================
FILE: Client/src/lib/mqttsnClientAppFw4Arduino.cpp
================================================
/*
 * mqttsnClientAppAppFw4Arduino.cpp
 *
 *                      The BSD License
 *
 *           Copyright (c) 2014, tomoaki@tomy-tech.com
 *                    All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: 2014/06/01
 *    Modified: 2014/11/25
 *      Author: Tomoaki YAMAGUCHI
 *     Version: 1.1.1
 */

#ifdef ARDUINO
#include <MQTTSN_Application.h>
#include <mqttsnClientAppFw4Arduino.h>
#include <avr/power.h>
#include <avr/sleep.h>

#if defined(MQTTSN_DEBUG) || defined(ZBEE_DEBUG) || defined(DEBUG)
	#include <SoftwareSerial.h>
	extern SoftwareSerial debug;
#endif

using namespace std;
using namespace tomyClient;

volatile uint8_t MQ_ErrFlag = MQ_OFF;
volatile uint8_t MQ_cnt = 0;

enum MQ_INT_STATUS MQ_intStat;
enum MQ_INT_STATUS MQ_wdtStat;

extern uint32_t getUint32(uint8_t* pos);

extern APP_CONFIG theAppConfig;
extern TaskList theTaskList[];
extern void interruptCallback(void);

MqttsnClientApplication* theApplication = new MqttsnClientApplication();

void loop(){
	theApplication->registerInt0Callback(interruptCallback);
	theApplication->addTask();

	theApplication->setKeepAlive(theAppConfig.mqttsnCfg.keepAlive);
	theApplication->setClean(theAppConfig.mqttsnCfg.cleanSession);
	if(theAppConfig.mqttsnCfg.willTopic){
		MQString* willTpc = new MQString(theAppConfig.mqttsnCfg.willTopic);
		theApplication->setWillTopic(willTpc);
	}
	if(theAppConfig.mqttsnCfg.willMsg){
		MQString* willMsg = new MQString(theAppConfig.mqttsnCfg.willMsg);
		theApplication->setWillMessage(willMsg);
	}
	if(theAppConfig.mqttsnCfg.endDevice){
		theApplication->setZBPinHibernate();
	}

	theApplication->initialize(theAppConfig);

	if(theAppConfig.mqttsnCfg.endDevice){
		theApplication->startSleepMode();
	}else{
		theApplication->startWdt();
	}

	while(true){
		theApplication->run();
	}
}

/*-------------------------------
 * Set WDT Interrupt procedure
 --------------------------------*/
ISR (WDT_vect) {
	cli();
	MCUSR = 0;
	wdt_reset();
	WDTCSR |= B00011000;   // WDCE:on, WDE:on
	WDTCSR = 0x00;
	MQ_wdtStat = INT_WDT;
	sei();
}

/*-------------------------------
 * INT0 Interrupt procedure
 --------------------------------*/
void MQInt0(){
	detachInterrupt(0);
	MQ_intStat = INT0_LL;
}

/*-------------------------------
 * Set WDT
 --------------------------------*/
void MQwatchdogEnable(){  // Turn on WDT
	cli();
	MCUSR = 0;
	wdt_reset();
	WDTCSR |= B00011000;   // WDCE:on, WDE:on
	WDTCSR = MQ_WDT_TIME;
	MQ_wdtStat = WAIT;
	sei();
}

/*--------------------------------
   Dummy function
---------------------------------*/
void IntHandleDummy(){
}

/*--------------------------------
        reset Arduino
---------------------------------*/
//void (*resetArduino)(void) = 0;
void resetArduino(){
#ifdef DEBUG
		debug.println("Reset");
#endif
	asm volatile("jmp 0000");
}

/*--------------------------------
        set UnixTime
---------------------------------*/
int setUTC(MqttsnPublish* msg){
	XTimer::setUnixTime(getUint32(msg->getData()));
	return 0;
}

/*========================================
		Class MqttsnClientApplication
=========================================*/
MqttsnClientApplication::MqttsnClientApplication(){
    _txFlag = false;
    _intHandler = IntHandleDummy;
    _sleepFlg = false;
    _deviceType = ZB_ROUTER_DEVICE;

}

MqttsnClientApplication::~MqttsnClientApplication(){

}

void MqttsnClientApplication::addTask(){
	for(int i = 0; theTaskList[i].callback; i++){
		_wdTimer.registerCallback(theTaskList[i].sec, theTaskList[i].callback);
	}
}

void MqttsnClientApplication::setSubscribe(){
	_mqttsn.setSubscribing(true);  // re-entrant control
	_mqttsn.subscribe();
    _mqttsn.subscribe(MQTTSN_TOPICID_PREDEFINED_TIME, setUTC,QOS0);
    _mqttsn.setSubscribing(false);
}

void MqttsnClientApplication::initialize(APP_CONFIG config){
	//blinkIndicator(100);
    _mqttsn.initialize(config);
    XTimer::initialize();
    setSubscribe();
}

int MqttsnClientApplication::run(){
	wakeupXB();
	_mqttsn.readPacket(); //ModemeStatus or Stored packet
	_mqttsn.readPacket();
	checkInterupt();   // WDT routines are executed

	int rc = _mqttsn.exec();
	if(rc == MQTTSN_ERR_REBOOT_REQUIRED){
		resetArduino();
	}else if(rc == MQTTSN_ERR_NO_ERROR){
		sleepXB();
		sleepApp();   // waiting WDT interruption
	}
	return 0;
}


void MqttsnClientApplication::setKeepAlive(uint16_t sec){
    _mqttsn.setKeepAlive(sec);
}


void MqttsnClientApplication::setWillTopic(MQString* willTopic){
    _mqttsn.setWillTopic(willTopic);
}

void MqttsnClientApplication::setWillMessage(MQString* willMsg){
    _mqttsn.setWillMessage(willMsg);
}

void MqttsnClientApplication::setRetain(bool retain){
    _mqttsn.setRetain(retain);
}

void MqttsnClientApplication::setClean(bool clean){
    _mqttsn.setClean(clean);
}

/*----------- Sleep related functions ------------*/
void MqttsnClientApplication::startWdt(){
    _wdTimer.start();
}

void MqttsnClientApplication::stopWdt(){
    _wdTimer.stop();
}

void MqttsnClientApplication::sleepApp(){
	if(_deviceType == ZB_PIN_HIBERNATE && _sleepFlg){
#if defined(DEBUG) && defined(ARDUINO)
		debug.println("sleep");
#endif
		set_sleep_mode(SLEEP_MODE_PWR_SAVE);
		MQwatchdogEnable();
		sleep_enable();
		MQ_wdtStat = WAIT;
		sleep_mode();      // waiting WDT interrupt
		sleep_disable();
		XTimer::setStopTimeDuration(MQ_WDT_TIME_MSEC);
	}
}

void MqttsnClientApplication::sleepXB(){
	if(_deviceType == ZB_PIN_HIBERNATE && _sleepFlg){
		pinMode(XB_SLEEP_PIN, INPUT);
	}
}

void MqttsnClientApplication::wakeupXB(){
	if(_deviceType == ZB_PIN_HIBERNATE){
		pinMode(XB_SLEEP_PIN, OUTPUT);
		digitalWrite(XB_SLEEP_PIN, LOW);
		delay(20);
	}
}

void MqttsnClientApplication::startSleepMode(){
	if(_deviceType == ZB_PIN_HIBERNATE){
		_sleepFlg = true;
		MQ_intStat = WAIT;
		MQ_wdtStat = WAIT;
		setInterrupt();
	}else{
		_sleepFlg = false;
	}
}

void MqttsnClientApplication::setZBPinHibernate(){
	_deviceType = ZB_PIN_HIBERNATE;
	pinMode(MQ_INT0_PIN,INPUT_PULLUP);
	wakeupXB();
	_mqttsn.getClientStatus()->setModeSleep();
}

/*-------------- Interrupt related functions ------*/
void MqttsnClientApplication::checkInterupt(){

    // interrupt Event
    if (MQ_intStat == INT0_LL){
        MQ_intStat = INT0_WAIT_HL;
        interruptHandler();
        setInterrupt();
    }
    // WDT event
    if (MQ_wdtStat == INT_WDT){
		_wdTimer.wakeUp();    // check interval & execute callback
        _wdTimer.start();     // WDT restart
    }
}

void MqttsnClientApplication::interruptHandler(){
    wakeupXB();
    _intHandler();
    sleepXB();
}

void MqttsnClientApplication::setInterrupt(){
    if (MQ_intStat == INT0_WAIT_HL){
		while(digitalRead(MQ_INT0_PIN) == 0){
				// wait LL to HL
		}
    }
    MQ_intStat = WAIT;
    attachInterrupt(0,MQInt0,LOW);
}


/*--------------------  MQTT-SN functions ---------------*/

int MqttsnClientApplication::registerTopic(MQString* topic){
    return _mqttsn.registerTopic(topic);
}

int MqttsnClientApplication::publish(MQString* topic, const char* data, int dataLength, uint8_t qos){
	return _mqttsn.publish(topic, data, dataLength, qos);
}

int MqttsnClientApplication::publish(MQString* topic, Payload* payload, uint8_t qos){
	return _mqttsn.publish(topic, payload, qos);
}

int MqttsnClientApplication::subscribe(MQString* topic, TopicCallback callback, uint8_t qos){
    return _mqttsn.subscribe(topic, callback,qos);
}

int MqttsnClientApplication::subscribe(uint16_t predefinedId, TopicCallback callback, uint8_t qos){
    return _mqttsn.subscribe(predefinedId, callback, qos);
}

int MqttsnClientApplication::publish(uint16_t predefinedId, const char* data, int dataLength, uint8_t qos){
    return _mqttsn.publish(predefinedId, data, dataLength, qos);
}

int MqttsnClientApplication::unsubscribe(MQString* topic){
    return _mqttsn.unsubscribe(topic);
}

int MqttsnClientApplication::disconnect(uint16_t duration){
    return _mqttsn.disconnect(duration);
}

/*------------- UTC functions -------------*/
uint32_t MqttsnClientApplication::getUnixTime(){
	return XTimer::getUnixTime();
}
/*
#define LEAP_YEAR(Y)  ( ((1970+Y)>0) && !((1970+Y)%4) && ( ((1970+Y)%100) || !((1970+Y)%400) ) )
static const uint8_t monthDays[]={31,28,31,30,31,30,31,31,30,31,30,31};

void MqttsnClientApplication::getDateTime(char* buf) {
	uint16_t year;
	uint8_t month;
	uint8_t mLen;
	uint8_t day;
	uint8_t hour;
	uint8_t min;
	uint8_t sec;
	uint16_t days;
	uint32_t utc;

	utc = getUnixTime();

	sec = utc % 60;
	utc /= 60;
	min = utc % 60;
	utc /= 60;
	hour = utc % 24;
	utc /= 24;

	year = days = 0;
	while((unsigned)(days += (LEAP_YEAR(year) ? 366 : 365)) <= utc) {
	    year++;
	}
	days -= LEAP_YEAR(year) ? 366 : 365;
	year += 1970;
	utc  -= days;

	days = 0;
	for (month = 0; month < 12; month++) {
		if (month == 1) {
		  if (LEAP_YEAR(year)) {
			  mLen = 29;
		  } else {
			  mLen = 28;
		  }
		} else {
			mLen = monthDays[month];
		}
		if (utc >= mLen) {
		  utc -= mLen;
		} else {
			break;
		}
	}
	month += 1;
	day = utc + 1;

	sprintf(buf,"GMT%4d%2d%2d%2d%2d%2d",year,month,day,hour,min,sec);
	char* pos = buf + 7;
	for(int i = 0; i < 10; i++){
		if(*( pos + i) == 0x20){
			*(pos + i) = 0x30;
		}
	}
}
*/

/*-------------- Indicator --------------*/
void MqttsnClientApplication::indicatorOn(){
    digitalWrite(MQ_LED_PIN,MQ_ON);
}

void MqttsnClientApplication::indicatorOff(){
    digitalWrite(MQ_LED_PIN,MQ_OFF);
}

void MqttsnClientApplication::blinkIndicator(int msec){
    digitalWrite(MQ_LED_PIN,MQ_ON);
    delay(msec);
    digitalWrite(MQ_LED_PIN,MQ_OFF);
}


/*-------------- Callback related functions ---------------*/
void MqttsnClientApplication::registerInt0Callback(void (*callback)()){
    _intHandler = callback;
}

void MqttsnClientApplication::refleshWdtCallbackTable(){
  _wdTimer.refleshRegisterTable();
}

/*======================================
               Class WdTimer
========================================*/
WdTimer::WdTimer(void) {
    _timerTbls = 0;
    _timerCnt = 0;
    _initFlg = true;

}

void WdTimer::start(void) {    
	MQwatchdogEnable();
}


void WdTimer::stop(void){
    cli();
    MCUSR = 0;
    wdt_reset();
    WDTCSR |= B00011000;   // WDCE:on, WDE:on
    WDTCSR = 0x00;
    MQ_wdtStat = WAIT;
    sei();
}


bool WdTimer::wakeUp(void){
    bool rcflg = false;
    int rc;
	for(uint8_t i = 0; i < _timerCnt; i++) {
		if ((_timerTbls[i].prevTime + _timerTbls[i].interval < getUnixTime()) || _initFlg){
#if defined(DEBUG) && defined(ARDUINO)
		debug.println("Task start");
#endif
			rc = (_timerTbls[i].callback)();
			if(rc == MQTTSN_ERR_REBOOT_REQUIRED || rc == MQTTSN_ERR_INVALID_TOPICID){
				resetArduino();
			}
			_timerTbls[i].prevTime = getUnixTime();
			rcflg = true;

#if defined(DEBUG) && defined(ARDUINO)
		debug.println("Task end");
#endif
		}
	}
	_initFlg = false;
    return rcflg;
}

int WdTimer::registerCallback(uint32_t sec, int (*callback)(void)){
    MQ_TimerTbl *savTbl = _timerTbls;
    MQ_TimerTbl *newTbl = (MQ_TimerTbl*)calloc(_timerCnt + 1,sizeof(MQ_TimerTbl));

    if ( newTbl != 0 ) {
        _timerTbls = newTbl;
        for(uint8_t i = 0; i < _timerCnt; i++ ){
			_timerTbls[i].prevTime = savTbl[i].prevTime;
			_timerTbls[i].interval = savTbl[i].interval;
			_timerTbls[i].callback = savTbl[i].callback;
        }
        free(savTbl);

        //_timerTbls[_timerCnt].prevTime = getUnixTime();
        _timerTbls[_timerCnt].prevTime = (uint32_t)0;
        _timerTbls[_timerCnt].interval = sec;
        _timerTbls[_timerCnt].callback = callback;
        _timerCnt++;
        return MQTTSN_ERR_NO_ERROR;
    }
    return MQTTSN_ERR_OUT_OF_MEMORY;
} 

void WdTimer::refleshRegisterTable(){
    for(uint8_t i = 0; i < _timerCnt; i++) {
        _timerTbls[i].prevTime = getUnixTime();
    }
}



#endif  /* ARDUINO */


================================================
FILE: Client/src/lib/mqttsnClientAppFw4Arduino.h
================================================
/*
 * mqttsnClientAppFw4Arduino.h
 *
 *                      The BSD License
 *
 *           Copyright (c) 2014, tomoaki@tomy-tech.com
 *                    All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: 2014/06/01
 *    Modified: 2014/09/05
 *      Author: Tomoaki YAMAGUCHI
 *     Version: 1.0.0
 */

#ifdef ARDUINO

#ifndef MQTTSNCLIENTAPPLICATION_H_
#define MQTTSNCLIENTAPPLICATION_H_

#include <MQTTSN_Application.h>
#include <mqttsnClient.h>
#include <mqUtil.h>
#include <avr/wdt.h>
#include <avr/sleep.h>
#include <avr/interrupt.h>
#include <inttypes.h>
#include <Ethernet.h>
#include <EthernetUdp.h>
#include <SPI.h>

#define MQ_LED_PIN  13
#define MQ_INT0_PIN 2
#define MQ_SLEEP_PIN 4  // Connect to XBee DTR for hibernation mode
#define MQ_ERROR_RECOVERY_DURATION_ON 8
#define MQ_ON      1
#define MQ_OFF     0

#define ZB_ROUTER_DEVICE  0
#define ZB_PIN_HIBERNATE 1


#define MQ_WDT_ERR   (B01100000)  // Error Indication time

#define MQ_WDT_TIME (B01000110)   // 1 Sec
#define MQ_WDT_TIME_MSEC   1000

//#define MQ_WDT_TIME (B01000111)   // 2 Secs
//#define MQ_WDT_TIME_MSEC   2000

//#define MQ_WDT_TIME (B01100000)   // 4 Secs
//#define MQ_WDT_TIME_MSEC   4000
 
//#define MQ_WDT_TIME (B01100001)   // 8 Secs
//#define MQ_WDT_TIME_MSEC   8000


enum MQ_INT_STATUS{ WAIT, INT0_LL, INT0_WAIT_HL, INT_WDT};


#define INDICATOR_ON()   theApplication->indicatorOn()
#define INDICATOR_OFF()  theApplication->indicatorOff()
#define BLINK_INDICATOR(...) theApplication->blinkIndicator(__VA_ARGS__)
#define GETUTC()         theApplication->getUnixTime()
//#define GET_DATETIME(...) theApplication->getDateTime(__VA_ARGS__)


extern void setUint32(uint8_t*, uint32_t);
extern uint32_t getUint32(uint8_t*);
extern void setUint16(uint8_t*, uint16_t);
extern uint16_t getUint16(uint8_t*);


/*======================================
               Class WdTimer
========================================*/
class WdTimer:public XTimer {
public:
	WdTimer(void);
	int  registerCallback(uint32_t sec, int (*proc)(void));
	void refleshRegisterTable();
	void start(void);
	void stop(void);
	bool wakeUp(void);

private:	
	MQ_TimerTbl *_timerTbls;
	uint8_t _timerCnt;
	bool _initFlg;
};


/*======================================
       Class MqttsnClientApplication
========================================*/
class MqttsnClientApplication{
public:
	MqttsnClientApplication();
	~MqttsnClientApplication();
	void registerInt0Callback(void (*callback)());
	void registerWdtCallback(long sec, int (*callback)(void));
	void refleshWdtCallbackTable();
	int  initialize(APP_CONFIG config);
	int  setSubscribe();
	void setKeepAlive(uint16_t msec);
	void setWillTopic(MQString* willTopic);
	void setWillMessage(MQString* willMsg);
	void setRetain(bool retain);
	void setClean(bool clean);
	void setClientId(MQString* id);
	void setZBPinHibernate();
	void blinkIndicator(int msec);
	void startSleepMode();
	void subscribe(void);
	void addTask(void);
	
	int registerTopic(MQString* topic);
	int publish(MQString* topic, const char* data, int dataLength, uint8_t qos = 1);
	int publish(uint16_t predefinedId, const char* data, int dataLength, uint8_t qos = 1);
	int publish(MQString* topic, Payload* payload, uint8_t qos);
	int subscribe(MQString* topic, TopicCallback callback, uint8_t qos = 1);
	int subscribe(uint16_t predefinedId, TopicCallback callback, uint8_t qos = 1);
	int unsubscribe(MQString* topic);
	int disconnect(uint16_t duration);

	void startWdt();
	void stopWdt();
	int  run();
	void setUnixTime(MqttsnPublish* msg);
	uint32_t getUnixTime();
	void getDateTime(char* buf);
	void reboot();
	void indicatorOn();
	void indicatorOff();

private:
	void checkInterupt();
	void interruptHandler();
    void setInterrupt();
	void sleepXB();
	void wakeupXB();
	void sleepApp();
	
	MqttsnClient _mqttsn;
	bool _txFlag;
	bool  _sleepFlg;
	uint8_t _deviceType;

	WdTimer _wdTimer;
	XTimer _keepAliveTimer;
	XTimer _advertiseTimer;

	void (*_intHandler)(void);

};

extern MqttsnClientApplication* theApplication;

#else

#endif /*ARDUINO*/




#endif /* MQTTSNCLIENTAPPLICATION_H_ */


================================================
FILE: Client/src/lib/mqttsnClientAppFw4Linux.cpp
================================================
/*
 * mqttsnClientAppAppFwLinux.cpp
 *
 *                      The BSD License
 *
 *           Copyright (c) 2014, tomoaki@tomy-tech.com
 *                    All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: 2014/06/01
 *    Modified: 2014/09/05
 *      Author: Tomoaki YAMAGUCHI
 *     Version: 1.0.0
 */

#ifndef ARDUINO
 
#include "MQTTSN_Application.h"

#ifdef LINUX
#include "mqttsnClientAppFw4Linux.h"
#include <unistd.h>
#include <time.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <termios.h>

using namespace std;
using namespace tomyClient;

MqttsnClientApplication* theApplication = new MqttsnClientApplication();

extern TaskList theTaskList[];
extern void  setup();
extern MQString theTopics[];

#ifdef NETWORK_XBEE
XBeeAppConfig  theAppConfig = { { 0, 0, 0 },{ 0, 0, false, false, 0, 0 } };
#endif

#ifdef NETWORK_UDP
UdpAppConfig   theAppConfig = {{ {0,0,0,0}, 0, {0,0,0,0}, 0, {0,0,0,0,0,0} },{ 0, 0, false, false, 0, 0 } };
#endif


/*========================================
		main function
=========================================*/
int main(int argc, char **argv){
	theApplication->addTask();
	setup();
	theApplication->initialize(argc, argv);
	theApplication->run();
	return 0;
}

/*========================================
		Class MqttsnClientApplication
=========================================*/
MqttsnClientApplication::MqttsnClientApplication(){

}

MqttsnClientApplication::~MqttsnClientApplication(){

}


void MqttsnClientApplication::startWdt(){
    _wdTimer.start();
}

void MqttsnClientApplication::stopWdt(){
    _wdTimer.stop();
}

/*------------ Client execution  forever --------------*/
int MqttsnClientApplication::run(){
	while(true){
		_wdTimer.wakeUp();
		_mqttsn.readPacket();
		int rc = _mqttsn.exec();
		if(rc == MQTTSN_ERR_REBOOT_REQUIRED){
			_mqttsn.subscribe();
		}
	}
	return 0;
}


void MqttsnClientApplication::initialize(int argc, char** argv){
	int arg;
	unsigned int val;
	char* id = 0;
	MQString* willTopic;
	MQString* willMsg;
#ifdef NETWORK_XBEE
	unsigned int br = B57600;
	char* dev = 0;
#endif
#ifdef NETWORK_UDP
	char* ipAddr = 0;
	uint16_t gPortNo = 0;
	uint16_t uPortNo = 0;
#endif

	while((arg = getopt(argc, argv, "hcb:d:u:i:k:t:m:g:p:"))!= -1){
		switch(arg){
		case 'h':
			printf("Usage:  -b: [baudrate]      (XBee)\n");
			printf("        -d: [device]        (XBee)\n");
			printf("        -g: [groupIp]       (UDP)\n");
			printf("        -p: [group portNo]  (UDP)\n");
			printf("        -u: [client portNo] (UDP)\n");
			printf("        -c: CleanSession\n");
			printf("        -i: [ClientId]\n");
			printf("        -k: [keepAliveTime in second]\n");
			printf("        -t: [willTopic]\n");
			printf("        -m: [willMessage]\n");
			exit(0);
			break;
		case 'c':
			_mqttsn.setClean(true);
			break;

		case 'i':
			id = strdup(optarg);
			break;
		case 't':
			willTopic = new MQString((const char*)optarg);
			_mqttsn.setWillTopic(willTopic);
			break;
		case 'm':
			willMsg = new MQString((const char*)optarg);
			_mqttsn.setWillMessage(willMsg);
			break;
		case 'k':
			val = atoi(optarg);
			_mqttsn.setKeepAlive(val);
			break;
#ifdef NETWORK_XBEE
		case 'd':
			dev = strdup(optarg);
			break;
		case 'b':
			val = atoi(optarg);
			switch(val){
			case 9600:
				br = B9600;
				break;
			case 19200:
				br =B19200;
				break;
			case 38400:
				br =B38400;
				break;
			case 57600:
				br =B57600;
				break;
			case 115200:
				br = B115200;
				break;
			default:
				printf("Invalid baudrate!\n");
				exit(-1);
			}
			break;
#endif
#ifdef NETWORK_UDP
		case 'g':
			ipAddr = optarg;
			break;
		case 'p':
			gPortNo = atoi(optarg);
			break;
		case 'u':
			uPortNo = atoi(optarg);
			break;
#endif
		case '?':
			printf("Unrecognized option! %c\n", arg);
			exit(-1);
		}
	}

#ifdef NETWORK_XBEE
    if(br && dev){
        theAppConfig.netCfg.baudrate = br;
        theAppConfig.netCfg.device = strdup(dev);
    }else{
        printf("argument error\n");
        exit(1);
    }
#endif

#ifdef NETWORK_UDP
	if(gPortNo && ipAddr[0] && uPortNo){
		theAppConfig.netCfg.gPortNo = gPortNo;
		theAppConfig.netCfg.uPortNo = uPortNo;
		uint32_t ipaddr = inet_addr(ipAddr);
		theAppConfig.netCfg.ipAddress[0] = (ipaddr & 0xff000000) >> 24;
		theAppConfig.netCfg.ipAddress[1] = (ipaddr & 0x00ff0000) >> 16;
		theAppConfig.netCfg.ipAddress[2] = (ipaddr & 0x0000ff00) >> 8;
		theAppConfig.netCfg.ipAddress[3] = (ipaddr & 0x000000ff);
	}else{
		printf("argument error\n");
		exit(1);
	}
#endif

	if(id){
		theAppConfig.mqttsnCfg.nodeId = strdup(id);
	}else{
		theAppConfig.mqttsnCfg.nodeId ="node";
	}

	_mqttsn.initialize(theAppConfig);

	setSubscribe();
}

void MqttsnClientApplication::setSubscribe(){
	_mqttsn.setSubscribing(true);
    _mqttsn.createTopics();
	_mqttsn.subscribe();
	_mqttsn.setSubscribing(false);
}

void MqttsnClientApplication::addTask(){
	for(int i = 0; theTaskList[i].sec; i++){
		_wdTimer.registerCallback(theTaskList[i].sec, theTaskList[i].callback);
	}
}

int MqttsnClientApplication::publish(MQString* topic, const char* data, int dataLength, uint8_t qos){
	return _mqttsn.publish(topic, data, dataLength, qos);
}

int MqttsnClientApplication::publish(uint16_t predefinedId, const char* data, int dataLength, uint8_t qos){
    return _mqttsn.publish(predefinedId, data, dataLength, qos);
}

int MqttsnClientApplication::publish(MQString* topic, Payload* payload, uint8_t qos){
	return _mqttsn.publish(topic, payload, qos);
}

int MqttsnClientApplication::subscribe(MQString* topic, TopicCallback callback, uint8_t qos){
    return _mqttsn.subscribe(topic, callback,qos);
}

int MqttsnClientApplication::subscribe(uint16_t predefinedId, TopicCallback callback, uint8_t qos){
    return _mqttsn.subscribe(predefinedId, callback, qos);
}


int MqttsnClientApplication::unsubscribe(MQString* topic){
    return _mqttsn.unsubscribe(topic);
}

int MqttsnClientApplication::disconnect(uint16_t duration){
    return _mqttsn.disconnect(duration);
}

void MqttsnClientApplication::setRetain(bool flag){
    _mqttsn.setRetain(flag);
}



/*-------------- Callback related functions ---------------*/


void MqttsnClientApplication::refleshWdtCallbackTable(){
  _wdTimer.refleshRegisterTable();
}

/*======================================
               Class WdTimer
========================================*/
WdTimer::WdTimer(void) {
    _timerTbls = 0;
    _timerCnt = 0;
    _initFlg = true;

}

void WdTimer::start(void) {    
	//MQwatchdogEnable();
}


void WdTimer::stop(void){
    //
}


bool WdTimer::wakeUp(void){
    bool rcflg = false;
    int rc;
	for(uint8_t i = 0; i < _timerCnt; i++) {
		if ((_timerTbls[i].prevTime + _timerTbls[i].interval < (uint32_t)time(0)) || _initFlg){
			rc = (_timerTbls[i].callback)();
			if(rc == MQTTSN_ERR_REBOOT_REQUIRED){
				theApplication->setSubscribe();
			}
			_timerTbls[i].prevTime = time(0);
			rcflg = true;
		}
	}
	_initFlg = false;
    return rcflg;
}

int WdTimer::registerCallback(uint32_t sec, int (*callback)(void)){
    MQ_TimerTbl *savTbl = _timerTbls;
    MQ_TimerTbl *newTbl = (MQ_TimerTbl*)calloc((unsigned int)_timerCnt + 1,sizeof(MQ_TimerTbl));

    if ( newTbl != 0 ) {
        _timerTbls = newTbl;
        for(uint8_t i = 0; i < _timerCnt; i++ ){
			_timerTbls[i].prevTime = savTbl[i].prevTime;
			_timerTbls[i].interval = savTbl[i].interval;
			_timerTbls[i].callback = savTbl[i].callback;
        }
        free(savTbl);

        _timerTbls[_timerCnt].prevTime = time(0);
        _timerTbls[_timerCnt].interval = sec;
        _timerTbls[_timerCnt].callback = callback;
        _timerCnt++;
        return MQTTSN_ERR_NO_ERROR;
    }
    return MQTTSN_ERR_OUT_OF_MEMORY;
} 

void WdTimer::refleshRegisterTable(){
    for(uint8_t i = 0; i < _timerCnt; i++) {
        _timerTbls[i].prevTime = time(0);
    }
}


#endif
#endif


================================================
FILE: Client/src/lib/mqttsnClientAppFw4Linux.h
================================================
/*
 * mqttsnClientAppFw4Linux.h
 *
 *                      The BSD License
 *
 *           Copyright (c) 2014, tomoaki@tomy-tech.com
 *                    All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: 2014/06/01
 *    Modified: 2014/09/05
 *      Author: Tomoaki YAMAGUCHI
 *     Version: 1.0.0
 */

#ifdef LINUX

#ifndef MQTTSNCLIENTAPPLICATION_H_
#define MQTTSNCLIENTAPPLICATION_H_

#include "MQTTSN_Application.h"
#include "mqttsnClient.h"
#include "mqUtil.h"


/*======================================
               Class WdTimer
========================================*/
class WdTimer:public XTimer {
public:
	WdTimer(void);
	int  registerCallback(uint32_t sec, int (*proc)(void));
	void refleshRegisterTable();
	void start(void);
	void stop(void);
	bool wakeUp(void);

private:	
	MQ_TimerTbl *_timerTbls;
	uint8_t _timerCnt;
	bool _initFlg;
};


/*======================================
       Class MqttsnClientApplication
========================================*/
class MqttsnClientApplication{
public:
	MqttsnClientApplication();
	~MqttsnClientApplication();
	void refleshWdtCallbackTable();
	void initialize(int argc, char** argv);
	void setSubscribe();

	int publish(MQString* topic, const char* data, int dataLength, uint8_t qos = 1);
	int publish(uint16_t predefinedId, const char* data, int dataLength, uint8_t qos = 1);
	int publish(MQString* topic, Payload* payload, uint8_t qos);
	int subscribe(MQString* topic, TopicCallback callback, uint8_t qos = 1);
	int subscribe(uint16_t predefinedId, TopicCallback callback, uint8_t qos = 1);
	int unsubscribe(MQString* topic);
	int disconnect(uint16_t duration);
	void setRetain(bool flag);

	void addTask();
	void startWdt();
	void stopWdt();
	int  run();


private:
	MqttsnClient _mqttsn;
	WdTimer _wdTimer;
	XTimer _keepAliveTimer;
	XTimer _advertiseTimer;

};

extern MqttsnClientApplication* theApplication;



#else

#endif /*LINUX*/




#endif /* MQTTSCLIENTAPPLICATION_H_ */


================================================
FILE: Client/src/lib/mqttsnClientAppFw4mbed.cpp
================================================
/*
 * mqttsnClientAppAppFwmbed.cpp
 *
 *                      The BSD License
 *
 *           Copyright (c) 2014, tomoaki@tomy-tech.com
 *                    All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: 2014/06/01
 *    Modified: 2014/09/05
 *      Author: Tomoaki YAMAGUCHI
 *     Version: 1.0.0
 */

#ifndef ARDUINO

#include "MQTTSN_Application.h"
#ifdef MBED
#include "mqttsnClientAppFw4mbed.h"
#include <time.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>

using namespace std;
using namespace tomyClient;

MqttsnClientApplication* theApplication = new MqttsnClientApplication();

extern TaskList theTaskList[];
extern OnPublishList theOnPublishList[];
extern APP_CONFIG    theAppConfig;
extern void  setup();

/*========================================
		main function
=========================================*/
int main(){
	theApplication->setKeepAlive(theAppConfig.mqttsnCfg.keepAlive);
	theApplication->setClean(theAppConfig.mqttsnCfg.cleanSession);
	if(theAppConfig.mqttsnCfg.willTopic){
		MQString* willTpc = new MQString(theAppConfig.mqttsnCfg.willTopic);
		theApplication->setWillTopic(willTpc);
	}
	if(theAppConfig.mqttsnCfg.willMsg){
		MQString* willMsg = new MQString(theAppConfig.mqttsnCfg.willMsg);
		theApplication->setWillMessage(willMsg);
	}

	theApplication->addTask();
	setup();
	theApplication->initialize(theAppConfig);
	theApplication->run();
	return 0;
}

/*========================================
		Class MqttsnClientApplication
=========================================*/
/*--------------------------------
        set UnixTime
---------------------------------*/
int setUTC(MqttsnPublish* msg){
	set_time(getUint32(msg->getData()));
	return 0;
}

MqttsnClientApplication::MqttsnClientApplication(){

}

MqttsnClientApplication::~MqttsnClientApplication(){

}


void MqttsnClientApplication::startWdt(){
    _wdTimer.start();
}

void MqttsnClientApplication::stopWdt(){
    _wdTimer.stop();
}

/*------------ Client execution  forever --------------*/
int MqttsnClientApplication::run(){
	while(true){
		_wdTimer.wakeUp();
		_mqttsn.readPacket();
		int rc = _mqttsn.exec();
		if(rc == MQTTSN_ERR_REBOOT_REQUIRED){
			_mqttsn.subscribe();
		}
	}
}


void MqttsnClientApplication::initialize(APP_CONFIG config){
	_mqttsn.initialize(config);
	setSubscribe();
}

void MqttsnClientApplication::setSubscribe(){
	_mqttsn.setSubscribing(true);  // re-entrant control
	_mqttsn.subscribe();
	_mqttsn.subscribe(MQTTSN_TOPICID_PREDEFINED_TIME, setUTC,1);
	_mqttsn.setSubscribing(false);
}
	
void MqttsnClientApplication::addTask(){
	for(int i = 0; theTaskList[i].sec; i++){
		_wdTimer.registerCallback(theTaskList[i].sec, theTaskList[i].callback);
	}
}

void MqttsnClientApplication::setKeepAlive(uint16_t sec){
    _mqttsn.setKeepAlive(sec);
}


void MqttsnClientApplication::setWillTopic(MQString* willTopic){
    _mqttsn.setWillTopic(willTopic);
}

void MqttsnClientApplication::setWillMessage(MQString* willMsg){
    _mqttsn.setWillMessage(willMsg);
}

void MqttsnClientApplication::setRetain(bool retain){
    _mqttsn.setRetain(retain);
}

void MqttsnClientApplication::setClean(bool clean){
    _mqttsn.setClean(clean);
}

int MqttsnClientApplication::publish(MQString* topic, const char* data, int dataLength, uint8_t qos){
	return _mqttsn.publish(topic, data, dataLength, qos);
}

int MqttsnClientApplication::publish(uint16_t predefinedId, const char* data, int dataLength, uint8_t qos){
    return _mqttsn.publish(predefinedId, data, dataLength, qos);
}

int MqttsnClientApplication::publish(MQString* topic, Payload* payload, uint8_t qos){
	return _mqttsn.publish(topic, payload, qos);
}


int MqttsnClientApplication::subscribe(MQString* topic, TopicCallback callback, uint8_t qos){
    return _mqttsn.subscribe(topic, callback,qos);
}

int MqttsnClientApplication::subscribe(uint16_t predefinedId, TopicCallback callback, uint8_t qos){
    return _mqttsn.subscribe(predefinedId, callback, qos);
}


int MqttsnClientApplication::unsubscribe(MQString* topic){
    return _mqttsn.unsubscribe(topic);
}

int MqttsnClientApplication::disconnect(uint16_t duration){
    return _mqttsn.disconnect(duration);
}

/*-------------- Callback related functions ---------------*/


void MqttsnClientApplication::refleshWdtCallbackTable(){
  _wdTimer.refleshRegisterTable();
}

/*======================================
               Class WdTimer
========================================*/
WdTimer::WdTimer(void) {
    _timerTbls = 0;
    _timerCnt = 0;
    _initFlg = true;

}

void WdTimer::start(void) {    
	//MQwatchdogEnable();
}


void WdTimer::stop(void){
    //
}


bool WdTimer::wakeUp(void){
    bool rcflg = false;
    int rc;
	for(uint8_t i = 0; i < _timerCnt; i++) {
		if ((_timerTbls[i].prevTime + _timerTbls[i].interval < (uint32_t)time(0)) || _initFlg){
			rc = (_timerTbls[i].callback)();
			if(rc == MQTTSN_ERR_REBOOT_REQUIRED){
				theApplication->initialize(theAppConfig);
			}
			_timerTbls[i].prevTime = time(0);
			rcflg = true;
		}
	}
	_initFlg = false;
    return rcflg;
}

int WdTimer::registerCallback(uint32_t sec, int (*callback)(void)){
    MQ_TimerTbl *savTbl = _timerTbls;
    MQ_TimerTbl *newTbl = (MQ_TimerTbl*)calloc(_timerCnt + 1,sizeof(MQ_TimerTbl));

    if ( newTbl != 0 ) {
        _timerTbls = newTbl;
        for(uint8_t i = 0; i < _timerCnt; i++ ){
			_timerTbls[i].prevTime = savTbl[i].prevTime;
			_timerTbls[i].interval = savTbl[i].interval;
			_timerTbls[i].callback = savTbl[i].callback;
        }
        free(savTbl);

        _timerTbls[_timerCnt].prevTime = time(0);
        _timerTbls[_timerCnt].interval = sec;
        _timerTbls[_timerCnt].callback = callback;
        _timerCnt++;
        return MQTTSN_ERR_NO_ERROR;
    }
    return MQTTSN_ERR_OUT_OF_MEMORY;
} 

void WdTimer::refleshRegisterTable(){
    for(uint8_t i = 0; i < _timerCnt; i++) {
        _timerTbls[i].prevTime = time(0);
    }
}


#endif
#endif





================================================
FILE: Client/src/lib/mqttsnClientAppFw4mbed.h
================================================
/*
 * mqttsnClientAppFw4mbed.h
 *
 *                      The BSD License
 *
 *           Copyright (c) 2014, tomoaki@tomy-tech.com
 *                    All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: 2014/06/01
 *    Modified: 2014/09/05
 *      Author: Tomoaki YAMAGUCHI
 *     Version: 1.0.0
 */

#ifdef MBED

#ifndef MQTTSNCLIENTAPPLICATION_H_
#define MQTTSNCLIENTAPPLICATION_H_

#include "mqttsnClient.h"


extern uint16_t getUint16(uint8_t* pos);
extern uint32_t getUint32(uint8_t* pos);
extern void setUint16(uint8_t* pos, uint16_t val);
extern void setUint32(uint8_t* pos, uint32_t val);

/*======================================
               Class WdTimer
========================================*/
class WdTimer:public XTimer {
public:
	WdTimer(void);
	int registerCallback(uint32_t sec, int (*proc)(void));
	void refleshRegisterTable();
	void start(void);
	void stop(void);
	bool wakeUp(void);

private:
	MQ_TimerTbl *_timerTbls;
	uint8_t _timerCnt;
	bool _initFlg;
};


/*======================================
       Class MqttsnClientApplication
========================================*/
class MqttsnClientApplication{
public:
	MqttsnClientApplication();
	~MqttsnClientApplication();
	void refleshWdtCallbackTable();
	void initialize(APP_CONFIG config);
	void setSubscribe();
	void setKeepAlive(uint16_t msec);
	void setWillTopic(MQString* willTopic);
	void setWillMessage(MQString* willMsg);
	void setRetain(bool retain);
	void setClean(bool clean);
	void setClientId(MQString* id);

	int publish(MQString* topic, const char* data, int dataLength, uint8_t qos = 1);
	int publish(uint16_t predefinedId, const char* data, int dataLength, uint8_t qos = 1);
	int publish(MQString* topic, Payload* payload, uint8_t qos);
	int subscribe(MQString* topic, TopicCallback callback, uint8_t qos = 1);
	int subscribe(uint16_t predefinedId, TopicCallback callback, uint8_t qos = 1);
	int unsubscribe(MQString* topic);
	int disconnect(uint16_t duration);

	void addTask();
	void startWdt();
	void stopWdt();
	int  run();


private:
	MqttsnClient _mqttsn;
	WdTimer _wdTimer;
	XTimer _keepAliveTimer;
	XTimer _advertiseTimer;

};

extern MqttsnClientApplication* theApplication;



#else

#endif /*LINUX*/




#endif /* MQTTSCLIENTAPPLICATION_H_ */




================================================
FILE: Client/src/lib/udpStack.cpp
================================================
/*
 * udpStack.cpp
 *
 *                      The BSD License
 *
 *           Copyright (c) 2014, tomoaki@tomy-tech.com
 *                    All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: 2014/06/01
 *    Modified: 2014/09/05
 *      Author: Tomoaki YAMAGUCHI
 *     Version: 1.0.0
 */

#ifndef ARDUINO
        #include "MQTTSN_Application.h"
		#include "Network.h"
#else
        #include <MQTTSN_Application.h>
		#include <Network.h>
#endif

#ifdef NETWORK_UDP

#ifdef ARDUINO
  #include <udpStack.h>
  #include <mqUtil.h>
  #include <SPI.h>
  #include <Ethernet.h>
  #include <EthernetUdp.h>

  #if defined( NW_DEBUG) || defined(MQTTSN_DEBUG)
        #include <SoftwareSerial.h>
        extern SoftwareSerial debug;
  #endif

#endif  /* ARDUINO */

#ifdef MBED
        #include "mbed.h"
        #include "udpStack.h"
		#include "mqUtil.h"
#endif /* MBED */

#ifdef LINUX
        #include "udpStack.h"
		#include "mqUtil.h"
        #include <stdio.h>
        #include <sys/time.h>
        #include <sys/types.h>
		#include <sys/socket.h>
        #include <sys/stat.h>
        #include <unistd.h>
        #include <stdlib.h>
        #include <string.h>
        #include <fcntl.h>
        #include <errno.h>
        #include <termios.h>

#endif /* LINUX */

using namespace std;
using namespace tomyClient;

extern uint16_t getUint16(uint8_t* pos);
extern void setUint16(uint8_t* pos, uint16_t val);
extern uint32_t getUint32(uint8_t* pos);
extern void setUint32(uint8_t* pos, uint32_t val);

/*=========================================
       Class Network
 =========================================*/
Network::Network(){
	_sleepflg = false;
	resetGwAddress();
}

Network::~Network(){

}

void Network::send(uint8_t* xmitData, uint8_t dataLen, SendReqType type){
	if(type == BcastReq){
		multicast(xmitData, (uint16_t)dataLen);
	}else if(type == UcastReq ){
		unicast(xmitData, (uint16_t)dataLen, _gwIpAddress, _gwPortNo);
	}
}

int  Network::readPacket(uint8_t type){
	_returnCode = 0;

	if(checkRecvBuf()){
		if(readApiFrame()){
			if(_nlResp.isAvailable()){
				if(_rxCallbackPtr){
					_rxCallbackPtr(&_nlResp, &_returnCode);
				}
			}
		}
	}
	return _returnCode;
}


int  Network::readApiFrame(){
	uint16_t portNo = 0;
	uint32_t ipAddress = 0;
	uint16_t len;

	if (_nlResp.isAvailable() || _nlResp.isError()){
	   _nlResp.setAvailable(false);
	   _nlResp.setErrorCode(NO_ERROR);
	}

	uint16_t recvLen = recv(_rxFrameDataBuf, MQTTSN_MAX_FRAME_SIZE, false, &ipAddress, &portNo);

	if( recvLen > 0){
		if(*_rxFrameDataBuf == 0x01){
			len = getUint16(_rxFrameDataBuf + 1);
		}else{
			len = *_rxFrameDataBuf;
		}

		if( len != recvLen){
			_nlResp.setErrorCode(PACKET_EXCEEDS_LENGTH);
						return false;
		}else if(_gwIpAddress && isUnicast() &&
		 		 (_nlResp.getAddress64().getLsb() != _gwIpAddress) &&
				 (_nlResp.getAddress16() != _gwPortNo)){
			D_NWSTACKW("  Sender is not Gateway!\r\n" );
			return false;
		}else{
			_nlResp.setLength(len);
			_nlResp.setAvailable(true);
			_nlResp.setFrame(_rxFrameDataBuf);
			_nlResp.setAddress16(portNo);
			_nlResp.setAddress64(0,ipAddress);
			return true;
		}
	}
	return false;
}

void Network::setGwAddress(){
	_gwPortNo = _nlResp.getAddress16();
	_gwIpAddress = _nlResp.getAddress64().getLsb();
}

void Network::resetGwAddress(void){
	_gwIpAddress = 0;
	_gwPortNo = 0;
}

void Network::setRxHandler(void (*callbackPtr)(NWResponse* data, int* returnCode)){
	_rxCallbackPtr = callbackPtr;
}

int Network::initialize(UdpConfig  config){
	return open(config);
}

void Network::setSleep(){
	_sleepflg = true;
}

/*=========================================
       Class udpStack
 =========================================*/
#ifdef ARDUINO
/**
 *  For Arduino
 */
UdpPort::UdpPort(){

}

UdpPort::~UdpPort(){
    close();
}

void UdpPort::close(){

}


bool UdpPort::open(UdpConfig config){
	_gIpAddr = IPAddress(config.ipAddress);
	_cIpAddr = IPAddress(config.ipLocal);
	_gPortNo = config.gPortNo;
	_uPortNo = config.uPortNo;

	memcpy(_macAddr, config.macAddr, 6);

	Ethernet.begin(_macAddr, _cIpAddr);

	if(_udpMulticast.beginMulti(_gIpAddr, _gPortNo) == 0){
		return false;
	}
	if(_udpUnicast.begin(_uPortNo) == 0){
		return false;
	}

	return true;
}

int UdpPort::unicast(const uint8_t* buf, uint32_t length, uint32_t ipAddress, uint16_t port  ){

	IPAddress ip = IPAddress(ipAddress);
	_udpUnicast.beginPacket(ip, port);
	_udpUnicast.write(buf, length);
	return _udpUnicast.endPacket();
}


int UdpPort::multicast( const uint8_t* buf, uint32_t length ){
	_udpMulticast.beginPacket(_gIpAddr, _gPortNo);
	_udpMulticast.write(buf, length);
	return _udpMulticast.endPacket();
}

bool UdpPort::checkRecvBuf(){
	int ps = _udpUnicast.parsePacket();
	if(ps > 0){
		_castStat = STAT_UNICAST;
		return true;
	}else if ( (ps = _udpMulticast.parsePacket()) > 0){
		_castStat = STAT_MULTICAST;
		return true;
	}
	_castStat = 0;
	return 0;
}

int UdpPort::recv(uint8_t* buf, uint16_t len, bool flg, uint32_t* ipAddressPtr, uint16_t* portPtr){
	return recvfrom ( buf, len, 0, ipAddressPtr, portPtr );
}

int UdpPort::recvfrom ( uint8_t* buf, uint16_t len, int flags, uint32_t* ipAddressPtr, uint16_t* portPtr ){
	IPAddress remoteIp;
	uint8_t packLen;
	if(_castStat == STAT_UNICAST){
		packLen = _udpUnicast.read(buf, len);
		*portPtr = _udpUnicast.remotePort();
		remoteIp = _udpUnicast.remoteIP();
	}else if(_castStat == STAT_MULTICAST){
		packLen = _udpMulticast.read(buf, len);
		*portPtr = _udpMulticast.remotePort();
		remoteIp = _udpMulticast.remoteIP();
	}else{
		return 0;
	}
	memcpy(ipAddressPtr,remoteIp.raw_address(), 4);
	return packLen;
}

bool UdpPort::isUnicast(){
	return ( _castStat == STAT_UNICAST);
}

#endif /* ARDUINO */


#ifdef MBED
/**
 *  For MBED
 */


#endif /* MBED */

#ifdef LINUX

UdpPort::UdpPort(){
    _disconReq = false;
    _sockfdUcast = -1;
    _sockfdMcast = -1;
    _castStat = 0;
}

UdpPort::~UdpPort(){
    close();
}


void UdpPort::close(){
	if(_sockfdMcast > 0){
		::close( _sockfdMcast);
		_sockfdMcast = -1;
	if(_sockfdUcast > 0){
			::close( _sockfdUcast);
			_sockfdUcast = -1;
		}
	}
}

bool UdpPort::open(UdpConfig config){
	const int reuse = 1;
	char loopch = 0;

	_gPortNo = htons(config.gPortNo);
	_gIpAddr = getUint32(config.ipAddress);
	_uPortNo = htons(config.uPortNo);

	if( _gPortNo == 0 || _gIpAddr == 0 || _uPortNo == 0){
		return false;
	}

	_sockfdUcast = socket(AF_INET, SOCK_DGRAM, 0);
	if (_sockfdUcast < 0){
		return false;
	}

	setsockopt(_sockfdUcast, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));

	struct sockaddr_in addr;
	addr.sin_family = AF_INET;
	addr.sin_port = _uPortNo;
	addr.sin_addr.s_addr = INADDR_ANY;

	if( ::bind ( _sockfdUcast, (struct sockaddr*)&addr,  sizeof(addr)) <0){
		return false;
	}

	_sockfdMcast = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
	if (_sockfdMcast < 0){
		return false;
	}

	struct sockaddr_in addrm;
	addrm.sin_family = AF_INET;
	addrm.sin_port = _gPortNo;
	addrm.sin_addr.s_addr = INADDR_ANY;

	setsockopt(_sockfdMcast, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));

	if( ::bind ( _sockfdMcast, (struct sockaddr*)&addrm,  sizeof(addrm)) <0){
		return false;
	}

	if(setsockopt(_sockfdUcast, IPPROTO_IP, IP_MULTICAST_LOOP,(char*)&loopch, sizeof(loopch)) <0 ){
		D_NWSTACKW("error IP_MULTICAST_LOOP in UdpPort::open\n");

		close();
		return false;
	}

	if(setsockopt(_sockfdMcast, IPPROTO_IP, IP_MULTICAST_LOOP,(char*)&loopch, sizeof(loopch)) <0 ){
		D_NWSTACKW("error IP_MULTICAST_LOOP in UdpPPort::open\n");
		close();
		return false;
	}

	ip_mreq mreq;
	mreq.imr_interface.s_addr = INADDR_ANY;
	mreq.imr_multiaddr.s_addr = getUint32(config.ipAddress);

	if( setsockopt(_sockfdMcast, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq) )< 0){
		D_NWSTACKF("error IP_ADD_MEMBERSHIP in UdpPort::open\n");
		close();
		return false;
	}
/*
	if( setsockopt(_sockfdUcast, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq) )< 0){
		D_NWSTACKF("error IP_ADD_MEMBERSHIP in UdpPort::open\n");
		close();
		return false;
	}
*/
	return true;
}

bool UdpPort::isUnicast(){
	return ( _castStat == STAT_UNICAST);
}


int UdpPort::unicast(const uint8_t* buf, uint32_t length, uint32_t ipAddress, uint16_t port  ){
	struct sockaddr_in dest;
	dest.sin_family = AF_INET;
	dest.sin_port = port;
	dest.sin_addr.s_addr = ipAddress;

	int status = ::sendto( _sockfdUcast, buf, length, 0, (const sockaddr*)&dest, sizeof(dest) );
	if( status < 0){
		D_NWSTACKF("errno == %d in UdpPort::unicast\n", errno);
	}else{
		D_NWSTACKF("sendto %s:%u  [",inet_ntoa(dest.sin_addr),htons(port));
		for(uint16_t i = 0; i < length ; i++){
			D_NWSTACKF(" %02x", *(buf + i));
		}
		D_NWSTACKF(" ]\n");
	}
	return status;
}


int UdpPort::multicast( const uint8_t* buf, uint32_t length ){
	struct sockaddr_in dest;
	dest.sin_family = AF_INET;
	dest.sin_port = _gPortNo;
	dest.sin_addr.s_addr = _gIpAddr;

	int status = ::sendto( _sockfdMcast, buf, length, 0, (const sockaddr*)&dest, sizeof(dest) );
	if( status < 0){
		D_NWSTACKF("errno == %d in UdpPort::multicast\n", errno);
	}else{
		D_NWSTACKF("sendto %s:%u  [",inet_ntoa(dest.sin_addr),htons(_gPortNo));
		for(uint16_t i = 0; i < length ; i++){
			D_NWSTACKF(" %02x", *(buf + i));
		}
		D_NWSTACKF(" ]\n");
	}
	return errno;
}

bool UdpPort::checkRecvBuf(){
	struct timeval timeout;
	timeout.tv_sec = 0;
	timeout.tv_usec = 500000;    // 500 msec

	uint8_t buf[2];
	fd_set recvfds;
	int maxSock = 0;

	FD_ZERO(&recvfds);
	FD_SET(_sockfdUcast, &recvfds);
	FD_SET(_sockfdMcast, &recvfds);

	if(_sockfdMcast > _sockfdUcast){
		maxSock = _sockfdMcast;
	}else{
		maxSock = _sockfdUcast;
	}

	select(maxSock + 1, &recvfds, 0, 0, &timeout);

	if(FD_ISSET(_sockfdUcast, &recvfds)){
		if( ::recv(_sockfdUcast, buf, 1,  MSG_DONTWAIT | MSG_PEEK) > 0){
			_castStat = STAT_UNICAST;
			return true;
		}
	}else if(FD_ISSET(_sockfdMcast, &recvfds)){
		if( ::recv(_sockfdMcast, buf, 1,  MSG_DONTWAIT | MSG_PEEK) > 0){
			_castStat = STAT_MULTICAST;
			return true;
		}
	}
	_castStat = 0;
	return false;
}

int UdpPort::recv(uint8_t* buf, uint16_t len, bool flg, uint32_t* ipAddressPtr, uint16_t* portPtr){
	int flags = flg ? MSG_DONTWAIT : 0;
	return recvfrom (buf, len, flags, ipAddressPtr, portPtr );
}

int UdpPort::recvfrom ( uint8_t* buf, uint16_t len, int flags, uint32_t* ipAddressPtr, uint16_t* portPtr ){
	struct sockaddr_in sender;
	int status;
	socklen_t addrlen = sizeof(sender);
	memset(&sender, 0, addrlen);

	if(_castStat == STAT_UNICAST){
		status = ::recvfrom( _sockfdUcast, buf, len, flags, (struct sockaddr*)&sender, &addrlen );
	}else if(_castStat == STAT_MULTICAST){
		status = ::recvfrom( _sockfdMcast, buf, len, flags, (struct sockaddr*)&sender, &addrlen );
	}else{
		return 0;
	}

	if (status < 0 && errno != EAGAIN)	{
		D_NWSTACKF("errno == %d in UdpPort::recvfrom \n", errno);
	}else if(status > 0){
		*ipAddressPtr = sender.sin_addr.s_addr;
		*portPtr = sender.sin_port;
		D_NWSTACKF("recved from %s:%u [",inet_ntoa(sender.sin_addr), htons(*portPtr));
		for(uint16_t i = 0; i < status ; i++){
			D_NWSTACKF(" %02x", *(buf + i));
		}
		D_NWSTACKF(" ]\n");
	}else{
		return 0;
	}
	return status;
}


#endif

/*=========================================
             Class NLLongAddress
 =========================================*/
NWAddress64::NWAddress64(){
    _msb = _lsb = 0;
}

NWAddress64::NWAddress64(uint32_t msb, uint32_t lsb){
    _msb = msb;
    _lsb = lsb;
}

uint32_t NWAddress64::getMsb(){
    return _msb;
}

uint32_t NWAddress64::getLsb(){
    return _lsb;
}

void NWAddress64::setMsb(uint32_t msb){
    _msb = msb;
}

void NWAddress64::setLsb(uint32_t lsb){
    _lsb = lsb;
}

/*=========================================
             Class ZBResponse
 =========================================*/
NWResponse::NWResponse(){
    _addr16 = 0;
}

uint8_t  NWResponse::getFrameLength(){
	return _len;
}

void NWResponse::setLength(uint16_t len){
	_len = len;
}

void NWResponse::setFrame(uint8_t* framePtr){
	_frameDataPtr = framePtr;
}

NWAddress64&  NWResponse::getAddress64(){
    return _addr64;
}

uint16_t NWResponse::getAddress16(){
  return _addr16;
}

void  NWResponse::setAddress64(uint32_t msb, uint32_t lsb){
    _addr64.setMsb(msb);
    _addr64.setLsb(lsb);
}

void  NWResponse::setAddress16(uint16_t addr16){
	_addr16 = addr16;
}

void NWResponse::setErrorCode(uint8_t errCode){
	_errorCode = errCode;
}

void NWResponse::setAvailable(bool complete){
	_complete = complete;
}

uint8_t NWResponse::getType(){
	if(_len > 255){
		return _frameDataPtr[3];
	}else{
		return _frameDataPtr[1];
	}
}

uint8_t* NWResponse::getBody(){
	if(_len > 255){
		return _frameDataPtr + 4;
	}else{
		return _frameDataPtr + 2;
	}
}

uint16_t NWResponse::getBodyLength(){
	if(_len > 255){
		return getPayloadLength() - 4;
	}else{
		return getPayloadLength() - 2;
	}
}

uint8_t NWResponse::getPayload(uint8_t index){
		return _frameDataPtr[index];
}

uint8_t* NWResponse::getPayload(){
		return _frameDataPtr;
}

uint8_t NWRespon
Download .txt
gitextract_nt1a49t7/

├── Client/
│   ├── Makefile
│   ├── README.md
│   └── src/
│       ├── LinuxClientSample.cpp
│       ├── SoilMoistureClientSample.ino
│       ├── lib/
│       │   ├── MQTTSN_Application.h
│       │   ├── Network.h
│       │   ├── mqUtil.cpp
│       │   ├── mqUtil.h
│       │   ├── mqttsn.cpp
│       │   ├── mqttsn.h
│       │   ├── mqttsnClient.cpp
│       │   ├── mqttsnClient.h
│       │   ├── mqttsnClientAppFw4Arduino.cpp
│       │   ├── mqttsnClientAppFw4Arduino.h
│       │   ├── mqttsnClientAppFw4Linux.cpp
│       │   ├── mqttsnClientAppFw4Linux.h
│       │   ├── mqttsnClientAppFw4mbed.cpp
│       │   ├── mqttsnClientAppFw4mbed.h
│       │   ├── udpStack.cpp
│       │   ├── udpStack.h
│       │   ├── zbeeStack.cpp
│       │   └── zbeeStack.h
│       └── mbedClientSample.cpp
├── Gateway/
│   ├── Makefile
│   ├── README.md
│   └── src/
│       ├── BrokerRecvTask.cpp
│       ├── BrokerRecvTask.h
│       ├── BrokerSendTask.cpp
│       ├── BrokerSendTask.h
│       ├── ClientRecvTask.cpp
│       ├── ClientRecvTask.h
│       ├── ClientSendTask.cpp
│       ├── ClientSendTask.h
│       ├── ErrorMessage.h
│       ├── GatewayControlTask.cpp
│       ├── GatewayControlTask.h
│       ├── GatewayDefines.h
│       ├── GatewayResourcesProvider.cpp
│       ├── GatewayResourcesProvider.h
│       ├── TomyGateway.cpp
│       └── lib/
│           ├── Defines.h
│           ├── Messages.cpp
│           ├── Messages.h
│           ├── ProcessFramework.cpp
│           ├── ProcessFramework.h
│           ├── TCPStack.cpp
│           ├── TCPStack.h
│           ├── TLSStack.cpp
│           ├── TLSStack.h
│           ├── Topics.cpp
│           ├── Topics.h
│           ├── UDPStack.cpp
│           ├── UDPStack.h
│           ├── XXXXXStack.cpp
│           ├── XXXXXStack.h
│           ├── ZBStack.cpp
│           └── ZBStack.h
├── LogMonitor/
│   ├── Makefile
│   └── src/
│       ├── LogMonitor.cpp
│       ├── LogMonitor.h
│       ├── LogMonitorApp.cpp
│       └── lib/
│           ├── Defines.h
│           ├── ProcessFramework.cpp
│           └── ProcessFramework.h
└── README.md
Download .txt
SYMBOL INDEX (238 symbols across 49 files)

FILE: Client/src/LinuxClientSample.cpp
  function f_publish_all (line 72) | int f_publish_all(){
  function f_onRequest_01 (line 112) | int f_onRequest_01(MqttsnPublish* msg){
  function f_onRequest_02 (line 131) | int f_onRequest_02(MqttsnPublish* msg){
  function setup (line 149) | void setup(){

FILE: Client/src/lib/MQTTSN_Application.h
  type MqttsnConfig (line 106) | typedef struct {
  type XBeeConfig (line 115) | typedef struct {
  type XBeeAppConfig (line 121) | typedef struct {
  type UdpConfig (line 126) | typedef struct {
  type UdpAppConfig (line 134) | typedef struct {

FILE: Client/src/lib/Network.h
  type SendReqType (line 48) | enum SendReqType{
  function namespace (line 74) | namespace tomyClient {

FILE: Client/src/lib/mqUtil.cpp
  function getUint16 (line 64) | uint16_t getUint16(uint8_t* pos){
  function setUint16 (line 69) | void setUint16(uint8_t* pos, uint16_t val){
  function getUint32 (line 74) | uint32_t getUint32(uint8_t* pos){
  function setUint32 (line 81) | void setUint32(uint8_t* pos, uint32_t val){
  function getFloat32 (line 88) | float getFloat32(uint8_t* pos){
  function setFloat32 (line 100) | void setFloat32(uint8_t* pos, float flt){
  function getUint16 (line 117) | uint16_t getUint16(uint8_t* pos){
  function setUint16 (line 122) | void setUint16(uint8_t* pos, uint16_t val){
  function getUint32 (line 127) | uint32_t getUint32(uint8_t* pos){
  function setUint32 (line 134) | void setUint32(uint8_t* pos, uint32_t val){
  function getFloat32 (line 141) | float getFloat32(uint8_t* pos){
  function setFloat32 (line 154) | void setFloat32(uint8_t* pos, float flt){
  type timeval (line 324) | struct timeval

FILE: Client/src/lib/mqUtil.h
  function namespace (line 67) | namespace tomyClient {

FILE: Client/src/lib/mqttsn.cpp
  function MQString (line 184) | MQString* MQString::create(){
  function MQString (line 662) | MQString* MqttsnWillTopic::getWillTopic(){
  function MQString (line 770) | MQString* MqttsnRegister::getTopicName(){
  function MQString (line 829) | MQString* MqttsnPublish::getTopic(MQString* topic){
  function MQString (line 1032) | MQString*  MqttsnSubscribe::getTopicName(){
  function MQString (line 1194) | MQString* Topic::getTopicName(){
  function TopicCallback (line 1202) | TopicCallback Topic::getCallback(){
  function Topic (line 1299) | Topic* Topics::getTopic(MQString* topic) {
  function Topic (line 1308) | Topic* Topics::getTopic(uint16_t id) {
  function Topic (line 1386) | Topic* Topics::match(MQString* topic){

FILE: Client/src/lib/mqttsn.h
  function class (line 195) | class MQString{
  function class (line 227) | class MqttsnMessage {
  function class (line 348) | class MqttsnWillTopic : public MqttsnMessage  {
  function class (line 389) | class MqttsnRegister : public MqttsnMessage  {
  function class (line 432) | class MqttsnPublish : public MqttsnMessage  {
  function class (line 515) | class MqttsnSubscribe : public MqttsnMessage  {
  function class (line 617) | class Topic {
  function class (line 645) | class Topics {
  function class (line 674) | class Payload{
  function class (line 711) | class PublishHandller {

FILE: Client/src/lib/mqttsnClient.cpp
  function ResponseHandler (line 94) | void ResponseHandler(NWResponse* resp, int* returnCode){
  function Topics (line 151) | Topics* MqttsnClient::getTopics(){
  function MQString (line 203) | MQString* MqttsnClient::getClientId(){
  function ClientStatus (line 207) | ClientStatus* MqttsnClient::getClientStatus(){
  function MqttsnMessage (line 1219) | MqttsnMessage* SendQue::getMessage(uint8_t index){

FILE: Client/src/lib/mqttsnClient.h
  type TaskList (line 84) | typedef struct {
  type OnPublishList (line 89) | typedef struct {
  type MQ_TimerTbl (line 95) | typedef struct {
  function class (line 104) | class ClientStatus{
  function class (line 151) | class SendQue {
  function class (line 174) | class MqttsnClient {

FILE: Client/src/lib/mqttsnClientAppFw4Arduino.cpp
  type MQ_INT_STATUS (line 53) | enum MQ_INT_STATUS
  type MQ_INT_STATUS (line 54) | enum MQ_INT_STATUS
  function loop (line 64) | void loop(){
  function ISR (line 98) | ISR (WDT_vect) {
  function MQInt0 (line 111) | void MQInt0(){
  function MQwatchdogEnable (line 119) | void MQwatchdogEnable(){  // Turn on WDT
  function IntHandleDummy (line 132) | void IntHandleDummy(){
  function resetArduino (line 139) | void resetArduino(){
  function setUTC (line 149) | int setUTC(MqttsnPublish* msg){

FILE: Client/src/lib/mqttsnClientAppFw4Arduino.h
  type MQ_INT_STATUS (line 78) | enum MQ_INT_STATUS{ WAIT, INT0_LL, INT0_WAIT_HL, INT_WDT}
  function class (line 97) | class WdTimer:public XTimer {
  function class (line 116) | class MqttsnClientApplication{

FILE: Client/src/lib/mqttsnClientAppFw4Linux.cpp
  function main (line 70) | int main(int argc, char **argv){

FILE: Client/src/lib/mqttsnClientAppFw4Linux.h
  function class (line 49) | class WdTimer:public XTimer {
  function class (line 68) | class MqttsnClientApplication{

FILE: Client/src/lib/mqttsnClientAppFw4mbed.cpp
  function main (line 59) | int main(){
  function setUTC (line 84) | int setUTC(MqttsnPublish* msg){

FILE: Client/src/lib/mqttsnClientAppFw4mbed.h
  function class (line 52) | class WdTimer:public XTimer {
  function class (line 71) | class MqttsnClientApplication{

FILE: Client/src/lib/udpStack.cpp
  type sockaddr_in (line 337) | struct sockaddr_in
  type sockaddr (line 342) | struct sockaddr
  type sockaddr_in (line 351) | struct sockaddr_in
  type sockaddr (line 358) | struct sockaddr
  type sockaddr_in (line 400) | struct sockaddr_in
  type sockaddr_in (line 420) | struct sockaddr_in
  type timeval (line 439) | struct timeval
  type sockaddr_in (line 480) | struct sockaddr_in
  type sockaddr (line 486) | struct sockaddr
  type sockaddr (line 488) | struct sockaddr
  function NWAddress64 (line 559) | NWAddress64&  NWResponse::getAddress64(){

FILE: Client/src/lib/udpStack.h
  function namespace (line 95) | namespace tomyClient {

FILE: Client/src/lib/zbeeStack.cpp
  function NWAddress64 (line 436) | NWAddress64&  NWResponse::getRemoteAddress64(){
  function NWAddress64 (line 589) | NWAddress64& Network::getRxRemoteAddress64(){

FILE: Client/src/lib/zbeeStack.h
  function namespace (line 81) | namespace tomyClient {

FILE: Client/src/mbedClientSample.cpp
  function task1 (line 75) | int task1(){
  function blinkIndicator (line 96) | int  blinkIndicator(MqttsnPublish* msg){
  function setup (line 113) | void setup(){

FILE: Gateway/src/BrokerRecvTask.cpp
  type timeval (line 68) | struct timeval

FILE: Gateway/src/BrokerRecvTask.h
  function class (line 44) | class BrokerRecvTask : public Thread{

FILE: Gateway/src/BrokerSendTask.h
  function class (line 43) | class BrokerSendTask : public Thread{

FILE: Gateway/src/ClientRecvTask.h
  function class (line 44) | class ClientRecvTask:public Thread{

FILE: Gateway/src/ClientSendTask.h
  function class (line 41) | class ClientSendTask:public Thread{

FILE: Gateway/src/GatewayControlTask.h
  function class (line 46) | class GatewayControlTask : public Thread{

FILE: Gateway/src/GatewayResourcesProvider.cpp
  function ClientList (line 90) | ClientList* GatewayResourcesProvider::getClientList(){
  function Network (line 94) | Network* GatewayResourcesProvider::getNetwork(){
  function LightIndicator (line 98) | LightIndicator* GatewayResourcesProvider::getLightIndicator(){
  function MQTTSnPubAck (line 158) | MQTTSnPubAck* ClientNode::getWaitedPubAck(){
  function MQTTSnSubAck (line 162) | MQTTSnSubAck* ClientNode::getWaitedSubAck(){
  function MQTTMessage (line 183) | MQTTMessage* ClientNode::getBrokerSendMessage(){
  function MQTTMessage (line 187) | MQTTMessage* ClientNode::getBrokerRecvMessage(){
  function MQTTSnMessage (line 191) | MQTTSnMessage* ClientNode::getClientSendMessage(){
  function MQTTSnMessage (line 195) | MQTTSnMessage* ClientNode::getClientSleepMessage(){
  function MQTTSnMessage (line 199) | MQTTSnMessage* ClientNode::getClientRecvMessage(){
  function MQTTConnect (line 203) | MQTTConnect*   ClientNode::getConnectMessage(){
  function TLSStack (line 207) | TLSStack* ClientNode::getStack(){
  function MQTTSnConnack (line 377) | MQTTSnConnack*  ClientNode::checkGetConnAck(){
  function Topics (line 417) | Topics* ClientNode::getTopics(){
  function NWAddress64 (line 422) | NWAddress64* ClientNode::getAddress64Ptr(){
  function string (line 430) | string* ClientNode::getNodeId(){
  function ClientNode (line 516) | ClientNode* ClientList::createNode(bool secure, NWAddress64* addr64, uin...
  function ClientNode (line 565) | ClientNode* ClientList::getClient(NWAddress64* addr64, uint16_t addr16){
  function ClientNode (line 585) | ClientNode* ClientList::operator[](int pos){
  function EventType (line 643) | EventType Event::getEventType(){
  function ClientNode (line 677) | ClientNode* Event::getClientNode(){
  function MQTTSnMessage (line 682) | MQTTSnMessage* Event::getMqttSnMessage(){

FILE: Gateway/src/GatewayResourcesProvider.h
  type ClientStatus (line 63) | enum ClientStatus {
  function class (line 76) | class ClientNode{
  function class (line 170) | class ClientList{
  type EventType (line 191) | enum EventType{
  function class (line 203) | class Event{
  function class (line 226) | class LightIndicator{
  function class (line 244) | class GatewayResourcesProvider: public MultiTaskProcess{

FILE: Gateway/src/lib/Defines.h
  type XBeeConfig (line 81) | typedef struct {
  type UdpConfig (line 87) | typedef struct {
  type XXXXXConfig (line 93) | typedef struct {

FILE: Gateway/src/lib/Messages.cpp
  function isUtf8Valid (line 55) | bool isUtf8Valid(string& string)
  function string (line 312) | string* MQTTSnConnect::getClientId(){
  function string (line 393) | string* MQTTSnWillTopic::getWillTopic(){
  function string (line 453) | string* MQTTSnWillMsg::getWillMsg(){
  function string (line 520) | string* MQTTSnRegister::getTopicName(){
  function string (line 647) | string* MQTTSnPublish::getTopic(string* str){
  function string (line 885) | string*  MQTTSnSubscribe::getTopicName(){
  function string (line 1003) | string* MQTTSnUnsubscribe::getTopicName(){
  function string (line 1699) | string* MQTTPublish::getTopic(){

FILE: Gateway/src/lib/Messages.h
  function class (line 173) | class MQTTSnMessage{
  function class (line 242) | class MQTTSnConnect : public MQTTSnMessage {
  function class (line 294) | class MQTTSnWillTopic : public MQTTSnMessage  {
  function class (line 327) | class MQTTSnWillMsg : public MQTTSnMessage  {
  function class (line 344) | class MQTTSnRegister : public MQTTSnMessage  {
  function class (line 388) | class MQTTSnPublish : public MQTTSnMessage  {
  function class (line 426) | class MQTTSnPubAck : public MQTTSnMessage  {
  function class (line 450) | class MQTTSnPubRec : public MQTTSnMessage  {
  function class (line 466) | class MQTTSnPubRel : public MQTTSnPubRec  {
  function class (line 486) | class MQTTSnSubscribe : public MQTTSnMessage  {
  function class (line 516) | class MQTTSnSubAck : public MQTTSnMessage  {
  function class (line 561) | class MQTTSnUnsubAck : public MQTTSnMessage  {
  function class (line 622) | class RemainingLength{
  function class (line 640) | class MQTTMessage{
  function class (line 682) | class MQTTPingReq : public MQTTMessage{
  function class (line 691) | class MQTTPingResp : public MQTTMessage{
  function class (line 700) | class MQTTDisconnect : public MQTTMessage{
  function class (line 709) | class MQTTPubAck : public MQTTMessage{
  function class (line 721) | class MQTTPubRec : public MQTTMessage{
  function class (line 733) | class MQTTPubRel : public MQTTPubRec{
  function class (line 744) | class MQTTPubComp : public MQTTPubRec{
  function class (line 754) | class MQTTConnAck : public MQTTMessage{
  function class (line 767) | class MQTTUnsubAck : public MQTTMessage{
  function class (line 778) | class MQTTSubAck : public MQTTMessage{
  function class (line 810) | class MQTTSubscribe : public MQTTMessage{
  function class (line 826) | class MQTTConnect : public MQTTMessage{
  function class (line 854) | class MQTTPublish : public MQTTMessage{

FILE: Gateway/src/lib/ProcessFramework.cpp
  function signalHandler (line 67) | static void signalHandler(int sig){
  function main (line 71) | int main(int argc, char** argv){
  function getUint16 (line 95) | uint16_t getUint16(uint8_t* pos){
  function setUint16 (line 100) | void setUint16(uint8_t* pos, uint16_t val){
  function getUint32 (line 105) | uint32_t getUint32(uint8_t* pos){
  function setUint32 (line 112) | void setUint32(uint8_t* pos, uint32_t val){
  function getUint16 (line 123) | uint16_t getUint16(uint8_t* pos){
  function setUint16 (line 128) | void setUint16(uint8_t* pos, uint16_t val){
  function getUint32 (line 133) | long getUint32(uint8_t* pos){
  function setUint32 (line 140) | void setUint32(uint8_t* pos, uint32_t val){
  function utfSerialize (line 151) | void utfSerialize(uint8_t* pos, string str){
  type tm (line 160) | struct tm
  function Semaphore (line 319) | Semaphore* MultiTaskProcess::getStopProcessEvent(){
  function pthread_t (line 364) | pthread_t Thread::getID(){
  type timespec (line 534) | struct timespec
  type timeval (line 799) | struct timeval

FILE: Gateway/src/lib/ProcessFramework.h
  function class (line 87) | class Mutex{
  function class (line 104) | class Semaphore{
  function class (line 142) | class RingBuffer{
  function class (line 167) | class Runnable{
  function class (line 175) | class Thread : virtual public Runnable{
  function class (line 198) | class Process{
  function class (line 227) | class MultiTaskProcess: public Process{
  type ExceptionType (line 255) | enum ExceptionType {
  function class (line 267) | class Exception : public exception {
  function class (line 295) | class Timer {

FILE: Gateway/src/lib/TCPStack.cpp
  type sockaddr (line 144) | struct sockaddr

FILE: Gateway/src/lib/TCPStack.h
  function class (line 58) | class TCPStack{

FILE: Gateway/src/lib/TLSStack.cpp
  function SSL (line 330) | SSL* TLSStack::getSSL(){

FILE: Gateway/src/lib/TLSStack.h
  function class (line 54) | class TLSStack:public TCPStack{

FILE: Gateway/src/lib/Topics.cpp
  function string (line 64) | string* Topic::getTopicName(){
  function Topic (line 153) | Topic* Topics::getTopic(string* topic) {
  function Topic (line 164) | Topic* Topics::getTopic(uint16_t id) {
  function Topic (line 205) | Topic* Topics::match(string* topic){

FILE: Gateway/src/lib/Topics.h
  function class (line 56) | class Topic {
  function class (line 77) | class Topics {

FILE: Gateway/src/lib/UDPStack.cpp
  function NWAddress64 (line 330) | NWAddress64*  NWResponse::getClientAddress64(){

FILE: Gateway/src/lib/UDPStack.h
  function namespace (line 66) | namespace tomyGateway{

FILE: Gateway/src/lib/XXXXXStack.cpp
  function NWAddress64 (line 186) | NWAddress64*  NWResponse::getClientAddress64(){

FILE: Gateway/src/lib/XXXXXStack.h
  function namespace (line 66) | namespace tomyGateway{

FILE: Gateway/src/lib/ZBStack.cpp
  function NWAddress64 (line 218) | NWAddress64* NWResponse::getClientAddress64(){
  function NWAddress64 (line 280) | NWAddress64& NWRequest::getAddress64(){

FILE: Gateway/src/lib/ZBStack.h
  function namespace (line 99) | namespace tomyGateway{

FILE: LogMonitor/src/LogMonitor.cpp
  class LogMonitor (line 42) | class LogMonitor:public Process{

FILE: LogMonitor/src/LogMonitor.h
  function class (line 42) | class LogMonitor:public Process{

FILE: LogMonitor/src/lib/ProcessFramework.cpp
  function signalHandler (line 63) | static void signalHandler(int sig){
  function main (line 70) | int main(int argc, char** argv){
  function getUint16 (line 96) | uint16_t getUint16(uint8_t* pos){
  function setUint16 (line 101) | void setUint16(uint8_t* pos, uint16_t val){
  function getUint32 (line 106) | uint32_t getUint32(uint8_t* pos){
  function setUint32 (line 113) | void setUint32(uint8_t* pos, uint32_t val){
  function getUint16 (line 124) | uint16_t getUint16(uint8_t* pos){
  function setUint16 (line 129) | void setUint16(uint8_t* pos, uint16_t val){
  function getUint32 (line 134) | long getUint32(uint8_t* pos){
  function setUint32 (line 141) | void setUint32(uint8_t* pos, uint32_t val){
  function utfSerialize (line 152) | void utfSerialize(uint8_t* pos, string str){
  type tm (line 161) | struct tm
  function Semaphore (line 325) | Semaphore* MultiTaskProcess::getStopProcessEvent(){
  function pthread_t (line 368) | pthread_t Thread::getID(){
  type timespec (line 538) | struct timespec
  type timeval (line 804) | struct timeval

FILE: LogMonitor/src/lib/ProcessFramework.h
  function class (line 83) | class Mutex{
  function class (line 100) | class Semaphore{
  function class (line 138) | class RingBuffer{
  function class (line 163) | class Runnable{
  function class (line 171) | class Thread : virtual public Runnable{
  function class (line 194) | class Process{
  function class (line 224) | class MultiTaskProcess: public Process{
  type ExceptionType (line 252) | enum ExceptionType {
  function class (line 264) | class Exception : public exception {
  function class (line 292) | class Timer {
Condensed preview — 65 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (589K chars).
[
  {
    "path": "Client/Makefile",
    "chars": 823,
    "preview": "PROGNAME := TomyClient\nSRCDIR := src\nSUBDIR := src/lib\n\nSRCS := $(SRCDIR)/LinuxClientSample.cpp \\\n$(SUBDIR)/mqttsnClient"
  },
  {
    "path": "Client/README.md",
    "chars": 6685,
    "preview": "MQTT-SN Client\n======\n  MQTT-SN Client over XBee (running on linux, Arduino and mbed)    \n  MQTT-SN Client over UDP  (ru"
  },
  {
    "path": "Client/src/LinuxClientSample.cpp",
    "chars": 4756,
    "preview": "/*\n * LinuxClientSample.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-"
  },
  {
    "path": "Client/src/SoilMoistureClientSample.ino",
    "chars": 5510,
    "preview": "/*\n * SoilMoisture.ino\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech."
  },
  {
    "path": "Client/src/lib/MQTTSN_Application.h",
    "chars": 5995,
    "preview": "/*\n * MQTTSN_Application.h\n *\n *                    The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tec"
  },
  {
    "path": "Client/src/lib/Network.h",
    "chars": 2647,
    "preview": "/*\n * Network.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com\n "
  },
  {
    "path": "Client/src/lib/mqUtil.cpp",
    "chars": 7092,
    "preview": "/*\n * mqUtil.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com\n *"
  },
  {
    "path": "Client/src/lib/mqUtil.h",
    "chars": 3909,
    "preview": "/*\r\n * mqUtil.h\r\n *\r\n *                      The BSD License\r\n *\r\n *           Copyright (c) 2014, tomoaki@tomy-tech.com"
  },
  {
    "path": "Client/src/lib/mqttsn.cpp",
    "chars": 37225,
    "preview": "/*\n * mqttsn.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com\n *"
  },
  {
    "path": "Client/src/lib/mqttsn.h",
    "chars": 18803,
    "preview": "/*\n * mqttsn.h\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com\n *  "
  },
  {
    "path": "Client/src/lib/mqttsnClient.cpp",
    "chars": 40248,
    "preview": "/*\n * mqttsnClient.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech."
  },
  {
    "path": "Client/src/lib/mqttsnClient.h",
    "chars": 7579,
    "preview": "/*\n * mqttsnClient.h\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.co"
  },
  {
    "path": "Client/src/lib/mqttsnClientAppFw4Arduino.cpp",
    "chars": 12999,
    "preview": "/*\n * mqttsnClientAppAppFw4Arduino.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, to"
  },
  {
    "path": "Client/src/lib/mqttsnClientAppFw4Arduino.h",
    "chars": 5555,
    "preview": "/*\r\n * mqttsnClientAppFw4Arduino.h\r\n *\r\n *                      The BSD License\r\n *\r\n *           Copyright (c) 2014, to"
  },
  {
    "path": "Client/src/lib/mqttsnClientAppFw4Linux.cpp",
    "chars": 9114,
    "preview": "/*\n * mqttsnClientAppAppFwLinux.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoa"
  },
  {
    "path": "Client/src/lib/mqttsnClientAppFw4Linux.h",
    "chars": 3353,
    "preview": "/*\r\n * mqttsnClientAppFw4Linux.h\r\n *\r\n *                      The BSD License\r\n *\r\n *           Copyright (c) 2014, tomo"
  },
  {
    "path": "Client/src/lib/mqttsnClientAppFw4mbed.cpp",
    "chars": 7205,
    "preview": "/*\n * mqttsnClientAppAppFwmbed.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoak"
  },
  {
    "path": "Client/src/lib/mqttsnClientAppFw4mbed.h",
    "chars": 3670,
    "preview": "/*\r\n * mqttsnClientAppFw4mbed.h\r\n *\r\n *                      The BSD License\r\n *\r\n *           Copyright (c) 2014, tomoa"
  },
  {
    "path": "Client/src/lib/udpStack.cpp",
    "chars": 14524,
    "preview": "/*\n * udpStack.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com\n"
  },
  {
    "path": "Client/src/lib/udpStack.h",
    "chars": 5840,
    "preview": "/*\n * udpStack.h\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com\n *"
  },
  {
    "path": "Client/src/lib/zbeeStack.cpp",
    "chars": 20243,
    "preview": "/*\n * zbeeStack.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com"
  },
  {
    "path": "Client/src/lib/zbeeStack.h",
    "chars": 9737,
    "preview": "/*\r\n * zbeeStack.h\r\n *\r\n *                      The BSD License\r\n *\r\n *           Copyright (c) 2014, tomoaki@tomy-tech."
  },
  {
    "path": "Client/src/mbedClientSample.cpp",
    "chars": 3610,
    "preview": "/*\n * mbedClientSample.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-t"
  },
  {
    "path": "Gateway/Makefile",
    "chars": 1129,
    "preview": "PROGNAME := TomyGateway\nSRCDIR := src\nSUBDIR := src/lib\n\nSRCS := $(SRCDIR)/TomyGateway.cpp \\\n$(SRCDIR)/BrokerRecvTask.cp"
  },
  {
    "path": "Gateway/README.md",
    "chars": 4840,
    "preview": "MQTT-SN Gateway\n======\nThis program is a Gateway over XBee and UDP.\nSelect from the network layer in lib/Defines.h, show"
  },
  {
    "path": "Gateway/src/BrokerRecvTask.cpp",
    "chars": 7709,
    "preview": "/*\n * BrokerRecvTask.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tec"
  },
  {
    "path": "Gateway/src/BrokerRecvTask.h",
    "chars": 2133,
    "preview": "/*\n * BrokerRecvTask.h\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech."
  },
  {
    "path": "Gateway/src/BrokerSendTask.cpp",
    "chars": 6275,
    "preview": "/*\n * BrokerSendTask.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tec"
  },
  {
    "path": "Gateway/src/BrokerSendTask.h",
    "chars": 2196,
    "preview": "/*\n * BrokerSendTask.h\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech."
  },
  {
    "path": "Gateway/src/ClientRecvTask.cpp",
    "chars": 8121,
    "preview": "/*\n * ClientRecvTask.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tec"
  },
  {
    "path": "Gateway/src/ClientRecvTask.h",
    "chars": 2054,
    "preview": "/*\n * ClientRecvTask.h\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech."
  },
  {
    "path": "Gateway/src/ClientSendTask.cpp",
    "chars": 3757,
    "preview": "/*\n * ClientSendTask.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tec"
  },
  {
    "path": "Gateway/src/ClientSendTask.h",
    "chars": 1952,
    "preview": "/*\n * ClientSendTask.h\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech."
  },
  {
    "path": "Gateway/src/ErrorMessage.h",
    "chars": 1933,
    "preview": "/*\n * ErrorMessage.h\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.co"
  },
  {
    "path": "Gateway/src/GatewayControlTask.cpp",
    "chars": 36017,
    "preview": "/*\n * GatewayControlTask.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy"
  },
  {
    "path": "Gateway/src/GatewayControlTask.h",
    "chars": 4081,
    "preview": "/*\n * GatewayControlTask.h\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-t"
  },
  {
    "path": "Gateway/src/GatewayDefines.h",
    "chars": 3753,
    "preview": "/*\n * GatewayDefines.h\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech."
  },
  {
    "path": "Gateway/src/GatewayResourcesProvider.cpp",
    "chars": 16179,
    "preview": "/*\n * GatewayResourcesProvider.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoak"
  },
  {
    "path": "Gateway/src/GatewayResourcesProvider.h",
    "chars": 7888,
    "preview": "/*\n * GatewayResourcesProvider.h\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@"
  },
  {
    "path": "Gateway/src/TomyGateway.cpp",
    "chars": 2254,
    "preview": "/*\n * A_ProgramStructure.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy"
  },
  {
    "path": "Gateway/src/lib/Defines.h",
    "chars": 3104,
    "preview": "/*\n *   Defines.h\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com\n *  "
  },
  {
    "path": "Gateway/src/lib/Messages.cpp",
    "chars": 37824,
    "preview": "/*\n * Messages.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com\n"
  },
  {
    "path": "Gateway/src/lib/Messages.h",
    "chars": 21700,
    "preview": "/*\n * Messages.h\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com\n *"
  },
  {
    "path": "Gateway/src/lib/ProcessFramework.cpp",
    "chars": 17816,
    "preview": "/*\n * ProcessFramework.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-t"
  },
  {
    "path": "Gateway/src/lib/ProcessFramework.h",
    "chars": 8557,
    "preview": "/*\n * ProcessFramework.h\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tec"
  },
  {
    "path": "Gateway/src/lib/TCPStack.cpp",
    "chars": 5243,
    "preview": "/*\n * Socket.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com\n *"
  },
  {
    "path": "Gateway/src/lib/TCPStack.h",
    "chars": 2599,
    "preview": "/*\n * TCPStack.h\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com\n *"
  },
  {
    "path": "Gateway/src/lib/TLSStack.cpp",
    "chars": 7835,
    "preview": "/*\n * TLSStack.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com\n"
  },
  {
    "path": "Gateway/src/lib/TLSStack.h",
    "chars": 2486,
    "preview": "/*\n * TLSStack.h\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com\n *"
  },
  {
    "path": "Gateway/src/lib/Topics.cpp",
    "chars": 5313,
    "preview": "/*\n * Topics.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com\n *"
  },
  {
    "path": "Gateway/src/lib/Topics.h",
    "chars": 3010,
    "preview": "/*\n * Topics.h\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com\n *  "
  },
  {
    "path": "Gateway/src/lib/UDPStack.cpp",
    "chars": 9605,
    "preview": "/*\n * UDPStack.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com\n"
  },
  {
    "path": "Gateway/src/lib/UDPStack.h",
    "chars": 4702,
    "preview": "/*\r\n * UDPStack.h\r\n *\r\n *                      The BSD License\r\n *\r\n *           Copyright (c) 2014, tomoaki@tomy-tech.c"
  },
  {
    "path": "Gateway/src/lib/XXXXXStack.cpp",
    "chars": 5170,
    "preview": "/*\n * XXXXXStack.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.co"
  },
  {
    "path": "Gateway/src/lib/XXXXXStack.h",
    "chars": 4347,
    "preview": "/*\r\n * UDPStack.h\r\n *\r\n *                      The BSD License\r\n *\r\n *           Copyright (c) 2014, tomoaki@tomy-tech.c"
  },
  {
    "path": "Gateway/src/lib/ZBStack.cpp",
    "chars": 14417,
    "preview": "/*\n * ZBeeStack.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com"
  },
  {
    "path": "Gateway/src/lib/ZBStack.h",
    "chars": 7545,
    "preview": "/*\r\n * ZBStack.h\r\n  *\r\n *                      The BSD License\r\n *\r\n *           Copyright (c) 2014, tomoaki@tomy-tech.c"
  },
  {
    "path": "LogMonitor/Makefile",
    "chars": 717,
    "preview": "PROGNAME := LogMonitor\nSRCDIR := src\nSUBDIR := src/lib\n\nSRCS := $(SRCDIR)/LogMonitorApp.cpp \\\n$(SRCDIR)/LogMonitor.cpp \\"
  },
  {
    "path": "LogMonitor/src/LogMonitor.cpp",
    "chars": 2216,
    "preview": "/*\n * LogMonitor.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.co"
  },
  {
    "path": "LogMonitor/src/LogMonitor.h",
    "chars": 1833,
    "preview": "/*\n * LogMonitor.h\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com\n"
  },
  {
    "path": "LogMonitor/src/LogMonitorApp.cpp",
    "chars": 1821,
    "preview": "/*\n * LogMonitorApp.cpp\n *\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech"
  },
  {
    "path": "LogMonitor/src/lib/Defines.h",
    "chars": 1967,
    "preview": "/*\n *   Defines.h\n *                      The BSD License\n *\n *           Copyright (c) 2014, tomoaki@tomy-tech.com\n *  "
  },
  {
    "path": "LogMonitor/src/lib/ProcessFramework.cpp",
    "chars": 17795,
    "preview": "/*\n * ProcessFramework.cpp\n *\n * The MIT License (MIT)\n *\n * Copyright (c) 2015 tomoaki@tomy-tech.com\n *\n * Permission i"
  },
  {
    "path": "LogMonitor/src/lib/ProcessFramework.h",
    "chars": 8285,
    "preview": "/*\n * ProcessFramework.h\n *\n * The MIT License (MIT)\n *\n * Copyright (c) 2015 tomoaki@tomy-tech.com\n *\n * Permission is "
  },
  {
    "path": "README.md",
    "chars": 2910,
    "preview": "MQTT-SN  over UDP and XBee \n======\n***Updated Gateway has been contributed to paho.***       \n***Latest Gateway can conn"
  }
]

About this extraction

This page contains the full source code of the ty4tw/MQTT-SN GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 65 files (535.4 KB), approximately 151.8k tokens, and a symbol index with 238 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.

Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.

Copied to clipboard!