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] 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  | 679 ++++++++++--------
 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, 1338 insertions(+), 449 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;
-
-	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){
-
-			*pos++ = 3;
-			*pos++ = MQTTSN_TYPE_SEARCHGW;
-			*pos = 0;                        // SERCHGW
-			_status = GW_SEARCHING;
-			writeGwMsg();
-
-		}
-		getConnectResponce();
-	}
-	return;
+    char* pos;
+
+    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){
+
+            *pos++ = 3;
+            *pos++ = MQTTSN_TYPE_SEARCHGW;
+            *pos = 0;                        // SERCHGW
+            _status = GW_SEARCHING;
+            writeGwMsg();
+
+        }
+        getConnectResponce();
+    }
+    return;
 }
 
 int LGwProxy::getConnectResponce(void){
-	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();
+    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();
         _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");
-
-				if ( _cleanSession || _initialized == 1 )
-				{
-	                _topicTbl.clearTopic();
-				    _initialized = 0;
-				    theClient->onConnect();  // SUBSCRIBEs are conducted
-				}
-			}
-		}else{
-			_status = GW_CONNECTING;
-		}
-	}
-	return 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");
+
+                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;
-
-	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);
-
-	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;
-		}
-	}
+    _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;
+    }
+
+    _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;
+        }
+    }
 }
 
 int LGwProxy::getDisconnectResponce(void){
-	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);
-
-			/* Wake up and starts from this point. */
-
-		}else{
-			_status = GW_DISCONNECTED;
-		}
-	}
-	return 0;
+    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);
+
+            /* Wake up and starts from this point. */
+
+        }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;
-
-	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 (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;
+    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] == 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;
 }
 
 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;
-	}
-
-	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;
+    int len = 0;
+    uint8_t* msg = _network.getMessage(&len);
+    _mqttsnMsg = msg;
+
+    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 @@ class WaitREGACKPacketList
 /*=====================================
  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 @@ class Client
     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 @@ class Client
     bool _sensorNetype;     // false: unstable network like a G3
     SensorNetAddress _sensorNetAddr;
 
+    Forwarder* _forwarder;
+
+
     bool _sessionStatus;
     bool _hasPredefTopic;
 
@@ -373,5 +381,7 @@ class ClientList
     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 @@ class ClientSendTask: public Thread
 
 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 @@ class Gateway: public MultiTaskProcess{
 	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;