From 74a9ebaa55f9872ec8a295d6226b2ba4ed1de36b Mon Sep 17 00:00:00 2001
From: tomoaki <tomoaki@tomy-tech.com>
Date: Wed, 18 Jul 2018 21:03:03 +0900
Subject: [PATCH 1/5] BugFix of #121

Signed-off-by: tomoaki <tomoaki@tomy-tech.com>
---
 .../samples/ClientPub/mainPub.cpp             |  12 +-
 .../samples/ClientSub/mainSub.cpp             |  23 +-
 .../GatewayTester/samples/mainTest.cpp        |   5 +-
 .../GatewayTester/src/LRegisterManager.cpp    |   2 +-
 .../GatewayTester/src/LSubscribeManager.cpp   | 545 +++++++++---------
 MQTTSNGateway/src/MQTTGWPublishHandler.cpp    |   2 +-
 MQTTSNGateway/src/MQTTSNGWClient.cpp          |  31 +-
 MQTTSNGateway/src/MQTTSNGWClient.h            |   8 +
 .../src/MQTTSNGWConnectionHandler.cpp         |  20 +-
 MQTTSNGateway/src/MQTTSNGWPublishHandler.cpp  |  11 +
 10 files changed, 366 insertions(+), 293 deletions(-)

diff --git a/MQTTSNGateway/GatewayTester/samples/ClientPub/mainPub.cpp b/MQTTSNGateway/GatewayTester/samples/ClientPub/mainPub.cpp
index e9a676f..816dba7 100644
--- a/MQTTSNGateway/GatewayTester/samples/ClientPub/mainPub.cpp
+++ b/MQTTSNGateway/GatewayTester/samples/ClientPub/mainPub.cpp
@@ -77,7 +77,7 @@ MQTTSNCONF = {
 const char* topic1 = "ty4tw/topic1";
 const char* topic2 = "ty4tw/topic2";
 const char* topic3 = "ty4tw/topic3";
-
+const char* topic57 = "ty4tw/topic5/7";
 
 /*------------------------------------------------------
  *       Callback routines for Subscribed Topics
@@ -112,6 +112,14 @@ void publishTopic2(void)
     PUBLISH(topic2,(uint8_t*)payload, strlen(payload), qos);
 }
 
+void publishTopic57(void)
+{
+    char payload[300];
+    sprintf(payload, "publish \"ty4tw/topic57\" \n");
+    uint8_t qos = 0;
+    PUBLISH(topic2,(uint8_t*)payload, strlen(payload), qos);
+}
+
 
 void disconnect(void)
 {
@@ -127,7 +135,7 @@ void disconnect(void)
 
 TEST_LIST = {// e.g. TEST( Label, Test),
              TEST("Step1:Publish topic1",     publishTopic1),
-             TEST("Step2:Publish topic2",     publishTopic2),
+             TEST("Step2:Publish topic57",     publishTopic57),
              TEST("Step3:Publish topic2",     publishTopic2),
              TEST("Step4:Disconnect",        disconnect),
              END_OF_TEST_LIST
diff --git a/MQTTSNGateway/GatewayTester/samples/ClientSub/mainSub.cpp b/MQTTSNGateway/GatewayTester/samples/ClientSub/mainSub.cpp
index f462fe7..e016732 100644
--- a/MQTTSNGateway/GatewayTester/samples/ClientSub/mainSub.cpp
+++ b/MQTTSNGateway/GatewayTester/samples/ClientSub/mainSub.cpp
@@ -62,7 +62,7 @@ UDPCONF  = {
  *    Client Configuration  (theMqcon)
  *------------------------------------------------------*/
 MQTTSNCONF = {
-	300,            //KeepAlive [seconds]
+	 60,            //KeepAlive [seconds]
 	false,          //Clean session
 	300,           //Sleep duration [seconds]
 	"",            //WillTopic
@@ -109,14 +109,22 @@ int on_Topic03(uint8_t* pload, uint16_t ploadlen)
 	return 0;
 }
 
+int on_Topic05(uint8_t* pload, uint16_t ploadlen)
+{
+    DISPLAY("\n\nNew callback recv TopicA\n");
+    pload[ploadlen-1]= 0;   // set null terminator
+    DISPLAY("Payload -->%s　<--\n\n",pload);
+    return 0;
+}
+
 /*------------------------------------------------------
  *      A Link list of Callback routines and Topics
  *------------------------------------------------------*/
 
 SUBSCRIBE_LIST = {// e.g. SUB(TopicType, topicName, TopicId, callback, QoSx),
 
-                  // SUB(MQTTSN_TOPIC_TYPE_NORMAL, topic1, 0, on_Topic01, QoS1),
-                  // SUB(MQTTSN_TOPIC_TYPE_NORMAL, topic2, 0, on_Topic02, QoS1),
+                  // SUB(MQTTSN_TOPIC_TYPE_NORMAL, topic5, 0, on_Topic05, QoS1),
+                  //SUB(MQTTSN_TOPIC_TYPE_NORMAL, topic2, 0, on_Topic02, QoS1),
                   END_OF_SUBSCRIBE_LIST
                  };
 
@@ -139,6 +147,11 @@ void subscribeTopic2(void)
 	SUBSCRIBE(topic2, on_Topic02, qos);
 }
 
+void subscribeTopic5(void)
+{
+    uint8_t qos = 1;
+    SUBSCRIBE(topic5, on_Topic05, qos);
+}
 
 void disconnect(void)
 {
@@ -162,9 +175,9 @@ void asleep(void)
  *------------------------------------------------------*/
 
 TEST_LIST = {// e.g. TEST( Label, Test),
-			 TEST("Step1:Subscribe topic1",     subscribeTopic1),
+			 TEST("Step1:Subscribe topic5",     subscribeTopic5),
 			 //TEST("Step2:Subscribe topic2",     subscribeTopic2),
-			 TEST("Step2:Disconnect",     disconnect),
+			 TEST("Step2:Disconnect",     asleep),
 			 END_OF_TEST_LIST
 			};
 
diff --git a/MQTTSNGateway/GatewayTester/samples/mainTest.cpp b/MQTTSNGateway/GatewayTester/samples/mainTest.cpp
index 000ad51..a5e001b 100644
--- a/MQTTSNGateway/GatewayTester/samples/mainTest.cpp
+++ b/MQTTSNGateway/GatewayTester/samples/mainTest.cpp
@@ -78,7 +78,10 @@ const char* topic1 = "ty4tw/topic1";
 const char* topic2 = "ty4tw/topic2";
 const char* topic3 = "ty4tw/topic3";
 const char* topic4 = "ty4tw/topic4";
-const char* topic5 = "ty4tw/topic5";
+const char* topic51 = "ty4tw/topic5/1";
+const char* topic52 = "ty4tw/topic5/2";
+const char* topic53 = "ty4tw/topic5/3";
+const char* topic50 = "ty4tw/topic5/+";
 
 
 /*------------------------------------------------------
diff --git a/MQTTSNGateway/GatewayTester/src/LRegisterManager.cpp b/MQTTSNGateway/GatewayTester/src/LRegisterManager.cpp
index 5a924d4..b7e4197 100644
--- a/MQTTSNGateway/GatewayTester/src/LRegisterManager.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LRegisterManager.cpp
@@ -206,7 +206,7 @@ void LRegisterManager::responceRegister(uint8_t* msg, uint16_t msglen)
 	uint8_t regack[7];
 	regack[0] = 7;
 	regack[1] = MQTTSN_TYPE_REGACK;
-	memcpy(regack + 2, msg + 2, 4);
+	memcpy(regack + 2, msg + 1, 4);
 
 	LTopic* tp = theClient->getGwProxy()->getTopicTable()->match((char*) msg + 5);
 	if (tp)
diff --git a/MQTTSNGateway/GatewayTester/src/LSubscribeManager.cpp b/MQTTSNGateway/GatewayTester/src/LSubscribeManager.cpp
index 9f9cf5d..5158003 100644
--- a/MQTTSNGateway/GatewayTester/src/LSubscribeManager.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LSubscribeManager.cpp
@@ -39,119 +39,119 @@ extern LScreen* theScreen;
  =======================================*/
 LSubscribeManager::LSubscribeManager()
 {
-	_first = 0;
-	_last = 0;
+    _first = 0;
+    _last = 0;
 }
 
 LSubscribeManager::~LSubscribeManager()
 {
-	SubElement* elm = _first;
-	SubElement* sav = 0;
-	while (elm)
-	{
-		sav = elm->next;
-		if (elm != 0)
-		{
-			free(elm);
-		}
-		elm = sav;
-	}
+    SubElement* elm = _first;
+    SubElement* sav = 0;
+    while (elm)
+    {
+        sav = elm->next;
+        if (elm != 0)
+        {
+            free(elm);
+        }
+        elm = sav;
+    }
 }
 
 void LSubscribeManager::onConnect(void)
 {
-	DISPLAY("\033[0m\033[0;32m Attempting OnConnect.....\033[0m\033[0;37m\n");
-	if (_first == 0)
-	{
-		for (uint8_t i = 0; theOnPublishList[i].topic != 0; i++)
-		{
-		    if ( theOnPublishList[i].type == MQTTSN_TOPIC_TYPE_PREDEFINED)
-		    {
-		        subscribe(theOnPublishList[i].id, theOnPublishList[i].pubCallback, theOnPublishList[i].qos);
-		    }
-		    else
-		    {
-		        subscribe(theOnPublishList[i].topic, theOnPublishList[i].pubCallback, theOnPublishList[i].qos);
-		    }
-		}
-	}
-	else
-	{
-		SubElement* elm = _first;
-		SubElement* pelm;
-		do
-		{
-			pelm = elm;
-			if (elm->msgType == MQTTSN_TYPE_SUBSCRIBE)
-			{
-				elm->done = SUB_READY;
-				elm->retryCount = MQTTSN_RETRY_COUNT;
-				subscribe(elm->topicName, elm->callback, elm->qos);
-			}
-			elm = pelm->next;
-		} while (pelm->next);
-	}
+    DISPLAY("\033[0m\033[0;32m Attempting OnConnect.....\033[0m\033[0;37m\n");
+    if (_first == 0)
+    {
+        for (uint8_t i = 0; theOnPublishList[i].topic != 0; i++)
+        {
+            if ( theOnPublishList[i].type == MQTTSN_TOPIC_TYPE_PREDEFINED)
+            {
+                subscribe(theOnPublishList[i].id, theOnPublishList[i].pubCallback, theOnPublishList[i].qos);
+            }
+            else
+            {
+                subscribe(theOnPublishList[i].topic, theOnPublishList[i].pubCallback, theOnPublishList[i].qos);
+            }
+        }
+    }
+    else
+    {
+        SubElement* elm = _first;
+        SubElement* pelm;
+        do
+        {
+            pelm = elm;
+            if (elm->msgType == MQTTSN_TYPE_SUBSCRIBE)
+            {
+                elm->done = SUB_READY;
+                elm->retryCount = MQTTSN_RETRY_COUNT;
+                subscribe(elm->topicName, elm->callback, elm->qos);
+            }
+            elm = pelm->next;
+        } while (pelm->next);
+    }
 
-	while (!theClient->getSubscribeManager()->isDone())
-	{
-		theClient->getGwProxy()->getMessage();
-	}
-	DISPLAY("\033[0m\033[0;32m OnConnect complete\033[0m\033[0;37m\n");
-	DISPLAY("\033[0m\033[0;32m Test is Ready.\033[0m\033[0;37m\n");
+    while (!theClient->getSubscribeManager()->isDone())
+    {
+        theClient->getGwProxy()->getMessage();
+    }
+    DISPLAY("\033[0m\033[0;32m OnConnect complete\033[0m\033[0;37m\n");
+    DISPLAY("\033[0m\033[0;32m Test is Ready.\033[0m\033[0;37m\n");
 }
 
 bool LSubscribeManager::isDone(void)
 {
-	SubElement* elm = _first;
-	SubElement* prevelm;
-	while (elm)
-	{
-		prevelm = elm;
-		if (elm->done == SUB_READY)
-		{
-			return false;
-		}
-		elm = prevelm->next;
-	}
-	return true;
+    SubElement* elm = _first;
+    SubElement* prevelm;
+    while (elm)
+    {
+        prevelm = elm;
+        if (elm->done == SUB_READY)
+        {
+            return false;
+        }
+        elm = prevelm->next;
+    }
+    return true;
 }
 
 void LSubscribeManager::send(SubElement* elm)
 {
-	if (elm->done == SUB_DONE)
-	{
-		return;
-	}
-	uint8_t msg[MQTTSN_MAX_MSG_LENGTH + 1];
-	if (elm->topicType == MQTTSN_TOPIC_TYPE_NORMAL)
-	{
-		msg[0] = 5 + strlen(elm->topicName);
-		strcpy((char*) msg + 5, elm->topicName);
-	}
-	else
-	{
-		msg[0] = 7;
-		setUint16(msg + 5, elm->topicId);
-	}
-	msg[1] = elm->msgType;
-	msg[2] = elm->qos | elm->topicType;
-	if (elm->retryCount == MQTTSN_RETRY_COUNT)
-	{
-		elm->msgId = theClient->getGwProxy()->getNextMsgId();
-	}
+    if (elm->done == SUB_DONE)
+    {
+        return;
+    }
+    uint8_t msg[MQTTSN_MAX_MSG_LENGTH + 1];
+    if (elm->topicType == MQTTSN_TOPIC_TYPE_PREDEFINED)
+    {
+        msg[0] = 7;
+        setUint16(msg + 5, elm->topicId);
+    }
+    else
+    {
+        msg[0] = 5 + strlen(elm->topicName);
+        strcpy((char*) msg + 5, elm->topicName);
+    }
+    msg[1] = elm->msgType;
+    msg[2] = elm->qos | elm->topicType;
+    if (elm->retryCount == MQTTSN_RETRY_COUNT)
+    {
+        elm->msgId = theClient->getGwProxy()->getNextMsgId();
+    }
 
-	if ((elm->retryCount < MQTTSN_RETRY_COUNT) && elm->msgType == MQTTSN_TYPE_SUBSCRIBE)
-	{
-		msg[2] = msg[2] | MQTTSN_FLAG_DUP;
-	}
+    if ((elm->retryCount < MQTTSN_RETRY_COUNT) && elm->msgType == MQTTSN_TYPE_SUBSCRIBE)
+    {
+        msg[2] = msg[2] | MQTTSN_FLAG_DUP;
+    }
 
-	setUint16(msg + 3, elm->msgId);
+    setUint16(msg + 3, elm->msgId);
 
-	theClient->getGwProxy()->connect();
-	theClient->getGwProxy()->writeMsg(msg);
-	theClient->getGwProxy()->setPingReqTimer();
-	elm->sendUTC = time(NULL);
-	elm->retryCount--;
+    theClient->getGwProxy()->connect();
+    theClient->getGwProxy()->writeMsg(msg);
+    theClient->getGwProxy()->setPingReqTimer();
+    elm->sendUTC = time(NULL);
+    elm->retryCount--;
 }
 
 void LSubscribeManager::subscribe(const char* topicName, TopicCallback onPublish, uint8_t qos)
@@ -171,8 +171,8 @@ void LSubscribeManager::subscribe(const char* topicName, TopicCallback onPublish
 
 void LSubscribeManager::subscribe(uint16_t topicId, TopicCallback onPublish, uint8_t qos)
 {
-	SubElement* elm = add(MQTTSN_TYPE_SUBSCRIBE, 0, MQTTSN_TOPIC_TYPE_PREDEFINED, topicId,  qos, onPublish);
-	send(elm);
+    SubElement* elm = add(MQTTSN_TYPE_SUBSCRIBE, 0, MQTTSN_TOPIC_TYPE_PREDEFINED, topicId,  qos, onPublish);
+    send(elm);
 }
 
 void LSubscribeManager::unsubscribe(const char* topicName)
@@ -186,220 +186,221 @@ void LSubscribeManager::unsubscribe(const char* topicName)
     {
         topicType = MQTTSN_TOPIC_TYPE_SHORT;
     }
-	SubElement* elm = add(MQTTSN_TYPE_UNSUBSCRIBE, topicName, topicType, 0, 0, 0);
-	send(elm);
+    SubElement* elm = add(MQTTSN_TYPE_UNSUBSCRIBE, topicName, topicType, 0, 0, 0);
+    send(elm);
 }
 
 void LSubscribeManager::unsubscribe( uint16_t topicId)
 {
-	SubElement* elm = add(MQTTSN_TYPE_UNSUBSCRIBE, 0, MQTTSN_TOPIC_TYPE_PREDEFINED, topicId, 0, 0);
-	send(elm);
+    SubElement* elm = add(MQTTSN_TYPE_UNSUBSCRIBE, 0, MQTTSN_TOPIC_TYPE_PREDEFINED, topicId, 0, 0);
+    send(elm);
 }
 
 void LSubscribeManager::checkTimeout(void)
 {
-	SubElement* elm = _first;
+    SubElement* elm = _first;
 
-	while (elm)
-	{
-		if (elm->sendUTC + MQTTSN_TIME_RETRY < time(NULL))
-		{
-			if (elm->retryCount >= 0)
-			{
-				send(elm);
-			}
-			else
-			{
-				if ( elm->done == SUB_READY )
-				{
-					if (elm->msgType == MQTTSN_TYPE_SUBSCRIBE)
-					{
-						DISPLAY("\033[0m\033[0;31m\n!!!!!! SUBSCRIBE  Error !!!!! Topic : %s\033[0m\033[0;37m\n\n", (char*)elm->topicName);
-					}else{
-						DISPLAY("\033[0m\033[0;31m\n!!!!!! UNSUBSCRIBE  Error !!!!! Topic : %s\033[0m\033[0;37m\n\n", (char*)elm->topicName);
-					}
-					elm->done = SUB_DONE;
-				}
-			}
-		}
-		elm = elm->next;
-	}
+    while (elm)
+    {
+        if (elm->sendUTC + MQTTSN_TIME_RETRY < time(NULL))
+        {
+            if (elm->retryCount >= 0)
+            {
+                send(elm);
+            }
+            else
+            {
+                if ( elm->done == SUB_READY )
+                {
+                    if (elm->msgType == MQTTSN_TYPE_SUBSCRIBE)
+                    {
+                        DISPLAY("\033[0m\033[0;31m\n!!!!!! SUBSCRIBE  Error !!!!! Topic : %s\033[0m\033[0;37m\n\n", (char*)elm->topicName);
+                    }else{
+                        DISPLAY("\033[0m\033[0;31m\n!!!!!! UNSUBSCRIBE  Error !!!!! Topic : %s\033[0m\033[0;37m\n\n", (char*)elm->topicName);
+                    }
+                    elm->done = SUB_DONE;
+                }
+            }
+        }
+        elm = elm->next;
+    }
 }
 
 void LSubscribeManager::responce(const uint8_t* msg)
 {
-	if (msg[0] == MQTTSN_TYPE_SUBACK)
-	{
-		uint16_t topicId = getUint16(msg + 2);
-		uint16_t msgId = getUint16(msg + 4);
-		uint8_t rc = msg[6];
+    if (msg[0] == MQTTSN_TYPE_SUBACK)
+    {
+        uint16_t topicId = getUint16(msg + 2);
+        uint16_t msgId = getUint16(msg + 4);
+        uint8_t rc = msg[6];
 
-		SubElement* elm = getElement(msgId);
-		if (elm)
-		{
-			if ( rc == MQTTSN_RC_ACCEPTED )
-			{
-			    theClient->getGwProxy()->getTopicTable()->add((char*) elm->topicName, elm->topicType, topicId, elm->callback);
-				getElement(msgId)->done = SUB_DONE;
-				DISPLAY("\033[0m\033[0;32m Topic \"%s\" Id : %d was Subscribed. \033[0m\033[0;37m\n\n", getElement(msgId)->topicName, topicId);
-			}
-			else
-			{
-				DISPLAY("\033[0m\033[0;31m SUBACK Invalid messageId. %s\033[0m\033[0;37m\n\n", getElement(msgId)->topicName);
-				remove(elm);
-			}
-		}
-	}
-	else if (msg[0] == MQTTSN_TYPE_UNSUBACK)
-	{
-		uint16_t msgId = getUint16(msg + 1);
-		SubElement* elm = getElement(msgId);
-		if (elm)
-		{
-			//theClient->getGwProxy()->getTopicTable()->setCallback(elm->topicName, 0);
-			DISPLAY("\033[0m\033[0;32m Topic  \"%s\"  was Unsubscribed. \033[0m\033[0;37m\n\n", getElement(msgId)->topicName);
-			remove(elm);
-		}
-		else
-		{
-			DISPLAY("\033[0m\033[0;31m UNSUBACK Invalid messageId. \033[0m\033[0;37m\n\n");
-		}
-	}
+        SubElement* elm = getElement(msgId);
+        if (elm)
+        {
+            if ( rc == MQTTSN_RC_ACCEPTED )
+            {
+                theClient->getGwProxy()->getTopicTable()->add((char*) elm->topicName, elm->topicType, topicId, elm->callback);
+                getElement(msgId)->done = SUB_DONE;
+                DISPLAY("\033[0m\033[0;32m Topic \"%s\" Id : %d was Subscribed. \033[0m\033[0;37m\n\n", getElement(msgId)->topicName, topicId);
+            }
+            else
+            {
+                DISPLAY("\033[0m\033[0;31m SUBACK Invalid messageId. %s\033[0m\033[0;37m\n\n", getElement(msgId)->topicName);
+                remove(elm);
+            }
+        }
+    }
+    else if (msg[0] == MQTTSN_TYPE_UNSUBACK)
+    {
+        uint16_t msgId = getUint16(msg + 1);
+        SubElement* elm = getElement(msgId);
+        if (elm)
+        {
+            //theClient->getGwProxy()->getTopicTable()->setCallback(elm->topicName, 0);
+            DISPLAY("\033[0m\033[0;32m Topic  \"%s\"  was Unsubscribed. \033[0m\033[0;37m\n\n", getElement(msgId)->topicName);
+            remove(elm);
+        }
+        else
+        {
+            DISPLAY("\033[0m\033[0;31m UNSUBACK Invalid messageId. \033[0m\033[0;37m\n\n");
+        }
+    }
 }
 
 /* SubElement operations */
 
 SubElement* LSubscribeManager::add(uint8_t msgType, const char* topicName, MQTTSN_topicTypes topicType, uint16_t topicId,
-		uint8_t qos, TopicCallback callback)
+        uint8_t qos, TopicCallback callback)
 {
-	SubElement* elm = 0;
-	if (topicName )
-	{
-		elm = getElement(topicName, msgType);
-	}
-	else
-	{
-		elm = getElement(topicId, topicType);
-	}
-	if ( elm  == 0 )
-	{
-		elm = (SubElement*) calloc(1, sizeof(SubElement));
-		if (elm == 0)
-		{
-			return 0;
-		}
-		if (_last == 0)
-		{
-			_first = elm;
-			_last = elm;
-		}
-		else
-		{
-			elm->prev = _last;
-			_last->next = elm;
-			_last = elm;
-		}
-	}
+    SubElement* elm = 0;
+    if (topicName )
+    {
+        elm = getElement(topicName, msgType);
+    }
+    else
+    {
+        elm = getElement(topicId, topicType);
+    }
 
-	elm->msgType = msgType;
-	elm->callback = callback;
-	elm->topicName = topicName;
-	elm->topicId = topicId;
-	elm->topicType = topicType;
+    if ( elm  == 0 )
+    {
+        elm = (SubElement*) calloc(1, sizeof(SubElement));
+        if (elm == 0)
+        {
+            return 0;
+        }
+        if (_last == 0)
+        {
+            _first = elm;
+            _last = elm;
+        }
+        else
+        {
+            elm->prev = _last;
+            _last->next = elm;
+            _last = elm;
+        }
+    }
 
-	if (qos == 1)
-	{
-		elm->qos = MQTTSN_FLAG_QOS_1;
-	}
-	else if (qos == 2)
-	{
-		elm->qos = MQTTSN_FLAG_QOS_2;
-	}
-	else
-	{
-		elm->qos = MQTTSN_FLAG_QOS_0;
-	}
-	elm->msgId = 0;
-	elm->retryCount = MQTTSN_RETRY_COUNT;
-	elm->done = SUB_READY;
-	elm->sendUTC = 0;
+    elm->msgType = msgType;
+    elm->callback = callback;
+    elm->topicName = topicName;
+    elm->topicId = topicId;
+    elm->topicType = topicType;
 
-	return elm;
+    if (qos == 1)
+    {
+        elm->qos = MQTTSN_FLAG_QOS_1;
+    }
+    else if (qos == 2)
+    {
+        elm->qos = MQTTSN_FLAG_QOS_2;
+    }
+    else
+    {
+        elm->qos = MQTTSN_FLAG_QOS_0;
+    }
+    elm->msgId = 0;
+    elm->retryCount = MQTTSN_RETRY_COUNT;
+    elm->done = SUB_READY;
+    elm->sendUTC = 0;
+
+    return elm;
 }
 
 void LSubscribeManager::remove(SubElement* elm)
 {
-	if (elm)
-	{
-		if (elm->prev == 0)
-		{
-			_first = elm->next;
-			if (elm->next != 0)
-			{
-				elm->next->prev = 0;
-				_last = elm->next;
-			}
-			free(elm);
-		}
-		else
-		{
-			if ( elm->next == 0 )
-			{
-				_last = elm->prev;
-			}
-			elm->prev->next = elm->next;
-			free(elm);
-		}
-	}
+    if (elm)
+    {
+        if (elm->prev == 0)
+        {
+            _first = elm->next;
+            if (elm->next != 0)
+            {
+                elm->next->prev = 0;
+                _last = elm->next;
+            }
+            free(elm);
+        }
+        else
+        {
+            if ( elm->next == 0 )
+            {
+                _last = elm->prev;
+            }
+            elm->prev->next = elm->next;
+            free(elm);
+        }
+    }
 }
 
 SubElement* LSubscribeManager::getElement(uint16_t msgId)
 {
-	SubElement* elm = _first;
-	while (elm)
-	{
-		if (elm->msgId == msgId)
-		{
-			return elm;
-		}
-		else
-		{
-			elm = elm->next;
-		}
-	}
-	return 0;
+    SubElement* elm = _first;
+    while (elm)
+    {
+        if (elm->msgId == msgId)
+        {
+            return elm;
+        }
+        else
+        {
+            elm = elm->next;
+        }
+    }
+    return 0;
 }
 
 SubElement* LSubscribeManager::getElement(const char* topicName, uint8_t msgType)
 {
-	SubElement* elm = _first;
-	while (elm)
-	{
-		if ( elm->msgType == msgType &&  strncmp(elm->topicName, topicName, strlen(topicName)) == 0 )
-		{
-			return elm;
-		}
-		else
-		{
-			elm = elm->next;
-		}
-	}
-	return 0;
+    SubElement* elm = _first;
+    while (elm)
+    {
+        if ( elm->msgType == msgType &&  strncmp(elm->topicName, topicName, strlen(topicName)) == 0 )
+        {
+            return elm;
+        }
+        else
+        {
+            elm = elm->next;
+        }
+    }
+    return 0;
 }
 
 SubElement* LSubscribeManager::getElement(uint16_t topicId, MQTTSN_topicTypes topicType)
 {
-	SubElement* elm = _first;
-	while (elm)
-	{
-		if (elm->topicId == topicId && elm->topicType == topicType)
-		{
-			return elm;
-		}
-		else
-		{
-			elm = elm->next;
-		}
-	}
-	return 0;
+    SubElement* elm = _first;
+    while (elm)
+    {
+        if (elm->topicId == topicId && elm->topicType == topicType)
+        {
+            return elm;
+        }
+        else
+        {
+            elm = elm->next;
+        }
+    }
+    return 0;
 }
diff --git a/MQTTSNGateway/src/MQTTGWPublishHandler.cpp b/MQTTSNGateway/src/MQTTGWPublishHandler.cpp
index f665cec..b29fea0 100644
--- a/MQTTSNGateway/src/MQTTGWPublishHandler.cpp
+++ b/MQTTSNGateway/src/MQTTGWPublishHandler.cpp
@@ -82,7 +82,7 @@ void MQTTGWPublishHandler::handlePublish(Client* client, MQTTGWPacket* packet)
 	MQTTSN_topicid topicId;
 	uint16_t id = 0;
 
-	if (pub.topiclen == 2)
+	if (pub.topiclen <= 2)
 	{
 		topicId.type = MQTTSN_TOPIC_TYPE_SHORT;
 		*(topicId.data.short_name) = *pub.topic;
diff --git a/MQTTSNGateway/src/MQTTSNGWClient.cpp b/MQTTSNGateway/src/MQTTSNGWClient.cpp
index 65e29d5..9aa2a18 100644
--- a/MQTTSNGateway/src/MQTTSNGWClient.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWClient.cpp
@@ -400,6 +400,7 @@ Client::Client(bool secure)
 	_nextClient = 0;
 	_clientSleepPacketQue.setMaxSize(MAX_SAVED_PUBLISH);
 	_hasPredefTopic = false;
+	_holdPingRequest = false;
 }
 
 Client::~Client()
@@ -796,6 +797,21 @@ void Client::setOTAClient(Client* cl)
 	_otaClient =cl;
 }
 
+void Client::holdPingRequest(void)
+{
+    _holdPingRequest = true;
+}
+
+void Client::resetPingRequest(void)
+{
+    _holdPingRequest = false;
+}
+
+bool Client::isHoldPringReqest(void)
+{
+    return _holdPingRequest;
+}
+
 /*=====================================
  Class Topic
  ======================================*/
@@ -1214,7 +1230,7 @@ TopicIdMapelement* TopicIdMap::getElement(uint16_t msgId)
 
 TopicIdMapelement* TopicIdMap::add(uint16_t msgId, uint16_t topicId, MQTTSN_topicTypes type)
 {
-	if ( _cnt > _maxInflight * 2 || topicId == 0)
+	if ( _cnt > _maxInflight * 2 || ( topicId == 0 && type != MQTTSN_TOPIC_TYPE_SHORT ) )
 	{
 		return 0;
 	}
@@ -1314,6 +1330,7 @@ WaitREGACKPacketList::WaitREGACKPacketList()
 {
 	_first = 0;
 	_end = 0;
+	_cnt = 0;
 }
 
 WaitREGACKPacketList::~WaitREGACKPacketList()
@@ -1346,6 +1363,7 @@ int WaitREGACKPacketList::setPacket(MQTTSNPacket* packet, uint16_t REGACKMsgId)
 		elm->_prev = _end;
 		_end = elm;
 	}
+	_cnt++;
 	return 1;
 }
 
@@ -1387,10 +1405,17 @@ void WaitREGACKPacketList::erase(uint16_t REGACKMsgId)
 			{
 				p->_next->_prev = p->_prev;
 			}
-			break;
-			// Do not delete element. Element is deleted after sending to Client.
+			_cnt--;
+            break;
+            // Do not delete element. Element is deleted after sending to Client.
 		}
 		p = p->_next;
 	}
 }
 
+uint8_t WaitREGACKPacketList::getCount(void)
+{
+    return _cnt;
+}
+
+
diff --git a/MQTTSNGateway/src/MQTTSNGWClient.h b/MQTTSNGateway/src/MQTTSNGWClient.h
index 9bb5ede..118239e 100644
--- a/MQTTSNGateway/src/MQTTSNGWClient.h
+++ b/MQTTSNGateway/src/MQTTSNGWClient.h
@@ -219,8 +219,10 @@ public:
     int setPacket(MQTTSNPacket* packet, uint16_t REGACKMsgId);
     MQTTSNPacket* getPacket(uint16_t REGACKMsgId);
     void erase(uint16_t REGACKMsgId);
+    uint8_t getCount(void);
 
 private:
+    uint8_t _cnt;
     waitREGACKPacket* _first;
     waitREGACKPacket* _end;
 };
@@ -298,6 +300,10 @@ public:
     bool isSensorNetStable(void);
     bool isWaitWillMsg(void);
 
+    void holdPingRequest(void);
+    void resetPingRequest(void);
+    bool isHoldPringReqest(void);
+
     Client* getNextClient(void);
     Client* getOTAClient(void);
     void    setOTAClient(Client* cl);
@@ -317,6 +323,8 @@ private:
     char* _willTopic;
     char* _willMsg;
 
+    bool _holdPingRequest;
+
     Timer _keepAliveTimer;
     uint32_t _keepAliveMsec;
 
diff --git a/MQTTSNGateway/src/MQTTSNGWConnectionHandler.cpp b/MQTTSNGateway/src/MQTTSNGWConnectionHandler.cpp
index 66e2d79..d9fab69 100644
--- a/MQTTSNGateway/src/MQTTSNGWConnectionHandler.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWConnectionHandler.cpp
@@ -270,7 +270,7 @@ void MQTTSNConnectionHandler::handlePingreq(Client* client, MQTTSNPacket* packet
 {
 	MQTTGWPacket* msg = 0;
 
-	if ( client->isSleep() || client->isAwake() )
+	if ( ( client->isSleep() || client->isAwake() ) &&  client->getClientSleepPacket() )
 	{
 		while  ( ( msg = client->getClientSleepPacket() ) != 0 )
 		{
@@ -281,12 +281,16 @@ void MQTTSNConnectionHandler::handlePingreq(Client* client, MQTTSNPacket* packet
 			ev->setBrokerRecvEvent(client, msg);
 			_gateway->getPacketEventQue()->post(ev);
 		}
+		client->holdPingRequest();
+	}
+	else
+	{
+        /* send PINGREQ to the broker */
+	    client->resetPingRequest();
+        MQTTGWPacket* pingreq = new MQTTGWPacket();
+        pingreq->setHeader(PINGREQ);
+        Event* evt = new Event();
+        evt->setBrokerSendEvent(client, pingreq);
+        _gateway->getBrokerSendQue()->post(evt);
 	}
-
-	/* send PINGREQ to the broker */
-	MQTTGWPacket* pingreq = new MQTTGWPacket();
-	pingreq->setHeader(PINGREQ);
-	Event* evt = new Event();
-	evt->setBrokerSendEvent(client, pingreq);
-	_gateway->getBrokerSendQue()->post(evt);
 }
diff --git a/MQTTSNGateway/src/MQTTSNGWPublishHandler.cpp b/MQTTSNGateway/src/MQTTSNGWPublishHandler.cpp
index f797ca0..4f0adc4 100644
--- a/MQTTSNGateway/src/MQTTSNGWPublishHandler.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWPublishHandler.cpp
@@ -198,6 +198,7 @@ void MQTTSNPublishHandler::handleRegAck( Client* client, MQTTSNPacket* packet)
         }
 
         MQTTSNPacket* regAck = client->getWaitREGACKPacketList()->getPacket(msgId);
+
         if ( regAck != 0 )
         {
             client->getWaitREGACKPacketList()->erase(msgId);
@@ -205,6 +206,16 @@ void MQTTSNPublishHandler::handleRegAck( Client* client, MQTTSNPacket* packet)
             ev->setClientSendEvent(client, regAck);
             _gateway->getClientSendQue()->post(ev);
         }
+        if (client->isHoldPringReqest() && client->getWaitREGACKPacketList()->getCount() == 0 )
+        {
+            /* send PINGREQ to the broker */
+           client->resetPingRequest();
+           MQTTGWPacket* pingreq = new MQTTGWPacket();
+           pingreq->setHeader(PINGREQ);
+           Event* evt = new Event();
+           evt->setBrokerSendEvent(client, pingreq);
+           _gateway->getBrokerSendQue()->post(evt);
+        }
     }
 
 }

From 521715e011399811572eec82959f005647f2a289 Mon Sep 17 00:00:00 2001
From: tomoaki <tomoaki@tomy-tech.com>
Date: Wed, 18 Jul 2018 21:32:46 +0900
Subject: [PATCH 2/5] Bugfix of #122

Signed-off-by: tomoaki <tomoaki@tomy-tech.com>
---
 .../src/MQTTSNGWConnectionHandler.cpp         | 27 ++++++++++++-------
 MQTTSNGateway/src/MQTTSNGWConnectionHandler.h |  2 ++
 2 files changed, 20 insertions(+), 9 deletions(-)

diff --git a/MQTTSNGateway/src/MQTTSNGWConnectionHandler.cpp b/MQTTSNGateway/src/MQTTSNGWConnectionHandler.cpp
index d9fab69..721003b 100644
--- a/MQTTSNGateway/src/MQTTSNGWConnectionHandler.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWConnectionHandler.cpp
@@ -82,6 +82,8 @@ void MQTTSNConnectionHandler::handleConnect(Client* client, MQTTSNPacket* packet
 		Event* ev = new Event();
 		ev->setClientSendEvent(client, packet);
 		_gateway->getClientSendQue()->post(ev);
+
+		sendStoredPublish(client);
 		return;
 	}
 
@@ -272,15 +274,7 @@ void MQTTSNConnectionHandler::handlePingreq(Client* client, MQTTSNPacket* packet
 
 	if ( ( client->isSleep() || client->isAwake() ) &&  client->getClientSleepPacket() )
 	{
-		while  ( ( msg = client->getClientSleepPacket() ) != 0 )
-		{
-			// ToDo:  This version can't re-send PUBLISH when PUBACK is not returned.
-			client->deleteFirstClientSleepPacket();  // pop the que to delete element.
-
-			Event* ev = new Event();
-			ev->setBrokerRecvEvent(client, msg);
-			_gateway->getPacketEventQue()->post(ev);
-		}
+	    sendStoredPublish(client);
 		client->holdPingRequest();
 	}
 	else
@@ -294,3 +288,18 @@ void MQTTSNConnectionHandler::handlePingreq(Client* client, MQTTSNPacket* packet
         _gateway->getBrokerSendQue()->post(evt);
 	}
 }
+
+void MQTTSNConnectionHandler::sendStoredPublish(Client* client)
+{
+    MQTTGWPacket* msg = 0;
+
+    while  ( ( msg = client->getClientSleepPacket() ) != 0 )
+    {
+        // ToDo:  This version can't re-send PUBLISH when PUBACK is not returned.
+        client->deleteFirstClientSleepPacket();  // pop the que to delete element.
+
+        Event* ev = new Event();
+        ev->setBrokerRecvEvent(client, msg);
+        _gateway->getPacketEventQue()->post(ev);
+    }
+}
diff --git a/MQTTSNGateway/src/MQTTSNGWConnectionHandler.h b/MQTTSNGateway/src/MQTTSNGWConnectionHandler.h
index 044e766..adf92d4 100644
--- a/MQTTSNGateway/src/MQTTSNGWConnectionHandler.h
+++ b/MQTTSNGateway/src/MQTTSNGWConnectionHandler.h
@@ -37,6 +37,8 @@ public:
 	void handleWillmsgupd(Client* client, MQTTSNPacket* packet);
 	void handlePingreq(Client* client, MQTTSNPacket* packet);
 private:
+	void sendStoredPublish(Client* client);
+
 	char _pbuf[MQTTSNGW_MAX_PACKET_SIZE * 3];
 	Gateway* _gateway;
 };

From 797ddf43f8d5143ca74e64c34f1614ef98d3b232 Mon Sep 17 00:00:00 2001
From: tomoaki <tomoaki@tomy-tech.com>
Date: Wed, 18 Jul 2018 21:40:20 +0900
Subject: [PATCH 3/5] update test program

Signed-off-by: tomoaki <tomoaki@tomy-tech.com>
---
 MQTTSNGateway/GatewayTester/samples/ClientSub/mainSub.cpp | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/MQTTSNGateway/GatewayTester/samples/ClientSub/mainSub.cpp b/MQTTSNGateway/GatewayTester/samples/ClientSub/mainSub.cpp
index e016732..585c8ec 100644
--- a/MQTTSNGateway/GatewayTester/samples/ClientSub/mainSub.cpp
+++ b/MQTTSNGateway/GatewayTester/samples/ClientSub/mainSub.cpp
@@ -63,7 +63,7 @@ UDPCONF  = {
  *------------------------------------------------------*/
 MQTTSNCONF = {
 	 60,            //KeepAlive [seconds]
-	false,          //Clean session
+	true,          //Clean session
 	300,           //Sleep duration [seconds]
 	"",            //WillTopic
 	"",            //WillMessage
@@ -78,7 +78,7 @@ const char* topic1 = "ty4tw/topic1";
 const char* topic2 = "ty4tw/topic2";
 const char* topic3 = "ty4tw/topic3";
 const char* topic4 = "a";
-const char* topic5 = "#";
+const char* topic5 = "ty4tw/#";
 
 
 /*------------------------------------------------------
@@ -178,6 +178,8 @@ TEST_LIST = {// e.g. TEST( Label, Test),
 			 TEST("Step1:Subscribe topic5",     subscribeTopic5),
 			 //TEST("Step2:Subscribe topic2",     subscribeTopic2),
 			 TEST("Step2:Disconnect",     asleep),
+			 TEST("Step3:Cconnect",     connect),
+			 TEST("Step4:Disconnect",     asleep),
 			 END_OF_TEST_LIST
 			};
 
@@ -202,3 +204,4 @@ void setup(void)
 
 
 /*****************  END OF  PROGRAM ********************/
+

From 4d75351a06ae07d04842f0364fa88430e761f9f7 Mon Sep 17 00:00:00 2001
From: tomoaki <tomoaki@tomy-tech.com>
Date: Tue, 24 Jul 2018 07:19:39 +0900
Subject: [PATCH 4/5] BugFix of #123

Signed-off-by: tomoaki <tomoaki@tomy-tech.com>
---
 MQTTSNGateway/src/MQTTSNGateway.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/MQTTSNGateway/src/MQTTSNGateway.cpp b/MQTTSNGateway/src/MQTTSNGateway.cpp
index 23f7e78..afd0197 100644
--- a/MQTTSNGateway/src/MQTTSNGateway.cpp
+++ b/MQTTSNGateway/src/MQTTSNGateway.cpp
@@ -216,7 +216,7 @@ void Gateway::initialize(int argc, char** argv)
 	    {
             if (getParam("PredefinedTopicFile", param) == 0)
             {
-                fileName =*getConfigDirName() + string(param);
+                fileName = string(param);
             }
             else
             {

From 9940aadd4becbff36aff3ec226e6c3e7e9fd3f99 Mon Sep 17 00:00:00 2001
From: tomoaki <tomoaki@tomy-tech.com>
Date: Thu, 26 Jul 2018 17:42:12 +0900
Subject: [PATCH 5/5] Add The forwarder Encapsulation mesage #27 #69

Signed-off-by: tomoaki <tomoaki@tomy-tech.com>
---
 .cproject                                     |   4 +-
 .../GatewayTester/samples/mainTest.cpp        |   2 +-
 MQTTSNGateway/GatewayTester/src/LGwProxy.cpp  | 647 ++++++++++--------
 MQTTSNGateway/GatewayTester/src/LGwProxy.h    | 108 +--
 .../GatewayTester/src/LMqttsnClientApp.h      |   4 +-
 .../GatewayTester/src/LNetworkUdp.cpp         |  10 +-
 MQTTSNGateway/Makefile                        |   5 +
 MQTTSNGateway/README.md                       |  10 +-
 MQTTSNGateway/forwarders.conf                 |  16 +
 MQTTSNGateway/gateway.conf                    |   9 +-
 MQTTSNGateway/src/MQTTSNGWClient.cpp          |  51 +-
 MQTTSNGateway/src/MQTTSNGWClient.h            |  12 +-
 MQTTSNGateway/src/MQTTSNGWClientRecvTask.cpp  |  97 ++-
 MQTTSNGateway/src/MQTTSNGWClientSendTask.cpp  |  26 +-
 MQTTSNGateway/src/MQTTSNGWClientSendTask.h    |   1 +
 .../src/MQTTSNGWConnectionHandler.cpp         |   5 +-
 MQTTSNGateway/src/MQTTSNGWDefines.h           |   3 +-
 .../src/MQTTSNGWEncapsulatedPacket.cpp        | 179 +++++
 .../src/MQTTSNGWEncapsulatedPacket.h          |  65 ++
 MQTTSNGateway/src/MQTTSNGWForwarder.cpp       | 321 +++++++++
 MQTTSNGateway/src/MQTTSNGWForwarder.h         |  88 +++
 MQTTSNGateway/src/MQTTSNGWVersion.h           |   2 +-
 MQTTSNGateway/src/MQTTSNGateway.cpp           |  50 +-
 MQTTSNGateway/src/MQTTSNGateway.h             |  13 +-
 MQTTSNGateway/src/tests/TestProcess.cpp       |  12 +-
 MQTTSNGateway/src/tests/mainTestProcess.cpp   |   2 +-
 MQTTSNPacket/src/MQTTSNPacket.c               |   5 +
 MQTTSNPacket/src/MQTTSNPacket.h               |   8 +-
 28 files changed, 1322 insertions(+), 433 deletions(-)
 create mode 100644 MQTTSNGateway/forwarders.conf
 create mode 100644 MQTTSNGateway/src/MQTTSNGWEncapsulatedPacket.cpp
 create mode 100644 MQTTSNGateway/src/MQTTSNGWEncapsulatedPacket.h
 create mode 100644 MQTTSNGateway/src/MQTTSNGWForwarder.cpp
 create mode 100644 MQTTSNGateway/src/MQTTSNGWForwarder.h

diff --git a/.cproject b/.cproject
index d61f694..c3137e8 100644
--- a/.cproject
+++ b/.cproject
@@ -14,14 +14,14 @@
 				</extensions>
 			</storageModule>
 			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-				<configuration artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug" cleanCommand="rm -rf" description="" id="cdt.managedbuild.config.gnu.exe.debug.1685199227" name="Debug" parent="cdt.managedbuild.config.gnu.exe.debug">
+				<configuration artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug" cleanCommand="rm -rf" description="" id="cdt.managedbuild.config.gnu.exe.debug.1685199227" name="Debug" optionalBuildProperties="org.eclipse.cdt.docker.launcher.containerbuild.property.volumes=,org.eclipse.cdt.docker.launcher.containerbuild.property.selectedvolumes=" parent="cdt.managedbuild.config.gnu.exe.debug">
 					<folderInfo id="cdt.managedbuild.config.gnu.exe.debug.1685199227." name="/" resourcePath="">
 						<toolChain id="cdt.managedbuild.toolchain.gnu.exe.debug.102264757" name="Linux GCC" superClass="cdt.managedbuild.toolchain.gnu.exe.debug">
 							<targetPlatform id="cdt.managedbuild.target.gnu.platform.exe.debug.2137160061" name="Debug Platform" superClass="cdt.managedbuild.target.gnu.platform.exe.debug"/>
 							<builder buildPath="${workspace_loc:/MQTTSN-embedded-C}/Debug" id="cdt.managedbuild.target.gnu.builder.exe.debug.767762182" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" superClass="cdt.managedbuild.target.gnu.builder.exe.debug"/>
 							<tool id="cdt.managedbuild.tool.gnu.archiver.base.602829827" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
 							<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug.109068141" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug">
-								<option id="gnu.cpp.compiler.exe.debug.option.optimization.level.1710260957" name="Optimization Level" superClass="gnu.cpp.compiler.exe.debug.option.optimization.level" useByScannerDiscovery="false" value="gnu.cpp.compiler.optimization.level.most" valueType="enumerated"/>
+								<option id="gnu.cpp.compiler.exe.debug.option.optimization.level.1710260957" name="Optimization Level" superClass="gnu.cpp.compiler.exe.debug.option.optimization.level" useByScannerDiscovery="false" value="gnu.cpp.compiler.optimization.level.none" valueType="enumerated"/>
 								<option id="gnu.cpp.compiler.exe.debug.option.debugging.level.1258567310" name="Debug Level" superClass="gnu.cpp.compiler.exe.debug.option.debugging.level" useByScannerDiscovery="false" value="gnu.cpp.compiler.debugging.level.max" valueType="enumerated"/>
 								<option id="gnu.cpp.compiler.option.dialect.std.1678347248" name="Language standard" superClass="gnu.cpp.compiler.option.dialect.std" useByScannerDiscovery="true" value="gnu.cpp.compiler.dialect.default" valueType="enumerated"/>
 								<option id="gnu.cpp.compiler.option.dialect.flags.1711182709" name="Other dialect flags" superClass="gnu.cpp.compiler.option.dialect.flags" useByScannerDiscovery="true" value="-std=c++11" valueType="string"/>
diff --git a/MQTTSNGateway/GatewayTester/samples/mainTest.cpp b/MQTTSNGateway/GatewayTester/samples/mainTest.cpp
index a5e001b..34db1c5 100644
--- a/MQTTSNGateway/GatewayTester/samples/mainTest.cpp
+++ b/MQTTSNGateway/GatewayTester/samples/mainTest.cpp
@@ -220,7 +220,7 @@ TASK_LIST = {// e.g. TASK( task, executing duration in second),
  *------------------------------------------------------*/
 void setup(void)
 {
-
+    //SetForwarderMode();
 }
 
 
diff --git a/MQTTSNGateway/GatewayTester/src/LGwProxy.cpp b/MQTTSNGateway/GatewayTester/src/LGwProxy.cpp
index 5ee7f99..78cc726 100644
--- a/MQTTSNGateway/GatewayTester/src/LGwProxy.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LGwProxy.cpp
@@ -1,5 +1,5 @@
 /**************************************************************************************
- * Copyright (c) 2016, Tomoaki Yamaguchi
+ * Copyright (c) 2016-2018, Tomoaki Yamaguchi
  *
  * All rights reserved. This program and the accompanying materials
  * are made available under the terms of the Eclipse Public License v1.0
@@ -29,44 +29,47 @@ extern void setUint16(uint8_t* pos, uint16_t val);
 extern uint16_t getUint16(const uint8_t* pos);
 extern LMqttsnClient* theClient;
 extern LScreen* theScreen;
+
+
 /*=====================================
         Class GwProxy
  ======================================*/
 static const char* packet_names[] =
 {
-	"ADVERTISE", "SEARCHGW", "GWINFO", "RESERVED", "CONNECT", "CONNACK",
-	"WILLTOPICREQ", "WILLTOPIC", "WILLMSGREQ", "WILLMSG", "REGISTER", "REGACK",
-	"PUBLISH", "PUBACK", "PUBCOMP", "PUBREC", "PUBREL", "RESERVED",
-	"SUBSCRIBE", "SUBACK", "UNSUBSCRIBE", "UNSUBACK", "PINGREQ", "PINGRESP",
-	"DISCONNECT", "RESERVED", "WILLTOPICUPD", "WILLTOPICRESP", "WILLMSGUPD",
-	"WILLMSGRESP"
+    "ADVERTISE", "SEARCHGW", "GWINFO", "RESERVED", "CONNECT", "CONNACK",
+    "WILLTOPICREQ", "WILLTOPIC", "WILLMSGREQ", "WILLMSG", "REGISTER", "REGACK",
+    "PUBLISH", "PUBACK", "PUBCOMP", "PUBREC", "PUBREL", "RESERVED",
+    "SUBSCRIBE", "SUBACK", "UNSUBSCRIBE", "UNSUBACK", "PINGREQ", "PINGRESP",
+    "DISCONNECT", "RESERVED", "WILLTOPICUPD", "WILLTOPICRESP", "WILLMSGUPD",
+    "WILLMSGRESP"
 };
 
 LGwProxy::LGwProxy(){
-	_nextMsgId = 0;
-	_status = GW_LOST;
+    _nextMsgId = 0;
+    _status = GW_LOST;
     _gwId = 0;
-	_willTopic = 0;
-	_willMsg = 0;
-	_qosWill = 0;
-	_retainWill = 0;
-	_tkeepAlive = MQTTSN_DEFAULT_KEEPALIVE;
-	_tAdv = MQTTSN_DEFAULT_DURATION;
-	_cleanSession = 0;
-	_pingStatus = 0;
-	_connectRetry = MQTTSN_RETRY_COUNT;
-	_tSleep = 0;
-	_tWake = 0;
-	_initialized = 0;
+    _willTopic = 0;
+    _willMsg = 0;
+    _qosWill = 0;
+    _retainWill = 0;
+    _tkeepAlive = MQTTSN_DEFAULT_KEEPALIVE;
+    _tAdv = MQTTSN_DEFAULT_DURATION;
+    _cleanSession = 0;
+    _pingStatus = 0;
+    _connectRetry = MQTTSN_RETRY_COUNT;
+    _tSleep = 0;
+    _tWake = 0;
+    _initialized = 0;
+    _isForwarderMode = false;
 }
 
 LGwProxy::~LGwProxy(){
-	_topicTbl.clearTopic();
+    _topicTbl.clearTopic();
 }
 
 void LGwProxy::initialize(LUdpConfig netconf, LMqttsnConfig mqconf){
-	_network.initialize(netconf);
-	_clientId = netconf.clientId;
+    _network.initialize(netconf);
+    _clientId = netconf.clientId;
     _willTopic = mqconf.willTopic;
     _willMsg = mqconf.willMsg;
     _qosWill = mqconf.willQos;
@@ -77,220 +80,220 @@ void LGwProxy::initialize(LUdpConfig netconf, LMqttsnConfig mqconf){
 }
 
 void LGwProxy::connect(){
-	char* pos;
+    char* pos;
 
-	while (_status != GW_CONNECTED){
-		pos = _msg;
+    while (_status != GW_CONNECTED){
+        pos = _msg;
 
-		if (_status == GW_SEND_WILLMSG){
-			*pos++ = 2 + (uint8_t)strlen(_willMsg);
-			*pos++ = MQTTSN_TYPE_WILLMSG;
-			strcpy(pos,_willMsg);          // WILLMSG
-			_status = GW_WAIT_CONNACK;
-			writeGwMsg();
-		}else if (_status == GW_SEND_WILLTOPIC){
-			*pos++ = 3 + (uint8_t)strlen(_willTopic);
-			*pos++ = MQTTSN_TYPE_WILLTOPIC;
-			*pos++ = _qosWill | _retainWill;
-			strcpy(pos,_willTopic);        // WILLTOPIC
-			_status = GW_WAIT_WILLMSGREQ;
-			writeGwMsg();
-		}else if (_status == GW_CONNECTING || _status == GW_DISCONNECTED || _status == GW_SLEPT ){
-			uint8_t clientIdLen = uint8_t(strlen(_clientId) > 23 ? 23 : strlen(_clientId));
-			*pos++ = 6 + clientIdLen;
-			*pos++ = MQTTSN_TYPE_CONNECT;
-			pos++;
-			if (_cleanSession){
-				_msg[2] = MQTTSN_FLAG_CLEAN;
-			}
-			*pos++ = MQTTSN_PROTOCOL_ID;
-			setUint16((uint8_t*)pos, _tkeepAlive);
-			pos += 2;
-			strncpy(pos, _clientId, clientIdLen);
-			_msg[ 6 + clientIdLen] = 0;
-			_status = GW_WAIT_CONNACK;
-			if ( _willMsg && _willTopic &&  _status != GW_SLEPT ){
-				if (strlen(_willMsg) && strlen(_willTopic)){
-					_msg[2] = _msg[2] | MQTTSN_FLAG_WILL;   // CONNECT
-					_status = GW_WAIT_WILLTOPICREQ;
-				}
-			}
-			writeGwMsg();
-			_connectRetry = MQTTSN_RETRY_COUNT;
-		}else if (_status == GW_LOST){
+        if (_status == GW_SEND_WILLMSG){
+            *pos++ = 2 + (uint8_t)strlen(_willMsg);
+            *pos++ = MQTTSN_TYPE_WILLMSG;
+            strcpy(pos,_willMsg);          // WILLMSG
+            _status = GW_WAIT_CONNACK;
+            writeGwMsg();
+        }else if (_status == GW_SEND_WILLTOPIC){
+            *pos++ = 3 + (uint8_t)strlen(_willTopic);
+            *pos++ = MQTTSN_TYPE_WILLTOPIC;
+            *pos++ = _qosWill | _retainWill;
+            strcpy(pos,_willTopic);        // WILLTOPIC
+            _status = GW_WAIT_WILLMSGREQ;
+            writeGwMsg();
+        }else if (_status == GW_CONNECTING || _status == GW_DISCONNECTED || _status == GW_SLEPT ){
+            uint8_t clientIdLen = uint8_t(strlen(_clientId) > 23 ? 23 : strlen(_clientId));
+            *pos++ = 6 + clientIdLen;
+            *pos++ = MQTTSN_TYPE_CONNECT;
+            pos++;
+            if (_cleanSession){
+                _msg[2] = MQTTSN_FLAG_CLEAN;
+            }
+            *pos++ = MQTTSN_PROTOCOL_ID;
+            setUint16((uint8_t*)pos, _tkeepAlive);
+            pos += 2;
+            strncpy(pos, _clientId, clientIdLen);
+            _msg[ 6 + clientIdLen] = 0;
+            _status = GW_WAIT_CONNACK;
+            if ( _willMsg && _willTopic &&  _status != GW_SLEPT ){
+                if (strlen(_willMsg) && strlen(_willTopic)){
+                    _msg[2] = _msg[2] | MQTTSN_FLAG_WILL;   // CONNECT
+                    _status = GW_WAIT_WILLTOPICREQ;
+                }
+            }
+            writeGwMsg();
+            _connectRetry = MQTTSN_RETRY_COUNT;
+        }else if (_status == GW_LOST){
 
-			*pos++ = 3;
-			*pos++ = MQTTSN_TYPE_SEARCHGW;
-			*pos = 0;                        // SERCHGW
-			_status = GW_SEARCHING;
-			writeGwMsg();
+            *pos++ = 3;
+            *pos++ = MQTTSN_TYPE_SEARCHGW;
+            *pos = 0;                        // SERCHGW
+            _status = GW_SEARCHING;
+            writeGwMsg();
 
-		}
-		getConnectResponce();
-	}
-	return;
+        }
+        getConnectResponce();
+    }
+    return;
 }
 
 int LGwProxy::getConnectResponce(void){
-	int len = readMsg();
+    int len = readMsg();
 
-	if (len == 0){
-		if (_sendUTC + MQTTSN_TIME_RETRY < time(NULL)){
-			if (_msg[1] == MQTTSN_TYPE_CONNECT)
-			{
-				_connectRetry--;
-			}
-			if (--_retryCount > 0){
-				writeMsg((const uint8_t*)_msg);  // Not writeGwMsg() : not to reset the counter.
-				_sendUTC = time(NULL);
-			}else{
-				_sendUTC = 0;
-				if ( _status > GW_SEARCHING && _connectRetry > 0){
-					_status = GW_CONNECTING;
-				}else{
-					_status = GW_LOST;
-					_gwId = 0;
-				}
-				return -1;
-			}
-		}
-		return 0;
-	}else if (_mqttsnMsg[0] == MQTTSN_TYPE_GWINFO && _status == GW_SEARCHING){
-		_network.setGwAddress();
+    if (len == 0){
+        if (_sendUTC + MQTTSN_TIME_RETRY < time(NULL)){
+            if (_msg[1] == MQTTSN_TYPE_CONNECT)
+            {
+                _connectRetry--;
+            }
+            if (--_retryCount > 0){
+                writeMsg((const uint8_t*)_msg);  // Not writeGwMsg() : not to reset the counter.
+                _sendUTC = time(NULL);
+            }else{
+                _sendUTC = 0;
+                if ( _status > GW_SEARCHING && _connectRetry > 0){
+                    _status = GW_CONNECTING;
+                }else{
+                    _status = GW_LOST;
+                    _gwId = 0;
+                }
+                return -1;
+            }
+        }
+        return 0;
+    }else if (_mqttsnMsg[0] == MQTTSN_TYPE_GWINFO && _status == GW_SEARCHING){
+        _network.setGwAddress();
         _gwId = _mqttsnMsg[1];
-		_status = GW_CONNECTING;
-	}else if (_mqttsnMsg[0] == MQTTSN_TYPE_WILLTOPICREQ && _status == GW_WAIT_WILLTOPICREQ){
-		_status = GW_SEND_WILLTOPIC;
-	}else if (_mqttsnMsg[0] == MQTTSN_TYPE_WILLMSGREQ && _status == GW_WAIT_WILLMSGREQ){
-		_status = GW_SEND_WILLMSG;
-	}else if (_mqttsnMsg[0] == MQTTSN_TYPE_CONNACK && _status == GW_WAIT_CONNACK){
-		if (_mqttsnMsg[1] == MQTTSN_RC_ACCEPTED){
-			_status = GW_CONNECTED;
-			_connectRetry = MQTTSN_RETRY_COUNT;
-			setPingReqTimer();
-			if ( _tSleep ){
-				_tSleep = 0;
-			}else{
-				DISPLAY("\033[0m\033[0;32m\n\n Connected to the Broker\033[0m\033[0;37m\n\n");
+        _status = GW_CONNECTING;
+    }else if (_mqttsnMsg[0] == MQTTSN_TYPE_WILLTOPICREQ && _status == GW_WAIT_WILLTOPICREQ){
+        _status = GW_SEND_WILLTOPIC;
+    }else if (_mqttsnMsg[0] == MQTTSN_TYPE_WILLMSGREQ && _status == GW_WAIT_WILLMSGREQ){
+        _status = GW_SEND_WILLMSG;
+    }else if (_mqttsnMsg[0] == MQTTSN_TYPE_CONNACK && _status == GW_WAIT_CONNACK){
+        if (_mqttsnMsg[1] == MQTTSN_RC_ACCEPTED){
+            _status = GW_CONNECTED;
+            _connectRetry = MQTTSN_RETRY_COUNT;
+            setPingReqTimer();
+            if ( _tSleep ){
+                _tSleep = 0;
+            }else{
+                DISPLAY("\033[0m\033[0;32m\n\n Connected to the Broker\033[0m\033[0;37m\n\n");
 
-				if ( _cleanSession || _initialized == 1 )
-				{
-	                _topicTbl.clearTopic();
-				    _initialized = 0;
-				    theClient->onConnect();  // SUBSCRIBEs are conducted
-				}
-			}
-		}else{
-			_status = GW_CONNECTING;
-		}
-	}
-	return 1;
+                if ( _cleanSession || _initialized == 1 )
+                {
+                    _topicTbl.clearTopic();
+                    _initialized = 0;
+                    theClient->onConnect();  // SUBSCRIBEs are conducted
+                }
+            }
+        }else{
+            _status = GW_CONNECTING;
+        }
+    }
+    return 1;
 }
 
 void LGwProxy::reconnect(void){
-	D_MQTTLOG("...Gateway reconnect\r\n");
-	_status = GW_DISCONNECTED;
-	connect();
+    D_MQTTLOG("...Gateway reconnect\r\n");
+    _status = GW_DISCONNECTED;
+    connect();
 }
 
 void LGwProxy::disconnect(uint16_t secs){
     _tSleep = secs;
     _tWake = 0;
 
-	_msg[1] = MQTTSN_TYPE_DISCONNECT;
+    _msg[1] = MQTTSN_TYPE_DISCONNECT;
 
-	if (secs){
-		_msg[0] = 4;
-		setUint16((uint8_t*) _msg + 2, secs);
-		_status = GW_SLEEPING;
-	}else{
-		_msg[0] = 2;
-		_keepAliveTimer.stop();
-		_status = GW_DISCONNECTING;
-	}
+    if (secs){
+        _msg[0] = 4;
+        setUint16((uint8_t*) _msg + 2, secs);
+        _status = GW_SLEEPING;
+    }else{
+        _msg[0] = 2;
+        _keepAliveTimer.stop();
+        _status = GW_DISCONNECTING;
+    }
 
-	_retryCount = MQTTSN_RETRY_COUNT;
-	writeMsg((const uint8_t*)_msg);
-	_sendUTC = time(NULL);
+    _retryCount = MQTTSN_RETRY_COUNT;
+    writeMsg((const uint8_t*)_msg);
+    _sendUTC = time(NULL);
 
-	while ( _status != GW_DISCONNECTED && _status != GW_SLEPT){
-		if (getDisconnectResponce() < 0){
-			_status = GW_LOST;
-			DISPLAY("\033[0m\033[0;31m\n\n!!!!!! DISCONNECT  Error !!!!!\033[0m\033[0;37m \n\n");
-			return;
-		}
-	}
+    while ( _status != GW_DISCONNECTED && _status != GW_SLEPT){
+        if (getDisconnectResponce() < 0){
+            _status = GW_LOST;
+            DISPLAY("\033[0m\033[0;31m\n\n!!!!!! DISCONNECT  Error !!!!!\033[0m\033[0;37m \n\n");
+            return;
+        }
+    }
 }
 
 int LGwProxy::getDisconnectResponce(void){
-	int len = readMsg();
+    int len = readMsg();
 
-	if (len == 0){
-		if (_sendUTC + MQTTSN_TIME_RETRY < time(NULL)){
-			if (--_retryCount >= 0){
-				writeMsg((const uint8_t*)_msg);
-				_sendUTC = time(NULL);
-			}else{
-				_status = GW_LOST;
-				_gwId = 0;
-				return -1;
-			}
-		}
-		return 0;
-	}else if (_mqttsnMsg[0] == MQTTSN_TYPE_DISCONNECT){
-		if (_status == GW_SLEEPING ){
-			_status = GW_SLEPT;
-			uint32_t remain = _keepAliveTimer.getRemain();
-			theClient->setSleepMode(remain);
+    if (len == 0){
+        if (_sendUTC + MQTTSN_TIME_RETRY < time(NULL)){
+            if (--_retryCount >= 0){
+                writeMsg((const uint8_t*)_msg);
+                _sendUTC = time(NULL);
+            }else{
+                _status = GW_LOST;
+                _gwId = 0;
+                return -1;
+            }
+        }
+        return 0;
+    }else if (_mqttsnMsg[0] == MQTTSN_TYPE_DISCONNECT){
+        if (_status == GW_SLEEPING ){
+            _status = GW_SLEPT;
+            uint32_t remain = _keepAliveTimer.getRemain();
+            theClient->setSleepMode(remain);
 
-			/* Wake up and starts from this point. */
+            /* Wake up and starts from this point. */
 
-		}else{
-			_status = GW_DISCONNECTED;
-		}
-	}
-	return 0;
+        }else{
+            _status = GW_DISCONNECTED;
+        }
+    }
+    return 0;
 }
 
 int LGwProxy::getMessage(void){
-	int len = readMsg();
-	if (len < 0){
-		return len;   //error
-	}
+    int len = readMsg();
+    if (len < 0){
+        return len;   //error
+    }
 #ifdef DEBUG_MQTTSN
-	if (len){
-		D_MQTTLOG(" recved msgType %x\n", _mqttsnMsg[0]);
-	}
+    if (len){
+        D_MQTTLOG(" recved msgType %x\n", _mqttsnMsg[0]);
+    }
 #endif
 
-	if (len == 0){
-		// Check PINGREQ required
-		checkPingReq();
+    if (len == 0){
+        // Check PINGREQ required
+        checkPingReq();
 
-		// Check ADVERTISE valid
-		checkAdvertise();
+        // Check ADVERTISE valid
+        checkAdvertise();
 
-		// Check Timeout of REGISTERs
-		_regMgr.checkTimeout();
+        // Check Timeout of REGISTERs
+        _regMgr.checkTimeout();
 
-		// Check Timeout of PUBLISHes,
-		theClient->getPublishManager()->checkTimeout();
+        // Check Timeout of PUBLISHes,
+        theClient->getPublishManager()->checkTimeout();
 
-		// Check Timeout of SUBSCRIBEs,
+        // Check Timeout of SUBSCRIBEs,
         theClient->getSubscribeManager()->checkTimeout();
 
-	}else if (_mqttsnMsg[0] == MQTTSN_TYPE_PUBLISH){
+    }else if (_mqttsnMsg[0] == MQTTSN_TYPE_PUBLISH){
         theClient->getPublishManager()->published(_mqttsnMsg, len);
 
     }else if (_mqttsnMsg[0] == MQTTSN_TYPE_PUBACK || _mqttsnMsg[0] == MQTTSN_TYPE_PUBCOMP ||
-			_mqttsnMsg[0] == MQTTSN_TYPE_PUBREC || _mqttsnMsg[0] == MQTTSN_TYPE_PUBREL ){
+            _mqttsnMsg[0] == MQTTSN_TYPE_PUBREC || _mqttsnMsg[0] == MQTTSN_TYPE_PUBREL ){
         theClient->getPublishManager()->responce(_mqttsnMsg, (uint16_t)len);
 
     }else if (_mqttsnMsg[0] == MQTTSN_TYPE_SUBACK || _mqttsnMsg[0] == MQTTSN_TYPE_UNSUBACK){
         theClient->getSubscribeManager()->responce(_mqttsnMsg);
 
     }else if (_mqttsnMsg[0] == MQTTSN_TYPE_REGISTER){
-    	_regMgr.responceRegister(_mqttsnMsg, len);
+        _regMgr.responceRegister(_mqttsnMsg, len);
 
     }else if (_mqttsnMsg[0] == MQTTSN_TYPE_REGACK){
         _regMgr.responceRegAck(getUint16(_mqttsnMsg + 3), getUint16(_mqttsnMsg + 1));
@@ -301,26 +304,26 @@ int LGwProxy::getMessage(void){
             setPingReqTimer();
 
             if ( _tSleep > 0 ){
-            	_tWake += _tkeepAlive;
-            	if ( _tWake < _tSleep ){
-            		theClient->setSleepMode(_tkeepAlive * 1000UL);
-            	}else{
-            		DISPLAY("\033[0m\033[0;32m\n\n Get back to ACTIVE.\033[0m\033[0;37m\n\n");
-            		_tWake = 0;
-            		connect();
-            	}
+                _tWake += _tkeepAlive;
+                if ( _tWake < _tSleep ){
+                    theClient->setSleepMode(_tkeepAlive * 1000UL);
+                }else{
+                    DISPLAY("\033[0m\033[0;32m\n\n Get back to ACTIVE.\033[0m\033[0;37m\n\n");
+                    _tWake = 0;
+                    connect();
+                }
             }
         }
     }else if (_mqttsnMsg[0] == MQTTSN_TYPE_DISCONNECT){
-    	_status = GW_LOST;
-    	_gwAliveTimer.stop();
-    	_keepAliveTimer.stop();
+        _status = GW_LOST;
+        _gwAliveTimer.stop();
+        _keepAliveTimer.stop();
     }else if (_mqttsnMsg[0] == MQTTSN_TYPE_ADVERTISE){
-    	if (getUint16((const uint8_t*)(_mqttsnMsg + 2)) < 61){
-    		_tAdv = getUint16((const uint8_t*)(_mqttsnMsg + 2)) * 1500;
-    	}else{
-    		_tAdv = getUint16((const uint8_t*)(_mqttsnMsg + 2)) * 1100;
-    	}
+        if (getUint16((const uint8_t*)(_mqttsnMsg + 2)) < 61){
+            _tAdv = getUint16((const uint8_t*)(_mqttsnMsg + 2)) * 1500;
+        }else{
+            _tAdv = getUint16((const uint8_t*)(_mqttsnMsg + 2)) * 1100;
+        }
         _gwAliveTimer.start(_tAdv);
     }
     return 0;
@@ -330,165 +333,203 @@ int LGwProxy::getMessage(void){
 
 
 uint16_t LGwProxy::registerTopic(char* topicName, uint16_t topicId){
-	uint16_t id = topicId;
+    uint16_t id = topicId;
     if (id == 0){
         id = _topicTbl.getTopicId(topicName);
-		_regMgr.registerTopic(topicName);
+        _regMgr.registerTopic(topicName);
     }
-	return id;
+    return id;
 }
 
 
 int LGwProxy::writeMsg(const uint8_t* msg){
-	uint16_t len;
-	uint8_t pos;
-	uint8_t  rc;
+    uint16_t len;
+    uint8_t pos;
+    uint8_t  rc = 0;
 
-	if (msg[0] == 0x01){
-		len = getUint16(msg + 1);
-		pos = 2;
-	}else{
-		len = msg[0];
-		pos = 1;
-	}
+    if (msg[0] == 0x01){
+        len = getUint16(msg + 1);
+        pos = 2;
+    }else{
+        len = msg[0];
+        pos = 1;
+    }
 
-	if (msg[0] == 3 && msg[1] == MQTTSN_TYPE_SEARCHGW){
-		rc = _network.broadcast(msg,len);
-	}else{
-		rc = _network.unicast(msg,len);
-	}
+    if (msg[0] == 3 && msg[1] == MQTTSN_TYPE_SEARCHGW){
+        rc = _network.broadcast(msg,len);
+    }else
+    {
+        if ( _isForwarderMode )
+        {
+            // create a forwarder encapsulation message  WirelessNodeId is a 4bytes fake data
+            uint8_t* buf = (uint8_t*)malloc(len + 7);
+            buf[0] = 7;
+            buf[1] = MQTTSN_TYPE_ENCAPSULATED;
+            buf[2] = 1;
+            buf[3] = 'w';
+            buf[4] = 'n';
+            buf[5] = 'I';
+            buf[6] = 'd';
+            memcpy(buf + 7, msg, len);
+            if ( buf)
+            rc = _network.unicast(buf, len + 7);
+            free(buf);
+            DISPLAY("  Encapsulated\n ");
+        }else{
+            rc = _network.unicast(msg,len);
+        }
 
-	if (rc > 0){
-		if ( msg[pos] >= MQTTSN_TYPE_ADVERTISE && msg[pos] <= MQTTSN_TYPE_WILLMSGRESP )
-		{
-			DISPLAY("  send %s\n", packet_names[msg[pos]]);
-		}
-		return rc;
-	}
-	//_status = GW_LOST;
-	//_gwId = 0;
-	return rc;
+        if (rc > 0){
+            if ( msg[pos] >= MQTTSN_TYPE_ADVERTISE && msg[pos] <= MQTTSN_TYPE_WILLMSGRESP )
+            {
+                DISPLAY("  send %s\n", packet_names[msg[pos]]);
+            }
+        }
+    }
+    return rc;
 }
 
 void LGwProxy::writeGwMsg(void){
-	_retryCount = MQTTSN_RETRY_COUNT;
-	writeMsg((const uint8_t*)_msg);
-	_sendUTC = time(NULL);
+    _retryCount = MQTTSN_RETRY_COUNT;
+    writeMsg((const uint8_t*)_msg);
+    _sendUTC = time(NULL);
 }
 
 int LGwProxy::readMsg(void){
-	int len = 0;
-	_mqttsnMsg = _network.getMessage(&len);
-	if (len == 0){
-		return 0;
-	}
+    int len = 0;
+    uint8_t* msg = _network.getMessage(&len);
+    _mqttsnMsg = msg;
 
-	if (_mqttsnMsg[0] == 0x01){
-		int msgLen = (int) getUint16((const uint8_t*)_mqttsnMsg + 1);
-		if (len != msgLen){
-			_mqttsnMsg += 3;
-			len = msgLen - 3;
-		}
-	}else{
-		_mqttsnMsg += 1;
-		len -= 1;
-	}
-	if ( *_mqttsnMsg >= MQTTSN_TYPE_ADVERTISE && *_mqttsnMsg <= MQTTSN_TYPE_WILLMSGRESP )
-	{
-		DISPLAY("  recv %s\n", packet_names[*_mqttsnMsg]);
-	}
-	return len;
+    if (len == 0){
+        return 0;
+    }
+
+    if (_mqttsnMsg[0] == 0x01){
+        int msgLen = (int) getUint16((const uint8_t*)_mqttsnMsg + 1);
+        if (len != msgLen){
+            _mqttsnMsg += 3;
+            len = msgLen - 3;
+        }
+    }else{
+        _mqttsnMsg += 1;
+        len -= 1;
+    }
+
+    if ( *_mqttsnMsg == MQTTSN_TYPE_ENCAPSULATED )
+    {
+        int lenEncap = len + 1;
+
+        if (msg[lenEncap] == 0x01){
+            int msgLen = (int) getUint16((const uint8_t*)(msg + lenEncap + 1));
+            msg += (lenEncap + 3);
+            len = msgLen - 3;
+        }else{
+            msg += (lenEncap + 1);
+            len = *(msg - 1);
+        }
+        _mqttsnMsg = msg;
+        DISPLAY("  recv encapslated message\n" );
+    }
+
+    if ( *_mqttsnMsg >= MQTTSN_TYPE_ADVERTISE && *_mqttsnMsg <= MQTTSN_TYPE_WILLMSGRESP )
+    {
+        DISPLAY("  recv %s\n", packet_names[*_mqttsnMsg]);
+    }
+    return len;
 }
 
 void LGwProxy::setWillTopic(const char* willTopic, uint8_t qos, bool retain){
-	_willTopic = willTopic;
-	_retainWill = _qosWill = 0;
-	if (qos == 1){
-		_qosWill = MQTTSN_FLAG_QOS_1;
-	}else if (qos == 2){
-		_qosWill = MQTTSN_FLAG_QOS_2;
-	}
-	if (retain){
-		_retainWill = MQTTSN_FLAG_RETAIN;
-	}
+    _willTopic = willTopic;
+    _retainWill = _qosWill = 0;
+    if (qos == 1){
+        _qosWill = MQTTSN_FLAG_QOS_1;
+    }else if (qos == 2){
+        _qosWill = MQTTSN_FLAG_QOS_2;
+    }
+    if (retain){
+        _retainWill = MQTTSN_FLAG_RETAIN;
+    }
 }
 void LGwProxy::setWillMsg(const char* willMsg){
-	_willMsg = willMsg;
+    _willMsg = willMsg;
 }
 
 
 void LGwProxy::setCleanSession(bool flg){
-	if (flg){
-		_cleanSession = MQTTSN_FLAG_CLEAN;
-	}else{
-		_cleanSession = 0;
-	}
+    if (flg){
+        _cleanSession = MQTTSN_FLAG_CLEAN;
+    }else{
+        _cleanSession = 0;
+    }
 }
 
 uint16_t LGwProxy::getNextMsgId(void){
-	_nextMsgId++;
+    _nextMsgId++;
     if (_nextMsgId == 0){
-    	_nextMsgId = 1;
+        _nextMsgId = 1;
     }
     return _nextMsgId;
 }
 
 void LGwProxy::checkPingReq(void){
     uint8_t msg[2];
-	msg[0] = 0x02;
-	msg[1] = MQTTSN_TYPE_PINGREQ;
+    msg[0] = 0x02;
+    msg[1] = MQTTSN_TYPE_PINGREQ;
     
-	if ( (_status == GW_CONNECTED || _status == GW_SLEPT) && isPingReqRequired() && _pingStatus != GW_WAIT_PINGRESP){
-		_pingStatus = GW_WAIT_PINGRESP;
+    if ( (_status == GW_CONNECTED || _status == GW_SLEPT) && isPingReqRequired() && _pingStatus != GW_WAIT_PINGRESP){
+        _pingStatus = GW_WAIT_PINGRESP;
         _pingRetryCount = MQTTSN_RETRY_COUNT;
 
-		writeMsg((const uint8_t*)msg);
+        writeMsg((const uint8_t*)msg);
         _pingSendUTC = time(NULL);
-	}else if (_pingStatus == GW_WAIT_PINGRESP){
+    }else if (_pingStatus == GW_WAIT_PINGRESP){
         if (_pingSendUTC + MQTTSN_TIME_RETRY < time(NULL)){
-    		if (--_pingRetryCount > 0){
-				writeMsg((const uint8_t*)msg);
-				_pingSendUTC = time(NULL);
-			}else{
-				_status = GW_LOST;
-				_gwId = 0;
+            if (--_pingRetryCount > 0){
+                writeMsg((const uint8_t*)msg);
+                _pingSendUTC = time(NULL);
+            }else{
+                _status = GW_LOST;
+                _gwId = 0;
                 _pingStatus = 0;
                 _keepAliveTimer.stop();
                 D_MQTTLOG("   !!! PINGREQ Timeout\n");
-			}
-		}
-	}
+            }
+        }
+    }
 }
 
 void LGwProxy::checkAdvertise(void){
-	if ( _gwAliveTimer.isTimeUp()){
-		_status = GW_LOST;
-		_gwId = 0;
-		_pingStatus = 0;
-		_gwAliveTimer.stop();
-		_keepAliveTimer.stop();
-		D_MQTTLOG("   !!! ADVERTISE Timeout\n");
-	}
+    if ( _gwAliveTimer.isTimeUp()){
+        _status = GW_LOST;
+        _gwId = 0;
+        _pingStatus = 0;
+        _gwAliveTimer.stop();
+        _keepAliveTimer.stop();
+        D_MQTTLOG("   !!! ADVERTISE Timeout\n");
+    }
 }
 
 LTopicTable* LGwProxy::getTopicTable(void){
-	return &_topicTbl;
+    return &_topicTbl;
 }
 
 LRegisterManager* LGwProxy::getRegisterManager(void){
-	return &_regMgr;
+    return &_regMgr;
 }
 
 bool LGwProxy::isPingReqRequired(void){
-	return _keepAliveTimer.isTimeUp(_tkeepAlive * 1000UL);
+    return _keepAliveTimer.isTimeUp(_tkeepAlive * 1000UL);
 }
 
 void LGwProxy::setPingReqTimer(void){
-	_keepAliveTimer.start(_tkeepAlive * 1000UL);
+    _keepAliveTimer.start(_tkeepAlive * 1000UL);
 }
 
 const char* LGwProxy::getClientId(void) {
-	return _clientId;
+    return _clientId;
 }
 
-
+void LGwProxy::setForwarderMode(void)
+{
+    _isForwarderMode = true;
+}
diff --git a/MQTTSNGateway/GatewayTester/src/LGwProxy.h b/MQTTSNGateway/GatewayTester/src/LGwProxy.h
index ac906e4..a34be1f 100644
--- a/MQTTSNGateway/GatewayTester/src/LGwProxy.h
+++ b/MQTTSNGateway/GatewayTester/src/LGwProxy.h
@@ -51,63 +51,65 @@ namespace linuxAsyncClient {
  =======================================*/
 class LGwProxy{
 public:
-	LGwProxy();
-	~LGwProxy();
+    LGwProxy();
+    ~LGwProxy();
 
-	void     initialize(LUdpConfig netconf, LMqttsnConfig mqconf);
-	void     connect(void);
-	void     disconnect(uint16_t sec = 0);
-	int      getMessage(void);
-	uint16_t registerTopic(char* topic, uint16_t toipcId);
+    void     initialize(LUdpConfig netconf, LMqttsnConfig mqconf);
+    void     connect(void);
+    void     disconnect(uint16_t sec = 0);
+    int      getMessage(void);
+    uint16_t registerTopic(char* topic, uint16_t toipcId);
 
-	void     setWillTopic(const char* willTopic, uint8_t qos, bool retain = false);
-	void     setWillMsg(const char* willMsg);
-	void     setCleanSession(bool);
-	void     setKeepAliveDuration(uint16_t duration);
-	void     setAdvertiseDuration(uint16_t duration);
-	void     reconnect(void);
-	int      writeMsg(const uint8_t* msg);
-	void     setPingReqTimer(void);
-	uint16_t getNextMsgId();
-	LTopicTable* getTopicTable(void);
-	LRegisterManager* getRegisterManager(void);
-	const char*    getClientId(void);
+    void     setWillTopic(const char* willTopic, uint8_t qos, bool retain = false);
+    void     setWillMsg(const char* willMsg);
+    void     setCleanSession(bool);
+    void     setKeepAliveDuration(uint16_t duration);
+    void     setAdvertiseDuration(uint16_t duration);
+    void     setForwarderMode(void);
+    void     reconnect(void);
+    int      writeMsg(const uint8_t* msg);
+    void     setPingReqTimer(void);
+    uint16_t getNextMsgId();
+    LTopicTable* getTopicTable(void);
+    LRegisterManager* getRegisterManager(void);
+    const char*    getClientId(void);
 private:
-	int      readMsg(void);
-	void     writeGwMsg(void);
-	void     checkPingReq(void);
-	void     checkAdvertise(void);
-	int      getConnectResponce(void);
-	int      getDisconnectResponce(void);
-	bool     isPingReqRequired(void);
+    int      readMsg(void);
+    void     writeGwMsg(void);
+    void     checkPingReq(void);
+    void     checkAdvertise(void);
+    int      getConnectResponce(void);
+    int      getDisconnectResponce(void);
+    bool     isPingReqRequired(void);
 
-	LNetwork     _network;
-	uint8_t*    _mqttsnMsg;
-	uint16_t    _nextMsgId;
-	const char* _clientId;
-	const char* _willTopic;
-	const char* _willMsg;
-	uint8_t     _cleanSession;
-	uint8_t    _initialized;
-	uint8_t     _retainWill;
-	uint8_t     _qosWill;
-	uint8_t     _gwId;
-	uint16_t    _tkeepAlive;
-	uint32_t    _tAdv;
-	time_t      _sendUTC;
-	int         _retryCount;
-	int         _connectRetry;
-	uint8_t     _status;
-	time_t    _pingSendUTC;
-	uint8_t     _pingRetryCount;
-	uint8_t     _pingStatus;
-	LRegisterManager _regMgr;
-	LTopicTable  _topicTbl;
-	LTimer       _gwAliveTimer;
-	LTimer       _keepAliveTimer;
-	uint16_t    _tSleep;
-	uint16_t    _tWake;
-	char        _msg[MQTTSN_MAX_MSG_LENGTH + 1];
+    LNetwork     _network;
+    uint8_t*    _mqttsnMsg;
+    uint16_t    _nextMsgId;
+    const char* _clientId;
+    const char* _willTopic;
+    const char* _willMsg;
+    uint8_t     _cleanSession;
+    uint8_t    _initialized;
+    uint8_t     _retainWill;
+    uint8_t     _qosWill;
+    uint8_t     _gwId;
+    uint16_t    _tkeepAlive;
+    uint32_t    _tAdv;
+    time_t      _sendUTC;
+    int         _retryCount;
+    int         _connectRetry;
+    uint8_t     _status;
+    time_t    _pingSendUTC;
+    uint8_t     _pingRetryCount;
+    uint8_t     _pingStatus;
+    LRegisterManager _regMgr;
+    LTopicTable  _topicTbl;
+    LTimer       _gwAliveTimer;
+    LTimer       _keepAliveTimer;
+    uint16_t    _tSleep;
+    uint16_t    _tWake;
+    bool _isForwarderMode;
+    char        _msg[MQTTSN_MAX_MSG_LENGTH + 1];
 };
 
 } /* end of namespace */
diff --git a/MQTTSNGateway/GatewayTester/src/LMqttsnClientApp.h b/MQTTSNGateway/GatewayTester/src/LMqttsnClientApp.h
index 2d41807..9891921 100644
--- a/MQTTSNGateway/GatewayTester/src/LMqttsnClientApp.h
+++ b/MQTTSNGateway/GatewayTester/src/LMqttsnClientApp.h
@@ -1,5 +1,5 @@
 /**************************************************************************************
- * Copyright (c) 2016, Tomoaki Yamaguchi
+ * Copyright (c) 2016-2018, Tomoaki Yamaguchi
  *
  * All rights reserved. This program and the accompanying materials
  * are made available under the terms of the Eclipse Public License v1.0
@@ -105,6 +105,7 @@ typedef enum
 #define END_OF_SUBSCRIBE_LIST {MQTTSN_TOPIC_TYPE_NORMAL,0,0,0, 0}
 #define UDPCONF  LUdpConfig theNetcon
 #define MQTTSNCONF LMqttsnConfig  theMqcon
+#define SetForwarderMode  theClient->getGwProxy()->setForwarderMode
 #ifdef CLIENT_MODE
 #define DISPLAY(...)
 #define PROMPT(...)
@@ -168,6 +169,7 @@ typedef enum
 #define MQTTSN_TYPE_WILLTOPICRESP 0x1B
 #define MQTTSN_TYPE_WILLMSGUPD    0x1C
 #define MQTTSN_TYPE_WILLMSGRESP   0x1D
+#define MQTTSN_TYPE_ENCAPSULATED  0xFE
 
 #define MQTTSN_TOPIC_TYPE           0x03
 
diff --git a/MQTTSNGateway/GatewayTester/src/LNetworkUdp.cpp b/MQTTSNGateway/GatewayTester/src/LNetworkUdp.cpp
index b61cf84..9471792 100644
--- a/MQTTSNGateway/GatewayTester/src/LNetworkUdp.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LNetworkUdp.cpp
@@ -76,12 +76,12 @@ uint8_t*  LNetwork::getMessage(int* len){
 			}else{
 				*len = _rxDataBuf[0];
 			}
-			if(recvLen != *len){
-				*len = 0;
-				return 0;
-			}else{
+			//if(recvLen != *len){
+			//	*len = 0;
+			//	return 0;
+			//}else{
 				return _rxDataBuf;
-			}
+			//}
 		}
 	}
 	return 0;
diff --git a/MQTTSNGateway/Makefile b/MQTTSNGateway/Makefile
index 9a66f0c..cfc9283 100644
--- a/MQTTSNGateway/Makefile
+++ b/MQTTSNGateway/Makefile
@@ -10,6 +10,7 @@ TESTAPPL := mainTestProcess
 CONFIG := gateway.conf
 CLIENTS := clients.conf
 PREDEFTOPIC := predefinedTopic.conf
+FORWARDERS := forwarders.conf
 
 SRCDIR := src
 SUBDIR := ../MQTTSNPacket/src
@@ -36,6 +37,8 @@ $(SRCDIR)/MQTTSNGWPacketHandleTask.cpp \
 $(SRCDIR)/MQTTSNGWProcess.cpp \
 $(SRCDIR)/MQTTSNGWPublishHandler.cpp \
 $(SRCDIR)/MQTTSNGWSubscribeHandler.cpp \
+$(SRCDIR)/MQTTSNGWEncapsulatedPacket.cpp \
+$(SRCDIR)/MQTTSNGWForwarder.cpp \
 $(SRCDIR)/$(OS)/$(SENSORNET)/SensorNetwork.cpp \
 $(SRCDIR)/$(OS)/Timer.cpp  \
 $(SRCDIR)/$(OS)/Network.cpp \
@@ -137,6 +140,8 @@ install:
 	cp -pf $(CONFIG) ../../
 	cp -pf $(CLIENTS) ../../
 	cp -pf $(PREDEFTOPIC) ../../
+	cp -pf $(FORWARDERS) ../../
+	
 
 exectest:
 	./$(OUTDIR)/$(TESTPROGNAME) -f ./gateway.conf
diff --git a/MQTTSNGateway/README.md b/MQTTSNGateway/README.md
index 4a0b5e5..28e149c 100644
--- a/MQTTSNGateway/README.md
+++ b/MQTTSNGateway/README.md
@@ -31,10 +31,13 @@ BrokerPortNo=1883
 BrokerSecurePortNo=8883    
 
 ClientAuthentication=NO
-#ClientsList=/path/to/your_clients.conf    
+ClientsList=/path/to/your_clients.conf    
 
 PredefinedTopic=NO
-#PredefinedTopicFile=/path/to/your_predefinedTopic.conf
+PredefinedTopicList=/path/to/your_predefinedTopic.conf
+
+Forwarder=NO
+ForwardersList=/home/tomoaki/tmp/forwarders.conf
     
 #RootCAfile=/path/to/your_Root_CA.crt    
 #RootCApath=/path/to/your_certs_directory/   
@@ -64,7 +67,8 @@ Client should know the MulticastIP and MulticastPortNo to send a SEARCHGW messag
 **GatewayId** is used by GWINFO message.    
 **KeepAlive** is a duration of ADVERTISE message in seconds.    
 when **ClientAuthentication** is YES, see MQTTSNGWClient.cpp line53, clients file specified by ClientsList is required. This file defines connect allowed clients by ClientId and SensorNetwork Address. e.g. IP address and Port No.   
-When **PredefinedTopic** is YES, Pre-definedTopicID file specified by PredefinedTopicFile is effective. This file defines Pre-definedTopics of the clients. In this file, ClientID,TopicName and TopicID are declared in CSV format.
+When **PredefinedTopic** is YES, Pre-definedTopicID file specified by PredefinedTopicList is effective. This file defines Pre-definedTopics of the clients. In this file, ClientID,TopicName and TopicID are declared in CSV format.    
+When **Forwarder** is YES, Forwarder Encapsulation Message is available. Connectable Forwarders are specifed by ForwardersList file. In this file, ForwarderIds and those sensorNet addresses are declared in CSV format.    
  
 
 
diff --git a/MQTTSNGateway/forwarders.conf b/MQTTSNGateway/forwarders.conf
new file mode 100644
index 0000000..3275924
--- /dev/null
+++ b/MQTTSNGateway/forwarders.conf
@@ -0,0 +1,16 @@
+#***********************************************************************
+# Copyright (c) 2018, Tomoaki Yamaguchi
+#
+# All rights reserved. This program and the accompanying materials
+# are made available under the terms of the Eclipse Public License v1.0
+# and Eclipse Distribution License v1.0 which accompany this distribution.
+#
+# The Eclipse Public License is available at
+#    http://www.eclipse.org/legal/epl-v10.html
+# and the Eclipse Distribution License is available at
+#   http://www.eclipse.org/org/documents/edl-v10.php.
+#***********************************************************************
+#
+# SensorNetwork address format is defined by SensorNetAddress::setAddress(string* data) function.
+#
+Forwarder01,172.16.1.7:12002
diff --git a/MQTTSNGateway/gateway.conf b/MQTTSNGateway/gateway.conf
index d92a12b..10e02a4 100644
--- a/MQTTSNGateway/gateway.conf
+++ b/MQTTSNGateway/gateway.conf
@@ -1,5 +1,5 @@
 #**************************************************************************
-# Copyright (c) 2016, Tomoaki Yamaguchi
+# Copyright (c) 2016-2018, Tomoaki Yamaguchi
 #
 # All rights reserved. This program and the accompanying materials
 # are made available under the terms of the Eclipse Public License v1.0
@@ -18,10 +18,13 @@ BrokerPortNo=1883
 BrokerSecurePortNo=8883
 
 ClientAuthentication=NO
-#ClientsList=/path/to/your_clients.conf
+ClientsList=/path/to/your_clients.conf
 
 PredefinedTopic=NO
-#PredefinedTopicFile=/path/to/your_predefinedTopic.conf
+PredefinedTopicList=/path/to/your_predefinedTopic.conf
+
+Forwarder=NO
+ForwardersList=/path/to/your_forwarers.conf
 
 #RootCAfile=/etc/ssl/certs/ca-certificates.crt
 #RootCApath=/etc/ssl/certs/
diff --git a/MQTTSNGateway/src/MQTTSNGWClient.cpp b/MQTTSNGateway/src/MQTTSNGWClient.cpp
index 9aa2a18..0a81ef8 100644
--- a/MQTTSNGateway/src/MQTTSNGWClient.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWClient.cpp
@@ -19,6 +19,7 @@
 #include "MQTTSNGWClient.h"
 #include "MQTTSNGateway.h"
 #include "SensorNetwork.h"
+#include "MQTTSNGWForwarder.h"
 #include "Network.h"
 #include <string>
 #include <string.h>
@@ -75,7 +76,7 @@ bool ClientList::authorize(const char* fileName)
 {
 	FILE* fp;
 	char buf[MAX_CLIENTID_LENGTH + 256];
-	size_t pos;;
+	size_t pos;
 	bool secure;
 	bool stable;
 	SensorNetAddress netAddr;
@@ -167,6 +168,11 @@ bool ClientList::setPredefinedTopics(const char* fileName)
         fclose(fp);
         rc = true;
     }
+    else
+    {
+        WRITELOG("Can not open the Predefined Topic List.     %s\n", fileName);
+        return false;
+    }
     return rc;
 }
 
@@ -196,6 +202,11 @@ void ClientList::erase(Client*& client)
 			_endClient = prev;
 		}
 		_clientCnt--;
+		Forwarder* fwd = client->getForwarder();
+		if ( fwd )
+		{
+		    fwd->eraseClient(client);
+		}
 		delete client;
 		client = 0;
 		_mutex.unlock();
@@ -204,19 +215,22 @@ void ClientList::erase(Client*& client)
 
 Client* ClientList::getClient(SensorNetAddress* addr)
 {
-	_mutex.lock();
-	Client* client = _firstClient;
+    if ( addr )
+    {
+        _mutex.lock();
+        Client* client = _firstClient;
 
-	while (client != 0)
-	{
-		if (client->getSensorNetAddress()->isMatch(addr) )
-		{
-			_mutex.unlock();
-			return client;
-		}
-		client = client->_nextClient;
-	}
-	_mutex.unlock();
+        while (client != 0)
+        {
+            if (client->getSensorNetAddress()->isMatch(addr) )
+            {
+                _mutex.unlock();
+                return client;
+            }
+            client = client->_nextClient;
+        }
+        _mutex.unlock();
+    }
 	return 0;
 }
 
@@ -401,6 +415,7 @@ Client::Client(bool secure)
 	_clientSleepPacketQue.setMaxSize(MAX_SAVED_PUBLISH);
 	_hasPredefTopic = false;
 	_holdPingRequest = false;
+	_forwarder = 0;
 }
 
 Client::~Client()
@@ -519,6 +534,16 @@ void Client::setKeepAlive(MQTTSNPacket* packet)
 	}
 }
 
+void Client::setForwarder(Forwarder* forwarder)
+{
+    _forwarder = forwarder;
+}
+
+Forwarder* Client::getForwarder(void)
+{
+    return _forwarder;
+}
+
 void Client::setSessionStatus(bool status)
 {
 	_sessionStatus = status;
diff --git a/MQTTSNGateway/src/MQTTSNGWClient.h b/MQTTSNGateway/src/MQTTSNGWClient.h
index 118239e..8900656 100644
--- a/MQTTSNGateway/src/MQTTSNGWClient.h
+++ b/MQTTSNGateway/src/MQTTSNGWClient.h
@@ -26,6 +26,8 @@
 #include "Network.h"
 #include "SensorNetwork.h"
 #include "MQTTSNPacket.h"
+#include "MQTTSNGWEncapsulatedPacket.h"
+#include "MQTTSNGWForwarder.h"
 
 namespace MQTTSNGW
 {
@@ -230,13 +232,13 @@ private:
 /*=====================================
  Class Client
  =====================================*/
-#define MQTTSN_CLIENTID_LENGTH 23
 
 typedef enum
 {
     Cstat_Disconnected = 0, Cstat_TryConnecting, Cstat_Connecting, Cstat_Active, Cstat_Asleep, Cstat_Awake, Cstat_Lost
 } ClientStatus;
 
+class Forwarder;
 
 class Client
 {
@@ -281,6 +283,9 @@ public:
     void setClientAddress(SensorNetAddress* sensorNetAddr);
     void setSensorNetType(bool stable);
 
+    Forwarder* getForwarder(void);
+    void setForwarder(Forwarder* forwader);
+
     void setClientId(MQTTSNString id);
     void setWillTopic(MQTTSNString willTopic);
     void setWillMsg(MQTTSNString willmsg);
@@ -339,6 +344,9 @@ private:
     bool _sensorNetype;     // false: unstable network like a G3
     SensorNetAddress _sensorNetAddr;
 
+    Forwarder* _forwarder;
+
+
     bool _sessionStatus;
     bool _hasPredefTopic;
 
@@ -373,5 +381,7 @@ private:
     bool _authorize;
 };
 
+
+
 }
 #endif /* MQTTSNGWCLIENT_H_ */
diff --git a/MQTTSNGateway/src/MQTTSNGWClientRecvTask.cpp b/MQTTSNGateway/src/MQTTSNGWClientRecvTask.cpp
index 8da9453..9b74341 100644
--- a/MQTTSNGateway/src/MQTTSNGWClientRecvTask.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWClientRecvTask.cpp
@@ -16,7 +16,12 @@
 
 #include "MQTTSNGWClientRecvTask.h"
 #include "MQTTSNGateway.h"
-#include <string.h>
+#include "MQTTSNPacket.h"
+#include "MQTTSNGWForwarder.h"
+#include "MQTTSNGWEncapsulatedPacket.h"
+#include <cstring>
+
+using namespace MQTTSNGW;
 char* currentDateTime(void);
 /*=====================================
  Class ClientRecvTask
@@ -55,8 +60,12 @@ void ClientRecvTask::run()
 	Client* client = 0;
 	char buf[128];
 
+
 	while (true)
 	{
+	    Forwarder* fwd = 0;
+	    WirelessNodeId nodeId;
+
 		MQTTSNPacket* packet = new MQTTSNPacket();
 		int packetLen = packet->recv(_sensorNetwork);
 
@@ -89,8 +98,38 @@ void ClientRecvTask::run()
 			continue;
 		}
 
-		/* get client from the ClientList of Gateway by sensorNetAddress. */
-		client = _gateway->getClientList()->getClient(_sensorNetwork->getSenderAddress());
+		if ( packet->getType() == MQTTSN_ENCAPSULATED )
+		{
+		    fwd = _gateway->getForwarderList()->getForwarder(_sensorNetwork->getSenderAddress());
+
+		    if ( fwd  == 0 )
+		    {
+		        log(0, packet);
+		        WRITELOG("%s Forwarder  %s is not authorized.%s\n", ERRMSG_HEADER, _sensorNetwork->getSenderAddress()->sprint(buf), ERRMSG_FOOTER);
+		        delete packet;
+		        continue;
+		    }
+		    else
+		    {
+		        MQTTSNString fwdName;
+		        fwdName.lenstring.data = const_cast<char *>( fwd->getName() );
+		        fwdName.lenstring.len = strlen(fwdName.lenstring.data);
+	            log(0, packet, &fwdName);
+
+		        MQTTSNGWEncapsulatedPacket  encap;
+		        encap.desirialize(packet->getPacketData(), packet->getPacketLength());
+		        nodeId.setId( encap.getWirelessNodeId() );
+		        client = fwd->getClient(&nodeId);
+		        delete packet;
+		        packet = encap.getMQTTSNPacket();
+		    }
+		}
+		else
+		{
+            /* get client from the ClientList of Gateway by sensorNetAddress. */
+            client = _gateway->getClientList()->getClient(_sensorNetwork->getSenderAddress());
+		}
+
 
 		if ( client )
 		{
@@ -117,27 +156,40 @@ void ClientRecvTask::run()
 
 				client = _gateway->getClientList()->getClient(&data.clientID);
 
-				if ( client )
+				if ( fwd )
 				{
-				    /* set SensorNet Address */
-				    client->setClientAddress(_sensorNetwork->getSenderAddress());
+				    if ( client == 0 )
+				    {
+				        /* create a new client */
+				        client = _gateway->getClientList()->createClient(0, &data.clientID, false, false);
+				    }
+				    /* Add to af forwarded client list of forwarder. */
+                    fwd->addClient(client, &nodeId);
 				}
 				else
 				{
-				    /* create a client */
-				    client = _gateway->getClientList()->createClient(_sensorNetwork->getSenderAddress(), &data.clientID, false, false);
+                    if ( client )
+                    {
+                        /* Client exists. Set SensorNet Address of it. */
+                        client->setClientAddress(_sensorNetwork->getSenderAddress());
+                    }
+                    else
+                    {
+                        /* create a new client */
+                        client = _gateway->getClientList()->createClient(_sensorNetwork->getSenderAddress(), &data.clientID, false, false);
+                    }
 				}
 
 				log(client, packet, &data.clientID);
 
 				if (!client)
 				{
-					WRITELOG("%s Client(%s) was rejected. CONNECT message has been discarded.%s\n", ERRMSG_HEADER, _sensorNetwork->getSenderAddress()->sprint(buf), ERRMSG_FOOTER);
+	                WRITELOG("%s Client(%s) was rejected. CONNECT message has been discarded.%s\n", ERRMSG_HEADER, _sensorNetwork->getSenderAddress()->sprint(buf), ERRMSG_FOOTER);
 					delete packet;
 					continue;
 				}
 
-				/* set sensorNetAddress & post Event */
+				/* post Client RecvEvent */
 				ev = new Event();
 				ev->setClientRecvEvent(client, packet);
 				_gateway->getPacketEventQue()->post(ev);
@@ -145,16 +197,18 @@ void ClientRecvTask::run()
 			else
 			{
 				log(client, packet);
-				delete packet;
-				/* Send DISCONNECT */
-				SensorNetAddress* addr = new SensorNetAddress();
-				*addr = (*_sensorNetwork->getSenderAddress());
-				packet = new MQTTSNPacket();
-				packet->setDISCONNECT(0);
-				ev = new Event();
-				ev->setClientSendEvent(addr, packet);
-				_gateway->getClientSendQue()->post(ev);
-				continue;
+                WRITELOG("%s Client(%s) is not connecting. message has been discarded.%s\n", ERRMSG_HEADER, _sensorNetwork->getSenderAddress()->sprint(buf), ERRMSG_FOOTER);
+                delete packet;
+
+                /* Send DISCONNECT */
+                if ( fwd == 0 )
+                {
+                    packet = new MQTTSNPacket();
+                    packet->setDISCONNECT(0);
+                    ev = new Event();
+                    ev->setClientSendEvent(_sensorNetwork->getSenderAddress(), packet);
+                    _gateway->getClientSendQue()->post(ev);
+                }
 			}
 		}
 	}
@@ -211,6 +265,9 @@ void ClientRecvTask::log(Client* client, MQTTSNPacket* packet, MQTTSNString* id)
 	case MQTTSN_PUBCOMP:
 		WRITELOG(FORMAT_G_MSGID_G_G, currentDateTime(), packet->getName(), packet->getMsgId(msgId), LEFTARROW, clientId, packet->print(pbuf));
 		break;
+	case MQTTSN_ENCAPSULATED:
+	        WRITELOG(FORMAT_Y_G_G, currentDateTime(), packet->getName(), LEFTARROW, clientId, packet->print(pbuf));
+	        break;
 	default:
 		WRITELOG(FORMAT_W_NL, currentDateTime(), packet->getName(), LEFTARROW, clientId, packet->print(pbuf));
 		break;
diff --git a/MQTTSNGateway/src/MQTTSNGWClientSendTask.cpp b/MQTTSNGateway/src/MQTTSNGWClientSendTask.cpp
index 8118124..87de924 100644
--- a/MQTTSNGateway/src/MQTTSNGWClientSendTask.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWClientSendTask.cpp
@@ -17,6 +17,7 @@
 #include "MQTTSNGWPacket.h"
 #include "MQTTSNGWPacket.h"
 #include "MQTTSNGateway.h"
+#include "MQTTSNGWEncapsulatedPacket.h"
 
 using namespace MQTTSNGW;
 using namespace std;
@@ -56,8 +57,22 @@ void ClientSendTask::run()
 		{
 			client = ev->getClient();
 			packet = ev->getMQTTSNPacket();
-			log(client, packet);
-			rc = packet->unicast(_sensorNetwork, client->getSensorNetAddress());
+			Forwarder* fwd = client->getForwarder();
+
+			if ( fwd )
+			{
+			    MQTTSNGWEncapsulatedPacket encap(packet);
+			    WirelessNodeId* wnId = fwd->getWirelessNodeId(client);
+			    encap.setWirelessNodeId(wnId);
+			    log(fwd, &encap);
+			    log(client, packet);
+			    rc = encap.unicast(_sensorNetwork,fwd->getSensorNetAddr());
+			}
+			else
+			{
+                log(client, packet);
+                rc = packet->unicast(_sensorNetwork, client->getSensorNetAddress());
+			}
 		}
 		else if (ev->getEventType() == EtBroadcast)
 		{
@@ -119,3 +134,10 @@ void ClientSendTask::log(Client* client, MQTTSNPacket* packet)
 		break;
 	}
 }
+
+void ClientSendTask::log(Forwarder* forwarder, MQTTSNGWEncapsulatedPacket* packet)
+{
+    char pbuf[SIZE_OF_LOG_PACKET * 3];
+    const char* forwarderId = forwarder->getId();
+    WRITELOG(FORMAT_Y_W_G, currentDateTime(), packet->getName(), RIGHTARROW, forwarderId, packet->print(pbuf));
+}
diff --git a/MQTTSNGateway/src/MQTTSNGWClientSendTask.h b/MQTTSNGateway/src/MQTTSNGWClientSendTask.h
index d9e1646..b6cff2f 100644
--- a/MQTTSNGateway/src/MQTTSNGWClientSendTask.h
+++ b/MQTTSNGateway/src/MQTTSNGWClientSendTask.h
@@ -35,6 +35,7 @@ public:
 
 private:
 	void log(Client*, MQTTSNPacket*);
+    void log(Forwarder*, MQTTSNGWEncapsulatedPacket*);
 
 	Gateway* _gateway;
 	SensorNetwork* _sensorNetwork;
diff --git a/MQTTSNGateway/src/MQTTSNGWConnectionHandler.cpp b/MQTTSNGateway/src/MQTTSNGWConnectionHandler.cpp
index 721003b..06b8d43 100644
--- a/MQTTSNGateway/src/MQTTSNGWConnectionHandler.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWConnectionHandler.cpp
@@ -218,7 +218,6 @@ void MQTTSNConnectionHandler::handleWillmsg(Client* client, MQTTSNPacket* packet
 void MQTTSNConnectionHandler::handleDisconnect(Client* client, MQTTSNPacket* packet)
 {
     uint16_t duration = 0;
-    Event* ev = new Event();
 
     if ( packet->getDISCONNECT(&duration) != 0 )
     {
@@ -226,7 +225,7 @@ void MQTTSNConnectionHandler::handleDisconnect(Client* client, MQTTSNPacket* pac
         {
             MQTTGWPacket* mqMsg = new MQTTGWPacket();
             mqMsg->setHeader(DISCONNECT);
-            ev = new Event();
+            Event* ev = new Event();
             ev->setBrokerSendEvent(client, mqMsg);
             _gateway->getBrokerSendQue()->post(ev);
         }
@@ -270,8 +269,6 @@ void MQTTSNConnectionHandler::handleWillmsgupd(Client* client, MQTTSNPacket* pac
  */
 void MQTTSNConnectionHandler::handlePingreq(Client* client, MQTTSNPacket* packet)
 {
-	MQTTGWPacket* msg = 0;
-
 	if ( ( client->isSleep() || client->isAwake() ) &&  client->getClientSleepPacket() )
 	{
 	    sendStoredPublish(client);
diff --git a/MQTTSNGateway/src/MQTTSNGWDefines.h b/MQTTSNGateway/src/MQTTSNGWDefines.h
index db30862..8f7f837 100644
--- a/MQTTSNGateway/src/MQTTSNGWDefines.h
+++ b/MQTTSNGateway/src/MQTTSNGWDefines.h
@@ -26,6 +26,7 @@ namespace MQTTSNGW
 #define CONFIG_FILE      "gateway.conf"
 #define CLIENT_LIST      "clients.conf"
 #define PREDEFINEDTOPIC_FILE      "predefinedTopic.conf"
+#define FORWARDER_LIST     "forwarders.conf"
 
 /*==========================================================
  *    Gateway default parameters
@@ -41,7 +42,7 @@ namespace MQTTSNGW
 #define MAX_INFLIGHTMESSAGES         (10)  // Number of inflight messages
 #define MAX_SAVED_PUBLISH            (20)  // Max number of PUBLISH message for Asleep state
 #define MAX_TOPIC_PAR_CLIENT     (50)    // Max Topic count for a client. it should be less than 256
-#define MQTTSNGW_MAX_PACKET_SIZE   (1024)  // Max Packet size  (5+2+TopicLen+PayloadLen)
+#define MQTTSNGW_MAX_PACKET_SIZE   (1024)  // Max Packet size  (5+2+TopicLen+PayloadLen + Foward Encapsulation)
 #define SIZE_OF_LOG_PACKET          (500)  // Length of the packet log in bytes
 
 /*=================================
diff --git a/MQTTSNGateway/src/MQTTSNGWEncapsulatedPacket.cpp b/MQTTSNGateway/src/MQTTSNGWEncapsulatedPacket.cpp
new file mode 100644
index 0000000..1c06d13
--- /dev/null
+++ b/MQTTSNGateway/src/MQTTSNGWEncapsulatedPacket.cpp
@@ -0,0 +1,179 @@
+/**************************************************************************************
+ * Copyright (c) 2018, Tomoaki Yamaguchi
+ *
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * and Eclipse Distribution License v1.0 which accompany this distribution.
+ *
+ * The Eclipse Public License is available at
+ *    http://www.eclipse.org/legal/epl-v10.html
+ * and the Eclipse Distribution License is available at
+ *   http://www.eclipse.org/org/documents/edl-v10.php.
+ *
+ * Contributors:
+ *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
+ **************************************************************************************/
+#include "MQTTSNGWPacket.h"
+#include "MQTTSNGWEncapsulatedPacket.h"
+#include "MQTTSNPacket.h"
+#include <string.h>
+
+using namespace MQTTSNGW;
+using namespace std;
+
+WirelessNodeId::WirelessNodeId()
+    :
+    _len{0},
+    _nodeId{0}
+{
+
+}
+
+WirelessNodeId::~WirelessNodeId()
+{
+    if ( _nodeId )
+    {
+        free(_nodeId);
+    }
+}
+
+void WirelessNodeId::setId(uint8_t* id, uint8_t len)
+{
+    if ( _nodeId )
+     {
+         free(_nodeId);
+     }
+    uint8_t* buf = (uint8_t*)malloc(len);
+    if ( buf )
+    {
+        memcpy(buf, id, len);
+        _len = len;
+        _nodeId = buf;
+    }
+    else
+    {
+        _nodeId = 0;
+        _len = 0;
+    }
+}
+
+void WirelessNodeId::setId(WirelessNodeId* id)
+{
+    setId(id->_nodeId, id->_len);
+}
+
+bool WirelessNodeId::operator ==(WirelessNodeId& id)
+{
+    if ( _len == id._len )
+    {
+        return memcmp(_nodeId, id._nodeId, _len) == 0;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+/*
+ *    Class MQTTSNGWEncapsulatedPacket
+ */
+MQTTSNGWEncapsulatedPacket::MQTTSNGWEncapsulatedPacket()
+    :  _mqttsn{0},
+       _ctrl{0}
+{
+
+}
+
+MQTTSNGWEncapsulatedPacket::MQTTSNGWEncapsulatedPacket(MQTTSNPacket* packet)
+    :  _mqttsn{packet},
+       _ctrl{0}
+{
+
+}
+
+MQTTSNGWEncapsulatedPacket::~MQTTSNGWEncapsulatedPacket()
+{
+    /*  Do not delete the MQTTSNPacket.  MQTTSNPacket is deleted by delete Event */
+}
+
+int MQTTSNGWEncapsulatedPacket::unicast(SensorNetwork* network, SensorNetAddress* sendTo)
+{
+    uint8_t buf[MQTTSNGW_MAX_PACKET_SIZE];
+    int len = serialize(buf);
+    return network->unicast(buf, len, sendTo);
+}
+
+int MQTTSNGWEncapsulatedPacket::serialize(uint8_t* buf)
+{
+    int len = 0;
+    buf[0] = _id._len + 3;
+    buf[1] = MQTTSN_ENCAPSULATED;
+    buf[2] = _ctrl;
+    memcpy( buf + 3, _id._nodeId, _id._len);
+    if ( _mqttsn )
+    {
+        len = _mqttsn->getPacketLength();
+        memcpy(buf + buf[0], _mqttsn->getPacketData(), len);
+    }
+    return  buf[0] + len;
+}
+
+int MQTTSNGWEncapsulatedPacket::desirialize(unsigned char* buf, unsigned short len)
+{
+    if ( _mqttsn )
+    {
+        delete _mqttsn;
+        _mqttsn = 0;
+    }
+
+    _ctrl = buf[2];
+    _id.setId(buf + 3, buf[0] - 3);
+
+    _mqttsn = new MQTTSNPacket;
+    _mqttsn->desirialize(buf + buf[0], len - buf[0]);
+    return buf[0];
+}
+
+int MQTTSNGWEncapsulatedPacket::getType(void)
+{
+    return MQTTSN_ENCAPSULATED;
+}
+
+const char* MQTTSNGWEncapsulatedPacket::getName()
+{
+    return MQTTSNPacket_name(MQTTSN_ENCAPSULATED);
+}
+
+MQTTSNPacket* MQTTSNGWEncapsulatedPacket::getMQTTSNPacket(void)
+{
+    return _mqttsn;
+}
+
+WirelessNodeId* MQTTSNGWEncapsulatedPacket::getWirelessNodeId(void)
+{
+    return &_id;
+}
+
+void MQTTSNGWEncapsulatedPacket::setWirelessNodeId(WirelessNodeId* id)
+{
+    _id.setId(id);
+}
+
+char* MQTTSNGWEncapsulatedPacket::print(char* pbuf)
+{
+    char* ptr = pbuf;
+    char** pptr = &pbuf;
+
+    uint8_t buf[MQTTSNGW_MAX_PACKET_SIZE];
+    int len = serialize(buf);
+    int size = len > SIZE_OF_LOG_PACKET ? SIZE_OF_LOG_PACKET : len;
+
+    for (int i = 1; i < size; i++)
+    {
+        sprintf(*pptr, " %02X", *(buf + i));
+        *pptr += 3;
+    }
+    **pptr = 0;
+    return ptr;
+}
+
diff --git a/MQTTSNGateway/src/MQTTSNGWEncapsulatedPacket.h b/MQTTSNGateway/src/MQTTSNGWEncapsulatedPacket.h
new file mode 100644
index 0000000..eb2cba6
--- /dev/null
+++ b/MQTTSNGateway/src/MQTTSNGWEncapsulatedPacket.h
@@ -0,0 +1,65 @@
+/**************************************************************************************
+ * Copyright (c) 2018, Tomoaki Yamaguchi
+ *
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * and Eclipse Distribution License v1.0 which accompany this distribution.
+ *
+ * The Eclipse Public License is available at
+ *    http://www.eclipse.org/legal/epl-v10.html
+ * and the Eclipse Distribution License is available at
+ *   http://www.eclipse.org/org/documents/edl-v10.php.
+ *
+ * Contributors:
+ *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
+ **************************************************************************************/
+
+#ifndef MQTTSNGATEWAY_SRC_MQTTSNGWENCAPSULATEDPACKET_H_
+#define MQTTSNGATEWAY_SRC_MQTTSNGWENCAPSULATEDPACKET_H_
+
+namespace MQTTSNGW
+{
+
+class WirelessNodeId
+{
+    friend class MQTTSNGWEncapsulatedPacket;
+public:
+    WirelessNodeId();
+    ~WirelessNodeId();
+    void setId(uint8_t* id, uint8_t len);
+    void setId(WirelessNodeId* id);
+    bool operator ==(WirelessNodeId& id);
+private:
+    uint8_t _len;
+    uint8_t* _nodeId;
+};
+
+class MQTTSNGWEncapsulatedPacket
+{
+public:
+    MQTTSNGWEncapsulatedPacket();
+    MQTTSNGWEncapsulatedPacket(MQTTSNPacket* packet);
+    ~MQTTSNGWEncapsulatedPacket();
+    int unicast(SensorNetwork* network, SensorNetAddress* sendTo);
+    int serialize(uint8_t* buf);
+    int desirialize(unsigned char* buf, unsigned short len);
+    int getType(void);
+    unsigned char* getPacketData(void);
+    int getPacketLength(void);
+    const char* getName();
+    MQTTSNPacket* getMQTTSNPacket(void);
+    void setWirelessNodeId(WirelessNodeId* id);
+    WirelessNodeId* getWirelessNodeId(void);
+    char* print(char* buf);
+
+private:
+    MQTTSNPacket* _mqttsn;
+    WirelessNodeId _id;
+    uint8_t _ctrl;
+};
+
+}
+
+
+
+#endif /* MQTTSNGATEWAY_SRC_MQTTSNGWENCAPSULATEDPACKET_H_ */
diff --git a/MQTTSNGateway/src/MQTTSNGWForwarder.cpp b/MQTTSNGateway/src/MQTTSNGWForwarder.cpp
new file mode 100644
index 0000000..0bad175
--- /dev/null
+++ b/MQTTSNGateway/src/MQTTSNGWForwarder.cpp
@@ -0,0 +1,321 @@
+/**************************************************************************************
+ * Copyright (c) 2018, Tomoaki Yamaguchi
+ *
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * and Eclipse Distribution License v1.0 which accompany this distribution.
+ *
+ * The Eclipse Public License is available at
+ *    http://www.eclipse.org/legal/epl-v10.html
+ * and the Eclipse Distribution License is available at
+ *   http://www.eclipse.org/org/documents/edl-v10.php.
+ *
+ * Contributors:
+ *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
+ **************************************************************************************/
+#include <string.h>
+#include "MQTTSNGWForwarder.h"
+
+using namespace MQTTSNGW;
+using namespace std;
+
+/*
+ *    Class ForwarderList
+ */
+
+ForwarderList::ForwarderList()
+{
+    _head = 0;
+}
+
+ForwarderList::~ForwarderList()
+{
+    if ( _head )
+    {
+        Forwarder* p = _head;
+        while ( p )
+        {
+            Forwarder* next = p->_next;
+            delete p;
+            p = next;
+        }
+    }
+}
+
+Forwarder* ForwarderList::getForwarder(SensorNetAddress* addr)
+{
+    Forwarder* p = _head;
+    while ( p )
+    {
+        if ( p->_sensorNetAddr.isMatch(addr) )
+        {
+            break;
+        }
+        p = p->_next;
+    }
+    return p;
+}
+
+bool ForwarderList::setFowerder(const char* fileName)
+{
+    FILE* fp;
+    char buf[MAX_CLIENTID_LENGTH + 256];
+    size_t pos;
+
+    SensorNetAddress netAddr;
+
+    if ((fp = fopen(fileName, "r")) != 0)
+    {
+        while (fgets(buf, MAX_CLIENTID_LENGTH + 254, fp) != 0)
+        {
+            if (*buf == '#')
+            {
+                continue;
+            }
+            string data = string(buf);
+            while ((pos = data.find_first_of(" 　\t\n")) != string::npos)
+            {
+                data.erase(pos, 1);
+            }
+            if (data.empty())
+            {
+                continue;
+            }
+
+            pos = data.find_first_of(",");
+            string id = data.substr(0, pos);
+            string addr = data.substr(pos + 1);
+
+            if (netAddr.setAddress(&addr) == 0)
+            {
+                addForwarder(&netAddr, &id);
+            }
+            else
+            {
+                WRITELOG("Invalid address     %s\n", data.c_str());
+                return false;
+            }
+        }
+        fclose(fp);
+    }
+    else
+    {
+        WRITELOG("Can not open the forwarders List.     %s\n", fileName);
+        return false;
+    }
+    return true;
+}
+
+
+Forwarder* ForwarderList::addForwarder(SensorNetAddress* addr,  string* forwarderId)
+{
+    Forwarder* fdr = new Forwarder(addr, forwarderId);
+    if ( _head == 0 )
+    {
+        _head = fdr;
+    }
+    else
+    {
+        Forwarder* p = _head;
+        while ( p )
+        {
+            if ( p->_next == 0 )
+            {
+                p->_next = fdr;
+                break;
+            }
+            else
+            {
+                p = p->_next;
+            }
+        }
+    }
+    return fdr;
+}
+
+Forwarder::Forwarder()
+{
+    _headClient = 0;
+    _next = 0;
+}
+
+/*
+ *    Class Forwarder
+ */
+
+Forwarder::Forwarder(SensorNetAddress* addr,  string* forwarderId)
+{
+    _forwarderName = *forwarderId;
+    _sensorNetAddr = *addr;
+    _headClient = 0;
+    _next = 0;
+}
+
+Forwarder::~Forwarder(void)
+{
+    if ( _headClient )
+    {
+        ForwardedClient* p = _headClient;
+        while ( p )
+        {
+            ForwardedClient* next = p->_next;
+            delete p;
+            p = next;
+        }
+    }
+}
+
+const char* Forwarder::getId(void)
+{
+    return _forwarderName.c_str();
+}
+
+void Forwarder::addClient(Client* client, WirelessNodeId* id)
+{
+    ForwardedClient* p = _headClient;
+    ForwardedClient* prev = 0;
+
+    client->setForwarder(this);
+
+    if ( p != 0 )
+    {
+        while ( p )
+        {
+            if ( p->_client == client )
+            {
+                client->setForwarder(this);
+                return;
+            }
+            prev = p;
+            p = p->_next;
+        }
+    }
+
+    ForwardedClient* fclient = new ForwardedClient();
+
+    fclient->setClient(client);
+    fclient->setWirelessNodeId(id);
+
+    if ( prev )
+    {
+        prev->_next = fclient;
+    }
+    else
+    {
+        _headClient = fclient;
+    }
+}
+
+Client* Forwarder::getClient(WirelessNodeId* id)
+{
+    Client* cl = 0;
+    _mutex.lock();
+    ForwardedClient* p = _headClient;
+    while ( p )
+    {
+        if ( *(p->_wirelessNodeId) == *id )
+        {
+            cl = p->_client;
+            break;
+        }
+        else
+        {
+            p = p->_next;
+        }
+    }
+    _mutex.unlock();
+    return cl;
+}
+
+const char* Forwarder::getName(void)
+{
+    return _forwarderName.c_str();
+}
+
+WirelessNodeId* Forwarder::getWirelessNodeId(Client* client)
+{
+    WirelessNodeId* nodeId = 0;
+    _mutex.lock();
+    ForwardedClient* p = _headClient;
+    while ( p )
+    {
+        if ( p->_client == client )
+        {
+            nodeId = p->_wirelessNodeId;
+            break;
+        }
+        else
+        {
+            p = p->_next;
+        }
+    }
+    _mutex.unlock();
+    return nodeId;
+}
+
+void Forwarder::eraseClient(Client* client)
+{
+    ForwardedClient* prev = 0;
+    _mutex.lock();
+    ForwardedClient* p = _headClient;
+
+    while ( p )
+    {
+        if ( p->_client == client )
+        {
+            if ( prev )
+            {
+                prev->_next = p->_next;
+            }
+            else
+            {
+                _headClient = p->_next;
+            }
+            delete p;
+            break;
+        }
+        else
+        {
+            p = p->_next;
+        }
+    }
+    _mutex.unlock();
+}
+
+SensorNetAddress* Forwarder::getSensorNetAddr(void)
+{
+    return &_sensorNetAddr;
+}
+
+/*
+ *    Class ForwardedClient
+ */
+
+ForwardedClient::ForwardedClient()
+    : _client{0}
+    , _wirelessNodeId{0}
+    , _next{0}
+{
+}
+
+ForwardedClient::~ForwardedClient()
+{
+    if (_wirelessNodeId)
+    {
+        delete _wirelessNodeId;
+    }
+}
+
+void ForwardedClient::setClient(Client* client)
+{
+    _client = client;
+}
+
+void ForwardedClient::setWirelessNodeId(WirelessNodeId* id)
+{
+    if ( _wirelessNodeId == 0 )
+    {
+        _wirelessNodeId = new WirelessNodeId();
+    }
+    _wirelessNodeId->setId(id);
+}
diff --git a/MQTTSNGateway/src/MQTTSNGWForwarder.h b/MQTTSNGateway/src/MQTTSNGWForwarder.h
new file mode 100644
index 0000000..db6d099
--- /dev/null
+++ b/MQTTSNGateway/src/MQTTSNGWForwarder.h
@@ -0,0 +1,88 @@
+/**************************************************************************************
+ * Copyright (c) 2018, Tomoaki Yamaguchi
+ *
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * and Eclipse Distribution License v1.0 which accompany this distribution.
+ *
+ * The Eclipse Public License is available at
+ *    http://www.eclipse.org/legal/epl-v10.html
+ * and the Eclipse Distribution License is available at
+ *   http://www.eclipse.org/org/documents/edl-v10.php.
+ *
+ * Contributors:
+ *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
+ **************************************************************************************/
+
+#ifndef MQTTSNGATEWAY_SRC_MQTTSNGWFORWARDER_H_
+#define MQTTSNGATEWAY_SRC_MQTTSNGWFORWARDER_H_
+
+#include "MQTTSNGWClient.h"
+#include "MQTTSNGWEncapsulatedPacket.h"
+#include "SensorNetwork.h"
+
+
+namespace MQTTSNGW
+{
+
+class Client;
+class WirelessNodeId;
+
+class ForwardedClient
+{
+    friend class Forwarder;
+public:
+    ForwardedClient();
+    ~ForwardedClient();
+    void setClient(Client* client);
+    void setWirelessNodeId(WirelessNodeId* id);
+private:
+    Client* _client;
+    WirelessNodeId* _wirelessNodeId;
+    ForwardedClient* _next;
+};
+
+
+class Forwarder
+{
+    friend class ForwarderList;
+public:
+    Forwarder();
+    Forwarder(SensorNetAddress* addr,  string* forwarderId);
+    ~Forwarder();
+
+    const char* getId(void);
+    void addClient(Client* client, WirelessNodeId* id);
+    Client* getClient(WirelessNodeId* id);
+    WirelessNodeId* getWirelessNodeId(Client* client);
+    void eraseClient(Client* client);
+    SensorNetAddress* getSensorNetAddr(void);
+    const char* getName(void);
+
+private:
+    string _forwarderName;
+    SensorNetAddress _sensorNetAddr;
+    ForwardedClient* _headClient;
+    Forwarder* _next;
+    Mutex _mutex;
+};
+
+class ForwarderList
+{
+public:
+    ForwarderList();
+    ~ForwarderList();
+
+    Forwarder* getForwarder(SensorNetAddress* addr);
+    bool setFowerder(const char* fileName);
+    Forwarder* addForwarder(SensorNetAddress* addr,  string* forwarderId);
+
+private:
+    Forwarder* _head;
+};
+
+}
+
+
+
+#endif /* MQTTSNGATEWAY_SRC_MQTTSNGWFORWARDER_H_ */
diff --git a/MQTTSNGateway/src/MQTTSNGWVersion.h b/MQTTSNGateway/src/MQTTSNGWVersion.h
index 90f4eab..04c55ce 100644
--- a/MQTTSNGateway/src/MQTTSNGWVersion.h
+++ b/MQTTSNGateway/src/MQTTSNGWVersion.h
@@ -17,6 +17,6 @@
 #ifndef MQTTSNGWVERSION_H_IN_
 #define MQTTSNGWVERSION_H_IN_
 
-#define PAHO_GATEWAY_VERSION "1.1.0"
+#define PAHO_GATEWAY_VERSION "1.2.0"
 
 #endif /* MQTTSNGWVERSION_H_IN_ */
diff --git a/MQTTSNGateway/src/MQTTSNGateway.cpp b/MQTTSNGateway/src/MQTTSNGateway.cpp
index afd0197..c6029ce 100644
--- a/MQTTSNGateway/src/MQTTSNGateway.cpp
+++ b/MQTTSNGateway/src/MQTTSNGateway.cpp
@@ -46,6 +46,8 @@ Gateway::Gateway()
 	_params.privateKey = 0;
 	_params.clientListName = 0;
 	_params.configName = 0;
+	_params.predefinedTopicFileName = 0;
+	_params.forwarderListName = 0;
 	_packetEventQue.setMaxSize(MAX_INFLIGHTMESSAGES * MAX_CLIENTS);
 }
 
@@ -99,6 +101,14 @@ Gateway::~Gateway()
 	{
 		free(_params.configName);
 	}
+    if ( _params.predefinedTopicFileName )
+    {
+        free(_params.predefinedTopicFileName);
+    }
+    if ( _params.forwarderListName )
+    {
+        free(_params.forwarderListName);
+    }
 }
 
 void Gateway::initialize(int argc, char** argv)
@@ -214,7 +224,7 @@ void Gateway::initialize(int argc, char** argv)
 	{
 	    if (!strcasecmp(param, "YES") )
 	    {
-            if (getParam("PredefinedTopicFile", param) == 0)
+            if (getParam("PredefinedTopicList", param) == 0)
             {
                 fileName = string(param);
             }
@@ -234,6 +244,30 @@ void Gateway::initialize(int argc, char** argv)
 	    }
 	}
 
+	if (getParam("Forwarder", param) == 0 )
+	    {
+	        if (!strcasecmp(param, "YES") )
+	        {
+	            if (getParam("ForwardersList", param) == 0)
+	            {
+	                fileName = string(param);
+	            }
+	            else
+	            {
+	                fileName = *getConfigDirName() + string(FORWARDER_LIST);
+	            }
+	           if ( !_forwarderList.setFowerder(fileName.c_str()) )
+	           {
+	               throw Exception("Gateway::initialize: No ForwardersList file defined by the configuration..");
+	           }
+	            _params.forwarderListName = strdup(fileName.c_str());
+	        }
+	        else
+	        {
+	            _params.forwarderListName = 0;
+	        }
+	    }
+
     fileName = *getConfigDirName() + *getConfigFileName();
     _params.configName = strdup(fileName.c_str());
 }
@@ -254,12 +288,17 @@ void Gateway::run(void)
 	{
 		WRITELOG(" ClientList:  %s\n", _params.clientListName);
 	}
-	WRITELOG(" SensorN/W:  %s\n", _sensorNetwork.getDescription());
-	WRITELOG(" Broker:     %s : %s, %s\n", _params.brokerName, _params.port, _params.portSecure);
     if (  _params.predefinedTopicFileName )
     {
         WRITELOG(" PreDefFile: %s\n", _params.predefinedTopicFileName);
     }
+    if (  _params.forwarderListName )
+    {
+        WRITELOG(" Forwarders: %s\n", _params.forwarderListName);
+    }
+	WRITELOG(" SensorN/W:  %s\n", _sensorNetwork.getDescription());
+	WRITELOG(" Broker:     %s : %s, %s\n", _params.brokerName, _params.port, _params.portSecure);
+
 	WRITELOG(" RootCApath: %s\n", _params.rootCApath);
 	WRITELOG(" RootCAfile: %s\n", _params.rootCAfile);
 	WRITELOG(" CertKey:    %s\n", _params.certKey);
@@ -305,6 +344,11 @@ ClientList* Gateway::getClientList()
 	return &_clientList;
 }
 
+ForwarderList* Gateway::getForwarderList(void)
+{
+    return &_forwarderList;
+}
+
 SensorNetwork* Gateway::getSensorNetwork()
 {
 	return &_sensorNetwork;
diff --git a/MQTTSNGateway/src/MQTTSNGateway.h b/MQTTSNGateway/src/MQTTSNGateway.h
index 3b8ab53..d219d6f 100644
--- a/MQTTSNGateway/src/MQTTSNGateway.h
+++ b/MQTTSNGateway/src/MQTTSNGateway.h
@@ -17,6 +17,7 @@
 #define MQTTSNGATEWAY_H_
 
 #include "MQTTSNGWClient.h"
+#include "MQTTSNGWForwarder.h"
 #include "MQTTSNGWProcess.h"
 #include "MQTTSNPacket.h"
 
@@ -42,6 +43,7 @@ namespace MQTTSNGW
 #define CLIENT      "Client"
 #define CLIENTS     "Clients"
 #define UNKNOWNCL   "Unknown Client !"
+
 #define LEFTARROW   "<---"
 #define RIGHTARROW  "--->"
 #define LEFTARROWB   "<==="
@@ -69,14 +71,6 @@ namespace MQTTSNGW
 #define ERRMSG_HEADER            "\033[0m\033[0;31mError:"
 #define ERRMSG_FOOTER            "\033[0m\033[0;37m"
 
-/*=====================================
-      Predefined TopicId for OTA
-  ====================================*/
-//#define OTA_CLIENTS
-//#define PREDEFINEDID_OTA_REQ       (0x0ff0)
-//#define PREDEFINEDID_OTA_READY     (0x0ff1)
-//#define PREDEFINEDID_OTA_NO_CLIENT (0x0ff2)
-
 /*=====================================
          Class Event
   ====================================*/
@@ -161,6 +155,7 @@ typedef struct
 	char* certKey;
 	char* privateKey;
 	char* predefinedTopicFileName;
+	char* forwarderListName;
 }GatewayParams;
 
 /*=====================================
@@ -177,12 +172,14 @@ public:
 	EventQue* getClientSendQue(void);
 	EventQue* getBrokerSendQue(void);
 	ClientList* getClientList(void);
+	ForwarderList* getForwarderList(void);
 	SensorNetwork* getSensorNetwork(void);
 	LightIndicator* getLightIndicator(void);
 	GatewayParams* getGWParams(void);
 
 private:
 	ClientList _clientList;
+	ForwarderList _forwarderList;
 	EventQue   _packetEventQue;
 	EventQue   _brokerSendQue;
 	EventQue   _clientSendQue;
diff --git a/MQTTSNGateway/src/tests/TestProcess.cpp b/MQTTSNGateway/src/tests/TestProcess.cpp
index 6ba7283..9b3bdb5 100644
--- a/MQTTSNGateway/src/tests/TestProcess.cpp
+++ b/MQTTSNGateway/src/tests/TestProcess.cpp
@@ -93,10 +93,10 @@ void TestProcess::run(void)
 	delete tque;
 
 	/* Test Tree23 */
-    printf("Test  Tree23         ");
-	TestTree23* tree23 = new TestTree23();
-	tree23->test();
-	delete tree23;
+    //printf("Test  Tree23         ");
+	//TestTree23* tree23 = new TestTree23();
+	//tree23->test();
+	//delete tree23;
 
 	/* Test TopicTable */
     printf("Test  Topic          ");
@@ -111,6 +111,7 @@ void TestProcess::run(void)
 	delete testMap;
 
 	/* Test EventQue */
+	/*
 	printf("Test  EventQue       ");
 	Client* client = new Client();
 	_evQue.setMaxSize(EVENT_CNT);
@@ -122,6 +123,7 @@ void TestProcess::run(void)
 		ev->setClientSendEvent(client, packet);
 		_evQue.post(ev);
 	}
-
+	delete client;
+	*/
 	//MultiTaskProcess::run();
 }
diff --git a/MQTTSNGateway/src/tests/mainTestProcess.cpp b/MQTTSNGateway/src/tests/mainTestProcess.cpp
index 84e91e4..1ffab06 100644
--- a/MQTTSNGateway/src/tests/mainTestProcess.cpp
+++ b/MQTTSNGateway/src/tests/mainTestProcess.cpp
@@ -19,7 +19,7 @@
 using namespace MQTTSNGW;
 
 TestProcess* test = new TestProcess();
-TestTask* task = new TestTask(test);
+//TestTask* task = new TestTask(test);
 
 int main(int argc, char** argv)
 {
diff --git a/MQTTSNPacket/src/MQTTSNPacket.c b/MQTTSNPacket/src/MQTTSNPacket.c
index 092eae0..03928b1 100644
--- a/MQTTSNPacket/src/MQTTSNPacket.c
+++ b/MQTTSNPacket/src/MQTTSNPacket.c
@@ -29,6 +29,7 @@ static const char* packet_names[] =
 		"WILLMSGRESP"
 };
 
+static const char* encapsulation_packet_name = "ENCAPSULATED";
 
 /**
  * Returns a character string representing the packet name given a MsgType code
@@ -37,6 +38,10 @@ static const char* packet_names[] =
  */
 const char* MQTTSNPacket_name(int code)
 {
+    if ( code == MQTTSN_ENCAPSULATED )
+    {
+        return encapsulation_packet_name;
+    }
 	return (code >= 0 && code <= MQTTSN_WILLMSGRESP) ? packet_names[code] : "UNKNOWN";
 }
 
diff --git a/MQTTSNPacket/src/MQTTSNPacket.h b/MQTTSNPacket/src/MQTTSNPacket.h
index a3466c6..ebde5b3 100644
--- a/MQTTSNPacket/src/MQTTSNPacket.h
+++ b/MQTTSNPacket/src/MQTTSNPacket.h
@@ -26,7 +26,7 @@ enum errors
 {
 	MQTTSNPACKET_BUFFER_TOO_SHORT = -2,
 	MQTTSNPACKET_READ_ERROR = -1,
-	MQTTSNPACKET_READ_COMPLETE,
+	MQTTSNPACKET_READ_COMPLETE
 };
 
 #define MQTTSN_PROTOCOL_VERSION 0x01
@@ -43,12 +43,12 @@ typedef enum
 {
 	MQTTSN_TOPIC_TYPE_NORMAL, /* topic id in publish, topic name in subscribe */
 	MQTTSN_TOPIC_TYPE_PREDEFINED,
-	MQTTSN_TOPIC_TYPE_SHORT,
+	MQTTSN_TOPIC_TYPE_SHORT
 }MQTTSN_topicTypes;
 
 enum MQTTSN_msgTypes
 {
-	MQTTSN_ADVERTISE, MQTTSN_SEARCHGW, MQTTSN_GWINFO, MQTTSN_RESERVED1, 
+	MQTTSN_ADVERTISE, MQTTSN_SEARCHGW, MQTTSN_GWINFO, MQTTSN_RESERVED1,
 	MQTTSN_CONNECT, MQTTSN_CONNACK,
 	MQTTSN_WILLTOPICREQ, MQTTSN_WILLTOPIC, MQTTSN_WILLMSGREQ, MQTTSN_WILLMSG, 
 	MQTTSN_REGISTER, MQTTSN_REGACK,
@@ -57,8 +57,10 @@ enum MQTTSN_msgTypes
 	MQTTSN_PINGREQ, MQTTSN_PINGRESP,
 	MQTTSN_DISCONNECT, MQTTSN_RESERVED3, 
 	MQTTSN_WILLTOPICUPD, MQTTSN_WILLTOPICRESP, MQTTSN_WILLMSGUPD, MQTTSN_WILLMSGRESP,
+	MQTTSN_ENCAPSULATED = 0xfe
 };
 
+
 typedef struct
 {
 	MQTTSN_topicTypes type;