Add required Eclipse meta docs

Merge pull request #251 from agalliazzo/udp6_multicast_hop_bugfix
Fix issue #250
2025-12-15 08:26:51 +01:00 · 2024-01-18 15:17:28 +00:00 · 2021-12-26 16:20:44 +09:00 · 2021-12-26 16:20:21 +09:00 · 2021-12-26 16:19:38 +09:00 · 2021-12-26 16:18:19 +09:00
162 changed files with 20340 additions and 7763 deletions
--- a/.cproject
+++ b/.cproject
@@ -14,30 +14,42 @@
 				</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.selectedvolumes=,org.eclipse.cdt.docker.launcher.containerbuild.property.volumes=" 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"/>
+							<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" parallelBuildOn="true" parallelizationNumber="optimal" 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.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"/>
-								<option id="gnu.cpp.compiler.option.include.paths.1361987608" name="Include paths (-I)" superClass="gnu.cpp.compiler.option.include.paths" useByScannerDiscovery="false" valueType="includePath">
-									<listOptionValue builtIn="false" value="/home/tomoaki/git/paho.mqtt-sn.embedded-c/MQTTSNGateway/src"/>
-									<listOptionValue builtIn="false" value="/home/tomoaki/git/paho.mqtt-sn.embedded-c/MQTTSNGateway/GatewayTester/src"/>
-									<listOptionValue builtIn="false" value="/home/tomoaki/git/paho.mqtt-sn.embedded-c/MQTTSNGateway/src/linux"/>
-									<listOptionValue builtIn="false" value="/home/tomoaki/git/paho.mqtt-sn.embedded-c/MQTTSNPacket/src"/>
-									<listOptionValue builtIn="false" value="/home/tomoaki/git/paho.mqtt-sn.embedded-c/MQTTSNClient/src"/>
-									<listOptionValue builtIn="false" value="/home/tomoaki/git/paho.mqtt-sn.embedded-c/MQTTSNGateway/src/linux/udp"/>
+								<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.default" 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.c++11" 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="" valueType="string"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.compiler.option.include.paths.1361987608" name="Include paths (-I)" superClass="gnu.cpp.compiler.option.include.paths" useByScannerDiscovery="false" valueType="includePath">
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/MQTTSNGateway/src}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/MQTTSNGateway/GatewayTester/src}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/MQTTSNGateway/src/linux/dtls}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/MQTTSNGateway/src/linux}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/MQTTSNPacket/src}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/MQTTSNClient/src}&quot;"/>
+								</option>
+								<option id="gnu.cpp.compiler.option.debugging.sanitleak.778932189" name="Sanitize memory leak (-fsanitize=leak)" superClass="gnu.cpp.compiler.option.debugging.sanitleak" useByScannerDiscovery="false" value="false" valueType="boolean"/>
+								<option id="gnu.cpp.compiler.option.debugging.sanitpointers.435146187" name="Sanitize pointer operations (-fsanitize=pointer-compare -fsanitize=pointer-subtract)" superClass="gnu.cpp.compiler.option.debugging.sanitpointers" useByScannerDiscovery="false" value="false" valueType="boolean"/>
+								<option id="gnu.cpp.compiler.option.debugging.sanitaddress.501742254" name="Sanitize address (-fsanitize=address)" superClass="gnu.cpp.compiler.option.debugging.sanitaddress" useByScannerDiscovery="false" value="false" valueType="boolean"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.compiler.option.preprocessor.def.1354093543" name="Defined symbols (-D)" superClass="gnu.cpp.compiler.option.preprocessor.def" useByScannerDiscovery="false" valueType="definedSymbols">
+									<listOptionValue builtIn="false" value="DTLS"/>
 								</option>
 								<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.1626802967" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
 							</tool>
 							<tool id="cdt.managedbuild.tool.gnu.c.compiler.exe.debug.2017162618" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.exe.debug">
 								<option defaultValue="gnu.c.optimization.level.none" id="gnu.c.compiler.exe.debug.option.optimization.level.1941536725" name="Optimization Level" superClass="gnu.c.compiler.exe.debug.option.optimization.level" useByScannerDiscovery="false" valueType="enumerated"/>
 								<option id="gnu.c.compiler.exe.debug.option.debugging.level.1668726974" name="Debug Level" superClass="gnu.c.compiler.exe.debug.option.debugging.level" useByScannerDiscovery="false" value="gnu.c.debugging.level.max" valueType="enumerated"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.c.compiler.option.preprocessor.def.symbols.798283837" name="Defined symbols (-D)" superClass="gnu.c.compiler.option.preprocessor.def.symbols" useByScannerDiscovery="false" valueType="definedSymbols">
+									<listOptionValue builtIn="false" value="OPENSSL_API_COMPAT=30000"/>
+								</option>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.c.compiler.option.include.paths.1870583388" name="Include paths (-I)" superClass="gnu.c.compiler.option.include.paths" useByScannerDiscovery="false" valueType="includePath">
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/MQTTSNGateway/src/linux/dtls}&quot;"/>
+								</option>
 								<inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.814497727" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
 							</tool>
 							<tool id="cdt.managedbuild.tool.gnu.c.linker.exe.debug.1881440001" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.exe.debug">
@@ -47,14 +59,16 @@
 								</inputType>
 							</tool>
 							<tool id="cdt.managedbuild.tool.gnu.cpp.linker.exe.debug.581660133" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.exe.debug">
-								<option id="gnu.cpp.link.option.libs.1848275182" name="Libraries (-l)" superClass="gnu.cpp.link.option.libs" useByScannerDiscovery="false" valueType="libs">
-									<listOptionValue builtIn="false" srcPrefixMapping="" srcRootPath="" value="pthread"/>
-									<listOptionValue builtIn="false" srcPrefixMapping="" srcRootPath="" value="ssl"/>
-									<listOptionValue builtIn="false" srcPrefixMapping="" srcRootPath="" value="crypto"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.link.option.libs.1848275182" name="Libraries (-l)" superClass="gnu.cpp.link.option.libs" useByScannerDiscovery="false" valueType="libs">
+									<listOptionValue builtIn="false" value="ssl"/>
+									<listOptionValue builtIn="false" value="crypto"/>
+									<listOptionValue builtIn="false" value="pthread"/>
+									<listOptionValue builtIn="false" value="bluetooth"/>
 								</option>
-								<option id="gnu.cpp.link.option.paths.170974409" name="Library search path (-L)" superClass="gnu.cpp.link.option.paths" useByScannerDiscovery="false" valueType="libPaths">
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.link.option.paths.170974409" name="Library search path (-L)" superClass="gnu.cpp.link.option.paths" useByScannerDiscovery="false" valueType="libPaths">
 									<listOptionValue builtIn="false" value="/usr/local/lib"/>
 								</option>
+								<option id="gnu.cpp.link.option.shared.1648098307" name="Shared (-shared)" superClass="gnu.cpp.link.option.shared" useByScannerDiscovery="false" value="false" valueType="boolean"/>
 								<inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.98211496" superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
 									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
 									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
@@ -66,7 +80,9 @@
 						</toolChain>
 					</folderInfo>
 					<sourceEntries>
-						<entry excluding="MQTTSNGateway/src/linux/udp|MQTTSNGateway/src/tests|MQTTSNGateway/GatewayTester|MQTTSNGateway/GatewayTester/samples/mainTest.cpp|MQTTSNGateway/GatewayTester/samples/mainTemplate.cpp|MQTTSNGateway/src/tests/mainTestProcessFramework.cpp|MQTTSNGateway/GatewayTester/samples/mainOTA.cpp|MQTTSNGateway/src/mainLogmonitor.cpp|MQTTSNPacket/test|MQTTSNPacket/samples" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
+						<entry excluding="GatewayTester|src/linux/udp6|src/linux/udp|src/linux/dtls/SensorNetSubTask.cpp|GatewayTester/samples/ClientSub|GatewayTester/samples/ClientPubQoS-1|GatewayTester/samples/ClientPub|src/linux/rfcomm|src/tests|src/linux/xbee|src/mainLogmonitor.cpp|src/linux/loralink" flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="MQTTSNGateway"/>
+						<entry excluding="mainTest.cpp|ClientSub|ClientPubQoS-1|ClientPub" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="MQTTSNGateway/GatewayTester/samples"/>
+						<entry excluding="samples|test" flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="MQTTSNPacket"/>
 					</sourceEntries>
 				</configuration>
 			</storageModule>
@@ -85,7 +101,7 @@
 				</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.release" cleanCommand="rm -rf" description="" id="cdt.managedbuild.config.gnu.exe.release.561557339" name="Release" parent="cdt.managedbuild.config.gnu.exe.release">
+				<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.release" cleanCommand="rm -rf" description="" id="cdt.managedbuild.config.gnu.exe.release.561557339" name="Release" optionalBuildProperties="" parent="cdt.managedbuild.config.gnu.exe.release">
 					<folderInfo id="cdt.managedbuild.config.gnu.exe.release.561557339." name="/" resourcePath="">
 						<toolChain id="cdt.managedbuild.toolchain.gnu.exe.release.474335535" name="Linux GCC" superClass="cdt.managedbuild.toolchain.gnu.exe.release">
 							<targetPlatform id="cdt.managedbuild.target.gnu.platform.exe.release.338327831" name="Debug Platform" superClass="cdt.managedbuild.target.gnu.platform.exe.release"/>
@@ -95,12 +111,12 @@
 								<option id="gnu.cpp.compiler.exe.release.option.optimization.level.15425920" name="Optimization Level" superClass="gnu.cpp.compiler.exe.release.option.optimization.level" useByScannerDiscovery="false" value="gnu.cpp.compiler.optimization.level.most" valueType="enumerated"/>
 								<option id="gnu.cpp.compiler.exe.release.option.debugging.level.857802503" name="Debug Level" superClass="gnu.cpp.compiler.exe.release.option.debugging.level" useByScannerDiscovery="false" value="gnu.cpp.compiler.debugging.level.none" valueType="enumerated"/>
 								<option id="gnu.cpp.compiler.option.dialect.flags.1078302249" name="Other dialect flags" superClass="gnu.cpp.compiler.option.dialect.flags" useByScannerDiscovery="true" value="-std=c++11" valueType="string"/>
-								<option id="gnu.cpp.compiler.option.include.paths.2114194326" name="Include paths (-I)" superClass="gnu.cpp.compiler.option.include.paths" useByScannerDiscovery="false" valueType="includePath">
-									<listOptionValue builtIn="false" value="/home/tomoaki/git/paho.mqtt-sn.embedded-c/MQTTSNGateway/src/linux/udp"/>
-									<listOptionValue builtIn="false" value="/home/tomoaki/git/paho.mqtt-sn.embedded-c/MQTTSNGateway/GatewayTester/src"/>
-									<listOptionValue builtIn="false" value="/home/tomoaki/git/paho.mqtt-sn.embedded-c/MQTTSNGateway/src"/>
-									<listOptionValue builtIn="false" value="/home/tomoaki/git/paho.mqtt-sn.embedded-c/MQTTSNGateway/src/linux"/>
-									<listOptionValue builtIn="false" value="/home/tomoaki/git/paho.mqtt-sn.embedded-c/MQTTSNPacket/src"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.compiler.option.include.paths.2114194326" name="Include paths (-I)" superClass="gnu.cpp.compiler.option.include.paths" useByScannerDiscovery="false" valueType="includePath">
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/MQTTSNGateway/src/linux/udp}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/MQTTSNPacket/src}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/MQTTSNGateway/src/linux}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/MQTTSNGateway/src}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/MQTTSNGateway/GatewayTester/src}&quot;"/>
 								</option>
 								<option id="gnu.cpp.compiler.option.dialect.std.216116103" name="Language standard" superClass="gnu.cpp.compiler.option.dialect.std" useByScannerDiscovery="true" value="gnu.cpp.compiler.dialect.default" valueType="enumerated"/>
 								<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.1606625536" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
@@ -117,12 +133,12 @@
 								</inputType>
 							</tool>
 							<tool id="cdt.managedbuild.tool.gnu.cpp.linker.exe.release.1623573602" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.exe.release">
-								<option id="gnu.cpp.link.option.libs.68366124" name="Libraries (-l)" superClass="gnu.cpp.link.option.libs" useByScannerDiscovery="false" valueType="libs">
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.link.option.libs.68366124" name="Libraries (-l)" superClass="gnu.cpp.link.option.libs" useByScannerDiscovery="false" valueType="libs">
 									<listOptionValue builtIn="false" srcPrefixMapping="" srcRootPath="" value="pthread"/>
 									<listOptionValue builtIn="false" srcPrefixMapping="" srcRootPath="" value="ssl"/>
 									<listOptionValue builtIn="false" srcPrefixMapping="" srcRootPath="" value="crypto"/>
 								</option>
-								<option id="gnu.cpp.link.option.paths.1879517971" name="Library search path (-L)" superClass="gnu.cpp.link.option.paths" useByScannerDiscovery="false" valueType="libPaths">
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.link.option.paths.1879517971" name="Library search path (-L)" superClass="gnu.cpp.link.option.paths" useByScannerDiscovery="false" valueType="libPaths">
 									<listOptionValue builtIn="false" value="/usr/local/lib"/>
 								</option>
 								<inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.2044523925" superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
@@ -136,9 +152,9 @@
 						</toolChain>
 					</folderInfo>
 					<sourceEntries>
-						<entry excluding="MQTTSNGateway/GatewayTester|MQTTSNGateway/GatewayTester/samples/mainTest.cpp|MQTTSNGateway/GatewayTester/samples/mainTemplate.cpp|MQTTSNGateway/GatewayTester/samples/mainOTA.cpp|MQTTSNPacket/src|MQTTSNGateway/src|MQTTSNGateway/src/linux|MQTTSNGateway/src/linux/udp|MQTTSNGateway/src/mainLogmonitor.cpp|MQTTSNGateway/src/linux/xbee|MQTTSNPacket/test|MQTTSNPacket/samples" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
-						<entry excluding="tests|tests/mainTestProcessFramework.cpp|mainLogmonitor.cpp|linux|linux/udp" flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="MQTTSNGateway/src"/>
-						<entry excluding="udp" flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="MQTTSNGateway/src/linux"/>
+						<entry excluding="MQTTSNPacket/test|MQTTSNPacket/samples|MQTTSNGateway/GatewayTester/samples/ClientPub|MQTTSNGateway/src/tests/mainTestProcess.cpp|MQTTSNGateway/src/linux|MQTTSNGateway|MQTTSNGateway/GatewayTester/samples/mainOTA.cpp|MQTTSNGateway/GatewayTester/samples/ClientSub|MQTTSNClient|MQTTSNGateway/src/mainLogmonitor.cpp|MQTTSNGateway/GatewayTester/samples/ClientPubQoS-1|MQTTSNGateway/GatewayTester/samples/mainTemplate.cpp|MQTTSNGateway/src|MQTTSNPacket/src|MQTTSNGateway/GatewayTester|ClientPubQoS-1" flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name=""/>
+						<entry excluding="mainTest.cpp|ClientSub|ClientPubQoS-1|ClientPub" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="MQTTSNGateway/GatewayTester/samples"/>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="MQTTSNGateway/src"/>
 						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="MQTTSNPacket/src"/>
 					</sourceEntries>
 				</configuration>
@@ -151,28 +167,26 @@
 	</storageModule>
 	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
 	<storageModule moduleId="refreshScope" versionNumber="2">
+		<configuration configurationName="Debug"/>
 		<configuration configurationName="Release">
 			<resource resourceType="PROJECT" workspacePath="/MQTTSN-embedded-C"/>
 		</configuration>
-		<configuration configurationName="Debug">
-			<resource resourceType="PROJECT" workspacePath="/MQTTSN-embedded-C"/>
-		</configuration>
 	</storageModule>
 	<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
+	<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets"/>
 	<storageModule moduleId="scannerConfiguration">
 		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.561557339;cdt.managedbuild.config.gnu.exe.release.561557339.;cdt.managedbuild.tool.gnu.cpp.compiler.exe.release.1903732701;cdt.managedbuild.tool.gnu.cpp.compiler.input.1606625536">
-			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-		</scannerConfigBuildInfo>
-		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.1685199227;cdt.managedbuild.config.gnu.exe.debug.1685199227.;cdt.managedbuild.tool.gnu.c.compiler.exe.debug.2017162618;cdt.managedbuild.tool.gnu.c.compiler.input.814497727">
-			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-		</scannerConfigBuildInfo>
 		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.1685199227;cdt.managedbuild.config.gnu.exe.debug.1685199227.;cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug.109068141;cdt.managedbuild.tool.gnu.cpp.compiler.input.1626802967">
 			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
 		</scannerConfigBuildInfo>
 		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.561557339;cdt.managedbuild.config.gnu.exe.release.561557339.;cdt.managedbuild.tool.gnu.c.compiler.exe.release.246546722;cdt.managedbuild.tool.gnu.c.compiler.input.456127079">
 			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
 		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.1685199227;cdt.managedbuild.config.gnu.exe.debug.1685199227.;cdt.managedbuild.tool.gnu.c.compiler.exe.debug.2017162618;cdt.managedbuild.tool.gnu.c.compiler.input.814497727">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.561557339;cdt.managedbuild.config.gnu.exe.release.561557339.;cdt.managedbuild.tool.gnu.cpp.compiler.exe.release.1903732701;cdt.managedbuild.tool.gnu.cpp.compiler.input.1606625536">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
 	</storageModule>
-	<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets"/>
-</cproject>
+</cproject>
--- a/.gitignore
+++ b/.gitignore
@@ -5,3 +5,15 @@
 *.pyc
 /doc/MQTTSNClient/
 /doc/MQTTSNPacket/
+rbmutex.key
+ringbuffer.key
+/Release/
+/Debug/
+/core
+Build/
+*.a
+CMakeFiles/
+*.cmake
+CMakeCache.txt
+bin/
+/build.gateway/
--- a/.settings/language.settings.xml
+++ b/.settings/language.settings.xml
@@ -0,0 +1,25 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<project>
+	<configuration id="cdt.managedbuild.config.gnu.exe.debug.1685199227" name="Debug">
+		<extension point="org.eclipse.cdt.core.LanguageSettingsProvider">
+			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
+			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
+			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
+			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-176156747064280842" id="org.eclipse.cdt.managedbuilder.core.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+				<language-scope id="org.eclipse.cdt.core.gcc"/>
+				<language-scope id="org.eclipse.cdt.core.g++"/>
+			</provider>
+		</extension>
+	</configuration>
+	<configuration id="cdt.managedbuild.config.gnu.exe.release.561557339" name="Release">
+		<extension point="org.eclipse.cdt.core.LanguageSettingsProvider">
+			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
+			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
+			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
+			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-176157551606184930" id="org.eclipse.cdt.managedbuilder.core.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+				<language-scope id="org.eclipse.cdt.core.gcc"/>
+				<language-scope id="org.eclipse.cdt.core.g++"/>
+			</provider>
+		</extension>
+	</configuration>
+</project>
--- a/.settings/org.eclipse.cdt.codan.core.prefs
+++ b/.settings/org.eclipse.cdt.codan.core.prefs
@@ -0,0 +1,77 @@
+eclipse.preferences.version=1
+fr.ac6.mcu.ide.source.checker.libnano.problem=Error
+fr.ac6.mcu.ide.source.checker.libnano.problem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Float formatting support\\")"}
+org.eclipse.cdt.codan.checkers.errnoreturn=Warning
+org.eclipse.cdt.codan.checkers.errnoreturn.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"No return\\")",implicit\=>false}
+org.eclipse.cdt.codan.checkers.errreturnvalue=Error
+org.eclipse.cdt.codan.checkers.errreturnvalue.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Unused return value\\")"}
+org.eclipse.cdt.codan.checkers.nocommentinside=-Error
+org.eclipse.cdt.codan.checkers.nocommentinside.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Nesting comments\\")"}
+org.eclipse.cdt.codan.checkers.nolinecomment=-Error
+org.eclipse.cdt.codan.checkers.nolinecomment.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Line comments\\")"}
+org.eclipse.cdt.codan.checkers.noreturn=Error
+org.eclipse.cdt.codan.checkers.noreturn.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"No return value\\")",implicit\=>false}
+org.eclipse.cdt.codan.internal.checkers.AbstractClassCreation=Error
+org.eclipse.cdt.codan.internal.checkers.AbstractClassCreation.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Abstract class cannot be instantiated\\")"}
+org.eclipse.cdt.codan.internal.checkers.AmbiguousProblem=Error
+org.eclipse.cdt.codan.internal.checkers.AmbiguousProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Ambiguous problem\\")"}
+org.eclipse.cdt.codan.internal.checkers.AssignmentInConditionProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.AssignmentInConditionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Assignment in condition\\")"}
+org.eclipse.cdt.codan.internal.checkers.AssignmentToItselfProblem=Error
+org.eclipse.cdt.codan.internal.checkers.AssignmentToItselfProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Assignment to itself\\")"}
+org.eclipse.cdt.codan.internal.checkers.CaseBreakProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.CaseBreakProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"No break at end of case\\")",no_break_comment\=>"no break",last_case_param\=>false,empty_case_param\=>false,enable_fallthrough_quickfix_param\=>false}
+org.eclipse.cdt.codan.internal.checkers.CatchByReference=Warning
+org.eclipse.cdt.codan.internal.checkers.CatchByReference.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Catching by reference is recommended\\")",unknown\=>false,exceptions\=>()}
+org.eclipse.cdt.codan.internal.checkers.CircularReferenceProblem=Error
+org.eclipse.cdt.codan.internal.checkers.CircularReferenceProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Circular inheritance\\")"}
+org.eclipse.cdt.codan.internal.checkers.ClassMembersInitialization=Warning
+org.eclipse.cdt.codan.internal.checkers.ClassMembersInitialization.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Class members should be properly initialized\\")",skip\=>true}
+org.eclipse.cdt.codan.internal.checkers.DecltypeAutoProblem=Error
+org.eclipse.cdt.codan.internal.checkers.DecltypeAutoProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Invalid 'decltype(auto)' specifier\\")"}
+org.eclipse.cdt.codan.internal.checkers.FieldResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.FieldResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Field cannot be resolved\\")"}
+org.eclipse.cdt.codan.internal.checkers.FunctionResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.FunctionResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Function cannot be resolved\\")"}
+org.eclipse.cdt.codan.internal.checkers.InvalidArguments=Error
+org.eclipse.cdt.codan.internal.checkers.InvalidArguments.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Invalid arguments\\")"}
+org.eclipse.cdt.codan.internal.checkers.InvalidTemplateArgumentsProblem=Error
+org.eclipse.cdt.codan.internal.checkers.InvalidTemplateArgumentsProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Invalid template argument\\")"}
+org.eclipse.cdt.codan.internal.checkers.LabelStatementNotFoundProblem=Error
+org.eclipse.cdt.codan.internal.checkers.LabelStatementNotFoundProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Label statement not found\\")"}
+org.eclipse.cdt.codan.internal.checkers.MemberDeclarationNotFoundProblem=Error
+org.eclipse.cdt.codan.internal.checkers.MemberDeclarationNotFoundProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Member declaration not found\\")"}
+org.eclipse.cdt.codan.internal.checkers.MethodResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.MethodResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Method cannot be resolved\\")"}
+org.eclipse.cdt.codan.internal.checkers.NamingConventionFunctionChecker=-Info
+org.eclipse.cdt.codan.internal.checkers.NamingConventionFunctionChecker.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Name convention for function\\")",pattern\=>"^[a-z]",macro\=>true,exceptions\=>()}
+org.eclipse.cdt.codan.internal.checkers.NonVirtualDestructorProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.NonVirtualDestructorProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Class has a virtual method and non-virtual destructor\\")"}
+org.eclipse.cdt.codan.internal.checkers.OverloadProblem=Error
+org.eclipse.cdt.codan.internal.checkers.OverloadProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Invalid overload\\")"}
+org.eclipse.cdt.codan.internal.checkers.RedeclarationProblem=Error
+org.eclipse.cdt.codan.internal.checkers.RedeclarationProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Invalid redeclaration\\")"}
+org.eclipse.cdt.codan.internal.checkers.RedefinitionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.RedefinitionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Invalid redefinition\\")"}
+org.eclipse.cdt.codan.internal.checkers.ReturnStyleProblem=-Warning
+org.eclipse.cdt.codan.internal.checkers.ReturnStyleProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Return with parenthesis\\")"}
+org.eclipse.cdt.codan.internal.checkers.ScanfFormatStringSecurityProblem=-Warning
+org.eclipse.cdt.codan.internal.checkers.ScanfFormatStringSecurityProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Format String Vulnerability\\")"}
+org.eclipse.cdt.codan.internal.checkers.StatementHasNoEffectProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.StatementHasNoEffectProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Statement has no effect\\")",macro\=>true,exceptions\=>()}
+org.eclipse.cdt.codan.internal.checkers.SuggestedParenthesisProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.SuggestedParenthesisProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Suggested parenthesis around expression\\")",paramNot\=>false}
+org.eclipse.cdt.codan.internal.checkers.SuspiciousSemicolonProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.SuspiciousSemicolonProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Suspicious semicolon\\")",else\=>false,afterelse\=>false}
+org.eclipse.cdt.codan.internal.checkers.TypeResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.TypeResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Type cannot be resolved\\")"}
+org.eclipse.cdt.codan.internal.checkers.UnusedFunctionDeclarationProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.UnusedFunctionDeclarationProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Unused function declaration\\")",macro\=>true}
+org.eclipse.cdt.codan.internal.checkers.UnusedStaticFunctionProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.UnusedStaticFunctionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Unused static function\\")",macro\=>true}
+org.eclipse.cdt.codan.internal.checkers.UnusedVariableDeclarationProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.UnusedVariableDeclarationProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Unused variable declaration in file scope\\")",macro\=>true,exceptions\=>("@(\#)","$Id")}
+org.eclipse.cdt.codan.internal.checkers.VariableResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.VariableResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Symbol is not resolved\\")"}
+org.eclipse.cdt.qt.core.qtproblem=Warning
+org.eclipse.cdt.qt.core.qtproblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>false,RUN_ON_INC_BUILD\=>false,RUN_ON_FILE_OPEN\=>true,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>null}
--- a/.settings/org.eclipse.cdt.ui.prefs
+++ b/.settings/org.eclipse.cdt.ui.prefs
@@ -0,0 +1,2 @@
+eclipse.preferences.version=1
+formatter_settings_version=1
--- a/.travis.yml
+++ b/.travis.yml
@@ -1,30 +1,23 @@
 language: cpp
-compiler: g++
+compiler: 
+    - g++

 install:
+    - if ["$CXX" = "g++" ]; then export CXX="g++-4.8"; fi

 addons:
  apt:
    sources:
    - ubuntu-toolchain-r-test
-    - george-edison55-precise-backports # cmake 3.2.3 / doxygen 1.8.3
    packages:
    - g++-4.8
    - cmake
    - cmake-data
+    - bluez
+    - libbluetooth-dev
    
 script: 
  - ./travis-build.sh
-
-  - cd MQTTSNGateway
-  - make CXX="g++-4.8" SENSORNET="xbee"
-  - make CXX="g++-4.8" SENSORNET="udp"  
-  - make CXX="g++-4.8" SENSORNET="udp6"
-  
-  - make test
-    
-  - cd GatewayTester
-  - make CXX="g++-4.8"
  
 notifications:
  emails:
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -14,7 +14,7 @@
 #     Ian Craggs - initial version
 #*******************************************************************************/

-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.1)
 PROJECT("paho-mqttsn" CXX)
 MESSAGE(STATUS "CMake version: " ${CMAKE_VERSION})
 MESSAGE(STATUS "CMake system name: " ${CMAKE_SYSTEM_NAME})
@@ -39,3 +39,4 @@ INCLUDE(CPack)
 ENABLE_TESTING()

 ADD_SUBDIRECTORY(MQTTSNPacket)
+ADD_SUBDIRECTORY(MQTTSNGateway)
--- a/CODE_OF_CONDUCT.md
+++ b/CODE_OF_CONDUCT.md
@@ -0,0 +1,94 @@
+# Community Code of Conduct
+
+**Version 2.0  
+January 1, 2023**
+
+## Our Pledge
+
+In the interest of fostering an open and welcoming environment, we as community members, contributors, Committers[^1], and Project Leads (collectively "Contributors") pledge to make participation in our projects and our community a harassment-free and inclusive experience for everyone.
+
+This Community Code of Conduct ("Code") outlines our behavior expectations as members of our community in all Eclipse Foundation activities, both offline and online. It is not intended to govern scenarios or behaviors outside of the scope of Eclipse Foundation activities. Nor is it intended to replace or supersede the protections offered to all our community members under the law. Please follow both the spirit and letter of this Code and encourage other Contributors to follow these principles into our work. Failure to read or acknowledge this Code does not excuse a Contributor from compliance with the Code.
+
+## Our Standards
+
+Examples of behavior that contribute to creating a positive and professional environment include:
+
+- Using welcoming and inclusive language;
+- Actively encouraging all voices;
+- Helping others bring their perspectives and listening actively. If you find yourself dominating a discussion, it is especially important to encourage other voices to join in;
+- Being respectful of differing viewpoints and experiences;
+- Gracefully accepting constructive criticism;
+- Focusing on what is best for the community;
+- Showing empathy towards other community members;
+- Being direct but professional; and
+- Leading by example by holding yourself and others accountable
+
+Examples of unacceptable behavior by Contributors include:
+
+- The use of sexualized language or imagery;
+- Unwelcome sexual attention or advances;
+- Trolling, insulting/derogatory comments, and personal or political attacks;
+- Public or private harassment, repeated harassment;
+- Publishing others' private information, such as a physical or electronic address, without explicit permission;
+- Violent threats or language directed against another person;
+- Sexist, racist, or otherwise discriminatory jokes and language;
+- Posting sexually explicit or violent material;
+- Sharing private content, such as emails sent privately or non-publicly, or unlogged forums such as IRC channel history;
+- Personal insults, especially those using racist or sexist terms;
+- Excessive or unnecessary profanity;
+- Advocating for, or encouraging, any of the above behavior; and
+- Other conduct which could reasonably be considered inappropriate in a professional setting
+
+## Our Responsibilities
+
+With the support of the Eclipse Foundation employees, consultants, officers, and directors (collectively, the "Staff"), Committers, and Project Leads, the Eclipse Foundation Conduct Committee (the "Conduct Committee") is responsible for clarifying the standards of acceptable behavior. The Conduct Committee takes appropriate and fair corrective action in response to any instances of unacceptable behavior.
+
+## Scope
+
+This Code applies within all Project, Working Group, and Interest Group spaces and communication channels of the Eclipse Foundation (collectively, "Eclipse spaces"), within any Eclipse-organized event or meeting, and in public spaces when an individual is representing an Eclipse Foundation Project, Working Group, Interest Group, or their communities. Examples of representing a Project or community include posting via an official social media account, personal accounts, or acting as an appointed representative at an online or offline event. Representation of Projects, Working Groups, and Interest Groups may be further defined and clarified by Committers, Project Leads, or the Staff.
+
+## Enforcement
+
+Instances of abusive, harassing, or otherwise unacceptable behavior may be reported by contacting the Conduct Committee via conduct@eclipse-foundation.org. All complaints will be reviewed and investigated and will result in a response that is deemed necessary and appropriate to the circumstances. Without the explicit consent of the reporter, the Conduct Committee is obligated to maintain confidentiality with regard to the reporter of an incident. The Conduct Committee is further obligated to ensure that the respondent is provided with sufficient information about the complaint to reply. If such details cannot be provided while maintaining confidentiality, the Conduct Committee will take the respondent‘s inability to provide a defense into account in its deliberations and decisions. Further details of enforcement guidelines may be posted separately.
+
+Staff, Committers and Project Leads have the right to report, remove, edit, or reject comments, commits, code, wiki edits, issues, and other contributions that are not aligned to this Code, or to block temporarily or permanently any Contributor for other behaviors that they deem inappropriate, threatening, offensive, or harmful. Any such actions will be reported to the Conduct Committee for transparency and record keeping.
+
+Any Staff (including officers and directors of the Eclipse Foundation), Committers, Project Leads, or Conduct Committee members who are the subject of a complaint to the Conduct Committee will be recused from the process of resolving any such complaint.
+
+## Responsibility
+
+The responsibility for administering this Code rests with the Conduct Committee, with oversight by the Executive Director and the Board of Directors. For additional information on the Conduct Committee and its process, please write to <conduct@eclipse-foundation.org>.
+
+## Investigation of Potential Code Violations
+
+All conflict is not bad as a healthy debate may sometimes be necessary to push us to do our best. It is, however, unacceptable to be disrespectful or offensive, or violate this Code. If you see someone engaging in objectionable behavior violating this Code, we encourage you to address the behavior directly with those involved. If for some reason, you are unable to resolve the matter or feel uncomfortable doing so, or if the behavior is threatening or harassing, please report it following the procedure laid out below.
+
+Reports should be directed to <conduct@eclipse-foundation.org>. It is the Conduct Committee’s role to receive and address reported violations of this Code and to ensure a fair and speedy resolution.
+
+The Eclipse Foundation takes all reports of potential Code violations seriously and is committed to confidentiality and a full investigation of all allegations. The identity of the reporter will be omitted from the details of the report supplied to the accused. Contributors who are being investigated for a potential Code violation will have an opportunity to be heard prior to any final determination. Those found to have violated the Code can seek reconsideration of the violation and disciplinary action decisions. Every effort will be made to have all matters disposed of within 60 days of the receipt of the complaint.
+
+## Actions
+Contributors who do not follow this Code in good faith may face temporary or permanent repercussions as determined by the Conduct Committee.
+
+This Code does not address all conduct. It works in conjunction with our [Communication Channel Guidelines](https://www.eclipse.org/org/documents/communication-channel-guidelines/), [Social Media Guidelines](https://www.eclipse.org/org/documents/social_media_guidelines.php), [Bylaws](https://www.eclipse.org/org/documents/eclipse-foundation-be-bylaws-en.pdf), and [Internal Rules](https://www.eclipse.org/org/documents/ef-be-internal-rules.pdf) which set out additional protections for, and obligations of, all contributors. The Foundation has additional policies that provide further guidance on other matters.
+
+It’s impossible to spell out every possible scenario that might be deemed a violation of this Code. Instead, we rely on one another’s good judgment to uphold a high standard of integrity within all Eclipse Spaces. Sometimes, identifying the right thing to do isn’t an easy call. In such a scenario, raise the issue as early as possible.
+
+## No Retaliation
+
+The Eclipse community relies upon and values the help of Contributors who identify potential problems that may need to be addressed within an Eclipse Space. Any retaliation against a Contributor who raises an issue honestly is a violation of this Code. That a Contributor has raised a concern honestly or participated in an investigation, cannot be the basis for any adverse action, including threats, harassment, or discrimination. If you work with someone who has raised a concern or provided information in an investigation, you should continue to treat the person with courtesy and respect. If you believe someone has retaliated against you, report the matter as described by this Code. Honest reporting does not mean that you have to be right when you raise a concern; you just have to believe that the information you are providing is accurate.
+
+False reporting, especially when intended to retaliate or exclude, is itself a violation of this Code and will not be accepted or tolerated.
+
+Everyone is encouraged to ask questions about this Code. Your feedback is welcome, and you will get a response within three business days. Write to <conduct@eclipse-foundation.org>.
+
+## Amendments
+
+The Eclipse Foundation Board of Directors may amend this Code from time to time and may vary the procedures it sets out where appropriate in a particular case.
+
+### Attribution
+
+This Code was inspired by the [Contributor Covenant](https://www.contributor-covenant.org/), version 1.4, available [here](https://www.contributor-covenant.org/version/1/4/code-of-conduct/).
+
+[^1]: Capitalized terms used herein without definition shall have the meanings assigned to them in the Bylaws.
+
--- a/CONTRIBUTING.md
+++ b/CONTRIBUTING.md
@@ -10,12 +10,12 @@ In order for your contribution to be accepted, it must comply with the Eclipse F

 Please read the [Eclipse Foundation policy on accepting contributions via Git](http://wiki.eclipse.org/Development_Resources/Contributing_via_Git).

-1. Sign the [Eclipse CLA](http://www.eclipse.org/legal/CLA.php)
+1. Sign the [Eclipse ECA](http://www.eclipse.org/legal/ECA.php)
  1. Register for an Eclipse Foundation User ID. You can register [here](https://dev.eclipse.org/site_login/createaccount.php).
-  2. Log into the [Projects Portal](https://projects.eclipse.org/), and click on the '[Eclipse CLA](https://projects.eclipse.org/user/sign/cla)' link.
+  2. Log into the [Eclipse projects forge](https://www.eclipse.org/contribute/cla), and click on 'Eclipse Contributor Agreement'.
 2. Go to your [account settings](https://dev.eclipse.org/site_login/myaccount.php#open_tab_accountsettings) and add your GitHub username to your account.
 3. Make sure that you _sign-off_ your Git commits in the following format:
-  ``` Signed-off-by: John Smith <johnsmith@nowhere.com> ``` This is usually at the bottom of the commit message. You can automate this by adding the '-s' flag when you make the commits. e.g.   ```git commit -s -m "Adding a cool feature"```
+  ``` Signed-off-by: Alex Smith <alexsmith@nowhere.com> ``` This is usually at the bottom of the commit message. You can automate this by adding the '-s' flag when you make the commits. e.g.   ```git commit -s -m "Adding a cool feature"```
 4. Ensure that the email address that you make your commits with is the same one you used to sign up to the Eclipse Foundation website with.

 ## Contributing a change
--- a/13
+++ b/13
@@ -0,0 +1,13 @@
+Eclipse Public License - v 2.0
+
+This program and the accompanying materials
+are made available under the terms of the Eclipse Public License v2.0
+and Eclipse Distribution License v1.0 which accompany this distribution.
+
+The Eclipse Public License is available at
+  https://www.eclipse.org/legal/epl-2.0/
+and the Eclipse Distribution License is available at
+  http://www.eclipse.org/org/documents/edl-v10.php.
+
+For an explanation of what dual-licensing means to you, see:
+https://www.eclipse.org/legal/eplfaq.php#DUALLIC
--- a/MQTTSNClient/src/MQTTSNClient.h
+++ b/MQTTSNClient/src/MQTTSNClient.h
@@ -390,7 +390,7 @@ int MQTTSN::Client<Network, Timer, MAX_PACKET_SIZE, b>::readPacket(Timer& timer)
    int lenlen = 0;
    int datalen = 0;

-    #define MQTTSN_MIN_PACKET_LENGTH 3
+    #define MQTTSN_MIN_PACKET_LENGTH 2
    // 1. read the packet, datagram style 
    if ((len = ipstack.read(readbuf, MAX_PACKET_SIZE, timer.left_ms())) < MQTTSN_MIN_PACKET_LENGTH)
        goto exit;
--- a/MQTTSNGateway/CMakeLists.txt
+++ b/MQTTSNGateway/CMakeLists.txt
@@ -0,0 +1,16 @@
+#*******************************************************************************
+#  Copyright (c) 2022 a1lu
+#
+#  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:
+#     a1lu - initial version
+#*******************************************************************************/
+ADD_SUBDIRECTORY(src)
--- a/MQTTSNGateway/Gateway
+++ b/MQTTSNGateway/Gateway
--- a/MQTTSNGateway/GatewayTester/Makefile
+++ b/MQTTSNGateway/GatewayTester/Makefile
@@ -1,15 +1,29 @@
-PROGNAME := MQTT-SNGatewayTester
-APPL := mainTest
+PROGTEST := MQTT-SNGatewayTester
+TESTAPPL := mainTest

+PRGPUB := MQTT-SNPub
+PUBAPPL := mainPub
+
+PRGSUB := MQTT-SNSub
+SUBAPPL := mainSub
+
+PRGQOS := MQTT-SNPubQoS-1
+QOSAPPL := mainPubQoS-1

 SRCDIR := samples
+SRCPUB := ClientPub
+SRCSUB := ClientSub
+SRCQOS := ClientPubQoS-1
 SUBDIR := src

 CPPSRCS :=  \
-$(SRCDIR)/$(APPL).cpp \
 $(SUBDIR)/LGwProxy.cpp \
 $(SUBDIR)/LMqttsnClient.cpp \
 $(SUBDIR)/LNetworkUdp.cpp \
+$(SUBDIR)/LNetworkUdp6.cpp \
+$(SUBDIR)/LNetworkRfcomm.cpp \
+$(SUBDIR)/LNetworkDtls.cpp \
+$(SUBDIR)/LNetworkDtls6.cpp \
 $(SUBDIR)/LPublishManager.cpp \
 $(SUBDIR)/LRegisterManager.cpp \
 $(SUBDIR)/LSubscribeManager.cpp \
@@ -19,51 +33,78 @@ $(SUBDIR)/LTopicTable.cpp \
 $(SUBDIR)/LScreen.cpp \
 $(SUBDIR)/Payload.cpp \
 $(SUBDIR)/Util.cpp \
-
+#$(SRCDIR)/$(TESTAPPL).cpp \
+#$(SRCDIR)/$(SRCPUB)/$(PUBAPPL).cpp \
+#$(SRCDIR)/$(SRCSUB)/$(SUBAPPL).cpp \

 CXX := g++ 
 CPPFLAGS += 

-INCLUDES += -I$(SUBDIR)
-DEFS :=
-LIBS +=
+INCLUDES += -I$(SUBDIR) -I/usr/local/opt/openssl/include
+DEF1 :=
+DEF2 :=
+DEFS := -D$(SN) $(DEF1) $(DEF2)
+LIBS += -L/usr/local/lib -L/usr/local/opt/openssl/lib
 LDFLAGS := 
 CXXFLAGS := -Wall -O3 -std=c++11
-LDADD := 
+LDADD := -lssl -lcrypto $(LDADDBLT)
 OUTDIR := Build

-PROG := $(OUTDIR)/$(PROGNAME)
+PROG := $(OUTDIR)/$(PROGTEST)
 OBJS := $(CPPSRCS:%.cpp=$(OUTDIR)/%.o)
 DEPS := $(CPPSRCS:%.cpp=$(OUTDIR)/%.d)

+PROGPUB := $(OUTDIR)/$(PRGPUB)
+PROGSUB := $(OUTDIR)/$(PRGSUB)
+PROGQOS := $(OUTDIR)/$(PRGQOS)

 .PHONY: install clean 

-all: $(PROG) 
+all: $(PROG) $(PROGPUB) $(PROGSUB) $(PROGQOS)



 -include $(DEPS)

-$(PROG): $(OBJS) $(OUTDIR)/$(SRCDIR)/$(APPL).o
-	$(CXX) $(LDFLAGS) -o $@ $^ $(LIBS) $(LDADD)
+$(PROG): $(OBJS) $(OUTDIR)/$(SRCDIR)/$(TESTAPPL).o
+	$(CXX) $(LDFLAGS) -o $(PROG) $(OUTDIR)/$(SRCDIR)/$(TESTAPPL).o $(OBJS) $(LIBS) $(LDADD)
+
+$(PROGPUB): $(OBJS) $(OUTDIR)/$(SRCDIR)/$(SRCPUB)/$(PUBAPPL).o
+	$(CXX) $(LDFLAGS) -o $(PROGPUB) $(OUTDIR)/$(SRCDIR)/$(SRCPUB)/$(PUBAPPL).o $(OBJS) $(LIBS) $(LDADD)
+
+$(PROGSUB): $(OBJS) $(OUTDIR)/$(SRCDIR)/$(SRCSUB)/$(SUBAPPL).o
+	$(CXX) $(LDFLAGS) -o $(PROGSUB) $(OUTDIR)/$(SRCDIR)/$(SRCSUB)/$(SUBAPPL).o $(OBJS) $(LIBS) $(LDADD)
+
+$(PROGQOS): $(OBJS) $(OUTDIR)/$(SRCDIR)/$(SRCQOS)/$(QOSAPPL).o
+	$(CXX) $(LDFLAGS) -o $(PROGQOS) $(OUTDIR)/$(SRCDIR)/$(SRCQOS)/$(QOSAPPL).o $(OBJS) $(LIBS) $(LDADD)


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

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

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

 clean:
 	rm -rf $(OUTDIR)

 install:
 	cp -pf $(PROG) ../../../
-
-	
-	
+	cp -pf $(PROGPUB) ../../../
+	cp -pf $(PROGSUB) ../../../	
+	cp -pf $(PROGQOS) ../../../	
+	
--- a/MQTTSNGateway/GatewayTester/README.md
+++ b/MQTTSNGateway/GatewayTester/README.md
@@ -1,119 +1,117 @@
-###Gateway Test Program.    
+# Gateway Test Program.    
 **sample/mainTest.cpp** is a Test sample coading.   
 Each test is described as one function. test1(), test2()...     
-````
-/*------------------------------------------------------
- *    Test functions
- *
- *    you can use 4 commands in Test functions
- *
- *    1) PUBLISH(const char* topicName,
- *               uint8_t*    payload,
- *               uint16_t    len,
- *               uint8_t     qos,
- *               bool        retain = false);
- *
- *    2) SUBSCRIBE(const char*   topicName,
- *                 TopicCallback onPublish,
- *                 uint8_t qos);
- *
- *    3) UNSUBSCRIBE(const char* topicName);
- *
- *    4) DISCONNECT(uint16_t sleepInSecs);
- *
- *------------------------------------------------------*/
-
+```
 void test1(void)
 {
-	char payload[300];
-	sprintf(payload, "ESP8266-08b133 ");
-	uint8_t qos = 0;
-	PUBLISH(topic1,(uint8_t*)payload, strlen(payload), qos);
+    char payload[300];
+    sprintf(payload, "ESP8266-08b133 ");
+    uint8_t qos = 0;
+    PUBLISH(topic1,(uint8_t*)payload, strlen(payload), qos);
 }

 void test2(void)
 {
-	uint8_t qos = 1;
-	SUBSCRIBE(topic2, on_publish02, qos);
+    uint8_t qos = 1;
+    SUBSCRIBE(topic2, on_publish02, qos);
 }
-````
-**TEST_LIST** is a test senario. Test functions are executed one by one. 
-```` 
+
+ *---------------------------------------------------------------------------
+ *
+ *   MQTT-SN GATEWAY TEST CLIENT
+ *
+ *   Supported functions.
+ *
+ *   void PUBLISH  ( const char* topicName, uint8_t* payload, uint16_t len, uint8_t qos, bool retain = false );
+ *
+ *   void PUBLISH  ( uint16_t topicId, uint8_t* payload, uint16_t len, uint8_t qos, bool retain = false );
+ *
+ *   void SUBSCRIBE ( const char* topicName, TopicCallback onPublish, uint8_t qos );
+ *
+ *   void SUBSCRIBE ( uint16_t topicId, TopicCallback onPublish, uint8_t qos );
+ *
+ *   void UNSUBSCRIBE ( const char* topicName );
+ *
+ *   void UNSUBSCRIBE ( uint16_t topicId );
+ *
+ *   void DISCONNECT ( uint16_t sleepInSecs );
+ *
+ *   void CONNECT ( void );
+ *
+ *   void DISPLAY( format, .....);    <== instead of printf()
+ *--------------------------------------------------------------------------
+ 
+```
+**TEST_LIST** is a test senario. Test functions are executed interactively. 
+``` 
 /*------------------------------------------------------
 *    A List of Test Tasks
 *------------------------------------------------------*/

 TEST_LIST = {// e.g. TEST( Label, Test),
-			 TEST("Publish topic1",     test1),
-			 TEST("Subscribe topic2",   test2),
-			 TEST("Publish topic2",     test3),
-			 TEST("Unsubscribe topic2", test4),
-			 TEST("Publish topic2",     test3),
-			 TEST("Disconnect",         test5),
-			 END_OF_TEST_LIST
-			};
-```` 
+             TEST("Publish topic1",     test1),
+             TEST("Subscribe topic2",   test2),
+             TEST("Publish topic2",     test3),
+             TEST("Unsubscribe topic2", test4),
+             TEST("Publish topic2",     test3),
+             TEST("Disconnect",         test5),
+             END_OF_TEST_LIST
+        };
+        
+``` 

-### **step1. Build **   
-````
-$ git clone https://github.com/eclipse/paho.mqtt-sn.embedded-c 
+**UDP**, **DTLS**, **UDP6**, **DTLS6** or **Bluetooth** is available as a sensor network.
+```
+/*------------------------------------------------------
+ *    UDP, DTLS Configuration    (theNetcon)
+ *------------------------------------------------------*/
+UDPCONF = { "GatewayTestClient",  // ClientId
+		{ 225, 1, 1, 1 },         // Multicast group IP
+        1883,                     // Multicast group Port
+        20020,                    // Local PortNo
+        };
+
+/*------------------------------------------------------
+ *    UDP6, DTLS6 Configuration    (theNetcon)
+ *------------------------------------------------------*/
+UDP6CONF = { "GatewayTestClient",  // ClientId
+        "ff12::feed:caca:dead",    // Multicast group IP
+        "wlp4s0",
+        1883,                      // Multicast group Port
+        20020,                     // Local PortNo
+        };
+
+/*------------------------------------------------------
+ *    RFCOMM Configuration    (theNetcon)
+ *------------------------------------------------------*/
+RFCOMMCONF = { "GatewayTestClient",      // ClientId
+        "60:57:18:06:8B:72",          // GW Address
+        1,                            // Rfcomm channel
+        };
+```
+
+
+## How to Build    
+```
+copy codes from the github.
 $ cd paho.mqtt-sn.embedded-c/MQTTSNGateway/GatewayTester       
-$ make   
-$ make install   
-$ make clean
+$ ./build.sh [udp | udp6 | dtls | dtls6 | rfcomm]
 ```       
-MQTT-SNGatewayTester program is copied into ../../../ directory.

    
-### **step2. Execute Gateway Tester.**     
-
-````    
-$ cd ../../..   
-$ ./MQTT-SNGatewayTester
+## Execute Gateway Tester
+```    
+$ ./Build/MQTT-SNGatewayTester
  
 ***************************************************************************
- * MQTT-SN Gateway Tester
+ * MQTT-SN Gateway Tester   DTLS
 * Part of Project Paho in Eclipse
 * (http://git.eclipse.org/c/paho/org.eclipse.paho.mqtt-sn.embedded-c.git/)
 *
 * Author : Tomoaki YAMAGUCHI
- * Version: 0.0.0
+ * Version: 2.0.0
 ***************************************************************************
-
 Attempting to Connect the Broker.....
+Execute "Step0:Connect" ? ( y/n ) : 

-sendto 225.1.1.1      :1883   03 01 00
-
-recved 192.168.11.5   :1883   03 01 00
-sendto 225.1.1.1      :1883   03 01 00
-
-recved 192.168.11.5   :1883   03 01 00
-
-recved 192.168.11.17  :10000  03 02 01
-sendto 192.168.11.17  :10000  13 04 0c 01 03 84 47 61 74 65 77 61 79 54 65 73 74 65 72
-
-recved 192.168.11.17  :10000  02 06
-sendto 192.168.11.17  :10000  0c 07 00 77 69 6c 6c 54 6f 70 69 63
-
-recved 192.168.11.17  :10000  02 08
-sendto 192.168.11.17  :10000  0d 09 77 69 6c 6c 4d 65 73 73 61 67 65
-
-recved 192.168.11.17  :10000  03 05 00
-
-
- Connected to the Broker
-
- Attempting OnConnect.....
-sendto 192.168.11.17  :10000  13 12 20 00 01 74 79 34 74 77 2f 63 6c 69 65 6e 74 49 64
-
-recved 192.168.11.17  :10000  08 13 20 00 01 00 01 00
-
-
- SUBSCRIBE complete. ty4tw/clientId
-
- OnConnect complete
- Test Ready.
-
-Execute Publish topic1 Test ? ( Y/N ) :  
-
-````    
+```    
--- a/MQTTSNGateway/GatewayTester/build.sh
+++ b/MQTTSNGateway/GatewayTester/build.sh
@@ -0,0 +1,23 @@
+#!/bin/bash
+
+DEF1="DEF1=${2}"
+DEF2="DEF2=${3}"
+
+if [ $1 == "udp" ] ; then
+    make SN=UDP $DEF1 $DEF2
+elif [ $1 == "udp6" ] ; then 
+    make SN=UDP6 $DEF1 $DEF2
+elif [ $1 == "rfcomm" ] ; then 
+	export LDADDBLT=-lbluetooth
+    make SN=RFCOMM $DEF1 $DEF2
+elif [ $1 == "dtls" ] ; then
+    make SN=DTLS $DEF1 $DEF2
+elif [ $1 == "dtls6" ] ; then
+    make SN=DTLS6 $DEF1 $DEF2
+elif [ $1 == "clean" ] ; then
+    make clean
+else
+    echo "Usage: build.sh  [ udp | udp6 | rfcomm | dtls | dtls6 | clean]"
+fi
+
+
--- a/MQTTSNGateway/GatewayTester/samples/ClientPub/mainPub.cpp
+++ b/MQTTSNGateway/GatewayTester/samples/ClientPub/mainPub.cpp
@@ -0,0 +1,183 @@
+/****************************************************************************
+ * Copyright (c) 2016, 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.
+ *
+ *---------------------------------------------------------------------------
+ *
+ *   MQTT-SN GATEWAY TEST CLIENT
+ *
+ *   Supported functions.
+ *
+ *   void PUBLISH  ( const char* topicName, uint8_t* payload, uint16_t len, uint8_t qos, bool retain = false );
+ *
+ *   void PUBLISH  ( uint16_t topicId, uint8_t* payload, uint16_t len, uint8_t qos, bool retain = false );
+ *
+ *  void SUBSCRIBE ( const char* topicName, TopicCallback onPublish, uint8_t qos );
+ *
+ *  void SUBSCRIBE ( uint16_t topicId, TopicCallback onPublish, uint8_t qos );
+ *
+ *  void UNSUBSCRIBE ( const char* topicName );
+ *
+ *  void UNSUBSCRIBE ( uint16_t topicId );
+ *
+ *   void DISCONNECT ( uint16_t sleepInSecs );
+ *
+ *   void CONNECT ( void );
+ *
+ *   void DISPLAY( format, .....);    <== instead of printf()
+ *
+ *
+ * Contributors:
+ *    Tomoaki Yamaguchi - initial API and implementation
+ ***************************************************************************/
+
+#include "LMqttsnClientApp.h"
+#include "LMqttsnClient.h"
+#include "LScreen.h"
+
+using namespace std;
+using namespace linuxAsyncClient;
+extern LMqttsnClient* theClient;
+extern LScreen* theScreen;
+
+/*------------------------------------------------------
+ *    UDP Configuration    (theNetcon)
+ *------------------------------------------------------*/
+UDPCONF = { "ClientPUB", // ClientId
+        { 225, 1, 1, 1 },         // Multicast group IP
+        1883,                // Multicast group Port
+        20010,               // Local PortNo
+        };
+
+/*------------------------------------------------------
+ *    UDP6 Configuration    (theNetcon)
+ *------------------------------------------------------*/
+UDP6CONF = { "ClientPUB",  // ClientId
+        "ff1e:feed:caca:dead::1", // Multicast group IP
+        "wlp4s0",                 // Network Interface
+        1883,                     // Multicast group Port
+        20020,                    // Local PortNo
+        };
+
+/*------------------------------------------------------
+ *    RFCOMM Configuration    (theNetcon)
+ *------------------------------------------------------*/
+RFCOMMCONF = { "ClientPUB",      // ClientId
+        "60:57:18:06:8B:72",          // GW Address
+        1,                            // Rfcomm channel
+        };
+
+/*------------------------------------------------------
+ *    Client Configuration  (theMqcon)
+ *------------------------------------------------------*/
+MQTTSNCONF = {
+    300,            //KeepAlive [seconds]
+    true,          //Clean session
+    300,           //Sleep duration [seconds]
+    "",            //WillTopic
+    "",            //WillMessage
+    0,             //WillQos
+    false          //WillRetain
+};
+
+/*------------------------------------------------------
+ *     Define Topics
+ *------------------------------------------------------*/
+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
+ *------------------------------------------------------*/
+
+/*------------------------------------------------------
+ *      A Link list of Callback routines and Topics
+ *------------------------------------------------------*/
+
+SUBSCRIBE_LIST = {// e.g. SUB(topic, callback, QoS),
+                  END_OF_SUBSCRIBE_LIST
+                 };
+
+
+/*------------------------------------------------------
+ *    Test functions
+ *------------------------------------------------------*/
+
+void publishTopic1(void)
+{
+    char payload[300];
+    sprintf(payload, "publish \"ty4tw/Topic1\" \n");
+    uint8_t qos = 0;
+    PUBLISH(topic1,(uint8_t*)payload, strlen(payload), qos);
+}
+
+void publishTopic2(void)
+{
+    char payload[300];
+    sprintf(payload, "publish \"ty4tw/topic2\" \n");
+    uint8_t qos = 0;
+    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(topic57, (uint8_t* )payload, strlen(payload), qos);
+}
+
+
+void disconnect(void)
+{
+    DISCONNECT(0);
+}
+
+
+/*------------------------------------------------------
+ *    A List of Test functions is valid in case of
+ *    line 23 of LMqttsnClientApp.h is commented out.
+ *    //#define CLIENT_MODE
+ *------------------------------------------------------*/
+
+TEST_LIST = {// e.g. TEST( Label, Test),
+             TEST("Step1:Publish topic1",     publishTopic1),
+             TEST("Step2:Publish topic57",     publishTopic57),
+             TEST("Step3:Publish topic2",     publishTopic2),
+             TEST("Step4:Disconnect",        disconnect),
+             END_OF_TEST_LIST
+            };
+
+
+/*------------------------------------------------------
+ *    List of tasks is valid in case of line23 of
+ *    LMqttsnClientApp.h is uncommented.
+ *    #define CLIENT_MODE
+ *------------------------------------------------------*/
+TASK_LIST = {// e.g. TASK( task, executing duration in second),
+            TASK(publishTopic1, 4),  // publishTopic1() is executed every 4 seconds
+            TASK(publishTopic2, 7),  // publishTopic2() is executed every 7 seconds
+             END_OF_TASK_LIST
+            };
+
+
+/*------------------------------------------------------
+ *    Initialize function
+ *------------------------------------------------------*/
+void setup(void)
+{
+    SetForwarderMode(false);
+}
+
+
+/*****************  END OF  PROGRAM ********************/
--- a/MQTTSNGateway/GatewayTester/samples/ClientPubQoS-1/mainPubQoS-1.cpp
+++ b/MQTTSNGateway/GatewayTester/samples/ClientPubQoS-1/mainPubQoS-1.cpp
@@ -0,0 +1,178 @@
+/****************************************************************************
+ * Copyright (c) 2016, 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.
+ *
+ *---------------------------------------------------------------------------
+ *
+ *   MQTT-SN GATEWAY TEST CLIENT
+ *
+ *   Supported functions.
+ *
+ *   void PUBLISH  ( const char* topicName, uint8_t* payload, uint16_t len, uint8_t qos, bool retain = false );
+ *
+ *   void PUBLISH  ( uint16_t topicId, uint8_t* payload, uint16_t len, uint8_t qos, bool retain = false );
+ *
+ *  void SUBSCRIBE ( const char* topicName, TopicCallback onPublish, uint8_t qos );
+ *
+ *  void SUBSCRIBE ( uint16_t topicId, TopicCallback onPublish, uint8_t qos );
+ *
+ *  void UNSUBSCRIBE ( const char* topicName );
+ *
+ *  void UNSUBSCRIBE ( uint16_t topicId );
+ *
+ *   void DISCONNECT ( uint16_t sleepInSecs );
+ *
+ *   void CONNECT ( void );
+ *
+ *   void DISPLAY( format, .....);    <== instead of printf()
+ *
+ *
+ * Contributors:
+ *    Tomoaki Yamaguchi - initial API and implementation
+ ***************************************************************************/
+
+#include "LMqttsnClientApp.h"
+#include "LMqttsnClient.h"
+#include "LScreen.h"
+
+using namespace std;
+using namespace linuxAsyncClient;
+extern LMqttsnClient* theClient;
+extern LScreen* theScreen;
+
+/*------------------------------------------------------
+ *    UDP Configuration    (theNetcon)
+ *------------------------------------------------------*/
+UDPCONF = { "QoS-1_Client01", // ClientId
+        { 225, 1, 1, 1 },         // Multicast group IP
+        1883,                // Multicast group Port
+        20001,               // Local PortNo
+        };
+
+/*------------------------------------------------------
+ *    UDP6 Configuration    (theNetcon)
+ *------------------------------------------------------*/
+UDP6CONF = { "QoS-1_Client01",  // ClientId
+        "ff1e:feed:caca:dead::1", // Multicast group IP
+        "wlp4s0",                 // Network Interface
+        1883,                     // Multicast group Port
+        20020,                    // Local PortNo
+        };
+
+/*------------------------------------------------------
+ *    RFCOMM Configuration    (theNetcon)
+ *------------------------------------------------------*/
+RFCOMMCONF = { "QoS-1_Client01",      // ClientId
+        "60:57:18:06:8B:72",          // GW Address
+        1,                            // Rfcomm channel
+        };
+
+/*------------------------------------------------------
+ *    Client Configuration  (theMqcon)
+ *------------------------------------------------------*/
+MQTTSNCONF = {
+    300,            //KeepAlive [seconds]
+    true,          //Clean session
+    300,           //Sleep duration [seconds]
+    "",            //WillTopic
+    "",            //WillMessage
+    0,             //WillQos
+    false          //WillRetain
+};
+
+/*------------------------------------------------------
+ *     Define Topics
+ *------------------------------------------------------*/
+
+
+/*------------------------------------------------------
+ *       Callback routines for Subscribed Topics
+ *------------------------------------------------------*/
+
+/*------------------------------------------------------
+ *      A Link list of Callback routines and Topics
+ *------------------------------------------------------*/
+SUBSCRIBE_LIST = {// e.g. SUB(TopicType, topicName, TopicId, callback, QoSx),
+
+                  END_OF_SUBSCRIBE_LIST
+                 };
+
+/*------------------------------------------------------
+ *    Test functions
+ *------------------------------------------------------*/
+
+void publishTopic1(void)
+{
+    char payload[300];
+    sprintf(payload, "publish \"ty4tw/Topic1\" \n");
+    uint8_t qos = 3;
+    PUBLISH(1,(uint8_t*)payload, strlen(payload), qos);
+}
+
+void publishTopic2(void)
+{
+    char payload[300];
+    sprintf(payload, "publish \"ty4tw/topic2\" \n");
+    uint8_t qos = 3;
+    PUBLISH(2,(uint8_t*)payload, strlen(payload), qos);
+}
+
+void publishTopic57(void)
+{
+    char payload[300];
+    sprintf(payload, "publish \"ty4tw/topic57\" \n");
+    uint8_t qos = 3;
+    PUBLISH(5,(uint8_t*)payload, strlen(payload), qos);
+}
+
+
+void disconnect(void)
+{
+    DISCONNECT(0);
+}
+
+
+/*------------------------------------------------------
+ *    A List of Test functions is valid in case of
+ *    line 23 of LMqttsnClientApp.h is commented out.
+ *    //#define CLIENT_MODE
+ *------------------------------------------------------*/
+
+TEST_LIST = {// e.g. TEST( Label, Test),
+             TEST("Step1:Publish topic1",     publishTopic1),
+             TEST("Step2:Publish topic57",     publishTopic57),
+             TEST("Step3:Publish topic2",     publishTopic2),
+             END_OF_TEST_LIST
+            };
+
+
+/*------------------------------------------------------
+ *    List of tasks is invalid in case of line23 of
+ *    LMqttsnClientApp.h is commented out.
+ *    #define CLIENT_MODE
+ *------------------------------------------------------*/
+TASK_LIST = {// e.g. TASK( task, executing duration in second),
+            TASK(publishTopic1, 4),  // publishTopic1() is executed every 4 seconds
+            TASK(publishTopic2, 7),  // publishTopic2() is executed every 7 seconds
+             END_OF_TASK_LIST
+            };
+
+
+/*------------------------------------------------------
+ *    Initialize function
+ *------------------------------------------------------*/
+void setup(void)
+{
+    SetQoSMinus1Mode(true);
+}
+
+
+/*****************  END OF  PROGRAM ********************/
--- a/MQTTSNGateway/GatewayTester/samples/ClientSub/mainSub.cpp
+++ b/MQTTSNGateway/GatewayTester/samples/ClientSub/mainSub.cpp
@@ -0,0 +1,224 @@
+/****************************************************************************
+ * Copyright (c) 2016, 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.
+ *
+ *---------------------------------------------------------------------------
+ *
+ *   MQTT-SN GATEWAY TEST CLIENT
+ *
+ *   Supported functions.
+ *
+ *   void PUBLISH  ( const char* topicName, uint8_t* payload, uint16_t len, uint8_t qos, bool retain = false );
+ *
+ *   void PUBLISH  ( uint16_t topicId, uint8_t* payload, uint16_t len, uint8_t qos, bool retain = false );
+ *
+ *  void SUBSCRIBE ( const char* topicName, TopicCallback onPublish, uint8_t qos );
+ *
+ *  void SUBSCRIBE ( uint16_t topicId, TopicCallback onPublish, uint8_t qos );
+ *
+ *  void UNSUBSCRIBE ( const char* topicName );
+ *
+ *  void UNSUBSCRIBE ( uint16_t topicId );
+ *
+ *   void DISCONNECT ( uint16_t sleepInSecs );
+ *
+ *   void CONNECT ( void );
+ *
+ *   void DISPLAY( format, .....);    <== instead of printf()
+ *
+ *
+ * Contributors:
+ *    Tomoaki Yamaguchi - initial API and implementation
+ ***************************************************************************/
+
+#include "LMqttsnClientApp.h"
+#include "LMqttsnClient.h"
+#include "LScreen.h"
+
+using namespace std;
+using namespace linuxAsyncClient;
+extern LMqttsnClient* theClient;
+extern LScreen* theScreen;
+
+/*------------------------------------------------------
+ *    UDP Configuration    (theNetcon)
+ *------------------------------------------------------*/
+UDPCONF = { "ClientSUB", // ClientId
+        { 225, 1, 1, 1 },         // Multicast group IP
+        1883,                // Multicast group Port
+        20011,               // Local PortNo
+        };
+
+/*------------------------------------------------------
+ *    UDP6 Configuration    (theNetcon)
+ *------------------------------------------------------*/
+UDP6CONF = { "ClientSUB",  // ClientId
+        "ff1e:feed:caca:dead::1", // Multicast group IP
+        "wlp4s0",                 // Network Interface
+        1883,                     // Multicast group Port
+        20020,                    // Local PortNo
+        };
+
+/*------------------------------------------------------
+ *    RFCOMM Configuration    (theNetcon)
+ *------------------------------------------------------*/
+RFCOMMCONF = { "ClientSUB",      // ClientId
+        "60:57:18:06:8B:72",          // GW Address
+        1,                            // Rfcomm channel
+        };
+
+/*------------------------------------------------------
+ *    Client Configuration  (theMqcon)
+ *------------------------------------------------------*/
+MQTTSNCONF = {
+	 60,            //KeepAlive [seconds]
+	true,          //Clean session
+	300,           //Sleep duration [seconds]
+	"",            //WillTopic
+	"",            //WillMessage
+    0,             //WillQos
+    false          //WillRetain
+};
+
+/*------------------------------------------------------
+ *     Define Topics
+ *------------------------------------------------------*/
+const char* topic1 = "ty4tw/topic1";
+const char* topic2 = "ty4tw/topic2";
+const char* topic3 = "ty4tw/topic3";
+const char* topic4 = "a";
+const char* topic5 = "ty4tw/#";
+
+
+/*------------------------------------------------------
+ *       Callback routines for Subscribed Topics
+ *------------------------------------------------------*/
+int on_Topic01(uint8_t* pload, uint16_t ploadlen)
+{
+	DISPLAY("\n\nTopic1 recv.\n");
+	char c = pload[ploadlen-1];
+	pload[ploadlen-1]= 0;   // set null terminator
+	DISPLAY("Payload -->%s%c<--\n\n",pload, c);
+	return 0;
+}
+
+int on_Topic02(uint8_t* pload, uint16_t ploadlen)
+{
+	DISPLAY("\n\nTopic2 recv.\n");
+	pload[ploadlen-1]= 0;   // set null terminator
+	DISPLAY("Payload -->%s　<--\n\n",pload);
+	return 0;
+}
+
+int on_Topic03(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;
+}
+
+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, topic5, 0, on_Topic05, QoS1),
+                  //SUB(MQTTSN_TOPIC_TYPE_NORMAL, topic2, 0, on_Topic02, QoS1),
+                  END_OF_SUBSCRIBE_LIST
+                 };
+
+
+
+/*------------------------------------------------------
+ *    Test functions
+ *------------------------------------------------------*/
+
+
+void subscribeTopic1(void)
+{
+    uint8_t qos = 1;
+    SUBSCRIBE(topic1, on_Topic01, qos);
+}
+
+void subscribeTopic2(void)
+{
+	uint8_t qos = 1;
+	SUBSCRIBE(topic2, on_Topic02, qos);
+}
+
+void subscribeTopic5(void)
+{
+    uint8_t qos = 1;
+    SUBSCRIBE(topic5, on_Topic05, qos);
+}
+
+void disconnect(void)
+{
+	DISCONNECT(0);
+}
+
+void connect(void)
+{
+	CONNECT();
+}
+
+void asleep(void)
+{
+	DISCONNECT(theMqcon.sleepDuration);
+}
+
+/*------------------------------------------------------
+ *    A List of Test functions is valid in case of
+ *    line 23 of LMqttsnClientApp.h is commented out.
+ *    //#define CLIENT_MODE
+ *------------------------------------------------------*/
+
+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
+			};
+
+
+/*------------------------------------------------------
+ *    List of tasks is valid in case of line23 of
+ *    LMqttsnClientApp.h is uncommented.
+ *    #define CLIENT_MODE
+ *------------------------------------------------------*/
+TASK_LIST = {// e.g. TASK( task, executing duration in second),
+             END_OF_TASK_LIST
+            };
+
+
+/*------------------------------------------------------
+ *    Initialize function
+ *------------------------------------------------------*/
+void setup(void)
+{
+    SetForwarderMode(false);
+}
+
+
+/*****************  END OF  PROGRAM ********************/
+
--- a/MQTTSNGateway/GatewayTester/samples/mainTemplate.cpp
+++ b/MQTTSNGateway/GatewayTester/samples/mainTemplate.cpp
@@ -1,162 +0,0 @@
-/**************************************************************************************
- * Copyright (c) 2016, 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 
- **************************************************************************************/
-
-#include "LMqttsnClientApp.h"
-#include "LMqttsnClient.h"
-#include "LScreen.h"
-
-using namespace std;
-using namespace linuxAsyncClient;
-extern LMqttsnClient* theClient;
-extern LScreen* theScreen;
-extern int run(void);
-
-/*
- *   MQTT-SN Functions supported :
- *
- *   void PUBLISH    ( const char* topicName, uint8_t* payload,
- *                     uint16_t len, uint8_t qos, bool retain = false );
- *
- *   void PUBLISH    ( uint16_t topicId, uint8_t* payload,
- *                     uint16_t len, uint8_t qos, bool retain = false );
- *
- *   void SUBSCRIBE  ( const char* topicName, TopicCallback onPublish,
- *                      uint8_t qos );
- *
- *   void UNSUBSCRIBE( const char* topicName );
- *
- *   void DISCONNECT ( uint16_t sleepInSecs );
- *
- *   void DISPLAY( format, valiables, .....);    <== instead of printf()
- *
- */
-
-/*------------------------------------------------------
- *    UDP Configuration
- *------------------------------------------------------*/
-UDPCONF  = {
-	"GatewayTester",     // ClientId
-	{225,1,1,1},         // Multicast group IP
-	1883,                // Multicast group Port
-	20000,               // Local PortNo
-};
-
-/*------------------------------------------------------
- *    Client Configuration
- *------------------------------------------------------*/
-MQTTSNCONF = {
-	60,             //KeepAlive (seconds)
-	true,           //Clean session
-	0,              //Sleep duration in msecs
-	"willTopic",    //WillTopic
-	"willMessage",  //WillMessage
-    0,              //WillQos
-    false           //WillRetain
-};
-
-/*------------------------------------------------------
- *     Define Topics
- *------------------------------------------------------*/
-const char* topic1 = "ty4tw/clientId";
-
-/*------------------------------------------------------
- *       Callback routines for Subscribed Topics
- *------------------------------------------------------*/
-int on_publish01(uint8_t* pload, uint16_t ploadlen)
-{
-	return 0;
-}
-
-
-/*------------------------------------------------------
- *      A Link list of Callback routines and Topics
- *------------------------------------------------------*/
-
-SUBSCRIBE_LIST = {// e.g. SUB(topic, callback, QoS),
-				  //SUB(topic1, on_publish01, 1),
-				  END_OF_SUBSCRIBE_LIST
-				 };
-
-/*------------------------------------------------------
- *    Test functions
- *------------------------------------------------------*/
-void test1(void)
-{
-	char payload[300];
-	sprintf(payload, "Client-01 ");
-	uint8_t qos = 0;
-	PUBLISH(topic1,(uint8_t*)payload, strlen(payload), qos);
-}
-
-void test2(void)
-{
-
-}
-
-void test3(void)
-{
-
-}
-
-void test4(void)
-{
-
-}
-
-void test5(void)
-{
-
-}
-
-/*------------------------------------------------------
- *    A List of Test functions
- *------------------------------------------------------*/
-
-TEST_LIST = {// e.g. TEST( Label, Test),
-			 TEST("Publish topic1",     test1),
-			 END_OF_TEST_LIST
-			};
-
-
-/*------------------------------------------------------
- *    unused for Test
- *------------------------------------------------------*/
-TASK_LIST = {// e.g. TASK( task, executing duration in second),
-			TASK(test1, 4),
-             END_OF_TASK_LIST
-            };
-
-
-/*------------------------------------------------------
- *    Initialize function
- *------------------------------------------------------*/
-void setup(void)
-{
-
-}
-
-/*======================================================
- *    main
- *======================================================*/
-
-/*   uncomment this
-
-int main(int argc, char** argv)
-{
-	return run();
-}
-
-*/
--- a/MQTTSNGateway/GatewayTester/samples/mainTest.cpp
+++ b/MQTTSNGateway/GatewayTester/samples/mainTest.cpp
@@ -1,4 +1,4 @@
-/**************************************************************************************
+/****************************************************************************
 * Copyright (c) 2016, Tomoaki Yamaguchi
 *
 * All rights reserved. This program and the accompanying materials
@@ -10,9 +10,34 @@
 * and the Eclipse Distribution License is available at
 *   http://www.eclipse.org/org/documents/edl-v10.php.
 *
+ *---------------------------------------------------------------------------
+ *
+ *   MQTT-SN GATEWAY TEST CLIENT
+ *
+ *   Supported functions.
+ *
+ *   void PUBLISH  ( const char* topicName, uint8_t* payload, uint16_t len, uint8_t qos, bool retain = false );
+ *
+ *   void PUBLISH  ( uint16_t topicId, uint8_t* payload, uint16_t len, uint8_t qos, bool retain = false );
+ *
+ *   void SUBSCRIBE ( const char* topicName, TopicCallback onPublish, uint8_t qos );
+ *
+ *   void SUBSCRIBE ( uint16_t topicId, TopicCallback onPublish, uint8_t qos );
+ *
+ *   void UNSUBSCRIBE ( const char* topicName );
+ *
+ *   void UNSUBSCRIBE ( uint16_t topicId );
+ *
+ *   void DISCONNECT ( uint16_t sleepInSecs );
+ *
+ *   void CONNECT ( void );
+ *
+ *   void DISPLAY( format, .....);    <== instead of printf()
+ *
+ *
 * Contributors:
- *    Tomoaki Yamaguchi - initial API and implementation 
- **************************************************************************************/
+ *    Tomoaki Yamaguchi - initial API and implementation
+ ***************************************************************************/

 #include "LMqttsnClientApp.h"
 #include "LMqttsnClient.h"
@@ -22,49 +47,45 @@ using namespace std;
 using namespace linuxAsyncClient;
 extern LMqttsnClient* theClient;
 extern LScreen* theScreen;
-extern int run(void);
-
-/*
- *   Functions supported.
- *
- *   void PUBLISH    ( const char* topicName, uint8_t* payload,
- *                     uint16_t len, uint8_t qos, bool retain = false );
- *
- *   void PUBLISH    ( uint16_t topicId, uint8_t* payload,
- *                     uint16_t len, uint8_t qos, bool retain = false );
- *
- *   void SUBSCRIBE  ( const char* topicName, TopicCallback onPublish,
- *                      uint8_t qos );
- *
- *   void UNSUBSCRIBE( const char* topicName );
- *
- *   void DISCONNECT ( uint16_t sleepInSecs );
- *
- *   void DISPLAY( format, .....);    <== instead of printf()
- *
- */
-/*------------------------------------------------------
- *    UDP Configuration
- *------------------------------------------------------*/
-UDPCONF  = {
-	"GatewayTester",     // ClientId
-	{225,1,1,1},         // Multicast group IP
-	1883,                // Multicast group Port
-	20001,               // Local PortNo
-};

 /*------------------------------------------------------
- *    Client Configuration
+ *    UDP,DTLS Configuration    (theNetcon)
 *------------------------------------------------------*/
-MQTTSNCONF = {
-	300,            //KeepAlive (seconds)
-	true,           //Clean session
-	0,              //Sleep duration in msecs
-	"willTopic",    //WillTopic
-	"willMessage",  //WillMessage
-    0,              //WillQos
-    false           //WillRetain
-};
+UDPCONF = { "GatewayTestClient",  // ClientId
+        { 225, 1, 1, 1 },         // Multicast group IP
+        1883,                     // Multicast group Port
+        20020,                    // Local PortNo
+        };
+
+/*------------------------------------------------------
+ *    UDP6, DTLS6 Configuration    (theNetcon)
+ *------------------------------------------------------*/
+UDP6CONF = { "GatewayTestClient",  // ClientId
+        "ff1e:feed:caca:dead::1", // Multicast group IP
+        "wlp4s0",                  // Network Interface
+        1883,                      // Multicast group Port
+        20020,                     // Local PortNo
+        };
+
+/*------------------------------------------------------
+ *    RFCOMM Configuration    (theNetcon)
+ *------------------------------------------------------*/
+RFCOMMCONF = { "GatewayTestClient",      // ClientId
+        "60:57:18:06:8B:72",          // GW Address
+        1,                            // Rfcomm channel
+        };
+
+/*------------------------------------------------------
+ *    Client Configuration  (theMqcon)
+ *------------------------------------------------------*/
+MQTTSNCONF = { 60,            //KeepAlive [seconds]
+        true,          //Clean session
+        300,           //Sleep duration [seconds]
+        "",            //WillTopic
+        "",            //WillMessage
+        0,             //WillQos
+        false          //WillRetain
+        };

 /*------------------------------------------------------
 *     Define Topics
@@ -72,7 +93,12 @@ MQTTSNCONF = {
 const char* topic1 = "ty4tw/topic1";
 const char* topic2 = "ty4tw/topic2";
 const char* topic3 = "ty4tw/topic3";
-
+const char* topic4 = "ty4tw/topic4";
+const char* topic40 = "ty4tw/#";
+const char* topic51 = "ty4tw/topic5/1";
+const char* topic52 = "ty4tw/topic5/2";
+const char* topic53 = "ty4tw/topic5/3";
+const char* topic50 = "ty4tw/topic5/+";

 /*------------------------------------------------------
 *       Callback routines for Subscribed Topics
@@ -80,25 +106,33 @@ const char* topic3 = "ty4tw/topic3";
 int on_Topic01(uint8_t* pload, uint16_t ploadlen)
 {
 	DISPLAY("\n\nTopic1 recv.\n");
-	char c = pload[ploadlen-1];
-	pload[ploadlen-1]= 0;   // set null terminator
-	DISPLAY("Payload -->%s%c<--\n\n",pload, c);
+	char c = pload[ploadlen - 1];
+	pload[ploadlen - 1] = 0;   // set null terminator
+	DISPLAY("Payload -->%s%c<--\n\n", pload, c);
 	return 0;
 }

 int on_Topic02(uint8_t* pload, uint16_t ploadlen)
 {
 	DISPLAY("\n\nTopic2 recv.\n");
-	pload[ploadlen-1]= 0;   // set null terminator
-	DISPLAY("Payload -->%s　<--\n\n",pload);
+	pload[ploadlen - 1] = 0;   // set null terminator
+	DISPLAY("Payload -->%s　<--\n\n", pload);
 	return 0;
 }

 int on_Topic03(uint8_t* pload, uint16_t ploadlen)
 {
-	DISPLAY("\n\nNew callback recv Topic2\n");
-	pload[ploadlen-1]= 0;   // set null terminator
-	DISPLAY("Payload -->%s　<--\n\n",pload);
+	DISPLAY("\n\nNew callback recv Topic3\n");
+	pload[ploadlen - 1] = 0;   // set null terminator
+	DISPLAY("Payload -->%s　<--\n\n", pload);
+	return 0;
+}
+
+int on_TopicWildcard(uint8_t* pload, uint16_t ploadlen)
+{
+	DISPLAY("\n\nNew callback recv TopicWildcard\n");
+	pload[ploadlen - 1] = 0;   // set null terminator
+	DISPLAY("Payload -->%s　<--\n\n", pload);
 	return 0;
 }

@@ -106,38 +140,53 @@ int on_Topic03(uint8_t* pload, uint16_t ploadlen)
 *      A Link list of Callback routines and Topics
 *------------------------------------------------------*/

-SUBSCRIBE_LIST = {// e.g. SUB(topic, callback, QoS),
-				  SUB(topic1, on_Topic01, 1),
-				  END_OF_SUBSCRIBE_LIST
-				 };
-
+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),
+	END_OF_SUBSCRIBE_LIST
+};

 /*------------------------------------------------------
 *    Test functions
 *------------------------------------------------------*/
+void subscribePredefTopic1(void)
+{
+	SUBSCRIBE_PREDEF(1, on_Topic03, QoS1);
+}

 void publishTopic1(void)
 {
 	char payload[300];
-	sprintf(payload, "publish \"ty4tw/Topic1\" \n");
-	uint8_t qos = 0;
-	PUBLISH(topic1,(uint8_t*)payload, strlen(payload), qos);
-}
-
-void subscribeTopic2(void)
-{
-	uint8_t qos = 1;
-	SUBSCRIBE(topic2, on_Topic02, qos);
+	sprintf(payload, "publish \"ty4tw/topic1\" \n");
+	PUBLISH(topic1, (uint8_t* )payload, strlen(payload), QoS0);
 }

 void publishTopic2(void)
 {
 	char payload[300];
 	sprintf(payload, "publish \"ty4tw/topic2\" \n");
-	uint8_t qos = 0;
-	PUBLISH(topic2,(uint8_t*)payload, strlen(payload), qos);
+	PUBLISH(topic2, (uint8_t* )payload, strlen(payload), QoS1);
 }

+void publishTopic4(void)
+{
+	char payload[300];
+	sprintf(payload, "publish \"ty4tw/topic40\" \n");
+	PUBLISH(topic4, (uint8_t* )payload, strlen(payload), QoS1);
+}
+
+void subscribeTopic10(void)
+{
+	SUBSCRIBE_PREDEF(10, on_Topic02, QoS1);
+}
+
+void subscribeWildcardTopic(void)
+{
+	SUBSCRIBE(topic50, on_TopicWildcard, QoS1);
+}
+
+
 void unsubscribe(void)
 {
 	UNSUBSCRIBE(topic2);
@@ -145,8 +194,7 @@ void unsubscribe(void)

 void subscribechangeCallback(void)
 {
-	uint8_t qos = 1;
-	SUBSCRIBE(topic2, on_Topic03, qos);
+	SUBSCRIBE(topic2, on_Topic02, QoS1);
 }

 void test3(void)
@@ -154,7 +202,7 @@ void test3(void)
 	char payload[300];
 	sprintf(payload, "TEST3 ");
 	uint8_t qos = 0;
-	PUBLISH(topic2,(uint8_t*)payload, strlen(payload), qos);
+	PUBLISH(topic2, (uint8_t* )payload, strlen(payload), qos);
 }

 void disconnect(void)
@@ -162,45 +210,81 @@ void disconnect(void)
 	DISCONNECT(0);
 }

-/*------------------------------------------------------
- *    A List of Test functions
- *------------------------------------------------------*/
+void asleep(void)
+{
+	DISCONNECT(theMqcon.sleepDuration);
+}

-TEST_LIST = {// e.g. TEST( Label, Test),
-			 TEST("Step1:Publish topic1",     publishTopic1),
-			 TEST("Step2:Publish topic2",     publishTopic2),
-			 TEST("Step3:Subscribe topic2",   subscribeTopic2),
-			 TEST("Step4:Publish topic2",     publishTopic2),
-			 TEST("Step5:Unsubscribe topic2", unsubscribe),
-			 TEST("Step6:Publish topic2",     publishTopic2),
-			 TEST("Step7:subscribe again",    subscribechangeCallback),
-			 TEST("Step8:Publish topic2",     publishTopic2),
-			 TEST("Step9:Disconnect",         disconnect),
-			 END_OF_TEST_LIST
-			};
+void onconnect(void)
+{
+	ONCONNECT();
+}

+void connect(void)
+{
+	CONNECT();
+}
+
+void DisableAutoPingreq(void)
+{
+	SetAutoPingReqMode(false);
+}
+
+void CleanSessionOff(void)
+{
+	SetCleanSession(false);
+}

 /*------------------------------------------------------
- *    unused for Test
+ *    A List of Test functions is valid in case of
+ *    line 23 of LMqttsnClientApp.h is commented out.
+ *    //#define CLIENT_MODE
 *------------------------------------------------------*/
-TASK_LIST = {// e.g. TASK( task, executing duration in second),
-			//TASK(test1, 4);
-             END_OF_TASK_LIST
-            };
+
+TEST_LIST =
+{   // e.g. TEST( Label, Test),
+	TEST("Step0:Connect", connect),
+	TEST("Step1:Subscribe list", onconnect),
+	TEST("Step2:Subscribe predef topic1", subscribePredefTopic1),
+	TEST("Step3:Publish topic1", publishTopic1),
+	TEST("Step4:Publish topic2", publishTopic2),
+	TEST("Step5:Subscribe PreDefined topic10. ID is not defined.", subscribeTopic10),
+	TEST("Step6:Publish topic2", publishTopic2),
+	TEST("Step7:Unsubscribe topic2", unsubscribe),
+	TEST("Step8:Publish topic2", publishTopic2),
+	TEST("Step9:subscribe again", subscribechangeCallback),
+	TEST("Step10:Publish topic2", publishTopic2),
+
+	TEST("Step11:Sleep     ", asleep),
+	TEST("Step12:Publish topic1", publishTopic1),
+	TEST("Step13:Disconnect", disconnect),
+	TEST("Step14:Publish topic2", publishTopic1),
+	TEST("Step15:Connect", connect),
+	TEST("Step16:Publish topic2", publishTopic2),
+	TEST("Step17:Auto Pingreq mode off", DisableAutoPingreq),
+	TEST("Step18:Publish topic2", publishTopic1),
+	TEST("Step19:Disconnect", disconnect),
+	END_OF_TEST_LIST
+};
+
+/*------------------------------------------------------
+ *    List of tasks is valid in case of line23 of
+ *    LMqttsnClientApp.h is uncommented.
+ *    #define CLIENT_MODE
+ *------------------------------------------------------*/
+TASK_LIST =
+{   // e.g. TASK( task, executing duration in second),
+	TASK(publishTopic1, 4),// publishTopic1() is executed every 4 seconds
+	TASK(publishTopic2, 7),// publishTopic2() is executed every 7 seconds
+	END_OF_TASK_LIST
+};

 /*------------------------------------------------------
 *    Initialize function
 *------------------------------------------------------*/
 void setup(void)
 {
-
+	SetForwarderMode(false);
 }

-/*------------------------------------------------------
- *    main
- *------------------------------------------------------*/
-
-int main(int argc, char** argv)
-{
-	return run();
-}
+/*****************  END OF  PROGRAM ********************/
--- a/MQTTSNGateway/GatewayTester/src/LGwProxy.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LGwProxy.cpp
--- a/MQTTSNGateway/GatewayTester/src/LGwProxy.h
+++ b/MQTTSNGateway/GatewayTester/src/LGwProxy.h
@@ -19,9 +19,14 @@

 #include <stdio.h>
 #include <string.h>
+#include <time.h>

 #include "LMqttsnClientApp.h"
 #include "LNetworkUdp.h"
+#include "LNetworkUdp6.h"
+#include "LNetworkRfcomm.h"
+#include "LNetworkDtls.h"
+#include "LNetworkDtls6.h"
 #include "LRegisterManager.h"
 #include "LTimer.h"
 #include "LTopicTable.h"
@@ -41,6 +46,7 @@ using namespace std;
 #define GW_SLEEPING         10
 #define GW_DISCONNECTED     11
 #define GW_SLEPT            12
+#define SSL_CONNECTING      13

 #define GW_WAIT_PINGRESP     1

@@ -50,60 +56,73 @@ 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(SENSORNET_CONFIG_t* 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     resetPingReqTimer(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(bool valid);
+    void     setQoSMinus1Mode(bool valid);
+	void     setPingReqMode(bool valid);
+	void     setAutoConnectMode(bool valid);
+	void     setSessionMode(bool valid);
+    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);
+	uint8_t getStatus(void);
 private:
-	int      readMsg(void);
-	void     writeGwMsg(void);
-	void     checkPingReq(void);
-	void     checkAdvertise(void);
-	int      getConnectResponce(void);
-	int      getDisconnectResponce(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     _retainWill;
-	uint8_t     _qosWill;
-	uint8_t     _gwId;
-	uint16_t    _tkeepAlive;
-	uint32_t    _tAdv;
-	uint32_t    _sendUTC;
-	int         _retryCount;
-	int         _connectRetry;
-	uint8_t     _status;
-	uint32_t    _pingSendUTC;
-	uint8_t     _pingRetryCount;
-	uint8_t     _pingStatus;
-	LRegisterManager _regMgr;
-	LTopicTable  _topicTbl;
-	LTimer       _gwAliveTimer;
-	LTimer       _keepAliveTimer;
-	uint16_t    _tSleep;
-	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;
+    bool        _isQoSMinus1Mode;
+	bool        _isPingReqMode;
+	bool        _isAutoConnectMode;
+    char        _msg[MQTTSN_MAX_MSG_LENGTH + 1];
 };

 } /* end of namespace */
--- a/MQTTSNGateway/GatewayTester/src/LMqttsnClient.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LMqttsnClient.cpp
@@ -29,8 +29,9 @@ extern TaskList      theTaskList[];
 extern TestList      theTestList[];
 extern OnPublishList theOnPublishList[];
 extern MQTTSNCONF;
-extern UDPCONF;
+extern SENSORNET_CONFIG_t theNetcon;
 extern void setup(void);
+
 /*=====================================
          LMqttsnClient
 ======================================*/
@@ -39,45 +40,63 @@ LScreen* theScreen = new LScreen();
 bool theOTAflag = false;
 bool theClientMode = true;

-int run(void)
+
+/*-------------------------------------
+ *    main
+ *------------------------------------*/
+
+int main(int argc, char** argv)
 {
+#ifndef CLIENT_MODE
 	char c = 0;
-	setup();
 	printf("\n%s", PAHO_COPYRIGHT4);
-	printf("\n%s\n", PAHO_COPYRIGHT0);
+    printf("\n%s", PAHO_COPYRIGHT0);
+#if defined(UDP)
+    printf("UDP ClientId:%s PortNo:%d\n", theNetcon.clientId, theNetcon.uPortNo);
+#elif defined(UDP6)
+    printf("UDP6 ClientId:%s PortNo:%d\n", theNetcon.clientId, theNetcon.uPortNo);
+#elif defined(DTLS)
+    printf("DTLS ClientId:%s PortNo:%d\n", theNetcon.clientId, theNetcon.uPortNo);
+#elif defined(DTLS6)
+    printf("DTLS6 ClientId:%s PortNo:%d\n", theNetcon.clientId, theNetcon.uPortNo);
+#elif defined(RFCOMM)
+    printf("RFCOMM ClientId:%s channel:%d\n", theNetcon.clientId, theNetcon.channel);
+#else
+    printf("\n");
+#endif
 	printf("%s\n", PAHO_COPYRIGHT1);
 	printf("%s\n", PAHO_COPYRIGHT2);
 	printf(" *\n%s\n", PAHO_COPYRIGHT3);
 	printf("%s\n", TESTER_VERSION);
 	printf("%s\n", PAHO_COPYRIGHT4);

-#ifndef CLIENT_MODE
+	theClientMode = false;
+	PROMPT(" Do you like Tomoaki ? ( y/n ) : ");
+	while (true)
 	{
-		theClientMode = false;
-		PROMPT(" Do you like Tomoaki ? ( y/n ) : ");
-		while (true)
+		if (CHECKKEYIN(&c))
 		{
-			if (CHECKKEYIN(&c))
+			if ( toupper(c) == 'N' )
 			{
-				if ( toupper(c) == 'N' )
-				{
-					DISPLAY("\033[0;31m\n**** Sorry ****\033[0;37m\n\n");
-					PROMPT("");
-					return 0;
-				}
-			}
-			else if ( toupper(c) == 'Y' )
-			{
-				DISPLAY("\033[0m\033[0;32mAttempting to Connect the Broker.....\033[0m\033[0;37m\n");
+				DISPLAY("\033[0;31m\n**** Sorry ****\033[0;37m\n\n");
 				PROMPT("");
-				break;
+				return 0;
 			}
 		}
+		else if ( toupper(c) == 'Y' )
+		{
+			DISPLAY("\033[0m\033[0;32mAttempting to Connect the Broker.....\033[0m\033[0;37m\n");
+			PROMPT("");
+			break;
+		}
 	}
+	theClient->setAutoConnectMode(false);
+	theClient->getPublishManager()->setAutoConnectMode(false);
 #endif

+	setup();
 	theClient->addTask(theClientMode);
-	theClient->initialize( theNetcon, theMqcon);
+	theClient->initialize( &theNetcon, &theMqcon);
 	do
 	{
 		theClient->run();
@@ -94,7 +113,7 @@ int run(void)
 ======================================*/
 LMqttsnClient::LMqttsnClient()
 {
-
+	_isAutoConnect = true;
 }

 LMqttsnClient::~LMqttsnClient()
@@ -102,10 +121,10 @@ LMqttsnClient::~LMqttsnClient()

 }

-void LMqttsnClient::initialize(LUdpConfig netconf, LMqttsnConfig mqconf)
+void LMqttsnClient::initialize(SENSORNET_CONFIG_t* netconf, LMqttsnConfig* mqconf)
 {
 	_gwProxy.initialize(netconf, mqconf);
-	setSleepDuration(mqconf.sleepDuration);
+	setSleepDuration(mqconf->sleepDuration);
 }

 void LMqttsnClient::addTask(bool clientMode)
@@ -179,9 +198,10 @@ void LMqttsnClient::subscribe(const char* topicName, TopicCallback onPublish, ui
 	_subMgr.subscribe(topicName, onPublish, qos);
 }

-void LMqttsnClient::subscribe(uint16_t topicId, TopicCallback onPublish, uint8_t qos, uint8_t topicType)
+void LMqttsnClient::subscribePredefinedId(uint16_t topicId, TopicCallback onPublish,
+		uint8_t qos)
 {
-	_subMgr.subscribe(topicId, onPublish, qos, topicType);
+	_subMgr.subscribePredefinedId(topicId, onPublish, qos);
 }

 void LMqttsnClient::unsubscribe(const char* topicName)
@@ -189,6 +209,11 @@ void LMqttsnClient::unsubscribe(const char* topicName)
 	_subMgr.unsubscribe(topicName);
 }

+void LMqttsnClient::unsubscribe(const uint16_t topicId)
+{
+    _subMgr.unsubscribe(topicId);
+}
+
 void LMqttsnClient::disconnect(uint16_t sleepInSecs)
 {
 	_gwProxy.disconnect(sleepInSecs);
@@ -196,14 +221,29 @@ void LMqttsnClient::disconnect(uint16_t sleepInSecs)

 void LMqttsnClient::run()
 {
-	_gwProxy.connect();
+	if (_isAutoConnect)
+	{
+		_gwProxy.connect();
+	}
 	_taskMgr.run();
-	sleep();
+}
+
+void LMqttsnClient::setAutoConnectMode(uint8_t flg)
+{
+	_isAutoConnect = flg;
+	_pubMgr.setAutoConnectMode(flg);
+	_gwProxy.setAutoConnectMode(flg);
+}
+
+void LMqttsnClient::setSleepMode(uint32_t duration)
+{
+	// ToDo:  set WDT and sleep mode
+	DISPLAY("\033[0m\033[0;32m\n\n Get into SLEEP mode %u [msec].\033[0m\033[0;37m\n\n", duration);
 }

 void LMqttsnClient::sleep(void)
 {
-
+	disconnect(_sleepDuration);
 }

 void LMqttsnClient::setSleepDuration(uint32_t duration)
@@ -213,7 +253,10 @@ void LMqttsnClient::setSleepDuration(uint32_t duration)

 void LMqttsnClient::onConnect(void)
 {
-	_subMgr.onConnect();
+	if (_isAutoConnect)
+	{
+        _subMgr.onConnect();
+	}
 }

 const char* LMqttsnClient::getClientId(void)
@@ -221,6 +264,8 @@ const char* LMqttsnClient::getClientId(void)
 	return _gwProxy.getClientId();
 }

-
-
+uint16_t LMqttsnClient::getTopicId(const char* topicName)
+{
+    return _gwProxy.getTopicTable()->getTopicId(topicName);
+}

--- a/MQTTSNGateway/GatewayTester/src/LMqttsnClient.h
+++ b/MQTTSNGateway/GatewayTester/src/LMqttsnClient.h
@@ -34,7 +34,9 @@ namespace linuxAsyncClient {

 struct OnPublishList
 {
+    MQTTSN_topicTypes type;
 	const char* topic;
+	uint16_t id;
 	int (*pubCallback)(uint8_t* payload, uint16_t payloadlen);
 	uint8_t qos;
 };
@@ -52,15 +54,19 @@ public:
    void publish(uint16_t topicId, Payload* payload, uint8_t qos, bool retain = false);
    void publish(uint16_t topicId, uint8_t* payload, uint16_t len, uint8_t qos, bool retain = false);
    void subscribe(const char* topicName, TopicCallback onPublish, uint8_t qos);
-    void subscribe(uint16_t topicId, TopicCallback onPublish, uint8_t qos, uint8_t topicType);
+    void subscribePredefinedId(uint16_t topicId, TopicCallback onPublish, uint8_t qos);
    void unsubscribe(const char* topicName);
+    void unsubscribe(const uint16_t topicId);
    void disconnect(uint16_t sleepInSecs);
-    void initialize(LUdpConfig netconf, LMqttsnConfig mqconf);
+    void initialize(SENSORNET_CONFIG_t* netconf, LMqttsnConfig* mqconf);
    void run(void);
    void addTask(bool test);
    void setSleepDuration(uint32_t duration);
+    void setSleepMode(uint32_t duration);
+	void setAutoConnectMode(uint8_t flg);
    void sleep(void);
 	const char* getClientId(void);
+	uint16_t getTopicId(const char* topicName);
    LGwProxy*          getGwProxy(void);
    LPublishManager*   getPublishManager(void);
    LSubscribeManager* getSubscribeManager(void);
@@ -73,6 +79,7 @@ private:
    LSubscribeManager _subMgr;
    LGwProxy          _gwProxy;
    uint32_t          _sleepDuration;
+	uint8_t _isAutoConnect;
 };


--- 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
@@ -17,17 +17,16 @@
 #ifndef MQTTSNCLIENTAPP_H_
 #define MQTTSNCLIENTAPP_H_

-/*======================================
- *     Program mode Flag
- ======================================*/
-//#define CLIENT_MODE
-
 /*======================================
 *         Debug Flag
 ======================================*/
 //#define DEBUG_NW
 //#define DEBUG_MQTTSN
-//#define DEBUG_OTA
+
+/*======================================
+ *     Program mode Flag
+ ======================================*/
+//#define CLIENT_MODE

 /****************************************
      MQTT-SN Parameters
@@ -56,7 +55,8 @@ typedef signed int     int32_t;
      Application config structures
 *****************************************/

-struct LMqttsnConfig{
+struct LMqttsnConfig
+{
 	uint16_t keepAlive;
 	bool     cleanSession;
 	uint32_t sleepDuration;
@@ -66,23 +66,87 @@ struct LMqttsnConfig{
    bool     willRetain;
 };

-struct LUdpConfig{
+struct LUdpConfig
+{
 	const char* clientId;
 	uint8_t  ipAddress[4];
 	uint16_t gPortNo;
 	uint16_t uPortNo;
 };

+struct LUdp6Config
+{
+    const char* clientId;
+    const char* ipAddress;
+    const char *interface;
+    uint16_t gPortNo;
+    uint16_t uPortNo;
+};
+
+struct LRfcommConfig
+{
+    const char* clientId;
+    const char* gwAddress;
+    uint8_t channel;
+};
+
+
+typedef enum
+{
+    MQTTSN_TOPIC_TYPE_NORMAL,
+    MQTTSN_TOPIC_TYPE_PREDEFINED,
+    MQTTSN_TOPIC_TYPE_SHORT
+} MQTTSN_topicTypes;
+
+
 /*======================================
      MACROs for Application
 =======================================*/
 #define MQTTSN_CONFIG    MqttsnConfig  theMqttsnConfig
-#define NETWORK_CONFIG   UdpConfig theNetworkConfig
+#define MQTTSNCONF LMqttsnConfig  theMqcon

+#if defined(UDP)
+#define UDPCONF  LUdpConfig theNetcon
+#define UDP6CONF  LUdp6Config theU6Conf
+#define RFCOMMCONF  LRfcommConfig theRfConf
+#define SENSORNET_CONFIG_t  LUdpConfig
+
+#elif defined(UDP6)
+#define UDP6CONF  LUdp6Config theNetcon
+#define UDPCONF  LUdpConfig theUConf
+#define RFCOMMCONF  LRfcommConfig theRfConf
+#define SENSORNET_CONFIG_t LUdp6Config
+
+#elif defined(RFCOMM)
+#define RFCOMMCONF  LRfcommConfig theNetcon
+#define UDPCONF  LUdpConfig theUConf
+#define UDP6CONF  LUdp6Config theU6Conf
+#define SENSORNET_CONFIG_t  LRfcommConfig
+
+#elif defined(DTLS)
+#define UDPCONF  LUdpConfig theNetcon
+#define UDP6CONF  LUdp6Config theU6Conf
+#define RFCOMMCONF  LRfcommConfig theRfConf
+#define SENSORNET_CONFIG_t  LUdpConfig
+
+#elif defined(DTLS6)
+#define UDPCONF  LUdpConfig theUConf
+#define UDP6CONF  LUdp6Config theNetcon
+#define RFCOMMCONF  LRfcommConfig theRfConf
+#define SENSORNET_CONFIG_t  LUdp6Config
+#else
+#error "UDP, UDP6, DTLS, DTLS6 or RFCOMM is not defined in LMqttsnClientApp.h"
+#endif
+
+
+
+#define CONNECT(...) theClient->getGwProxy()->connect(__VA_ARGS__)
 #define PUBLISH(...)     theClient->publish(__VA_ARGS__)
 #define SUBSCRIBE(...)   theClient->subscribe(__VA_ARGS__)
+#define SUBSCRIBE_PREDEF(...)   theClient->subscribePredefinedId(__VA_ARGS__)
 #define UNSUBSCRIBE(...) theClient->unsubscribe(__VA_ARGS__)
 #define DISCONNECT(...)  theClient->disconnect(__VA_ARGS__)
+#define ONCONNECT() theClient->getSubscribeManager()->onConnect()

 #define TASK_LIST         TaskList theTaskList[]
 #define TASK(...)         {__VA_ARGS__, 0, 0}
@@ -92,9 +156,14 @@ struct LUdpConfig{
 #define END_OF_TEST_LIST  {0, 0, 0}
 #define SUBSCRIBE_LIST    OnPublishList theOnPublishList[]
 #define SUB(...)          {__VA_ARGS__}
-#define END_OF_SUBSCRIBE_LIST {0,0,0}
-#define UDPCONF  LUdpConfig theNetcon
-#define MQTTSNCONF LMqttsnConfig  theMqcon
+#define END_OF_SUBSCRIBE_LIST {MQTTSN_TOPIC_TYPE_NORMAL,0,0,0, 0}
+
+
+#define SetForwarderMode(...)  theClient->getGwProxy()->setForwarderMode(__VA_ARGS__)
+#define SetQoSMinus1Mode(...) theClient->getGwProxy()->setQoSMinus1Mode(__VA_ARGS__)
+#define SetAutoConnectMode(...) theClient->setAutoConnectMode(__VA_ARGS__)
+#define SetAutoPingReqMode(...) theClient->getGwProxy()->setPingReqMode(__VA_ARGS__)
+#define SetCleanSession(...) theClient->getGwProxy()->setSessionMode(__VA_ARGS__)
 #ifdef CLIENT_MODE
 #define DISPLAY(...)
 #define PROMPT(...)
@@ -128,6 +197,10 @@ struct LUdpConfig{
 /*======================================
      MQTT-SN Defines
 ========================================*/
+#define QoS0  0
+#define QoS1  1
+#define QoS2  2
+#define Q0Sm1 3
 #define MQTTSN_TYPE_ADVERTISE     0x00
 #define MQTTSN_TYPE_SEARCHGW      0x01
 #define MQTTSN_TYPE_GWINFO        0x02
@@ -155,17 +228,15 @@ struct LUdpConfig{
 #define MQTTSN_TYPE_WILLTOPICRESP 0x1B
 #define MQTTSN_TYPE_WILLMSGUPD    0x1C
 #define MQTTSN_TYPE_WILLMSGRESP   0x1D
+#define MQTTSN_TYPE_ENCAPSULATED  0xFE

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

 #define MQTTSN_FLAG_DUP     0x80
 #define MQTTSN_FLAG_QOS_0   0x0
 #define MQTTSN_FLAG_QOS_1   0x20
 #define MQTTSN_FLAG_QOS_2   0x40
-#define MQTTSN_FLAG_QOS_N1  0xc0
+#define MQTTSN_FLAG_QOS_M1  0x60
 #define MQTTSN_FLAG_RETAIN  0x10
 #define MQTTSN_FLAG_WILL    0x08
 #define MQTTSN_FLAG_CLEAN   0x04
@@ -178,18 +249,14 @@ struct LUdpConfig{
 #define MQTTSN_RC_REJECTED_INVALID_TOPIC_ID 0x02
 #define MQTTSN_RC_REJECTED_NOT_SUPPORTED    0x03

-#define PREDEFINEDID_OTA_REQ       (0x0ff0)
-#define PREDEFINEDID_OTA_READY     (0x0ff1)
-#define PREDEFINEDID_OTA_NO_CLIENT (0x0ff2)
-
 /*=================================
 *    Starting prompt
 ==================================*/
-#define TESTER_VERSION " * Version: 1.0.0"
+#define TESTER_VERSION " * Version: 2.1.0"

-#define PAHO_COPYRIGHT0 " * MQTT-SN Gateway Tester"
+#define PAHO_COPYRIGHT0 " * "
 #define PAHO_COPYRIGHT1 " * Part of Project Paho in Eclipse"
-#define PAHO_COPYRIGHT2 " * (http://git.eclipse.org/c/paho/org.eclipse.paho.mqtt-sn.embedded-c.git/)"
+#define PAHO_COPYRIGHT2 " * (https://github.com/eclipse/paho.mqtt-sn.embedded-c.git)"
 #define PAHO_COPYRIGHT3 " * Author : Tomoaki YAMAGUCHI"
 #define PAHO_COPYRIGHT4 " ***************************************************************************"

--- a/MQTTSNGateway/GatewayTester/src/LNetworkDtls.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LNetworkDtls.cpp
@@ -0,0 +1,544 @@
+/**************************************************************************************
+ * Copyright (c) 2016, 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
+ **************************************************************************************/
+#ifdef DTLS
+
+#include <stdio.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <openssl/ssl.h>
+#include <openssl/err.h>
+#include <openssl/rand.h>
+#include "LMqttsnClientApp.h"
+#include "LNetworkDtls.h"
+#include "LTimer.h"
+#include "LScreen.h"
+
+using namespace std;
+using namespace linuxAsyncClient;
+
+extern uint16_t getUint16(const uint8_t* pos);
+extern uint32_t getUint32(const uint8_t* pos);
+extern LScreen* theScreen;
+extern bool theClientMode;
+
+/* Certificate verification. Returns 1 if trusted, else 0 */
+int verify_cert(int ok, X509_STORE_CTX *ctx);
+
+/*=========================================
+       Class LNetwork
+ =========================================*/
+LNetwork::LNetwork()
+{
+    _sleepflg = false;
+    resetGwAddress();
+}
+
+LNetwork::~LNetwork()
+{
+}
+
+int LNetwork::broadcast(const uint8_t *xmitData, uint16_t dataLen)
+{
+    return LDtlsPort::multicast(xmitData, (uint32_t) dataLen);
+}
+
+int LNetwork::unicast(const uint8_t *xmitData, uint16_t dataLen)
+{
+    return LDtlsPort::unicast(xmitData, dataLen);
+}
+
+
+uint8_t* LNetwork::getMessage(int *len)
+{
+    *len = 0;
+    uint16_t recvLen = 0;
+    if (checkRecvBuf())
+    {
+        recvLen = LDtlsPort::recv(_rxDataBuf, MQTTSN_MAX_PACKET_SIZE, false, &_ipAddress, &_portNo);
+        if (_gwIpAddress && isUnicast() && (_ipAddress != _gwIpAddress) && (_portNo != _gwPortNo))
+        {
+            return 0;
+        }
+
+        if (recvLen < 0)
+        {
+            *len = recvLen;
+            return 0;
+        }
+        else
+        {
+            if (_rxDataBuf[0] == 0x01)
+            {
+                *len = getUint16(_rxDataBuf + 1);
+            }
+            else
+            {
+                *len = _rxDataBuf[0];
+            }
+            return _rxDataBuf;
+        }
+    }
+    return 0;
+}
+
+void LNetwork::setGwAddress(void)
+{
+    _gwPortNo = _portNo;
+    _gwIpAddress = _ipAddress;
+}
+
+void LNetwork::resetGwAddress(void)
+{
+    _gwIpAddress = 0;
+    _gwPortNo = 0;
+}
+
+
+bool LNetwork::initialize(LUdpConfig *config)
+{
+    return LDtlsPort::open(config);
+}
+
+void LNetwork::setSleep()
+{
+    _sleepflg = true;
+}
+
+bool LNetwork::isBroadcastable()
+{
+    return true;
+}
+
+int LNetwork::sslConnect(void)
+{
+    return LDtlsPort::sslConnect(_gwIpAddress, _gwPortNo);
+}
+
+/*=========================================
+ Class DtlsPort
+ =========================================*/
+LDtlsPort::LDtlsPort()
+{
+    _disconReq = false;
+    _sockfdMcast = 0;
+    _sockfdSsl = 0;
+    _castStat = 0;
+}
+
+LDtlsPort::~LDtlsPort()
+{
+    close();
+}
+
+
+void LDtlsPort::close()
+{
+    if (_sockfdMcast > 0)
+    {
+        ::close(_sockfdMcast);
+        _sockfdMcast = 0;
+        if (_sockfdSsl > 0)
+        {
+            ::close(_sockfdSsl);
+            _sockfdSsl = 0;
+        }
+    }
+}
+
+bool LDtlsPort::open(LUdpConfig *config)
+{
+    char errmsg[256];
+    int optval = 0;
+
+    uint8_t sav = config->ipAddress[3];
+    config->ipAddress[3] = config->ipAddress[0];
+    config->ipAddress[0] = sav;
+    sav = config->ipAddress[2];
+    config->ipAddress[2] = config->ipAddress[1];
+    config->ipAddress[1] = sav;
+
+    _gIpAddr = getUint32((const uint8_t*) config->ipAddress);
+    _gPortNo = htons(config->gPortNo);
+    _uPortNo = htons(config->uPortNo);
+
+    if (_gPortNo == 0 || _gIpAddr == 0 || _uPortNo == 0)
+    {
+        return false;
+    }
+
+    SSL_load_error_strings();
+    SSL_library_init();
+    _ctx = SSL_CTX_new(DTLS_client_method());
+
+    if (_ctx == 0)
+    {
+        ERR_error_string_n(ERR_get_error(), errmsg, sizeof(errmsg));
+        DISPLAY("SSL_CTX_new() %s\n", errmsg);
+        return false;
+    }
+
+    /* Client certification and cookie are not required */
+    SSL_CTX_set_verify(_ctx, SSL_VERIFY_PEER, verify_cert);
+
+    /* setup Multicast socket */
+    _sockfdMcast = socket(AF_INET, SOCK_DGRAM, 0);
+    if (_sockfdMcast < 0)
+    {
+        return false;
+    }
+
+    struct sockaddr_in addrm;
+    addrm.sin_family = AF_INET;
+    addrm.sin_port = _gPortNo;
+    addrm.sin_addr.s_addr = INADDR_ANY;
+
+    optval = 1;
+    setsockopt(_sockfdMcast, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
+
+    if (::bind(_sockfdMcast, (struct sockaddr*) &addrm, sizeof(addrm)) < 0)
+    {
+        return false;
+    }
+
+    optval = 1;
+    if (setsockopt(_sockfdMcast, IPPROTO_IP, IP_MULTICAST_LOOP,  &optval, sizeof(optval)) < 0)
+    {
+        D_NWLOG("\033[0m\033[0;31merror IP_MULTICAST_LOOP in LDtlsPort::open\033[0m\033[0;37m\n");
+        close();
+        return false;
+    }
+
+    ip_mreq mreq;
+    mreq.imr_interface.s_addr = INADDR_ANY;
+    mreq.imr_multiaddr.s_addr = _gIpAddr;
+
+    if (setsockopt(_sockfdMcast, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0)
+    {
+        D_NWLOG("\033[0m\033[0;31merror IP_ADD_MEMBERSHIP in LDtlsPort::open\033[0m\033[0;37m\n");
+        close();
+        return false;
+    }
+    return true;
+}
+
+bool LDtlsPort::isUnicast()
+{
+    return (_castStat == STAT_UNICAST);
+}
+
+
+int LDtlsPort::unicast(const uint8_t *buf, uint32_t length)
+{
+    int status = SSL_write(_ssl, buf, length);
+    if (status <= 0)
+    {
+        int rc = 0;
+        SSL_get_error(_ssl, rc);
+        DISPLAY("errno == %d in LDtlsPort::unicast\n", rc);
+    }
+    else
+    {
+        D_NWLOG("sendto gateway via DTLS ");
+        for (uint16_t i = 0; i < length; i++)
+        {
+            D_NWLOG(" %02x", *(buf + i));
+        }
+        D_NWLOG("\n");
+
+        if (!theClientMode)
+        {
+            char sbuf[SCREEN_BUFF_SIZE];
+            int pos = 0;
+            sprintf(sbuf, "\033[0;34msendto the gateway via SSL  ");
+            pos = strlen(sbuf);
+            for (uint16_t i = 0; i < length; i++)
+            {
+                sprintf(sbuf + pos, " %02x", *(buf + i));
+                if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20)  // -20 for Escape sequence
+                {
+                    break;
+                }
+                pos += 3;
+            }
+            sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
+            theScreen->display(sbuf);
+        }
+    }
+    return status;
+}
+
+
+int LDtlsPort::multicast(const uint8_t *buf, uint32_t length)
+{
+    struct sockaddr_in dest;
+    dest.sin_family = AF_INET;
+    dest.sin_port = _gPortNo;
+    dest.sin_addr.s_addr = _gIpAddr;
+
+    int status = ::sendto(_sockfdMcast, buf, length, 0, (const sockaddr*) &dest, sizeof(dest));
+    if (status < 0)
+    {
+        D_NWLOG("\033[0m\033[0;31merrno == %d in LDtlsPort::multicast\033[0m\033[0;37m\n", errno);
+        DISPLAY("\033[0m\033[0;31merrno == %d in LDtlsPort::multicast\033[0m\033[0;37m\n", errno);
+        return errno;
+    }
+    else
+    {
+        D_NWLOG("sendto %-15s:%-6u", inet_ntoa(dest.sin_addr), htons(_gPortNo));
+
+        for (uint16_t i = 0; i < length; i++)
+        {
+            D_NWLOG(" %02x", *(buf + i));
+            DISPLAY(" %02x", *(buf + i));
+        }
+        D_NWLOG("\n");
+
+        if (!theClientMode)
+        {
+            char sbuf[SCREEN_BUFF_SIZE];
+            int pos = 0;
+            sprintf(sbuf, "\033[0;34msendto %-15s:%-6u", inet_ntoa(dest.sin_addr), htons(_gPortNo));
+            pos = strlen(sbuf);
+            for (uint16_t i = 0; i < length; i++)
+            {
+                sprintf(sbuf + pos, " %02x", *(buf + i));
+                if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20)
+                {
+                    break;
+                }
+                pos += 3;
+            }
+            sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
+            theScreen->display(sbuf);
+        }
+        return status;
+    }
+
+}
+
+bool LDtlsPort::checkRecvBuf()
+{
+    struct timeval timeout;
+    timeout.tv_sec = 0;
+    timeout.tv_usec = 50000;    // 50 msec
+
+    uint8_t buf[2];
+    fd_set recvfds;
+    int maxSock = 0;
+
+    FD_ZERO(&recvfds);
+    if (_sockfdMcast)
+    {
+        FD_SET(_sockfdMcast, &recvfds);
+    }
+    if (_sockfdSsl)
+    {
+        FD_SET(_sockfdSsl, &recvfds);
+    }
+
+    if (_sockfdMcast > _sockfdSsl)
+    {
+        maxSock = _sockfdMcast;
+    }
+    else
+    {
+        maxSock = _sockfdSsl;
+    }
+
+    select(maxSock + 1, &recvfds, 0, 0, &timeout);
+
+    if (FD_ISSET(_sockfdMcast, &recvfds))
+    {
+        if (::recv(_sockfdMcast, buf, 1, MSG_DONTWAIT | MSG_PEEK) > 0)
+        {
+            _castStat = STAT_MULTICAST;
+            return true;
+        }
+    }
+    else if (FD_ISSET(_sockfdSsl, &recvfds))
+    {
+        if (::recv(_sockfdSsl, buf, 1, MSG_DONTWAIT | MSG_PEEK) > 0)
+        {
+            _castStat = STAT_SSL;
+            return true;
+        }
+    }
+    _castStat = STAT_NONE;
+    return false;
+}
+
+int LDtlsPort::recv(uint8_t *buf, uint16_t len, bool flg, uint32_t *ipAddressPtr, in_port_t *portPtr)
+{
+    int flags = flg ? MSG_DONTWAIT : 0;
+    return recvfrom(buf, len, flags, ipAddressPtr, portPtr);
+}
+
+int LDtlsPort::recvfrom(uint8_t *buf, uint16_t length, int flags, uint32_t *ipAddressPtr, in_port_t *portPtr)
+{
+    struct sockaddr_in sender;
+    int status = 0;
+    socklen_t addrlen = sizeof(sender);
+    memset(&sender, 0, addrlen);
+
+    if (_castStat == STAT_SSL)
+    {
+        D_NWLOG("Ucast ");
+        if (SSL_read(_ssl, buf, length) == 0)
+        {
+            return 0;
+        }
+    }
+    else if (_castStat == STAT_MULTICAST)
+    {
+        D_NWLOG("Mcast ");
+        status = ::recvfrom(_sockfdMcast, buf, length, flags, (struct sockaddr*) &sender, &addrlen);
+    }
+    else
+    {
+        return 0;
+    }
+
+    if (status < 0 && errno != EAGAIN)
+    {
+        D_NWLOG("\033[0m\033[0;31merrno == %d in LDtlsPort::recvfrom \033[0m\033[0;37m\n", errno);
+        DISPLAY("\033[0m\033[0;31merrno == %d in LDtlsPort::recvfrom \033[0m\033[0;37m\n", errno);
+    }
+    else if (status > 0)
+    {
+        *ipAddressPtr = sender.sin_addr.s_addr;
+        *portPtr = sender.sin_port;
+
+        D_NWLOG("recved %-15s:%-6u", inet_ntoa(sender.sin_addr), ntohs(*portPtr));
+
+        for (uint16_t i = 0; i < status; i++)
+        {
+            D_NWLOG(" %02x", *(buf + i));
+        }
+        D_NWLOG("\n");
+
+        if (!theClientMode)
+        {
+            char sbuf[SCREEN_BUFF_SIZE];
+            int pos = 0;
+            sprintf(sbuf, "\033[0;34mrecved %-15s:%-6u", inet_ntoa(sender.sin_addr), ntohs(*portPtr));
+            pos = strlen(sbuf);
+            for (uint16_t i = 0; i < status; i++)
+            {
+                sprintf(sbuf + pos, " %02x", *(buf + i));
+                if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20)
+                {
+                    break;
+                }
+                pos += 3;
+            }
+            sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
+            theScreen->display(sbuf);
+        }
+        return status;
+    }
+    else
+    {
+        return 0;
+    }
+    return status;
+}
+
+int LDtlsPort::sslConnect(uint32_t ipAddress, in_port_t portNo)
+{
+    int reuse = 1;
+    if (_ssl != 0)
+    {
+        D_NWLOG("LDtlsPort::sslConnect SSL exists.\n");
+        SSL_shutdown(_ssl);
+        SSL_free(_ssl);
+        _sockfdSsl = 0;
+        _ssl = 0;
+    }
+
+    if (_sockfdSsl > 0)
+    {
+        D_NWLOG("LDtlsPort::sslConnect socket exists.\n");
+        ::close(_sockfdSsl);
+    }
+
+    _sockfdSsl = socket(AF_INET, SOCK_DGRAM, 0);
+    if (_sockfdSsl < 0)
+    {
+        D_NWLOG("LDtlsPort::sslConnect Can't create a socket\n");
+        return -1;
+    }
+    setsockopt(_sockfdSsl, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
+
+    struct sockaddr_in addr;
+    addr.sin_family = AF_INET;
+    addr.sin_port = _uPortNo;
+    addr.sin_addr.s_addr = INADDR_ANY;
+    if (::bind(_sockfdSsl, (struct sockaddr*) &addr, sizeof(addr)) < 0)
+    {
+        ::close(_sockfdSsl);
+        _sockfdSsl = 0;
+        D_NWLOG("LDtlsPort::sslConnect Can't bind a socket\n");
+        return -1;
+    }
+
+    struct sockaddr_in dest;
+    dest.sin_family = AF_INET;
+    dest.sin_port = portNo;
+    dest.sin_addr.s_addr = ipAddress;
+    int rc = 0;
+    errno = 0;
+    BIO *cbio = BIO_new_dgram(_sockfdSsl, BIO_NOCLOSE);
+    connect(_sockfdSsl, (sockaddr*) &dest, sizeof(sockaddr_in));
+    BIO_ctrl(cbio, BIO_CTRL_DGRAM_SET_CONNECTED, 0, &dest);
+    _ssl = SSL_new(_ctx);
+    SSL_set_bio(_ssl, cbio, cbio);
+
+    D_NWLOG("LDtlsPort::sslConnect connect to %-15s:%-6u\n", inet_ntoa(dest.sin_addr), htons(dest.sin_port));
+
+    timeval timeout;
+    timeout.tv_sec = 5;
+    timeout.tv_usec = 0;
+    BIO_ctrl(cbio, BIO_CTRL_DGRAM_SET_RECV_TIMEOUT, 0, &timeout);
+
+    int stat = SSL_connect(_ssl);
+    if (stat != 1)
+    {
+        rc = -1;
+        D_NWLOG("SSL fail to connect\n");
+    }
+    else
+    {
+        rc = 1;
+        D_NWLOG("SSL connected\n");
+    }
+    return rc;
+}
+
+int verify_cert(int ok, X509_STORE_CTX *ctx)
+{
+    return 1;
+}
+
+
+#endif
+
--- a/MQTTSNGateway/GatewayTester/src/LNetworkDtls.h
+++ b/MQTTSNGateway/GatewayTester/src/LNetworkDtls.h
@@ -0,0 +1,118 @@
+/**************************************************************************************
+ * Copyright (c) 2016, 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 NETWORKDTLS_H_
+#define NETWORKDTLS_H_
+
+#ifdef DTLS
+
+#include <sys/time.h>
+#include <iostream>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netdb.h>
+#include <unistd.h>
+#include <string>
+#include <arpa/inet.h>
+#include <openssl/ssl.h>
+#include <openssl/err.h>
+
+#define SOCKET_MAXHOSTNAME  200
+#define SOCKET_MAXCONNECTIONS  5
+#define SOCKET_MAXRECV  500
+#define SOCKET_MAXBUFFER_LENGTH 500 // buffer size
+
+
+using namespace std;
+
+namespace linuxAsyncClient {
+#define STAT_NONE   0
+#define STAT_UNICAST   1
+#define STAT_MULTICAST 2
+#define STAT_SSL 3
+/*========================================
+ Class LDtlsPort
+ =======================================*/
+class LDtlsPort
+{
+    friend class LNetwork;
+public:
+    LDtlsPort();
+    virtual ~LDtlsPort();
+
+	bool open(LUdpConfig* config);
+
+    int unicast(const uint8_t *buf, uint32_t length);
+	int multicast( const uint8_t* buf, uint32_t length );
+	int recv(uint8_t* buf, uint16_t len, bool nonblock, uint32_t* ipaddress, in_port_t* port );
+	int recv(uint8_t* buf, int flags);
+	bool checkRecvBuf();
+	bool isUnicast();
+    SSL* getSSL(void);
+    int sslConnect(uint32_t ipAddress, in_port_t port);
+private:
+	void close();
+	int recvfrom ( uint8_t* buf, uint16_t len, int flags, uint32_t* ipaddress, in_port_t* port );
+
+    int _sockfdMcast;
+    int _sockfdSsl;
+    SSL_CTX *_ctx;
+    SSL *_ssl;
+    in_port_t _gPortNo;
+    in_port_t _uPortNo;
+	uint32_t _gIpAddr;
+	uint8_t  _castStat;
+	bool   _disconReq;
+
+};
+
+#define NO_ERROR	0
+#define PACKET_EXCEEDS_LENGTH  1
+/*===========================================
+               Class  Network
+ ============================================*/
+class LNetwork: public LDtlsPort
+{
+public:
+    LNetwork();
+    ~LNetwork();
+
+    int  broadcast(const uint8_t* payload, uint16_t payloadLen);
+    int  unicast(const uint8_t* payload, uint16_t payloadLen);
+    void setGwAddress(void);
+    void resetGwAddress(void);
+    bool initialize(LUdpConfig* config);
+    uint8_t*  getMessage(int* len);
+    bool isBroadcastable();
+    int sslConnect(void);
+private:
+    void setSleep();
+    int  readApiFrame(void);
+
+    uint32_t _gwIpAddress;
+	uint32_t _ipAddress;
+    in_port_t _gwPortNo;
+	in_port_t _portNo;
+    int     _returnCode;
+    bool _sleepflg;
+    uint8_t _rxDataBuf[MQTTSN_MAX_PACKET_SIZE + 1];  // defined in MqttsnClientApp.h
+
+};
+
+}    /* end of namespace */
+#endif /* DTLS */
+#endif /* NETWORKDTLS_H_ */
--- a/MQTTSNGateway/GatewayTester/src/LNetworkDtls6.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LNetworkDtls6.cpp
@@ -0,0 +1,586 @@
+/**************************************************************************************
+ * Copyright (c) 2021, 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
+ **************************************************************************************/
+#ifdef DTLS6
+
+#include <stdio.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <net/if.h>
+#include <termios.h>
+#include <openssl/ssl.h>
+#include <openssl/err.h>
+#include <openssl/rand.h>
+#include <poll.h>
+#include "LMqttsnClientApp.h"
+#include "LNetworkDtls6.h"
+#include "LTimer.h"
+#include "LScreen.h"
+
+using namespace std;
+using namespace linuxAsyncClient;
+
+extern uint16_t getUint16(const uint8_t *pos);
+extern uint32_t getUint32(const uint8_t *pos);
+extern LScreen *theScreen;
+extern bool theClientMode;
+
+/* Certificate verification. Returns 1 if trusted, else 0 */
+int verify_cert(int ok, X509_STORE_CTX *ctx);
+
+/*=========================================
+ Class LNetwork
+ =========================================*/
+LNetwork::LNetwork()
+{
+    _sleepflg = false;
+    resetGwAddress();
+}
+
+LNetwork::~LNetwork()
+{
+}
+
+int LNetwork::broadcast(const uint8_t *xmitData, uint16_t dataLen)
+{
+    return LDtls6Port::multicast(xmitData, (uint32_t) dataLen);
+}
+
+int LNetwork::unicast(const uint8_t *xmitData, uint16_t dataLen)
+{
+    return LDtls6Port::unicast(xmitData, dataLen);
+}
+
+uint8_t* LNetwork::getMessage(int *len)
+{
+    *len = 0;
+    if (checkRecvBuf())
+    {
+        uint16_t recvLen = LDtls6Port::recv(_rxDataBuf, MQTTSN_MAX_PACKET_SIZE, false, &_ipAddress, &_portNo);
+        int addrFlg = memcmp(_ipAddress.s6_addr, _gwIpAddress.s6_addr, sizeof(_gwIpAddress.s6_addr));
+        if (isUnicast() && addrFlg && (_portNo != _gwPortNo))
+        {
+            return 0;
+        }
+
+        if (recvLen < 0)
+        {
+            *len = recvLen;
+            return 0;
+        }
+        else
+        {
+            if (_rxDataBuf[0] == 0x01)
+            {
+                *len = getUint16(_rxDataBuf + 1);
+            }
+            else
+            {
+                *len = _rxDataBuf[0];
+            }
+            return _rxDataBuf;
+        }
+    }
+    return 0;
+}
+
+void LNetwork::setGwAddress(void)
+{
+    _gwPortNo = _portNo;
+    memcpy(&_gwIpAddress.s6_addr, &_ipAddress.s6_addr, sizeof(_ipAddress.s6_addr));
+
+}
+
+void LNetwork::resetGwAddress(void)
+{
+    memset(&_gwIpAddress, 0, sizeof(_gwIpAddress));
+    _gwPortNo = 0;
+}
+
+bool LNetwork::initialize(LUdp6Config *config)
+{
+    return LDtls6Port::open(config);
+}
+
+void LNetwork::setSleep()
+{
+    _sleepflg = true;
+}
+
+bool LNetwork::isBroadcastable()
+{
+    return true;
+}
+
+int LNetwork::sslConnect(void)
+{
+    return LDtls6Port::sslConnect(_gwIpAddress, _gwPortNo);
+}
+
+/*=========================================
+ Class Dtls6Port
+ =========================================*/
+LDtls6Port::LDtls6Port()
+{
+    _disconReq = false;
+    _castStat = STAT_NONE;
+    _ifIndex = 0;
+    _gIpAddrStr = nullptr;
+    _sockfdMcast = 0;
+    _sockfdSsl = 0;
+    _ctx = nullptr;
+    _ssl = nullptr;
+    _gPortNo = _uPortNo = 0;
+}
+
+LDtls6Port::~LDtls6Port()
+{
+    close();
+    if (_gIpAddrStr)
+    {
+        free(_gIpAddrStr);
+    }
+}
+
+void LDtls6Port::close()
+{
+    if (_sockfdMcast > 0)
+    {
+        ::close(_sockfdMcast);
+        _sockfdMcast = 0;
+        if (_sockfdSsl > 0)
+        {
+            ::close(_sockfdSsl);
+            _sockfdSsl = 0;
+        }
+    }
+}
+
+bool LDtls6Port::open(LUdp6Config *config)
+{
+    int optval = 1;
+    sockaddr_in6 addr6;
+    char errmsg[256];
+
+    _gPortNo = htons(config->gPortNo);
+    _uPortNo = htons(config->uPortNo);
+
+    if (_gPortNo == 0 || _uPortNo == 0)
+    {
+        return false;
+    }
+
+    SSL_load_error_strings();
+    SSL_library_init();
+    _ctx = SSL_CTX_new(DTLS_client_method());
+
+    if (_ctx == 0)
+    {
+        ERR_error_string_n(ERR_get_error(), errmsg, sizeof(errmsg));
+        DISPLAY("SSL_CTX_new() %s\n", errmsg);
+        return false;
+    }
+
+    /* Client certification and cookie are not required */
+    SSL_CTX_set_verify(_ctx, SSL_VERIFY_PEER, verify_cert);
+
+    if (strlen(config->interface) > 0)
+    {
+        _ifIndex = if_nametoindex(config->interface);
+        _interfaceName = config->interface;
+    }
+
+    /* create a multicast socket */
+    _sockfdMcast = socket(AF_INET6, SOCK_DGRAM, 0);
+    if (_sockfdMcast < 0)
+    {
+        return false;
+    }
+
+    optval = 1;
+    setsockopt(_sockfdMcast, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
+    setsockopt(_sockfdMcast, IPPROTO_IPV6, IPV6_V6ONLY, &optval, sizeof(optval));
+
+    memset(&addr6, 0, sizeof(addr6));
+    addr6.sin6_family = AF_INET6;
+    addr6.sin6_port = _gPortNo;
+    addr6.sin6_addr = in6addr_any;
+
+    if (::bind(_sockfdMcast, (sockaddr*) &addr6, sizeof(addr6)) < 0)
+    {
+        D_NWLOG("\033[0m\033[0;31merror %s ::bind() in Udp6Port::open\033[0m\033[0;37m\n", strerror(errno));
+        return false;
+    }
+
+    ipv6_mreq addrm;
+    addrm.ipv6mr_interface = _ifIndex;
+    inet_pton(AF_INET6, config->ipAddress, &addrm.ipv6mr_multiaddr);
+    if (setsockopt(_sockfdMcast, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, &addrm, sizeof(addrm)) < 0)
+    {
+        D_NWLOG("\033[0m\033[0;31merror %s IPV6_ADD_MEMBERSHIP in Udp6Port::open\033[0m\033[0;37m\n", strerror(errno));
+        close();
+        return false;
+    }
+
+    optval = 1;
+    if (setsockopt(_sockfdMcast, IPPROTO_IPV6, IPV6_MULTICAST_LOOP, &optval, sizeof(optval)) < 0)
+    {
+        D_NWLOG("\033[0m\033[0;31merror %s IPV6_MULTICAST_LOOP in Udp6Port::open\033[0m\033[0;37m\n", strerror(errno));
+        close();
+        return false;
+    }
+
+    _gIpAddr.sin6_family = AF_INET6;
+    _gIpAddr.sin6_port = _gPortNo;
+    memcpy(&_gIpAddr.sin6_addr, (const void*) &addrm.ipv6mr_multiaddr, sizeof(addrm.ipv6mr_multiaddr));
+    _gIpAddrStr = strdup(config->ipAddress);
+    return true;
+}
+
+bool LDtls6Port::isUnicast()
+{
+    return (_castStat == STAT_UNICAST);
+}
+
+int LDtls6Port::unicast(const uint8_t *buf, uint32_t length)
+{
+    int status = SSL_write(_ssl, buf, length);
+    if (status <= 0)
+    {
+        int rc = 0;
+        SSL_get_error(_ssl, rc);
+        DISPLAY("errno == %d in LDtls6Port::unicast\n", rc);
+    }
+    else
+    {
+        D_NWLOG("sendto gateway via DTLS6 ");
+        for (uint16_t i = 0; i < length; i++)
+        {
+            D_NWLOG(" %02x", *(buf + i));
+        }
+
+        D_NWLOG("\n");
+
+        if (!theClientMode)
+        {
+            char sbuf[SCREEN_BUFF_SIZE];
+            int pos = 0;
+            sprintf(sbuf, "\033[0;34msendto the gateway via SSL  ");
+            pos = strlen(sbuf);
+            for (uint16_t i = 0; i < length; i++)
+            {
+                sprintf(sbuf + pos, " %02x", *(buf + i));
+                if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20)  // -20 for Escape sequence
+                {
+                    break;
+                }
+                pos += 3;
+            }
+            sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
+            theScreen->display(sbuf);
+        }
+    }
+    return status;
+}
+
+int LDtls6Port::multicast(const uint8_t *buf, uint32_t length)
+{
+    char sbuf[SCREEN_BUFF_SIZE];
+    char portStr[8];
+    sprintf(portStr, "%d", ntohs(_gIpAddr.sin6_port));
+
+    int status = ::sendto(_sockfdMcast, buf, length, 0, (sockaddr*) &_gIpAddr, sizeof(_gIpAddr));
+    if (status < 0)
+    {
+        DISPLAY("\033[0m\033[0;31merrno == %d in LDtls6Port::multicast\033[0m\033[0;37m\n", errno);
+        return errno;
+    }
+    else
+    {
+        D_NWLOG("multicast to [%s]:%-6s", _gIpAddrStr, portStr);
+        for (uint16_t i = 0; i < length; i++)
+        {
+            D_NWLOG(" %02x", *(buf + i));
+        }
+
+        D_NWLOG("\n");
+
+        if (!theClientMode)
+        {
+            memset(sbuf, 0, SCREEN_BUFF_SIZE);
+            int pos = 0;
+            sprintf(sbuf, "\033[0;34mmulticast to [%s]:%-6s", _gIpAddrStr, portStr);
+            pos = strlen(sbuf);
+            for (uint16_t i = 0; i < length; i++)
+            {
+                sprintf(sbuf + pos, " %02x", *(buf + i));
+                if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20)
+                {
+                    break;
+                }
+                pos += 3;
+            }
+            sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
+            theScreen->display(sbuf);
+        }
+        return status;
+    }
+}
+
+bool LDtls6Port::checkRecvBuf()
+{
+    timeval timeout;
+    timeout.tv_sec = 0;
+    timeout.tv_usec = 50000;    // 50 msec
+
+    uint8_t buf[2];
+    fd_set recvfds;
+    int maxSock = 0;
+
+    FD_ZERO(&recvfds);
+    if (_sockfdMcast)
+    {
+        FD_SET(_sockfdMcast, &recvfds);
+    }
+    if (_sockfdSsl)
+    {
+        FD_SET(_sockfdSsl, &recvfds);
+    }
+
+    if (_sockfdMcast > _sockfdSsl)
+    {
+        maxSock = _sockfdMcast;
+    }
+    else
+    {
+        maxSock = _sockfdSsl;
+    }
+
+    select(maxSock + 1, &recvfds, 0, 0, &timeout);
+
+    if (FD_ISSET(_sockfdMcast, &recvfds))
+    {
+        if (::recv(_sockfdMcast, buf, 1, MSG_DONTWAIT | MSG_PEEK) > 0)
+        {
+            _castStat = STAT_MULTICAST;
+            return true;
+        }
+    }
+    else if (FD_ISSET(_sockfdSsl, &recvfds))
+    {
+        if (::recv(_sockfdSsl, buf, 1, MSG_DONTWAIT | MSG_PEEK) > 0)
+        {
+            _castStat = STAT_SSL;
+            return true;
+        }
+    }
+    _castStat = STAT_NONE;
+    return false;
+}
+
+int LDtls6Port::recv(uint8_t *buf, uint16_t len, bool flg, in6_addr *ipAddressPtr, in_port_t *portPtr)
+{
+    int flags = flg ? MSG_DONTWAIT : 0;
+    return recvfrom(buf, len, flags, ipAddressPtr, portPtr);
+}
+
+int LDtls6Port::recvfrom(uint8_t *buf, uint16_t length, int flags, in6_addr *ipAddressPtr, in_port_t *portPtr)
+{
+    sockaddr_in6 sender;
+    int status = 0;
+    socklen_t addrlen = sizeof(sender);
+    memset(&sender, 0, addrlen);
+    char addrBuf[INET6_ADDRSTRLEN];
+
+    if (_castStat == STAT_SSL)
+    {
+        D_NWLOG("Ucast ");
+        if (SSL_read(_ssl, buf, length) == 0)
+        {
+            return 0;
+        }
+    }
+    else if (_castStat == STAT_MULTICAST)
+    {
+        D_NWLOG("Mcast ");
+        status = ::recvfrom(_sockfdMcast, buf, length, flags, (sockaddr*) &sender, &addrlen);
+    }
+    else
+    {
+        return 0;
+    }
+
+    if (status < 0 && errno != EAGAIN)
+    {
+        D_NWLOG("\033[0m\033[0;31merrno == %d in LDtls6Port::recvfrom \033[0m\033[0;37m\n", errno);
+        DISPLAY("\033[0m\033[0;31merrno == %d in LDtls6Port::recvfrom \033[0m\033[0;37m\n", errno);
+    }
+    else if (status > 0)
+    {
+        inet_ntop(AF_INET6, &sender.sin6_addr, addrBuf, INET6_ADDRSTRLEN);
+        memcpy(ipAddressPtr->s6_addr, (const void*) sender.sin6_addr.s6_addr, sizeof(sender.sin6_addr.s6_addr));
+        *portPtr = sender.sin6_port;
+
+        D_NWLOG("recved %-15s:%-6u", addrBuf, htons(*portPtr));
+
+        for (uint16_t i = 0; i < status; i++)
+        {
+            D_NWLOG(" %02x", *(buf + i));
+        }D_NWLOG("\n");
+
+        if (!theClientMode)
+        {
+            char sbuf[SCREEN_BUFF_SIZE];
+            int pos = 0;
+            sprintf(sbuf, "\033[0;34mrecved %-15s:%-6u", addrBuf, htons(*portPtr));
+            pos = strlen(sbuf);
+            for (uint16_t i = 0; i < status; i++)
+            {
+                sprintf(sbuf + pos, " %02x", *(buf + i));
+                if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20)
+                {
+                    break;
+                }
+                pos += 3;
+            }
+            sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
+            theScreen->display(sbuf);
+        }
+        return status;
+    }
+    else
+    {
+        return 0;
+    }
+    return status;
+}
+
+int LDtls6Port::sslConnect(in6_addr ipAddress, in_port_t portNo)
+{
+    int optval = 1;
+
+    if (_ssl != 0)
+    {
+        D_NWLOG("LDtls6Port::sslConnect SSL exists.\n");
+        SSL_shutdown(_ssl);
+        SSL_free(_ssl);
+        _sockfdSsl = 0;
+        _ssl = 0;
+    }
+
+    if (_sockfdSsl > 0)
+    {
+        D_NWLOG("LDtls6Port::sslConnect socket exists.\n");
+        ::close(_sockfdSsl);
+    }
+
+    _sockfdSsl = socket(AF_INET6, SOCK_DGRAM, 0);
+    if (_sockfdSsl <= 0)
+    {
+        D_NWLOG("LDtls6Port::sslConnect Can't create a socket\n");
+        return -1;
+    }
+    optval = 1;
+    setsockopt(_sockfdSsl, IPPROTO_IPV6, IPV6_V6ONLY, &optval, sizeof(optval));
+    setsockopt(_sockfdSsl, SOL_SOCKET, SO_REUSEADDR || SO_REUSEPORT, &optval, sizeof(optval));
+
+    if (_ifIndex > 0)
+    {
+#ifdef __APPLE__
+     setsockopt(_sockfdSsl, IPPROTO_IP, IP_BOUND_IF, &_ifIndex, sizeof(_ifIndex));
+#else
+        setsockopt(_sockfdSsl, SOL_SOCKET, SO_BINDTODEVICE, _interfaceName.c_str(), _interfaceName.size());
+#endif
+    }
+
+    sockaddr_in6 addr;
+    addr.sin6_family = AF_INET6;
+    addr.sin6_port = _uPortNo;
+    addr.sin6_addr = in6addr_any;
+
+    if (::bind(_sockfdSsl, (struct sockaddr*) &addr, sizeof(addr)) < 0)
+    {
+        ::close(_sockfdSsl);
+        D_NWLOG("LDtlsPort::sslConnect Can't bind a socket\n");
+        return -1;
+    }
+
+    // Destination is a gateway address and portNo
+    int rc = 0;
+    sockaddr_in6 dest;
+    dest.sin6_family = AF_INET6;
+    dest.sin6_port = portNo;
+    memcpy(dest.sin6_addr.s6_addr, (const void*) ipAddress.s6_addr, sizeof(ipAddress.s6_addr));
+
+    BIO *cbio = BIO_new_dgram(_sockfdSsl, BIO_NOCLOSE);
+    if (connect(_sockfdSsl, (sockaddr*) &dest, sizeof(sockaddr_in6)) < 0)
+    {
+        D_NWLOG("socket can't connect %s\n",strerror(errno));
+        return -1;
+    }
+
+    if (BIO_ctrl(cbio, BIO_CTRL_DGRAM_SET_CONNECTED, 0, &dest) <0)
+    {
+        D_NWLOG("BIO_ctrl %s\n",strerror(errno));
+        return -1;
+    }
+
+    _ssl = SSL_new(_ctx);
+    if (_ssl == nullptr)
+    {
+        D_NWLOG("SSL_new  %s\n",strerror(errno));
+       return -1;
+    }
+    SSL_set_bio(_ssl, cbio, cbio);
+
+#ifdef DEBUG_NW
+    char addrBuf[INET6_ADDRSTRLEN];
+    inet_ntop(AF_INET6, &dest.sin6_addr, addrBuf, INET6_ADDRSTRLEN);
+    D_NWLOG("connect to %-15s:%-6u\n", addrBuf, ntohs(dest.sin6_port));
+#endif
+
+    timeval timeout;
+    timeout.tv_sec = 5;
+    timeout.tv_usec = 0;
+    BIO_ctrl(cbio, BIO_CTRL_DGRAM_SET_RECV_TIMEOUT, 0, &timeout);
+    errno = 0;
+
+    int stat = SSL_connect(_ssl);
+    if (stat != 1)
+    {
+        rc = -1;
+        D_NWLOG("SSL fail to connect %s\n",strerror(errno));
+    }
+    else
+    {
+        rc = 1;
+        D_NWLOG("SSL connected\n");
+    }
+    return rc;
+}
+
+int verify_cert(int ok, X509_STORE_CTX *ctx)
+{
+    return 1;
+}
+
+#endif
+
--- a/MQTTSNGateway/GatewayTester/src/LNetworkDtls6.h
+++ b/MQTTSNGateway/GatewayTester/src/LNetworkDtls6.h
@@ -0,0 +1,122 @@
+/**************************************************************************************
+ * Copyright (c) 2021, 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 NETWORKDTLS6_H_
+#define NETWORKDTLS6_H_
+
+#ifdef DTLS6
+
+#include <sys/time.h>
+#include <iostream>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netdb.h>
+#include <unistd.h>
+#include <string>
+#include <arpa/inet.h>
+#include <openssl/ssl.h>
+#include <openssl/err.h>
+#include <poll.h>
+
+#define SOCKET_MAXHOSTNAME  200
+#define SOCKET_MAXCONNECTIONS  5
+#define SOCKET_MAXRECV  500
+#define SOCKET_MAXBUFFER_LENGTH 500 // buffer size
+
+#define STAT_NONE   0
+#define STAT_UNICAST   1
+#define STAT_MULTICAST 2
+#define STAT_SSL 3
+
+using namespace std;
+
+namespace linuxAsyncClient {
+/*========================================
+ Class LDtls6Port
+ =======================================*/
+class LDtls6Port
+{
+    friend class LNetwork;
+public:
+    LDtls6Port();
+    virtual ~LDtls6Port();
+
+    bool open(LUdp6Config* config);
+
+    int unicast(const uint8_t *buf, uint32_t length);
+    int multicast( const uint8_t* buf, uint32_t length );
+    int recv(uint8_t* buf, uint16_t len, bool nonblock, in6_addr* ipaddress, in_port_t* port );
+    int recv(uint8_t* buf, int flags);
+    bool checkRecvBuf();
+    bool isUnicast();
+    SSL* getSSL(void);
+    int sslConnect(in6_addr ipAddress, uint16_t port);
+private:
+    void close();
+    int recvfrom ( uint8_t* buf, uint16_t len, int flags, in6_addr* ipaddress, in_port_t* port );
+
+    int _sockfdMcast;
+    int _sockfdSsl;
+    SSL_CTX *_ctx;
+    SSL *_ssl;
+    in_port_t _gPortNo;
+    in_port_t _uPortNo;
+    sockaddr_in6 _gIpAddr;
+    char *_gIpAddrStr;
+    uint32_t _ifIndex;
+    string _interfaceName;
+    uint8_t  _castStat;
+    bool   _disconReq;
+
+};
+
+#define NO_ERROR    0
+#define PACKET_EXCEEDS_LENGTH  1
+/*===========================================
+               Class  Network
+ ============================================*/
+class LNetwork: public LDtls6Port
+{
+public:
+    LNetwork();
+    ~LNetwork();
+
+    int  broadcast(const uint8_t* payload, uint16_t payloadLen);
+    int  unicast(const uint8_t* payload, uint16_t payloadLen);
+    void setGwAddress(void);
+    void resetGwAddress(void);
+    bool initialize(LUdp6Config* config);
+    uint8_t*  getMessage(int* len);
+    bool isBroadcastable();
+    int sslConnect(void);
+private:
+    void setSleep();
+    int  readApiFrame(void);
+
+    in6_addr _gwIpAddress;
+    in6_addr _ipAddress;
+    in_port_t _gwPortNo;
+    in_port_t _portNo;
+    int     _returnCode;
+    bool _sleepflg;
+    uint8_t _rxDataBuf[MQTTSN_MAX_PACKET_SIZE + 1];  // defined in MqttsnClientApp.h
+
+};
+
+}    /* end of namespace */
+#endif /* DTLS6 */
+#endif /* NETWORKDTLS6_H_ */
--- a/MQTTSNGateway/GatewayTester/src/LNetworkRfcomm.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LNetworkRfcomm.cpp
@@ -0,0 +1,286 @@
+/**************************************************************************************
+ * Copyright (c) 2021, 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
+ **************************************************************************************/
+#ifdef RFCOMM
+
+#include <stdio.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/rfcomm.h>
+
+#include "LMqttsnClientApp.h"
+#include "LNetworkRfcomm.h"
+#include "LTimer.h"
+#include "LScreen.h"
+
+using namespace std;
+using namespace linuxAsyncClient;
+
+extern uint16_t getUint16(const uint8_t* pos);
+extern uint32_t getUint32(const uint8_t* pos);
+extern LScreen* theScreen;
+extern bool theClientMode;
+extern LRfcommConfig theNetcon;
+/*=========================================
+ Class LNetwork
+ =========================================*/
+LNetwork::LNetwork()
+{
+    _sleepflg = false;
+    _returnCode = 0;
+}
+
+LNetwork::~LNetwork()
+{
+
+}
+
+int LNetwork::broadcast(const uint8_t* xmitData, uint16_t dataLen)
+{
+    return LRfcommPort::unicast(xmitData, dataLen);
+}
+
+int LNetwork::unicast(const uint8_t* xmitData, uint16_t dataLen)
+{
+    return LRfcommPort::unicast(xmitData, dataLen);
+}
+
+uint8_t* LNetwork::getMessage(int* len)
+{
+    *len = 0;
+    if (checkRecvBuf())
+    {
+        uint16_t recvLen = LRfcommPort::recv(_rxDataBuf, MQTTSN_MAX_PACKET_SIZE, false);
+
+        if (recvLen < 0)
+        {
+            *len = recvLen;
+            return 0;
+        }
+        else
+        {
+            if (_rxDataBuf[0] == 0x01)
+            {
+                *len = getUint16(_rxDataBuf + 1);
+            }
+            else
+            {
+                *len = _rxDataBuf[0];
+            }
+            return _rxDataBuf;
+        }
+    }
+    return 0;
+}
+
+void LNetwork::setGwAddress(void)
+{
+}
+
+void LNetwork::setFixedGwAddress(void)
+{
+    _channel = LRfcommPort::_channel;
+    str2ba( theNetcon.gwAddress, (bdaddr_t*)_gwAddress);
+}
+
+bool LNetwork::initialize(LRfcommConfig* config)
+{
+    return LRfcommPort::open(config);
+}
+
+void LNetwork::setSleep()
+{
+    _sleepflg = true;
+}
+
+bool LNetwork::isBroadcastable()
+{
+    return false;
+}
+
+/*=========================================
+ Class RFCOMM Stack
+ =========================================*/
+LRfcommPort::LRfcommPort()
+{
+    _disconReq = false;
+    _sockRfcomm = 0;
+    _channel = 0;
+}
+
+LRfcommPort::~LRfcommPort()
+{
+    close();
+}
+
+void LRfcommPort::close()
+{
+    if (_sockRfcomm > 0)
+    {
+        ::close(_sockRfcomm);
+        _sockRfcomm = 0;
+    }
+}
+
+bool LRfcommPort::open(LRfcommConfig* config)
+{
+    const int reuse = 1;
+    str2ba(config->gwAddress, (bdaddr_t*)_gwAddress);
+    _channel = config->channel;
+
+    if (_channel == 0 || _gwAddress == 0 )
+    {
+        D_NWLOG("\033[0m\033[0;31merror Bluetooth Address in LRfcommPort::open\033[0m\033[0;37m\n");
+        DISPLAY("\033[0m\033[0;31m\nerror Bluetooth Address in LRfcommPort::open\033[0m\033[0;37m\n");
+        return false;
+    }
+
+    _sockRfcomm = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
+    if (_sockRfcomm < 0)
+    {
+        D_NWLOG("\033[0m\033[0;31merror Can't create socket in LRfcommPort::open\033[0m\033[0;37m\n");
+        DISPLAY("\033[0m\033[0;31m\nerror Can't create socket in LRfcommPort::open\033[0m\033[0;37m\n");
+        return false;
+    }
+
+    setsockopt(_sockRfcomm, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
+
+    struct sockaddr_rc addru = { 0 };
+    addru.rc_family = AF_BLUETOOTH;
+    addru.rc_channel = _channel;
+    memcpy(&addru.rc_bdaddr, _gwAddress, 6);
+
+    char bufgw[30];
+    ba2str(&addru.rc_bdaddr, bufgw);
+    DISPLAY("GW MAC = %s  RFCOMM CH = %d\n", bufgw, addru.rc_channel);
+
+    // connect to server
+    errno = 0;
+    int status = connect(_sockRfcomm, (struct sockaddr *) &addru, sizeof(addru));
+    if (status < 0)
+    {
+        D_NWLOG("\033[0m\033[0;31merror = %d Can't connect to GW in LRfcommPort::open\033[0m\033[0;37m\n", errno);
+        DISPLAY("\033[0m\033[0;31merror = %d Can't connect to GW Ble socket in LRfcommPort::open\033[0m\033[0;37m\n",errno);
+        close();
+        return false;
+    }
+    return true;
+}
+
+int LRfcommPort::unicast(const uint8_t* buf, uint32_t length)
+{
+    int status = ::write(_sockRfcomm, buf, length);
+    if (status < 0)
+    {
+        D_NWLOG("errno == %d in LRfcommPort::unicast\n", errno);
+        DISPLAY("errno == %d in LRfcommPort::unicast\n", errno);
+    }
+    else
+    {
+        D_NWLOG("sendto %-2d", _channel);
+        for (uint16_t i = 0; i < length; i++)
+        {
+            D_NWLOG(" %02x", *(buf + i));
+        }
+        D_NWLOG("\n");
+
+        if (!theClientMode)
+        {
+            char sbuf[SCREEN_BUFF_SIZE];
+            int pos = 0;
+            sprintf(sbuf, "\033[0;34msendto %-2dch", _channel);
+            pos = strlen(sbuf);
+            for (uint16_t i = 0; i < length; i++)
+            {
+                sprintf(sbuf + pos, " %02x", *(buf + i));
+                if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20)  // -20 for Escape sequence
+                {
+                    break;
+                }
+                pos += 3;
+            }
+            sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
+            theScreen->display(sbuf);
+        }
+    }
+    return status;
+}
+
+bool LRfcommPort::checkRecvBuf()
+{
+    uint8_t buf[2];
+    if (::recv(_sockRfcomm, buf, 1, MSG_DONTWAIT | MSG_PEEK) > 0)
+    {
+        return true;
+    }
+    return false;
+}
+
+int LRfcommPort::recv(uint8_t* buf, uint16_t length, bool flg)
+{
+    int flags = flg ? MSG_DONTWAIT : 0;
+    int status = ::recv(_sockRfcomm, buf, length, flags);
+
+    if (status < 0 && errno != EAGAIN)
+    {
+        D_NWLOG("\033[0m\033[0;31merrno = %d in LRfcommPort::recv \033[0m\033[0;37m\n", errno);
+        DISPLAY("\033[0m\033[0;31merrno = %d in LRfcommPort::recv \033[0m\033[0;37m\n", errno);
+    }
+    else if (status > 0)
+    {
+        D_NWLOG("\nrecved  ");
+        for (uint16_t i = 0; i < status; i++)
+        {
+            D_NWLOG(" %02x", *(buf + i));
+        }
+        D_NWLOG("\n");
+
+        if (!theClientMode)
+        {
+            char sbuf[SCREEN_BUFF_SIZE];
+            int pos = 0;
+            sprintf(sbuf, "\033[0;34mrecved ");
+            pos = strlen(sbuf);
+            for (uint16_t i = 0; i < status; i++)
+            {
+                sprintf(sbuf + pos, " %02x", *(buf + i));
+                if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20)
+                {
+                    break;
+                }
+                pos += 3;
+            }
+            sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
+            theScreen->display(sbuf);
+        }
+        return status;
+    }
+    else
+    {
+        return 0;
+    }
+    return status;
+}
+
+#endif
+
--- a/MQTTSNGateway/GatewayTester/src/LNetworkRfcomm.h
+++ b/MQTTSNGateway/GatewayTester/src/LNetworkRfcomm.h
@@ -0,0 +1,102 @@
+/**************************************************************************************
+ * Copyright (c) 2021, 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 NETWORKRFCOMM_H_
+#define NETWORKRFCOMM_H_
+
+#ifdef RFCOMM
+
+#include <sys/time.h>
+#include <iostream>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netdb.h>
+#include <unistd.h>
+#include <string>
+#include <bluetooth/bluetooth.h>
+
+
+#define SOCKET_MAXHOSTNAME  200
+#define SOCKET_MAXCONNECTIONS  5
+#define SOCKET_MAXRECV  500
+#define SOCKET_MAXBUFFER_LENGTH 500 // buffer size
+
+#define STAT_UNICAST   1
+#define STAT_MULTICAST 2
+
+using namespace std;
+
+namespace linuxAsyncClient
+{
+/*========================================
+     Class LRfcommPort
+ =======================================*/
+class LRfcommPort
+{
+    friend class LNetwork;
+public:
+    LRfcommPort();
+    virtual ~LRfcommPort();
+    bool open(LRfcommConfig* config);
+    int unicast(const uint8_t* buf, uint32_t length);
+    int recv(uint8_t* buf, uint16_t len, bool nonblock);
+    bool checkRecvBuf();
+    bool isUnicast();
+
+private:
+    void close();
+
+    int _sockRfcomm;
+    uint8_t _gwAddress[6];
+    uint8_t _channel;
+    bool _disconReq;
+
+};
+
+#define NO_ERROR    0
+#define PACKET_EXCEEDS_LENGTH  1
+/*===========================================
+ Class  Network
+ ============================================*/
+class LNetwork: public LRfcommPort
+{
+public:
+    LNetwork();
+    ~LNetwork();
+
+    int broadcast(const uint8_t* payload, uint16_t payloadLen);
+    int unicast(const uint8_t* payload, uint16_t payloadLen);
+    void setGwAddress(void);
+    void resetGwAddress(void);
+    void setFixedGwAddress(void);
+        bool initialize(LRfcommConfig* config);
+    uint8_t* getMessage(int* len);
+    bool isBroadcastable();
+
+private:
+    void setSleep();
+    int readApiFrame(void);
+
+    int _returnCode;
+    bool _sleepflg;
+    uint8_t _rxDataBuf[MQTTSN_MAX_PACKET_SIZE + 1];  // defined in MqttsnClientApp.h
+
+};
+
+} /* end of namespace */
+#endif /* RFCOMM */
+#endif /* NETWORKRFCOM_H_ */
--- a/MQTTSNGateway/GatewayTester/src/LNetworkUdp.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LNetworkUdp.cpp
@@ -13,6 +13,7 @@
 * Contributors:
 *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
 **************************************************************************************/
+#ifdef UDP

 #include <stdio.h>
 #include <sys/time.h>
@@ -23,368 +24,428 @@
 #include <stdlib.h>
 #include <string.h>
 #include <fcntl.h>
-#include <errno.h>
 #include <termios.h>

+#include "LMqttsnClientApp.h"
 #include "LNetworkUdp.h"
 #include "LTimer.h"
 #include "LScreen.h"

-#include "LMqttsnClientApp.h"
-
 using namespace std;
 using namespace linuxAsyncClient;

-extern uint16_t getUint16(const uint8_t* pos);
-extern uint32_t getUint32(const uint8_t* pos);
-extern LScreen* theScreen;
+extern uint16_t getUint16(const uint8_t *pos);
+extern uint32_t getUint32(const uint8_t *pos);
+extern LScreen *theScreen;
 extern bool theClientMode;
 /*=========================================
-       Class LNetwork
+ Class LNetwork
 =========================================*/
-LNetwork::LNetwork(){
-	_sleepflg = false;
-	resetGwAddress();
+LNetwork::LNetwork()
+{
+    _sleepflg = false;
+    resetGwAddress();
 }

-LNetwork::~LNetwork(){
+LNetwork::~LNetwork()
+{

 }

-int LNetwork::broadcast(const uint8_t* xmitData, uint16_t dataLen){
-	return LUdpPort::multicast(xmitData, (uint32_t)dataLen);
+int LNetwork::broadcast(const uint8_t *xmitData, uint16_t dataLen)
+{
+    return LUdpPort::multicast(xmitData, (uint32_t) dataLen);
 }

-int  LNetwork::unicast(const uint8_t* xmitData, uint16_t dataLen){
-	return LUdpPort::unicast(xmitData, dataLen, _gwIpAddress, _gwPortNo);
+int LNetwork::unicast(const uint8_t *xmitData, uint16_t dataLen)
+{
+    return LUdpPort::unicast(xmitData, dataLen, _gwIpAddress, _gwPortNo);
 }

+uint8_t* LNetwork::getMessage(int *len)
+{
+    *len = 0;
+    if (checkRecvBuf())
+    {
+        uint16_t recvLen = LUdpPort::recv(_rxDataBuf, MQTTSN_MAX_PACKET_SIZE, false, &_ipAddress, &_portNo);
+        if (_gwIpAddress && isUnicast() && (_ipAddress != _gwIpAddress) && (_portNo != _gwPortNo))
+        {
+            return 0;
+        }

-uint8_t*  LNetwork::getMessage(int* len){
-	*len = 0;
-	if (checkRecvBuf()){
-		uint16_t recvLen = LUdpPort::recv(_rxDataBuf, MQTTSN_MAX_PACKET_SIZE, false, &_ipAddress, &_portNo);
-		if(_gwIpAddress && isUnicast() && (_ipAddress != _gwIpAddress) && (_portNo != _gwPortNo)){
-			return 0;
-		}
-
-		if(recvLen < 0){
-			*len = recvLen;
-			return 0;
-		}else{
-			if(_rxDataBuf[0] == 0x01){
-				*len = getUint16(_rxDataBuf + 1 );
-			}else{
-				*len = _rxDataBuf[0];
-			}
-			if(recvLen != *len){
-				*len = 0;
-				return 0;
-			}else{
-				return _rxDataBuf;
-			}
-		}
-	}
-	return 0;
+        if (recvLen < 0)
+        {
+            *len = recvLen;
+            return 0;
+        }
+        else
+        {
+            if (_rxDataBuf[0] == 0x01)
+            {
+                *len = getUint16(_rxDataBuf + 1);
+            }
+            else
+            {
+                *len = _rxDataBuf[0];
+            }
+            //if(recvLen != *len){
+            //	*len = 0;
+            //	return 0;
+            //}else{
+            return _rxDataBuf;
+            //}
+        }
+    }
+    return 0;
 }

-void LNetwork::setGwAddress(void){
-	_gwPortNo = _portNo;
-	_gwIpAddress = _ipAddress;
+void LNetwork::setGwAddress(void)
+{
+    _gwPortNo = _portNo;
+    _gwIpAddress = _ipAddress;
 }

-void LNetwork::setFixedGwAddress(void){
+void LNetwork::setFixedGwAddress(void)
+{
    _gwPortNo = LUdpPort::_gPortNo;
    _gwIpAddress = LUdpPort::_gIpAddr;
 }

-void LNetwork::resetGwAddress(void){
-	_gwIpAddress = 0;
-	_gwPortNo = 0;
+void LNetwork::resetGwAddress(void)
+{
+    _gwIpAddress = 0;
+    _gwPortNo = 0;
 }

-
-bool LNetwork::initialize(LUdpConfig  config){
-	return LUdpPort::open(config);
+bool LNetwork::initialize(LUdpConfig *config)
+{
+    return LUdpPort::open(config);
 }

-void LNetwork::setSleep(){
-	_sleepflg = true;
+void LNetwork::setSleep()
+{
+    _sleepflg = true;
 }

+bool LNetwork::isBroadcastable()
+{
+    return true;
+}
 /*=========================================
-       Class udpStack
+ Class udpStack
 =========================================*/
-LUdpPort::LUdpPort(){
+LUdpPort::LUdpPort()
+{
    _disconReq = false;
    _sockfdUcast = -1;
    _sockfdMcast = -1;
    _castStat = 0;
 }

-LUdpPort::~LUdpPort(){
+LUdpPort::~LUdpPort()
+{
    close();
 }

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

-bool LUdpPort::open(LUdpConfig config){
-	const int reuse = 1;
-	char loopch = 1;
+bool LUdpPort::open(LUdpConfig *config)
+{
+    int optval = 0;

-	uint8_t sav = config.ipAddress[3];
-	config.ipAddress[3] = config.ipAddress[0];
-	config.ipAddress[0] = sav;
-	sav = config.ipAddress[2];
-	config.ipAddress[2] = config.ipAddress[1];
-	config.ipAddress[1] = sav;
+    uint8_t sav = config->ipAddress[3];
+    config->ipAddress[3] = config->ipAddress[0];
+    config->ipAddress[0] = sav;
+    sav = config->ipAddress[2];
+    config->ipAddress[2] = config->ipAddress[1];
+    config->ipAddress[1] = sav;

-	_gPortNo = htons(config.gPortNo);
-	_gIpAddr = getUint32((const uint8_t*)config.ipAddress);
-	_uPortNo = htons(config.uPortNo);
+    _gPortNo = htons(config->gPortNo);
+    _gIpAddr = getUint32((const uint8_t*) config->ipAddress);
+    _uPortNo = htons(config->uPortNo);

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

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

-	setsockopt(_sockfdUcast, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
+    optval = 1;
+    setsockopt(_sockfdUcast, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));

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

-	if( ::bind ( _sockfdUcast, (struct sockaddr*)&addr,  sizeof(addr)) <0){
-		return false;
-	}
+    if (::bind(_sockfdUcast, (struct sockaddr*) &addr, sizeof(addr)) < 0)
+    {
+        D_NWLOG("\033[0m\033[0;31merror %s ::bind() to unicast address\033[0m\033[0;37m\n", strerror(errno));
+        return false;
+    }

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

-	struct sockaddr_in addrm;
-	addrm.sin_family = AF_INET;
-	addrm.sin_port = _gPortNo;
-	addrm.sin_addr.s_addr = htonl(INADDR_ANY);
+    sockaddr_in addrm;
+    addrm.sin_family = AF_INET;
+    addrm.sin_port = _gPortNo;
+    addrm.sin_addr.s_addr = INADDR_ANY;

-	setsockopt(_sockfdMcast, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
+    optval = 1;
+    setsockopt(_sockfdMcast, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));

-	if( ::bind ( _sockfdMcast, (struct sockaddr*)&addrm,  sizeof(addrm)) <0){
-		return false;
-	}
+    if (::bind(_sockfdMcast, (struct sockaddr*) &addrm, sizeof(addrm)) < 0)
+    {
+        D_NWLOG("\033[0m\033[0;31merror %s ::bind() in UdpPort::open\033[0m\033[0;37m\n", strerror(errno));
+        return false;
+    }

-	if(setsockopt(_sockfdMcast, IPPROTO_IP, IP_MULTICAST_LOOP,(char*)&loopch, sizeof(loopch)) <0 ){
-		D_NWLOG("\033[0m\033[0;31merror IP_MULTICAST_LOOP in UdpPPort::open\033[0m\033[0;37m\n");
-		DISPLAY("\033[0m\033[0;31m\nerror IP_MULTICAST_LOOP in UdpPPort::open\033[0m\033[0;37m\n");
-		close();
-		return false;
-	}
+    ip_mreq mreq;
+    mreq.imr_interface.s_addr = INADDR_ANY;
+    mreq.imr_multiaddr.s_addr = _gIpAddr;

-	ip_mreq mreq;
-	mreq.imr_interface.s_addr = INADDR_ANY;
-	mreq.imr_multiaddr.s_addr = _gIpAddr;
+    if (setsockopt(_sockfdMcast, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0)
+    {
+        D_NWLOG("\033[0m\033[0;31merror IP_ADD_MEMBERSHIP in UdpPort::open\033[0m\033[0;37m\n");
+        close();
+        return false;
+    }

-	if( setsockopt(_sockfdMcast, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq) )< 0){
-		D_NWLOG("\033[0m\033[0;31merror IP_ADD_MEMBERSHIP in UdpPort::open\033[0m\033[0;37m\n");
-		DISPLAY("\033[0m\033[0;31m\nerror IP_ADD_MEMBERSHIP in UdpPort::open\033[0m\033[0;37m\n");
-		close();
-		return false;
-	}
-	return true;
+    optval= 1;
+    if (setsockopt(_sockfdMcast, IPPROTO_IP, IP_MULTICAST_LOOP, &optval, sizeof(optval)) < 0)
+    {
+        D_NWLOG("\033[0m\033[0;31merror IP_MULTICAST_LOOP in UdpPPort::open\033[0m\033[0;37m\n");
+        close();
+        return false;
+    }
+    return true;
 }

-bool LUdpPort::isUnicast(){
-	return ( _castStat == STAT_UNICAST);
+bool LUdpPort::isUnicast()
+{
+    return (_castStat == STAT_UNICAST);
 }

+int LUdpPort::unicast(const uint8_t *buf, uint32_t length, uint32_t ipAddress, uint16_t port)
+{
+    struct sockaddr_in dest;
+    dest.sin_family = AF_INET;
+    dest.sin_port = port;
+    dest.sin_addr.s_addr = ipAddress;

-int LUdpPort::unicast(const uint8_t* buf, uint32_t length, uint32_t ipAddress, uint16_t port  ){
-	struct sockaddr_in dest;
-	dest.sin_family = AF_INET;
-	dest.sin_port = port;
-	dest.sin_addr.s_addr = ipAddress;
+    int status = ::sendto(_sockfdUcast, buf, length, 0, (const sockaddr*) &dest, sizeof(dest));
+    if (status < 0)
+    {
+        D_NWLOG("errno == %d in UdpPort::unicast\n", errno);
+    }
+    else
+    {
+        D_NWLOG("sendto %-15s:%-6u",inet_ntoa(dest.sin_addr),htons(port));
+        for (uint16_t i = 0; i < length; i++)
+        {
+            D_NWLOG(" %02x", *(buf + i));
+        } D_NWLOG("\n");

-	int status = ::sendto( _sockfdUcast, buf, length, 0, (const sockaddr*)&dest, sizeof(dest) );
-	if( status < 0){
-		D_NWLOG("errno == %d in UdpPort::unicast\n", errno);
-		DISPLAY("errno == %d in UdpPort::unicast\n", errno);
-	}else{
-		D_NWLOG("sendto %-15s:%-6u",inet_ntoa(dest.sin_addr),htons(port));
-		for(uint16_t i = 0; i < length ; i++){
-			D_NWLOG(" %02x", *(buf + i));
-		}
-		D_NWLOG("\n");
-
-		if ( !theClientMode )
-		{
-			char sbuf[SCREEN_BUFF_SIZE];
-			int pos = 0;
-			sprintf(sbuf,"\033[0;34msendto %-15s:%-6u",inet_ntoa(dest.sin_addr),htons(port));
-			pos = strlen(sbuf);
-			for(uint16_t i = 0; i < length ; i++){
-				sprintf(sbuf + pos, " %02x", *(buf + i));
-				if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20 )  // -20 for Escape sequence
-				{
-					break;
-				}
-				pos += 3;
-			}
-			sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
-			theScreen->display(sbuf);
-		}
-	}
-	return status;
+        if (!theClientMode)
+        {
+            char sbuf[SCREEN_BUFF_SIZE];
+            int pos = 0;
+            sprintf(sbuf, "\033[0;34msendto %-15s:%-6u", inet_ntoa(dest.sin_addr), htons(port));
+            pos = strlen(sbuf);
+            for (uint16_t i = 0; i < length; i++)
+            {
+                sprintf(sbuf + pos, " %02x", *(buf + i));
+                if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20)  // -20 for Escape sequence
+                {
+                    break;
+                }
+                pos += 3;
+            }
+            sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
+        }
+    }
+    return status;
 }

+int LUdpPort::multicast(const uint8_t *buf, uint32_t length)
+{
+    struct sockaddr_in dest;
+    dest.sin_family = AF_INET;
+    dest.sin_port = _gPortNo;
+    dest.sin_addr.s_addr = _gIpAddr;

-int LUdpPort::multicast( const uint8_t* buf, uint32_t length ){
-	struct sockaddr_in dest;
-	dest.sin_family = AF_INET;
-	dest.sin_port = _gPortNo;
-	dest.sin_addr.s_addr = _gIpAddr;
+    int status = ::sendto(_sockfdMcast, buf, length, 0, (const sockaddr*) &dest, sizeof(dest));
+    if (status < 0)
+    {
+        D_NWLOG("\033[0m\033[0;31merrno == %d in UdpPort::multicast\033[0m\033[0;37m\n", errno);
+        return errno;
+    }
+    else
+    {
+        D_NWLOG("sendto %-15s:%-6u",inet_ntoa(dest.sin_addr),htons(_gPortNo));

-	int status = ::sendto( _sockfdMcast, buf, length, 0, (const sockaddr*)&dest, sizeof(dest) );
-	if( status < 0){
-		D_NWLOG("\033[0m\033[0;31merrno == %d in UdpPort::multicast\033[0m\033[0;37m\n", errno);
-		DISPLAY("\033[0m\033[0;31merrno == %d in UdpPort::multicast\033[0m\033[0;37m\n", errno);
-		return errno;
-	}else{
-		D_NWLOG("sendto %-15s:%-6u",inet_ntoa(dest.sin_addr),htons(_gPortNo));
+        for (uint16_t i = 0; i < length; i++)
+        {
+            D_NWLOG(" %02x", *(buf + i));
+        } D_NWLOG("\n");

-		for(uint16_t i = 0; i < length ; i++){
-			D_NWLOG(" %02x", *(buf + i));
-			DISPLAY(" %02x", *(buf + i));
-		}
-		D_NWLOG("\n");
-
-		if ( !theClientMode )
-		{
-			char sbuf[SCREEN_BUFF_SIZE];
-			int pos = 0;
-			sprintf(sbuf,"\033[0;34msendto %-15s:%-6u",inet_ntoa(dest.sin_addr),htons(_gPortNo));
-			pos = strlen(sbuf);
-			for(uint16_t i = 0; i < length ; i++){
-				sprintf(sbuf + pos, " %02x", *(buf + i));
-				if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20 )
-				{
-					break;
-				}
-				pos += 3;
-			}
-			sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
-			theScreen->display(sbuf);
-		}
-		return status;
-	}
+        if (!theClientMode)
+        {
+            char sbuf[SCREEN_BUFF_SIZE];
+            int pos = 0;
+            sprintf(sbuf, "\033[0;34msendto %-15s:%-6u", inet_ntoa(dest.sin_addr), htons(_gPortNo));
+            pos = strlen(sbuf);
+            for (uint16_t i = 0; i < length; i++)
+            {
+                sprintf(sbuf + pos, " %02x", *(buf + i));
+                if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20)
+                {
+                    break;
+                }
+                pos += 3;
+            }
+            sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
+            theScreen->display(sbuf);
+        }
+        return status;
+    }

 }

-bool LUdpPort::checkRecvBuf(){
-	struct timeval timeout;
-	timeout.tv_sec = 0;
-	timeout.tv_usec = 50000;    // 50 msec
+bool LUdpPort::checkRecvBuf()
+{
+    struct timeval timeout;
+    timeout.tv_sec = 0;
+    timeout.tv_usec = 50000;    // 50 msec

-	uint8_t buf[2];
-	fd_set recvfds;
-	int maxSock = 0;
+    uint8_t buf[2];
+    fd_set recvfds;
+    int maxSock = 0;

-	FD_ZERO(&recvfds);
-	FD_SET(_sockfdUcast, &recvfds);
-	FD_SET(_sockfdMcast, &recvfds);
+    FD_ZERO(&recvfds);
+    FD_SET(_sockfdUcast, &recvfds);
+    FD_SET(_sockfdMcast, &recvfds);

-	if(_sockfdMcast > _sockfdUcast){
-		maxSock = _sockfdMcast;
-	}else{
-		maxSock = _sockfdUcast;
-	}
+    if (_sockfdMcast > _sockfdUcast)
+    {
+        maxSock = _sockfdMcast;
+    }
+    else
+    {
+        maxSock = _sockfdUcast;
+    }

-	select(maxSock + 1, &recvfds, 0, 0, &timeout);
+    select(maxSock + 1, &recvfds, 0, 0, &timeout);

-	if(FD_ISSET(_sockfdUcast, &recvfds)){
-		if( ::recv(_sockfdUcast, buf, 1,  MSG_DONTWAIT | MSG_PEEK) > 0){
-			_castStat = STAT_UNICAST;
-			return true;
-		}
-	}else if(FD_ISSET(_sockfdMcast, &recvfds)){
-		if( ::recv(_sockfdMcast, buf, 1,  MSG_DONTWAIT | MSG_PEEK) > 0){
-			_castStat = STAT_MULTICAST;
-			return true;
-		}
-	}
-	_castStat = 0;
-	return false;
+    if (FD_ISSET(_sockfdUcast, &recvfds))
+    {
+        if (::recv(_sockfdUcast, buf, 1, MSG_DONTWAIT | MSG_PEEK) > 0)
+        {
+            _castStat = STAT_UNICAST;
+            return true;
+        }
+    }
+    else if (FD_ISSET(_sockfdMcast, &recvfds))
+    {
+        if (::recv(_sockfdMcast, buf, 1, MSG_DONTWAIT | MSG_PEEK) > 0)
+        {
+            _castStat = STAT_MULTICAST;
+            return true;
+        }
+    }
+    _castStat = 0;
+    return false;
 }

-int LUdpPort::recv(uint8_t* buf, uint16_t len, bool flg, uint32_t* ipAddressPtr, uint16_t* portPtr){
-	int flags = flg ? MSG_DONTWAIT : 0;
-	return recvfrom (buf, len, flags, ipAddressPtr, portPtr );
+int LUdpPort::recv(uint8_t *buf, uint16_t len, bool flg, uint32_t *ipAddressPtr, uint16_t *portPtr)
+{
+    int flags = flg ? MSG_DONTWAIT : 0;
+    return recvfrom(buf, len, flags, ipAddressPtr, portPtr);
 }

-int LUdpPort::recvfrom (uint8_t* buf, uint16_t length, int flags, uint32_t* ipAddressPtr, uint16_t* portPtr ){
-	struct sockaddr_in sender;
-	int status;
-	socklen_t addrlen = sizeof(sender);
-	memset(&sender, 0, addrlen);
+int LUdpPort::recvfrom(uint8_t *buf, uint16_t length, int flags, uint32_t *ipAddressPtr, uint16_t *portPtr)
+{
+    struct sockaddr_in sender;
+    int status;
+    socklen_t addrlen = sizeof(sender);
+    memset(&sender, 0, addrlen);

-	if(isUnicast()){
-		status = ::recvfrom( _sockfdUcast, buf, length, flags, (struct sockaddr*)&sender, &addrlen );
-	}else if(_castStat == STAT_MULTICAST){
-		status = ::recvfrom( _sockfdMcast, buf, length, flags, (struct sockaddr*)&sender, &addrlen );
-	}else{
-		return 0;
-	}
+    if (isUnicast())
+    {
+        D_NWLOG("Ucast ");
+        status = ::recvfrom(_sockfdUcast, buf, length, flags, (struct sockaddr*) &sender, &addrlen);
+    }
+    else if (_castStat == STAT_MULTICAST)
+    {
+        D_NWLOG("Mcast ");
+        status = ::recvfrom(_sockfdMcast, buf, length, flags, (struct sockaddr*) &sender, &addrlen);
+    }
+    else
+    {
+        return 0;
+    }

-	if (status < 0 && errno != EAGAIN)	{
-		D_NWLOG("\033[0m\033[0;31merrno == %d in UdpPort::recvfrom \033[0m\033[0;37m\n", errno);
-		DISPLAY("\033[0m\033[0;31merrno == %d in UdpPort::recvfrom \033[0m\033[0;37m\n", errno);
-	}else if(status > 0){
-		*ipAddressPtr = sender.sin_addr.s_addr;
-		*portPtr = sender.sin_port;
-		D_NWLOG("\nrecved %-15s:%-6u",inet_ntoa(sender.sin_addr), htons(*portPtr));
-		for(uint16_t i = 0; i < status ; i++){
-			D_NWLOG(" %02x", *(buf + i));
-		}
-		D_NWLOG("\n");
+    if (status < 0 && errno != EAGAIN)
+    {
+        D_NWLOG("\033[0m\033[0;31merrno == %d in UdpPort::recvfrom \033[0m\033[0;37m\n", errno);
+    }
+    else if (status > 0)
+    {
+        *ipAddressPtr = sender.sin_addr.s_addr;
+        *portPtr = sender.sin_port;
+        D_NWLOG("recved %-15s:%-6u",inet_ntoa(sender.sin_addr), htons(*portPtr));
+        for (uint16_t i = 0; i < status; i++)
+        {
+            D_NWLOG(" %02x", *(buf + i));
+        } D_NWLOG("\n");

-		if ( !theClientMode )
-		{
-			char sbuf[SCREEN_BUFF_SIZE];
-			int pos = 0;
-			sprintf(sbuf, "\033[0;34mrecved %-15s:%-6u",inet_ntoa(sender.sin_addr), htons(*portPtr));
-			pos = strlen(sbuf);
-			for(uint16_t i = 0; i < status ; i++){
-				sprintf(sbuf + pos, " %02x", *(buf + i));
-				if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20 )
-				{
-					break;
-				}
-				pos += 3;
-			}
-			sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
-			theScreen->display(sbuf);
-		}
-		return status;
-	}else{
-		return 0;
-	}
-	return status;
+        if (!theClientMode)
+        {
+            char sbuf[SCREEN_BUFF_SIZE];
+            int pos = 0;
+            sprintf(sbuf, "\033[0;34mrecved %-15s:%-6u", inet_ntoa(sender.sin_addr), htons(*portPtr));
+            pos = strlen(sbuf);
+            for (uint16_t i = 0; i < status; i++)
+            {
+                sprintf(sbuf + pos, " %02x", *(buf + i));
+                if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20)
+                {
+                    break;
+                }
+                pos += 3;
+            }
+            sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
+            theScreen->display(sbuf);
+        }
+        return status;
+    }
+    else
+    {
+        return 0;
+    }
+    return status;
 }

-
+#endif

--- a/MQTTSNGateway/GatewayTester/src/LNetworkUdp.h
+++ b/MQTTSNGateway/GatewayTester/src/LNetworkUdp.h
@@ -17,6 +17,8 @@
 #ifndef NETWORKUDP_H_
 #define NETWORKUDP_H_

+#ifdef UDP
+
 #include <sys/time.h>
 #include <iostream>
 #include <sys/types.h>
@@ -27,8 +29,6 @@
 #include <string>
 #include <arpa/inet.h>

-#include "LMqttsnClientApp.h"
-
 #define SOCKET_MAXHOSTNAME  200
 #define SOCKET_MAXCONNECTIONS  5
 #define SOCKET_MAXRECV  500
@@ -49,7 +49,7 @@ public:
 	LUdpPort();
 	virtual ~LUdpPort();

-	bool open(LUdpConfig config);
+	bool open(LUdpConfig* config);

 	int unicast(const uint8_t* buf, uint32_t length, uint32_t ipaddress, uint16_t port  );
 	int multicast( const uint8_t* buf, uint32_t length );
@@ -87,8 +87,9 @@ public:
    void setGwAddress(void);
    void resetGwAddress(void);
    void setFixedGwAddress(void);
-    bool initialize(LUdpConfig  config);
+    bool initialize(LUdpConfig* config);
    uint8_t*  getMessage(int* len);
+        bool isBroadcastable();
 private:
    void setSleep();
    int  readApiFrame(void);
@@ -103,6 +104,6 @@ private:

 };

-
 }    /* end of namespace */
+#endif /* UDP */
 #endif /* NETWORKUDP_H_ */
--- a/MQTTSNGateway/GatewayTester/src/LNetworkUdp6.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LNetworkUdp6.cpp
@@ -0,0 +1,433 @@
+/**************************************************************************************
+ * Copyright (c) 2021, 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
+ **************************************************************************************/
+#ifdef UDP6
+
+#include <stdio.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <netinet/in.h>
+#include <net/if.h>
+#include "LMqttsnClientApp.h"
+#include "LNetworkUdp6.h"
+#include "LTimer.h"
+#include "LScreen.h"
+
+using namespace std;
+using namespace linuxAsyncClient;
+
+extern uint16_t getUint16(const uint8_t *pos);
+extern uint32_t getUint32(const uint8_t *pos);
+extern LScreen *theScreen;
+extern bool theClientMode;
+/*=========================================
+ Class LNetwork
+ =========================================*/
+LNetwork::LNetwork()
+{
+    _sleepflg = false;
+    resetGwAddress();
+}
+
+LNetwork::~LNetwork()
+{
+
+}
+
+int LNetwork::broadcast(const uint8_t *xmitData, uint16_t dataLen)
+{
+    return LUdp6Port::multicast(xmitData, (uint32_t) dataLen);
+}
+
+int LNetwork::unicast(const uint8_t *xmitData, uint16_t dataLen)
+{
+    return LUdp6Port::unicast(xmitData, dataLen, _gwIpAddress, _gwPortNo);
+}
+
+uint8_t* LNetwork::getMessage(int *len)
+{
+    *len = 0;
+    if (checkRecvBuf())
+    {
+        uint16_t recvLen = LUdp6Port::recv(_rxDataBuf, MQTTSN_MAX_PACKET_SIZE, false, &_ipAddress, &_portNo);
+        int diffAddr = memcmp(_ipAddress.s6_addr, _gwIpAddress.s6_addr, sizeof(_gwIpAddress.s6_addr));
+        if (isUnicast() && diffAddr && (_portNo != _gwPortNo))
+        {
+            return 0;
+        }
+
+        if (recvLen < 0)
+        {
+            *len = recvLen;
+            return 0;
+        }
+        else
+        {
+            if (_rxDataBuf[0] == 0x01)
+            {
+                *len = getUint16(_rxDataBuf + 1);
+            }
+            else
+            {
+                *len = _rxDataBuf[0];
+            }
+            return _rxDataBuf;
+        }
+    }
+    return 0;
+}
+
+void LNetwork::setGwAddress(void)
+{
+    memcpy(_gwIpAddress.s6_addr, _ipAddress.s6_addr, sizeof(_gwIpAddress.s6_addr));
+    _gwPortNo = _portNo;
+}
+
+void LNetwork::resetGwAddress(void)
+{
+    memset(_gwIpAddress.s6_addr, 0, sizeof(_gwIpAddress.s6_addr));
+    _gwPortNo = 0;
+}
+
+bool LNetwork::initialize(LUdp6Config *config)
+{
+    return LUdp6Port::open(config);
+}
+
+void LNetwork::setSleep()
+{
+    _sleepflg = true;
+}
+
+bool LNetwork::isBroadcastable()
+{
+    return true;
+}
+/*=========================================
+ Class udp6Stack
+ =========================================*/
+LUdp6Port::LUdp6Port()
+{
+    _disconReq = false;
+    memset(_pollfds, 0, sizeof(_pollfds));
+    _sock = 0;
+    _interface = NULL;
+    _gIpAddrStr = NULL;
+}
+
+LUdp6Port::~LUdp6Port()
+{
+    close();
+    if (_gIpAddrStr)
+    {
+        free(_gIpAddrStr);
+    }
+    if (_interface)
+    {
+        free(_interface);
+    }
+}
+
+void LUdp6Port::close()
+{
+    for (int i = 0; i < 2; i++)
+    {
+        if (_pollfds[i].fd > 0)
+        {
+            ::close(_pollfds[i].fd);
+            _pollfds[i].fd = 0;
+        }
+    }
+}
+
+bool LUdp6Port::open(LUdp6Config *config)
+{
+    int optval = 1;
+    int sock = 0;
+    uint32_t ifindex = 0;
+    sockaddr_in6 addr6;
+
+    _gPortNo = htons(config->gPortNo);
+    _uPortNo = htons(config->uPortNo);
+
+    if (_gPortNo == 0 || _uPortNo == 0)
+    {
+        return false;
+    }
+
+    /* create a unicast socket */
+    sock = socket(AF_INET6, SOCK_DGRAM, 0);
+    if (sock < 0)
+    {
+        return false;
+    }
+
+    optval = 1;
+    setsockopt(sock, IPPROTO_IPV6, IPV6_V6ONLY, &optval, sizeof(optval));
+    setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
+
+    if (strlen(config->interface) > 0)
+    {
+        ifindex = if_nametoindex(config->interface);
+#ifdef __APPLE__
+        setsockopt(sock, IPPROTO_IP, IP_BOUND_IF, &ifindex, sizeof(ifindex));
+#else
+        setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, config->interface, strlen(config->interface));
+#endif
+    }
+
+    memset(&addr6, 0, sizeof(addr6));
+    addr6.sin6_family = AF_INET6;
+    addr6.sin6_port = _uPortNo;
+    addr6.sin6_addr = in6addr_any;
+
+    if (::bind(sock, (sockaddr*) &addr6, sizeof(addr6)) < 0)
+    {
+        D_NWLOG("\033[0m\033[0;31merror %s ::bind() to unicast address\033[0m\033[0;37m\n", strerror(errno));
+        return false;
+    }
+    _pollfds[0].fd = sock;
+    _pollfds[0].events = POLLIN;
+
+    /* create a multicast socket */
+    sock = socket(AF_INET6, SOCK_DGRAM, 0);
+    if (sock < 0)
+    {
+        return false;
+    }
+
+    optval = 1;
+    setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
+    setsockopt(sock, IPPROTO_IPV6, IPV6_V6ONLY, &optval, sizeof(optval));
+
+    memset(&addr6, 0, sizeof(addr6));
+    addr6.sin6_family = AF_INET6;
+    addr6.sin6_port = _gPortNo;
+    addr6.sin6_addr = in6addr_any;
+
+    if (::bind(sock, (sockaddr*) &addr6, sizeof(addr6)) < 0)
+    {
+        D_NWLOG("\033[0m\033[0;31merror %s ::bind() in Udp6Port::open\033[0m\033[0;37m\n", strerror(errno));
+        return false;
+    }
+
+
+    ipv6_mreq addrm;
+    addrm.ipv6mr_interface = ifindex;
+    inet_pton(AF_INET6, config->ipAddress, &addrm.ipv6mr_multiaddr);
+    if (setsockopt(sock, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, &addrm, sizeof(addrm)) < 0)
+    {
+        D_NWLOG("\033[0m\033[0;31merror %s IPV6_ADD_MEMBERSHIP in Udp6Port::open\033[0m\033[0;37m\n", strerror(errno));
+        close();
+        return false;
+    }
+    optval = 1;
+    if (setsockopt(sock, IPPROTO_IPV6, IPV6_MULTICAST_LOOP, &optval, sizeof(optval)) < 0)
+    {
+        D_NWLOG("\033[0m\033[0;31merror %s IPV6_MULTICAST_LOOP in Udp6Port::open\033[0m\033[0;37m\n", strerror(errno));
+        close();
+        return false;
+    }
+    _pollfds[1].fd = sock;
+    _pollfds[1].events = POLLIN;
+    _gIpAddr.sin6_family = AF_INET6;
+    _gIpAddr.sin6_port = _gPortNo;
+    memcpy(&_gIpAddr.sin6_addr, (const void*) &addrm.ipv6mr_multiaddr, sizeof(addrm.ipv6mr_multiaddr));
+    _gIpAddrStr = strdup(config->ipAddress);
+    return true;
+}
+
+int LUdp6Port::unicast(const uint8_t *buf, uint32_t length, in6_addr ipAddress, uint16_t port)
+{
+    struct sockaddr_in6 dest;
+    dest.sin6_family = AF_INET6;
+    dest.sin6_port = port;
+    memcpy(dest.sin6_addr.s6_addr, (const void*) ipAddress.s6_addr, sizeof(ipAddress.s6_addr));
+
+    char addrBuf[INET6_ADDRSTRLEN];
+    inet_ntop(AF_INET6, &dest.sin6_addr, addrBuf, INET6_ADDRSTRLEN);
+    D_NWLOG("unicast to [%s]:%-6u", addrBuf, htons(port));
+
+    int status = ::sendto(_pollfds[0].fd, buf, length, 0, (const sockaddr*) &dest, sizeof(dest));
+    if (status < 0)
+    {
+        D_NWLOG(" errno = %d %s  in Udp6Port::unicast\n", errno, strerror(errno));
+    }
+    else
+    {
+        for (uint16_t i = 0; i < length; i++)
+        {
+            D_NWLOG(" %02x", *(buf + i));
+        }
+        D_NWLOG("\n");
+
+        if (!theClientMode)
+        {
+            char sbuf[SCREEN_BUFF_SIZE];
+            int pos = 0;
+            sprintf(sbuf, "\033[0;34municast to [%s[:%-6u", addrBuf, htons(port));
+            pos = strlen(sbuf);
+            for (uint16_t i = 0; i < length; i++)
+            {
+                sprintf(sbuf + pos, " %02x", *(buf + i));
+                if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20)  // -20 for Escape sequence
+                {
+                    break;
+                }
+                pos += 3;
+            }
+            sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
+            theScreen->display(sbuf);
+        }
+    }
+    return status;
+}
+
+int LUdp6Port::multicast(const uint8_t *buf, uint32_t length)
+{
+    char sbuf[SCREEN_BUFF_SIZE];
+    char portStr[8];
+    sprintf(portStr, "%d", ntohs(_gPortNo));
+
+    int status = ::sendto(_pollfds[1].fd, buf, length, 0, (sockaddr*) &_gIpAddr, sizeof(_gIpAddr));
+    if (status < 0)
+    {
+        D_NWLOG("multicast to [%s]:%-6s  ", _gIpAddrStr, portStr);
+        D_NWLOG("\033[0m\033[0;31merrno = %d %s in Udp6Port::multicast\033[0m\033[0;37m\n", errno, strerror(errno));
+        return errno;
+    }
+    else
+    {
+        D_NWLOG("multicast to [%s]:%-6s", _gIpAddrStr, portStr);
+        for (uint16_t i = 0; i < length; i++)
+        {
+            D_NWLOG(" %02x", *(buf + i));
+        }
+        D_NWLOG("\n");
+
+        if (!theClientMode)
+        {
+            memset(sbuf, 0, SCREEN_BUFF_SIZE);
+            int pos = 0;
+            sprintf(sbuf, "\033[0;34mmulticast to [%s]:%-6s", _gIpAddrStr, portStr);
+            pos = strlen(sbuf);
+            for (uint16_t i = 0; i < length; i++)
+            {
+                sprintf(sbuf + pos, " %02x", *(buf + i));
+                if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20)
+                {
+                    break;
+                }
+                pos += 3;
+            }
+            sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
+            theScreen->display(sbuf);
+        }
+        return status;
+    }
+}
+
+bool LUdp6Port::checkRecvBuf()
+{
+    uint8_t buf[2];
+
+    int cnt = poll(_pollfds, 2, 50);  // Timeout 50m secs
+    if (cnt == 0)
+    {
+        return false;
+    }
+
+    for (int i = 0; i < 2; i++)
+    {
+        if (_pollfds[i].revents & POLLIN)
+        {
+            if (::recv(_pollfds[i].fd, buf, 1, MSG_DONTWAIT | MSG_PEEK) > 0)
+            {
+                _sock = _pollfds[i].fd;
+                return true;
+            }
+        }
+    }
+    return false;
+
+}
+
+int LUdp6Port::recv(uint8_t *buf, uint16_t len, bool flg, in6_addr *ipAddressPtr, uint16_t *portPtr)
+{
+    int flags = flg ? MSG_DONTWAIT : 0;
+    return recvfrom(buf, len, flags, ipAddressPtr, portPtr);
+}
+
+int LUdp6Port::recvfrom(uint8_t *buf, uint16_t length, int flags, in6_addr *ipAddressPtr, uint16_t *portPtr)
+{
+    struct sockaddr_in6 sender;
+    int status;
+    socklen_t addrlen = sizeof(sender);
+    memset(&sender, 0, addrlen);
+    char addrBuf[INET6_ADDRSTRLEN];
+
+    status = ::recvfrom(_sock, buf, length, flags, (struct sockaddr*) &sender, &addrlen);
+
+    if (status < 0 && errno != EAGAIN)
+    {
+        D_NWLOG("\033[0m\033[0;31merrno == %d in Udp6Port::recvfrom \033[0m\033[0;37m\n", errno);
+    }
+
+    if (status > 0)
+    {
+        inet_ntop(AF_INET6, &sender.sin6_addr, addrBuf, INET6_ADDRSTRLEN);
+        memcpy(ipAddressPtr->s6_addr, (const void*) sender.sin6_addr.s6_addr, sizeof(sender.sin6_addr.s6_addr));
+        *portPtr = sender.sin6_port;
+
+        if (!theClientMode)
+        {
+            char sbuf[SCREEN_BUFF_SIZE];
+            int pos = 0;
+            sprintf(sbuf, "\033[0;34mrecv from [%s]:%-6u", addrBuf, htons(*portPtr));
+            pos = strlen(sbuf);
+            for (uint16_t i = 0; i < status; i++)
+            {
+                sprintf(sbuf + pos, " %02x", *(buf + i));
+                if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20)
+                {
+                    break;
+                }
+                pos += 3;
+            }
+            sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
+            theScreen->display(sbuf);
+        }
+        return status;
+    }
+    else
+    {
+        return 0;
+    }
+    return status;
+}
+
+bool LUdp6Port::isUnicast(void)
+{
+    return (_sock == _pollfds[0].fd && _sock > 0);
+}
+#endif  /* UDP6 */
+
--- a/MQTTSNGateway/GatewayTester/src/LNetworkUdp6.h
+++ b/MQTTSNGateway/GatewayTester/src/LNetworkUdp6.h
@@ -0,0 +1,113 @@
+/**************************************************************************************
+ * Copyright (c) 2021, 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 NETWORKUDP6_H_
+#define NETWORKUDP6_H_
+
+#ifdef UDP6
+
+#include <sys/time.h>
+#include <iostream>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netdb.h>
+#include <unistd.h>
+#include <string>
+#include <arpa/inet.h>
+#include <poll.h>
+
+
+#define SOCKET_MAXHOSTNAME  200
+#define SOCKET_MAXCONNECTIONS  5
+#define SOCKET_MAXRECV  500
+#define SOCKET_MAXBUFFER_LENGTH 500 // buffer size
+
+#define STAT_UNICAST   1
+#define STAT_MULTICAST 2
+
+using namespace std;
+
+namespace linuxAsyncClient {
+/*========================================
+ Class LUpd6Port
+ =======================================*/
+class LUdp6Port
+{
+    friend class LNetwork;
+public:
+    LUdp6Port();
+    virtual ~LUdp6Port();
+
+    bool open(LUdp6Config *config);
+
+    int unicast(const uint8_t *buf, uint32_t length, in6_addr ipaddress, uint16_t port);
+	int multicast( const uint8_t* buf, uint32_t length );
+    int recv(uint8_t *buf, uint16_t len, bool nonblock, in6_addr *ipaddress, uint16_t *port);
+	int recv(uint8_t* buf, int flags);
+	bool checkRecvBuf();
+	bool isUnicast();
+
+private:
+	void close();
+    int recvfrom(uint8_t *buf, uint16_t len, int flags, in6_addr *ipaddress, uint16_t *port);
+
+    pollfd _pollfds[2];
+	uint16_t _gPortNo;
+	uint16_t _uPortNo;
+    sockaddr_in6 _gIpAddr;
+    char *_gIpAddrStr;
+    char* _interface;
+    int _sock;
+	bool   _disconReq;
+
+};
+
+#define NO_ERROR	0
+#define PACKET_EXCEEDS_LENGTH  1
+/*===========================================
+               Class  Network
+ ============================================*/
+class LNetwork: public LUdp6Port
+{
+public:
+    LNetwork();
+    ~LNetwork();
+
+    int  broadcast(const uint8_t* payload, uint16_t payloadLen);
+    int  unicast(const uint8_t* payload, uint16_t payloadLen);
+    void setGwAddress(void);
+    void resetGwAddress(void);
+    bool initialize(LUdp6Config *config);
+    uint8_t*  getMessage(int* len);
+    bool isBroadcastable();
+private:
+    void setSleep();
+    int  readApiFrame(void);
+
+    in6_addr _gwIpAddress;
+    in6_addr _ipAddress;
+	uint16_t _gwPortNo;
+	uint16_t _portNo;
+    int     _returnCode;
+    bool _sleepflg;
+    uint8_t _rxDataBuf[MQTTSN_MAX_PACKET_SIZE + 1];  // defined in MqttsnClientApp.h
+
+};
+
+}    /* end of namespace */
+#endif /* UDP6 */
+#endif /* NETWORKUDP_H_ */
--- a/MQTTSNGateway/GatewayTester/src/LPublishManager.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LPublishManager.cpp
@@ -40,354 +40,360 @@ const char* NULLCHAR = "";

 LPublishManager::LPublishManager()
 {
-	_first = 0;
-	_last = 0;
-	_elmCnt = 0;
-	_publishedFlg = SAVE_TASK_INDEX;
+    _first = 0;
+    _last = 0;
+    _elmCnt = 0;
+    _publishedFlg = SAVE_TASK_INDEX;
+	_autoConnectFlg = false;
 }

 LPublishManager::~LPublishManager()
 {
-	PubElement* elm = _first;
-	PubElement* sav = 0;
-	while (elm)
-	{
-		sav = elm->next;
-		if (elm != 0)
-		{
-			delElement(elm);
-		}
-		elm = sav;
-	}
+    PubElement* elm = _first;
+    PubElement* sav = 0;
+    while (elm)
+    {
+        sav = elm->next;
+        if (elm != 0)
+        {
+            delElement(elm);
+        }
+        elm = sav;
+    }
 }

 void LPublishManager::publish(const char* topicName, Payload* payload, uint8_t qos, bool retain)
 {
-	publish(topicName, payload->getRowData(), payload->getLen(), qos, retain);
+    publish(topicName, payload->getRowData(), payload->getLen(), qos, retain);
 }

-
 void LPublishManager::publish(const char* topicName, uint8_t* payload, uint16_t len, uint8_t qos, bool retain)
 {
-	uint8_t topicType = MQTTSN_TOPIC_TYPE_NORMAL;
-	if ( strlen(topicName) < 2 )
-	{
-		topicType = MQTTSN_TOPIC_TYPE_SHORT;
-	}
-	publish(topicName, payload, len, qos, topicType, retain);
-}
+    uint16_t msgId = 0;
+    uint8_t topicType = MQTTSN_TOPIC_TYPE_SHORT;
+    if ( strlen(topicName) > 2 )
+    {
+        topicType = MQTTSN_TOPIC_TYPE_NORMAL;
+    }

-void LPublishManager::publish(const char* topicName, uint8_t* payload, uint16_t len, uint8_t qos, uint8_t topicType, bool retain)
-{
-	uint16_t msgId = 0;
-	if ( qos > 0 )
-	{
-		msgId = theClient->getGwProxy()->getNextMsgId();
-	}
-	PubElement* elm = add(topicName, 0, payload, len, qos, retain, msgId, topicType);
-	if (elm->status == TOPICID_IS_READY)
-	{
-		sendPublish(elm);
-	}
-	else
-	{
-		theClient->getGwProxy()->registerTopic((char*) topicName, 0);
-	}
+    if ( qos > 0  && qos < 3 )
+    {
+        msgId = theClient->getGwProxy()->getNextMsgId();
+    }
+
+    PubElement* elm = add(topicName, 0, payload, len, qos, retain, msgId, topicType);
+
+    if (elm->status == TOPICID_IS_READY)
+    {
+        sendPublish(elm);
+    }
+    else
+    {
+        theClient->getGwProxy()->registerTopic((char*) topicName, 0);
+    }
 }

 void LPublishManager::publish(uint16_t topicId, Payload* payload, uint8_t qos, bool retain)
 {
-	publish(topicId, payload->getRowData(), payload->getLen(), qos, retain);
+    publish(topicId, payload->getRowData(), payload->getLen(), qos, retain);
 }

 void LPublishManager::publish(uint16_t topicId, uint8_t* payload, uint16_t len, uint8_t qos, bool retain)
 {
-	uint16_t msgId = 0;
-	if ( qos > 0 )
-	{
-		msgId = theClient->getGwProxy()->getNextMsgId();
-	}
-	PubElement* elm = add(NULLCHAR, topicId, payload, len, qos, retain, msgId, MQTTSN_TOPIC_TYPE_PREDEFINED);
-	sendPublish(elm);
+    uint16_t msgId = 0;
+    if ( qos > 0 && qos < 3 )
+    {
+        msgId = theClient->getGwProxy()->getNextMsgId();
+    }
+    PubElement* elm = add(NULLCHAR, topicId, payload, len, qos, retain, msgId, MQTTSN_TOPIC_TYPE_PREDEFINED);
+    sendPublish(elm);
 }

 void LPublishManager::sendPublish(PubElement* elm)
 {
-	if (elm == 0)
+    if (elm == 0)
+    {
+        return;
+    }
+
+	if (_autoConnectFlg)
 	{
-		return;
+		theClient->getGwProxy()->connect();
 	}

-	theClient->getGwProxy()->connect();
+    uint8_t msg[MQTTSN_MAX_MSG_LENGTH + 1];
+    uint8_t org = 0;
+    if (elm->payloadlen > 248)
+    {
+        msg[0] = 0x01;
+        setUint16(msg + 1, elm->payloadlen + 9);
+        org = 2;
+    }
+    else
+    {
+        msg[0] = (uint8_t) elm->payloadlen + 7;
+    }
+    msg[org + 1] = MQTTSN_TYPE_PUBLISH;
+    msg[org + 2] = elm->flag;
+    if ((elm->retryCount < MQTTSN_RETRY_COUNT))
+    {
+        msg[org + 2] = msg[org + 2] | MQTTSN_FLAG_DUP;
+    }
+    if ((elm->flag & 0x03) == MQTTSN_TOPIC_TYPE_SHORT )
+    {
+        memcpy(msg + org + 3, elm->topicName, 2);
+    }
+    else
+    {
+        setUint16(msg + org + 3, elm->topicId);
+    }
+    setUint16(msg + org + 5, elm->msgId);
+    memcpy(msg + org + 7, elm->payload, elm->payloadlen);

-	uint8_t msg[MQTTSN_MAX_MSG_LENGTH + 1];
-	uint8_t org = 0;
-	if (elm->payloadlen > 128)
-	{
-		msg[0] = 0x01;
-		setUint16(msg + 1, elm->payloadlen + 9);
-		org = 2;
-	}
-	else
-	{
-		msg[0] = (uint8_t) elm->payloadlen + 7;
-	}
-	msg[org + 1] = MQTTSN_TYPE_PUBLISH;
-	msg[org + 2] = elm->flag;
-	if ((elm->retryCount < MQTTSN_RETRY_COUNT))
-	{
-		msg[org + 2] = msg[org + 2] | MQTTSN_FLAG_DUP;
-	}
-	if ((elm->flag & 0x03) == MQTTSN_TOPIC_TYPE_SHORT )
-	{
-		memcpy(msg + org + 3, elm->topicName, 2);
-	}
-	else
-	{
-		setUint16(msg + org + 3, elm->topicId);
-	}
-	setUint16(msg + org + 5, elm->msgId);
-	memcpy(msg + org + 7, elm->payload, elm->payloadlen);
+    theClient->getGwProxy()->writeMsg(msg);
+    theClient->getGwProxy()->setPingReqTimer();
+    if ( ((elm->flag & 0x60) == MQTTSN_FLAG_QOS_0 ) || ( (elm->flag & 0x60) == MQTTSN_FLAG_QOS_M1) )
+    {
+        DISPLAY("\033[0m\033[0;32m Topic \"%s\" was Published. \033[0m\033[0;37m\n\n", elm->topicName);
+        remove(elm);  // PUBLISH Done
+        return;
+    }
+    else if ((elm->flag & 0x60) == MQTTSN_FLAG_QOS_1)
+    {
+        elm->status = WAIT_PUBACK;
+    }
+    else if ((elm->flag & 0x60) == MQTTSN_FLAG_QOS_2)
+    {
+        elm->status = WAIT_PUBREC;
+    }

-	theClient->getGwProxy()->writeMsg(msg);
-	theClient->getGwProxy()->resetPingReqTimer();
-	if ((elm->flag & 0x60) == MQTTSN_FLAG_QOS_0)
-	{
-		DISPLAY("\033[0m\033[0;32m Topic \"%s\" was Published. \033[0m\033[0;37m\n\n", elm->topicName);
-		remove(elm);  // PUBLISH Done
-		return;
-	}
-	else if ((elm->flag & 0x60) == MQTTSN_FLAG_QOS_1)
-	{
-		elm->status = WAIT_PUBACK;
-	}
-	else if ((elm->flag & 0x60) == MQTTSN_FLAG_QOS_2)
-	{
-		elm->status = WAIT_PUBREC;
-	}
-
-	elm->sendUTC = time(NULL);
-	elm->retryCount--;
+    elm->sendUTC = time(NULL);
+    elm->retryCount--;
 }

 void LPublishManager::sendSuspend(const char* topicName, uint16_t topicId, uint8_t topicType)
 {
-	PubElement* elm = _first;
-	while (elm)
-	{
-		if (strcmp(elm->topicName, topicName) == 0 && elm->status == TOPICID_IS_SUSPEND)
-		{
-			elm->topicId = topicId;
-			elm->flag |= topicType;
-			elm->status = TOPICID_IS_READY;
-			sendPublish(elm);
-			elm = 0;
-		}
-		else
-		{
-			elm = elm->next;
-		}
-	}
+    PubElement* elm = _first;
+    while (elm)
+    {
+        if (strcmp(elm->topicName, topicName) == 0 && elm->status == TOPICID_IS_SUSPEND)
+        {
+            elm->topicId = topicId;
+            elm->flag |= topicType;
+            elm->status = TOPICID_IS_READY;
+            sendPublish(elm);
+            elm = 0;
+        }
+        else
+        {
+            elm = elm->next;
+        }
+    }
 }

 void LPublishManager::sendPubAck(uint16_t topicId, uint16_t msgId, uint8_t rc)
 {
-	uint8_t msg[7];
-	msg[0] = 7;
-	msg[1] = MQTTSN_TYPE_PUBACK;
-	setUint16(msg + 2, topicId);
-	setUint16(msg + 4, msgId);
-	msg[6] = rc;
-	theClient->getGwProxy()->writeMsg(msg);
+    uint8_t msg[7];
+    msg[0] = 7;
+    msg[1] = MQTTSN_TYPE_PUBACK;
+    setUint16(msg + 2, topicId);
+    setUint16(msg + 4, msgId);
+    msg[6] = rc;
+    theClient->getGwProxy()->writeMsg(msg);
 }

 void LPublishManager::sendPubRel(PubElement* elm)
 {
-	uint8_t msg[4];
-	msg[0] = 4;
-	msg[1] = MQTTSN_TYPE_PUBREL;
-	setUint16(msg + 2, elm->msgId);
-	theClient->getGwProxy()->writeMsg(msg);
+    uint8_t msg[4];
+    msg[0] = 4;
+    msg[1] = MQTTSN_TYPE_PUBREL;
+    setUint16(msg + 2, elm->msgId);
+    theClient->getGwProxy()->writeMsg(msg);
 }

 bool LPublishManager::isDone(void)
 {
-	return (_first == 0);
+    return (_first == 0);
 }

 bool LPublishManager::isMaxFlight(void)
 {
-	return (_elmCnt > MAX_INFLIGHT_MSG / 2);
+    return (_elmCnt > MAX_INFLIGHT_MSG / 2);
 }

 void LPublishManager::responce(const uint8_t* msg, uint16_t msglen)
 {
-	if (msg[0] == MQTTSN_TYPE_PUBACK)
-	{
-		uint16_t msgId = getUint16(msg + 3);
-		PubElement* elm = getElement(msgId);
-		if (elm == 0)
-		{
-			return;
-		}
-		if (msg[5] == MQTTSN_RC_ACCEPTED)
-		{
-			if (elm->status == WAIT_PUBACK)
-			{
-				DISPLAY("\033[0m\033[0;32m Topic \"%s\"  Id : %d was Published. \033[0m\033[0;37m\n\n", elm->topicName, elm->topicId);
-				remove(elm); // PUBLISH Done
-			}
-		}
-		else if (msg[5] == MQTTSN_RC_REJECTED_INVALID_TOPIC_ID)
-		{
-			elm->status = TOPICID_IS_SUSPEND;
-			elm->topicId = 0;
-			elm->retryCount = MQTTSN_RETRY_COUNT;
-			elm->sendUTC = 0;
-			theClient->getGwProxy()->registerTopic((char*) elm->topicName, 0);
-		}
-	}
-	else if (msg[0] == MQTTSN_TYPE_PUBREC)
-	{
-		PubElement* elm = getElement(getUint16(msg + 1));
-		if (elm == 0)
-		{
-			return;
-		}
-		if (elm->status == WAIT_PUBREC || elm->status == WAIT_PUBCOMP)
-		{
-			sendPubRel(elm);
-			elm->status = WAIT_PUBCOMP;
-			elm->sendUTC = time(NULL);
-		}
-	}
-	else if (msg[0] == MQTTSN_TYPE_PUBCOMP)
-	{
-		PubElement* elm = getElement(getUint16(msg + 1));
-		if (elm == 0)
-		{
-			return;
-		}
-		if (elm->status == WAIT_PUBCOMP)
-		{
-			DISPLAY("\033[0m\033[0;32m Topic \"%s\"  Id : %d was Published. \033[0m\033[0;37m\n\n", elm->topicName, elm->topicId);
-			remove(elm);  // PUBLISH Done
-		}
-	}
+    if (msg[0] == MQTTSN_TYPE_PUBACK)
+    {
+        uint16_t msgId = getUint16(msg + 3);
+        PubElement* elm = getElement(msgId);
+        if (elm == 0)
+        {
+            return;
+        }
+        if (msg[5] == MQTTSN_RC_ACCEPTED)
+        {
+            if (elm->status == WAIT_PUBACK)
+            {
+                DISPLAY("\033[0m\033[0;32m Topic \"%s\"  Id : %d was Published. \033[0m\033[0;37m\n\n", elm->topicName, elm->topicId);
+                remove(elm); // PUBLISH Done
+            }
+        }
+        else if (msg[5] == MQTTSN_RC_REJECTED_INVALID_TOPIC_ID)
+        {
+            elm->status = TOPICID_IS_SUSPEND;
+            elm->topicId = 0;
+            elm->retryCount = MQTTSN_RETRY_COUNT;
+            elm->sendUTC = 0;
+            theClient->getGwProxy()->registerTopic((char*) elm->topicName, 0);
+        }
+    }
+    else if (msg[0] == MQTTSN_TYPE_PUBREC)
+    {
+        PubElement* elm = getElement(getUint16(msg + 1));
+        if (elm == 0)
+        {
+            return;
+        }
+        if (elm->status == WAIT_PUBREC || elm->status == WAIT_PUBCOMP)
+        {
+            sendPubRel(elm);
+            elm->status = WAIT_PUBCOMP;
+            elm->sendUTC = time(NULL);
+        }
+    }
+    else if (msg[0] == MQTTSN_TYPE_PUBCOMP)
+    {
+        PubElement* elm = getElement(getUint16(msg + 1));
+        if (elm == 0)
+        {
+            return;
+        }
+        if (elm->status == WAIT_PUBCOMP)
+        {
+            DISPLAY("\033[0m\033[0;32m Topic \"%s\"  Id : %d was Published. \033[0m\033[0;37m\n\n", elm->topicName, elm->topicId);
+            remove(elm);  // PUBLISH Done
+        }
+    }
 }

 void LPublishManager::published(uint8_t* msg, uint16_t msglen)
 {
-	uint16_t topicId = getUint16(msg + 2);
+    uint16_t topicId = getUint16(msg + 2);

-	if (msg[1] & MQTTSN_FLAG_QOS_1)
-	{
-		sendPubAck(topicId, getUint16(msg + 4), MQTTSN_RC_ACCEPTED);
-	}
+    if (msg[1] & MQTTSN_FLAG_QOS_1)
+    {
+        sendPubAck(topicId, getUint16(msg + 4), MQTTSN_RC_ACCEPTED);
+    }

-	_publishedFlg = NEG_TASK_INDEX;
-	theClient->getTopicTable()->execCallback(topicId, msg + 6, msglen - 6, msg[1] & 0x03);
-	_publishedFlg = SAVE_TASK_INDEX;
+    _publishedFlg = NEG_TASK_INDEX;
+    theClient->getTopicTable()->execCallback(topicId, msg + 6, msglen - 6,  (MQTTSN_topicTypes)(msg[1] & MQTTSN_TOPIC_TYPE));
+    _publishedFlg = SAVE_TASK_INDEX;
 }

 void LPublishManager::checkTimeout(void)
 {
-	PubElement* elm = _first;
-	while (elm)
-	{
-		if (elm->sendUTC > 0 && elm->sendUTC + MQTTSN_TIME_RETRY < time(NULL))
-		{
-			if (elm->retryCount >= 0)
-			{
-				sendPublish(elm);
-				D_MQTTLOG("...Timeout retry\r\n");
-			}
-			else
-			{
-				theClient->getGwProxy()->reconnect();
-				elm->retryCount = MQTTSN_RETRY_COUNT;
-				break;
-			}
-		}
-		elm = elm->next;
-	}
+    PubElement* elm = _first;
+    while (elm)
+    {
+        if (elm->sendUTC > 0 && elm->sendUTC + MQTTSN_TIME_RETRY < time(NULL))
+        {
+            if (elm->retryCount >= 0)
+            {
+                sendPublish(elm);
+                D_MQTTLOG("...Timeout retry\r\n");
+            }
+            else
+            {
+                theClient->getGwProxy()->reconnect();
+                elm->retryCount = MQTTSN_RETRY_COUNT;
+                break;
+            }
+        }
+        elm = elm->next;
+    }
+}
+
+void LPublishManager::setAutoConnectMode(bool flg)
+{
+	_autoConnectFlg = flg;
 }

 PubElement* LPublishManager::getElement(uint16_t msgId)
 {
-	PubElement* elm = _first;
-	while (elm)
-	{
-		if (elm->msgId == msgId)
-		{
-			break;
-		}
-		else
-		{
-			elm = elm->next;
-		}
-	}
-	return elm;
+    PubElement* elm = _first;
+    while (elm)
+    {
+        if (elm->msgId == msgId)
+        {
+            break;
+        }
+        else
+        {
+            elm = elm->next;
+        }
+    }
+    return elm;
 }

 PubElement* LPublishManager::getElement(const char* topicName)
 {
-	PubElement* elm = _first;
-	while (elm)
-	{
-		if (strcmp(elm->topicName, topicName) == 0)
-		{
-			break;
-		}
-		else
-		{
-			elm = elm->next;
-		}
-	}
-	return elm;
+    PubElement* elm = _first;
+    while (elm)
+    {
+        if (strcmp(elm->topicName, topicName) == 0)
+        {
+            break;
+        }
+        else
+        {
+            elm = elm->next;
+        }
+    }
+    return elm;
 }

 void LPublishManager::remove(PubElement* elm)
 {
-	if (elm)
-		{
-			if (elm->prev == 0)
-			{
-				_first = elm->next;
-				if (elm->next == 0)
-				{
-					_last = 0;
-				}
-				else
-				{
-					elm->next->prev = 0;
-					_last = elm->next;
-				}
-			}
-			else
-			{
-				if ( elm->next == 0 )
-				{
-					_last = elm->prev;
-				}
-				elm->prev->next = elm->next;
-			}
-			delElement(elm);
-		}
+    if (elm)
+        {
+            if (elm->prev == 0)
+            {
+                _first = elm->next;
+                if (elm->next == 0)
+                {
+                    _last = 0;
+                }
+                else
+                {
+                    elm->next->prev = 0;
+                    _last = elm->next;
+                }
+            }
+            else
+            {
+                if ( elm->next == 0 )
+                {
+                    _last = elm->prev;
+                }
+                elm->prev->next = elm->next;
+            }
+            delElement(elm);
+        }
 }

 void LPublishManager::delElement(PubElement* elm)
 {
-	if (elm->taskIndex >= 0)
-	{
-		theClient->getTaskManager()->done(elm->taskIndex);
-	}
-	_elmCnt--;
-	if ( elm->payload )
-	{
-		free(elm->payload);
-	}
-	free(elm);
+    if (elm->taskIndex >= 0)
+    {
+        theClient->getTaskManager()->done(elm->taskIndex);
+    }
+    _elmCnt--;
+    if ( elm->payload )
+    {
+        free(elm->payload);
+    }
+    free(elm);
 }

 /*
@@ -396,75 +402,88 @@ void LPublishManager::delElement(PubElement* elm)
 }*/

 PubElement* LPublishManager::add(const char* topicName, uint16_t topicId, uint8_t* payload, uint16_t len, uint8_t qos,
-		uint8_t retain, uint16_t msgId, uint8_t topicType)
+        uint8_t retain, uint16_t msgId, uint8_t topicType)
 {
-	PubElement* elm = (PubElement*) calloc(1, sizeof(PubElement));
+    PubElement* elm = (PubElement*) calloc(1, sizeof(PubElement));

-	if (elm == 0)
-	{
-		return elm;
-	}
-	if (_last == 0)
-	{
-		_first = elm;
-		_last = elm;
-	}
-	else
-	{
-		elm->prev = _last;
-		_last->next = elm;
-		_last = elm;
-	}
+    if (elm == 0)
+    {
+        return elm;
+    }
+    if (_last == 0)
+    {
+        _first = elm;
+        _last = elm;
+    }
+    else
+    {
+        elm->prev = _last;
+        _last->next = elm;
+        _last = elm;
+    }

-	elm->topicName = topicName;
-	elm->flag |= topicType;
+    elm->topicName = topicName;
+    elm->flag |= topicType;

-	if (qos == 0)
-	{
-		elm->flag |= MQTTSN_FLAG_QOS_0;
-	}
-	else if (qos == 1)
-	{
-		elm->flag |= MQTTSN_FLAG_QOS_1;
-	}
-	else if (qos == 2)
-	{
-		elm->flag |= MQTTSN_FLAG_QOS_2;
-	}
-	if (retain)
-	{
-		elm->flag |= MQTTSN_FLAG_RETAIN;
-	}
+    if (qos == 0)
+    {
+        elm->flag |= MQTTSN_FLAG_QOS_0;
+    }
+    else if (qos == 1)
+    {
+        elm->flag |= MQTTSN_FLAG_QOS_1;
+    }
+    else if (qos == 2)
+    {
+        elm->flag |= MQTTSN_FLAG_QOS_2;
+    }
+    else if (qos == 3)
+    {
+        elm->flag |= MQTTSN_FLAG_QOS_M1;
+    }
+    if (retain)
+    {
+        elm->flag |= MQTTSN_FLAG_RETAIN;
+    }

-	if (topicId)
-	{
-		elm->status = TOPICID_IS_READY;
-		elm->topicId = topicId;
-	}
+    if (topicId)
+    {
+        elm->status = TOPICID_IS_READY;
+        elm->topicId = topicId;
+    }
+    else
+    {
+        uint16_t id = theClient->getTopicId(topicName);
+        if ( id )
+        {
+            elm->status = TOPICID_IS_READY;
+            elm->topicId = id;
+        }
+    }

-	elm->payloadlen = len;
-	elm->msgId = msgId;
-	elm->retryCount = MQTTSN_RETRY_COUNT;
-	elm->sendUTC = 0;
+    elm->payloadlen = len;
+    elm->msgId = msgId;
+    elm->retryCount = MQTTSN_RETRY_COUNT;
+    elm->sendUTC = 0;

-	if (_publishedFlg == NEG_TASK_INDEX)
-	{
-		elm->taskIndex = -1;
-	}
-	else
-	{
-		elm->taskIndex = theClient->getTaskManager()->getIndex();
-		theClient->getTaskManager()->suspend(elm->taskIndex);
-	}
+    if (_publishedFlg == NEG_TASK_INDEX)
+    {
+        elm->taskIndex = -1;
+    }
+    else
+    {
+        elm->taskIndex = theClient->getTaskManager()->getIndex();
+        theClient->getTaskManager()->suspend(elm->taskIndex);
+    }

-	elm->payload = (uint8_t*) malloc(len);
-	if (elm->payload == 0)
-	{
-		delElement(elm);
-		return 0;
-	}
-	memcpy(elm->payload, payload, len);
+    elm->payload = (uint8_t*) malloc(len);
+    if (elm->payload == 0)
+    {
+        delElement(elm);
+        return 0;
+    }
+    memcpy(elm->payload, payload, len);

-	++_elmCnt;
-	return elm;
+    ++_elmCnt;
+    return elm;
 }
--- a/MQTTSNGateway/GatewayTester/src/LPublishManager.h
+++ b/MQTTSNGateway/GatewayTester/src/LPublishManager.h
@@ -16,6 +16,7 @@
 #ifndef PUBLISHMANAGER_H_
 #define PUBLISHMANAGER_H_

+#include <time.h>
 #include "LMqttsnClientApp.h"
 #include "LTimer.h"
 #include "LTopicTable.h"
@@ -42,7 +43,7 @@ typedef struct PubElement{
    const char* topicName;
    uint8_t*  payload;
    uint16_t  payloadlen;
-    uint32_t  sendUTC;
+    time_t    sendUTC;
    int       (*callback)(void);
    int       retryCount;
    int       taskIndex;
@@ -61,7 +62,6 @@ public:
    ~LPublishManager();
    void publish(const char* topicName, Payload* payload, uint8_t qos, bool retain = false);
    void publish(const char* topicName, uint8_t* payload, uint16_t len, uint8_t qos, bool retain = false);
-    void publish(const char* topicName, uint8_t* payload, uint16_t len, uint8_t qos, uint8_t topicType, bool retain = false);
    void publish(uint16_t topicId, Payload* payload, uint8_t qos, bool retain = false);
    void publish(uint16_t topicId, uint8_t* payload, uint16_t len, uint8_t qos, bool retain = false);
    void responce(const uint8_t* msg, uint16_t msglen);
@@ -70,6 +70,7 @@ public:
    void sendSuspend(const char* topicName, uint16_t topicId, uint8_t topicType);
    bool isDone(void);
    bool isMaxFlight(void);
+	void setAutoConnectMode(bool);
 private:
    PubElement* getElement(uint16_t msgId);
    PubElement* getElement(const char* topicName);
@@ -84,7 +85,8 @@ private:
 	PubElement* _last;
 	uint8_t     _elmCnt;
 	uint8_t     _publishedFlg;
+	uint8_t _autoConnectFlg;
 };
- 
+
 } /* tomyAsyncClient */
 #endif /* PUBLISHMANAGER_H_ */
--- a/MQTTSNGateway/GatewayTester/src/LRegisterManager.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LRegisterManager.cpp
@@ -190,13 +190,13 @@ void LRegisterManager::registerTopic(char* topicName)
 void LRegisterManager::responceRegAck(uint16_t msgId, uint16_t topicId)
 {
 	const char* topicName = getTopic(msgId);
+	MQTTSN_topicTypes type = MQTTSN_TOPIC_TYPE_NORMAL;
 	if (topicName)
 	{
-		uint8_t topicType = strlen((char*) topicName) > 2 ? MQTTSN_TOPIC_TYPE_NORMAL : MQTTSN_TOPIC_TYPE_SHORT;
-		theClient->getGwProxy()->getTopicTable()->setTopicId((char*) topicName, topicId, topicType); // Add Topic to TopicTable
+		theClient->getGwProxy()->getTopicTable()->setTopicId((char*) topicName, topicId,  type); // Add Topic to TopicTable
 		RegQueElement* elm = getElement(msgId);
 		remove(elm);
-		theClient->getPublishManager()->sendSuspend((char*) topicName, topicId, topicType);
+		theClient->getPublishManager()->sendSuspend((char*) topicName, topicId, type);
 	}
 }

@@ -206,14 +206,14 @@ 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)
 	{
 		TopicCallback callback = tp->getCallback();
 		void* topicName = calloc(strlen((char*) msg + 5) + 1, sizeof(char));
-		theClient->getGwProxy()->getTopicTable()->add((char*) topicName, 0, MQTTSN_TOPIC_TYPE_NORMAL, callback, 1);
+		theClient->getGwProxy()->getTopicTable()->add((char*) topicName, MQTTSN_TOPIC_TYPE_NORMAL, 0, callback, 1);
 		regack[6] = MQTTSN_RC_ACCEPTED;
 	}
 	else
--- a/MQTTSNGateway/GatewayTester/src/LRegisterManager.h
+++ b/MQTTSNGateway/GatewayTester/src/LRegisterManager.h
@@ -17,6 +17,7 @@
 #ifndef REGISTERQUE_H_
 #define REGISTERQUE_H_

+#include <time.h>
 #include "LMqttsnClientApp.h"

 namespace linuxAsyncClient {
@@ -27,7 +28,7 @@ typedef struct RegQueElement{
 	const char* topicName;
 	uint16_t msgId;
    int      retryCount;
-    uint32_t sendUTC;
+    time_t   sendUTC;
 	RegQueElement* prev;
 	RegQueElement* next;
 }RegQueElement;
--- a/MQTTSNGateway/GatewayTester/src/LScreen.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LScreen.cpp
@@ -23,7 +23,6 @@
 #include <termios.h>
 #include <unistd.h>
 #include <fcntl.h>
-#include <errno.h>
 #include <stdarg.h>

 #include "LScreen.h"
--- a/MQTTSNGateway/GatewayTester/src/LSubscribeManager.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LSubscribeManager.cpp
@@ -39,350 +39,373 @@ 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++)
-		{
-			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)
+            {
+				subscribePredefinedId(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;
+    }

-	if ((elm->retryCount < MQTTSN_RETRY_COUNT) && elm->msgType == MQTTSN_TYPE_SUBSCRIBE)
-	{
-		msg[2] = msg[2] | MQTTSN_FLAG_DUP;
-	}
+    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();
+    }

-	setUint16(msg + 3, elm->msgId);
+    if ((elm->retryCount < MQTTSN_RETRY_COUNT) && elm->msgType == MQTTSN_TYPE_SUBSCRIBE)
+    {
+        msg[2] = msg[2] | MQTTSN_FLAG_DUP;
+    }

-	theClient->getGwProxy()->connect();
-	theClient->getGwProxy()->writeMsg(msg);
-	theClient->getGwProxy()->resetPingReqTimer();
-	elm->sendUTC = time(NULL);
-	elm->retryCount--;
+    setUint16(msg + 3, elm->msgId);
+
+    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)
 {
-	uint8_t topicType = MQTTSN_TOPIC_TYPE_NORMAL;
-	if ( strlen(topicName) <= 2)
-	{
-		topicType = MQTTSN_TOPIC_TYPE_SHORT;
-	}
-	SubElement* elm = add(MQTTSN_TYPE_SUBSCRIBE, topicName, 0, topicType, qos, onPublish);
-	send(elm);
+    MQTTSN_topicTypes topicType;
+    if ( strlen(topicName) > 2 )
+    {
+        topicType = MQTTSN_TOPIC_TYPE_NORMAL;
+    }
+    else
+    {
+        topicType = MQTTSN_TOPIC_TYPE_SHORT;
+    }
+    SubElement* elm = add(MQTTSN_TYPE_SUBSCRIBE, topicName, topicType, 0,  qos, onPublish);
+    send(elm);
 }

-void LSubscribeManager::subscribe(uint16_t topicId, TopicCallback onPublish, uint8_t qos, uint8_t topicType)
+void LSubscribeManager::subscribePredefinedId(uint16_t topicId, TopicCallback onPublish, uint8_t qos)
 {
-	SubElement* elm = add(MQTTSN_TYPE_SUBSCRIBE, 0, topicId, topicType, 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)
 {
-	SubElement* elm = add(MQTTSN_TYPE_UNSUBSCRIBE, topicName, 0, MQTTSN_TOPIC_TYPE_NORMAL, 0, 0);
-	send(elm);
+    MQTTSN_topicTypes topicType;
+    if ( strlen(topicName) > 2 )
+    {
+        topicType = MQTTSN_TOPIC_TYPE_NORMAL;
+    }
+    else
+    {
+        topicType = MQTTSN_TOPIC_TYPE_SHORT;
+    }
+    SubElement* elm = add(MQTTSN_TYPE_UNSUBSCRIBE, topicName, topicType, 0, 0, 0);
+    send(elm);
 }

-void LSubscribeManager::unsubscribe(uint16_t topicId, uint8_t topicType)
+void LSubscribeManager::unsubscribe( uint16_t topicId)
 {
-	SubElement* elm = add(MQTTSN_TYPE_UNSUBSCRIBE, 0, topicId, topicType, 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];

-		LTopicTable* tt = theClient->getGwProxy()->getTopicTable();
-		SubElement* elm = getElement(msgId);
-		if (elm)
-		{
-			if ( rc == MQTTSN_RC_ACCEPTED )
-			{
-				tt->add((char*) elm->topicName, topicId, elm->topicType, 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
-			{
-				remove(elm);
-				DISPLAY("\033[0m\033[0;31m SUBACK Invalid messageId. %s\033[0m\033[0;37m\n\n", getElement(msgId)->topicName);
-			}
-		}
-	}
-	else if (msg[0] == MQTTSN_TYPE_UNSUBACK)
-	{
-		uint16_t msgId = getUint16(msg + 1);
-		SubElement* elm = getElement(msgId);
-		if (elm)
-		{
-			LTopicTable* tt = theClient->getGwProxy()->getTopicTable();
-			tt->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(getElement(msgId));
-		}
-		else
-		{
-			DISPLAY("\033[0m\033[0;31m UNSUBACK Invalid messageId. %s\033[0m\033[0;37m\n\n", getElement(msgId)->topicName);
-			remove(getElement(msgId));
-		}
-	}
+        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=%04x. \033[0m\033[0;37m\n\n",
+					msgId);
+        }
+    }
 }

 /* SubElement operations */

-SubElement* LSubscribeManager::add(uint8_t msgType, const char* topicName, uint16_t topicId, uint8_t topicType,
-		uint8_t qos, TopicCallback callback)
+SubElement* LSubscribeManager::add(uint8_t msgType, const char* topicName, MQTTSN_topicTypes topicType, uint16_t topicId,
+        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 (strcmp(elm->topicName, topicName) == 0 && elm->msgType == msgType)
-		{
-			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, uint8_t topicType)
+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;
 }
--- a/MQTTSNGateway/GatewayTester/src/LSubscribeManager.h
+++ b/MQTTSNGateway/GatewayTester/src/LSubscribeManager.h
@@ -19,6 +19,7 @@

 #include <stdio.h>
 #include <string.h>
+#include <time.h>

 #include "LMqttsnClientApp.h"
 #include "LRegisterManager.h"
@@ -33,10 +34,10 @@ typedef struct SubElement{
    TopicCallback callback;
    const char* topicName;
    uint16_t  msgId;
-    uint32_t  sendUTC;
+    time_t    sendUTC;
    uint16_t  topicId;
    uint8_t   msgType;
-    uint8_t   topicType;
+    MQTTSN_topicTypes   topicType;
    uint8_t   qos;

    int       retryCount;
@@ -55,9 +56,9 @@ public:
    ~LSubscribeManager();
    void onConnect(void);
    void subscribe(const char* topicName, TopicCallback onPublish, uint8_t qos);
-    void subscribe(uint16_t topicId, TopicCallback onPublish, uint8_t qos, uint8_t topicType);
+	void subscribePredefinedId(uint16_t topicId, TopicCallback onPublish, uint8_t qos);
    void unsubscribe(const char* topicName);
-    void unsubscribe(uint16_t topicId, uint8_t topicType);
+    void unsubscribe(uint16_t topicId);
    void responce(const uint8_t* msg);
    void checkTimeout(void);
    bool isDone(void);
@@ -65,9 +66,9 @@ private:
    void send(SubElement* elm);
    SubElement* getFirstElement(void);
    SubElement* getElement(uint16_t msgId);
-    SubElement* getElement(uint16_t topicId, uint8_t topicType);
+    SubElement* getElement(uint16_t topicId, MQTTSN_topicTypes topicType);
    SubElement* getElement(const char* topicName, uint8_t msgType);
-	SubElement* add(uint8_t msgType, const char* topicName, uint16_t topicId, uint8_t topicType, uint8_t qos, TopicCallback callback);
+	SubElement* add(uint8_t msgType, const char* topicName, MQTTSN_topicTypes topicType, uint16_t topicId, uint8_t qos, TopicCallback callback);
 	void remove(SubElement* elm);
 	SubElement* _first;
 	SubElement* _last;
--- a/MQTTSNGateway/GatewayTester/src/LTaskManager.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LTaskManager.cpp
@@ -14,7 +14,6 @@
 *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
 **************************************************************************************/

-
 #include <stdio.h>
 #include <string.h>

@@ -32,146 +31,153 @@ extern LMqttsnClient* theClient;
 extern LScreen* theScreen;
 extern bool theClientMode;
 /*=====================================
-           TaskManager
+ TaskManager
 ======================================*/
-LTaskManager::LTaskManager(void){
-	_tasks = 0;
-	_tests = 0;
-	_index = 0;
+LTaskManager::LTaskManager(void)
+{
+    _tasks = 0;
+    _tests = 0;
+    _index = 0;
 }

-LTaskManager::~LTaskManager(void){
-    
+LTaskManager::~LTaskManager(void)
+{
+
 }

-void LTaskManager::add(TaskList* task){
+void LTaskManager::add(TaskList* task)
+{
    _tasks = task;
 }

-void LTaskManager::add(TestList* test){
+void LTaskManager::add(TestList* test)
+{
    _tests = test;
 }

-void LTaskManager::run(void){
-	int i = 0;
-	char c = 0;
-	bool cancelFlg = false;
+void LTaskManager::run(void)
+{
+    int i = 0;
+    char c = 0;
+    bool cancelFlg = false;
+    TestList test = { 0 };
+    TaskList task = { 0 };

-	if ( !theClientMode )
-	{
-			theClient->getGwProxy()->getMessage();
+    if (!theClientMode)
+    {
+        theClient->getGwProxy()->getMessage();

-			for (i = 0; _tests[i].testTask > 0; i++)
-			{
-				PROMPT("Execute \"%s\" ? ( y/n ) : ", _tests[i].testLabel);
-				while (true)
-				{
-					if (CHECKKEYIN(&c))
-					{
-						if ( toupper(c) == 'N' )
-						{
+        for (i = 0; _tests[i].testTask > test.testTask; i++)
+        {
+            PROMPT("Execute \"%s\" ? ( y/n ) : ", _tests[i].testLabel);
+            while (true)
+            {
+                if (CHECKKEYIN(&c))
+                {
+                    if (toupper(c) == 'N')
+                    {

-							DISPLAY("\033[0m\033[0;32m\n**** %s is canceled ****\033[0m\033[0;37m\n\n", _tests[i].testLabel);
-							theScreen->prompt("");
-							cancelFlg = true;
-							break;
-						}
-						else if ( toupper(c) == 'Y' )
-						{
-							DISPLAY("\033[0m\033[0;32m\n\n**** %s start ****\033[0m\033[0;37m\n", _tests[i].testLabel);
-							theScreen->prompt("");
-							(_tests[i].testTask)();
-							cancelFlg = false;
-							break;
-						}
-					}
-					else
-					{
-						theClient->getGwProxy()->getMessage();
-					}
-				}
+                        DISPLAY("\033[0m\033[0;32m\n**** %s is canceled ****\033[0m\033[0;37m\n\n", _tests[i].testLabel);
+                        theScreen->prompt("");
+                        cancelFlg = true;
+                        break;
+                    }
+                    else if (toupper(c) == 'Y')
+                    {
+                        DISPLAY("\033[0m\033[0;32m\n\n**** %s start ****\033[0m\033[0;37m\n", _tests[i].testLabel);
+                        theScreen->prompt("");
+                        (_tests[i].testTask)();
+                        cancelFlg = false;
+                        break;
+                    }
+                }
+                else
+                {
+                    theClient->getGwProxy()->getMessage();
+                }
+            }

-				while ( true )
-				{
-					do
-					{
-						theClient->getGwProxy()->getMessage();
-					}
-					while(theClient->getPublishManager()->isMaxFlight() ||
-						   !theClient->getSubscribeManager()->isDone() ||
-						   !theClient->getRegisterManager()->isDone());
+            while (true)
+            {
+                do
+                {
+                    theClient->getGwProxy()->getMessage();
+                }
+                while (theClient->getPublishManager()->isMaxFlight() || !theClient->getSubscribeManager()->isDone()
+                        || !theClient->getRegisterManager()->isDone());

-					if (theClient->getPublishManager()->isDone())
-					{
-						break;
-					}
-				}
-				if ( !cancelFlg )
-				{
-					DISPLAY("\033[0m\033[0;32m\n**** %s complete ****\033[0m\033[0;37m\n\n", _tests[i].testLabel);
-				}
-			}
-			DISPLAY("\033[0m\033[0;32m\n\n#########  All tests complete!  ###########\033[0m\033[0;37m\n\n");
-	}
-	else
-	{
-		while (true)
-		{
-			theClient->getGwProxy()->getMessage();
-			for (_index = 0; _tasks[_index].callback > 0; _index++)
-			{
-				if ((_tasks[_index].prevTime + _tasks[_index].interval <= time(NULL)) &&
-					 _tasks[_index].count == 0)
-				{
-					_tasks[_index].prevTime = time(NULL);
-					(_tasks[_index].callback)();
-				}
-			}
+                if (theClient->getPublishManager()->isDone())
+                {
+                    break;
+                }
+            }
+            if (!cancelFlg)
+            {
+                DISPLAY("\033[0m\033[0;32m\n**** %s complete ****\033[0m\033[0;37m\n\n", _tests[i].testLabel);
+            }
+        }
+        DISPLAY("\033[0m\033[0;32m\n\n#########  All tests complete!  ###########\033[0m\033[0;37m\n\n");
+    }
+    else
+    {
+        while (true)
+        {
+            theClient->getGwProxy()->getMessage();
+            for (_index = 0; _tasks[_index].callback > task.callback; _index++)
+            {
+                if ((_tasks[_index].prevTime + _tasks[_index].interval <= time(NULL)) && _tasks[_index].count == 0)
+                {
+                    _tasks[_index].prevTime = time(NULL);
+                    (_tasks[_index].callback)();
+                }
+            }

-			do
-			{
-				theClient->getGwProxy()->getMessage();
-			}
-			while(theClient->getPublishManager()->isMaxFlight() ||
-				   !theClient->getSubscribeManager()->isDone() ||
-				   !theClient->getRegisterManager()->isDone());
+            do
+            {
+                theClient->getGwProxy()->getMessage();
+            }
+            while (theClient->getPublishManager()->isMaxFlight() || !theClient->getSubscribeManager()->isDone()
+                    || !theClient->getRegisterManager()->isDone());

-			if (theClient->getPublishManager()->isDone())
-			{
-				break;
-			}
-		}
-	}
+            if (theClient->getPublishManager()->isDone())
+            {
+                break;
+            }
+        }
+    }
 }

-uint8_t LTaskManager::getIndex(void){
-	return _index;
+uint8_t LTaskManager::getIndex(void)
+{
+    return _index;
 }

-void LTaskManager::done(uint8_t index){
-	if (_tasks )
-	{
-		if (_tasks[index].count > 0)
-		{
-			_tasks[index].count--;
-		}
-	}
-	if (_tests )
-		{
-			if (_tests[index].count > 0)
-			{
-				_tests[index].count--;
-			}
-		}
+void LTaskManager::done(uint8_t index)
+{
+    if (_tasks)
+    {
+        if (_tasks[index].count > 0)
+        {
+            _tasks[index].count--;
+        }
+    }
+    if (_tests)
+    {
+        if (_tests[index].count > 0)
+        {
+            _tests[index].count--;
+        }
+    }
 }

-void LTaskManager::suspend(uint8_t index){
-	if ( _tasks )
-	{
-		_tasks[index].count++;
-	}
-	if ( _tests )
-	{
-		_tests[index].count++;
-	}
+void LTaskManager::suspend(uint8_t index)
+{
+    if (_tasks)
+    {
+        _tasks[index].count++;
+    }
+    if (_tests)
+    {
+        _tests[index].count++;
+    }
 }
--- a/MQTTSNGateway/GatewayTester/src/LTaskManager.h
+++ b/MQTTSNGateway/GatewayTester/src/LTaskManager.h
@@ -19,6 +19,7 @@

 #include <stdio.h>
 #include <string.h>
+#include <time.h>

 #include "LMqttsnClientApp.h"
 #include "LTimer.h"
@@ -29,8 +30,8 @@ namespace linuxAsyncClient {

 struct TaskList{
    void     (*callback)(void);
-	uint32_t interval;
-    uint32_t prevTime;
+	time_t   interval;
+    time_t    prevTime;
    uint8_t  count;
 };

--- a/MQTTSNGateway/GatewayTester/src/LTimer.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LTimer.cpp
@@ -16,11 +16,12 @@

 #include <stdlib.h>
 #include <string.h>
+#include <sys/time.h>

 #include "LMqttsnClientApp.h"
 #include "LTimer.h"

-using namespace std;
+//using namespace std;
 using namespace linuxAsyncClient;

 /*=====================================
@@ -63,3 +64,17 @@ void LTimer::stop(){
  _millis = 0;
 }

+uint32_t LTimer::getRemain(void)
+{
+    struct timeval curTime;
+    uint32_t secs, usecs;
+    if (_millis <= 0){
+        return 0;
+    }else{
+        gettimeofday(&curTime, 0);
+        secs  = (curTime.tv_sec  - _startTime.tv_sec) * 1000;
+        usecs = (curTime.tv_usec - _startTime.tv_usec) / 1000.0;
+        secs = _millis - (secs + usecs);
+        return secs;
+    }
+}
--- a/MQTTSNGateway/GatewayTester/src/LTimer.h
+++ b/MQTTSNGateway/GatewayTester/src/LTimer.h
@@ -35,6 +35,7 @@ public:
    bool isTimeUp(void);
    void stop(void);
    void changeUTC(void){};
+    uint32_t getRemain(void);
    static void setUnixTime(uint32_t utc){};
 private:
    struct timeval _startTime;
--- a/MQTTSNGateway/GatewayTester/src/LTopicTable.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LTopicTable.cpp
@@ -131,7 +131,7 @@ LTopic* LTopicTable::getTopic(const char* topic){
 	return 0;
 }

-LTopic* LTopicTable::getTopic(uint16_t topicId, uint8_t topicType){
+LTopic* LTopicTable::getTopic(uint16_t topicId, MQTTSN_topicTypes topicType){
 	LTopic* p = _first;
 	while(p){
 		if (p->_topicId == topicId && p->_topicType == topicType){
@@ -156,12 +156,12 @@ char* LTopicTable::getTopicName(LTopic* topic){
 }


-void LTopicTable::setTopicId(const char* topic, uint16_t id, uint8_t type){
+void LTopicTable::setTopicId(const char* topic, uint16_t id, MQTTSN_topicTypes type){
    LTopic* tp = getTopic(topic);
    if (tp){
        tp->_topicId = id;
    }else{
-    	add(topic, id, type, 0);
+    	add(topic, type, id, 0);
    }
 }

@@ -176,7 +176,7 @@ bool LTopicTable::setCallback(const char* topic, TopicCallback callback){
 }


-bool LTopicTable::setCallback(uint16_t topicId, uint8_t topicType, TopicCallback callback){
+bool LTopicTable::setCallback(uint16_t topicId, MQTTSN_topicTypes topicType, TopicCallback callback){
 	LTopic* p = getTopic(topicId, topicType);
 	if (p){
 		p->_callback = callback;
@@ -186,7 +186,7 @@ bool LTopicTable::setCallback(uint16_t topicId, uint8_t topicType, TopicCallback
 }


-int LTopicTable::execCallback(uint16_t  topicId, uint8_t* payload, uint16_t payloadlen, uint8_t topicType){
+int LTopicTable::execCallback(uint16_t  topicId, uint8_t* payload, uint16_t payloadlen, MQTTSN_topicTypes topicType){
 	LTopic* p = getTopic(topicId, topicType);
 	if (p){;
 		return p->execCallback(payload, payloadlen);
@@ -195,7 +195,7 @@ int LTopicTable::execCallback(uint16_t  topicId, uint8_t* payload, uint16_t payl
 }


-LTopic* LTopicTable::add(const char* topicName, uint16_t id, uint8_t type, TopicCallback callback, uint8_t alocFlg)
+LTopic* LTopicTable::add(const char* topicName, MQTTSN_topicTypes type, uint16_t id, TopicCallback callback, uint8_t alocFlg)
 {
 	LTopic* elm;

@@ -234,9 +234,9 @@ exit:
 	return elm;
 }

-void LTopicTable::remove(uint16_t topicId)
+void LTopicTable::remove(uint16_t topicId, MQTTSN_topicTypes type)
 {
-	LTopic* elm = getTopic(topicId);
+	LTopic* elm = getTopic(topicId, type);

 	if (elm){
 		if (elm->_prev == 0)
--- a/MQTTSNGateway/GatewayTester/src/LTopicTable.h
+++ b/MQTTSNGateway/GatewayTester/src/LTopicTable.h
@@ -42,7 +42,7 @@ public:
    TopicCallback getCallback(void);
 private:
    uint16_t  _topicId;
-    uint8_t   _topicType;
+    MQTTSN_topicTypes   _topicType;
    char*     _topicStr;
    TopicCallback  _callback;
    uint8_t  _malocFlg;
@@ -60,16 +60,16 @@ public:
      uint16_t getTopicId(const char* topic);
      char*    getTopicName(LTopic* topic);
      LTopic*   getTopic(const char* topic);
-      LTopic*   getTopic(uint16_t topicId, uint8_t topicType = MQTTSN_TOPIC_TYPE_NORMAL);
-      void     setTopicId(const char* topic, uint16_t id, uint8_t topicType);
+      LTopic*   getTopic(uint16_t topicId, MQTTSN_topicTypes topicType);
+      void     setTopicId(const char* topic, uint16_t id, MQTTSN_topicTypes topicType);
      bool     setCallback(const char* topic, TopicCallback callback);
-      bool     setCallback(uint16_t topicId, uint8_t type, TopicCallback callback);
-      int      execCallback(uint16_t topicId, uint8_t* payload, uint16_t payloadlen, uint8_t topicType = MQTTSN_TOPIC_TYPE_NORMAL);
-      LTopic*  add(const char* topic, uint16_t id = 0, uint8_t type = MQTTSN_TOPIC_TYPE_NORMAL, TopicCallback callback = 0, uint8_t alocFlg = 0);
-      LTopic*  add(uint16_t topicId, uint16_t id, uint8_t type, TopicCallback callback, uint8_t alocFlg);
+      bool     setCallback(uint16_t topicId, MQTTSN_topicTypes type, TopicCallback callback);
+      int      execCallback(uint16_t topicId, uint8_t* payload, uint16_t payloadlen, MQTTSN_topicTypes topicType);
+      LTopic*  add(const char* topic, MQTTSN_topicTypes type, uint16_t id = 0, TopicCallback callback = 0, uint8_t alocFlg = 0);
+      //LTopic*  add(uint16_t topicId, uint16_t id, MQTTSN_topicTypes type, TopicCallback callback, uint8_t alocFlg);
      LTopic*  match(const char* topic);
      void     clearTopic(void);
-      void     remove(uint16_t topicId);
+      void     remove(uint16_t topicId, MQTTSN_topicTypes type);

 private:
    LTopic*  _first;
--- a/MQTTSNGateway/Makefile
+++ b/MQTTSNGateway/Makefile
@@ -1,140 +0,0 @@
-PROGNAME := MQTT-SNGateway
-APPL := mainGateway
-
-LPROGNAME := MQTT-SNLogmonitor
-LAPPL := mainLogmonitor
-
-TESTPROGNAME := testPFW
-TESTAPPL := mainTestProcess
-
-CONFIG := gateway.conf
-CLIENTS := clients.conf
-
-SRCDIR := src
-SUBDIR := ../MQTTSNPacket/src
-
-OS := linux
-SENSORNET := udp
-TEST := tests
-
-CPPSRCS :=  \
-$(SRCDIR)/MQTTGWConnectionHandler.cpp \
-$(SRCDIR)/MQTTGWPacket.cpp \
-$(SRCDIR)/MQTTGWPublishHandler.cpp \
-$(SRCDIR)/MQTTGWSubscribeHandler.cpp \
-$(SRCDIR)/MQTTSNGateway.cpp \
-$(SRCDIR)/MQTTSNGWBrokerRecvTask.cpp \
-$(SRCDIR)/MQTTSNGWBrokerSendTask.cpp \
-$(SRCDIR)/MQTTSNGWClient.cpp \
-$(SRCDIR)/MQTTSNGWClientRecvTask.cpp \
-$(SRCDIR)/MQTTSNGWClientSendTask.cpp \
-$(SRCDIR)/MQTTSNGWConnectionHandler.cpp \
-$(SRCDIR)/MQTTSNGWLogmonitor.cpp \
-$(SRCDIR)/MQTTSNGWPacket.cpp \
-$(SRCDIR)/MQTTSNGWPacketHandleTask.cpp \
-$(SRCDIR)/MQTTSNGWProcess.cpp \
-$(SRCDIR)/MQTTSNGWPublishHandler.cpp \
-$(SRCDIR)/MQTTSNGWSubscribeHandler.cpp \
-$(SRCDIR)/$(OS)/$(SENSORNET)/SensorNetwork.cpp \
-$(SRCDIR)/$(OS)/Timer.cpp  \
-$(SRCDIR)/$(OS)/Network.cpp \
-$(SRCDIR)/$(OS)/Threading.cpp \
-$(SRCDIR)/$(TEST)/TestProcess.cpp \
-$(SRCDIR)/$(TEST)/TestQue.cpp \
-$(SRCDIR)/$(TEST)/TestTree23.cpp \
-$(SRCDIR)/$(TEST)/TestTopics.cpp \
-$(SRCDIR)/$(TEST)/TestTopicIdMap.cpp \
-$(SRCDIR)/$(TEST)/TestTask.cpp
-
-
-CSRCS := $(SUBDIR)/MQTTSNConnectClient.c \
-$(SUBDIR)/MQTTSNConnectServer.c \
-$(SUBDIR)/MQTTSNDeserializePublish.c \
-$(SUBDIR)/MQTTSNPacket.c \
-$(SUBDIR)/MQTTSNSearchClient.c \
-$(SUBDIR)/MQTTSNSearchServer.c \
-$(SUBDIR)/MQTTSNSerializePublish.c \
-$(SUBDIR)/MQTTSNSubscribeClient.c \
-$(SUBDIR)/MQTTSNSubscribeServer.c \
-$(SUBDIR)/MQTTSNUnsubscribeClient.c \
-$(SUBDIR)/MQTTSNUnsubscribeServer.c 
-
-CXX := g++
-CPPFLAGS += 
-
-INCLUDES += -I$(SRCDIR) \
-I$(SRCDIR)/$(OS) \
-I$(SRCDIR)/$(OS)/$(SENSORNET) \
-I$(SUBDIR) \
-I$(SRCDIR)/$(TEST)
-
-DEFS :=
-LIBS += -L/usr/local/lib
-LDFLAGS := 
-CXXFLAGS := -Wall -O3 -std=c++11
-LDADD := -lpthread -lssl -lcrypto -lrt
-OUTDIR := Build
-
-PROG := $(OUTDIR)/$(PROGNAME)
-LPROG := $(OUTDIR)/$(LPROGNAME)
-TPROG := $(OUTDIR)/$(TESTPROGNAME)
-
-OBJS := $(CPPSRCS:%.cpp=$(OUTDIR)/%.o)
-OBJS += $(CSRCS:%.c=$(OUTDIR)/%.o) 
-DEPS := $(CPPSRCS:%.cpp=$(OUTDIR)/%.d)
-DEPS += $(CSRCS:%.c=$(OUTDIR)/%.d)
-
-.PHONY: install clean exectest
-
-all: $(PROG) $(LPROG) $(TPROG)
-
-monitor: $(LPROG)
-
-test: $(TPROG) $(LPROG) exectest
-	
-
-include $(DEPS)
-
-$(PROG): $(OBJS) $(OUTDIR)/$(SRCDIR)/$(APPL).o
-	$(CXX) $(LDFLAGS) -o $@ $^ $(LIBS) $(LDADD)
-
-$(LPROG): $(OBJS) $(OUTDIR)/$(SRCDIR)/$(LAPPL).o
-	$(CXX) $(LDFLAGS) -o $@ $^ $(LIBS) $(LDADD)
-
-$(TPROG): $(OBJS) $(OUTDIR)/$(SRCDIR)/$(TEST)/$(TESTAPPL).o
-	$(CXX) $(LDFLAGS) -o $@ $^ $(LIBS) $(LDADD)
-
-
-$(OUTDIR)/$(SRCDIR)/%.o:$(SRCDIR)/%.cpp
-	@if [ ! -e `dirname $@` ]; then mkdir -p `dirname $@`; fi
-	$(CXX) $(CXXFLAGS) $(CPPFLAGS) $(INCLUDES) $(DEFS) -o $@ -c -MMD -MP -MF $(@:%.o=%.d) $<
-
-$(OUTDIR)/$(SRCDIR)/$(APPL).o:$(SRCDIR)/$(APPL).cpp
-	@if [ ! -e `dirname $@` ]; then mkdir -p `dirname $@`; fi
-	$(CXX) $(CXXFLAGS) $(CPPFLAGS) $(INCLUDES) $(DEFS) -o $@ -c -MMD -MP -MF $(@:%.o=%.d) $<
-
-$(OUTDIR)/$(SRCDIR)/$(TEST)/$(TESTAPPL).o:$(SRCDIR)/$(TEST)/$(TESTAPPL).cpp
-	@if [ ! -e `dirname $@` ]; then mkdir -p `dirname $@`; fi
-	$(CXX) $(CXXFLAGS) $(CPPFLAGS) $(INCLUDES) $(DEFS) -o $@ -c -MMD -MP -MF $(@:%.o=%.d) $<
-
-$(OUTDIR)/$(SRCDIR)/$(LAPPL).o:$(SRCDIR)/$(LAPPL).cpp
-	@if [ ! -e `dirname $@` ]; then mkdir -p `dirname $@`; fi
-	$(CXX) $(CXXFLAGS) $(CPPFLAGS) $(INCLUDES) $(DEFS) -o $@ -c -MMD -MP -MF $(@:%.o=%.d) $<
-
-$(OUTDIR)/$(SUBDIR)/%.o:$(SUBDIR)/%.c
-	@if [ ! -e `dirname $@` ]; then mkdir -p `dirname $@`; fi
-	$(CXX) $(CXXFLAGS) $(CPPFLAGS) $(INCLUDES) $(DEFS) -o $@ -c -MMD -MP -MF $(@:%.o=%.d) $<
-
-clean:
-	rm -rf $(OUTDIR)
-
-install:
-	cp -pf $(PROG) ../../
-	cp -pf $(LPROG) ../../
-	cp -pf $(CONFIG) ../../
-	cp -pf $(CLIENTS) ../../
-
-exectest:
-	./$(OUTDIR)/$(TESTPROGNAME) -f ./gateway.conf
-
-	
--- a/MQTTSNGateway/README.md
+++ b/MQTTSNGateway/README.md
@@ -1,84 +1,211 @@
-**MQTT-SN** requires a MQTT-SN Gateway which acts as a protocol converter to convert **MQTT-SN messages to MQTT messages**. MQTT-SN client (UDP) can not communicate directly with MQTT broker(TCP/IP).   
+# MQTT-SN Transparent / Aggregating Gateway
+MQTT-SN requires a MQTT-SN Gateway which acts as a protocol converter to convert MQTT-SN messages to MQTT messages.
+MQTT-SN client over SensorNetwork can not communicate directly with MQTT broker(TCP/IP). 
+This Gateway can run as a transparent or aggregating Gateway by specifying the gateway.conf. 
+### step1. Build the gateway
+copy and expand source code then, 
+```
+$ cd paho.mqtt-sn.embedded-c/MQTTSNGateway 
+```
+In order to build a gateway, one sensor network argument is required. 
+```
+$ ./build.sh [udp|udp6|xbee|loralink|rfcomm|dtls|dtls6]  
+```     

-### **step1. Build the gateway**   
-````
-$ git clone https://github.com/eclipse/paho.mqtt-sn.embedded-c   
-$ cd paho.mqtt-sn.embedded-c/MQTTSNGateway       
-$ make   
-$ make install   
-$ make clean    
-````      
-MQTT-SNGateway, MQTT-SNLogmonitor and param.conf are copied into ../ directory.
+MQTT-SNGateway and MQTT-SNLogmonitor (executable programs) are built in ./bin directory.

-    
-### **step2. Execute the Gateway.**     
+### step2. Execute the Gateway.    

-````    
-$ cd ../   
-$ ./MQTT-SNGateway [-f Config file name]
-````   
-
-
-### **How to Change the configuration of the gateway**    
-**../gateway.conf**   Contents are follows: 
-   
-````    
+``` 
+$ cd bin 
+$ ./MQTT-SNGateway 
+```
+If you get the error message as follows:
+ 
+RingBuffer can't create a shared memory. ABORT Gateway!!! 
+You have to start using sudo command only once for the first time.    
+```
+$ sudo ./MQTT-SNGateway 
+```
+## Contents of the gateway configuration file
+**gateway.conf** is in bin directory.  It's contents are follows: 

+```
+#**************************************************************************
+# Copyright (c) 2016-2021, 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.
+#***************************************************************************
+#
 # config file of MQTT-SN Gateway
+#

-BrokerName=iot.eclipse.org
+GatewayID=1
+GatewayName=PahoGateway-01
+MaxNumberOfClients=30
+KeepAlive=60
+#LoginID=your_ID
+#Password=your_Password
+
+BrokerName=mqtt.eclipseprojects.io
 BrokerPortNo=1883
-BrokerSecurePortNo=8883    
+BrokerSecurePortNo=8883
+```
+**GatewayID** is a gateway ID which  used by GWINFO message.    
+**GatewayName** is a name of the gateway.    
+**MaxNumberOfClients** is a maxmum number of clients. Clients are dynamically allocated.    
+**KeepAlive** is KeepAlive time in seconds.   
+**LoginID** is used by CONNECT message.  
+**Password** is used by CONNECT message.    
+**BrokerName**is a domain name or IP address of a broker.    
+**BrokerPortNo** is a broker's port no.    
+**BrokerSecurePortNo** is a broker's port no of TLS connection.    
+```
+#
+# CertKey for TLS connections to a broker
+#

+#RootCAfile=/etc/ssl/certs/ca-certificates.crt
+#RootCApath=/etc/ssl/certs/
+#CertKey=/path/to/certKey.pem
+#PrivateKey=/path/to/privateKey.pem
+```
+**RootCAfile** is a CA file name.    
+**RootCApath** is a CA path. **SSL_CTX_load_verify_locations(ctx, CAfile, CApath)** function requires these parameters.        
+**CertKey** is a certificate pem file.
+**PrivateKey** is a private key pem file.   
+Clients can connect to the broker via TLS by setting '**Secure Connection**' for each client in the client conf file.   
+```
+#
+# When AggregatingGateway=YES or ClientAuthentication=YES,
+# All clients must be specified by the ClientList File  
+#
+
+AggregatingGateway=NO
+QoS-1=NO
+Forwarder=NO
+PredefinedTopic=NO
 ClientAuthentication=NO
-#ClientsList=/path/to/your_clients.conf    
-    
-#RootCAfile=/path/to/your_Root_CA.crt    
-#RootCApath=/path/to/your_certs_directory/   
-#CertKey=/path/to/your_cert.pem
-#PrivateKey=/path/to/your_private-key.pem
-    
-GatewayID=1    
-GatewayName=PahoGateway-01    
-KeepAlive=900    
-#LoginID=your_ID    
-#Password=your_Password    

-# UDP
-GatewayPortNo=10000    
-MulticastIP=225.1.1.1    
-MulticastPortNo=1883    
+ClientsList=/path/to/your_clients.conf
+PredefinedTopicList=/path/to/your_predefinedTopic.conf
+```
+The gateway runs as a aggregating gateway when **AggregatingGateway** is 'YES'.   
+If **QoS-1** is 'YES, the gateway prepares a proxy for the QoS-1 client.　QoS-1 client has a 'QoS-1' parameter in a clients.conf file.　For QoS-1 clients, set the QoS-1 parameters in the clients.conf file.
+If **Forwarder** is 'YES', the gateway prepare a forwarder agent.   
+If **ClientAuthentication** is 'YES', the client cannot connect unless it is registered in the clients.conf file.  
+**ClientsList** defines clients and those address so on.    
+**PredefinedTopicList** file defines Predefined Topic.    

+
+```
+#==============================
+#  SensorNetworks parameters
+#==============================
+#
+# UDP | DTLS 
+# 
+
+GatewayPortNo=10000
+MulticastPortNo=1883
+MulticastIP=225.1.1.1
+MulticastTTL=1
+```
+**GatewayPortNo** is a unicast port no of the gateway.  
+**MulticastIP** and **MulticastPortNo** are for GWSEARCH messages. Clients can get the gateway address (Gateway IP address and GatewayPortNo) from GWINFO message by means of std::recvfrom().   
+Client needs to know the MulticastIP and MulticastPortNo to send a SEARCHGW message. 
+**MulticastTTL** is a multicast TTL.    
+```
+#
+# UDP6 | DTLS6
+#
+
+GatewayIPv6PortNo=10000
+MulticastIPv6PortNo=1883
+MulticastIPv6=ff1e:feed:caca:dead::feed:caca:dead
+MulticastIPv6If=wlp4s0
+MulticastHops=1
+```
+**GatewayIPv6PortNo** is a unicast port no of the gateway.
+**MulticastIPv6PortNo** and **MulticastIPv6** are for GWSEARCH messages. Set the Global scope Multicast address so that the Global address is used for sending GWINFO.   
+Clients can get the gateway address (Gateway IPv6 address and GatewayPortNo) from GWINFO message by means of std::recvfrom(). 
+**MulticastIPv6If** is a  multicast interface name.    
+**MulticastHops** is a multicast hops.    
+```
+#
+# DTLS | DTLS6  DTLS CertsKey  
+#
+
+DtlsCertsKey=/etc/ssl/certs/gateway.pem
+DtlsPrivKey=/etc/ssl/private/privkey.pem
+```
+**DtlsCertsKey** is a certs Key pem file for DTLS connection.        
+**DtlsPrivKey** is a private key pem file for DTLS connection.    
+```
+#
 # XBee
-Baudrate=38400    
-SerialDevice=/dev/ttyUSB0    
-ApiMode=2    
-````    
+#

-**BrokerName** to specify a domain name of the Broker, and **BrokerPortNo** is a port No of the Broker. **BrokerSecurePortNo** is for TLS connection.       
-**MulticastIP** and **MulticastPortNo** is a multicast address for ADVERTISE, GWSEARCH and GWINFO messages. Gateway is waiting GWSEARCH multicast message and when receiving it send GWINFO message via Broadcast address. Clients can get the gateway address (Gateway IP address and **GatewayPortNo**) from GWINFO message by means of std::recvfrom(),
-Client should know the BroadcastIP and PortNo to send a SEARCHGW message.    
-**GatewayId** is defined by GWSEARCH 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.    
+Baudrate=38400
+SerialDevice=/dev/ttyUSB0
+ApiMode=2
+```
+**Baudrate** is a baudrate of xbee.    
+```
+#
+# LoRaLink
+#

+BaudrateLoRaLink=115200
+DeviceRxLoRaLink=/dev/loralinkRx
+DeviceTxLoRaLink=/dev/loralinkTx
+```
+https://github.com/ty4tw/MQTT-SN-LoRa    

+```
+#
+# Bluetooth RFCOMM
+#

-### ** How to monitor the gateway from remote. **
+RFCOMMAddress=60:57:18:06:8B:72.*
+```
+**RFCOMMAddress** is a bluetooth mac address and channel. channel should be * for the gateway.
+```
+#
+# LOG
+#

-Uncomment line32 in MQTTSNGWDefined.h.
+ShearedMemory=NO
+```

-`//#define RINGBUFFER     // print out Packets log into shared memory./"`    
-````    
-$ make   
-$ make install 
-$ make clean
-````
-restart the gateway.    
+### How to monitor the gateway from a remote terminal.
+Change gateway.conf as follows:
+```
+# LOG
+ShearedMemory=YES;
+```
+Restart the gateway with sudo only once to create shared memories. 
 open ssh terminal and execute LogMonitor.
-
-`$ ./MQTT-SNLogmonitor`    
-
+```
+ $ cd bin
+ $ ./MQTT-SNLogmonitor
+```
 Now you can get the Log on your terminal.


+##### Tips 
+Use compiler definitions then you can get more precise logs.    
+**-DDEBUG_NW** is a flag for debug logs of Sensor network.   
+**-DDEBUG_MQTTSN** is a flag for debug logs of MQTT-SN message haandling.    
+One or both flags can be specified.    
+
+```
+./build.sh udp -DDEBUG -DDEBUG_NW
+```
--- a/MQTTSNGateway/build.sh
+++ b/MQTTSNGateway/build.sh
@@ -0,0 +1,46 @@
+#!/bin/bash
+
+WORK_DIR=$(realpath $(dirname "$0")/..)
+BDIR=build.gateway
+ODIR=bin/
+
+build () {
+    echo "Start building MQTT-SN Gateway  $1"
+    
+    pushd "$WORK_DIR"
+    if [ ! -d ./$BDIR ]; then
+        mkdir $BDIR
+    fi
+    cd $BDIR
+    cmake .. -DSENSORNET=$1 -DDEFS="${2} ${3}"
+    make MQTTSNPacket
+    make MQTT-SNGateway
+    make MQTT-SNLogmonitor
+    popd
+    cp *.conf ./$ODIR
+}
+
+if [ $1 == "udp" ] ; then
+    build $1 $2 $3
+elif [ $1 == "udp6" ] ; then 
+    build $1 $2 $3
+elif [ $1 == "xbee" ] ; then
+    build $1 $2 $3
+elif [ $1 == "loralink" ]; then
+    build $1 $2 $3
+elif [ $1 == "rfcomm" ] ; then 
+    build $1 $2 $3
+elif [ $1 == "dtls" ] ; then
+    build $1 $2 $3
+elif [ $1 == "dtls6" ] ; then
+    build dtls "${2} ${3} -DDTLS6"
+elif [ $1 == "clean" ] ; then
+    pushd "$WORK_DIR"
+    rm -rf ./$BDIR
+    popd
+    rm -rf ./$ODIR
+else
+    echo "Usage: build.sh  [ udp | udp6 | xbee | loralink | rfcomm | dtls | dtls6 | clean]"
+fi
+
+
--- a/MQTTSNGateway/clients.conf
+++ b/MQTTSNGateway/clients.conf
@@ -11,8 +11,48 @@
 #   http://www.eclipse.org/org/documents/edl-v10.php.
 #***********************************************************************
 #
+# File format is:
+#     Lines bigning with # are comment line.
+#     ClientId, SensorNetAddress, "unstableLine", "secureConnection"
+#     in case of UDP, SensorNetAddress format is IPAddress: port no.
+#     if the SensorNetwork is not stable, specify "unstableLine".
+#     if Broker's Connection is TLS, specify "secureConnection".
+#     if the client is a forwarder,specify "forwarder".
+#     if the client send PUBLISH QoS-1, specify "QoS-1".
+#
+# Ex:
+#     #Client List
+#     ClientId1,192.168.10.10:11200
+#     ClientID2,192.168.50.200:35000,unstableLine
+#     ClientID3,192.168.200.50:40000,secureConnection
+#     ClientID4,192.168.200.52:41000,unstableLine,secureConnection
+#     ClientID5,192.168.200.53:41000,unstableLine,secureConnection,QoS-1 
+#     ClientID6,192.168.200.54:41000,unstableLine,secureConnection,forwarder
+#
 # SensorNetwork address format is defined by SensorNetAddress::setAddress(string* data) function.
 #
-Client02,172.16.1.7:12002
-Client03,172.16.1.8:13003
-Client01,172.16.1.6:12001
+# UDP6 (IPv6 UDP) [IPv6 address]:PortNo
+# RFCOMM          Device_address.channel (1-30)
+# XBee            FFFFFFFFFFFFFFFF　8bytes Hex
+# LoRaLink        1-254 
+#
+#
+# This is a sample of UDP. 
+#
+# REWRITE ALL ACCORDING TO YOUR CLIENTS.
+#
+
+GatewayTester, 172.16.1.11:20020
+ClientPUB,172.16.1.11:2010
+Client01,172.16.1.11:12001
+Client02,172.16.1.11:12002
+Client03,172.16.1.11:13003
+
+QoS-1_Client01,172.16.1.11:20001,QoS-1
+QoS-1_Client02,172.16.1.11:20002,QoS-1
+QoS-1_Client03,172.16.1.11:20003,QoS-1
+
+Forwarder01,172.16.1.11:22002,forwarder
+Forwarder02,172.16.1.11:22003,forwarder
+Forwarder03,172.16.1.11:22004,forwarder
+
--- a/MQTTSNGateway/gateway.conf
+++ b/MQTTSNGateway/gateway.conf
@@ -1,5 +1,5 @@
 #**************************************************************************
-# Copyright (c) 2016, Tomoaki Yamaguchi
+# Copyright (c) 2016-2021, Tomoaki Yamaguchi
 #
 # All rights reserved. This program and the accompanying materials
 # are made available under the terms of the Eclipse Public License v1.0
@@ -10,37 +10,99 @@
 # and the Eclipse Distribution License is available at
 #   http://www.eclipse.org/org/documents/edl-v10.php.
 #***************************************************************************
-
+#
 # config file of MQTT-SN Gateway
-
-BrokerName=iot.eclipse.org
-BrokerPortNo=1883
-BrokerSecurePortNo=8883
-
-ClientAuthentication=NO
-#ClientsList=/path/to/your_clients.conf
-
-#RootCAfile=/etc/ssl/certs/ca-certificates.crt
-#RootCApath=/etc/ssl/certs/
-#CertsFile=/path/to/certKey.pem
-#PrivateKey=/path/to/privateKey.pem
+#

 GatewayID=1
 GatewayName=PahoGateway-01
-KeepAlive=900
+MaxNumberOfClients=30
+KeepAlive=60
 #LoginID=your_ID
 #Password=your_Password

-# UDP
-GatewayPortNo=10000
-MulticastIP=225.1.1.1
-MulticastPortNo=1883
+BrokerName=mqtt.eclipseprojects.io
+BrokerPortNo=1883
+BrokerSecurePortNo=8883

+#
+# CertsKey for TLS connections to a broker
+#
+
+#RootCAfile=/etc/ssl/certs/ca-certificates.crt
+#RootCApath=/etc/ssl/certs/
+#CertKey=/path/to/certKey.pem
+#PrivateKey=/path/to/privateKey.pem
+
+#
+# When AggregatingGateway=YES or ClientAuthentication=YES,
+# All clients must be specified by the ClientList File  
+#
+
+AggregatingGateway=NO
+QoS-1=NO
+Forwarder=NO
+PredefinedTopic=NO
+ClientAuthentication=NO
+
+ClientsList=/path/to/your_clients.conf
+PredefinedTopicList=/path/to/your_predefinedTopic.conf
+
+
+#==============================
+#  SensorNetworks parameters
+#==============================
+#
+# UDP | DTLS 
+# 
+
+GatewayPortNo=10000
+MulticastPortNo=1883
+MulticastIP=225.1.1.1
+MulticastTTL=1
+
+#
+# UDP6 | DTLS6
+#
+
+GatewayIPv6PortNo=10000
+MulticastIPv6PortNo=1883
+MulticastIPv6=ff1e:feed:caca:dead::1
+MulticastIPv6If=wlp4s0
+MulticastHops=1
+
+#
+# DTLS | DTLS6  
+#
+
+DtlsCertsKey=/etc/ssl/certs/gateway.pem
+DtlsPrivKey=/etc/ssl/private/privkey.pem
+
+#
 # XBee
+#
+
 Baudrate=38400
 SerialDevice=/dev/ttyUSB0
 ApiMode=2

-# LOG
-ShearedMemory=NO;
+#
+# LoRaLink
+#
+
+BaudrateLoRaLink=115200
+DeviceRxLoRaLink=/dev/loralinkRx
+DeviceTxLoRaLink=/dev/loralinkTx
+
+#
+# Bluetooth RFCOMM
+#
+
+RFCOMMAddress=60:57:18:06:8B:72.*
+
+#
+# LOG
+#
+
+ShearedMemory=NO

--- a/MQTTSNGateway/predefinedTopic.conf
+++ b/MQTTSNGateway/predefinedTopic.conf
@@ -0,0 +1,36 @@
+#***********************************************************************
+# Copyright (c) 2017, 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.
+#***********************************************************************
+#
+#  pre-defined-topics are defined by this file.
+#  A format of this file is in CSV as follows:
+#
+#     ClientID, TopicName, TopicID
+#
+#     Topics is common to all clients, ClientID should be *.
+
+#
+#  pre-defined-topics for Clients
+#
+
+*,ty4tw/predefinedTopic1, 1
+GatewayTestClient,ty4tw/predefinedTopic2, 2
+GatewayTestClient,ty4tw/predefinedTopic3, 3
+
+#
+#  pre-defined-topics for QoS-1 clients.
+# 
+
+
+QoS-1_Client03,ty4tw/proxy/predefTopic4, 1
+QoS-1_Client03,ty4tw/proxy/predefTopic5, 2
+QoS-1_Client03,ty4tw/proxy/predefTopic6, 3
--- a/MQTTSNGateway/src/CMakeLists.txt
+++ b/MQTTSNGateway/src/CMakeLists.txt
@@ -0,0 +1,151 @@
+#*******************************************************************************
+#  Copyright (c) 2022 a1lu
+#
+#  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:
+#     a1lu  - initial version
+#     ty4tw - modify
+#*******************************************************************************/
+PROJECT(mqtt-sn-gateway CXX)
+
+SET(GW_VERSION_MAJOR 1)
+SET(GW_VERSION_MINOR 5)
+SET(GW_VERSION_PATCH 1)
+
+SET(GATEWAY_VERSION ${GW_VERSION_MAJOR}.${GW_VERSION_MINOR}.${GW_VERSION_PATCH})
+MESSAGE(STATUS "VERSION : ${GATEWAY_VERSION}")
+
+configure_file( MQTTSNGWVersion.h.in ${CMAKE_CURRENT_SOURCE_DIR}/MQTTSNGWVersion.h )
+
+SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/../bin)
+SET(CMAKE_CXX_STANDARD 11)
+
+SET(OS linux)
+
+IF(NOT DEFINED SENSORNET)
+    SET(SENSORNET udp)
+ENDIF()
+MESSAGE(STATUS "SENSORNET: " ${SENSORNET})
+
+ADD_DEFINITIONS(${DEFS})
+MESSAGE(STATUS "Definitions: " ${DEFS})
+
+ADD_LIBRARY(mqtt-sngateway_common
+       MQTTGWConnectionHandler.cpp
+       MQTTGWPacket.cpp
+       MQTTGWPublishHandler.cpp
+       MQTTGWSubscribeHandler.cpp
+       MQTTSNGateway.cpp
+       MQTTSNGWBrokerRecvTask.cpp
+       MQTTSNGWBrokerSendTask.cpp
+       MQTTSNGWClient.cpp
+       MQTTSNGWClientRecvTask.cpp
+       MQTTSNGWClientSendTask.cpp
+       MQTTSNGWConnectionHandler.cpp
+       MQTTSNGWLogmonitor.cpp
+       MQTTSNGWPacket.cpp
+       MQTTSNGWPacketHandleTask.cpp
+       MQTTSNGWProcess.cpp
+       MQTTSNGWPublishHandler.cpp
+       MQTTSNGWSubscribeHandler.cpp
+       MQTTSNGWEncapsulatedPacket.cpp
+       MQTTSNGWForwarder.cpp
+       MQTTSNGWQoSm1Proxy.cpp
+       MQTTSNGWAdapter.cpp
+       MQTTSNGWAggregater.cpp
+       MQTTSNGWClientList.cpp
+       MQTTSNGWTopic.cpp
+       MQTTSNGWAdapterManager.cpp
+       MQTTSNAggregateConnectionHandler.cpp
+       MQTTSNGWMessageIdTable.cpp
+       MQTTSNGWAggregateTopicTable.cpp
+       ${OS}/${SENSORNET}/SensorNetwork.cpp
+       ${OS}/${SENSORNET}/SensorNetwork.h
+       ${OS}/Timer.cpp
+       ${OS}/Timer.h
+       ${OS}/Network.cpp
+       ${OS}/Network.h
+       ${OS}/Threading.cpp
+       ${OS}/Threading.h
+       )
+
+# linux
+link_directories("/usr/local/lib")
+
+# Mac
+link_directories("/usr/local/opt/openssl/lib")
+
+
+TARGET_INCLUDE_DIRECTORIES(mqtt-sngateway_common
+       PUBLIC
+       .
+       ${OS}
+       ${OS}/${SENSORNET}
+       ../../MQTTSNPacket/src
+       /usr/local/include
+       /usr/local/opt/openssl/include
+       )
+
+IF(SENSORNET MATCHES "rfcomm")
+
+TARGET_LINK_LIBRARIES(mqtt-sngateway_common
+       PRIVATE
+       MQTTSNPacket
+       pthread
+       ssl
+       crypto
+       bluetooth
+       )
+ELSE()
+
+TARGET_LINK_LIBRARIES(mqtt-sngateway_common
+       PRIVATE
+       MQTTSNPacket
+       pthread
+       ssl
+       crypto
+       )
+       
+ENDIF()
+
+ADD_EXECUTABLE(MQTT-SNGateway
+       mainGateway.cpp
+       )
+
+TARGET_LINK_LIBRARIES(MQTT-SNGateway
+       mqtt-sngateway_common
+       )
+
+ADD_EXECUTABLE(MQTT-SNLogmonitor
+       mainLogmonitor.cpp
+       )
+
+TARGET_LINK_LIBRARIES(MQTT-SNLogmonitor
+       mqtt-sngateway_common
+       )
+
+ADD_EXECUTABLE(testPFW
+       tests/mainTestProcess.cpp
+       tests/TestProcess.cpp
+       tests/TestQue.cpp
+       tests/TestTree23.cpp
+       tests/TestTopics.cpp
+       tests/TestTopicIdMap.cpp
+       tests/TestTask.cpp
+       )
+TARGET_LINK_LIBRARIES(testPFW
+       mqtt-sngateway_common
+       )
+
+
+ADD_TEST(NAME testPFW
+       WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..
+       COMMAND testPFW -f ./gateway.conf)
--- a/MQTTSNGateway/src/MQTTGWConnectionHandler.cpp
+++ b/MQTTSNGateway/src/MQTTGWConnectionHandler.cpp
@@ -1,5 +1,5 @@
 /**************************************************************************************
- * Copyright (c) 2016, Tomoaki Yamaguchi
+ * Copyright (c) 2016, 2020 Tomoaki Yamaguchi and others
 *
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the Eclipse Public License v1.0
@@ -22,7 +22,7 @@ using namespace MQTTSNGW;

 MQTTGWConnectionHandler::MQTTGWConnectionHandler(Gateway* gateway)
 {
-	_gateway = gateway;
+    _gateway = gateway;
 }

 MQTTGWConnectionHandler::~MQTTGWConnectionHandler()
@@ -32,75 +32,67 @@ MQTTGWConnectionHandler::~MQTTGWConnectionHandler()

 void MQTTGWConnectionHandler::handleConnack(Client* client, MQTTGWPacket* packet)
 {
-	uint8_t rc = MQTT_SERVER_UNAVAILABLE;
-	Connack resp;
-	packet->getCONNACK(&resp);
+    uint8_t rc = MQTT_SERVER_UNAVAILABLE;
+    Connack resp;
+    packet->getCONNACK(&resp);

-	/* convert MQTT ReturnCode to MQTT-SN one */
-	if (resp.rc == MQTT_CONNECTION_ACCEPTED)
-	{
-		rc = MQTTSN_RC_ACCEPTED;
-	}
-	else if (resp.rc == MQTT_UNACCEPTABLE_PROTOCOL_VERSION)
-	{
-		rc = MQTTSN_RC_NOT_SUPPORTED;
-		WRITELOG(" ClientID : %s Requested Protocol version is not supported.\n", client->getClientId());
-	}
-	else if (resp.rc == MQTT_IDENTIFIER_REJECTED)
-	{
-		rc = MQTTSN_RC_NOT_SUPPORTED;
-		WRITELOG(" ClientID : %s ClientID is collect UTF-8 but not allowed by the Server.\n",
-				client->getClientId());
-	}
-	else if (resp.rc == MQTT_SERVER_UNAVAILABLE)
-	{
-		rc = MQTTSN_RC_REJECTED_CONGESTED;
-		WRITELOG(" ClientID : %s The Network Connection has been made but the MQTT service is unavailable.\n",
-				client->getClientId());
-	}
-	else if (resp.rc == MQTT_BAD_USERNAME_OR_PASSWORD)
-	{
-		rc = MQTTSN_RC_NOT_SUPPORTED;
-		WRITELOG(" Gateway Configuration Error: The data in the user name or password is malformed.\n");
-	}
-	else if (resp.rc == MQTT_NOT_AUTHORIZED)
-	{
-		rc = MQTTSN_RC_NOT_SUPPORTED;
-		WRITELOG(" Gateway Configuration Error: The Client is not authorized to connect.\n");
-	}
+    /* convert MQTT ReturnCode to MQTT-SN one */
+    if (resp.rc == MQTT_CONNECTION_ACCEPTED)
+    {
+        rc = MQTTSN_RC_ACCEPTED;
+    }
+    else if (resp.rc == MQTT_UNACCEPTABLE_PROTOCOL_VERSION)
+    {
+        rc = MQTTSN_RC_NOT_SUPPORTED;
+        WRITELOG(" ClientID : %s Requested Protocol version is not supported.\n", client->getClientId());
+    }
+    else if (resp.rc == MQTT_IDENTIFIER_REJECTED)
+    {
+        rc = MQTTSN_RC_NOT_SUPPORTED;
+        WRITELOG(" ClientID : %s ClientID is collect UTF-8 but not allowed by the Server.\n", client->getClientId());
+    }
+    else if (resp.rc == MQTT_SERVER_UNAVAILABLE)
+    {
+        rc = MQTTSN_RC_REJECTED_CONGESTED;
+        WRITELOG(" ClientID : %s The Network Connection has been made but the MQTT service is unavailable.\n",
+                client->getClientId());
+    }
+    else if (resp.rc == MQTT_BAD_USERNAME_OR_PASSWORD)
+    {
+        rc = MQTTSN_RC_NOT_SUPPORTED;
+        WRITELOG(" Gateway Configuration Error: The data in the user name or password is malformed.\n");
+    }
+    else if (resp.rc == MQTT_NOT_AUTHORIZED)
+    {
+        rc = MQTTSN_RC_NOT_SUPPORTED;
+        WRITELOG(" Gateway Configuration Error: The Client is not authorized to connect.\n");
+    }

-	MQTTSNPacket* snPacket = new MQTTSNPacket();
-	snPacket->setCONNACK(rc);
+    MQTTSNPacket* snPacket = new MQTTSNPacket();
+    snPacket->setCONNACK(rc);

-	Event* ev1 = new Event();
-	ev1->setClientSendEvent(client, snPacket);
-	client->connackSended(rc);  // update the client's status
-	_gateway->getClientSendQue()->post(ev1);
-
-	MQTTSNPacket* sleepPacket = 0;
-	while ( (sleepPacket = client->getClientSleepPacket()) )
-	{
-		Event* ev1 = new Event();
-		ev1->setClientSendEvent(client, sleepPacket);
-		_gateway->getClientSendQue()->post(ev1);
-	}
+    Event* ev1 = new Event();
+    ev1->setClientSendEvent(client, snPacket);
+    client->connackSended(rc);  // update the client's status
+    _gateway->getClientSendQue()->post(ev1);
 }

 void MQTTGWConnectionHandler::handlePingresp(Client* client, MQTTGWPacket* packet)
 {
-	MQTTSNPacket* snPacket = new MQTTSNPacket();
-	snPacket->setPINGRESP();
-	Event* ev1 = new Event();
-	ev1->setClientSendEvent(client, snPacket);
-	_gateway->getClientSendQue()->post(ev1);
+    MQTTSNPacket* snPacket = new MQTTSNPacket();
+    snPacket->setPINGRESP();
+    Event* ev1 = new Event();
+    ev1->setClientSendEvent(client, snPacket);
+    client->updateStatus(snPacket);
+    _gateway->getClientSendQue()->post(ev1);
 }

 void MQTTGWConnectionHandler::handleDisconnect(Client* client, MQTTGWPacket* packet)
 {
-	MQTTSNPacket* snPacket = new MQTTSNPacket();
-	snPacket->setDISCONNECT(0);
-	client->disconnected();
-	client->getNetwork()->close();
-	Event* ev1 = new Event();
-	ev1->setClientSendEvent(client, snPacket);
+    MQTTSNPacket* snPacket = new MQTTSNPacket();
+    snPacket->setDISCONNECT(0);
+    client->disconnected();
+    client->getNetwork()->close();
+    Event* ev1 = new Event();
+    ev1->setClientSendEvent(client, snPacket);
 }
--- a/MQTTSNGateway/src/MQTTGWConnectionHandler.h
+++ b/MQTTSNGateway/src/MQTTGWConnectionHandler.h
@@ -26,13 +26,13 @@ namespace MQTTSNGW
 class MQTTGWConnectionHandler
 {
 public:
-	MQTTGWConnectionHandler(Gateway* gateway);
-	~MQTTGWConnectionHandler();
-	void handleConnack(Client* client, MQTTGWPacket* packet);
-	void handlePingresp(Client* client, MQTTGWPacket* packet);
-	void handleDisconnect(Client* client, MQTTGWPacket* packet);
+    MQTTGWConnectionHandler(Gateway* gateway);
+    ~MQTTGWConnectionHandler();
+    void handleConnack(Client* client, MQTTGWPacket* packet);
+    void handlePingresp(Client* client, MQTTGWPacket* packet);
+    void handleDisconnect(Client* client, MQTTGWPacket* packet);
 private:
-	Gateway* _gateway;
+    Gateway* _gateway;
 };

 }
--- a/MQTTSNGateway/src/MQTTGWPacket.cpp
+++ b/MQTTSNGateway/src/MQTTGWPacket.cpp
@@ -21,13 +21,15 @@

 using namespace MQTTSNGW;

+int readInt(char** ptr);
+void writeInt(unsigned char** pptr, int msgId);
+
 #define MAX_NO_OF_REMAINING_LENGTH_BYTES    3
 /**
 * List of the predefined MQTT v3 packet names.
 */
-static const char* mqtt_packet_names[] =
-{ "RESERVED", "CONNECT", "CONNACK", "PUBLISH", "PUBACK", "PUBREC", "PUBREL", "PUBCOMP", "SUBSCRIBE", "SUBACK",
-		"UNSUBSCRIBE", "UNSUBACK", "PINGREQ", "PINGRESP", "DISCONNECT" };
+static const char* mqtt_packet_names[] = { "RESERVED", "CONNECT", "CONNACK", "PUBLISH", "PUBACK", "PUBREC", "PUBREL", "PUBCOMP",
+        "SUBSCRIBE", "SUBACK", "UNSUBSCRIBE", "UNSUBACK", "PINGREQ", "PINGRESP", "DISCONNECT" };

 /**
 * Encodes the message length according to the MQTT algorithm
@@ -37,17 +39,18 @@ static const char* mqtt_packet_names[] =
 */
 int MQTTPacket_encode(char* buf, int length)
 {
-	int rc = 0;
-	do
-	{
-		char d = length % 128;
-		length /= 128;
-		/* if there are more digits to encode, set the top bit of this digit */
-		if (length > 0)
-			d |= 0x80;
-		buf[rc++] = d;
-	} while (length > 0);
-	return rc;
+    int rc = 0;
+    do
+    {
+        char d = length % 128;
+        length /= 128;
+        /* if there are more digits to encode, set the top bit of this digit */
+        if (length > 0)
+            d |= 0x80;
+        buf[rc++] = d;
+    }
+    while (length > 0);
+    return rc;
 }

 /**
@@ -57,10 +60,10 @@ int MQTTPacket_encode(char* buf, int length)
 */
 int readInt(char** pptr)
 {
-	char* ptr = *pptr;
-	int len = 256 * ((unsigned char) (*ptr)) + (unsigned char) (*(ptr + 1));
-	*pptr += 2;
-	return len;
+    char* ptr = *pptr;
+    int len = 256 * ((unsigned char) (*ptr)) + (unsigned char) (*(ptr + 1));
+    *pptr += 2;
+    return len;
 }

 /**
@@ -77,20 +80,20 @@ int readInt(char** pptr)
 */
 char* readUTFlen(char** pptr, char* enddata, int* len)
 {
-	char* string = NULL;
+    char* string = NULL;

-	if (enddata - (*pptr) > 1) /* enough length to read the integer? */
-	{
-		*len = readInt(pptr);
-		if (&(*pptr)[*len] <= enddata)
-		{
-			string = (char*)calloc(*len + 1, 1);
-			memcpy(string, *pptr, (size_t)*len);
-			string[*len] = '\0';
-			*pptr += *len;
-		}
-	}
-	return string;
+    if (enddata - (*pptr) > 1) /* enough length to read the integer? */
+    {
+        *len = readInt(pptr);
+        if (&(*pptr)[*len] <= enddata)
+        {
+            string = (char*) calloc(*len + 1, 1);
+            memcpy(string, *pptr, (size_t) *len);
+            string[*len] = '\0';
+            *pptr += *len;
+        }
+    }
+    return string;
 }

 /**
@@ -105,8 +108,8 @@ char* readUTFlen(char** pptr, char* enddata, int* len)
 */
 char* readUTF(char** pptr, char* enddata)
 {
-	int len;
-	return readUTFlen(pptr, enddata, &len);
+    int len;
+    return readUTFlen(pptr, enddata, &len);
 }

 /**
@@ -116,9 +119,9 @@ char* readUTF(char** pptr, char* enddata)
 */
 unsigned char readChar(char** pptr)
 {
-	unsigned char c = **pptr;
-	(*pptr)++;
-	return c;
+    unsigned char c = **pptr;
+    (*pptr)++;
+    return c;
 }

 /**
@@ -128,8 +131,8 @@ unsigned char readChar(char** pptr)
 */
 void writeChar(unsigned char** pptr, char c)
 {
-	**pptr = c;
-	(*pptr)++;
+    **pptr = c;
+    (*pptr)++;
 }

 /**
@@ -139,10 +142,10 @@ void writeChar(unsigned char** pptr, char c)
 */
 void writeInt(unsigned char** pptr, int anInt)
 {
-	**pptr = (unsigned char)(anInt / 256);
-	(*pptr)++;
-	**pptr = (unsigned char)(anInt % 256);
-	(*pptr)++;
+    **pptr = (unsigned char) (anInt / 256);
+    (*pptr)++;
+    **pptr = (unsigned char) (anInt % 256);
+    (*pptr)++;
 }

 /**
@@ -152,10 +155,10 @@ void writeInt(unsigned char** pptr, int anInt)
 */
 void writeUTF(unsigned char** pptr, const char* string)
 {
-	int len = (int)strlen(string);
-	writeInt(pptr, len);
-	memcpy(*pptr, string, (size_t)len);
-	*pptr += len;
+    int len = (int) strlen(string);
+    writeInt(pptr, len);
+    memcpy(*pptr, string, (size_t) len);
+    *pptr += len;
 }

 /**
@@ -164,379 +167,480 @@ void writeUTF(unsigned char** pptr, const char* string)
 */
 MQTTGWPacket::MQTTGWPacket()
 {
-	_data = 0;
-	_header.byte = 0;
-	_remainingLength = 0;
+    _data = 0;
+    _header.byte = 0;
+    _remainingLength = 0;
 }

 MQTTGWPacket::~MQTTGWPacket()
 {
-	if (_data)
-	{
-		free(_data);
-	}
+    if (_data)
+    {
+        free(_data);
+    }
 }

 int MQTTGWPacket::recv(Network* network)
 {
-	int len = 0;
-	int multiplier = 1;
-	unsigned char c;
+    int len = 0;
+    int multiplier = 1;
+    unsigned char c;

-	/* read First Byte of Packet */
-	int rc = network->recv((unsigned char*)&_header.byte, 1);
-	if ( rc <= 0)
-	{
-		return rc;
-	}
-	/* read RemainingLength */
-	do
-	{
-		if (++len > MAX_NO_OF_REMAINING_LENGTH_BYTES)
-		{
-			return -2;
-		}
-		if (network->recv(&c, 1) == -1)
-		{
-			return -1;
-		}
-		_remainingLength += (c & 127) * multiplier;
-		multiplier *= 128;
-	} while ((c & 128) != 0);
+    /* read First Byte of Packet */
+    int rc = network->recv((unsigned char*) &_header.byte, 1);
+    if (rc <= 0)
+    {
+        return rc;
+    }
+    /* read RemainingLength */
+    do
+    {
+        if (++len > MAX_NO_OF_REMAINING_LENGTH_BYTES)
+        {
+            return -2;
+        }
+        if (network->recv(&c, 1) == -1)
+        {
+            return -1;
+        }
+        _remainingLength += (c & 127) * multiplier;
+        multiplier *= 128;
+    }
+    while ((c & 128) != 0);

-	/* allocate buffer */
-	_data = (unsigned char*)calloc(_remainingLength, 1);
-	if ( !_data )
-	{
-		return -3;
-	}
+    if (_remainingLength > 0)
+    {
+        /* allocate buffer */
+        _data = (unsigned char*) calloc(_remainingLength, 1);
+        if (!_data)
+        {
+            return -3;
+        }

-	/* read Payload */
-	int remlen = network->recv(_data, _remainingLength);
+        /* read Payload */
+        int remlen = network->recv(_data, _remainingLength);

-	if (remlen == -1 )
-	{
-		return -1;
-	}
-	else if ( remlen != _remainingLength )
-	{
-		return -2;
-	}
-	return 1 + len + _remainingLength;
+        if (remlen == -1)
+        {
+            return -1;
+        }
+        else if (remlen != _remainingLength)
+        {
+            return -2;
+        }
+    }
+    return 1 + len + _remainingLength;
 }

 int MQTTGWPacket::send(Network* network)
 {
-	unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
-	memset(buf, 0, MQTTSNGW_MAX_PACKET_SIZE);
-	int len = getPacketData(buf);
-	return network->send(buf, len);
+    unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
+    memset(buf, 0, MQTTSNGW_MAX_PACKET_SIZE);
+    int len = getPacketData(buf);
+    return network->send(buf, len);

 }

 int MQTTGWPacket::getAck(Ack* ack)
 {
-	if (PUBACK != _header.bits.type && PUBREC != _header.bits.type && PUBREL != _header.bits.type
-			&& PUBCOMP != _header.bits.type && UNSUBACK != _header.bits.type)
-	{
-		return 0;
-	}
-	char* ptr = (char*) _data;
-	ack->header.byte = _header.byte;
-	ack->msgId = readInt((char**) &ptr);
-	return 1;
+    if (PUBACK != _header.bits.type && PUBREC != _header.bits.type && PUBREL != _header.bits.type
+            && PUBCOMP != _header.bits.type && UNSUBACK != _header.bits.type)
+    {
+        return 0;
+    }
+    char* ptr = (char*) _data;
+    ack->header.byte = _header.byte;
+    ack->msgId = readInt((char**) &ptr);
+    return 1;
 }

 int MQTTGWPacket::getCONNACK(Connack* resp)
 {
-	if (_header.bits.type != CONNACK)
-	{
-		return 0;
-	}
-	char* ptr = (char*) _data;
-	resp->header.byte = _header.byte;
-	resp->flags.all = *ptr++;
-	resp->rc = readChar(&ptr);
-	return 1;
+    if (_header.bits.type != CONNACK)
+    {
+        return 0;
+    }
+    char* ptr = (char*) _data;
+    resp->header.byte = _header.byte;
+    resp->flags.all = *ptr++;
+    resp->rc = readChar(&ptr);
+    return 1;
 }

 int MQTTGWPacket::getSUBACK(unsigned short* msgId, unsigned char* rc)
 {
-	if (_header.bits.type != SUBACK)
-	{
-		return 0;
-	}
-	char *ptr = (char*) _data;
-	*msgId = readInt((char**) &ptr);
-	*rc = readChar(&ptr);
-	return 1;
+    if (_header.bits.type != SUBACK)
+    {
+        return 0;
+    }
+    char *ptr = (char*) _data;
+    *msgId = readInt((char**) &ptr);
+    *rc = readChar(&ptr);
+    return 1;
 }

 int MQTTGWPacket::getPUBLISH(Publish* pub)
 {
-	if (_header.bits.type != PUBLISH)
-	{
-		return 0;
-	}
-	char* ptr = (char*) _data;
-	pub->header = _header;
-	pub->topiclen = readInt((char**) &ptr);
-	pub->topic = (char*) _data + 2;
-	ptr += pub->topiclen;
-	if (_header.bits.qos > 0)
-	{
-		pub->msgId = readInt(&ptr);
-		pub->payloadlen = _remainingLength - pub->topiclen - 4;
-	}
-	else
-	{
-		pub->msgId = 0;
-		pub->payloadlen = _remainingLength - pub->topiclen - 2;
-	}
-	pub->payload = ptr;
-	return 1;
+    if (_header.bits.type != PUBLISH)
+    {
+        return 0;
+    }
+    char* ptr = (char*) _data;
+    pub->header = _header;
+    pub->topiclen = readInt((char**) &ptr);
+    pub->topic = (char*) _data + 2;
+    ptr += pub->topiclen;
+    if (_header.bits.qos > 0)
+    {
+        pub->msgId = readInt(&ptr);
+        pub->payloadlen = _remainingLength - pub->topiclen - 4;
+    }
+    else
+    {
+        pub->msgId = 0;
+        pub->payloadlen = _remainingLength - pub->topiclen - 2;
+    }
+    pub->payload = ptr;
+    return 1;
 }

 int MQTTGWPacket::setCONNECT(Connect* connect, unsigned char* username, unsigned char* password)
 {
-	clearData();
-	_header = connect->header;
+    clearData();
+    _header = connect->header;

-	_remainingLength = ((connect->version == 3) ? 12 : 10) + (int)strlen(connect->clientID) + 2;
-	if (connect->flags.bits.will)
-	{
-		_remainingLength += (int)strlen(connect->willTopic) + 2 + (int)strlen(connect->willMsg) + 2;
-	}
-	if ( connect->flags.bits.username )
-	{
-		_remainingLength += (int)strlen((char*) username) + 2;
-	}
-	if (connect->flags.bits.password)
-	{
-		_remainingLength += (int)strlen((char*) password) + 2;
-	}
+    _remainingLength = ((connect->version == 3) ? 12 : 10) + (int) strlen(connect->clientID) + 2;
+    if (connect->flags.bits.will)
+    {
+        _remainingLength += (int) strlen(connect->willTopic) + 2 + (int) strlen(connect->willMsg) + 2;
+    }
+    if (connect->flags.bits.username)
+    {
+        _remainingLength += (int) strlen((char*) username) + 2;
+    }
+    if (connect->flags.bits.password)
+    {
+        _remainingLength += (int) strlen((char*) password) + 2;
+    }

-	_data = (unsigned char*)calloc(_remainingLength, 1);
-	unsigned char* ptr = _data;
+    _data = (unsigned char*) calloc(_remainingLength, 1);
+    unsigned char* ptr = _data;

-	if (connect->version == 3)
-	{
-		writeUTF(&ptr, "MQIsdp");
-		writeChar(&ptr, (char) 3);
-	}
-	else if (connect->version == 4)
-	{
-		writeUTF(&ptr, "MQTT");
-		writeChar(&ptr, (char) 4);
-	}
-	else
-	{
-		return 0;
-	}
+    if (connect->version == 3)
+    {
+        writeUTF(&ptr, "MQIsdp");
+        writeChar(&ptr, (char) 3);
+    }
+    else if (connect->version == 4)
+    {
+        writeUTF(&ptr, "MQTT");
+        writeChar(&ptr, (char) 4);
+    }
+    else
+    {
+        return 0;
+    }

-	writeChar(&ptr, connect->flags.all);
-	writeInt(&ptr, connect->keepAliveTimer);
-	writeUTF(&ptr, connect->clientID);
-	if (connect->flags.bits.will)
-	{
-		writeUTF(&ptr, connect->willTopic);
-		writeUTF(&ptr, connect->willMsg);
-	}
+    writeChar(&ptr, connect->flags.all);
+    writeInt(&ptr, connect->keepAliveTimer);
+    writeUTF(&ptr, connect->clientID);
+    if (connect->flags.bits.will)
+    {
+        writeUTF(&ptr, connect->willTopic);
+        writeUTF(&ptr, connect->willMsg);
+    }

-	if (connect->flags.bits.username)
-	{
-		writeUTF(&ptr, (const char*) username);
-	}
-	if (connect->flags.bits.password)
-	{
-		writeUTF(&ptr, (const char*) password);
-	}
-	return 1;
+    if (connect->flags.bits.username)
+    {
+        writeUTF(&ptr, (const char*) username);
+    }
+    if (connect->flags.bits.password)
+    {
+        writeUTF(&ptr, (const char*) password);
+    }
+    return 1;
 }

 int MQTTGWPacket::setSUBSCRIBE(const char* topic, unsigned char qos, unsigned short msgId)
 {
-	clearData();
-	_header.byte = 0;
-	_header.bits.type = SUBSCRIBE;
-	_header.bits.qos = 1;          // Reserved
-	_remainingLength = (int)strlen(topic) + 5;
-	_data = (unsigned char*)calloc(_remainingLength, 1);
-	if (_data)
-	{
-		unsigned char* ptr = _data;
-		writeInt(&ptr, msgId);
-		writeUTF(&ptr, topic);
-		writeChar(&ptr, (char) qos);
-		return 1;
-	}
-	clearData();
-	return 0;
+    clearData();
+    _header.byte = 0;
+    _header.bits.type = SUBSCRIBE;
+    _header.bits.qos = 1;          // Reserved
+    _remainingLength = (int) strlen(topic) + 5;
+    _data = (unsigned char*) calloc(_remainingLength, 1);
+    if (_data)
+    {
+        unsigned char* ptr = _data;
+        writeInt(&ptr, msgId);
+        writeUTF(&ptr, topic);
+        writeChar(&ptr, (char) qos);
+        return 1;
+    }
+    clearData();
+    return 0;
 }

 int MQTTGWPacket::setUNSUBSCRIBE(const char* topic, unsigned short msgid)
 {
-	clearData();
-	_header.byte = 0;
-	_header.bits.type = UNSUBSCRIBE;
-	_header.bits.qos = 1;
-	_remainingLength = (int)strlen(topic) + 4;
-	_data = (unsigned char*)calloc(_remainingLength, 1);
-	if (_data)
-	{
-		unsigned char* ptr = _data;
-		writeInt(&ptr, msgid);
-		writeUTF(&ptr, topic);
-		return 1;
-	}
-	clearData();
-	return 0;
+    clearData();
+    _header.byte = 0;
+    _header.bits.type = UNSUBSCRIBE;
+    _header.bits.qos = 1;
+    _remainingLength = (int) strlen(topic) + 4;
+    _data = (unsigned char*) calloc(_remainingLength, 1);
+    if (_data)
+    {
+        unsigned char* ptr = _data;
+        writeInt(&ptr, msgid);
+        writeUTF(&ptr, topic);
+        return 1;
+    }
+    clearData();
+    return 0;

 }

 int MQTTGWPacket::setPUBLISH(Publish* pub)
 {
-	clearData();
-	_header.byte = pub->header.byte;
-	_header.bits.type = PUBLISH;
-	_remainingLength = 4 + pub->topiclen + pub->payloadlen;
-	_data = (unsigned char*)calloc(_remainingLength, 1);
-	if (_data)
-	{
-		unsigned char* ptr = _data;
-		writeInt(&ptr, pub->topiclen);
-		memcpy(ptr, pub->topic, pub->topiclen);
-		ptr += pub->topiclen;
-		if ( _header.bits.qos > 0 )
-		{
-			writeInt(&ptr, pub->msgId);
-		}
-		else
-		{
-			_remainingLength -= 2;
-		}
-		memcpy(ptr, pub->payload, pub->payloadlen);
-		return 1;
-	}
-	else
-	{
-		clearData();
-		return 0;
-	}
+    clearData();
+    _header.byte = pub->header.byte;
+    _header.bits.type = PUBLISH;
+    _remainingLength = 4 + pub->topiclen + pub->payloadlen;
+    _data = (unsigned char*) calloc(_remainingLength, 1);
+    if (_data)
+    {
+        unsigned char* ptr = _data;
+        writeInt(&ptr, pub->topiclen);
+        memcpy(ptr, pub->topic, pub->topiclen);
+        ptr += pub->topiclen;
+        if (_header.bits.qos > 0)
+        {
+            writeInt(&ptr, pub->msgId);
+        }
+        else
+        {
+            _remainingLength -= 2;
+        }
+        memcpy(ptr, pub->payload, pub->payloadlen);
+        return 1;
+    }
+    else
+    {
+        clearData();
+        return 0;
+    }
 }

 int MQTTGWPacket::setAck(unsigned char msgType, unsigned short msgid)
 {
-	clearData();
-	_remainingLength = 2;
-	_header.bits.type = msgType;
-	_header.bits.qos = (msgType == PUBREL) ? 1 : 0;
+    clearData();
+    _remainingLength = 2;
+    _header.bits.type = msgType;
+    _header.bits.qos = (msgType == PUBREL) ? 1 : 0;

-	_data = (unsigned char*)calloc(_remainingLength, 1);
-	if (_data)
-	{
-		unsigned char* data = _data;
-		writeInt(&data, msgid);
-		return 1;
-	}
-	return 0;
+    _data = (unsigned char*) calloc(_remainingLength, 1);
+    if (_data)
+    {
+        unsigned char* data = _data;
+        writeInt(&data, msgid);
+        return 1;
+    }
+    return 0;
 }

 int MQTTGWPacket::setHeader(unsigned char msgType)
 {
-	clearData();
-	if (msgType < CONNECT || msgType > DISCONNECT)
-	{
-		return 0;
-	}
-	_header.bits.type = msgType;
-	return 0;
+    clearData();
+    if (msgType < CONNECT || msgType > DISCONNECT)
+    {
+        return 0;
+    }
+    _header.bits.type = msgType;
+    return 0;
 }

 int MQTTGWPacket::getType(void)
 {
-	return _header.bits.type;
+    return _header.bits.type;
 }

 const char* MQTTGWPacket::getName(void)
 {
-	return getType() > DISCONNECT ? "UNKNOWN" : mqtt_packet_names[getType()];
+    return getType() > DISCONNECT ? "UNKNOWN" : mqtt_packet_names[getType()];
 }

 int MQTTGWPacket::getPacketData(unsigned char* buf)
 {
-	unsigned char* ptr = buf;
-	*ptr++ = _header.byte;
-	int len = MQTTPacket_encode((char*)ptr, _remainingLength);
-	ptr += len;
-	memcpy(ptr, _data, _remainingLength);
-	return 1 + len + _remainingLength;
+    unsigned char* ptr = buf;
+    *ptr++ = _header.byte;
+    int len = MQTTPacket_encode((char*) ptr, _remainingLength);
+    ptr += len;
+    memcpy(ptr, _data, _remainingLength);
+    return 1 + len + _remainingLength;
 }

 int MQTTGWPacket::getPacketLength(void)
 {
-	char buf[4];
-	return 1 + MQTTPacket_encode(buf, _remainingLength) + _remainingLength;
+    char buf[4];
+    return 1 + MQTTPacket_encode(buf, _remainingLength) + _remainingLength;
 }

 void MQTTGWPacket::clearData(void)
 {
-	if (_data)
-	{
-		free(_data);
-	}
-	_header.byte = 0;
-	_remainingLength = 0;
+    if (_data)
+    {
+        free(_data);
+    }
+    _header.byte = 0;
+    _remainingLength = 0;
 }

 char* MQTTGWPacket::getMsgId(char* pbuf)
 {
-	int type = getType();
+    int type = getType();

-	switch ( type )
-	{
-	case PUBLISH:
-		Publish pub;
-		getPUBLISH(&pub);
-		if ( _header.bits.dup )
-		{
-			sprintf(pbuf, "+%04X", pub.msgId);
-		}
-		else
-		{
-			sprintf(pbuf, " %04X", pub.msgId);
-		}
-		break;
-	case PUBACK:
-	case PUBREC:
-	case PUBREL:
-	case PUBCOMP:
-	case SUBSCRIBE:
-	case UNSUBSCRIBE:
-	case SUBACK:
-	case UNSUBACK:
-		sprintf(pbuf, " %02X%02X", _data[0], _data[1]);
-		break;
-	default:
-		sprintf(pbuf, "    ");
-		break;
-	}
-	return pbuf;
+    switch (type)
+    {
+    case PUBLISH:
+        Publish pub;
+        pub.msgId = 0;
+        getPUBLISH(&pub);
+        if (_header.bits.dup)
+        {
+            sprintf(pbuf, "+%04X", pub.msgId);
+        }
+        else
+        {
+            sprintf(pbuf, " %04X", pub.msgId);
+        }
+        break;
+    case SUBSCRIBE:
+    case UNSUBSCRIBE:
+    case PUBACK:
+    case PUBREC:
+    case PUBREL:
+    case PUBCOMP:
+    case SUBACK:
+    case UNSUBACK:
+        sprintf(pbuf, " %02X%02X", _data[0], _data[1]);
+        break;
+    default:
+        sprintf(pbuf, "    ");
+        break;
+    }
+    if (strcmp(pbuf, " 0000") == 0)
+    {
+        sprintf(pbuf, "    ");
+    }
+    return pbuf;
+}
+
+int MQTTGWPacket::getMsgId(void)
+{
+    int type = getType();
+    int msgId = 0;
+
+    switch (type)
+    {
+    case PUBLISH:
+        Publish pub;
+        pub.msgId = 0;
+        getPUBLISH(&pub);
+        msgId = pub.msgId;
+        break;
+    case PUBACK:
+    case PUBREC:
+    case PUBREL:
+    case PUBCOMP:
+    case SUBSCRIBE:
+    case UNSUBSCRIBE:
+    case SUBACK:
+    case UNSUBACK:
+        msgId = 256 * (unsigned char) _data[0] + (unsigned char) _data[1];
+        break;
+    default:
+        break;
+    }
+    return msgId;
+}
+
+void MQTTGWPacket::setMsgId(int msgId)
+{
+    int type = getType();
+    unsigned char* ptr = 0;
+
+    switch (type)
+    {
+    case PUBLISH:
+        Publish pub;
+        pub.topiclen = 0;
+        pub.msgId = 0;
+        getPUBLISH(&pub);
+        pub.msgId = msgId;
+        ptr = _data + pub.topiclen;
+        writeInt(&ptr, pub.msgId);
+        *ptr++ = (unsigned char) (msgId / 256);
+        *ptr = (unsigned char) (msgId % 256);
+        break;
+    case SUBSCRIBE:
+    case UNSUBSCRIBE:
+    case PUBACK:
+    case PUBREC:
+    case PUBREL:
+    case PUBCOMP:
+    case SUBACK:
+    case UNSUBACK:
+        ptr = _data;
+        *ptr++ = (unsigned char) (msgId / 256);
+        *ptr = (unsigned char) (msgId % 256);
+        break;
+    default:
+        break;
+    }
 }

 char* MQTTGWPacket::print(char* pbuf)
 {
-	uint8_t packetData[MQTTSNGW_MAX_PACKET_SIZE];
-	char* ptr = pbuf;
-	char** pptr = &pbuf;
-	int len = getPacketData(packetData);
-
-	for (int i = 0; i < len; i++)
-	{
-		sprintf(*pptr, " %02X", packetData[i]);
-		*pptr += 3;
-	}
-	**pptr = 0;
-	return ptr;
+    uint8_t packetData[MQTTSNGW_MAX_PACKET_SIZE];
+    char* ptr = pbuf;
+    char** pptr = &pbuf;
+    int len = getPacketData(packetData);
+    int size = len > SIZE_OF_LOG_PACKET ? SIZE_OF_LOG_PACKET : len;
+    for (int i = 0; i < size; i++)
+    {
+        sprintf(*pptr, " %02X", packetData[i]);
+        *pptr += 3;
+    }
+    **pptr = 0;
+    return ptr;
 }

+MQTTGWPacket& MQTTGWPacket::operator =(MQTTGWPacket& packet)
+{
+    clearData();
+    this->_header.byte = packet._header.byte;
+    this->_remainingLength = packet._remainingLength;
+    _data = (unsigned char*) calloc(_remainingLength, 1);
+    if (_data)
+    {
+        memcpy(this->_data, packet._data, _remainingLength);
+    }
+    else
+    {
+        clearData();
+    }
+    return *this;
+}
+
+UTF8String MQTTGWPacket::getTopic(void)
+{
+    UTF8String str = { 0, nullptr };
+    if (_header.bits.type == SUBSCRIBE || _header.bits.type == UNSUBSCRIBE)
+    {
+        char* ptr = (char*) (_data + 2);
+        str.len = readInt(&ptr);
+        str.data = (char*) (_data + 4);
+    }
+    return str;
+}
--- a/MQTTSNGateway/src/MQTTGWPacket.h
+++ b/MQTTSNGateway/src/MQTTGWPacket.h
@@ -31,186 +31,218 @@ typedef void* (*pf)(unsigned char, char*, size_t);

 enum msgTypes
 {
-	CONNECT = 1, CONNACK, PUBLISH, PUBACK, PUBREC, PUBREL,
-	PUBCOMP, SUBSCRIBE, SUBACK, UNSUBSCRIBE, UNSUBACK,
-	PINGREQ, PINGRESP, DISCONNECT
+    CONNECT = 1,
+    CONNACK,
+    PUBLISH,
+    PUBACK,
+    PUBREC,
+    PUBREL,
+    PUBCOMP,
+    SUBSCRIBE,
+    SUBACK,
+    UNSUBSCRIBE,
+    UNSUBACK,
+    PINGREQ,
+    PINGRESP,
+    DISCONNECT
 };

-
 /**
 * Bitfields for the MQTT header byte.
 */
 typedef union
 {
-	/*unsigned*/ char byte;	/**< the whole byte */
+    /*unsigned*/
+    char byte; /**< the whole byte */
 #if defined(REVERSED)
-	struct
-	{
-		unsigned int type : 4;	/**< message type nibble */
-		bool dup : 1;			/**< DUP flag bit */
-		unsigned int qos : 2;	/**< QoS value, 0, 1 or 2 */
-		bool retain : 1;		/**< retained flag bit */
-	} bits;
+    struct
+    {
+        unsigned int type : 4; /**< message type nibble */
+        bool dup : 1; /**< DUP flag bit */
+        unsigned int qos : 2; /**< QoS value, 0, 1 or 2 */
+        bool retain : 1; /**< retained flag bit */
+    }bits;
 #else
-	struct
-	{
-		bool retain : 1;		/**< retained flag bit */
-		unsigned int qos : 2;	/**< QoS value, 0, 1 or 2 */
-		bool dup : 1;			/**< DUP flag bit */
-		unsigned int type : 4;	/**< message type nibble */
-	} bits;
+    struct
+    {
+        bool retain :1; /**< retained flag bit */
+        unsigned int qos :2; /**< QoS value, 0, 1 or 2 */
+        bool dup :1; /**< DUP flag bit */
+        unsigned int type :4; /**< message type nibble */
+    } bits;
 #endif
 } Header;

-
 /**
 * Data for a connect packet.
 */

-enum MQTT_connackCodes{
-	MQTT_CONNECTION_ACCEPTED ,
-	MQTT_UNACCEPTABLE_PROTOCOL_VERSION,
-	MQTT_IDENTIFIER_REJECTED,
-	MQTT_SERVER_UNAVAILABLE,
-	MQTT_BAD_USERNAME_OR_PASSWORD,
-	MQTT_NOT_AUTHORIZED
+enum MQTT_connackCodes
+{
+    MQTT_CONNECTION_ACCEPTED,
+    MQTT_UNACCEPTABLE_PROTOCOL_VERSION,
+    MQTT_IDENTIFIER_REJECTED,
+    MQTT_SERVER_UNAVAILABLE,
+    MQTT_BAD_USERNAME_OR_PASSWORD,
+    MQTT_NOT_AUTHORIZED
 };

 typedef struct
 {
-	Header header;	/**< MQTT header byte */
-	union
-	{
-		unsigned char all;	/**< all connect flags */
+    Header header; /**< MQTT header byte */
+    union
+    {
+        unsigned char all; /**< all connect flags */
 #if defined(REVERSED)
-		struct
-		{
-			bool username : 1;			/**< 3.1 user name */
-			bool password : 1; 			/**< 3.1 password */
-			bool willRetain : 1;		/**< will retain setting */
-			unsigned int willQoS : 2;	/**< will QoS value */
-			bool will : 1;			/**< will flag */
-			bool cleanstart : 1;	/**< cleansession flag */
-			int : 1;	/**< unused */
-		} bits;
+        struct
+        {
+            bool username : 1; /**< 3.1 user name */
+            bool password : 1; /**< 3.1 password */
+            bool willRetain : 1; /**< will retain setting */
+            unsigned int willQoS : 2; /**< will QoS value */
+            bool will : 1; /**< will flag */
+            bool cleanstart : 1; /**< cleansession flag */
+            int : 1; /**< unused */
+        }bits;
 #else
-		struct
-		{
-			int : 1;	/**< unused */
-			bool cleanstart : 1;	/**< cleansession flag */
-			bool will : 1;			/**< will flag */
-			unsigned int willQoS : 2;	/**< will QoS value */
-			bool willRetain : 1;		/**< will retain setting */
-			bool password : 1; 			/**< 3.1 password */
-			bool username : 1;			/**< 3.1 user name */
-		} bits;
+        struct
+        {
+            int :1; /**< unused */
+            bool cleanstart :1; /**< cleansession flag */
+            bool will :1; /**< will flag */
+            unsigned int willQoS :2; /**< will QoS value */
+            bool willRetain :1; /**< will retain setting */
+            bool password :1; /**< 3.1 password */
+            bool username :1; /**< 3.1 user name */
+        } bits;
 #endif
-	} flags;	/**< connect flags byte */
+    } flags; /**< connect flags byte */

-	char *Protocol, /**< MQTT protocol name */
-		*clientID,	/**< string client id */
-        *willTopic,	/**< will topic */
-        *willMsg;	/**< will payload */
+    char *Protocol, /**< MQTT protocol name */
+    *clientID, /**< string client id */
+    *willTopic, /**< will topic */
+    *willMsg; /**< will payload */

-	int keepAliveTimer;		/**< keepalive timeout value in seconds */
-	unsigned char version;	/**< MQTT version number */
+    int keepAliveTimer; /**< keepalive timeout value in seconds */
+    unsigned char version; /**< MQTT version number */
 } Connect;

+#define MQTTPacket_Connect_Initializer {{0}, {0}, nullptr, nullptr, nullptr, nullptr, 0, 0}
+#define MQTTPacket_willOptions_initializer { {'M', 'Q', 'T', 'W'}, 0, {NULL, {0, NULL}}, {NULL, {0, NULL}}, 0, 0 }
+#define MQTTPacket_connectData_initializer { {'M', 'Q', 'T', 'C'}, 0, 4, {NULL, {0, NULL}}, 60, 1, 0, \
+        MQTTPacket_willOptions_initializer, {NULL, {0, NULL}}, {NULL, {0, NULL}} }
+
 /**
 * Data for a willMessage.
 */
 typedef struct
 {
-	char* topic;
-	char* msg;
-	int retained;
-	int qos;
-}willMessages;
+    char* topic;
+    char* msg;
+    int retained;
+    int qos;
+} willMessages;

 /**
 * Data for a connack packet.
 */
 typedef struct
 {
-	Header header; /**< MQTT header byte */
-	union
-	{
-		unsigned char all;	/**< all connack flags */
+    Header header; /**< MQTT header byte */
+    union
+    {
+        unsigned char all; /**< all connack flags */
 #if defined(REVERSED)
-		struct
-		{
-			unsigned int reserved : 7;	/**< message type nibble */
-			bool sessionPresent : 1;    /**< was a session found on the server? */
-		} bits;
+        struct
+        {
+            unsigned int reserved : 7; /**< message type nibble */
+            bool sessionPresent : 1; /**< was a session found on the server? */
+        }bits;
 #else
-		struct
-		{
-			bool sessionPresent : 1;    /**< was a session found on the server? */
-			unsigned int reserved : 7;	/**< message type nibble */
-		} bits;
+        struct
+        {
+            bool sessionPresent :1; /**< was a session found on the server? */
+            unsigned int reserved :7; /**< message type nibble */
+        } bits;
 #endif
-	} flags;	 /**< connack flags byte */
-	char rc; /**< connack return code */
+    } flags; /**< connack flags byte */
+    char rc; /**< connack return code */
 } Connack;

-
 /**
 * Data for a publish packet.
 */
 typedef struct
 {
-	Header header;	/**< MQTT header byte */
-	char* topic;	/**< topic string */
-	int topiclen;
-	int msgId;		/**< MQTT message id */
-	char* payload;	/**< binary payload, length delimited */
-	int payloadlen;	/**< payload length */
+    Header header; /**< MQTT header byte */
+    char* topic; /**< topic string */
+    int topiclen;
+    int msgId; /**< MQTT message id */
+    char* payload; /**< binary payload, length delimited */
+    int payloadlen; /**< payload length */
 } Publish;

+#define MQTTPacket_Publish_Initializer {{0}, nullptr, 0, 0, nullptr, 0}

 /**
 * Data for one of the ack packets.
 */
 typedef struct
 {
-	Header header;	/**< MQTT header byte */
-	int msgId;		/**< MQTT message id */
+    Header header; /**< MQTT header byte */
+    int msgId; /**< MQTT message id */
 } Ack;

+/**
+ * UTF8String.
+ */
+typedef struct
+{
+    unsigned char len;
+    char* data;
+} UTF8String;
+
 /**
 * Class MQTT Packet
 */
 class MQTTGWPacket
 {
 public:
-	MQTTGWPacket();
-	~MQTTGWPacket();
-	int recv(Network* network);
-	int send(Network* network);
-	int getType(void);
-	int getPacketData(unsigned char* buf);
-	int getPacketLength(void);
-	const char* getName(void);
+    MQTTGWPacket();
+    ~MQTTGWPacket();
+    int recv(Network* network);
+    int send(Network* network);
+    int getType(void);
+    int getPacketData(unsigned char* buf);
+    int getPacketLength(void);
+    const char* getName(void);

-	int getAck(Ack* ack);
-	int getCONNACK(Connack* resp);
-	int getSUBACK(unsigned short* msgId, unsigned char* rc);
-	int getPUBLISH(Publish* pub);
+    int getAck(Ack* ack);
+    int getCONNACK(Connack* resp);
+    int getSUBACK(unsigned short* msgId, unsigned char* rc);
+    int getPUBLISH(Publish* pub);

-	int setCONNECT(Connect* conect, unsigned char* username, unsigned char* password);
-	int setPUBLISH(Publish* pub);
-	int setAck(unsigned char msgType, unsigned short msgid);
-	int setHeader(unsigned char msgType);
-	int setSUBSCRIBE(const char* topic, unsigned char qos, unsigned short msgId);
-	int setUNSUBSCRIBE(const char* topics, unsigned short msgid);
-	char* getMsgId(char* buf);
-	char* print(char* buf);
+    int setCONNECT(Connect* conect, unsigned char* username,
+            unsigned char* password);
+    int setPUBLISH(Publish* pub);
+    int setAck(unsigned char msgType, unsigned short msgid);
+    int setHeader(unsigned char msgType);
+    int setSUBSCRIBE(const char* topic, unsigned char qos,
+            unsigned short msgId);
+    int setUNSUBSCRIBE(const char* topics, unsigned short msgid);
+
+    UTF8String getTopic(void);
+    char* getMsgId(char* buf);
+    int getMsgId(void);
+    void setMsgId(int msgId);
+    char* print(char* buf);
+    MQTTGWPacket& operator =(MQTTGWPacket& packet);

 private:
-	void  clearData(void);
-	Header	 _header;
-	int _remainingLength;
-	unsigned char* _data;
+    void clearData(void);
+    Header _header;
+    int _remainingLength;
+    unsigned char* _data;
 };

 }
--- a/MQTTSNGateway/src/MQTTGWPublishHandler.cpp
+++ b/MQTTSNGateway/src/MQTTGWPublishHandler.cpp
@@ -12,6 +12,7 @@
 *
 * Contributors:
 *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
+ *    Tieto Poland Sp. z o.o. - Gateway improvements
 **************************************************************************************/

 #include "MQTTGWPublishHandler.h"
@@ -26,7 +27,7 @@ char* currentDateTime(void);

 MQTTGWPublishHandler::MQTTGWPublishHandler(Gateway* gateway)
 {
-	_gateway = gateway;
+    _gateway = gateway;
 }

 MQTTGWPublishHandler::~MQTTGWPublishHandler()
@@ -36,187 +37,284 @@ MQTTGWPublishHandler::~MQTTGWPublishHandler()

 void MQTTGWPublishHandler::handlePublish(Client* client, MQTTGWPacket* packet)
 {
-	if ( !client->isActive() && !client->isSleep() )
-	{
-		WRITELOG("     The client is neither active nor sleep %s\n", client->getStatus());
-		return;
-	}
+    if (!client->isActive() && !client->isSleep() && !client->isAwake())
+    {
+        WRITELOG("%s     The client is neither active nor sleep %s%s\n",
+        ERRMSG_HEADER, client->getStatus(), ERRMSG_FOOTER);
+        return;
+    }

-	Publish pub;
-	packet->getPUBLISH(&pub);
+    /* client is sleeping. save PUBLISH */
+    if (client->isSleep())
+    {
+        Publish pub;
+        packet->getPUBLISH(&pub);

-	MQTTSNPacket* snPacket = new MQTTSNPacket();
+        WRITELOG(FORMAT_Y_G_G, currentDateTime(), packet->getName(),
+        RIGHTARROW, client->getClientId(), "is sleeping. a message was saved.");

-	/* create MQTTSN_topicid */
-	MQTTSN_topicid topicId;
-	uint16_t id = 0;
+        if (pub.header.bits.qos == 1)
+        {
+            replyACK(client, &pub, PUBACK);
+        }
+        else if (pub.header.bits.qos == 2)
+        {
+            replyACK(client, &pub, PUBREC);
+        }

-	if (pub.topiclen == 2)
-	{
-		topicId.type = MQTTSN_TOPIC_TYPE_SHORT;
-		*(topicId.data.short_name) = *pub.topic;
-		*(topicId.data.short_name + 1) = *(pub.topic + 1);
-	}
-	else
-	{
-		topicId.type = MQTTSN_TOPIC_TYPE_NORMAL;
-		topicId.data.long_.len = pub.topiclen;
-		topicId.data.long_.name = pub.topic;
-		id = client->getTopics()->getTopicId(&topicId);
+        MQTTGWPacket* msg = new MQTTGWPacket();
+        *msg = *packet;
+        if (msg->getType() == 0)
+        {
+            WRITELOG("%s MQTTGWPublishHandler::handlePublish can't allocate memories for Packet.%s\n",
+            ERRMSG_HEADER, ERRMSG_FOOTER);
+            delete msg;
+            return;
+        }
+        client->setClientSleepPacket(msg);
+        return;
+    }

-		if ( id > 0 )
-		{
-			topicId.data.id = id;
-		}
-		else
-		{
-			/* This message might be subscribed with wild card. */
-			Topic* topic = client->getTopics()->match(&topicId);
-			if (topic == 0)
-			{
-				WRITELOG(" Invalid Topic. PUBLISH message is canceled.\n");
-				if (pub.header.bits.qos == 1)
-				{
-					replyACK(client, &pub, PUBACK);
-				}
-				else if ( pub.header.bits.qos == 2 )
-				{
-					replyACK(client, &pub, PUBREC);
-				}
-				return;
-			}
+    Publish pub;
+    packet->getPUBLISH(&pub);

-			/* add the Topic and get a TopicId */
-			topic = client->getTopics()->add(&topicId);
-			id = topic->getTopicId();
+    MQTTSNPacket* snPacket = new MQTTSNPacket();

-			if (id > 0)
-			{
-				/* create REGACK */
-				MQTTSNPacket* regPacket = new MQTTSNPacket();
+    /* create MQTTSN_topicid */
+    MQTTSN_topicid topicId;
+    uint16_t id = 0;

-				MQTTSNString topicName;
-				topicName.lenstring.len = topicId.data.long_.len;
-				topicName.lenstring.data = topicId.data.long_.name;
+    if (pub.topiclen <= 2)
+    {
+        topicId.type = MQTTSN_TOPIC_TYPE_SHORT;
+        *(topicId.data.short_name) = *pub.topic;
+        *(topicId.data.short_name + 1) = *(pub.topic + 1);
+    }
+    else
+    {
+        topicId.data.long_.len = pub.topiclen;
+        topicId.data.long_.name = pub.topic;
+        Topic* tp = client->getTopics()->getTopicByName(&topicId);

-				uint16_t regackMsgId = client->getNextSnMsgId();
-				regPacket->setREGISTER(id, regackMsgId, &topicName);
+        if (tp)
+        {
+            topicId.type = tp->getType();
+            topicId.data.long_.len = pub.topiclen;
+            topicId.data.long_.name = pub.topic;
+            topicId.data.id = tp->getTopicId();
+        }
+        else
+        {
+            /* This message might be subscribed with wild card or not cleanSession*/
+            topicId.type = MQTTSN_TOPIC_TYPE_NORMAL;
+            Topic* topic = client->getTopics()->match(&topicId);

-				if (client->isSleep())
-				{
-					client->setClientSleepPacket(regPacket);
-					WRITELOG(FORMAT_BL_NL, currentDateTime(), regPacket->getName(),
-					RIGHTARROW, client->getClientId(), "is sleeping. REGISTER was saved.");
-				}
-				else if (client->isActive())
-				{
-					/* send REGISTER */
-					Event* evrg = new Event();
-					evrg->setClientSendEvent(client, regPacket);
-					_gateway->getClientSendQue()->post(evrg);
-				}
+            if (topic == nullptr && client->isCleanSession())
+            {
+                WRITELOG("%sMQTTGWPublishHandler Invalid Topic. PUBLISH message is discarded.%s\n",
+                ERRMSG_HEADER, ERRMSG_FOOTER);
+                if (pub.header.bits.qos == 1)
+                {
+                    replyACK(client, &pub, PUBACK);
+                }
+                else if (pub.header.bits.qos == 2)
+                {
+                    replyACK(client, &pub, PUBREC);
+                }

-				/* send PUBLISH */
-				topicId.data.id = id;
-				snPacket->setPUBLISH((uint8_t) pub.header.bits.dup, (int) pub.header.bits.qos,
-						(uint8_t) pub.header.bits.retain, (uint16_t) pub.msgId, topicId, (uint8_t*) pub.payload,
-						pub.payloadlen);
-				client->getWaitREGACKPacketList()->setPacket(snPacket, regackMsgId);
-			}
-			else
-			{
-				WRITELOG("\x1b[0m\x1b[31mMQTTGWPublishHandler Can't create a Topic.\n");
-				delete snPacket;
-				return;
-			}
-		}
-	}
+                delete snPacket;
+                return;
+            }

-	/* TopicId was acquired. */
-	if (client->isSleep())
-	{
-		/* client is sleeping. save PUBLISH */
-		client->setClientSleepPacket(snPacket);
-		WRITELOG(FORMAT_Y_G_G, currentDateTime(), packet->getName(),
-		RIGHTARROW, client->getClientId(), "is sleeping. a message was saved.");
-		int type = 0;
-		if (pub.header.bits.qos == 1)
-		{
-			type = PUBACK;
-		}
-		else if ( pub.header.bits.qos == 2)
-		{
-			WRITELOG(" While Client is sleeping, QoS2 is not supported.\n");
-			type = PUBREC;
-		}
-		replyACK(client, &pub, type);
-		pub.header.bits.qos = 0;
-		replyACK(client, &pub, PUBACK);
-		pub.msgId = 0;
-		snPacket->setPUBLISH((uint8_t) pub.header.bits.dup, (int) pub.header.bits.qos,
-				(uint8_t) pub.header.bits.retain, (uint16_t) pub.msgId, topicId, (uint8_t*) pub.payload,
-				pub.payloadlen);
-		client->setClientSleepPacket(snPacket);
-	}
-	else if (client->isActive())
-	{
-		snPacket->setPUBLISH((uint8_t) pub.header.bits.dup, (int) pub.header.bits.qos, (uint8_t) pub.header.bits.retain,
-				(uint16_t) pub.msgId, topicId, (uint8_t*) pub.payload, pub.payloadlen);
-		Event* ev1 = new Event();
-		ev1->setClientSendEvent(client, snPacket);
-		_gateway->getClientSendQue()->post(ev1);
-	}
+            if (topic == nullptr)
+            {
+                topicId.type = MQTTSN_TOPIC_TYPE_NORMAL;
+                topicId.data.long_.len = pub.topiclen;
+                topicId.data.long_.name = pub.topic;
+                topicId.data.id = 0;
+            }
+
+            /* add the Topic and get a TopicId */
+            topic = client->getTopics()->add(&topicId);
+            if (topic == nullptr)
+            {
+                WRITELOG(
+                        "%sMQTTGWPublishHandler Can't Add a Topic. MAX_TOPIC_PAR_CLIENT is exceeded. PUBLISH message is discarded.%s\n",
+                        ERRMSG_HEADER, ERRMSG_FOOTER);
+                delete snPacket;
+                return;
+            }
+            id = topic->getTopicId();
+            if (id > 0)
+            {
+                /* create REGISTER */
+                MQTTSNPacket* regPacket = new MQTTSNPacket();
+
+                MQTTSNString topicName = MQTTSNString_initializer;
+                topicName.lenstring.len = topicId.data.long_.len;
+                topicName.lenstring.data = topicId.data.long_.name;
+
+                uint16_t regackMsgId = client->getNextSnMsgId();
+                regPacket->setREGISTER(id, regackMsgId, &topicName);
+
+                /* send REGISTER */
+                Event* evrg = new Event();
+                evrg->setClientSendEvent(client, regPacket);
+                _gateway->getClientSendQue()->post(evrg);
+
+                /* send PUBLISH */
+                topicId.data.id = id;
+                snPacket->setPUBLISH((uint8_t) pub.header.bits.dup, (int) pub.header.bits.qos, (uint8_t) pub.header.bits.retain,
+                        (uint16_t) pub.msgId, topicId, (uint8_t*) pub.payload, pub.payloadlen);
+                client->getWaitREGACKPacketList()->setPacket(snPacket, regackMsgId);
+                return;
+            }
+            else
+            {
+                WRITELOG("%sMQTTGWPublishHandler Can't create a Topic. PUBLISH message is discarded.%s\n",
+                ERRMSG_HEADER, ERRMSG_FOOTER);
+                delete snPacket;
+                return;
+            }
+        }
+    }
+
+    snPacket->setPUBLISH((uint8_t) pub.header.bits.dup, (int) pub.header.bits.qos, (uint8_t) pub.header.bits.retain,
+            (uint16_t) pub.msgId, topicId, (uint8_t*) pub.payload, pub.payloadlen);
+    Event* ev1 = new Event();
+    ev1->setClientSendEvent(client, snPacket);
+    _gateway->getClientSendQue()->post(ev1);

 }

 void MQTTGWPublishHandler::replyACK(Client* client, Publish* pub, int type)
 {
-	MQTTGWPacket* pubAck = new MQTTGWPacket();
-	pubAck->setAck(type, (uint16_t)pub->msgId);
-	Event* ev1 = new Event();
-	ev1->setBrokerSendEvent(client, pubAck);
-	_gateway->getBrokerSendQue()->post(ev1);
+    MQTTGWPacket* pubAck = new MQTTGWPacket();
+    pubAck->setAck(type, (uint16_t) pub->msgId);
+    Event* ev1 = new Event();
+    ev1->setBrokerSendEvent(client, pubAck);
+    _gateway->getBrokerSendQue()->post(ev1);
 }

 void MQTTGWPublishHandler::handlePuback(Client* client, MQTTGWPacket* packet)
 {
-	Ack ack;
-	packet->getAck(&ack);
-	uint16_t topicId = client->getWaitedPubTopicId((uint16_t)ack.msgId);
-	if (topicId)
-	{
-		MQTTSNPacket* mqttsnPacket = new MQTTSNPacket();
-		mqttsnPacket->setPUBACK(topicId, (uint16_t)ack.msgId, 0);
+    Ack ack;
+    packet->getAck(&ack);
+    TopicIdMapElement* topicId = client->getWaitedPubTopicId((uint16_t) ack.msgId);
+    if (topicId)
+    {
+        MQTTSNPacket* mqttsnPacket = new MQTTSNPacket();
+        mqttsnPacket->setPUBACK(topicId->getTopicId(), (uint16_t) ack.msgId, 0);

-		client->eraseWaitedPubTopicId((uint16_t)ack.msgId);
-		Event* ev1 = new Event();
-		ev1->setClientSendEvent(client, mqttsnPacket);
-		_gateway->getClientSendQue()->post(ev1);
-		return;
-	}
-	WRITELOG(" PUBACK from the Broker is invalid. PacketID : %04X  ClientID : %s \n", (uint16_t)ack.msgId, client->getClientId());
+        client->eraseWaitedPubTopicId((uint16_t) ack.msgId);
+        Event* ev1 = new Event();
+        ev1->setClientSendEvent(client, mqttsnPacket);
+        _gateway->getClientSendQue()->post(ev1);
+        return;
+    }
+    WRITELOG(" PUBACK from the Broker is invalid. PacketID : %04X  ClientID : %s \n", (uint16_t) ack.msgId,
+            client->getClientId());
 }

 void MQTTGWPublishHandler::handleAck(Client* client, MQTTGWPacket* packet, int type)
 {
-	Ack ack;
-	packet->getAck(&ack);
-	MQTTSNPacket* mqttsnPacket = new MQTTSNPacket();
-	if (type == PUBREC)
-	{
-		mqttsnPacket->setPUBREC((uint16_t) ack.msgId);
-	}
-	else if (type == PUBREL)
-	{
-		mqttsnPacket->setPUBREL((uint16_t) ack.msgId);
-	}
-	else if (type == PUBCOMP)
-	{
-		mqttsnPacket->setPUBCOMP((uint16_t) ack.msgId);
-	}
+    Ack ack;
+    packet->getAck(&ack);

-	Event* ev1 = new Event();
-	ev1->setClientSendEvent(client, mqttsnPacket);
-	_gateway->getClientSendQue()->post(ev1);
+    if (client->isActive() || client->isAwake())
+    {
+        MQTTSNPacket* mqttsnPacket = new MQTTSNPacket();
+        if (type == PUBREC)
+        {
+            mqttsnPacket->setPUBREC((uint16_t) ack.msgId);
+        }
+        else if (type == PUBREL)
+        {
+            mqttsnPacket->setPUBREL((uint16_t) ack.msgId);
+        }
+        else if (type == PUBCOMP)
+        {
+            mqttsnPacket->setPUBCOMP((uint16_t) ack.msgId);
+        }
+
+        Event* ev1 = new Event();
+        ev1->setClientSendEvent(client, mqttsnPacket);
+        _gateway->getClientSendQue()->post(ev1);
+    }
+    else if (client->isSleep())
+    {
+        if (type == PUBREL)
+        {
+            MQTTGWPacket* pubComp = new MQTTGWPacket();
+            pubComp->setAck(PUBCOMP, (uint16_t) ack.msgId);
+            Event* ev1 = new Event();
+            ev1->setBrokerSendEvent(client, pubComp);
+            _gateway->getBrokerSendQue()->post(ev1);
+        }
+    }
+}
+
+void MQTTGWPublishHandler::handleAggregatePuback(Client* client, MQTTGWPacket* packet)
+{
+    uint16_t msgId = packet->getMsgId();
+    uint16_t clientMsgId = 0;
+    Client* newClient = _gateway->getAdapterManager()->convertClient(msgId, &clientMsgId);
+    if (newClient != nullptr)
+    {
+        packet->setMsgId((int) clientMsgId);
+        handlePuback(newClient, packet);
+    }
+}
+
+void MQTTGWPublishHandler::handleAggregateAck(Client* client, MQTTGWPacket* packet, int type)
+{
+    uint16_t msgId = packet->getMsgId();
+    uint16_t clientMsgId = 0;
+    Client* newClient = _gateway->getAdapterManager()->convertClient(msgId, &clientMsgId);
+    if (newClient != nullptr)
+    {
+        packet->setMsgId((int) clientMsgId);
+        handleAck(newClient, packet, type);
+    }
+}
+
+void MQTTGWPublishHandler::handleAggregatePubrel(Client* client, MQTTGWPacket* packet)
+{
+    Publish pub;
+    packet->getPUBLISH(&pub);
+    replyACK(client, &pub, PUBCOMP);
+}
+
+void MQTTGWPublishHandler::handleAggregatePublish(Client* client, MQTTGWPacket* packet)
+{
+    Publish pub;
+    packet->getPUBLISH(&pub);
+
+    string* topicName = new string(pub.topic, pub.topiclen); // topic deletes topicName when the topic is deleted
+    Topic topic = Topic(topicName, MQTTSN_TOPIC_TYPE_NORMAL);
+
+    // ToDo: need to refactor
+    ClientTopicElement* elm = _gateway->getAdapterManager()->getAggregater()->getClientElement(&topic);
+
+    while (elm != nullptr)
+    {
+        Client* devClient = elm->getClient();
+        MQTTGWPacket* msg = new MQTTGWPacket();
+        *msg = *packet;
+
+        if (msg->getType() == 0)
+        {
+            WRITELOG("%s MQTTGWPublishHandler::handleAggregatePublish can't allocate memories for Packet.%s\n",
+            ERRMSG_HEADER, ERRMSG_FOOTER);
+            delete msg;
+            break;
+        }
+
+        Event* ev = new Event();
+        ev->setBrokerRecvEvent(devClient, msg);
+        _gateway->getPacketEventQue()->post(ev);
+
+        elm = elm->getNextClientElement();
+    }
 }

--- a/MQTTSNGateway/src/MQTTGWPublishHandler.h
+++ b/MQTTSNGateway/src/MQTTGWPublishHandler.h
@@ -26,20 +26,23 @@ namespace MQTTSNGW
 class MQTTGWPublishHandler
 {
 public:
-	MQTTGWPublishHandler(Gateway* gateway);
-	~MQTTGWPublishHandler();
-	void handlePublish(Client* client, MQTTGWPacket* packet);
-	void handlePuback(Client* client, MQTTGWPacket* packet);
-	void handleAck(Client* client, MQTTGWPacket* packet, int type);
+    MQTTGWPublishHandler(Gateway* gateway);
+    ~MQTTGWPublishHandler();
+    void handlePublish(Client* client, MQTTGWPacket* packet);
+    void handlePuback(Client* client, MQTTGWPacket* packet);
+    void handleAck(Client* client, MQTTGWPacket* packet, int type);
+
+    void handleAggregatePublish(Client* client, MQTTGWPacket* packet);
+    void handleAggregatePuback(Client* client, MQTTGWPacket* packet);
+    void handleAggregateAck(Client* client, MQTTGWPacket* packet, int type);
+    void handleAggregatePubrel(Client* client, MQTTGWPacket* packet);

 private:
-	void replyACK(Client* client, Publish* pub, int type);
+    void replyACK(Client* client, Publish* pub, int type);

-	Gateway* _gateway;
+    Gateway* _gateway;
 };

 }

-
-
 #endif /* MQTTGWPUBLISHHANDLER_H_ */
--- a/MQTTSNGateway/src/MQTTGWSubscribeHandler.cpp
+++ b/MQTTSNGateway/src/MQTTGWSubscribeHandler.cpp
@@ -22,7 +22,7 @@ using namespace MQTTSNGW;

 MQTTGWSubscribeHandler::MQTTGWSubscribeHandler(Gateway* gateway)
 {
-	_gateway = gateway;
+    _gateway = gateway;
 }

 MQTTGWSubscribeHandler::~MQTTGWSubscribeHandler()
@@ -32,43 +32,67 @@ MQTTGWSubscribeHandler::~MQTTGWSubscribeHandler()

 void MQTTGWSubscribeHandler::handleSuback(Client* client, MQTTGWPacket* packet)
 {
-	uint16_t msgId;
-	uint8_t rc;
-	uint8_t returnCode;
-	int qos = 0;
+    uint16_t msgId;
+    uint8_t rc;
+    uint8_t returnCode;
+    int qos = 0;

-	packet->getSUBACK(&msgId, &rc);
-	uint16_t topicId = client->getWaitedSubTopicId(msgId);
+    packet->getSUBACK(&msgId, &rc);
+    TopicIdMapElement* topicId = client->getWaitedSubTopicId(msgId);

-	if (topicId)
-	{
-		MQTTSNPacket* snPacket = new MQTTSNPacket();
-		client->eraseWaitedSubTopicId(msgId);
+    if (topicId)
+    {
+        MQTTSNPacket* snPacket = new MQTTSNPacket();

-		if (rc == 0x80)
-		{
-			returnCode = MQTTSN_RC_REJECTED_INVALID_TOPIC_ID;
-		}
-		else
-		{
-			returnCode = MQTTSN_RC_ACCEPTED;
-			qos = rc;
-		}
-		snPacket->setSUBACK(qos, topicId, msgId, returnCode);
-		Event* evt = new Event();
-		evt->setClientSendEvent(client, snPacket);
-		_gateway->getClientSendQue()->post(evt);
-	}
+        if (rc == 0x80)
+        {
+            returnCode = MQTTSN_RC_REJECTED_INVALID_TOPIC_ID;
+        }
+        else
+        {
+            returnCode = MQTTSN_RC_ACCEPTED;
+            qos = rc;
+        }
+        snPacket->setSUBACK(qos, topicId->getTopicId(), msgId, returnCode);
+        Event* evt = new Event();
+        evt->setClientSendEvent(client, snPacket);
+        _gateway->getClientSendQue()->post(evt);
+        client->eraseWaitedSubTopicId(msgId);
+    }
 }

 void MQTTGWSubscribeHandler::handleUnsuback(Client* client, MQTTGWPacket* packet)
 {
-	Ack ack;
-	packet->getAck(&ack);
-	MQTTSNPacket* snPacket = new MQTTSNPacket();
-	snPacket->setUNSUBACK(ack.msgId);
-	Event* evt = new Event();
-	evt->setClientSendEvent(client, snPacket);
-	_gateway->getClientSendQue()->post(evt);
+    Ack ack;
+    packet->getAck(&ack);
+    MQTTSNPacket* snPacket = new MQTTSNPacket();
+    snPacket->setUNSUBACK(ack.msgId);
+    Event* evt = new Event();
+    evt->setClientSendEvent(client, snPacket);
+    _gateway->getClientSendQue()->post(evt);
+}
+
+void MQTTGWSubscribeHandler::handleAggregateSuback(Client* client, MQTTGWPacket* packet)
+{
+    uint16_t msgId = packet->getMsgId();
+    uint16_t clientMsgId = 0;
+    Client* newClient = _gateway->getAdapterManager()->getAggregater()->convertClient(msgId, &clientMsgId);
+    if (newClient != nullptr)
+    {
+        packet->setMsgId((int) clientMsgId);
+        handleSuback(newClient, packet);
+    }
+}
+
+void MQTTGWSubscribeHandler::handleAggregateUnsuback(Client* client, MQTTGWPacket* packet)
+{
+    uint16_t msgId = packet->getMsgId();
+    uint16_t clientMsgId = 0;
+    Client* newClient = _gateway->getAdapterManager()->getAggregater()->convertClient(msgId, &clientMsgId);
+    if (newClient != nullptr)
+    {
+        packet->setMsgId((int) clientMsgId);
+        handleUnsuback(newClient, packet);
+    }
 }

--- a/MQTTSNGateway/src/MQTTGWSubscribeHandler.h
+++ b/MQTTSNGateway/src/MQTTGWSubscribeHandler.h
@@ -27,13 +27,15 @@ namespace MQTTSNGW
 class MQTTGWSubscribeHandler
 {
 public:
-	MQTTGWSubscribeHandler(Gateway* gateway);
-	~MQTTGWSubscribeHandler();
-	void handleSuback(Client* clnode, MQTTGWPacket* packet);
-	void handleUnsuback(Client* clnode, MQTTGWPacket* packet);
+    MQTTGWSubscribeHandler(Gateway* gateway);
+    ~MQTTGWSubscribeHandler();
+    void handleSuback(Client* clnode, MQTTGWPacket* packet);
+    void handleUnsuback(Client* clnode, MQTTGWPacket* packet);
+    void handleAggregateSuback(Client* client, MQTTGWPacket* packet);
+    void handleAggregateUnsuback(Client* client, MQTTGWPacket* packet);

 private:
-	Gateway* _gateway;
+    Gateway* _gateway;
 };

 }
--- a/MQTTSNGateway/src/MQTTSNAggregateConnectionHandler.cpp
+++ b/MQTTSNGateway/src/MQTTSNAggregateConnectionHandler.cpp
@@ -0,0 +1,204 @@
+/**************************************************************************************
+ * 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 "MQTTSNAggregateConnectionHandler.h"
+#include "MQTTSNGateway.h"
+#include "MQTTSNGWPacket.h"
+#include "MQTTGWPacket.h"
+#include <string.h>
+
+using namespace std;
+using namespace MQTTSNGW;
+
+/*=====================================
+ Class MQTTSNAggregateConnectionHandler
+ =====================================*/
+MQTTSNAggregateConnectionHandler::MQTTSNAggregateConnectionHandler(Gateway* gateway)
+{
+    _gateway = gateway;
+}
+
+MQTTSNAggregateConnectionHandler::~MQTTSNAggregateConnectionHandler()
+{
+
+}
+
+/*
+ *  CONNECT
+ */
+void MQTTSNAggregateConnectionHandler::handleConnect(Client* client, MQTTSNPacket* packet)
+{
+    MQTTSNPacket_connectData data;
+    if (packet->getCONNECT(&data) == 0)
+    {
+        return;
+    }
+
+    /* return CONNACK when the client is sleeping */
+    if (client->isSleep() || client->isAwake())
+    {
+        MQTTSNPacket* packet = new MQTTSNPacket();
+        packet->setCONNACK(MQTTSN_RC_ACCEPTED);
+        Event* ev = new Event();
+        ev->setClientSendEvent(client, packet);
+        _gateway->getClientSendQue()->post(ev);
+        sendStoredPublish(client);
+        return;
+    }
+
+    //* clear ConnectData of Client */
+    Connect* connectData = client->getConnectData();
+    memset(connectData, 0, sizeof(Connect));
+
+    client->disconnected();
+
+    Topics* topics = client->getTopics();
+
+    /* CONNECT was not sent yet. prepare Connect data */
+
+    client->setSessionStatus(false);
+    if (data.cleansession)
+    {
+        /* reset the table of msgNo and TopicId pare */
+        client->clearWaitedPubTopicId();
+        client->clearWaitedSubTopicId();
+
+        /* renew the TopicList */
+        if (topics)
+        {
+            Topic* tp = topics->getFirstTopic();
+            while (tp != nullptr)
+            {
+                if (tp->getType() == MQTTSN_TOPIC_TYPE_NORMAL)
+                {
+                    _gateway->getAdapterManager()->getAggregater()->removeAggregateTopic(tp, client);
+                }
+                tp = topics->getNextTopic(tp);
+            }
+            topics->eraseNormal();
+        }
+        client->setSessionStatus(true);
+    }
+
+    if (data.willFlag)
+    {
+        /* create & send WILLTOPICREQ message to the client */
+        MQTTSNPacket* reqTopic = new MQTTSNPacket();
+        reqTopic->setWILLTOPICREQ();
+        Event* evwr = new Event();
+        evwr->setClientSendEvent(client, reqTopic);
+
+        /* Send WILLTOPICREQ to the client */
+        _gateway->getClientSendQue()->post(evwr);
+    }
+    else
+    {
+        /* create CONNACK & send it to the client */
+        MQTTSNPacket* packet = new MQTTSNPacket();
+        packet->setCONNACK(MQTTSN_RC_ACCEPTED);
+        Event* ev = new Event();
+        ev->setClientSendEvent(client, packet);
+        _gateway->getClientSendQue()->post(ev);
+        client->connackSended(MQTTSN_RC_ACCEPTED);
+        sendStoredPublish(client);
+        return;
+    }
+}
+
+/*
+ *  WILLMSG
+ */
+void MQTTSNAggregateConnectionHandler::handleWillmsg(Client* client, MQTTSNPacket* packet)
+{
+    if (!client->isWaitWillMsg())
+    {
+        DEBUGLOG("     MQTTSNConnectionHandler::handleWillmsg  WaitWillMsgFlg is off.\n");
+        return;
+    }
+
+    MQTTSNString willmsg = MQTTSNString_initializer;
+    //Connect* connectData = client->getConnectData();
+
+    if (client->isConnectSendable())
+    {
+        /* save WillMsg in the client */
+        if (packet->getWILLMSG(&willmsg) == 0)
+        {
+            return;
+        }
+        client->setWillMsg(willmsg);
+
+        /* Send CONNACK to the client */
+        MQTTSNPacket* packet = new MQTTSNPacket();
+        packet->setCONNACK(MQTTSN_RC_ACCEPTED);
+        Event* ev = new Event();
+        ev->setClientSendEvent(client, packet);
+        _gateway->getClientSendQue()->post(ev);
+
+        sendStoredPublish(client);
+        return;
+    }
+}
+
+/*
+ *  DISCONNECT
+ */
+void MQTTSNAggregateConnectionHandler::handleDisconnect(Client* client, MQTTSNPacket* packet)
+{
+    MQTTSNPacket* snMsg = new MQTTSNPacket();
+    snMsg->setDISCONNECT(0);
+    Event* evt = new Event();
+    evt->setClientSendEvent(client, snMsg);
+    _gateway->getClientSendQue()->post(evt);
+}
+
+/*
+ *  PINGREQ
+ */
+void MQTTSNAggregateConnectionHandler::handlePingreq(Client* client, MQTTSNPacket* packet)
+{
+    if ((client->isSleep() || client->isAwake()) && client->getClientSleepPacket())
+    {
+        sendStoredPublish(client);
+        client->holdPingRequest();
+    }
+
+    /* create and send PINGRESP to the PacketHandler */
+    client->resetPingRequest();
+
+    MQTTGWPacket* pingresp = new MQTTGWPacket();
+
+    pingresp->setHeader(PINGRESP);
+
+    Event* evt = new Event();
+    evt->setBrokerRecvEvent(client, pingresp);
+    _gateway->getPacketEventQue()->post(evt);
+}
+
+void MQTTSNAggregateConnectionHandler::sendStoredPublish(Client* client)
+{
+    MQTTGWPacket* msg = nullptr;
+
+    while ((msg = client->getClientSleepPacket()) != nullptr)
+    {
+        client->deleteFirstClientSleepPacket(); // pop the que to delete element.
+
+        Event* ev = new Event();
+        ev->setBrokerRecvEvent(client, msg);
+        _gateway->getPacketEventQue()->post(ev);
+    }
+}
+
--- a/MQTTSNGateway/src/MQTTSNAggregateConnectionHandler.h
+++ b/MQTTSNGateway/src/MQTTSNAggregateConnectionHandler.h
@@ -0,0 +1,47 @@
+/**************************************************************************************
+ * 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_MQTTSNAGGREGATECONNECTIONHANDLER_H_
+#define MQTTSNGATEWAY_SRC_MQTTSNAGGREGATECONNECTIONHANDLER_H_
+
+#include "MQTTSNGWDefines.h"
+
+namespace MQTTSNGW
+{
+class Gateway;
+class Client;
+class MQTTSNPacket;
+
+class MQTTSNAggregateConnectionHandler
+{
+public:
+    MQTTSNAggregateConnectionHandler(Gateway* gateway);
+    ~MQTTSNAggregateConnectionHandler(void);
+
+    void handleConnect(Client* client, MQTTSNPacket* packet);
+    void handleWillmsg(Client* client, MQTTSNPacket* packet);
+    void handleDisconnect(Client* client, MQTTSNPacket* packet);
+    void handlePingreq(Client* client, MQTTSNPacket* packet);
+
+private:
+    void sendStoredPublish(Client* client);
+
+    Gateway* _gateway;
+};
+
+}
+
+#endif /* MQTTSNGATEWAY_SRC_MQTTSNAGGREGATECONNECTIONHANDLER_H_ */
--- a/MQTTSNGateway/src/MQTTSNGWAdapter.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWAdapter.cpp
@@ -0,0 +1,323 @@
+/**************************************************************************************
+ * Copyright (c) 2016, 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 "Timer.h"
+#include "MQTTSNGWDefines.h"
+#include "MQTTSNGateway.h"
+#include "MQTTSNGWAdapter.h"
+#include "SensorNetwork.h"
+#include "MQTTSNGWProcess.h"
+#include "MQTTSNGWClient.h"
+
+#include <string.h>
+using namespace MQTTSNGW;
+
+/*=====================================
+ Class Adapter
+ =====================================*/
+Adapter::Adapter(Gateway* gw)
+{
+    _gateway = gw;
+    _proxy = new Proxy(gw);
+    _proxySecure = new Proxy(gw);
+}
+
+Adapter::~Adapter(void)
+{
+    if (_proxy)
+    {
+        delete _proxy;
+    }
+
+    if (_proxySecure)
+    {
+        delete _proxySecure;
+    }
+}
+
+void Adapter::setup(const char* adpterName, AdapterType adapterType)
+{
+    _isSecure = false;
+    if (_gateway->hasSecureConnection())
+    {
+        _isSecure = true;
+    }
+
+    MQTTSNString id = MQTTSNString_initializer;
+    MQTTSNString idSecure = MQTTSNString_initializer;
+
+    string name = string(adpterName);
+    id.cstring = const_cast<char*>(name.c_str());
+    string nameSecure = string(adpterName) + "-S";
+    idSecure.cstring = const_cast<char*>(nameSecure.c_str());
+
+    Client* client = _gateway->getClientList()->createClient(0, &id, true, false, TRANSPEARENT_TYPE);
+    setClient(client, false);
+    client->setAdapterType(adapterType);
+
+    client = _gateway->getClientList()->createClient(0, &idSecure, true, true,
+    TRANSPEARENT_TYPE);
+    setClient(client, true);
+    client->setAdapterType(adapterType);
+}
+
+Client* Adapter::getClient(SensorNetAddress* addr)
+{
+    Client* client = _gateway->getClientList()->getClient(addr);
+    if (!client)
+    {
+        return nullptr;
+    }
+    else if (client->isQoSm1())
+    {
+        return client;
+    }
+    else
+    {
+        return nullptr;
+    }
+}
+
+const char* Adapter::getClientId(SensorNetAddress* addr)
+{
+    Client* client = getClient(addr);
+    if (!client)
+    {
+        return nullptr;
+    }
+    else if (client->isQoSm1())
+    {
+        return client->getClientId();
+    }
+    else
+    {
+        return nullptr;
+    }
+}
+
+bool Adapter::isSecure(SensorNetAddress* addr)
+{
+    Client* client = getClient(addr);
+    if (!client)
+    {
+        return false;
+    }
+    else if (client->isSecureNetwork())
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+bool Adapter::isSecure(void)
+{
+    return _isSecure;
+}
+
+void Adapter::setClient(Client* client, bool secure)
+{
+    if (secure)
+    {
+        _clientSecure = client;
+    }
+    else
+    {
+        _client = client;
+    }
+}
+
+Client* Adapter::getClient(void)
+{
+    return _client;
+}
+
+Client* Adapter::getSecureClient(void)
+{
+    return _clientSecure;
+}
+
+void Adapter::checkConnection(void)
+{
+    _proxy->checkConnection(_client);
+
+    if (_isSecure)
+    {
+        _proxySecure->checkConnection(_clientSecure);
+    }
+}
+
+void Adapter::send(MQTTSNPacket* packet, Client* client)
+{
+    Proxy* proxy = _proxy;
+    if (client->isSecureNetwork() && !_isSecure)
+    {
+        if (_isSecure)
+        {
+            proxy = _proxySecure;
+        }
+        else
+        {
+            WRITELOG("%s %s  No Secure connections %s 's packet is discarded.%s\n",
+            ERRMSG_HEADER, client->getClientId(), ERRMSG_FOOTER);
+            return;
+        }
+    }
+
+    proxy->recv(packet, client);
+
+}
+
+void Adapter::resetPingTimer(bool secure)
+{
+    if (secure)
+    {
+        _proxySecure->resetPingTimer();
+    }
+    else
+    {
+        _proxy->resetPingTimer();
+    }
+}
+
+bool Adapter::isActive(void)
+{
+    return _isActive;
+}
+
+void Adapter::savePacket(Client* client, MQTTSNPacket* packet)
+{
+    if (client->isSecureNetwork())
+    {
+        _proxySecure->savePacket(client, packet);
+    }
+    else
+    {
+        _proxy->savePacket(client, packet);
+    }
+}
+
+Client* Adapter::getAdapterClient(Client* client)
+{
+    if (client->isSecureNetwork())
+    {
+        return _clientSecure;
+    }
+    else
+    {
+        return _client;
+    }
+}
+
+/*=====================================
+ Class Proxy
+ =====================================*/
+Proxy::Proxy(Gateway* gw)
+{
+    _gateway = gw;
+    _suspendedPacketEventQue = new EventQue();
+}
+Proxy::~Proxy(void)
+{
+    if (_suspendedPacketEventQue)
+    {
+        delete _suspendedPacketEventQue;
+    }
+}
+
+void Proxy::checkConnection(Client* client)
+{
+    if (client->isDisconnect() || (client->isConnecting() && _responseTimer.isTimeup()))
+    {
+        client->connectSended();
+        _responseTimer.start(PROXY_RESPONSE_DURATION * 1000UL);
+        MQTTSNPacket_connectData options = MQTTSNPacket_connectData_initializer;
+        options.clientID.cstring = client->getClientId();
+        options.duration = PROXY_KEEPALIVE_DURATION;
+
+        MQTTSNPacket* packet = new MQTTSNPacket();
+        packet->setCONNECT(&options);
+        Event* ev = new Event();
+        ev->setClientRecvEvent(client, packet);
+        _gateway->getPacketEventQue()->post(ev);
+    }
+    else if ((client->isActive() && _keepAliveTimer.isTimeup()) || (_isWaitingResp && _responseTimer.isTimeup()))
+    {
+        MQTTSNPacket* packet = new MQTTSNPacket();
+        MQTTSNString clientId = MQTTSNString_initializer;
+        packet->setPINGREQ(&clientId);
+        Event* ev = new Event();
+        ev->setClientRecvEvent(client, packet);
+        _gateway->getPacketEventQue()->post(ev);
+        _responseTimer.start(PROXY_RESPONSE_DURATION * 1000UL);
+        _isWaitingResp = true;
+
+        if (++_retryCnt > PROXY_MAX_RETRY_CNT)
+        {
+            client->disconnected();
+        }
+        resetPingTimer();
+    }
+}
+
+void Proxy::resetPingTimer(void)
+{
+    _keepAliveTimer.start(PROXY_KEEPALIVE_DURATION * 1000UL);
+}
+
+void Proxy::recv(MQTTSNPacket* packet, Client* client)
+{
+    if (packet->getType() == MQTTSN_CONNACK)
+    {
+        if (packet->isAccepted())
+        {
+            _responseTimer.stop();
+            _retryCnt = 0;
+            resetPingTimer();
+            sendSuspendedPacket();
+        }
+    }
+    else if (packet->getType() == MQTTSN_PINGRESP)
+    {
+        _isWaitingResp = false;
+        _responseTimer.stop();
+        _retryCnt = 0;
+        resetPingTimer();
+    }
+    else if (packet->getType() == MQTTSN_DISCONNECT)
+    {
+        // blank
+    }
+}
+
+void Proxy::savePacket(Client* client, MQTTSNPacket* packet)
+{
+    MQTTSNPacket* pk = new MQTTSNPacket(*packet);
+    Event* ev = new Event();
+    ev->setClientRecvEvent(client, pk);
+    _suspendedPacketEventQue->post(ev);
+}
+
+void Proxy::sendSuspendedPacket(void)
+{
+    while (_suspendedPacketEventQue->size())
+    {
+        Event* ev = _suspendedPacketEventQue->wait();
+        _gateway->getPacketEventQue()->post(ev);
+    }
+}
+
--- a/MQTTSNGateway/src/MQTTSNGWAdapter.h
+++ b/MQTTSNGateway/src/MQTTSNGWAdapter.h
@@ -0,0 +1,101 @@
+/**************************************************************************************
+ * Copyright (c) 2016, 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_MQTTSNGWADAPTER_H_
+#define MQTTSNGATEWAY_SRC_MQTTSNGWADAPTER_H_
+
+#include <stdint.h>
+#include "Timer.h"
+namespace MQTTSNGW
+{
+class Gateway;
+class Client;
+class Proxy;
+class SensorNetAddress;
+class MQTTSNPacket;
+class MQTTSNGWPacket;
+class EventQue;
+class Timer;
+
+/* When you add a new type, Client::setAdapterType() and Client::isAdapter() functions must be modified. */
+typedef enum
+{
+    Atype_QoSm1Proxy, Atype_Aggregater
+} AdapterType;
+
+/*=====================================
+ Class Adapter
+ =====================================*/
+class Adapter
+{
+public:
+    Adapter(Gateway* gw);
+    ~Adapter(void);
+
+    void setup(const char* adpterName, AdapterType adapterType);
+    const char* getClientId(SensorNetAddress* addr);
+    void setClient(Client* client, bool secure);
+    Client* getClient(SensorNetAddress* addr);
+    Client* getClient(void);
+    Client* getSecureClient(void);
+    Client* getAdapterClient(Client* client);
+    void resetPingTimer(bool secure);
+    void checkConnection(void);
+    void send(MQTTSNPacket* packet, Client* client);
+    bool isActive(void);
+    bool isSecure(SensorNetAddress* addr);
+    bool isSecure(void);
+    void savePacket(Client* client, MQTTSNPacket* packet);
+
+private:
+    Gateway* _gateway { nullptr };
+    Proxy* _proxy { nullptr };
+    Proxy* _proxySecure { nullptr };
+    Client* _client { nullptr };
+    Client* _clientSecure { nullptr };
+    bool _isActive { false };
+    bool _isSecure { false };
+};
+
+/*=====================================
+ Class Proxy
+ =====================================*/
+class Proxy
+{
+public:
+    Proxy(Gateway* gw);
+    ~Proxy(void);
+
+    void setKeepAlive(uint16_t secs);
+    void checkConnection(Client* client);
+    void resetPingTimer(void);
+    void recv(MQTTSNPacket* packet, Client* client);
+    void savePacket(Client* client, MQTTSNPacket* packet);
+
+private:
+    void sendSuspendedPacket(void);
+    Gateway* _gateway;
+    EventQue* _suspendedPacketEventQue
+    { nullptr };
+    Timer _keepAliveTimer;
+    Timer _responseTimer;
+    bool _isWaitingResp { false };
+    int _retryCnt { 0 };
+};
+
+}
+
+#endif /* MQTTSNGATEWAY_SRC_MQTTSNGWADAPTER_H_ */
--- a/MQTTSNGateway/src/MQTTSNGWAdapterManager.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWAdapterManager.cpp
@@ -0,0 +1,208 @@
+/**************************************************************************************
+ * Copyright (c) 2016, 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 "MQTTSNGWDefines.h"
+#include "MQTTSNGateway.h"
+#include "SensorNetwork.h"
+#include "MQTTSNGWProcess.h"
+#include "MQTTSNGWVersion.h"
+#include "MQTTSNGWClientRecvTask.h"
+#include "MQTTSNGWClientSendTask.h"
+#include "MQTTSNGWClient.h"
+#include "MQTTSNGWAggregater.h"
+#include "MQTTSNGWQoSm1Proxy.h"
+#include <string.h>
+using namespace MQTTSNGW;
+
+char* currentDateTime(void);
+
+/*=====================================
+ Class AdapterManager
+ =====================================*/
+AdapterManager::AdapterManager(Gateway* gw)
+{
+    _gateway = gw;
+    _forwarders = new ForwarderList();
+    _qosm1Proxy = new QoSm1Proxy(gw);
+    _aggregater = new Aggregater(gw);
+}
+
+void AdapterManager::initialize(char* gwName, bool aggregate, bool forwarder, bool qosM1)
+{
+    if (aggregate)
+    {
+        _aggregater->initialize(gwName);
+    }
+
+    if (qosM1)
+    {
+        _qosm1Proxy->initialize(gwName);
+    }
+
+    if (forwarder)
+    {
+        _forwarders->initialize(_gateway);
+    }
+}
+
+AdapterManager::~AdapterManager(void)
+{
+    if (_forwarders)
+    {
+        delete _forwarders;
+    }
+    if (_qosm1Proxy)
+    {
+        delete _qosm1Proxy;
+    }
+    if (_aggregater)
+    {
+        delete _aggregater;
+    }
+}
+
+ForwarderList* AdapterManager::getForwarderList(void)
+{
+    return _forwarders;
+}
+
+QoSm1Proxy* AdapterManager::getQoSm1Proxy(void)
+{
+    return _qosm1Proxy;
+}
+
+Aggregater* AdapterManager::getAggregater(void)
+{
+    return _aggregater;
+}
+
+bool AdapterManager::isAggregatedClient(Client* client)
+{
+    if (!_aggregater->isActive() || client->isQoSm1() || client->isAggregater() || client->isQoSm1Proxy())
+    {
+        return false;
+    }
+    else
+    {
+        return true;
+    }
+}
+
+Client* AdapterManager::getClient(Client* client)
+{
+    bool secure = client->isSecureNetwork();
+    Client* newClient = client;
+
+    if (client->isQoSm1())
+    {
+        newClient = _qosm1Proxy->getAdapterClient(client);
+        _qosm1Proxy->resetPingTimer(secure);
+    }
+    else if (client->isAggregated())
+    {
+        newClient = _aggregater->getAdapterClient(client);
+        _aggregater->resetPingTimer(secure);
+    }
+    else if (client->isQoSm1Proxy())
+    {
+        _qosm1Proxy->resetPingTimer(secure);
+    }
+    else if (client->isAggregater())
+    {
+        _aggregater->resetPingTimer(secure);
+    }
+    return newClient;
+}
+
+int AdapterManager::unicastToClient(Client* client, MQTTSNPacket* packet, ClientSendTask* task)
+{
+    char pbuf[SIZE_OF_LOG_PACKET * 3];
+    Forwarder* fwd = client->getForwarder();
+    int rc = 0;
+
+    if (fwd)
+    {
+        MQTTSNGWEncapsulatedPacket encap(packet);
+        WirelessNodeId* wnId = fwd->getWirelessNodeId(client);
+        encap.setWirelessNodeId(wnId);
+        task->log(client, packet);
+        WRITELOG(FORMAT_Y_W_G, currentDateTime(), encap.getName(), RIGHTARROW, fwd->getId(), encap.print(pbuf));
+        rc = encap.unicast(_gateway->getSensorNetwork(), fwd->getSensorNetAddr());
+    }
+    else
+    {
+        task->log(client, packet);
+        if (client->isQoSm1Proxy())
+        {
+            _qosm1Proxy->send(packet, client);
+        }
+        else if (client->isAggregater())
+        {
+            _aggregater->send(packet, client);
+        }
+        else
+        {
+            rc = packet->unicast(_gateway->getSensorNetwork(), client->getSensorNetAddress());
+        }
+    }
+    return rc;
+}
+
+void AdapterManager::checkConnection(void)
+{
+    if (_aggregater->isActive())
+    {
+        _aggregater->checkConnection();
+    }
+
+    if (_qosm1Proxy->isActive())
+    {
+        _qosm1Proxy->checkConnection();
+    }
+}
+
+Client* AdapterManager::convertClient(uint16_t msgId, uint16_t* clientMsgId)
+{
+    return _aggregater->convertClient(msgId, clientMsgId);
+}
+
+bool AdapterManager::isAggregaterActive(void)
+{
+    return _aggregater->isActive();
+}
+
+/*
+ AggregateTopicElement* AdapterManager::findTopic(Topic* topic)
+ {
+ return _aggregater->findTopic(topic);
+ }
+
+ AggregateTopicElement* AdapterManager::addAggregateTopic(Topic* topic, Client* client)
+ {
+ return _aggregater->addAggregateTopic(topic, client);
+ }
+
+
+ void AdapterManager::removeAggregateTopic(Topic* topic, Client* client)
+ {
+ //_aggregater->removeAggregateTopic(topic, client);
+ }
+
+ void AdapterManager::removeAggregateTopicList(Topics* topics, Client* client)
+ {
+
+ }
+ */
+
--- a/MQTTSNGateway/src/MQTTSNGWAdapterManager.h
+++ b/MQTTSNGateway/src/MQTTSNGWAdapterManager.h
@@ -0,0 +1,64 @@
+/**************************************************************************************
+ * 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_MQTTSNGWADAPTERMANAGER_H_
+#define MQTTSNGATEWAY_SRC_MQTTSNGWADAPTERMANAGER_H_
+
+#include "MQTTSNGWAggregater.h"
+#include "MQTTSNGWQoSm1Proxy.h"
+namespace MQTTSNGW
+{
+class Gateway;
+class Client;
+class QoSm1Proxy;
+class Aggregater;
+class ForwarderList;
+class Forwarder;
+class MQTTSNPacket;
+class MQTTSNGWPacket;
+class ClientRecvTask;
+class ClientSendTask;
+
+/*=====================================
+ Class AdapterManager
+ =====================================*/
+class AdapterManager
+{
+public:
+    AdapterManager(Gateway* gw);
+    ~AdapterManager(void);
+    void initialize(char* gwName, bool aggregater, bool fowarder, bool qosM1);
+    ForwarderList* getForwarderList(void);
+    QoSm1Proxy* getQoSm1Proxy(void);
+    Aggregater* getAggregater(void);
+    void checkConnection(void);
+
+    bool isAggregatedClient(Client* client);
+    Client* getClient(Client* client);
+    Client* convertClient(uint16_t msgId, uint16_t* clientMsgId);
+    int unicastToClient(Client* client, MQTTSNPacket* packet,
+            ClientSendTask* task);
+    bool isAggregaterActive(void);
+
+private:
+    Gateway* _gateway { nullptr };
+    ForwarderList* _forwarders { nullptr };
+    QoSm1Proxy* _qosm1Proxy { nullptr };
+    Aggregater* _aggregater { nullptr };
+};
+
+}
+#endif /* MQTTSNGATEWAY_SRC_MQTTSNGWADAPTERMANAGER_H_ */
--- a/MQTTSNGateway/src/MQTTSNGWAggregateTopicTable.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWAggregateTopicTable.cpp
@@ -0,0 +1,320 @@
+/**************************************************************************************
+ * 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 "MQTTSNGWAggregateTopicTable.h"
+#include "MQTTSNGWClient.h"
+
+/*=====================================
+ Class ClientTopicElement
+ =====================================*/
+ClientTopicElement::ClientTopicElement(Client* client)
+{
+    _client = client;
+}
+
+ClientTopicElement::~ClientTopicElement()
+{
+
+}
+
+Client* ClientTopicElement::getClient(void)
+{
+    return _client;
+}
+
+ClientTopicElement* ClientTopicElement::getNextClientElement(void)
+{
+    return _next;
+}
+
+/*=====================================
+ Class AggregateTopicElement
+ =====================================*/
+AggregateTopicElement::AggregateTopicElement(void)
+{
+
+}
+
+AggregateTopicElement::AggregateTopicElement(Topic* topic, Client* client)
+{
+    _topic = topic;
+    ClientTopicElement* elm = new ClientTopicElement(client);
+    if (elm != nullptr)
+    {
+        _head = elm;
+        _tail = elm;
+    }
+}
+
+AggregateTopicElement::~AggregateTopicElement(void)
+{
+    _mutex.lock();
+    if (_head != nullptr)
+    {
+        ClientTopicElement* p = _tail;
+        while (p)
+        {
+            ClientTopicElement* pPrev = p;
+            delete p;
+            p = pPrev->_prev;
+        }
+        _head = _tail = nullptr;
+    }
+    _mutex.unlock();
+}
+
+ClientTopicElement* AggregateTopicElement::add(Client* client)
+{
+    ClientTopicElement* elm = new ClientTopicElement(client);
+    if (elm == nullptr)
+    {
+        return nullptr;
+    }
+
+    _mutex.lock();
+
+    if (_head == nullptr)
+    {
+        _head = elm;
+        _tail = elm;
+    }
+    else
+    {
+        ClientTopicElement* p = find(client);
+        if (p == nullptr)
+        {
+            p = _tail;
+            _tail = elm;
+            elm->_prev = p;
+            p->_next = elm;
+        }
+        else
+        {
+            delete elm;
+            elm = p;
+        }
+    }
+    _mutex.unlock();
+    return elm;
+}
+
+ClientTopicElement* AggregateTopicElement::find(Client* client)
+{
+    ClientTopicElement* p = _head;
+    while (p != nullptr)
+    {
+        if (p->_client == client)
+        {
+            break;
+        }
+        p = p->_next;
+    }
+    return p;
+}
+
+ClientTopicElement* AggregateTopicElement::getFirstClientTopicElement(void)
+{
+    return _head;
+}
+
+ClientTopicElement* AggregateTopicElement::getNextClientTopicElement(ClientTopicElement* elmClient)
+{
+    return elmClient->_next;
+}
+
+void AggregateTopicElement::eraseClient(Client* client)
+{
+    _mutex.lock();
+
+    ClientTopicElement* p = find(client);
+    if (p != nullptr)
+    {
+        if (p->_prev == nullptr)    // head element
+        {
+            _head = p->_next;
+            if (p->_next == nullptr)   // head & only one
+            {
+                _tail = nullptr;
+            }
+            else
+            {
+                p->_next->_prev = nullptr;  // head & midle
+            }
+        }
+        else if (p->_next != nullptr)  // middle
+        {
+            p->_prev->_next = p->_next;
+        }
+        else    // tail
+        {
+            p->_prev->_next = nullptr;
+            _tail = p->_prev;
+        }
+        delete p;
+    }
+    _mutex.unlock();
+}
+
+/*=====================================
+ Class AggregateTopicTable
+ ======================================*/
+
+AggregateTopicTable::AggregateTopicTable()
+{
+
+}
+
+AggregateTopicTable::~AggregateTopicTable()
+{
+
+}
+
+AggregateTopicElement* AggregateTopicTable::add(Topic* topic, Client* client)
+{
+    AggregateTopicElement* elm = nullptr;
+    _mutex.lock();
+    elm = getAggregateTopicElement(topic);
+    if (elm != nullptr)
+    {
+        if (elm->find(client) == nullptr)
+        {
+            elm->add(client);
+        }
+    }
+    else
+    {
+        Topic* newTopic = topic->duplicate();
+        elm = new AggregateTopicElement(newTopic, client);
+        if (_head == nullptr)
+        {
+            _head = elm;
+            _tail = elm;
+        }
+        else
+        {
+            elm->_prev = _tail;
+            _tail->_next = elm;
+            _tail = elm;
+        }
+    }
+    _mutex.unlock();
+    return elm;
+}
+
+void AggregateTopicTable::erase(Topic* topic, Client* client)
+{
+    AggregateTopicElement* elm = nullptr;
+
+    _mutex.lock();
+    elm = getAggregateTopicElement(topic);
+
+    if (elm != nullptr)
+    {
+        elm->eraseClient(client);
+    }
+    if (elm->_head == nullptr)
+    {
+        erase(elm);
+    }
+    _mutex.unlock();
+    return;
+}
+
+void AggregateTopicTable::erase(AggregateTopicElement* elmTopic)
+{
+    if (elmTopic != nullptr)
+    {
+        if (elmTopic->_prev == nullptr)    // head element
+        {
+            _head = elmTopic->_next;
+            if (elmTopic->_next == nullptr)   // head & only one
+            {
+                _tail = nullptr;
+            }
+            else
+            {
+                elmTopic->_next->_prev = nullptr;  // head & midle
+            }
+        }
+        else if (elmTopic->_next != nullptr)  // middle
+        {
+            elmTopic->_prev->_next = elmTopic->_next;
+        }
+        else    // tail
+        {
+            elmTopic->_prev->_next = nullptr;
+            _tail = elmTopic->_prev;
+        }
+        delete elmTopic;
+    }
+}
+
+AggregateTopicElement* AggregateTopicTable::getAggregateTopicElement(Topic* topic)
+{
+    AggregateTopicElement* elm = _head;
+
+    while (elm != nullptr)
+    {
+        if (elm->_topic->isMatch(topic->_topicName))
+        {
+            break;
+        }
+        elm = elm->_next;
+    }
+    return elm;
+}
+
+ClientTopicElement* AggregateTopicTable::getClientElement(Topic* topic)
+{
+    AggregateTopicElement* elm = getAggregateTopicElement(topic);
+    if (elm != nullptr)
+    {
+        return elm->_head;
+    }
+    else
+    {
+        return nullptr;
+    }
+}
+
+void AggregateTopicTable::print(void)
+{
+    AggregateTopicElement* elm = _head;
+
+    printf("Beginning of AggregateTopicTable\n");
+    while (elm != nullptr)
+    {
+        printf("%s\n", elm->_topic->getTopicName()->c_str());
+
+        ClientTopicElement* clElm = elm->getFirstClientTopicElement();
+        Client* client = clElm->getClient();
+
+        while (client != nullptr)
+        {
+            printf("    %s\n", client->getClientId());
+            clElm = clElm->getNextClientElement();
+            if (clElm != nullptr)
+            {
+                client = clElm->getClient();
+            }
+            else
+            {
+                client = nullptr;
+            }
+        }
+        elm = elm->_next;
+    }
+    printf("End of AggregateTopicTable\n");
+}
--- a/MQTTSNGateway/src/MQTTSNGWAggregateTopicTable.h
+++ b/MQTTSNGateway/src/MQTTSNGWAggregateTopicTable.h
@@ -0,0 +1,107 @@
+/**************************************************************************************
+ * 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_MQTTSNGWAGGREGATETOPICTABLE_H_
+#define MQTTSNGATEWAY_SRC_MQTTSNGWAGGREGATETOPICTABLE_H_
+
+#include "MQTTSNGWDefines.h"
+#include "MQTTSNGWProcess.h"
+#include <stdint.h>
+namespace MQTTSNGW
+{
+
+class Client;
+class Topic;
+class AggregateTopicElement;
+class ClientTopicElement;
+class Mutex;
+
+/*=====================================
+ Class AggregateTopicTable
+ ======================================*/
+class AggregateTopicTable
+{
+public:
+    AggregateTopicTable();
+    ~AggregateTopicTable();
+
+    AggregateTopicElement* add(Topic* topic, Client* client);
+    AggregateTopicElement* getAggregateTopicElement(Topic* topic);
+    ClientTopicElement* getClientElement(Topic* topic);
+    void erase(Topic* topic, Client* client);
+    void clear(void);
+
+    void print(void);
+
+private:
+    void erase(AggregateTopicElement* elmTopic);
+    Mutex _mutex;
+    AggregateTopicElement* _head { nullptr };
+    AggregateTopicElement* _tail { nullptr };
+    int _cnt { 0 };
+    int _maxSize { MAX_MESSAGEID_TABLE_SIZE };
+};
+
+/*=====================================
+ Class AggregateTopicElement
+ =====================================*/
+class AggregateTopicElement
+{
+    friend class AggregateTopicTable;
+public:
+    AggregateTopicElement(void);
+    AggregateTopicElement(Topic* topic, Client* client);
+    ~AggregateTopicElement(void);
+
+    ClientTopicElement* add(Client* client);
+    ClientTopicElement* getFirstClientTopicElement(void);
+    ClientTopicElement* getNextClientTopicElement(
+            ClientTopicElement* elmClient);
+    void eraseClient(Client* client);
+    ClientTopicElement* find(Client* client);
+
+private:
+    Mutex _mutex;
+    Topic* _topic { nullptr };
+    AggregateTopicElement* _next { nullptr };
+    AggregateTopicElement* _prev { nullptr };
+    ClientTopicElement* _head { nullptr };
+    ClientTopicElement* _tail { nullptr };
+};
+
+/*=====================================
+ Class ClientTopicElement
+ =====================================*/
+class ClientTopicElement
+{
+    friend class AggregateTopicTable;
+    friend class AggregateTopicElement;
+public:
+    ClientTopicElement(Client* client);
+    ~ClientTopicElement(void);
+
+    ClientTopicElement* getNextClientElement(void);
+    Client* getClient(void);
+
+private:
+    Client* _client { nullptr };
+    ClientTopicElement* _next { nullptr };
+    ClientTopicElement* _prev { nullptr };
+};
+
+}
+
+#endif /* MQTTSNGATEWAY_SRC_MQTTSNGWAGGREGATETOPICTABLE_H_ */
--- a/MQTTSNGateway/src/MQTTSNGWAggregater.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWAggregater.cpp
@@ -0,0 +1,147 @@
+/**************************************************************************************
+ * Copyright (c) 2016, 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 "MQTTSNGWAggregater.h"
+#include "MQTTSNGateway.h"
+#include "MQTTSNGWClient.h"
+#include "MQTTSNGWAdapter.h"
+#include "MQTTSNGWAdapterManager.h"
+#include "MQTTSNGWMessageIdTable.h"
+#include "MQTTSNGWTopic.h"
+#include <stdio.h>
+#include <string.h>
+#include <string>
+
+using namespace MQTTSNGW;
+
+Aggregater::Aggregater(Gateway* gw) :
+        Adapter(gw)
+{
+    _gateway = gw;
+}
+
+Aggregater::~Aggregater(void)
+{
+
+}
+
+void Aggregater::initialize(char* gwName)
+{
+    /* Create Aggregater Client */
+    string name = string(gwName) + string("_Aggregater");
+    setup(name.c_str(), Atype_Aggregater);
+    _isActive = true;
+
+    //testMessageIdTable();
+
+}
+
+bool Aggregater::isActive(void)
+{
+    return _isActive;
+}
+
+uint16_t Aggregater::msgId(void)
+{
+    // Only SecureClient generates msgId to avoid duplication of msgId. Client does not generate it.
+    return Adapter::getSecureClient()->getNextPacketId();
+}
+
+Client* Aggregater::convertClient(uint16_t msgId, uint16_t* clientMsgId)
+{
+    return _msgIdTable.getClientMsgId(msgId, clientMsgId);
+}
+
+uint16_t Aggregater::addMessageIdTable(Client* client, uint16_t msgId)
+{
+    /* set Non secure client`s nextMsgId. otherwise Id is duplicated.*/
+
+    MessageIdElement* elm = _msgIdTable.add(this, client, msgId);
+    if (elm == nullptr)
+    {
+        return 0;
+    }
+    else
+    {
+        return elm->_msgId;
+    }
+}
+
+uint16_t Aggregater::getMsgId(Client* client, uint16_t clientMsgId)
+{
+    return _msgIdTable.getMsgId(client, clientMsgId);
+}
+
+AggregateTopicElement* Aggregater::addAggregateTopic(Topic* topic, Client* client)
+{
+    return _topicTable.add(topic, client);
+}
+
+void Aggregater::removeAggregateTopic(Topic* topic, Client* client)
+{
+    _topicTable.erase(topic, client);
+}
+
+AggregateTopicElement* Aggregater::findTopic(Topic* topic)
+{
+    return _topicTable.getAggregateTopicElement(topic);
+}
+
+ClientTopicElement* Aggregater::getClientElement(Topic* topic)
+{
+    AggregateTopicElement* elm = findTopic(topic);
+    if (elm != nullptr)
+    {
+        return elm->getFirstClientTopicElement();
+    }
+    else
+    {
+        return nullptr;
+    }
+}
+
+void Aggregater::printAggregateTopicTable(void)
+{
+    _topicTable.print();
+}
+
+bool Aggregater::testMessageIdTable(void)
+{
+    Client* client = new Client();
+    uint16_t msgId = 0;
+
+    printf("msgId=%d\n", addMessageIdTable(client, 1));
+    printf("msgId=%d\n", addMessageIdTable(client, 2));
+    printf("msgId=%d\n", addMessageIdTable(client, 3));
+    printf("msgId=%d\n", addMessageIdTable(client, 1));
+    printf("msgId=%d\n", addMessageIdTable(client, 2));
+    printf("msgId=%d\n", addMessageIdTable(client, 3));
+    printf("msgId=%d\n", addMessageIdTable(client, 4));
+    printf("msgId=%d\n", addMessageIdTable(client, 4));
+    printf("msgId=%d\n", addMessageIdTable(client, 4));
+
+    convertClient(1, &msgId);
+    printf("msgId=%d\n", msgId);
+    convertClient(2, &msgId);
+    printf("msgId=%d\n", msgId);
+    convertClient(5, &msgId);
+    printf("msgId=%d\n", msgId);
+    convertClient(4, &msgId);
+    printf("msgId=%d\n", msgId);
+    convertClient(3, &msgId);
+    printf("msgId=%d\n", msgId);
+    return true;
+}
+
--- a/MQTTSNGateway/src/MQTTSNGWAggregater.h
+++ b/MQTTSNGateway/src/MQTTSNGWAggregater.h
@@ -0,0 +1,78 @@
+/**************************************************************************************
+ * 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_MQTTSNGWAGGREGATER_H_
+#define MQTTSNGATEWAY_SRC_MQTTSNGWAGGREGATER_H_
+
+#include "MQTTSNGWAdapter.h"
+#include "MQTTSNGWMessageIdTable.h"
+#include "MQTTSNGWAggregateTopicTable.h"
+
+namespace MQTTSNGW
+{
+class Gateway;
+class Adapter;
+class Client;
+class SensorNetAddress;
+class MessageIdTable;
+class AggregateTopicTable;
+class Topics;
+
+/*=====================================
+ Class Aggregater
+ =====================================*/
+class Aggregater: public Adapter
+{
+    friend class MessageIdTable;
+public:
+    Aggregater(Gateway* gw);
+    ~Aggregater(void);
+
+    void initialize(char* gwName);
+
+    const char* getClientId(SensorNetAddress* addr);
+    Client* getClient(SensorNetAddress* addr);
+    Client* convertClient(uint16_t msgId, uint16_t* clientMsgId);
+    uint16_t addMessageIdTable(Client* client, uint16_t msgId);
+    uint16_t getMsgId(Client* client, uint16_t clientMsgId);
+
+    ClientTopicElement* getClientElement(Topic* topic);
+    ClientTopicElement* getNextClientElement(ClientTopicElement* clientElement);
+    Client* getClient(ClientTopicElement* clientElement);
+
+    AggregateTopicElement* findTopic(Topic* topic);
+    AggregateTopicElement* addAggregateTopic(Topic* topic, Client* client);
+
+    void removeAggregateTopic(Topic* topic, Client* client);
+    void removeAggregateAllTopic(Client* client);
+    bool isActive(void);
+
+    void printAggregateTopicTable(void);
+    bool testMessageIdTable(void);
+
+private:
+    uint16_t msgId(void);
+    Gateway* _gateway { nullptr };
+    MessageIdTable _msgIdTable;
+    AggregateTopicTable _topicTable;
+
+    bool _isActive { false };
+    bool _isSecure { false };
+};
+
+}
+
+#endif /* MQTTSNGATEWAY_SRC_MQTTSNGWAGGREGATER_H_ */
--- a/MQTTSNGateway/src/MQTTSNGWBrokerRecvTask.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWBrokerRecvTask.cpp
@@ -16,6 +16,8 @@

 #include "MQTTSNGWBrokerRecvTask.h"
 #include "MQTTSNGWClient.h"
+#include "MQTTSNGWClientList.h"
+#include "MQTTSNGateway.h"
 #include <unistd.h>

 using namespace std;
@@ -28,14 +30,14 @@ char* currentDateTime(void);
 =====================================*/
 BrokerRecvTask::BrokerRecvTask(Gateway* gateway)
 {
-	_gateway = gateway;
-	_gateway->attach((Thread*)this);
-	_light = 0;
+    _gateway = gateway;
+    _gateway->attach((Thread*) this);
+    _light = nullptr;
+    setTaskName("BrokerRecvTask");
 }

 BrokerRecvTask::~BrokerRecvTask()
 {
-
 }

 /**
@@ -43,7 +45,7 @@ BrokerRecvTask::~BrokerRecvTask()
 */
 void BrokerRecvTask::initialize(int argc, char** argv)
 {
-	_light = _gateway->getLightIndicator();
+    _light = _gateway->getLightIndicator();
 }

 /**
@@ -51,133 +53,130 @@ void BrokerRecvTask::initialize(int argc, char** argv)
 */
 void BrokerRecvTask::run(void)
 {
-	struct timeval timeout;
-	MQTTGWPacket* packet = 0;
-	int rc;
-	Event* ev = 0;
-	fd_set rset;
-	fd_set wset;
+    struct timeval timeout;
+    MQTTGWPacket* packet = nullptr;
+    int rc;
+    Event* ev = nullptr;
+    fd_set rset;
+    fd_set wset;

-	while (true)
-	{
-		_light->blueLight(false);
-		if (CHK_SIGINT)
-		{
-			WRITELOG("%s BrokerRecvTask   stopped.\n", currentDateTime());
-			return;
-		}
-		timeout.tv_sec = 0;
-		timeout.tv_usec = 500000;    // 500 msec
-		FD_ZERO(&rset);
-		FD_ZERO(&wset);
-		int maxSock = 0;
-		int sockfd = 0;
+    while (true)
+    {
+        _light->blueLight(false);
+        if (CHK_SIGINT)
+        {
+            WRITELOG("%s %s stopped.\n", currentDateTime(), getTaskName());
+            return;
+        }
+        timeout.tv_sec = 0;
+        timeout.tv_usec = 500000;    // 500 msec
+        FD_ZERO(&rset);
+        FD_ZERO(&wset);
+        int maxSock = 0;
+        int sockfd = 0;

-		/* Prepare sockets list to read */
-		Client* client = _gateway->getClientList()->getClient();
+        /* Prepare sockets list to read */
+        Client* client = _gateway->getClientList()->getClient(0);

-		while (client > 0)
-		{
-			if (client->getNetwork()->isValid())
-			{
-				sockfd = client->getNetwork()->getSock();
-				FD_SET(sockfd, &rset);
-				FD_SET(sockfd, &wset);
-				if (sockfd > maxSock)
-				{
-					maxSock = sockfd;
-				}
-			}
-			client = client->getNextClient();
-		}
+        while (client)
+        {
+            if (client->getNetwork()->isValid())
+            {
+                sockfd = client->getNetwork()->getSock();
+                FD_SET(sockfd, &rset);
+                FD_SET(sockfd, &wset);
+                if (sockfd > maxSock)
+                {
+                    maxSock = sockfd;
+                }
+            }
+            client = client->getNextClient();
+        }

-		if (maxSock == 0)
-		{
-			usleep(500 * 1000);
-		}
-		else
-		{
-			/* Check sockets is ready to read */
-			int activity = select(maxSock + 1, &rset, 0, 0, &timeout);
-			if (activity > 0)
-			{
-				client = _gateway->getClientList()->getClient();
+        if (maxSock == 0)
+        {
+            usleep(500 * 1000);
+        }
+        else
+        {
+            /* Check sockets is ready to read */
+            int activity = select(maxSock + 1, &rset, 0, 0, &timeout);

-				while (client > 0)
-				{
-					_light->blueLight(false);
-					if (client->getNetwork()->isValid())
-					{
-						int sockfd = client->getNetwork()->getSock();
-						if (FD_ISSET(sockfd, &rset))
-						{
-							packet = new MQTTGWPacket();
-							rc = 0;
-							/* read sockets */
-							_light->blueLight(true);
-							rc = packet->recv(client->getNetwork());
-							if ( rc > 0 )
-							{
-								if ( log(client, packet) == -1 )
-								{
-									delete packet;
-									goto nextClient;
-								}
+            if (activity > 0)
+            {
+                client = _gateway->getClientList()->getClient(0);

-								/* post a BrokerRecvEvent */
-								ev = new Event();
-								ev->setBrokerRecvEvent(client, packet);
-								_gateway->getPacketEventQue()->post(ev);
-							}
-							else
-							{
-								if ( rc == 0 )  // Disconnected
-								{
-									client->getNetwork()->close();
-									delete packet;
+                while (client)
+                {
+                    _light->blueLight(false);
+                    if (client->getNetwork()->isValid())
+                    {
+                        int sockfd = client->getNetwork()->getSock();
+                        if (FD_ISSET(sockfd, &rset))
+                        {
+                            packet = new MQTTGWPacket();
+                            rc = 0;
+                            /* read sockets */
+                            _light->blueLight(true);
+                            rc = packet->recv(client->getNetwork());
+                            if (rc > 0)
+                            {
+                                if (log(client, packet) == -1)
+                                {
+                                    delete packet;
+                                    goto nextClient;
+                                }

-									/* delete client when the client is not authorized & session is clean */
-									_gateway->getClientList()->erase(client);
+                                /* post a BrokerRecvEvent */
+                                ev = new Event();
+                                ev->setBrokerRecvEvent(client, packet);
+                                _gateway->getPacketEventQue()->post(ev);
+                            }
+                            else
+                            {
+                                if (rc == 0)  // Disconnected
+                                {
+                                    WRITELOG("%s BrokerRecvTask %s is disconnected by the broker.%s\n",
+                                    ERRMSG_HEADER, client->getClientId(),
+                                    ERRMSG_FOOTER);
+                                    client->getNetwork()->close();
+                                    client->disconnected();
+                                }
+                                else if (rc == -1)
+                                {
+                                    WRITELOG("%s BrokerRecvTask can't receive a packet from the broker errno=%d %s%s\n",
+                                    ERRMSG_HEADER, errno, client->getClientId(),
+                                    ERRMSG_FOOTER);
+                                }
+                                else if (rc == -2)
+                                {
+                                    WRITELOG(
+                                            "%s BrokerRecvTask receive invalid length of packet from the broker.  DISCONNECT  %s %s\n",
+                                            ERRMSG_HEADER, client->getClientId(),
+                                            ERRMSG_FOOTER);
+                                }
+                                else if (rc == -3)
+                                {
+                                    WRITELOG("%s BrokerRecvTask can't allocate memories for the packet %s%s\n",
+                                    ERRMSG_HEADER, client->getClientId(),
+                                    ERRMSG_FOOTER);
+                                }

-									if ( client )
-									{
-										client = client->getNextClient();
-									}
-									continue;
-								}
-								else if (rc == -1)
-								{
-									WRITELOG("%s BrokerRecvTask can't receive a packet from the broker errno=%d %s%s\n", ERRMSG_HEADER, errno, client->getClientId(), ERRMSG_FOOTER);
-								}
-								else if ( rc == -2 )
-								{
-									WRITELOG("%s BrokerRecvTask receive invalid length of packet from the broker.  DISCONNECT  %s %s\n", ERRMSG_HEADER, client->getClientId(),ERRMSG_FOOTER);
-								}
-								else if ( rc == -3 )
-								{
-									WRITELOG("%s BrokerRecvTask can't get memories for the packet %s%s\n", ERRMSG_HEADER, client->getClientId(), ERRMSG_FOOTER);
-								}
+                                delete packet;

-								delete packet;
-
-								if ( (rc == -1 || rc == -2) && client->isActive() )
-								{
-									/* disconnect the client */
-									packet = new MQTTGWPacket();
-									packet->setHeader(DISCONNECT);
-									ev = new Event();
-									ev->setBrokerRecvEvent(client, packet);
-									_gateway->getPacketEventQue()->post(ev);
-								}
-							}
-						}
-					}
-					nextClient:
-					client = client->getNextClient();
-				}
-			}
-		}
-	}
+                                if ((rc == -1 || rc == -2) && (client->isActive() || client->isSleep() || client->isAwake()))
+                                {
+                                    client->getNetwork()->close();
+                                    client->disconnected();
+                                }
+                            }
+                        }
+                    }
+                    nextClient: client = client->getNextClient();
+                }
+            }
+        }
+    }
 }

 /**
@@ -185,35 +184,38 @@ void BrokerRecvTask::run(void)
 */
 int BrokerRecvTask::log(Client* client, MQTTGWPacket* packet)
 {
-	char pbuf[(SIZE_OF_LOG_PACKET + 5 )* 3];
-	char msgId[6];
-	int rc = 0;
+    char pbuf[(SIZE_OF_LOG_PACKET + 5) * 3];
+    char msgId[6];
+    int rc = 0;

-	switch (packet->getType())
-	{
-	case CONNACK:
-		WRITELOG(FORMAT_Y_Y_W, currentDateTime(), packet->getName(), LEFTARROW, client->getClientId(), packet->print(pbuf));
-		break;
-	case PUBLISH:
-		WRITELOG(FORMAT_W_MSGID_Y_W_NL, currentDateTime(), packet->getName(), packet->getMsgId(msgId), LEFTARROW, client->getClientId(), packet->print(pbuf));
-		break;
-	case PUBACK:
-	case PUBREC:
-	case PUBREL:
-	case PUBCOMP:
-		WRITELOG(FORMAT_W_MSGID_Y_W, currentDateTime(), packet->getName(), packet->getMsgId(msgId), LEFTARROW, client->getClientId(), packet->print(pbuf));
-		break;
-	case SUBACK:
-	case UNSUBACK:
-		WRITELOG(FORMAT_W_MSGID_Y_W, currentDateTime(), packet->getName(), packet->getMsgId(msgId), LEFTARROW, client->getClientId(), packet->print(pbuf));
-		break;
-	case PINGRESP:
-		WRITELOG(FORMAT_Y_Y_W, currentDateTime(), packet->getName(), LEFTARROW, client->getClientId(), packet->print(pbuf));
-		break;
-	default:
-		WRITELOG("Type=%x\n", packet->getType());
-		rc = -1;
-		break;
-	}
-	return rc;
+    switch (packet->getType())
+    {
+    case CONNACK:
+        WRITELOG(FORMAT_Y_Y_W, currentDateTime(), packet->getName(), LEFTARROWB, client->getClientId(), packet->print(pbuf));
+        break;
+    case PUBLISH:
+        WRITELOG(FORMAT_W_MSGID_Y_W_NL, currentDateTime(), packet->getName(), packet->getMsgId(msgId), LEFTARROWB,
+                client->getClientId(), packet->print(pbuf));
+        break;
+    case PUBACK:
+    case PUBREC:
+    case PUBREL:
+    case PUBCOMP:
+        WRITELOG(FORMAT_W_MSGID_Y_W, currentDateTime(), packet->getName(), packet->getMsgId(msgId), LEFTARROWB,
+                client->getClientId(), packet->print(pbuf));
+        break;
+    case SUBACK:
+    case UNSUBACK:
+        WRITELOG(FORMAT_W_MSGID_Y_W, currentDateTime(), packet->getName(), packet->getMsgId(msgId), LEFTARROWB,
+                client->getClientId(), packet->print(pbuf));
+        break;
+    case PINGRESP:
+        WRITELOG(FORMAT_Y_Y_W, currentDateTime(), packet->getName(), LEFTARROWB, client->getClientId(), packet->print(pbuf));
+        break;
+    default:
+        WRITELOG("Type=%x\n", packet->getType());
+        rc = -1;
+        break;
+    }
+    return rc;
 }
--- a/MQTTSNGateway/src/MQTTSNGWBrokerRecvTask.h
+++ b/MQTTSNGateway/src/MQTTSNGWBrokerRecvTask.h
@@ -28,21 +28,20 @@ namespace MQTTSNGW
 class BrokerRecvTask: public Thread
 {
 MAGIC_WORD_FOR_THREAD;
-	;
+
 public:
-	BrokerRecvTask(Gateway* gateway);
-	~BrokerRecvTask();
-	void initialize(int argc, char** argv);
-	void run(void);
+    BrokerRecvTask(Gateway* gateway);
+    ~BrokerRecvTask();
+    void initialize(int argc, char** argv);
+    void run(void);

 private:
-	int log(Client*, MQTTGWPacket*);
+    int log(Client*, MQTTGWPacket*);

-	Gateway* _gateway;
-	LightIndicator* _light;
+    Gateway* _gateway;
+    LightIndicator* _light;
 };

 }

-
 #endif /* MQTTSNGWBROKERRECVTASK_H_ */
--- a/MQTTSNGateway/src/MQTTSNGWBrokerSendTask.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWBrokerSendTask.cpp
@@ -14,6 +14,7 @@
 *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
 **************************************************************************************/

+#include <MQTTSNGWAdapterManager.h>
 #include "MQTTSNGWBrokerSendTask.h"
 #include "MQTTSNGWDefines.h"
 #include "MQTTSNGateway.h"
@@ -32,15 +33,15 @@ char* currentDateTime();
 =====================================*/
 BrokerSendTask::BrokerSendTask(Gateway* gateway)
 {
-	_gateway = gateway;
-	_gateway->attach((Thread*)this);
-	_gwparams = 0;
-	_light = 0;
+    _gateway = gateway;
+    _gateway->attach((Thread*) this);
+    _gwparams = nullptr;
+    _light = nullptr;
+    setTaskName("BrokerSendTask");
 }

 BrokerSendTask::~BrokerSendTask()
 {
-
 }

 /**
@@ -48,8 +49,8 @@ BrokerSendTask::~BrokerSendTask()
 */
 void BrokerSendTask::initialize(int argc, char** argv)
 {
-	_gwparams = _gateway->getGWParams();
-	_light = _gateway->getLightIndicator();
+    _gwparams = _gateway->getGWParams();
+    _light = _gateway->getLightIndicator();
 }

 /**
@@ -57,120 +58,137 @@ void BrokerSendTask::initialize(int argc, char** argv)
 */
 void BrokerSendTask::run()
 {
-	Event* ev = 0;
-	MQTTGWPacket* packet = 0;
-	Client* client = 0;
-	int rc = 0;
+    Event* ev = nullptr;
+    MQTTGWPacket* packet = nullptr;
+    Client* client = nullptr;
+    AdapterManager* adpMgr = _gateway->getAdapterManager();
+    int rc = 0;

-	while (true)
-	{
-		ev = _gateway->getBrokerSendQue()->wait();
+    while (true)
+    {
+        ev = _gateway->getBrokerSendQue()->wait();

-		if ( ev->getEventType() == EtStop )
-		{
-			WRITELOG("%s BrokerSendTask   stopped.\n", currentDateTime());
-			delete ev;
-			return;
-		}
+        if (ev->getEventType() == EtStop)
+        {
+            WRITELOG("%s %s stopped.\n", currentDateTime(), getTaskName());
+            delete ev;
+            return;
+        }

-		if ( ev->getEventType() == EtBrokerSend)
-		{
-			client = ev->getClient();
-			packet = ev->getMQTTGWPacket();
+        if (ev->getEventType() == EtBrokerSend)
+        {
+            client = ev->getClient();
+            packet = ev->getMQTTGWPacket();

-			if ( packet->getType() == CONNECT && client->getNetwork()->isValid() )
-			{
-				client->getNetwork()->close();
-			}
+            /* Check Client is managed by Adapters */
+            client = adpMgr->getClient(client);

-			if ( !client->getNetwork()->isValid() )
-			{
-				/* connect to the broker and send a packet */
+            if (packet->getType() == CONNECT && client->getNetwork()->isValid())
+            {
+                client->getNetwork()->close();
+            }

-				if (client->isSecureNetwork())
-				{
-					rc = client->getNetwork()->connect(_gwparams->brokerName, _gwparams->portSecure, _gwparams->rootCApath,
-							  _gwparams->rootCAfile, _gwparams->certKey, _gwparams->privateKey);
-				}
-				else
-				{
-					rc = client->getNetwork()->connect(_gwparams->brokerName, _gwparams->port);
-				}
+            if (!client->getNetwork()->isValid())
+            {
+                /* connect to the broker and send a packet */

-				if ( !rc )
-				{
-					/* disconnect the broker and the client */
-					WRITELOG("%s BrokerSendTask can't connect to the broker.  %s%s\n",
-							ERRMSG_HEADER, client->getClientId(), ERRMSG_FOOTER);
-					delete ev;
-					client->getNetwork()->close();
-					continue;
-				}
-			}
+                if (client->isSecureNetwork())
+                {
+                    rc = client->getNetwork()->connect((const char*) _gwparams->brokerName, (const char*) _gwparams->portSecure,
+                            (const char*) _gwparams->rootCApath, (const char*) _gwparams->rootCAfile,
+                            (const char*) _gwparams->certKey, (const char*) _gwparams->privateKey);
+                }
+                else
+                {
+                    rc = client->getNetwork()->connect((const char*) _gwparams->brokerName, (const char*) _gwparams->port);
+                }

-			/* send a packet */
-			_light->blueLight(true);
-			if ( (rc = packet->send(client->getNetwork())) > 0 )
-			{
-				if ( packet->getType() == CONNECT )
-				{
-					client->connectSended();
-				}
-				log(client, packet);
-			}
-			else
-			{
-				WRITELOG("%s BrokerSendTask can't send a packet to the broker errno=%d %s%s\n",
-						ERRMSG_HEADER, rc == -1 ? errno : 0, client->getClientId(), ERRMSG_FOOTER);
-				client->getNetwork()->close();
+                if (!rc)
+                {
+                    /* disconnect the broker and the client */
+                    WRITELOG("%s BrokerSendTask: %s can't connect to the broker. errno=%d %s %s\n",
+                    ERRMSG_HEADER, client->getClientId(), errno, strerror(errno), ERRMSG_FOOTER);
+                    delete ev;
+                    client->getNetwork()->close();
+                    continue;
+                }
+            }

-				/* Disconnect the client */
-				packet = new MQTTGWPacket();
-				packet->setHeader(DISCONNECT);
-				Event* ev1 = new Event();
-				ev1->setBrokerRecvEvent(client, packet);
-				_gateway->getPacketEventQue()->post(ev1);
-			}
+            /* send a packet */
+            _light->blueLight(true);
+            if ((rc = packet->send(client->getNetwork())) > 0)
+            {
+                if (packet->getType() == CONNECT)
+                {
+                    client->connectSended();
+                }
+                else if (packet->getType() == DISCONNECT)
+                {
+                    client->getNetwork()->close();
+                    client->disconnected();
+                }
+                log(client, packet);
+            }
+            else
+            {
+                WRITELOG("%s BrokerSendTask: %s can't send a packet to the broker. errno=%d %s %s\n",
+                ERRMSG_HEADER, client->getClientId(), rc == -1 ? errno : 0, strerror(errno), ERRMSG_FOOTER);
+                if ( errno != EBADF)
+                {
+                    client->getNetwork()->close();
+                }

-			_light->blueLight(false);
-		}
-		delete ev;
-	}
+                /* Disconnect the client */
+                packet = new MQTTGWPacket();
+                packet->setHeader(DISCONNECT);
+                Event* ev1 = new Event();
+                ev1->setBrokerRecvEvent(client, packet);
+                _gateway->getPacketEventQue()->post(ev1);
+            }
+
+            _light->blueLight(false);
+        }
+        delete ev;
+    }
 }

-
 /**
 *  write message content into stdout or Ringbuffer
 */
 void BrokerSendTask::log(Client* client, MQTTGWPacket* packet)
 {
-	char pbuf[(SIZE_OF_LOG_PACKET + 5 )* 3];
-	char msgId[6];
+    char pbuf[(SIZE_OF_LOG_PACKET + 5) * 3];
+    char msgId[6];

-	switch (packet->getType())
-	{
-	case CONNECT:
-		WRITELOG(FORMAT_Y_Y_W, currentDateTime(), packet->getName(), RIGHTARROW, client->getClientId(), packet->print(pbuf));
-		break;
-	case PUBLISH:
-		WRITELOG(FORMAT_W_MSGID_Y_W, currentDateTime(), packet->getName(), packet->getMsgId(msgId), RIGHTARROW, client->getClientId(), packet->print(pbuf));
-		break;
-	case SUBSCRIBE:
-	case UNSUBSCRIBE:
-	case PUBACK:
-	case PUBREC:
-	case PUBREL:
-	case PUBCOMP:
-		WRITELOG(FORMAT_W_MSGID_Y_W, currentDateTime(), packet->getName(), packet->getMsgId(msgId), RIGHTARROW, client->getClientId(), packet->print(pbuf));
-		break;
-	case PINGREQ:
-		WRITELOG(FORMAT_Y_Y_W, currentDateTime(), packet->getName(), RIGHTARROW, client->getClientId(), packet->print(pbuf));
-		break;
-	case DISCONNECT:
-		WRITELOG(FORMAT_Y_Y_W, currentDateTime(), packet->getName(), RIGHTARROW, client->getClientId(), packet->print(pbuf));
-		break;
-	default:
-		break;
-	}
+    switch (packet->getType())
+    {
+    case CONNECT:
+        WRITELOG(FORMAT_Y_Y_W, currentDateTime(), packet->getName(),
+        RIGHTARROWB, client->getClientId(), packet->print(pbuf));
+        break;
+    case PUBLISH:
+        WRITELOG(FORMAT_W_MSGID_Y_W, currentDateTime(), packet->getName(), packet->getMsgId(msgId), RIGHTARROWB,
+                client->getClientId(), packet->print(pbuf));
+        break;
+    case SUBSCRIBE:
+    case UNSUBSCRIBE:
+    case PUBACK:
+    case PUBREC:
+    case PUBREL:
+    case PUBCOMP:
+        WRITELOG(FORMAT_W_MSGID_Y_W, currentDateTime(), packet->getName(), packet->getMsgId(msgId), RIGHTARROWB,
+                client->getClientId(), packet->print(pbuf));
+        break;
+    case PINGREQ:
+        WRITELOG(FORMAT_Y_Y_W, currentDateTime(), packet->getName(),
+        RIGHTARROWB, client->getClientId(), packet->print(pbuf));
+        break;
+    case DISCONNECT:
+        WRITELOG(FORMAT_Y_Y_W, currentDateTime(), packet->getName(),
+        RIGHTARROWB, client->getClientId(), packet->print(pbuf));
+        break;
+    default:
+        break;
+    }
 }

--- a/MQTTSNGateway/src/MQTTSNGWBrokerSendTask.h
+++ b/MQTTSNGateway/src/MQTTSNGWBrokerSendTask.h
@@ -22,22 +22,25 @@

 namespace MQTTSNGW
 {
+class Adapter;
+
 /*=====================================
-     Class BrokerSendTask
+ Class BrokerSendTask
 =====================================*/
-class BrokerSendTask : public Thread
+class BrokerSendTask: public Thread
 {
-	MAGIC_WORD_FOR_THREAD;
+MAGIC_WORD_FOR_THREAD;
+    friend AdapterManager;
 public:
-	BrokerSendTask(Gateway* gateway);
-	~BrokerSendTask();
-	void initialize(int argc, char** argv);
-	void run();
+    BrokerSendTask(Gateway* gateway);
+    ~BrokerSendTask();
+    void initialize(int argc, char** argv);
+    void run();
 private:
-	void log(Client*, MQTTGWPacket*);
-	Gateway* _gateway;
-	GatewayParams* _gwparams;
-	LightIndicator* _light;
+    void log(Client*, MQTTGWPacket*);
+    Gateway* _gateway;
+    GatewayParams* _gwparams;
+    LightIndicator* _light;
 };

 }
--- a/MQTTSNGateway/src/MQTTSNGWClient.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWClient.cpp
--- a/MQTTSNGateway/src/MQTTSNGWClient.h
+++ b/MQTTSNGateway/src/MQTTSNGWClient.h
@@ -12,6 +12,7 @@
 *
 * Contributors:
 *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
+ *    Tieto Poland Sp. z o.o. - Gateway improvements
 **************************************************************************************/

 #ifndef MQTTSNGWCLIENT_H_
@@ -25,6 +26,11 @@
 #include "Network.h"
 #include "SensorNetwork.h"
 #include "MQTTSNPacket.h"
+#include "MQTTSNGWEncapsulatedPacket.h"
+#include "MQTTSNGWForwarder.h"
+#include "MQTTSNGWTopic.h"
+#include "MQTTSNGWClientList.h"
+#include "MQTTSNGWAdapter.h"

 namespace MQTTSNGW
 {
@@ -38,142 +44,71 @@ namespace MQTTSNGW
 template<class T> class PacketQue
 {
 public:
-	PacketQue()
-	{
-		_que = new Que<T>;
-	}
+    PacketQue()
+    {
+        _que = new Que<T>;
+    }

-	~PacketQue()
-	{
-		clear();
-		delete _que;
-	}
+    ~PacketQue()
+    {
+        clear();
+        delete _que;
+    }

-	T* getPacket()
-	{
-		T* packet;
-		if (_que->size() > 0)
-		{
-			_mutex.lock();
-			packet = _que->front();
-			_mutex.unlock();
-			return packet;
-		}
-		else
-		{
-			return 0;
-		}
-	}
+    T* getPacket()
+    {
+        T* packet;
+        if (_que->size() > 0)
+        {
+            _mutex.lock();
+            packet = _que->front();
+            _mutex.unlock();
+            return packet;
+        }
+        else
+        {
+            return 0;
+        }
+    }

-	void post(T* packet)
-	{
-		_mutex.lock();
-		_que->post(packet);
-		_mutex.unlock();
-	}
+    int post(T* packet)
+    {
+        int rc;
+        _mutex.lock();
+        rc = _que->post(packet);
+        _mutex.unlock();
+        return rc;
+    }

-	void pop()
-	{
-		if (_que->size() > 0)
-		{
-			_mutex.lock();
-			_que->pop();
-			_mutex.unlock();
-		}
-	}
+    void pop()
+    {
+        if (_que->size() > 0)
+        {
+            _mutex.lock();
+            _que->pop();
+            _mutex.unlock();
+        }
+    }

-	void clear()
-	{
-		_mutex.lock();
-		while (_que->size() > 0)
-		{
-			delete _que->front();
-			_que->pop();
-		}
-		_mutex.unlock();
-	}
+    void clear()
+    {
+        _mutex.lock();
+        while (_que->size() > 0)
+        {
+            delete _que->front();
+            _que->pop();
+        }
+        _mutex.unlock();
+    }
+
+    void setMaxSize(int size)
+    {
+        _que->setMaxSize(size);
+    }

 private:
-	Que<T>* _que;
-	Mutex _mutex;
-};
-
-
-/*=====================================
- Class Topic
- ======================================*/
-class Topic
-{
-	friend class Topics;
-public:
-	Topic();
-	Topic(string* topic);
-	~Topic();
-	string* getTopicName(void);
-	uint16_t getTopicId(void);
-	bool isMatch(string* topicName);
-
-private:
-	uint16_t _topicId;
-	string*  _topicName;
-	Topic* _next;
-};
-
-/*=====================================
- Class Topics
- ======================================*/
-class Topics
-{
-public:
-	Topics();
-	~Topics();
-	Topic* add(const MQTTSN_topicid* topicid);
-	Topic* add(const string* topic);
-	uint16_t getTopicId(const MQTTSN_topicid* topic);
-	uint16_t getNextTopicId();
-	Topic* getTopic(uint16_t topicId);
-	Topic* getTopic(const MQTTSN_topicid* topicid);
-	Topic* match(const MQTTSN_topicid* topicid);
-
-private:
-	uint16_t _nextTopicId;
-	Topic* _first;
-
-};
-
-/*=====================================
- Class TopicIdMap
- =====================================*/
-class TopicIdMapelement
-{
-	friend class TopicIdMap;
-public:
-	TopicIdMapelement(uint16_t msgId, uint16_t topicId, MQTTSN_topicTypes type);
-	~TopicIdMapelement();
-
-private:
-	uint16_t _msgId;
-	uint16_t _topicId;
-	MQTTSN_topicTypes _type;
-	TopicIdMapelement* _next;
-	TopicIdMapelement* _prev;
-};
-
-class TopicIdMap
-{
-public:
-	TopicIdMap();
-	~TopicIdMap();
-	uint16_t getTopicId(uint16_t msgId, MQTTSN_topicTypes* type);
-	int add(uint16_t msgId, uint16_t topicId, MQTTSN_topicTypes type);
-	void erase(uint16_t msgId);
-	void clear(void);
-private:
-	uint16_t* _msgIds;
-	TopicIdMapelement* _first;
-	TopicIdMapelement* _end;
-	int _cnt;
-	int _maxInflight;
+    Que<T>* _que;
+    Mutex _mutex;
 };

 /*=====================================
@@ -181,16 +116,16 @@ private:
 =====================================*/
 class waitREGACKPacket
 {
-	friend class WaitREGACKPacketList;
+    friend class WaitREGACKPacketList;
 public:
-	waitREGACKPacket(MQTTSNPacket* packet, uint16_t REGACKMsgId);
-	~waitREGACKPacket();
+    waitREGACKPacket(MQTTSNPacket* packet, uint16_t REGACKMsgId);
+    ~waitREGACKPacket();

 private:
-	uint16_t _msgId;
-	MQTTSNPacket* _packet;
-	waitREGACKPacket* _next;
-	waitREGACKPacket* _prev;
+    uint16_t _msgId;
+    MQTTSNPacket* _packet;
+    waitREGACKPacket* _next;
+    waitREGACKPacket* _prev;
 };

 /*=====================================
@@ -199,151 +134,176 @@ private:
 class WaitREGACKPacketList
 {
 public:
-	WaitREGACKPacketList();
-	~WaitREGACKPacketList();
-	int setPacket(MQTTSNPacket* packet, uint16_t REGACKMsgId);
-	MQTTSNPacket* getPacket(uint16_t REGACKMsgId);
-	void erase(uint16_t REGACKMsgId);
+    WaitREGACKPacketList();
+    ~WaitREGACKPacketList();
+    int setPacket(MQTTSNPacket* packet, uint16_t REGACKMsgId);
+    MQTTSNPacket* getPacket(uint16_t REGACKMsgId);
+    void erase(uint16_t REGACKMsgId);
+    uint8_t getCount(void);

 private:
-	waitREGACKPacket* _first;
-	waitREGACKPacket* _end;
+    uint8_t _cnt;
+    waitREGACKPacket* _first;
+    waitREGACKPacket* _end;
 };

 /*=====================================
 Class Client
 =====================================*/
-#define MQTTSN_CLIENTID_LENGTH 23
+typedef enum
+{
+	Cstat_Free = 0,
+	Cstat_Disconnected,
+    Cstat_TryConnecting,
+    Cstat_Connecting,
+    Cstat_Active,
+    Cstat_Asleep,
+    Cstat_Awake,
+    Cstat_Lost
+} ClientStatus;

 typedef enum
 {
-	Cstat_Disconnected = 0, Cstat_TryConnecting, Cstat_Connecting, Cstat_Active, Cstat_Asleep, Cstat_Awake, Cstat_Lost
-} ClientStatus;
+    Ctype_Normal = 0,
+    Ctype_Forwarded,
+    Ctype_QoS_1,
+    Ctype_Aggregated,
+    Ctype_Proxy,
+    Ctype_Aggregater
+} ClientType;

+class Forwarder;

 class Client
 {
-	friend class ClientList;
+    friend class ClientList;
+    friend class ClientsPool;
 public:
-	Client(bool secure = false);
-	Client(uint8_t maxInflightMessages, bool secure);
-	~Client();
+    Client();
+    ~Client();

-	Connect* getConnectData(void);
-	uint16_t getWaitedPubTopicId(uint16_t msgId);
-	uint16_t getWaitedSubTopicId(uint16_t msgId);
-	MQTTSNPacket* getClientSleepPacket();
-	WaitREGACKPacketList* getWaitREGACKPacketList(void);
+    Connect* getConnectData(void);
+    TopicIdMapElement* getWaitedPubTopicId(uint16_t msgId);
+    TopicIdMapElement* getWaitedSubTopicId(uint16_t msgId);
+    MQTTGWPacket* getClientSleepPacket(void);
+    void deleteFirstClientSleepPacket(void);

-	void eraseWaitedPubTopicId(uint16_t msgId);
-	void eraseWaitedSubTopicId(uint16_t msgId);
-	void clearWaitedPubTopicId(void);
-	void clearWaitedSubTopicId(void);
+    MQTTSNPacket* getProxyPacket(void);
+    void deleteFirstProxyPacket(void);
+    WaitREGACKPacketList* getWaitREGACKPacketList(void);

-	void setClientSleepPacket(MQTTSNPacket*);
-	void setWaitedPubTopicId(uint16_t msgId, uint16_t topicId, MQTTSN_topicTypes type);
-	void setWaitedSubTopicId(uint16_t msgId, uint16_t topicId, MQTTSN_topicTypes type);
+    void eraseWaitedPubTopicId(uint16_t msgId);
+    void eraseWaitedSubTopicId(uint16_t msgId);
+    void clearWaitedPubTopicId(void);
+    void clearWaitedSubTopicId(void);

-	bool checkTimeover(void);
-	void updateStatus(MQTTSNPacket*);
-	void updateStatus(ClientStatus);
-	void connectSended(void);
-	void connackSended(int rc);
-	void disconnected(void);
-	bool isConnectSendable(void);
+    int setClientSleepPacket(MQTTGWPacket*);
+    int setProxyPacket(MQTTSNPacket* packet);
+    void setWaitedPubTopicId(uint16_t msgId, uint16_t topicId, MQTTSN_topicid* topic);
+    void setWaitedSubTopicId(uint16_t msgId, uint16_t topicId, MQTTSN_topicid* topic);

-	uint16_t getNextPacketId(void);
-	uint8_t getNextSnMsgId(void);
-	Topics* getTopics(void);
-	void setTopics(Topics* topics);
-	void setKeepAlive(MQTTSNPacket* packet);
+    bool checkTimeover(void);
+    void updateStatus(MQTTSNPacket*);
+    void updateStatus(ClientStatus);
+    void connectSended(void);
+    void connackSended(int rc);
+    void disconnected(void);
+    bool isConnectSendable(void);
+    void tryConnect(void);
+    ClientStatus getClientStatus(void);

-	SensorNetAddress* getSensorNetAddress(void);
-	Network* getNetwork(void);
-	void setClientAddress(SensorNetAddress* sensorNetAddr);
-	void setSensorNetType(bool stable);
+    uint16_t getNextPacketId(void);
+    uint8_t getNextSnMsgId(void);
+    Topics* getTopics(void);
+    void setTopics(Topics* topics);
+    void setKeepAlive(MQTTSNPacket* packet);

-	void setClientId(MQTTSNString id);
-	void setWillTopic(MQTTSNString willTopic);
-	void setWillMsg(MQTTSNString willmsg);
-	char* getClientId(void);
-	char* getWillTopic(void);
-	char* getWillMsg(void);
-	const char* getStatus(void);
-	void setWaitWillMsgFlg(bool);
-	void setSessionStatus(bool);  // true: clean session
-	bool erasable(void);
+    SensorNetAddress* getSensorNetAddress(void);
+    Network* getNetwork(void);
+    void setClientAddress(SensorNetAddress* sensorNetAddr);
+    void setSensorNetType(bool stable);

-	bool isDisconnect(void);
-	bool isActive(void);
-	bool isSleep(void);
-	bool isSecureNetwork(void);
-	bool isSensorNetStable(void);
-	bool isWaitWillMsg(void);
+    Forwarder* getForwarder(void);
+    void setForwarder(Forwarder* forwader);

-	Client* getNextClient(void);
-	Client* getOTAClient(void);
-	void    setOTAClient(Client* cl);
+    void setAdapterType(AdapterType type);
+    void setQoSm1(void);
+    void setAggregated(void);
+    bool isQoSm1Proxy(void);
+    bool isForwarded(void);
+    bool isAggregated(void);
+    bool isAggregater(void);
+    bool isQoSm1(void);
+    bool isAdapter(void);
+
+    void setClientId(MQTTSNString id);
+    void setWillTopic(MQTTSNString willTopic);
+    void setWillMsg(MQTTSNString willmsg);
+    char* getClientId(void);
+    char* getWillTopic(void);
+    char* getWillMsg(void);
+    const char* getStatus(void);
+    void setWaitWillMsgFlg(bool);
+    void setSessionStatus(bool);  // true: clean session
+    bool erasable(void);
+
+    bool isDisconnect(void);
+    bool isConnecting(void);
+    bool isActive(void);
+    bool isSleep(void);
+    bool isAwake(void);
+    bool isSecureNetwork(void);
+    bool isSensorNetStable(void);
+    bool isWaitWillMsg(void);
+	bool isCleanSession(void);
+
+    void holdPingRequest(void);
+    void resetPingRequest(void);
+    bool isHoldPingReqest(void);
+
+    Client* getNextClient(void);

 private:
-	PacketQue<MQTTSNPacket> _clientSleepPacketQue;
-	WaitREGACKPacketList    _waitREGACKList;
+    PacketQue<MQTTGWPacket> _clientSleepPacketQue;
+    PacketQue<MQTTSNPacket> _proxyPacketQue;

-	Topics* _topics;
-	TopicIdMap _waitedPubTopicIdMap;
-	TopicIdMap _waitedSubTopicIdMap;
+    WaitREGACKPacketList _waitREGACKList;

-	Connect _connectData;
-	MQTTSNPacket* _connAck;
+    Topics* _topics;
+    TopicIdMap _waitedPubTopicIdMap;
+    TopicIdMap _waitedSubTopicIdMap;

-	char* _clientId;
-	char* _willTopic;
-	char* _willMsg;
+    Connect _connectData;
+    MQTTSNPacket* _connAck;

-	Timer _keepAliveTimer;
-	uint32_t _keepAliveMsec;
+    char* _clientId;
+    char* _willTopic;
+    char* _willMsg;

-	ClientStatus _status;
-	bool _waitWillMsgFlg;
+    bool _holdPingRequest;

-	uint16_t _packetId;
-	uint8_t _snMsgId;
+    Timer _keepAliveTimer;
+    uint32_t _keepAliveMsec;

-	Network* _network;      // Broker
-	bool  _secureNetwork;    // SSL
-	bool _sensorNetype;     // false: unstable network like a G3
-	SensorNetAddress _sensorNetAddr;
+    ClientStatus _status;
+    bool _waitWillMsgFlg;

-	bool _sessionStatus;
+    uint16_t _packetId;
+    uint8_t _snMsgId;

-	Client* _nextClient;
-	Client* _prevClient;
-	Client* _otaClient;
-};
+    Network* _network;      // Broker
+    bool _secureNetwork;    // SSL
+    bool _sensorNetype;     // false: unstable network like a G3
+    SensorNetAddress _sensorNetAddr;

-/*=====================================
- Class ClientList
- =====================================*/
-class ClientList
-{
-public:
-	ClientList();
-	~ClientList();
-	bool authorize(const char* fileName);
-	void erase(Client*);
-	Client* getClient(SensorNetAddress* addr);
-	Client* getClient(uint8_t* clientId);
-	Client* createClient(SensorNetAddress* addr, MQTTSNString* clientId, bool unstableLine,
-			bool secure);
-	uint16_t getClientCount(void);
-	Client* getClient(void);
-	bool isAuthorized();
-private:
-	Client* _firstClient;
-	Client* _endClient;
-	Mutex _mutex;
-	uint16_t _clientCnt;
-	bool _authorize;
+    Forwarder* _forwarder;
+    ClientType _clientType;
+
+    bool _sessionStatus;
+    bool _hasPredefTopic;
+
+    Client* _nextClient;
+    Client* _prevClient;
 };

 }
--- a/MQTTSNGateway/src/MQTTSNGWClientList.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWClientList.cpp
@@ -0,0 +1,517 @@
+/**************************************************************************************
+ * Copyright (c) 2016, 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
+ *    Tieto Poland Sp. z o.o. - Gateway improvements
+ **************************************************************************************/
+#include "MQTTSNGWClientList.h"
+#include "MQTTSNGateway.h"
+#include <string.h>
+#include <string>
+
+using namespace MQTTSNGW;
+char* currentDateTime(void);
+/*=====================================
+ Class ClientList
+ =====================================*/
+const char* common_topic = "*";
+
+ClientList::ClientList(Gateway* gw)
+{
+    _clientCnt = 0;
+    _authorize = false;
+    _firstClient = nullptr;
+    _endClient = nullptr;
+    _clientsPool = new ClientsPool();
+    _gateway = gw;
+}
+
+ClientList::~ClientList()
+{
+    _mutex.lock();
+    Client* cl = _firstClient;
+    Client* ncl;
+
+    while (cl != nullptr)
+    {
+        ncl = cl->_nextClient;
+        delete cl;
+        cl = ncl;
+    };
+
+    if (_clientsPool)
+    {
+        delete _clientsPool;
+    }
+    _mutex.unlock();
+}
+
+void ClientList::initialize(bool aggregate)
+{
+    _maxClients = _gateway->getGWParams()->maxClients;
+    _clientsPool->allocate(_gateway->getGWParams()->maxClients);
+
+    if (_gateway->getGWParams()->clientAuthentication)
+    {
+        int type = TRANSPEARENT_TYPE;
+        if (aggregate)
+        {
+            type = AGGREGATER_TYPE;
+        }
+        setClientList(type);
+        _authorize = true;
+    }
+
+    if (_gateway->getGWParams()->predefinedTopic)
+    {
+        setPredefinedTopics(aggregate);
+    }
+}
+
+void ClientList::setClientList(int type)
+{
+    if (!createList(_gateway->getGWParams()->clientListName, type))
+    {
+        throw EXCEPTION("ClientList::setClientList Client list not found!", 0);
+    }
+}
+
+void ClientList::setPredefinedTopics(bool aggrecate)
+{
+    if (!readPredefinedList(_gateway->getGWParams()->predefinedTopicFileName, aggrecate))
+    {
+        throw EXCEPTION("ClientList::setPredefinedTopics PredefindTopic list not found!", 0);
+
+    }
+}
+
+/**
+ * Create ClientList from a client list file.
+ * @param File name of the client list
+ * @return true: Reject client connection that is not registered in the client list
+ *
+ * File format is:
+ *     Lines bigning with # are comment line.
+ *     ClientId, SensorNetAddress, "unstableLine", "secureConnection"
+ *     in case of UDP, SensorNetAddress format is IPAddress:portNo.
+ *     if the SensorNetwork is not stable, write unstableLine.
+ *     if BrokerConnection is SSL, write secureConnection.
+ *     if the client send PUBLISH QoS-1, QoS-1 is required.
+ *
+ * Ex:
+ *     #Client List
+ *     ClientId1,192.168.10.10:11200
+ *     ClientID2,192.168.50.200:35000,unstableLine
+ *     ClientID3,192.168.200.50:40000,secureConnection
+ *     ClientID4,192.168.200.51:41000,unstableLine,secureConnection
+ *     ClientID5,192.168.200.51:41000,unstableLine,secureConnection,QoS-1
+ */
+
+bool ClientList::createList(const char* fileName, int type)
+{
+    FILE* fp;
+    char buf[MAX_CLIENTID_LENGTH + 256];
+    size_t pos;
+    bool secure;
+    bool stable;
+    bool qos_1;
+    bool forwarder;
+    bool rc = false;
+    SensorNetAddress netAddr;
+    MQTTSNString clientId = MQTTSNString_initializer;
+
+    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);
+            clientId.cstring = strdup(id.c_str());
+            string addr = data.substr(pos + 1);
+
+            if (netAddr.setAddress(&addr) == 0)
+            {
+                qos_1 = (data.find("QoS-1") != string::npos);
+                forwarder = (data.find("forwarder") != string::npos);
+                secure = (data.find("secureConnection") != string::npos);
+                stable = !(data.find("unstableLine") != string::npos);
+                if ((qos_1 && type == QOSM1PROXY_TYPE) || (!qos_1 && type == AGGREGATER_TYPE))
+                {
+                    createClient(&netAddr, &clientId, stable, secure, type);
+                }
+                else if (forwarder && type == FORWARDER_TYPE)
+                {
+                    _gateway->getAdapterManager()->getForwarderList()->addForwarder(&netAddr, &clientId);
+                }
+                else if (type == TRANSPEARENT_TYPE)
+                {
+                    createClient(&netAddr, &clientId, stable, secure, type);
+                }
+            }
+            else
+            {
+                WRITELOG("Invalid address     %s\n", data.c_str());
+                rc = false;
+            }
+            free(clientId.cstring);
+        }
+        fclose(fp);
+        rc = true;
+    }
+    return rc;
+}
+
+bool ClientList::readPredefinedList(const char* fileName, bool aggregate)
+{
+    FILE* fp;
+    char buf[MAX_CLIENTID_LENGTH + 256];
+    size_t pos0, pos1;
+    MQTTSNString clientId = MQTTSNString_initializer;
+    ;
+    bool rc = false;
+
+    if ((fp = fopen(fileName, "r")) != 0)
+    {
+        while (fgets(buf, MAX_CLIENTID_LENGTH + 254, fp) != 0)
+        {
+            if (*buf == '#')
+            {
+                continue;
+            }
+            string data = string(buf);
+            while ((pos0 = data.find_first_of(" 　\t\n")) != string::npos)
+            {
+                data.erase(pos0, 1);
+            }
+            if (data.empty())
+            {
+                continue;
+            }
+
+            pos0 = data.find_first_of(",");
+            pos1 = data.find(",", pos0 + 1);
+            string id = data.substr(0, pos0);
+            clientId.cstring = strdup(id.c_str());
+            string topicName = data.substr(pos0 + 1, pos1 - pos0 - 1);
+            uint16_t topicID = stoul(data.substr(pos1 + 1));
+            createPredefinedTopic(&clientId, topicName, topicID, aggregate);
+            free(clientId.cstring);
+        }
+        fclose(fp);
+        rc = true;
+    }
+    else
+    {
+        WRITELOG("ClientList can not open the Predefined Topic List.     %s\n", fileName);
+        return false;
+    }
+    return rc;
+}
+
+void ClientList::erase(Client*& client)
+{
+    if (!_authorize && client->erasable())
+    {
+        _mutex.lock();
+        Client* prev = client->_prevClient;
+        Client* next = client->_nextClient;
+
+        if (prev)
+        {
+            prev->_nextClient = next;
+        }
+        else
+        {
+            _firstClient = next;
+
+        }
+        if (next)
+        {
+            next->_prevClient = prev;
+        }
+        else
+        {
+            _endClient = prev;
+        }
+        _clientCnt--;
+        Forwarder* fwd = client->getForwarder();
+        if (fwd)
+        {
+            fwd->eraseClient(client);
+        }
+        delete client;
+        client = nullptr;
+        _mutex.unlock();
+    }
+}
+
+Client* ClientList::getClient(SensorNetAddress* addr)
+{
+    if (addr)
+    {
+        _mutex.lock();
+        Client* client = _firstClient;
+
+        while (client != nullptr)
+        {
+            if (client->getSensorNetAddress()->isMatch(addr))
+            {
+                _mutex.unlock();
+                return client;
+            }
+            client = client->_nextClient;
+        }
+        _mutex.unlock();
+    }
+    return 0;
+}
+
+Client* ClientList::getClient(int index)
+{
+    Client* client = _firstClient;
+    int p = 0;
+    while (client != nullptr)
+    {
+        if (p == index)
+        {
+            return client;
+        }
+        else
+        {
+            client = client->_nextClient;
+            p++;
+        }
+    }
+    return nullptr;
+}
+
+Client* ClientList::getClient(MQTTSNString* clientId)
+{
+    _mutex.lock();
+    Client* client = _firstClient;
+    const char* clID = clientId->cstring;
+
+    if (clID == nullptr)
+    {
+        clID = clientId->lenstring.data;
+    }
+
+    while (client != nullptr)
+    {
+        if (strncmp((const char*) client->getClientId(), clID, MQTTSNstrlen(*clientId)) == 0)
+        {
+            _mutex.unlock();
+            return client;
+        }
+        client = client->_nextClient;
+    }
+    _mutex.unlock();
+    return 0;
+}
+
+Client* ClientList::createClient(SensorNetAddress* addr, MQTTSNString* clientId, int type)
+{
+    return createClient(addr, clientId, false, false, type);
+}
+
+Client* ClientList::createClient(SensorNetAddress* addr, MQTTSNString* clientId, bool unstableLine, bool secure, int type)
+{
+    Client* client = getClient(addr);
+    if (client)
+    {
+        return client;
+    }
+
+    /* acquire a free client */
+    client = _clientsPool->getClient();
+
+    if (!client)
+    {
+        WRITELOG("%s%sMax number of Clients%s\n", currentDateTime(),
+        ERRMSG_HEADER, ERRMSG_FOOTER);
+        return nullptr;
+    }
+
+    client->disconnected();
+    if (addr)
+    {
+        client->setClientAddress(addr);
+    }
+    client->setSensorNetType(unstableLine);
+    if (MQTTSNstrlen(*clientId))
+    {
+        client->setClientId(*clientId);
+    }
+    else
+    {
+        MQTTSNString dummyId MQTTSNString_initializer;
+        dummyId.cstring = strdup("");
+        client->setClientId(dummyId);
+        free(dummyId.cstring);
+    }
+
+    if (type == AGGREGATER_TYPE)
+    {
+        client->setAggregated();
+    }
+    else if (type == QOSM1PROXY_TYPE)
+    {
+        client->setQoSm1();
+    }
+    client->getNetwork()->setSecure(secure);
+
+    _mutex.lock();
+
+    /* add the list */
+    if (_firstClient == nullptr)
+    {
+        _firstClient = client;
+        _endClient = client;
+    }
+    else
+    {
+        _endClient->_nextClient = client;
+        client->_prevClient = _endClient;
+        _endClient = client;
+    }
+    _clientCnt++;
+    _mutex.unlock();
+    return client;
+}
+
+Client* ClientList::createPredefinedTopic(MQTTSNString* clientId, string topicName, uint16_t topicId, bool aggregate)
+{
+    if (topicId == 0)
+    {
+        WRITELOG("Invalid TopicId. Predefined Topic %s,  TopicId is 0. \n", topicName.c_str());
+        return nullptr;
+    }
+
+    if (strcmp(clientId->cstring, common_topic) == 0)
+    {
+        _gateway->getTopics()->add((const char*) topicName.c_str(), topicId);
+        return nullptr;
+    }
+    else
+    {
+        Client *client = getClient(clientId);
+
+        if (_authorize && client == nullptr)
+        {
+            return nullptr;
+        }
+
+        client = createClient(NULL, clientId, aggregate);
+
+        if (client == nullptr)
+        {
+            return nullptr;
+        }
+
+        // create Topic & Add it
+        client->getTopics()->add((const char*) topicName.c_str(), topicId);
+        client->_hasPredefTopic = true;
+        return client;
+    }
+}
+
+uint16_t ClientList::getClientCount()
+{
+    return _clientCnt;
+}
+
+bool ClientList::isAuthorized()
+{
+    return _authorize;
+}
+
+/******************************
+ * Class ClientsPool
+ ******************************/
+
+ClientsPool::ClientsPool()
+{
+    _clientCnt = 0;
+    _firstClient = nullptr;
+    _endClient = nullptr;
+}
+
+ClientsPool::~ClientsPool()
+{
+    Client* cl = _firstClient;
+    Client* ncl;
+
+    while (cl != nullptr)
+    {
+        ncl = cl->_nextClient;
+        delete cl;
+        cl = ncl;
+    };
+}
+
+void ClientsPool::allocate(int maxClients)
+{
+    Client* cl = nullptr;
+
+    _firstClient = new Client();
+
+    for (int i = 0; i < maxClients; i++)
+    {
+        if ((cl = new Client()) == nullptr)
+        {
+            throw Exception("ClientsPool::Can't allocate max number of clients\n", 0);
+        }
+        cl->_nextClient = _firstClient;
+        _firstClient = cl;
+        _clientCnt++;
+    }
+
+}
+
+Client* ClientsPool::getClient(void)
+{
+    Client *cl = nullptr;
+
+    while (_firstClient != nullptr)
+    {
+        cl = _firstClient;
+        _firstClient = cl->_nextClient;
+        cl->_nextClient = nullptr;
+        _clientCnt--;
+        break;
+    }
+    return cl;
+}
+
+void ClientsPool::setClient(Client* client)
+{
+    if (client)
+    {
+        client->_nextClient = _firstClient;
+        _firstClient = client;
+        _clientCnt++;
+    }
+}
--- a/MQTTSNGateway/src/MQTTSNGWClientList.h
+++ b/MQTTSNGateway/src/MQTTSNGWClientList.h
@@ -0,0 +1,92 @@
+/**************************************************************************************
+ * Copyright (c) 2016, 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
+ *    Tieto Poland Sp. z o.o. - Gateway improvements
+ **************************************************************************************/
+
+#ifndef MQTTSNGATEWAY_SRC_MQTTSNGWCLIENTLIST_H_
+#define MQTTSNGATEWAY_SRC_MQTTSNGWCLIENTLIST_H_
+
+#include "MQTTSNGWClient.h"
+#include "MQTTSNGateway.h"
+
+namespace MQTTSNGW
+{
+#define TRANSPEARENT_TYPE 0
+#define QOSM1PROXY_TYPE 1
+#define AGGREGATER_TYPE 2
+#define FORWARDER_TYPE  3
+
+class Client;
+
+/*=====================================
+ Class ClientsPool
+ =====================================*/
+class ClientsPool
+{
+public:
+	ClientsPool();
+	~ClientsPool();
+	void allocate(int maxClients);
+	Client* getClient(void);
+	void setClient(Client* client);
+
+private:
+	Client* _firstClient;
+	Client* _endClient;
+	int _clientCnt;
+};
+
+/*=====================================
+ Class ClientList
+ =====================================*/
+class ClientList
+{
+public:
+	ClientList(Gateway* gw);
+    ~ClientList();
+
+    void initialize(bool aggregate);
+    void setClientList(int type);
+    void setPredefinedTopics(bool aggregate);
+    void erase(Client*&);
+    Client* createClient(SensorNetAddress* addr, MQTTSNString* clientId,
+            int type);
+    Client* createClient(SensorNetAddress* addr, MQTTSNString* clientId,
+            bool unstableLine, bool secure, int type);
+    bool createList(const char* fileName, int type);
+    Client* getClient(SensorNetAddress* addr);
+    Client* getClient(MQTTSNString* clientId);
+    Client* getClient(int index);
+    uint16_t getClientCount(void);
+    Client* getClient(void);
+    bool isAuthorized();
+
+private:
+    bool readPredefinedList(const char* fileName, bool _aggregate);
+	ClientsPool* _clientsPool;
+	Gateway* _gateway;
+    Client* createPredefinedTopic(MQTTSNString* clientId, string topicName,
+            uint16_t toipcId, bool _aggregate);
+    Client* _firstClient;
+    Client* _endClient;
+    Mutex _mutex;
+    uint16_t _clientCnt;
+    uint16_t _maxClients;
+    bool _authorize { false };
+};
+
+}
+
+#endif /* MQTTSNGATEWAY_SRC_MQTTSNGWCLIENTLIST_H_ */
--- a/MQTTSNGateway/src/MQTTSNGWClientRecvTask.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWClientRecvTask.cpp
@@ -15,193 +15,319 @@
 **************************************************************************************/

 #include "MQTTSNGWClientRecvTask.h"
-#include "MQTTSNGateway.h"
-#include <string.h>
+#include "MQTTSNPacket.h"
+#include "MQTTSNGWQoSm1Proxy.h"
+#include "MQTTSNGWEncapsulatedPacket.h"
+#include <cstring>
+
+using namespace MQTTSNGW;
 char* currentDateTime(void);
 /*=====================================
 Class ClientRecvTask
 =====================================*/
 ClientRecvTask::ClientRecvTask(Gateway* gateway)
 {
-	_gateway = gateway;
-	_gateway->attach((Thread*)this);
-	_sensorNetwork = _gateway->getSensorNetwork();
+    _gateway = gateway;
+    _gateway->attach((Thread*) this);
+    _sensorNetwork = _gateway->getSensorNetwork();
+    setTaskName("ClientRecvTask");
 }

 ClientRecvTask::~ClientRecvTask()
 {
-
 }

-/**
- * Initialize SensorNetwork
- */
 void ClientRecvTask::initialize(int argc, char** argv)
 {
-	if ( _sensorNetwork->initialize() < 0 )
-	{
-		throw Exception(" Can't open the sensor network.\n");
-	}
 }

 /*
 * Receive a packet from clients via sensor netwwork
- * and generate a event to execute the packet handling  procedure
+ * and creats a event to execute the packet handling procedure
 * of MQTTSNPacketHandlingTask.
 */
 void ClientRecvTask::run()
 {
-	Event* ev = 0;
-	Client* client = 0;
-	char buf[128];
+    Event* ev = nullptr;
+    AdapterManager* adpMgr = _gateway->getAdapterManager();
+    QoSm1Proxy* qosm1Proxy = adpMgr->getQoSm1Proxy();
+    int clientType = adpMgr->isAggregaterActive() ? AGGREGATER_TYPE : TRANSPEARENT_TYPE;
+    ClientList* clientList = _gateway->getClientList();
+    EventQue* packetEventQue = _gateway->getPacketEventQue();
+    EventQue* clientsendQue = _gateway->getClientSendQue();

-	while (true)
-	{
-		MQTTSNPacket* packet = new MQTTSNPacket();
-		int packetLen = packet->recv(_sensorNetwork);
+    char buf[128];

-		if (CHK_SIGINT)
-		{
-			WRITELOG("%s ClientRecvTask   stopped.\n", currentDateTime());
-			delete packet;
-			return;
-		}
+    while (true)
+    {
+        Client* client = nullptr;
+        Forwarder* fwd = nullptr;
+        WirelessNodeId nodeId;

-		if (packetLen < 2 )
-		{
-			delete packet;
-			continue;
-		}
+        MQTTSNPacket* packet = new MQTTSNPacket();
+        int packetLen = packet->recv(_sensorNetwork);

-		if ( packet->getType() <= MQTTSN_ADVERTISE || packet->getType() == MQTTSN_GWINFO )
-		{
-			delete packet;
-			continue;
-		}
+        if (CHK_SIGINT)
+        {
+            WRITELOG("%s %s stopped.\n", currentDateTime(), getTaskName());
+            delete packet;
+            return;
+        }

-		if ( packet->getType() == MQTTSN_SEARCHGW )
-		{
-			/* write log and post Event */
-			log(0, packet);
-			ev = new Event();
-			ev->setBrodcastEvent(packet);
-			_gateway->getPacketEventQue()->post(ev);
-			continue;
-		}
+        if (packetLen < 2)
+        {
+            delete packet;
+            continue;
+        }

-		/* get client from the ClientList of Gateway by sensorNetAddress. */
-		client = _gateway->getClientList()->getClient(_sensorNetwork->getSenderAddress());
+        if (packet->getType() <= MQTTSN_ADVERTISE || packet->getType() == MQTTSN_GWINFO)
+        {
+            delete packet;
+            continue;
+        }

-		if ( client )
-		{
-			/* write log and post Event */
-			log(client, packet);
-			ev = new Event();
-			ev->setClientRecvEvent(client,packet);
-			_gateway->getPacketEventQue()->post(ev);
-		}
-		else
-		{
-			/* new client */
-			if (packet->getType() == MQTTSN_CONNECT)
-			{
-				MQTTSNPacket_connectData data;
-				memset(&data, 0, sizeof(MQTTSNPacket_connectData));
-				if ( !packet->getCONNECT(&data) )
-				{
-					log(0, packet, &data.clientID);
-					WRITELOG("%s CONNECT message form %s is incorrect.%s\n", ERRMSG_HEADER, _sensorNetwork->getSenderAddress()->sprint(buf), ERRMSG_FOOTER);
-					delete packet;
-					continue;
-				}
+        if (packet->getType() == MQTTSN_SEARCHGW)
+        {
+            /* write log and post Event */
+            log(0, packet, 0);
+            ev = new Event();
+            ev->setBrodcastEvent(packet);
+            packetEventQue->post(ev);
+            continue;
+        }

-				/* create a 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);
-					delete packet;
-					continue;
-				}
+        SensorNetAddress senderAddr = *_gateway->getSensorNetwork()->getSenderAddress();

-				/* set sensorNetAddress & post Event */
-				client->setClientAddress(_sensorNetwork->getSenderAddress());
-				ev = new Event();
-				ev->setClientRecvEvent(client, packet);
-				_gateway->getPacketEventQue()->post(ev);
-			}
-			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;
-			}
-		}
-	}
+        if (packet->getType() == MQTTSN_ENCAPSULATED)
+        {
+            fwd = _gateway->getAdapterManager()->getForwarderList()->getForwarder(&senderAddr);
+
+            if (fwd != nullptr)
+            {
+                MQTTSNString fwdName = MQTTSNString_initializer;
+                fwdName.cstring = const_cast<char *>(fwd->getName());
+                log(0, packet, &fwdName);
+
+                /* get the packet from the encapsulation message */
+                MQTTSNGWEncapsulatedPacket encap;
+                encap.desirialize(packet->getPacketData(), packet->getPacketLength());
+                nodeId.setId(encap.getWirelessNodeId());
+                client = fwd->getClient(&nodeId);
+                packet = encap.getMQTTSNPacket();
+            }
+        }
+        else
+        {
+            /*   Check the client belonging to QoS-1Proxy  ?    */
+
+            if (qosm1Proxy->isActive())
+            {
+                const char *clientName = qosm1Proxy->getClientId(&senderAddr);
+
+                if (clientName != nullptr)
+                {
+                    client = qosm1Proxy->getClient();
+
+                    if (!packet->isQoSMinusPUBLISH())
+                    {
+                        log(clientName, packet);
+                        WRITELOG("%s %s  %s can send only PUBLISH with QoS-1.%s\n",
+                        ERRMSG_HEADER, clientName, senderAddr.sprint(buf), ERRMSG_FOOTER);
+                        delete packet;
+                        continue;
+                    }
+                }
+            }
+
+            if (client == nullptr)
+            {
+                client = _gateway->getClientList()->getClient(&senderAddr);
+            }
+        }
+
+        if (client != nullptr)
+        {
+            log(client, packet, 0);
+
+            if (client->isDisconnect() && packet->getType() != MQTTSN_CONNECT)
+            {
+                WRITELOG("%s MQTTSNGWClientRecvTask %s is not connecting.%s\n",
+                ERRMSG_HEADER, client->getClientId(), ERRMSG_FOOTER);
+
+                /* send DISCONNECT to the client, if it is not connected */
+                MQTTSNPacket* snPacket = new MQTTSNPacket();
+                snPacket->setDISCONNECT(0);
+                ev = new Event();
+                ev->setClientSendEvent(client, snPacket);
+                clientsendQue->post(ev);
+                delete packet;
+                continue;
+            }
+            else
+            {
+                ev = new Event();
+                ev->setClientRecvEvent(client, packet);
+                packetEventQue->post(ev);
+            }
+        }
+        else
+        {
+            /* new client */
+            if (packet->getType() == MQTTSN_CONNECT)
+            {
+                MQTTSNPacket_connectData data;
+                memset(&data, 0, sizeof(MQTTSNPacket_connectData));
+                if (!packet->getCONNECT(&data))
+                {
+                    log(0, packet, &data.clientID);
+                    WRITELOG("%s CONNECT message form %s is incorrect.%s\n",
+                    ERRMSG_HEADER, senderAddr.sprint(buf),
+                    ERRMSG_FOOTER);
+                    delete packet;
+                    continue;
+                }
+
+                client = clientList->getClient(&data.clientID);
+
+                if (fwd != nullptr)
+                {
+                    if (client == nullptr)
+                    {
+                        /* create a new client */
+                        client = clientList->createClient(0, &data.clientID, clientType);
+                    }
+                    /* Add to a forwarded client list of forwarder. */
+                    fwd->addClient(client, &nodeId);
+                }
+                else
+                {
+                    if (client)
+                    {
+                        /* Authentication is not required */
+                        if (_gateway->getGWParams()->clientAuthentication == false)
+                        {
+                            client->setClientAddress(&senderAddr);
+                        }
+                    }
+                    else
+                    {
+                        /* create a new client */
+                        client = clientList->createClient(&senderAddr, &data.clientID, clientType);
+                    }
+                }
+
+                log(client, packet, &data.clientID);
+
+                if (client == nullptr)
+                {
+                    WRITELOG("%s Client(%s) was rejected. CONNECT message has been discarded.%s\n",
+                    ERRMSG_HEADER, senderAddr.sprint(buf),
+                    ERRMSG_FOOTER);
+                    delete packet;
+                    continue;
+                }
+
+                /* post Client RecvEvent */
+                ev = new Event();
+                ev->setClientRecvEvent(client, packet);
+                packetEventQue->post(ev);
+            }
+            else
+            {
+                log(client, packet, 0);
+                if (packet->getType() == MQTTSN_ENCAPSULATED)
+                {
+                    WRITELOG(
+                            "%s MQTTSNGWClientRecvTask  Forwarder(%s) is not declared by ClientList file. message has been discarded.%s\n",
+                            ERRMSG_HEADER, _sensorNetwork->getSenderAddress()->sprint(buf),
+                            ERRMSG_FOOTER);
+                }
+                else
+                {
+                    WRITELOG("%s MQTTSNGWClientRecvTask  Client(%s) is not connecting. message has been discarded.%s\n",
+                    ERRMSG_HEADER, senderAddr.sprint(buf),
+                    ERRMSG_FOOTER);
+                }
+                delete packet;
+            }
+        }
+    }
 }

 void ClientRecvTask::log(Client* client, MQTTSNPacket* packet, MQTTSNString* id)
 {
-	char pbuf[SIZE_OF_LOG_PACKET * 3];
-	const char* clientId;
-	char cstr[MAX_CLIENTID_LENGTH + 1];
-	char msgId[6];
+    const char* clientId;
+    char cstr[MAX_CLIENTID_LENGTH + 1];

-	if ( id )
-	{
-		memset((void*)cstr, 0, id->lenstring.len + 1);
-		strncpy(cstr, id->lenstring.data, id->lenstring.len) ;
-		clientId = cstr;
-	}
-	else if ( client )
-	{
-		clientId = client->getClientId();
-	}
-	else
-	{
-		clientId = UNKNOWNCL;
-	}
+    if (id)
+    {
+        if (id->cstring)
+        {
+            strncpy(cstr, id->cstring, strlen(id->cstring));
+            clientId = cstr;
+        }
+        else
+        {
+            memset((void*) cstr, 0, id->lenstring.len + 1);
+            strncpy(cstr, id->lenstring.data, id->lenstring.len);
+            clientId = cstr;
+        }
+    }
+    else if (client)
+    {
+        clientId = client->getClientId();
+    }
+    else
+    {
+        clientId = UNKNOWNCL;
+    }

-	switch (packet->getType())
-	{
-	case MQTTSN_SEARCHGW:
-	case MQTTSN_PINGREQ:
-		WRITELOG(FORMAT_Y_G_G_NL, currentDateTime(), packet->getName(), LEFTARROW, CLIENT, packet->print(pbuf));
-		break;
-	case MQTTSN_CONNECT:
-		WRITELOG(FORMAT_Y_G_G_NL, currentDateTime(), packet->getName(), LEFTARROW, clientId, packet->print(pbuf));
-		break;
-	case MQTTSN_DISCONNECT:
-	case MQTTSN_WILLTOPICUPD:
-	case MQTTSN_WILLMSGUPD:
-	case MQTTSN_WILLTOPIC:
-	case MQTTSN_WILLMSG:
-		WRITELOG(FORMAT_Y_G_G, currentDateTime(), packet->getName(), LEFTARROW, clientId, packet->print(pbuf));
-		break;
-	case MQTTSN_PUBLISH:
-	case MQTTSN_REGISTER:
-	case MQTTSN_SUBSCRIBE:
-	case MQTTSN_UNSUBSCRIBE:
-		WRITELOG(FORMAT_G_MSGID_G_G_NL, currentDateTime(), packet->getName(), packet->getMsgId(msgId), LEFTARROW, clientId, packet->print(pbuf));
-		break;
-	case MQTTSN_REGACK:
-	case MQTTSN_PUBACK:
-	case MQTTSN_PUBREC:
-	case MQTTSN_PUBREL:
-	case MQTTSN_PUBCOMP:
-		WRITELOG(FORMAT_G_MSGID_G_G, currentDateTime(), packet->getName(), packet->getMsgId(msgId), LEFTARROW, clientId, packet->print(pbuf));
-		break;
-	default:
-		WRITELOG(FORMAT_W_NL, currentDateTime(), packet->getName(), LEFTARROW, clientId, packet->print(pbuf));
-		break;
-	}
+    log(clientId, packet);
+}
+
+void ClientRecvTask::log(const char* clientId, MQTTSNPacket* packet)
+{
+    char pbuf[ SIZE_OF_LOG_PACKET * 3 + 1];
+    char msgId[6];
+
+    switch (packet->getType())
+    {
+    case MQTTSN_SEARCHGW:
+        WRITELOG(FORMAT_Y_G_G_NL, currentDateTime(), packet->getName(),
+        LEFTARROW, CLIENT, packet->print(pbuf));
+        break;
+    case MQTTSN_CONNECT:
+    case MQTTSN_PINGREQ:
+        WRITELOG(FORMAT_Y_G_G_NL, currentDateTime(), packet->getName(),
+        LEFTARROW, clientId, packet->print(pbuf));
+        break;
+    case MQTTSN_DISCONNECT:
+    case MQTTSN_WILLTOPICUPD:
+    case MQTTSN_WILLMSGUPD:
+    case MQTTSN_WILLTOPIC:
+    case MQTTSN_WILLMSG:
+        WRITELOG(FORMAT_Y_G_G, currentDateTime(), packet->getName(), LEFTARROW, clientId, packet->print(pbuf));
+        break;
+    case MQTTSN_PUBLISH:
+    case MQTTSN_REGISTER:
+    case MQTTSN_SUBSCRIBE:
+    case MQTTSN_UNSUBSCRIBE:
+        WRITELOG(FORMAT_G_MSGID_G_G_NL, currentDateTime(), packet->getName(), packet->getMsgId(msgId), LEFTARROW, clientId,
+                packet->print(pbuf));
+        break;
+    case MQTTSN_REGACK:
+    case MQTTSN_PUBACK:
+    case MQTTSN_PUBREC:
+    case MQTTSN_PUBREL:
+    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;
+    }
 }
--- a/MQTTSNGateway/src/MQTTSNGWClientRecvTask.h
+++ b/MQTTSNGateway/src/MQTTSNGWClientRecvTask.h
@@ -21,24 +21,27 @@

 namespace MQTTSNGW
 {
+class AdapterManager;

 /*=====================================
-     Class ClientRecvTask
+ Class ClientRecvTask
 =====================================*/
-class ClientRecvTask:public Thread
+class ClientRecvTask: public Thread
 {
-	MAGIC_WORD_FOR_THREAD;
+MAGIC_WORD_FOR_THREAD;
+    friend AdapterManager;
 public:
-	ClientRecvTask(Gateway*);
-	~ClientRecvTask();
-	virtual void initialize(int argc, char** argv);
-	void run();
+    ClientRecvTask(Gateway*);
+    ~ClientRecvTask(void);
+    virtual void initialize(int argc, char** argv);
+    void run(void);

 private:
-	void log(Client*, MQTTSNPacket*, MQTTSNString* id = 0);
+    void log(Client*, MQTTSNPacket*, MQTTSNString* id);
+    void log(const char* clientId, MQTTSNPacket* packet);

-	Gateway*       _gateway;
-	SensorNetwork* _sensorNetwork;
+    Gateway* _gateway;
+    SensorNetwork* _sensorNetwork;
 };

 }
--- a/MQTTSNGateway/src/MQTTSNGWClientSendTask.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWClientSendTask.cpp
@@ -15,8 +15,10 @@
 **************************************************************************************/
 #include "MQTTSNGWClientSendTask.h"
 #include "MQTTSNGWPacket.h"
-#include "MQTTSNGWPacket.h"
 #include "MQTTSNGateway.h"
+#include "MQTTSNGWEncapsulatedPacket.h"
+#include "MQTTSNGWQoSm1Proxy.h"
+#include <errno.h>

 using namespace MQTTSNGW;
 using namespace std;
@@ -26,96 +28,110 @@ char* currentDateTime(void);
 =====================================*/
 ClientSendTask::ClientSendTask(Gateway* gateway)
 {
-	_gateway = gateway;
-	_gateway->attach((Thread*)this);
-	_sensorNetwork = _gateway->getSensorNetwork();
+    _gateway = gateway;
+    _gateway->attach((Thread*) this);
+    _sensorNetwork = _gateway->getSensorNetwork();
+    setTaskName("ClientSendTask");
 }

 ClientSendTask::~ClientSendTask()
 {
-
 }

 void ClientSendTask::run()
 {
-	Client* client = 0;
-	MQTTSNPacket* packet = 0;
-	int rc = 0;
+    Client* client = nullptr;
+    MQTTSNPacket* packet = nullptr;
+    AdapterManager* adpMgr = _gateway->getAdapterManager();
+    int rc = 0;

-	while (true)
-	{
-		Event* ev = _gateway->getClientSendQue()->wait();
+    while (true)
+    {
+        Event* ev = _gateway->getClientSendQue()->wait();

-		if (ev->getEventType() == EtStop)
-		{
-			WRITELOG("%s ClientSendTask   stopped.\n", currentDateTime());
-			delete ev;
-			break;
-		}
-		if (ev->getEventType() == EtClientSend)
-		{
-			client = ev->getClient();
-			packet = ev->getMQTTSNPacket();
-			log(client, packet);
-			rc = packet->unicast(_sensorNetwork, client->getSensorNetAddress());
-		}
-		else if (ev->getEventType() == EtBroadcast)
-		{
-			packet = ev->getMQTTSNPacket();
-			log(client, packet);
-			rc = packet->broadcast(_sensorNetwork);
-		}
-		else if (ev->getEventType() == EtSensornetSend)
-		{
-			packet = ev->getMQTTSNPacket();
-			log(client, packet);
-			rc = packet->unicast(_sensorNetwork, ev->getSensorNetAddress());
-		}
+        if (ev->getEventType() == EtStop || _gateway->IsStopping())
+        {
+            WRITELOG("%s %s stopped.\n", currentDateTime(), getTaskName());
+            delete ev;
+            break;
+        }

-		if ( rc < 0 )
-		{
-			WRITELOG("%s ClientSendTask can't send a packet to the client.\n",
-				ERRMSG_HEADER, (client ? (const char*)client->getClientId() : UNKNOWNCL ), ERRMSG_FOOTER);
-		}
-		delete ev;
-	}
+        if (ev->getEventType() == EtBroadcast)
+        {
+            packet = ev->getMQTTSNPacket();
+            log(client, packet);
+
+            if (packet->broadcast(_sensorNetwork) < 0)
+            {
+                WRITELOG("%s ClientSendTask can't multicast a packet Error=%d%s\n",
+                ERRMSG_HEADER, errno, ERRMSG_FOOTER);
+            }
+        }
+        else
+        {
+            if (ev->getEventType() == EtClientSend)
+            {
+                client = ev->getClient();
+                packet = ev->getMQTTSNPacket();
+                rc = adpMgr->unicastToClient(client, packet, this);
+            }
+            else if (ev->getEventType() == EtSensornetSend)
+            {
+                packet = ev->getMQTTSNPacket();
+                log(client, packet);
+                rc = packet->unicast(_sensorNetwork, ev->getSensorNetAddress());
+            }
+
+            if (rc < 0)
+            {
+                WRITELOG("%s ClientSendTask can't send a packet to the client %s. Error=%d%s\n",
+                ERRMSG_HEADER, (client ? (const char*) client->getClientId() : UNKNOWNCL),
+                errno, ERRMSG_FOOTER);
+            }
+        }
+        delete ev;
+    }
 }

 void ClientSendTask::log(Client* client, MQTTSNPacket* packet)
 {
-	char pbuf[SIZE_OF_LOG_PACKET * 3];
-	char msgId[6];
-	const char* clientId = client ? (const char*)client->getClientId() : UNKNOWNCL ;
+    char pbuf[SIZE_OF_LOG_PACKET * 3 + 1];
+    char msgId[6];
+    const char* clientId = client ? (const char*) client->getClientId() : UNKNOWNCL;

-	switch (packet->getType())
-	{
-	case MQTTSN_ADVERTISE:
-	case MQTTSN_GWINFO:
-	case MQTTSN_PINGRESP:
-		WRITELOG(FORMAT_Y_W_G, currentDateTime(), packet->getName(), RIGHTARROW, CLIENTS, packet->print(pbuf));
-		break;
-	case MQTTSN_CONNACK:
-	case MQTTSN_DISCONNECT:
-	case MQTTSN_WILLTOPICREQ:
-	case MQTTSN_WILLMSGREQ:
-	case MQTTSN_WILLTOPICRESP:
-	case MQTTSN_WILLMSGRESP:
-		WRITELOG(FORMAT_Y_W_G, currentDateTime(), packet->getName(), RIGHTARROW, clientId, packet->print(pbuf));
-		break;
-	case MQTTSN_REGISTER:
-	case MQTTSN_PUBLISH:
-		WRITELOG(FORMAT_W_MSGID_W_G, currentDateTime(), packet->getName(), packet->getMsgId(msgId), RIGHTARROW, clientId,	packet->print(pbuf));
-		break;
-	case MQTTSN_REGACK:
-	case MQTTSN_PUBACK:
-	case MQTTSN_PUBREC:
-	case MQTTSN_PUBREL:
-	case MQTTSN_PUBCOMP:
-	case MQTTSN_SUBACK:
-	case MQTTSN_UNSUBACK:
-		WRITELOG(FORMAT_W_MSGID_W_G, currentDateTime(), packet->getName(), packet->getMsgId(msgId), RIGHTARROW, clientId,	packet->print(pbuf));
-		break;
-	default:
-		break;
-	}
+    switch (packet->getType())
+    {
+    case MQTTSN_ADVERTISE:
+    case MQTTSN_GWINFO:
+        WRITELOG(FORMAT_Y_W_G, currentDateTime(), packet->getName(), RIGHTARROW,
+        CLIENTS, packet->print(pbuf));
+        break;
+    case MQTTSN_CONNACK:
+    case MQTTSN_DISCONNECT:
+    case MQTTSN_WILLTOPICREQ:
+    case MQTTSN_WILLMSGREQ:
+    case MQTTSN_WILLTOPICRESP:
+    case MQTTSN_WILLMSGRESP:
+    case MQTTSN_PINGRESP:
+        WRITELOG(FORMAT_Y_W_G, currentDateTime(), packet->getName(), RIGHTARROW, clientId, packet->print(pbuf));
+        break;
+    case MQTTSN_REGISTER:
+    case MQTTSN_PUBLISH:
+        WRITELOG(FORMAT_W_MSGID_W_G, currentDateTime(), packet->getName(), packet->getMsgId(msgId), RIGHTARROW, clientId,
+                packet->print(pbuf));
+        break;
+    case MQTTSN_REGACK:
+    case MQTTSN_PUBACK:
+    case MQTTSN_PUBREC:
+    case MQTTSN_PUBREL:
+    case MQTTSN_PUBCOMP:
+    case MQTTSN_SUBACK:
+    case MQTTSN_UNSUBACK:
+        WRITELOG(FORMAT_W_MSGID_W_G, currentDateTime(), packet->getName(), packet->getMsgId(msgId), RIGHTARROW, clientId,
+                packet->print(pbuf));
+        break;
+    default:
+        break;
+    }
 }
+
--- a/MQTTSNGateway/src/MQTTSNGWClientSendTask.h
+++ b/MQTTSNGateway/src/MQTTSNGWClientSendTask.h
@@ -21,23 +21,25 @@

 namespace MQTTSNGW
 {
+class AdapterManager;

 /*=====================================
 Class ClientSendTask
 =====================================*/
 class ClientSendTask: public Thread
 {
-	MAGIC_WORD_FOR_THREAD;
+MAGIC_WORD_FOR_THREAD;
+    friend AdapterManager;
 public:
-	ClientSendTask(Gateway* gateway);
-	~ClientSendTask();
-	void run();
+    ClientSendTask(Gateway* gateway);
+    ~ClientSendTask(void);
+    void run(void);

 private:
-	void log(Client*, MQTTSNPacket*);
+    void log(Client* client, MQTTSNPacket* packet);

-	Gateway* _gateway;
-	SensorNetwork* _sensorNetwork;
+    Gateway* _gateway;
+    SensorNetwork* _sensorNetwork;
 };

 }
--- a/MQTTSNGateway/src/MQTTSNGWConnectionHandler.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWConnectionHandler.cpp
@@ -1,5 +1,5 @@
 /**************************************************************************************
- * Copyright (c) 2016, Tomoaki Yamaguchi
+ * Copyright (c) 2016, 2020 Tomoaki Yamaguchi and others
 *
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the Eclipse Public License v1.0
@@ -28,7 +28,7 @@ using namespace MQTTSNGW;
 =====================================*/
 MQTTSNConnectionHandler::MQTTSNConnectionHandler(Gateway* gateway)
 {
-	_gateway = gateway;
+    _gateway = gateway;
 }

 MQTTSNConnectionHandler::~MQTTSNConnectionHandler()
@@ -41,11 +41,11 @@ MQTTSNConnectionHandler::~MQTTSNConnectionHandler()
 */
 void MQTTSNConnectionHandler::sendADVERTISE()
 {
-	MQTTSNPacket* adv = new MQTTSNPacket();
-	adv->setADVERTISE(_gateway->getGWParams()->gatewayId, _gateway->getGWParams()->keepAlive);
-	Event* ev1 = new Event();
-	ev1->setBrodcastEvent(adv);  //broadcast
-	_gateway->getClientSendQue()->post(ev1);
+    MQTTSNPacket* adv = new MQTTSNPacket();
+    adv->setADVERTISE(_gateway->getGWParams()->gatewayId, _gateway->getGWParams()->keepAlive);
+    Event* ev1 = new Event();
+    ev1->setBrodcastEvent(adv);  //broadcast
+    _gateway->getClientSendQue()->post(ev1);
 }

 /*
@@ -53,14 +53,14 @@ void MQTTSNConnectionHandler::sendADVERTISE()
 */
 void MQTTSNConnectionHandler::handleSearchgw(MQTTSNPacket* packet)
 {
-	if (packet->getType() == MQTTSN_SEARCHGW)
-	{
-		MQTTSNPacket* gwinfo = new MQTTSNPacket();
-		gwinfo->setGWINFO(_gateway->getGWParams()->gatewayId);
-		Event* ev1 = new Event();
-		ev1->setBrodcastEvent(gwinfo);
-		_gateway->getClientSendQue()->post(ev1);
-	}
+    if (packet->getType() == MQTTSN_SEARCHGW)
+    {
+        MQTTSNPacket* gwinfo = new MQTTSNPacket();
+        gwinfo->setGWINFO(_gateway->getGWParams()->gatewayId);
+        Event* ev1 = new Event();
+        ev1->setBrodcastEvent(gwinfo);
+        _gateway->getClientSendQue()->post(ev1);
+    }
 }

 /*
@@ -68,72 +68,91 @@ void MQTTSNConnectionHandler::handleSearchgw(MQTTSNPacket* packet)
 */
 void MQTTSNConnectionHandler::handleConnect(Client* client, MQTTSNPacket* packet)
 {
-	MQTTSNPacket_connectData data;
-	if ( packet->getCONNECT(&data) == 0 )
-	{
-		return;
-	}
+    MQTTSNPacket_connectData data;
+    if (packet->getCONNECT(&data) == 0)
+    {
+        return;
+    }

-	/* clear ConnectData of Client */
-	Connect* connectData = client->getConnectData();
-	memset(connectData, 0, sizeof(Connect));
-	client->disconnected();
+    /* return CONNACK when the client is sleeping */
+    if (client->isSleep() || client->isAwake())
+    {
+        MQTTSNPacket* packet = new MQTTSNPacket();
+        packet->setCONNACK(MQTTSN_RC_ACCEPTED);
+        Event* ev = new Event();
+        ev->setClientSendEvent(client, packet);
+        _gateway->getClientSendQue()->post(ev);

-	Topics* topics = client->getTopics();
+        sendStoredPublish(client);
+        return;
+    }

-	/* CONNECT was not sent yet. prepare Connect data */
-	connectData->header.bits.type = CONNECT;
-	connectData->clientID = client->getClientId();
-	connectData->version = _gateway->getGWParams()->mqttVersion;
-	connectData->keepAliveTimer = data.duration;
-	connectData->flags.bits.will = data.willFlag;
+    //* clear ConnectData of Client */
+    Connect* connectData = client->getConnectData();
+    memset(connectData, 0, sizeof(Connect));
+    if (!client->isAdapter())
+    {
+        client->disconnected();
+    }

-	if ((const char*) _gateway->getGWParams()->loginId != 0 && (const char*) _gateway->getGWParams()->password != 0)
-	{
-		connectData->flags.bits.password = 1;
-		connectData->flags.bits.username = 1;
-	}
+    Topics* topics = client->getTopics();

-	client->setSessionStatus(false);
-	if (data.cleansession)
-	{
-		connectData->flags.bits.cleanstart = 1;
-		/* reset the table of msgNo and TopicId pare */
-		client->clearWaitedPubTopicId();
-		client->clearWaitedSubTopicId();
+    /* CONNECT was not sent yet. prepare Connect data */
+    connectData->header.bits.type = CONNECT;
+    connectData->clientID = client->getClientId();
+    connectData->version = _gateway->getGWParams()->mqttVersion;
+    connectData->keepAliveTimer = data.duration;
+    connectData->flags.bits.will = data.willFlag;

-		/* renew the TopicList */
-		if (topics)
-		{
-			delete topics;
-		}
-		topics = new Topics();
-		client->setTopics(topics);
-		client->setSessionStatus(true);
-	}
+    if ((const char*) _gateway->getGWParams()->loginId != nullptr)
+    {
+        connectData->flags.bits.username = 1;
+    }

-	if (data.willFlag)
-	{
-		/* create & send WILLTOPICREQ message to the client */
-		MQTTSNPacket* reqTopic = new MQTTSNPacket();
-		reqTopic->setWILLTOPICREQ();
-		Event* evwr = new Event();
-		evwr->setClientSendEvent(client, reqTopic);
+    if ((const char*) _gateway->getGWParams()->password != 0)
+    {
+        connectData->flags.bits.password = 1;
+    }

-		/* Send WILLTOPICREQ to the client */
-		_gateway->getClientSendQue()->post(evwr);
-	}
-	else
-	{
+    client->setSessionStatus(false);
+    if (data.cleansession)
+    {
+        connectData->flags.bits.cleanstart = 1;
+        /* reset the table of msgNo and TopicId pare */
+        client->clearWaitedPubTopicId();
+        client->clearWaitedSubTopicId();

-		/* CONNECT message was not qued in.
-		 * create CONNECT message & send it to the broker */
-		MQTTGWPacket* mqMsg = new MQTTGWPacket();
-		mqMsg->setCONNECT(client->getConnectData(), (unsigned char*)_gateway->getGWParams()->loginId, (unsigned char*)_gateway->getGWParams()->password);
-		Event* ev1 = new Event();
-		ev1->setBrokerSendEvent(client, mqMsg);
-		_gateway->getBrokerSendQue()->post(ev1);
-	}
+        /* renew the TopicList */
+        if (topics)
+        {
+            topics->eraseNormal();
+            ;
+        }
+        client->setSessionStatus(true);
+    }
+
+    if (data.willFlag)
+    {
+        /* create & send WILLTOPICREQ message to the client */
+        MQTTSNPacket* reqTopic = new MQTTSNPacket();
+        reqTopic->setWILLTOPICREQ();
+        Event* evwr = new Event();
+        evwr->setClientSendEvent(client, reqTopic);
+
+        /* Send WILLTOPICREQ to the client */
+        _gateway->getClientSendQue()->post(evwr);
+    }
+    else
+    {
+        /* CONNECT message was not qued in.
+         * create CONNECT message & send it to the broker */
+        MQTTGWPacket* mqMsg = new MQTTGWPacket();
+        mqMsg->setCONNECT(client->getConnectData(), (unsigned char*) _gateway->getGWParams()->loginId,
+                (unsigned char*) _gateway->getGWParams()->password);
+        Event* ev1 = new Event();
+        ev1->setBrokerSendEvent(client, mqMsg);
+        _gateway->getBrokerSendQue()->post(ev1);
+    }
 }

 /*
@@ -141,29 +160,29 @@ void MQTTSNConnectionHandler::handleConnect(Client* client, MQTTSNPacket* packet
 */
 void MQTTSNConnectionHandler::handleWilltopic(Client* client, MQTTSNPacket* packet)
 {
-	int willQos;
-	uint8_t willRetain;
-	MQTTSNString willTopic;
+    int willQos;
+    uint8_t willRetain;
+    MQTTSNString willTopic = MQTTSNString_initializer;

-	if ( packet->getWILLTOPIC(&willQos, &willRetain, &willTopic) == 0 )
-	{
-		return;
-	}
-	client->setWillTopic(willTopic);
-	Connect* connectData = client->getConnectData();
+    if (packet->getWILLTOPIC(&willQos, &willRetain, &willTopic) == 0)
+    {
+        return;
+    }
+    client->setWillTopic(willTopic);
+    Connect* connectData = client->getConnectData();

-	/* add the connectData for MQTT CONNECT message */
-	connectData->willTopic = client->getWillTopic();
-	connectData->flags.bits.willQoS = willQos;
-	connectData->flags.bits.willRetain = willRetain;
+    /* add the connectData for MQTT CONNECT message */
+    connectData->willTopic = client->getWillTopic();
+    connectData->flags.bits.willQoS = willQos;
+    connectData->flags.bits.willRetain = willRetain;

-	/* Send WILLMSGREQ to the client */
-	client->setWaitWillMsgFlg(true);
-	MQTTSNPacket* reqMsg = new MQTTSNPacket();
-	reqMsg->setWILLMSGREQ();
-	Event* evt = new Event();
-	evt->setClientSendEvent(client, reqMsg);
-	_gateway->getClientSendQue()->post(evt);
+    /* Send WILLMSGREQ to the client */
+    client->setWaitWillMsgFlg(true);
+    MQTTSNPacket* reqMsg = new MQTTSNPacket();
+    reqMsg->setWILLMSGREQ();
+    Event* evt = new Event();
+    evt->setClientSendEvent(client, reqMsg);
+    _gateway->getClientSendQue()->post(evt);
 }

 /*
@@ -171,35 +190,36 @@ void MQTTSNConnectionHandler::handleWilltopic(Client* client, MQTTSNPacket* pack
 */
 void MQTTSNConnectionHandler::handleWillmsg(Client* client, MQTTSNPacket* packet)
 {
-	if ( !client->isWaitWillMsg() )
-	{
-		DEBUGLOG("     MQTTSNConnectionHandler::handleWillmsg  WaitWillMsgFlg is off.\n");
-		return;
-	}
+    if (!client->isWaitWillMsg())
+    {
+        DEBUGLOG("     MQTTSNConnectionHandler::handleWillmsg  WaitWillMsgFlg is off.\n");
+        return;
+    }

-	MQTTSNString willmsg;
-	Connect* connectData = client->getConnectData();
+    MQTTSNString willmsg = MQTTSNString_initializer;
+    Connect* connectData = client->getConnectData();

-	if( client->isConnectSendable() )
-	{
-		/* save WillMsg in the client */
-		if ( packet->getWILLMSG(&willmsg) == 0 )
-		{
-			return;
-		}
-		client->setWillMsg(willmsg);
+    if (client->isConnectSendable())
+    {
+        /* save WillMsg in the client */
+        if (packet->getWILLMSG(&willmsg) == 0)
+        {
+            return;
+        }
+        client->setWillMsg(willmsg);

-		/* create CONNECT message */
-		MQTTGWPacket* mqttPacket =  new MQTTGWPacket();
-		connectData->willMsg = client->getWillMsg();
-		mqttPacket->setCONNECT(connectData, (unsigned char*)_gateway->getGWParams()->loginId, (unsigned char*)_gateway->getGWParams()->password);
+        /* create CONNECT message */
+        MQTTGWPacket* mqttPacket = new MQTTGWPacket();
+        connectData->willMsg = client->getWillMsg();
+        mqttPacket->setCONNECT(connectData, (unsigned char*) _gateway->getGWParams()->loginId,
+                (unsigned char*) _gateway->getGWParams()->password);

-		/* Send CONNECT to the broker */
-		Event* evt = new Event();
-		evt->setBrokerSendEvent(client, mqttPacket);
-		client->setWaitWillMsgFlg(false);	
-		_gateway->getBrokerSendQue()->post(evt);
-	}
+        /* Send CONNECT to the broker */
+        Event* evt = new Event();
+        evt->setBrokerSendEvent(client, mqttPacket);
+        client->setWaitWillMsgFlg(false);
+        _gateway->getBrokerSendQue()->post(evt);
+    }
 }

 /*
@@ -207,25 +227,25 @@ void MQTTSNConnectionHandler::handleWillmsg(Client* client, MQTTSNPacket* packet
 */
 void MQTTSNConnectionHandler::handleDisconnect(Client* client, MQTTSNPacket* packet)
 {
-	Event* ev = new Event();
-	MQTTSNPacket* snMsg = new MQTTSNPacket();
-	snMsg->setDISCONNECT(0);
-	ev->setClientSendEvent(client, snMsg);
-	_gateway->getClientSendQue()->post(ev);
+    uint16_t duration = 0;

-	uint16_t duration = 0;
-	if ( packet->getDISCONNECT(&duration) == 0 )
-	{
-		return;
-	}
-	if ( duration == 0 )
-	{
-		MQTTGWPacket* mqMsg = new MQTTGWPacket();
-		mqMsg->setHeader(DISCONNECT);
-		ev = new Event();
-		ev->setBrokerSendEvent(client, mqMsg);
-		_gateway->getBrokerSendQue()->post(ev);
-	}
+    if (packet->getDISCONNECT(&duration) != 0)
+    {
+        if (duration == 0)
+        {
+            MQTTGWPacket* mqMsg = new MQTTGWPacket();
+            mqMsg->setHeader(DISCONNECT);
+            Event* ev = new Event();
+            ev->setBrokerSendEvent(client, mqMsg);
+            _gateway->getBrokerSendQue()->post(ev);
+        }
+    }
+
+    MQTTSNPacket* snMsg = new MQTTSNPacket();
+    snMsg->setDISCONNECT(0);
+    Event* evt = new Event();
+    evt->setClientSendEvent(client, snMsg);
+    _gateway->getClientSendQue()->post(evt);
 }

 /*
@@ -233,12 +253,12 @@ void MQTTSNConnectionHandler::handleDisconnect(Client* client, MQTTSNPacket* pac
 */
 void MQTTSNConnectionHandler::handleWilltopicupd(Client* client, MQTTSNPacket* packet)
 {
-	/* send NOT_SUPPORTED responce to the client */
-	MQTTSNPacket* respMsg = new MQTTSNPacket();
-	respMsg->setWILLTOPICRESP(MQTTSN_RC_NOT_SUPPORTED);
-	Event* evt = new Event();
-	evt->setClientSendEvent(client, respMsg);
-	_gateway->getClientSendQue()->post(evt);
+    /* send NOT_SUPPORTED responce to the client */
+    MQTTSNPacket* respMsg = new MQTTSNPacket();
+    respMsg->setWILLTOPICRESP(MQTTSN_RC_NOT_SUPPORTED);
+    Event* evt = new Event();
+    evt->setClientSendEvent(client, respMsg);
+    _gateway->getClientSendQue()->post(evt);
 }

 /*
@@ -246,12 +266,12 @@ void MQTTSNConnectionHandler::handleWilltopicupd(Client* client, MQTTSNPacket* p
 */
 void MQTTSNConnectionHandler::handleWillmsgupd(Client* client, MQTTSNPacket* packet)
 {
-	/* send NOT_SUPPORTED responce to the client */
-	MQTTSNPacket* respMsg = new MQTTSNPacket();
-	respMsg->setWILLMSGRESP(MQTTSN_RC_NOT_SUPPORTED);
-	Event* evt = new Event();
-	evt->setClientSendEvent(client, respMsg);
-	_gateway->getClientSendQue()->post(evt);
+    /* send NOT_SUPPORTED responce to the client */
+    MQTTSNPacket* respMsg = new MQTTSNPacket();
+    respMsg->setWILLMSGRESP(MQTTSN_RC_NOT_SUPPORTED);
+    Event* evt = new Event();
+    evt->setClientSendEvent(client, respMsg);
+    _gateway->getClientSendQue()->post(evt);
 }

 /*
@@ -259,10 +279,34 @@ void MQTTSNConnectionHandler::handleWillmsgupd(Client* client, MQTTSNPacket* pac
 */
 void MQTTSNConnectionHandler::handlePingreq(Client* client, MQTTSNPacket* packet)
 {
-	/* send PINGREQ to the broker */
-	MQTTGWPacket* pingreq = new MQTTGWPacket();
-	pingreq->setHeader(PINGREQ);
-	Event* evt = new Event();
-	evt->setBrokerSendEvent(client, pingreq);
-	_gateway->getBrokerSendQue()->post(evt);
+    if ((client->isSleep() || client->isAwake()) && client->getClientSleepPacket())
+    {
+        sendStoredPublish(client);
+        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);
+    }
+}
+
+void MQTTSNConnectionHandler::sendStoredPublish(Client* client)
+{
+    MQTTGWPacket* msg = nullptr;
+
+    while ((msg = client->getClientSleepPacket()) != nullptr)
+    {
+        // 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);
+    }
 }
--- a/MQTTSNGateway/src/MQTTSNGWConnectionHandler.h
+++ b/MQTTSNGateway/src/MQTTSNGWConnectionHandler.h
@@ -25,20 +25,21 @@ namespace MQTTSNGW
 class MQTTSNConnectionHandler
 {
 public:
-	MQTTSNConnectionHandler(Gateway* gateway);
-	~MQTTSNConnectionHandler();
-	void sendADVERTISE(void);
-	void handleSearchgw(MQTTSNPacket* packet);
-	void handleConnect(Client* client, MQTTSNPacket* packet);
-	void handleWilltopic(Client* client, MQTTSNPacket* packet);
-	void handleWillmsg(Client* client, MQTTSNPacket* packet);
-	void handleDisconnect(Client* client, MQTTSNPacket* packet);
-	void handleWilltopicupd(Client* client, MQTTSNPacket* packet);
-	void handleWillmsgupd(Client* client, MQTTSNPacket* packet);
-	void handlePingreq(Client* client, MQTTSNPacket* packet);
+    MQTTSNConnectionHandler(Gateway* gateway);
+    ~MQTTSNConnectionHandler();
+    void sendADVERTISE(void);
+    void handleSearchgw(MQTTSNPacket* packet);
+    void handleConnect(Client* client, MQTTSNPacket* packet);
+    void handleWilltopic(Client* client, MQTTSNPacket* packet);
+    void handleWillmsg(Client* client, MQTTSNPacket* packet);
+    void handleDisconnect(Client* client, MQTTSNPacket* packet);
+    void handleWilltopicupd(Client* client, MQTTSNPacket* packet);
+    void handleWillmsgupd(Client* client, MQTTSNPacket* packet);
+    void handlePingreq(Client* client, MQTTSNPacket* packet);
 private:
-	char _pbuf[MQTTSNGW_MAX_PACKET_SIZE * 3];
-	Gateway* _gateway;
+    void sendStoredPublish(Client* client);
+
+    Gateway* _gateway;
 };

 }
--- a/MQTTSNGateway/src/MQTTSNGWDefines.h
+++ b/MQTTSNGateway/src/MQTTSNGWDefines.h
@@ -25,6 +25,8 @@ namespace MQTTSNGW
 #define CONFIG_DIRECTORY "./"
 #define CONFIG_FILE      "gateway.conf"
 #define CLIENT_LIST      "clients.conf"
+#define PREDEFINEDTOPIC_FILE      "predefinedTopic.conf"
+#define FORWARDER_LIST     "forwarders.conf"

 /*==========================================================
 *    Gateway default parameters
@@ -35,30 +37,42 @@ namespace MQTTSNGW
 /*=================================
 *    MQTT-SN Parametrs
 ==================================*/
-#define MAX_CLIENTS                 (100)  // Number of Clients can be handled.
+#define MAX_CLIENTS                 (100)  // Default number of Clients can be handled.
 #define MAX_CLIENTID_LENGTH          (64)  // Max length of clientID
 #define MAX_INFLIGHTMESSAGES         (10)  // Number of inflight messages
-#define MQTTSNGW_MAX_PACKET_SIZE   (1024)  // Max Packet size  (5+2+TopicLen+PayloadLen)
+#define MAX_MESSAGEID_TABLE_SIZE    (500)  // Number of MessageIdTable size
+#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 + Foward Encapsulation)
 #define SIZE_OF_LOG_PACKET          (500)  // Length of the packet log in bytes

+#define PROXY_KEEPALIVE_DURATION   (900)   // Seconds
+#define PROXY_RESPONSE_DURATION     (10)   // Seconds
+#define PROXY_MAX_RETRY_CNT          (3)
+
 /*=================================
 *    Data Type
 ==================================*/
-typedef unsigned char  uint8_t;
+typedef unsigned char uint8_t;
 typedef unsigned short uint16_t;
-typedef unsigned int   uint32_t;
+typedef unsigned int uint32_t;

 /*=================================
 *    Log controls
 ==================================*/
-//#define DEBUG          // print out log for debug
-//#define DEBUG_NWSTACK  // print out SensorNetwork log
-
-#ifdef  DEBUG
+//#define DEBUG_MQTTSN   // print out log for debug
+//#define DEBUG_NW       // print out SensorNetwork log
+#ifdef  DEBUG_MQTTSN
 #define DEBUGLOG(...) printf(__VA_ARGS__)
 #else
 #define DEBUGLOG(...)
 #endif

+#ifdef  DEBUG_NW
+#define D_NWSTACK(...) printf(__VA_ARGS__)
+#else
+#define D_NWSTACK(...)
+#endif
+
 }
 #endif /* MQTTSNGWDEFINES_H_ */
--- a/MQTTSNGateway/src/MQTTSNGWEncapsulatedPacket.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWEncapsulatedPacket.cpp
@@ -0,0 +1,175 @@
+/**************************************************************************************
+ * Copyright (c) 2018, Tomoaki Yamaguchi
+ *
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * and Eclipse Distribution License v1.0 which accompany this distribution.
+ *
+ * The Eclipse Public License is available at
+ *    http://www.eclipse.org/legal/epl-v10.html
+ * and the Eclipse Distribution License is available at
+ *   http://www.eclipse.org/org/documents/edl-v10.php.
+ *
+ * Contributors:
+ *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
+ **************************************************************************************/
+#include "MQTTSNGWPacket.h"
+#include "MQTTSNGWEncapsulatedPacket.h"
+#include "MQTTSNPacket.h"
+#include <string.h>
+
+using namespace MQTTSNGW;
+using namespace std;
+
+WirelessNodeId::WirelessNodeId() :
+        _len { 0 }, _nodeId { 0 }
+{
+
+}
+
+WirelessNodeId::~WirelessNodeId()
+{
+    if (_nodeId)
+    {
+        free(_nodeId);
+    }
+}
+
+void WirelessNodeId::setId(uint8_t* id, uint8_t len)
+{
+    if (_nodeId)
+    {
+        free(_nodeId);
+    }
+    uint8_t* buf = (uint8_t*) malloc(len);
+    if (buf)
+    {
+        memcpy(buf, id, len);
+        _len = len;
+        _nodeId = buf;
+    }
+    else
+    {
+        _nodeId = nullptr;
+        _len = 0;
+    }
+}
+
+void WirelessNodeId::setId(WirelessNodeId* id)
+{
+    setId(id->_nodeId, id->_len);
+}
+
+bool WirelessNodeId::operator ==(WirelessNodeId& id)
+{
+    if (_len == id._len)
+    {
+        return memcmp(_nodeId, id._nodeId, _len) == 0;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+/*
+ *    Class MQTTSNGWEncapsulatedPacket
+ */
+MQTTSNGWEncapsulatedPacket::MQTTSNGWEncapsulatedPacket() :
+        _mqttsn { 0 }, _ctrl { 0 }
+{
+
+}
+
+MQTTSNGWEncapsulatedPacket::MQTTSNGWEncapsulatedPacket(MQTTSNPacket* packet) :
+        _mqttsn { packet }, _ctrl { 0 }
+{
+
+}
+
+MQTTSNGWEncapsulatedPacket::~MQTTSNGWEncapsulatedPacket()
+{
+    /*  Do not delete the MQTTSNPacket.  MQTTSNPacket is deleted by delete Event */
+}
+
+int MQTTSNGWEncapsulatedPacket::unicast(SensorNetwork* network, SensorNetAddress* sendTo)
+{
+    uint8_t buf[MQTTSNGW_MAX_PACKET_SIZE];
+    int len = serialize(buf);
+    return network->unicast(buf, len, sendTo);
+}
+
+int MQTTSNGWEncapsulatedPacket::serialize(uint8_t* buf)
+{
+    int len = 0;
+    buf[0] = _id._len + 3;
+    buf[1] = MQTTSN_ENCAPSULATED;
+    buf[2] = _ctrl;
+    memcpy(buf + 3, _id._nodeId, _id._len);
+    if (_mqttsn)
+    {
+        len = _mqttsn->getPacketLength();
+        memcpy(buf + buf[0], _mqttsn->getPacketData(), len);
+    }
+    return buf[0] + len;
+}
+
+int MQTTSNGWEncapsulatedPacket::desirialize(unsigned char* buf, unsigned short len)
+{
+    if (_mqttsn)
+    {
+        delete _mqttsn;
+        _mqttsn = nullptr;
+    }
+
+    _ctrl = buf[2];
+    _id.setId(buf + 3, buf[0] - 3);
+
+    _mqttsn = new MQTTSNPacket;
+    _mqttsn->desirialize(buf + buf[0], len - buf[0]);
+    return buf[0];
+}
+
+int MQTTSNGWEncapsulatedPacket::getType(void)
+{
+    return MQTTSN_ENCAPSULATED;
+}
+
+const char* MQTTSNGWEncapsulatedPacket::getName()
+{
+    return MQTTSNPacket_name(MQTTSN_ENCAPSULATED);
+}
+
+MQTTSNPacket* MQTTSNGWEncapsulatedPacket::getMQTTSNPacket(void)
+{
+    return _mqttsn;
+}
+
+WirelessNodeId* MQTTSNGWEncapsulatedPacket::getWirelessNodeId(void)
+{
+    return &_id;
+}
+
+void MQTTSNGWEncapsulatedPacket::setWirelessNodeId(WirelessNodeId* id)
+{
+    _id.setId(id);
+}
+
+char* MQTTSNGWEncapsulatedPacket::print(char* pbuf)
+{
+    char* ptr = pbuf;
+    char** pptr = &pbuf;
+
+    uint8_t buf[MQTTSNGW_MAX_PACKET_SIZE];
+    int len = serialize(buf);
+    int size = len > SIZE_OF_LOG_PACKET ? SIZE_OF_LOG_PACKET : len;
+
+    for (int i = 1; i < size; i++)
+    {
+        sprintf(*pptr, " %02X", *(buf + i));
+        *pptr += 3;
+    }
+    **pptr = 0;
+    return ptr;
+}
+
--- a/MQTTSNGateway/src/MQTTSNGWEncapsulatedPacket.h
+++ b/MQTTSNGateway/src/MQTTSNGWEncapsulatedPacket.h
@@ -0,0 +1,63 @@
+/**************************************************************************************
+ * 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_ */
--- a/MQTTSNGateway/src/MQTTSNGWForwarder.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWForwarder.cpp
@@ -0,0 +1,277 @@
+/**************************************************************************************
+ * 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 "MQTTSNGWForwarder.h"
+#include "SensorNetwork.h"
+
+#include <string.h>
+
+using namespace MQTTSNGW;
+using namespace std;
+
+/*=====================================
+ Class ForwarderList
+ =====================================*/
+
+ForwarderList::ForwarderList()
+{
+    _head = nullptr;
+}
+
+ForwarderList::~ForwarderList()
+{
+    if (_head)
+    {
+        Forwarder* p = _head;
+        while (p)
+        {
+            Forwarder* next = p->_next;
+            delete p;
+            p = next;
+        }
+    }
+}
+
+void ForwarderList::initialize(Gateway* gw)
+{
+    /* Create Fowarders from clients.conf */
+    gw->getClientList()->setClientList(FORWARDER_TYPE);
+}
+
+Forwarder* ForwarderList::getForwarder(SensorNetAddress* addr)
+{
+    Forwarder* p = _head;
+    while (p)
+    {
+        if (p->_sensorNetAddr.isMatch(addr))
+        {
+            break;
+        }
+        p = p->_next;
+    }
+    return p;
+}
+
+Forwarder* ForwarderList::addForwarder(SensorNetAddress* addr, MQTTSNString* forwarderId)
+{
+    Forwarder* fdr = new Forwarder(addr, forwarderId);
+    if (_head == nullptr)
+    {
+        _head = fdr;
+    }
+    else
+    {
+        Forwarder* p = _head;
+        while (p)
+        {
+            if (p->_next == nullptr)
+            {
+                p->_next = fdr;
+                break;
+            }
+            else
+            {
+                p = p->_next;
+            }
+        }
+    }
+    return fdr;
+}
+
+Forwarder::Forwarder()
+{
+    _headClient = nullptr;
+    _next = nullptr;
+}
+
+/*=====================================
+ Class ForwarderList
+ =====================================*/
+
+Forwarder::Forwarder(SensorNetAddress* addr, MQTTSNString* forwarderId)
+{
+    _forwarderName = string(forwarderId->cstring);
+    _sensorNetAddr = *addr;
+    _headClient = nullptr;
+    _next = nullptr;
+}
+
+Forwarder::~Forwarder(void)
+{
+    if (_headClient)
+    {
+        ForwarderElement* p = _headClient;
+        while (p)
+        {
+            ForwarderElement* next = p->_next;
+            delete p;
+            p = next;
+        }
+    }
+}
+
+const char* Forwarder::getId(void)
+{
+    return _forwarderName.c_str();
+}
+
+void Forwarder::addClient(Client* client, WirelessNodeId* id)
+{
+    ForwarderElement* p = _headClient;
+    ForwarderElement* prev = nullptr;
+
+    client->setForwarder(this);
+
+    if (p != nullptr)
+    {
+        while (p)
+        {
+            if (p->_client == client)
+            {
+                client->setForwarder(this);
+                p->setWirelessNodeId(id);
+                return;
+            }
+            prev = p;
+            p = p->_next;
+        }
+    }
+
+    ForwarderElement* fclient = new ForwarderElement();
+
+    fclient->setClient(client);
+    fclient->setWirelessNodeId(id);
+
+    if (prev)
+    {
+        prev->_next = fclient;
+    }
+    else
+    {
+        _headClient = fclient;
+    }
+}
+
+Client* Forwarder::getClient(WirelessNodeId* id)
+{
+    Client* cl = nullptr;
+    _mutex.lock();
+    ForwarderElement* 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 = nullptr;
+    _mutex.lock();
+    ForwarderElement* 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)
+{
+    ForwarderElement* prev = nullptr;
+    _mutex.lock();
+    ForwarderElement* 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
+ */
+
+ForwarderElement::ForwarderElement() :
+        _client { 0 }, _wirelessNodeId { 0 }, _next { 0 }
+{
+}
+
+ForwarderElement::~ForwarderElement()
+{
+    if (_wirelessNodeId)
+    {
+        delete _wirelessNodeId;
+    }
+}
+
+void ForwarderElement::setClient(Client* client)
+{
+    _client = client;
+}
+
+void ForwarderElement::setWirelessNodeId(WirelessNodeId* id)
+{
+    if (_wirelessNodeId == nullptr)
+    {
+        _wirelessNodeId = new WirelessNodeId();
+    }
+    _wirelessNodeId->setId(id);
+}
--- a/MQTTSNGateway/src/MQTTSNGWForwarder.h
+++ b/MQTTSNGateway/src/MQTTSNGWForwarder.h
@@ -0,0 +1,96 @@
+/**************************************************************************************
+ * 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 "MQTTSNGateway.h"
+#include "MQTTSNGWEncapsulatedPacket.h"
+#include "SensorNetwork.h"
+
+namespace MQTTSNGW
+{
+class Gateway;
+class Client;
+class WirelessNodeId;
+
+/*=====================================
+ Class ForwarderElement
+ =====================================*/
+class ForwarderElement
+{
+    friend class Forwarder;
+public:
+    ForwarderElement();
+    ~ForwarderElement();
+
+    void setClient(Client* client);
+    void setWirelessNodeId(WirelessNodeId* id);
+private:
+    Client* _client;
+    WirelessNodeId* _wirelessNodeId;
+    ForwarderElement* _next;
+};
+
+/*=====================================
+ Class Forwarder
+ =====================================*/
+class Forwarder
+{
+    friend class ForwarderList;
+public:
+    Forwarder(void);
+    Forwarder(SensorNetAddress* addr, MQTTSNString* forwarderId);
+    ~Forwarder();
+
+    void initialize(void);
+    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;
+    ForwarderElement* _headClient { nullptr };
+    Forwarder* _next { nullptr };
+    Mutex _mutex;
+};
+
+/*=====================================
+ Class ForwarderList
+ =====================================*/
+class ForwarderList
+{
+public:
+    ForwarderList();
+    ~ForwarderList();
+
+    void initialize(Gateway* gw);
+    Forwarder* getForwarder(SensorNetAddress* addr);
+    Forwarder* addForwarder(SensorNetAddress* addr, MQTTSNString* forwarderId);
+
+private:
+    Forwarder* _head;
+};
+
+}
+
+#endif /* MQTTSNGATEWAY_SRC_MQTTSNGWFORWARDER_H_ */
--- a/MQTTSNGateway/src/MQTTSNGWLogmonitor.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWLogmonitor.cpp
@@ -13,8 +13,6 @@
 * Contributors:
 *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
 **************************************************************************************/
-#define RINGBUFFER
-
 #include "MQTTSNGWProcess.h"
 #include "MQTTSNGWLogmonitor.h"
 #include <stdio.h>
@@ -24,7 +22,7 @@ using namespace MQTTSNGW;

 Logmonitor::Logmonitor()
 {
-	theProcess = this;
+    theProcess = this;
 }

 Logmonitor::~Logmonitor()
@@ -34,17 +32,17 @@ Logmonitor::~Logmonitor()

 void Logmonitor::run()
 {
-	while (true)
-	{
-		const char* data = getLog();
-		if ( *data == 0 )
-		{
-			break;
-		}
-		else
-		{
-			printf("%s", data);
-		}
-	}
+    while (true)
+    {
+        const char* data = getLog();
+        if (*data == 0)
+        {
+            break;
+        }
+        else
+        {
+            printf("%s", data);
+        }
+    }
 }

--- a/MQTTSNGateway/src/MQTTSNGWLogmonitor.h
+++ b/MQTTSNGateway/src/MQTTSNGWLogmonitor.h
@@ -23,9 +23,9 @@ namespace MQTTSNGW
 class Logmonitor: public Process
 {
 public:
-	Logmonitor();
-	~Logmonitor();
-	void run();
+    Logmonitor();
+    ~Logmonitor();
+    void run();
 };

 }
--- a/MQTTSNGateway/src/MQTTSNGWMessageIdTable.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWMessageIdTable.cpp
@@ -0,0 +1,212 @@
+/**************************************************************************************
+ * 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 "MQTTSNGWMessageIdTable.h"
+#include "MQTTSNGWClient.h"
+
+using namespace MQTTSNGW;
+
+/*===============================
+ * Class MessageIdTable
+ ===============================*/
+MessageIdTable::MessageIdTable()
+{
+
+}
+
+MessageIdTable::~MessageIdTable()
+{
+    _mutex.lock();
+    if (_head != nullptr)
+    {
+        MessageIdElement* p = _tail;
+        while (p)
+        {
+            MessageIdElement* pPrev = p;
+            delete p;
+            _cnt--;
+            p = pPrev->_prev;
+        }
+        _head = _tail = nullptr;
+    }
+    _mutex.unlock();
+}
+
+MessageIdElement* MessageIdTable::add(Aggregater* aggregater, Client* client, uint16_t clientMsgId)
+{
+    if (_cnt > _maxSize)
+    {
+        return nullptr;
+    }
+
+    MessageIdElement* elm = new MessageIdElement(0, client, clientMsgId);
+    if (elm == nullptr)
+    {
+        return nullptr;
+    }
+    _mutex.lock();
+    if (_head == nullptr)
+    {
+        elm->_msgId = aggregater->msgId();
+        _head = elm;
+        _tail = elm;
+        _cnt++;
+    }
+    else
+    {
+        MessageIdElement* p = find(client, clientMsgId);
+        if (p == nullptr)
+        {
+            elm->_msgId = aggregater->msgId();
+            p = _tail;
+            _tail = elm;
+            elm->_prev = p;
+            p->_next = elm;
+            _cnt++;
+        }
+        else
+        {
+            delete elm;
+            elm = nullptr;
+        }
+    }
+    _mutex.unlock();
+    return elm;
+}
+
+MessageIdElement* MessageIdTable::find(uint16_t msgId)
+{
+    MessageIdElement* p = _head;
+    while (p)
+    {
+        if (p->_msgId == msgId)
+        {
+            break;
+        }
+        p = p->_next;
+    }
+    return p;
+}
+
+MessageIdElement* MessageIdTable::find(Client* client, uint16_t clientMsgId)
+{
+    MessageIdElement* p = _head;
+    while (p)
+    {
+        if (p->_clientMsgId == clientMsgId && p->_client == client)
+        {
+            break;
+        }
+        p = p->_next;
+    }
+    return p;
+}
+
+Client* MessageIdTable::getClientMsgId(uint16_t msgId, uint16_t* clientMsgId)
+{
+    Client* clt = nullptr;
+    *clientMsgId = 0;
+    _mutex.lock();
+    MessageIdElement* p = find(msgId);
+    if (p != nullptr)
+    {
+        clt = p->_client;
+        *clientMsgId = p->_clientMsgId;
+        clear(p);
+    }
+    _mutex.unlock();
+    return clt;
+}
+
+void MessageIdTable::erase(uint16_t msgId)
+{
+    _mutex.lock();
+    MessageIdElement* p = find(msgId);
+    clear(p);
+    _mutex.unlock();
+}
+
+void MessageIdTable::clear(MessageIdElement* elm)
+{
+    if (elm == nullptr)
+    {
+        return;
+    }
+
+    if (elm->_prev == nullptr)
+    {
+        _head = elm->_next;
+        if (_head == nullptr)
+        {
+            _tail = nullptr;
+        }
+        else
+        {
+            _head->_prev = nullptr;
+        }
+        delete elm;
+        _cnt--;
+        return;
+    }
+    else
+    {
+        elm->_prev->_next = elm->_next;
+        if (elm->_next == nullptr)
+        {
+            _tail = elm->_prev;
+        }
+        else
+        {
+            elm->_next->_prev = elm->_prev;
+        }
+        delete elm;
+        _cnt--;
+        return;
+    }
+}
+
+uint16_t MessageIdTable::getMsgId(Client* client, uint16_t clientMsgId)
+{
+    uint16_t msgId = 0;
+    MessageIdElement* p = find(client, clientMsgId);
+    if (p != nullptr)
+    {
+        msgId = p->_msgId;
+    }
+    return msgId;
+}
+
+/*===============================
+ * Class MessageIdElement
+ ===============================*/
+MessageIdElement::MessageIdElement(void) :
+        _msgId { 0 }, _clientMsgId { 0 }, _client { nullptr }, _next { nullptr }, _prev { nullptr }
+{
+
+}
+
+MessageIdElement::MessageIdElement(uint16_t msgId, Client* client, uint16_t clientMsgId) :
+        MessageIdElement()
+{
+    _msgId = msgId;
+    _client = client;
+    _clientMsgId = clientMsgId;
+}
+
+MessageIdElement::~MessageIdElement(void)
+{
+
+}
--- a/MQTTSNGateway/src/MQTTSNGWMessageIdTable.h
+++ b/MQTTSNGateway/src/MQTTSNGWMessageIdTable.h
@@ -0,0 +1,78 @@
+/**************************************************************************************
+ * 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_MQTTSNGWMESSAGEIDTABLE_H_
+#define MQTTSNGATEWAY_SRC_MQTTSNGWMESSAGEIDTABLE_H_
+
+#include "MQTTSNGWDefines.h"
+#include "MQTTSNGWProcess.h"
+
+#include <stdint.h>
+namespace MQTTSNGW
+{
+
+class Client;
+class MessageIdElement;
+class Meutex;
+class Aggregater;
+/*=====================================
+ Class MessageIdTable
+ ======================================*/
+class MessageIdTable
+{
+public:
+    MessageIdTable();
+    ~MessageIdTable();
+
+    MessageIdElement* add(Aggregater* aggregater, Client* client,
+            uint16_t clientMsgId);
+    Client* getClientMsgId(uint16_t msgId, uint16_t* clientMsgId);
+    uint16_t getMsgId(Client* client, uint16_t clientMsgId);
+    void erase(uint16_t msgId);
+    void clear(MessageIdElement* elm);
+private:
+    MessageIdElement* find(uint16_t msgId);
+    MessageIdElement* find(Client* client, uint16_t clientMsgId);
+    MessageIdElement* _head { nullptr };
+    MessageIdElement* _tail{ nullptr };
+    int _cnt { 0 };
+    int _maxSize { MAX_MESSAGEID_TABLE_SIZE };
+    Mutex _mutex;
+};
+
+/*=====================================
+ Class MessageIdElement
+ =====================================*/
+class MessageIdElement
+{
+    friend class MessageIdTable;
+    friend class Aggregater;
+public:
+    MessageIdElement(void);
+    MessageIdElement(uint16_t msgId, Client* client, uint16_t clientMsgId);
+    ~MessageIdElement(void);
+
+private:
+    uint16_t _msgId;
+    uint16_t _clientMsgId;
+    Client* _client;
+    MessageIdElement* _next;
+    MessageIdElement* _prev;
+};
+
+}
+
+#endif /* MQTTSNGATEWAY_SRC_MQTTSNGWMESSAGEIDTABLE_H_ */
--- a/MQTTSNGateway/src/MQTTSNGWPacket.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWPacket.cpp
@@ -24,390 +24,553 @@

 using namespace std;
 using namespace MQTTSNGW;
+int readInt(char** pptr);
+void writeInt(unsigned char** pptr, int msgId);

-MQTTSNPacket::MQTTSNPacket()
+MQTTSNPacket::MQTTSNPacket(void)
 {
-	_buf = 0;
-	_bufLen = 0;
+    _buf = nullptr;
+    _bufLen = 0;
 }
+
+MQTTSNPacket::MQTTSNPacket(MQTTSNPacket& packet)
+{
+    _buf = (unsigned char*) malloc(packet._bufLen);
+    if (_buf)
+    {
+        _bufLen = packet._bufLen;
+        memcpy(_buf, packet._buf, _bufLen);
+    }
+    else
+    {
+        _buf = nullptr;
+        _bufLen = 0;
+    }
+}
+
 MQTTSNPacket::~MQTTSNPacket()
 {
-	if (_buf)
-	{
-		free(_buf);
-	}
+    if (_buf)
+    {
+        free(_buf);
+    }
 }

 int MQTTSNPacket::unicast(SensorNetwork* network, SensorNetAddress* sendTo)
 {
-	return network->unicast(_buf, _bufLen, sendTo);
+    return network->unicast(_buf, _bufLen, sendTo);
 }

 int MQTTSNPacket::broadcast(SensorNetwork* network)
 {
-	return network->broadcast(_buf, _bufLen);
+    return network->broadcast(_buf, _bufLen);
 }

 int MQTTSNPacket::serialize(uint8_t* buf)
 {
-	buf = _buf;
-	return _bufLen;
+    buf = _buf;
+    return _bufLen;
 }

 int MQTTSNPacket::desirialize(unsigned char* buf, unsigned short len)
 {
-	if ( _buf )
-	{
-		free(_buf);
-	}
+    if (_buf)
+    {
+        free(_buf);
+    }

-	_buf = (unsigned char*)calloc(len, sizeof(unsigned char));
-	if ( _buf )
-	{
-		memcpy(_buf, buf, len);
-		_bufLen = len;
-	}
-	else
-	{
-		_bufLen = 0;
-	}
-	return _bufLen;
+    _buf = (unsigned char*) calloc(len, sizeof(unsigned char));
+    if (_buf)
+    {
+        memcpy(_buf, buf, len);
+        _bufLen = len;
+    }
+    else
+    {
+        _bufLen = 0;
+    }
+    return _bufLen;
 }

 int MQTTSNPacket::recv(SensorNetwork* network)
 {
-	uint8_t buf[MQTTSNGW_MAX_PACKET_SIZE];
-	int len = network->read((uint8_t*) buf, MQTTSNGW_MAX_PACKET_SIZE);
-	if (len > 1)
-	{
-		len = desirialize(buf, len);
-	}
-	else
-	{
-		len = 0;
-	}
-	return len;
+    uint8_t buf[MQTTSNGW_MAX_PACKET_SIZE];
+    int len = network->read((uint8_t*) buf, MQTTSNGW_MAX_PACKET_SIZE);
+    if (len > 1)
+    {
+        len = desirialize(buf, len);
+    }
+    return len;

 }

 int MQTTSNPacket::getType(void)
 {
-	if ( _bufLen == 0 )
-	{
-		return 0;
-	}
-	int value = 0;
-	int p = MQTTSNPacket_decode(_buf, _bufLen, &value);
-	return _buf[p];
+    if (_bufLen == 0)
+    {
+        return 0;
+    }
+    int value = 0;
+    int p = MQTTSNPacket_decode(_buf, _bufLen, &value);
+    return _buf[p];
+}
+
+bool MQTTSNPacket::isQoSMinusPUBLISH(void)
+{
+    if (_bufLen == 0)
+    {
+        return false;;
+    }
+    int value = 0;
+    int p = MQTTSNPacket_decode(_buf, _bufLen, &value);
+    return ((_buf[p] == MQTTSN_PUBLISH) && ((_buf[p + 1] & 0x60) == 0x60));
 }

 unsigned char* MQTTSNPacket::getPacketData(void)
 {
-	return _buf;
+    return _buf;
 }

 int MQTTSNPacket::getPacketLength(void)
 {
-	return _bufLen;
+    return _bufLen;
 }

 const char* MQTTSNPacket::getName()
 {
-	return MQTTSNPacket_name(getType());
+    return MQTTSNPacket_name(getType());
 }

 int MQTTSNPacket::setADVERTISE(uint8_t gatewayid, uint16_t duration)
 {
-	unsigned char buf[5];
-	int len = MQTTSNSerialize_advertise(buf, 5, (unsigned char) gatewayid,
-			(unsigned short) duration);
-	return desirialize(buf, len);
+    unsigned char buf[5];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_advertise(buf, buflen, (unsigned char) gatewayid, (unsigned short) duration);
+    return desirialize(buf, len);
 }

 int MQTTSNPacket::setGWINFO(uint8_t gatewayId)
 {
-	unsigned char buf[3];
-	int len = MQTTSNSerialize_gwinfo(buf, 3, (unsigned char) gatewayId, 0, 0);
-	return desirialize(buf, len);
+    unsigned char buf[3];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_gwinfo(buf, buflen, (unsigned char) gatewayId, 0, 0);
+    return desirialize(buf, len);
 }

 int MQTTSNPacket::setConnect(void)
 {
-	unsigned char buf[40];
-	MQTTSNPacket_connectData data;
-	data.clientID.cstring = (char*)"client01";
-	int len = MQTTSNSerialize_connect(buf, 40, &data);
-	return desirialize(buf, len);
+    unsigned char buf[40];
+    int buflen = sizeof(buf);
+    MQTTSNPacket_connectData data;
+    data.clientID.cstring = (char*) "client01";
+    int len = MQTTSNSerialize_connect(buf, buflen, &data);
+    return desirialize(buf, len);
+}
+
+bool MQTTSNPacket::isAccepted(void)
+{
+    return (getType() == MQTTSN_CONNACK) && (_buf[2] == MQTTSN_RC_ACCEPTED);
 }

 int MQTTSNPacket::setCONNACK(uint8_t returnCode)
 {
-	unsigned char buf[3];
-	int len = MQTTSNSerialize_connack(buf, 3, (int) returnCode);
-	return desirialize(buf, len);
+    unsigned char buf[3];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_connack(buf, buflen, (int) returnCode);
+    return desirialize(buf, len);
 }

 int MQTTSNPacket::setWILLTOPICREQ(void)
 {
-	unsigned char buf[2];
-	int len = MQTTSNSerialize_willtopicreq(buf, 2);
-	return desirialize(buf, len);
+    unsigned char buf[2];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_willtopicreq(buf, buflen);
+    return desirialize(buf, len);
 }

 int MQTTSNPacket::setWILLMSGREQ(void)
 {
-	unsigned char buf[2];
-	int len = MQTTSNSerialize_willmsgreq(buf, 2);
-	return desirialize(buf, len);
+    unsigned char buf[2];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_willmsgreq(buf, buflen);
+    return desirialize(buf, len);
 }

 int MQTTSNPacket::setREGISTER(uint16_t topicId, uint16_t msgId, MQTTSNString* topicName)
 {
-	unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
-	int len = MQTTSNSerialize_register(buf, MQTTSNGW_MAX_PACKET_SIZE, (unsigned short) topicId, (unsigned short) msgId,
-			topicName);
-	return desirialize(buf, len);
+    unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_register(buf, buflen, (unsigned short) topicId, (unsigned short) msgId, topicName);
+    return desirialize(buf, len);
 }

 int MQTTSNPacket::setREGACK(uint16_t topicId, uint16_t msgId, uint8_t returnCode)
 {
-	unsigned char buf[7];
-	int len = MQTTSNSerialize_regack(buf, 7, (unsigned short) topicId, (unsigned short) msgId,
-			(unsigned char) returnCode);
-	return desirialize(buf, len);
+    unsigned char buf[7];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_regack(buf, buflen, (unsigned short) topicId, (unsigned short) msgId, (unsigned char) returnCode);
+    return desirialize(buf, len);
 }

-int MQTTSNPacket::setPUBLISH(uint8_t dup, int qos, uint8_t retained, uint16_t msgId, MQTTSN_topicid topic,
-		uint8_t* payload, uint16_t payloadlen)
+int MQTTSNPacket::setPUBLISH(uint8_t dup, int qos, uint8_t retained, uint16_t msgId, MQTTSN_topicid topic, uint8_t* payload,
+        uint16_t payloadlen)
 {
-	unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
-	int len = MQTTSNSerialize_publish(buf, MQTTSNGW_MAX_PACKET_SIZE, (unsigned char) dup, qos, (unsigned char) retained,
-			(unsigned short) msgId, topic, (unsigned char*) payload, (int) payloadlen);
-	return desirialize(buf, len);
+    unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_publish(buf, buflen, (unsigned char) dup, qos, (unsigned char) retained, (unsigned short) msgId,
+            topic, (unsigned char*) payload, (int) payloadlen);
+    return desirialize(buf, len);
 }

 int MQTTSNPacket::setPUBACK(uint16_t topicId, uint16_t msgId, uint8_t returnCode)
 {
-	unsigned char buf[7];
-	int len = MQTTSNSerialize_puback(buf, 7, (unsigned short) topicId, (unsigned short) msgId,
-			(unsigned char) returnCode);
-	return desirialize(buf, len);
+    unsigned char buf[7];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_puback(buf, buflen, (unsigned short) topicId, (unsigned short) msgId, (unsigned char) returnCode);
+    return desirialize(buf, len);
 }

 int MQTTSNPacket::setPUBREC(uint16_t msgId)
 {
-	unsigned char buf[4];
-	int len = MQTTSNSerialize_pubrec(buf, 4, (unsigned short) msgId);
-	return desirialize(buf, len);
+    unsigned char buf[4];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_pubrec(buf, buflen, (unsigned short) msgId);
+    return desirialize(buf, len);
 }

 int MQTTSNPacket::setPUBREL(uint16_t msgId)
 {
-	unsigned char buf[4];
-	int len = MQTTSNSerialize_pubrel(buf, 4, (unsigned short) msgId);
-	return desirialize(buf, len);
+    unsigned char buf[4];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_pubrel(buf, buflen, (unsigned short) msgId);
+    return desirialize(buf, len);
 }

 int MQTTSNPacket::setPUBCOMP(uint16_t msgId)
 {
-	unsigned char buf[4];
-	int len = MQTTSNSerialize_pubcomp(buf, 4, (unsigned short) msgId);
-	return desirialize(buf, len);
+    unsigned char buf[4];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_pubcomp(buf, buflen, (unsigned short) msgId);
+    return desirialize(buf, len);
 }

 int MQTTSNPacket::setSUBACK(int qos, uint16_t topicId, uint16_t msgId, uint8_t returnCode)
 {
-	unsigned char buf[8];
-	int len = MQTTSNSerialize_suback(buf, 8, qos, (unsigned short) topicId,
-			(unsigned short) msgId, (unsigned char) returnCode);
-	return desirialize(buf, len);
+    unsigned char buf[8];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_suback(buf, buflen, qos, (unsigned short) topicId, (unsigned short) msgId,
+            (unsigned char) returnCode);
+    return desirialize(buf, len);
 }

 int MQTTSNPacket::setUNSUBACK(uint16_t msgId)
 {
-	unsigned char buf[4];
-	int len = MQTTSNSerialize_unsuback(buf, 4, (unsigned short) msgId);
-	return desirialize(buf, len);
+    unsigned char buf[4];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_unsuback(buf, buflen, (unsigned short) msgId);
+    return desirialize(buf, len);
 }

 int MQTTSNPacket::setPINGRESP(void)
 {
-	unsigned char buf[32];
-	int len = MQTTSNSerialize_pingresp(buf, 32);
-	return desirialize(buf, len);
+    unsigned char buf[32];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_pingresp(buf, buflen);
+    return desirialize(buf, len);
 }

 int MQTTSNPacket::setDISCONNECT(uint16_t duration)
 {
-	unsigned char buf[4];
-	int len = MQTTSNSerialize_disconnect(buf, 4, (int) duration);
-	return desirialize(buf, len);
+    unsigned char buf[4];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_disconnect(buf, buflen, (int) duration);
+    return desirialize(buf, len);
 }

 int MQTTSNPacket::setWILLTOPICRESP(uint8_t returnCode)
 {
-	unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
-	int len = MQTTSNSerialize_willtopicresp(buf, MQTTSNGW_MAX_PACKET_SIZE, (int) returnCode);
-	return desirialize(buf, len);
+    unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_willtopicresp(buf, buflen, (int) returnCode);
+    return desirialize(buf, len);
 }

 int MQTTSNPacket::setWILLMSGRESP(uint8_t returnCode)
 {
-	unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
-	int len = MQTTSNSerialize_willmsgresp(buf, MQTTSNGW_MAX_PACKET_SIZE, (int) returnCode);
-	return desirialize(buf, len);
+    unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_willmsgresp(buf, buflen, (int) returnCode);
+    return desirialize(buf, len);
+}
+
+int MQTTSNPacket::setCONNECT(MQTTSNPacket_connectData* options)
+{
+    unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_connect(buf, buflen, options);
+    return desirialize(buf, len);
+}
+
+int MQTTSNPacket::setPINGREQ(MQTTSNString* clientId)
+{
+    unsigned char buf[MQTTSNGW_MAX_PACKET_SIZE];
+    int buflen = sizeof(buf);
+    int len = MQTTSNSerialize_pingreq(buf, buflen, *clientId);
+    return desirialize(buf, len);
 }

 int MQTTSNPacket::getSERCHGW(uint8_t* radius)
 {
-	return MQTTSNDeserialize_searchgw((unsigned char*) radius, (unsigned char*) _buf, _bufLen);
+    return MQTTSNDeserialize_searchgw((unsigned char*) radius, (unsigned char*) _buf, _bufLen);
 }

 int MQTTSNPacket::getCONNECT(MQTTSNPacket_connectData* data)
 {
-	return MQTTSNDeserialize_connect(data, _buf, _bufLen);
+    return MQTTSNDeserialize_connect(data, _buf, _bufLen);
 }

 int MQTTSNPacket::getCONNACK(uint8_t* returnCode)
 {
-	return MQTTSNSerialize_connack(_buf, _bufLen, (int) *returnCode);
+    return MQTTSNSerialize_connack(_buf, _bufLen, (int) *returnCode);
 }

 int MQTTSNPacket::getWILLTOPIC(int* willQoS, uint8_t* willRetain, MQTTSNString* willTopic)
 {
-	return MQTTSNDeserialize_willtopic((int*) willQoS, (unsigned char*) willRetain, willTopic, _buf, _bufLen);
+    return MQTTSNDeserialize_willtopic((int*) willQoS, (unsigned char*) willRetain, willTopic, _buf, _bufLen);
 }

 int MQTTSNPacket::getWILLMSG(MQTTSNString* willmsg)
 {
-	return MQTTSNDeserialize_willmsg(willmsg, _buf, _bufLen);
+    return MQTTSNDeserialize_willmsg(willmsg, _buf, _bufLen);
 }

 int MQTTSNPacket::getREGISTER(uint16_t* topicId, uint16_t* msgId, MQTTSNString* topicName)
 {
-	return MQTTSNDeserialize_register((unsigned short*) topicId, (unsigned short*) msgId, topicName,
-			_buf, _bufLen);
+    return MQTTSNDeserialize_register((unsigned short*) topicId, (unsigned short*) msgId, topicName, _buf, _bufLen);
 }

 int MQTTSNPacket::getREGACK(uint16_t* topicId, uint16_t* msgId, uint8_t* returnCode)
 {
-	return MQTTSNDeserialize_regack((unsigned short*) topicId, (unsigned short*) msgId, (unsigned char*) returnCode, _buf, _bufLen);
+    return MQTTSNDeserialize_regack((unsigned short*) topicId, (unsigned short*) msgId, (unsigned char*) returnCode, _buf,
+            _bufLen);
 }

 int MQTTSNPacket::getPUBLISH(uint8_t* dup, int* qos, uint8_t* retained, uint16_t* msgId, MQTTSN_topicid* topic,
-		uint8_t** payload, int* payloadlen)
+        uint8_t** payload, int* payloadlen)
 {
-	return MQTTSNDeserialize_publish((unsigned char*) dup, qos, (unsigned char*) retained, (unsigned short*) msgId,
-			topic, (unsigned char**) payload, (int*) payloadlen, _buf, _bufLen);
+    return MQTTSNDeserialize_publish((unsigned char*) dup, qos, (unsigned char*) retained, (unsigned short*) msgId, topic,
+            (unsigned char**) payload, (int*) payloadlen, _buf, _bufLen);
 }

 int MQTTSNPacket::getPUBACK(uint16_t* topicId, uint16_t* msgId, uint8_t* returnCode)
 {
-	return MQTTSNDeserialize_puback((unsigned short*) topicId, (unsigned short*) msgId, (unsigned char*) returnCode,
-			_buf, _bufLen);
+    return MQTTSNDeserialize_puback((unsigned short*) topicId, (unsigned short*) msgId, (unsigned char*) returnCode, _buf,
+            _bufLen);
 }

 int MQTTSNPacket::getACK(uint16_t* msgId)
 {
-	unsigned char type;
-	return MQTTSNDeserialize_ack(&type, (unsigned short*) msgId, _buf, _bufLen);
+    unsigned char type;
+    return MQTTSNDeserialize_ack(&type, (unsigned short*) msgId, _buf, _bufLen);
 }

 int MQTTSNPacket::getSUBSCRIBE(uint8_t* dup, int* qos, uint16_t* msgId, MQTTSN_topicid* topicFilter)
 {
-	return MQTTSNDeserialize_subscribe((unsigned char*) dup, qos, (unsigned short*) msgId, topicFilter,	_buf, _bufLen);
+    return MQTTSNDeserialize_subscribe((unsigned char*) dup, qos, (unsigned short*) msgId, topicFilter, _buf, _bufLen);
 }

 int MQTTSNPacket::getUNSUBSCRIBE(uint16_t* msgId, MQTTSN_topicid* topicFilter)
 {
-	return MQTTSNDeserialize_unsubscribe((unsigned short*) msgId, topicFilter, _buf, _bufLen);
+    return MQTTSNDeserialize_unsubscribe((unsigned short*) msgId, topicFilter, _buf, _bufLen);
 }

 int MQTTSNPacket::getPINGREQ(void)
 {
-	if (getType() == MQTTSN_PINGRESP && _bufLen > 2 )
-	{
-		return _bufLen - 2;
-	}
-	return 0;
+    if (getType() == MQTTSN_PINGRESP && _bufLen > 2)
+    {
+        return _bufLen - 2;
+    }
+    return 0;
 }

 int MQTTSNPacket::getDISCONNECT(uint16_t* duration)
 {
-	int dur = 0;
-	int rc = MQTTSNDeserialize_disconnect(&dur, _buf, _bufLen);
-	*duration = (uint16_t)dur;
-	return rc;
+    int dur = 0;
+    int rc = MQTTSNDeserialize_disconnect(&dur, _buf, _bufLen);
+    *duration = (uint16_t) dur;
+    return rc;
 }

 int MQTTSNPacket::getWILLTOPICUPD(uint8_t* willQoS, uint8_t* willRetain, MQTTSNString* willTopic)
 {
-	return MQTTSNDeserialize_willtopicupd((int*) willQoS, (unsigned char*) willRetain, willTopic, _buf, _bufLen);
+    return MQTTSNDeserialize_willtopicupd((int*) willQoS, (unsigned char*) willRetain, willTopic, _buf, _bufLen);
 }

 int MQTTSNPacket::getWILLMSGUPD(MQTTSNString* willMsg)
 {
-	return MQTTSNDeserialize_willmsgupd(willMsg, _buf, _bufLen);
+    return MQTTSNDeserialize_willmsgupd(willMsg, _buf, _bufLen);
 }

 char* MQTTSNPacket::print(char* pbuf)
 {
-	char* ptr = pbuf;
-	char** pptr = &pbuf;
-	int value = 0;
+    char* ptr = pbuf;
+    char** pptr = &pbuf;
+    int size = _bufLen > SIZE_OF_LOG_PACKET ? SIZE_OF_LOG_PACKET : _bufLen;

-	int i = MQTTSNPacket_decode(_buf, _bufLen, &value);
-	int size = _bufLen > SIZE_OF_LOG_PACKET ? SIZE_OF_LOG_PACKET : _bufLen;
-
-	for (; i < size; i++)
-	{
-		sprintf(*pptr, " %02X", *(_buf + i));
-		*pptr += 3;
-	}
-	**pptr = 0;
-	return ptr;
+    for (int i = 0; i < size; i++)
+    {
+        sprintf(*pptr, " %02X", *(_buf + i));
+        *pptr += 3;
+    }
+    **pptr = 0;
+    return ptr;
 }

 char* MQTTSNPacket::getMsgId(char* pbuf)
 {
-	int value = 0;
-	int p = 0;
+    int value = 0;
+    int p = 0;

-	switch ( getType() )
-	{
-	case MQTTSN_PUBLISH:
-		p = MQTTSNPacket_decode(_buf, _bufLen, &value);
-		if ( _buf[p + 1] & 0x80 )
-		{
-			sprintf(pbuf, "+%02X%02X", _buf[p + 4], _buf[p + 5]);
-		}
-		else
-		{
-			sprintf(pbuf, " %02X%02X", _buf[p + 4], _buf[p + 5]);
-		}
-		break;
-	case MQTTSN_PUBACK:
-	case MQTTSN_REGISTER:
-	case MQTTSN_REGACK:
-		sprintf(pbuf, " %02X%02X", _buf[4], _buf[5]);
-		break;
-	case MQTTSN_PUBREC:
-	case MQTTSN_PUBREL:
-	case MQTTSN_PUBCOMP:
-	case MQTTSN_UNSUBACK:
-		sprintf(pbuf, " %02X%02X", _buf[2], _buf[3]);
-		break;
-	case MQTTSN_SUBSCRIBE:
-	case MQTTSN_UNSUBSCRIBE:
-		p = MQTTSNPacket_decode(_buf, _bufLen, &value);
-		sprintf(pbuf, " %02X%02X", _buf[p + 2], _buf[p + 3]);
-		break;
-	case MQTTSN_SUBACK:
-		sprintf(pbuf, " %02X%02X", _buf[5], _buf[6]);
-		break;
-	default:
-		sprintf(pbuf, "    ");
-		break;
-	}
-	return pbuf;
+    switch (getType())
+    {
+    case MQTTSN_PUBLISH:
+        p = MQTTSNPacket_decode(_buf, _bufLen, &value);
+        if (_buf[p + 1] & 0x80)
+        {
+            sprintf(pbuf, "+%02X%02X", _buf[p + 4], _buf[p + 5]);
+        }
+        else
+        {
+            sprintf(pbuf, " %02X%02X", _buf[p + 4], _buf[p + 5]);
+        }
+        break;
+    case MQTTSN_PUBACK:
+    case MQTTSN_REGISTER:
+    case MQTTSN_REGACK:
+        sprintf(pbuf, " %02X%02X", _buf[4], _buf[5]);
+        break;
+    case MQTTSN_PUBREC:
+    case MQTTSN_PUBREL:
+    case MQTTSN_PUBCOMP:
+    case MQTTSN_UNSUBACK:
+        sprintf(pbuf, " %02X%02X", _buf[2], _buf[3]);
+        break;
+    case MQTTSN_SUBSCRIBE:
+    case MQTTSN_UNSUBSCRIBE:
+        p = MQTTSNPacket_decode(_buf, _bufLen, &value);
+        sprintf(pbuf, " %02X%02X", _buf[p + 2], _buf[p + 3]);
+        break;
+    case MQTTSN_SUBACK:
+        sprintf(pbuf, " %02X%02X", _buf[5], _buf[6]);
+        break;
+    default:
+        sprintf(pbuf, "    ");
+        break;
+    }
+    if (strcmp(pbuf, " 0000") == 0)
+    {
+        sprintf(pbuf, "    ");
+    }
+    return pbuf;
+}
+
+int MQTTSNPacket::getMsgId(void)
+{
+    int value = 0;
+    int p = 0;
+    int msgId = 0;
+    char* ptr = 0;
+
+    switch (getType())
+    {
+    case MQTTSN_PUBLISH:
+        p = MQTTSNPacket_decode(_buf, _bufLen, &value);
+        ptr = (char*) _buf + p + 4;
+        msgId = readInt((char**) &ptr);
+        break;
+    case MQTTSN_PUBACK:
+    case MQTTSN_REGISTER:
+    case MQTTSN_REGACK:
+        ptr = (char*) _buf + 4;
+        msgId = readInt((char**) &ptr);
+        break;
+    case MQTTSN_PUBREC:
+    case MQTTSN_PUBREL:
+    case MQTTSN_PUBCOMP:
+    case MQTTSN_UNSUBACK:
+        ptr = (char*) _buf + 2;
+        msgId = readInt((char**) &ptr);
+        break;
+    case MQTTSN_SUBSCRIBE:
+    case MQTTSN_UNSUBSCRIBE:
+        p = MQTTSNPacket_decode(_buf, _bufLen, &value);
+        ptr = (char*) _buf + p + 2;
+        msgId = readInt((char**) &ptr);
+        break;
+    case MQTTSN_SUBACK:
+        ptr = (char*) _buf + 5;
+        msgId = readInt((char**) &ptr);
+        break;
+    default:
+        break;
+    }
+    return msgId;
+}
+
+void MQTTSNPacket::setMsgId(uint16_t msgId)
+{
+    int value = 0;
+    int p = 0;
+    //unsigned char* ptr = 0;
+
+    switch (getType())
+    {
+    case MQTTSN_PUBLISH:
+        p = MQTTSNPacket_decode(_buf, _bufLen, &value);
+        _buf[p + 4] = (unsigned char) (msgId / 256);
+        _buf[p + 5] = (unsigned char) (msgId % 256);
+        //ptr = _buf + p + 4;
+        //writeInt(&ptr, msgId);
+        break;
+    case MQTTSN_PUBACK:
+    case MQTTSN_REGISTER:
+    case MQTTSN_REGACK:
+        _buf[4] = (unsigned char) (msgId / 256);
+        _buf[5] = (unsigned char) (msgId % 256);
+        //ptr = _buf + 4;
+        //writeInt(&ptr, msgId);
+        break;
+    case MQTTSN_PUBREC:
+    case MQTTSN_PUBREL:
+    case MQTTSN_PUBCOMP:
+    case MQTTSN_UNSUBACK:
+        _buf[2] = (unsigned char) (msgId / 256);
+        _buf[3] = (unsigned char) (msgId % 256);
+        //ptr = _buf + 2;
+        //writeInt(&ptr, msgId);
+        break;
+    case MQTTSN_SUBSCRIBE:
+    case MQTTSN_UNSUBSCRIBE:
+        p = MQTTSNPacket_decode(_buf, _bufLen, &value);
+        _buf[p + 2] = (unsigned char) (msgId / 256);
+        _buf[p + 3] = (unsigned char) (msgId % 256);
+        //ptr = _buf + p + 2;
+        //writeInt(&ptr, msgId);
+        break;
+    case MQTTSN_SUBACK:
+        _buf[5] = (unsigned char) (msgId / 256);
+        _buf[6] = (unsigned char) (msgId % 256);
+        //ptr = _buf + 5;
+        //writeInt(&ptr, msgId);
+        break;
+    default:
+        break;
+    }
+}
+
+bool MQTTSNPacket::isDuplicate(void)
+{
+    int value = 0;
+    int p = MQTTSNPacket_decode(_buf, _bufLen, &value);
+    return (_buf[p + 1] & 0x80);
 }
--- a/MQTTSNGateway/src/MQTTSNGWPacket.h
+++ b/MQTTSNGateway/src/MQTTSNGWPacket.h
@@ -22,66 +22,82 @@

 namespace MQTTSNGW
 {
+class SensorNetwork;

 class MQTTSNPacket
 {
 public:
-	MQTTSNPacket();
-	~MQTTSNPacket();
-	int unicast(SensorNetwork* network, SensorNetAddress* sendTo);
-	int broadcast(SensorNetwork* network);
-	int recv(SensorNetwork* network);
-	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(void);
+    MQTTSNPacket(MQTTSNPacket &packet);
+    ~MQTTSNPacket(void);
+    int unicast(SensorNetwork* network, SensorNetAddress* sendTo);
+    int broadcast(SensorNetwork* network);
+    int recv(SensorNetwork* network);
+    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();

-													int setConnect(void);   // Debug
-	int setADVERTISE(uint8_t gatewayid, uint16_t duration);
-	int setGWINFO(uint8_t gatewayId);
-	int setCONNACK(uint8_t returnCode);
-	int setWILLTOPICREQ(void);
-	int setWILLMSGREQ(void);
-	int setREGISTER(uint16_t topicId, uint16_t msgId, MQTTSNString* TopicName);
-	int setREGACK(uint16_t topicId, uint16_t msgId, uint8_t returnCode);
-	int setPUBLISH(uint8_t dup, int qos, uint8_t retained, uint16_t msgId,
-			MQTTSN_topicid topic, uint8_t* payload, uint16_t payloadlen);
-	int setPUBACK(uint16_t topicId, uint16_t msgId, uint8_t returnCode);
-	int setPUBREC(uint16_t msgId);
-	int setPUBREL(uint16_t msgId);
-	int setPUBCOMP(uint16_t msgId);
-	int setSUBACK(int qos, uint16_t topicId, uint16_t msgId, uint8_t returnCode);
-	int setUNSUBACK(uint16_t msgId);
-	int setPINGRESP(void);
-	int setDISCONNECT(uint16_t duration);
-	int setWILLTOPICRESP(uint8_t returnCode);
-	int setWILLMSGRESP(uint8_t returnCode);
+    int setConnect(void);   // Debug
+    int setADVERTISE(uint8_t gatewayid, uint16_t duration);
+    int setGWINFO(uint8_t gatewayId);
+    int setCONNACK(uint8_t returnCode);
+    int setWILLTOPICREQ(void);
+    int setWILLMSGREQ(void);
+    int setREGISTER(uint16_t topicId, uint16_t msgId, MQTTSNString* TopicName);
+    int setREGACK(uint16_t topicId, uint16_t msgId, uint8_t returnCode);
+    int setPUBLISH(uint8_t dup, int qos, uint8_t retained, uint16_t msgId,
+            MQTTSN_topicid topic, uint8_t* payload, uint16_t payloadlen);
+    int setPUBACK(uint16_t topicId, uint16_t msgId, uint8_t returnCode);
+    int setPUBREC(uint16_t msgId);
+    int setPUBREL(uint16_t msgId);
+    int setPUBCOMP(uint16_t msgId);
+    int setSUBACK(int qos, uint16_t topicId, uint16_t msgId,
+            uint8_t returnCode);
+    int setUNSUBACK(uint16_t msgId);
+    int setPINGRESP(void);
+    int setDISCONNECT(uint16_t duration);
+    int setWILLTOPICRESP(uint8_t returnCode);
+    int setWILLMSGRESP(uint8_t returnCode);

-	int getSERCHGW(uint8_t* radius);
-	int getCONNECT(MQTTSNPacket_connectData* option);
-	int getCONNACK(uint8_t* returnCode);
-	int getWILLTOPIC(int* willQoS, uint8_t* willRetain, MQTTSNString* willTopic);
-	int getWILLMSG(MQTTSNString* willmsg);
-	int getREGISTER(uint16_t* topicId, uint16_t* msgId, MQTTSNString* topicName);
-	int getREGACK(uint16_t* topicId, uint16_t* msgId, uint8_t* returnCode);
-	int getPUBLISH(uint8_t* dup, int* qos, uint8_t* retained, uint16_t* msgId,
-			MQTTSN_topicid* topic, unsigned char** payload, int* payloadlen);
-	int getPUBACK(uint16_t* topicId, uint16_t* msgId, uint8_t* returnCode);
-	int getACK(uint16_t* msgId);
-	int getSUBSCRIBE(uint8_t* dup, int* qos, uint16_t* msgId, MQTTSN_topicid* topicFilter);
-	int getUNSUBSCRIBE(uint16_t* msgId, MQTTSN_topicid* topicFilter);
-	int getPINGREQ(void);
-	int getDISCONNECT(uint16_t* duration);
-	int getWILLTOPICUPD(uint8_t* willQoS, uint8_t* willRetain, MQTTSNString* willTopic);
-	int getWILLMSGUPD(MQTTSNString* willMsg);
-	char* getMsgId(char* buf);
-	char* print(char* buf);
+    int setCONNECT(MQTTSNPacket_connectData* options);
+    int setPINGREQ(MQTTSNString* clientId);
+
+    int getSERCHGW(uint8_t* radius);
+    int getCONNECT(MQTTSNPacket_connectData* option);
+    int getCONNACK(uint8_t* returnCode);
+    int getWILLTOPIC(int* willQoS, uint8_t* willRetain,
+            MQTTSNString* willTopic);
+    int getWILLMSG(MQTTSNString* willmsg);
+    int getREGISTER(uint16_t* topicId, uint16_t* msgId,
+            MQTTSNString* topicName);
+    int getREGACK(uint16_t* topicId, uint16_t* msgId, uint8_t* returnCode);
+    int getPUBLISH(uint8_t* dup, int* qos, uint8_t* retained, uint16_t* msgId,
+            MQTTSN_topicid* topic, unsigned char** payload, int* payloadlen);
+    int getPUBACK(uint16_t* topicId, uint16_t* msgId, uint8_t* returnCode);
+    int getACK(uint16_t* msgId);
+    int getSUBSCRIBE(uint8_t* dup, int* qos, uint16_t* msgId,
+            MQTTSN_topicid* topicFilter);
+    int getUNSUBSCRIBE(uint16_t* msgId, MQTTSN_topicid* topicFilter);
+    int getPINGREQ(void);
+    int getDISCONNECT(uint16_t* duration);
+    int getWILLTOPICUPD(uint8_t* willQoS, uint8_t* willRetain,
+            MQTTSNString* willTopic);
+    int getWILLMSGUPD(MQTTSNString* willMsg);
+
+    bool isAccepted(void);
+    bool isDuplicate(void);
+    bool isQoSMinusPUBLISH(void);
+    char* getMsgId(char* buf);
+    int getMsgId(void);
+    void setMsgId(uint16_t msgId);
+    char* print(char* buf);

 private:
-	unsigned char* _buf;    // Ptr to a packet data
-	int            _bufLen; // length of the packet data
+    unsigned char* _buf;    // Ptr to a packet data
+    int _bufLen; // length of the packet data
 };

 }
--- a/MQTTSNGateway/src/MQTTSNGWPacketHandleTask.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWPacketHandleTask.cpp
@@ -12,6 +12,7 @@
 *
 * Contributors:
 *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
+ *    Tieto Poland Sp. z o.o. - Gateway improvements
 **************************************************************************************/

 #include "MQTTSNGWDefines.h"
@@ -20,12 +21,16 @@
 #include "MQTTGWPacket.h"
 #include "MQTTSNGWClient.h"
 #include "MQTTSNGWProcess.h"
+#include "MQTTSNGWAdapterManager.h"
 #include "MQTTGWConnectionHandler.h"
 #include "MQTTGWPublishHandler.h"
 #include "MQTTGWSubscribeHandler.h"
 #include "MQTTSNGWConnectionHandler.h"
 #include "MQTTSNGWPublishHandler.h"
 #include "MQTTSNGWSubscribeHandler.h"
+#include "Timer.h"
+#include "MQTTSNAggregateConnectionHandler.h"
+
 #include <string.h>

 using namespace std;
@@ -33,20 +38,24 @@ using namespace MQTTSNGW;

 #define EVENT_QUE_TIME_OUT  2000      // 2000 msecs
 char* currentDateTime(void);
+
 /*=====================================
 Class PacketHandleTask
 =====================================*/

 PacketHandleTask::PacketHandleTask(Gateway* gateway)
 {
-	_gateway = gateway;
-	_gateway->attach((Thread*)this);
-	_mqttConnection = new MQTTGWConnectionHandler(_gateway);
-	_mqttPublish = new MQTTGWPublishHandler(_gateway);
-	_mqttSubscribe = new MQTTGWSubscribeHandler(_gateway);
-	_mqttsnConnection = new MQTTSNConnectionHandler(_gateway);
-	_mqttsnPublish = new MQTTSNPublishHandler(_gateway);
-	_mqttsnSubscribe = new MQTTSNSubscribeHandler(_gateway);
+    _gateway = gateway;
+    _gateway->attach((Thread*) this);
+    _mqttConnection = new MQTTGWConnectionHandler(_gateway);
+    _mqttPublish = new MQTTGWPublishHandler(_gateway);
+    _mqttSubscribe = new MQTTGWSubscribeHandler(_gateway);
+    _mqttsnConnection = new MQTTSNConnectionHandler(_gateway);
+    _mqttsnPublish = new MQTTSNPublishHandler(_gateway);
+    _mqttsnSubscribe = new MQTTSNSubscribeHandler(_gateway);
+
+    _mqttsnAggrConnection = new MQTTSNAggregateConnectionHandler(_gateway);
+    setTaskName("PacketHandleTask");
 }

 /**
@@ -54,175 +63,309 @@ PacketHandleTask::PacketHandleTask(Gateway* gateway)
 */
 PacketHandleTask::~PacketHandleTask()
 {
-	if ( _mqttConnection )
-	{
-		delete _mqttConnection;
-	}
-	if ( _mqttPublish )
-	{
-		delete _mqttPublish;
-	}
-	if ( _mqttSubscribe )
-	{
-		delete _mqttSubscribe;
-	}
-	if ( _mqttsnConnection )
-	{
-		delete _mqttsnConnection;
-	}
-	if ( _mqttsnPublish )
-	{
-		delete _mqttsnPublish;
-	}
-	if ( _mqttsnSubscribe )
-	{
-		delete _mqttsnSubscribe;
-	}
+    if (_mqttConnection)
+    {
+        delete _mqttConnection;
+    }
+    if (_mqttPublish)
+    {
+        delete _mqttPublish;
+    }
+    if (_mqttSubscribe)
+    {
+        delete _mqttSubscribe;
+    }
+    if (_mqttsnConnection)
+    {
+        delete _mqttsnConnection;
+    }
+    if (_mqttsnPublish)
+    {
+        delete _mqttsnPublish;
+    }
+    if (_mqttsnSubscribe)
+    {
+        delete _mqttsnSubscribe;
+    }
+
+    if (_mqttsnAggrConnection)
+    {
+        delete _mqttsnAggrConnection;
+    }
 }

 void PacketHandleTask::run()
 {
-	Event* ev = 0;
-	EventQue* eventQue = _gateway->getPacketEventQue();
-	Client* client = 0;
-	MQTTSNPacket* snPacket = 0;
-	MQTTGWPacket* brPacket = 0;
-	char msgId[6];
-	memset(msgId, 0, 6);
+    Event* ev = nullptr;
+    EventQue* eventQue = _gateway->getPacketEventQue();
+    AdapterManager* adpMgr = _gateway->getAdapterManager();

-	_advertiseTimer.start(_gateway->getGWParams()->keepAlive * 1000UL);
+    Client* client = nullptr;
+    MQTTSNPacket* snPacket = nullptr;
+    MQTTGWPacket* brPacket = nullptr;
+    char msgId[6];
+    memset(msgId, 0, 6);

-	while (true)
-	{
-		/* wait Event */
-		ev = eventQue->timedwait(EVENT_QUE_TIME_OUT);
+    _advertiseTimer.start(_gateway->getGWParams()->keepAlive * 1000UL);

-		if (ev->getEventType() == EtStop)
-		{
-			WRITELOG("%s PacketHandleTask stopped.\n", currentDateTime());
-			delete ev;
-			return;
-		}
+    while (true)
+    {
+        /* wait Event */
+        ev = eventQue->timedwait(EVENT_QUE_TIME_OUT);

-		if (ev->getEventType() == EtTimeout)
-		{
-			/*------ Check Keep Alive Timer & send Advertise ------*/
-			if (_advertiseTimer.isTimeup())
-			{
-				_mqttsnConnection->sendADVERTISE();
-				_advertiseTimer.start(_gateway->getGWParams()->keepAlive * 1000UL);
-			}
-		}
+        if (ev->getEventType() == EtStop)
+        {
+            WRITELOG("%s %s stopped.\n", currentDateTime(), getTaskName());
+            delete ev;
+            return;
+        }

-		/*------    Handle SEARCHGW Message     ---------*/
-		else if (ev->getEventType() == EtBroadcast)
-		{
-			snPacket = ev->getMQTTSNPacket();
-			_mqttsnConnection->handleSearchgw(snPacket);
-		}
+        if (ev->getEventType() == EtTimeout)
+        {
+            /*------ Check Keep Alive Timer & send Advertise ------*/
+            if (_advertiseTimer.isTimeup())
+            {
+                _mqttsnConnection->sendADVERTISE();
+                _advertiseTimer.start(_gateway->getGWParams()->keepAlive * 1000UL);
+            }

-		/*------    Handle Messages form Clients      ---------*/
-		else if (ev->getEventType() == EtClientRecv)
-		{
-			client = ev->getClient();
-			snPacket = ev->getMQTTSNPacket();
+            /*------ Check Adapters   Connect or PINGREQ ------*/
+            adpMgr->checkConnection();
+        }

-			DEBUGLOG("     PacketHandleTask gets %s %s from the client.\n", snPacket->getName(), snPacket->getMsgId(msgId));
+        /*------    Handle SEARCHGW Message     ---------*/
+        else if (ev->getEventType() == EtBroadcast)
+        {
+            snPacket = ev->getMQTTSNPacket();
+            _mqttsnConnection->handleSearchgw(snPacket);
+        }

-			switch (snPacket->getType())
-			{
-			case MQTTSN_CONNECT:
-				_mqttsnConnection->handleConnect(client, snPacket);
-				break;
-			case MQTTSN_WILLTOPIC:
-				_mqttsnConnection->handleWilltopic(client, snPacket);
-				break;
-			case MQTTSN_WILLMSG:
-				_mqttsnConnection->handleWillmsg(client, snPacket);
-				break;
-			case MQTTSN_DISCONNECT:
-				_mqttsnConnection->handleDisconnect(client, snPacket);
-				break;
-			case MQTTSN_WILLMSGUPD:
-				_mqttsnConnection->handleWillmsgupd(client, snPacket);
-				break;
-			case MQTTSN_PINGREQ:
-				_mqttsnConnection->handlePingreq(client, snPacket);
-				break;
-			case MQTTSN_PUBLISH:
-				_mqttsnPublish->handlePublish(client, snPacket);
-				break;
-			case MQTTSN_PUBACK:
-				_mqttsnPublish->handlePuback(client, snPacket);
-				break;
-				_mqttsnPublish->handleAck(client, snPacket, PUBREC);
-				break;
-			case MQTTSN_PUBREL:
-				_mqttsnPublish->handleAck(client, snPacket, PUBREL);
-				break;
-			case MQTTSN_PUBCOMP:
-				_mqttsnPublish->handleAck(client, snPacket, PUBCOMP);
-				break;
-			case MQTTSN_REGISTER:
-				_mqttsnPublish->handleRegister(client, snPacket);
-				break;
-			case MQTTSN_REGACK:
-				// NOP
-				break;
-			case MQTTSN_SUBSCRIBE:
-				_mqttsnSubscribe->handleSubscribe(client, snPacket);
-				break;
-			case MQTTSN_UNSUBSCRIBE:
-				_mqttsnSubscribe->handleUnsubscribe(client, snPacket);
-				break;
-			default:
-				break;
-			}
+        /*------    Handle Messages form Clients      ---------*/
+        else if (ev->getEventType() == EtClientRecv)
+        {
+            client = ev->getClient();
+            snPacket = ev->getMQTTSNPacket();

-			/* Reset the Timer for PINGREQ. */
-			client->updateStatus(snPacket);
-		}
+            DEBUGLOG("     PacketHandleTask gets %s %s from the client.\n", snPacket->getName(), snPacket->getMsgId(msgId));

-		/*------  Handle Messages form Broker      ---------*/
-		else if (ev->getEventType() == EtBrokerRecv)
-		{
-			client = ev->getClient();
-			brPacket = ev->getMQTTGWPacket();
-			DEBUGLOG("     PacketHandleTask gets %s %s from the broker.\n", brPacket->getName(), brPacket->getMsgId(msgId));
-			switch (brPacket->getType())
-			{
-			case CONNACK:
-				_mqttConnection->handleConnack(client, brPacket);
-				break;
-			case PINGRESP:
-				_mqttConnection->handlePingresp(client, brPacket);
-				break;
-			case PUBLISH:
-				_mqttPublish->handlePublish(client, brPacket);
-				break;
-			case PUBACK:
-				_mqttPublish->handlePuback(client, brPacket);
-				break;
-			case PUBREC:
-				_mqttPublish->handleAck(client, brPacket, PUBREC);
-				break;
-			case PUBREL:
-				_mqttPublish->handleAck(client, brPacket, PUBREL);
-				break;
-			case PUBCOMP:
-				_mqttPublish->handleAck(client, brPacket, PUBCOMP);
-				break;
-			case SUBACK:
-				_mqttSubscribe->handleSuback(client, brPacket);
-				break;
-			case UNSUBACK:
-				_mqttSubscribe->handleUnsuback(client, brPacket);
-				break;
-			default:
-				break;
-			}
-		}
-		delete ev;
-	}
+            if (adpMgr->isAggregatedClient(client))
+            {
+                aggregatePacketHandler(client, snPacket); // client is converted to Aggregater by BrokerSendTask
+            }
+            else
+            {
+                transparentPacketHandler(client, snPacket);
+            }
+
+            /* Reset the Timer for PINGREQ. */
+            client->updateStatus(snPacket);
+        }
+        /*------  Handle Messages form Broker      ---------*/
+        else if (ev->getEventType() == EtBrokerRecv)
+        {
+            client = ev->getClient();
+            brPacket = ev->getMQTTGWPacket();
+            DEBUGLOG("     PacketHandleTask gets %s %s from the broker.\n", brPacket->getName(), brPacket->getMsgId(msgId));
+
+            if (client->isAggregater())
+            {
+                aggregatePacketHandler(client, brPacket);
+            }
+            else
+            {
+                transparentPacketHandler(client, brPacket);
+            }
+        }
+        delete ev;
+    }
+}
+
+void PacketHandleTask::aggregatePacketHandler(Client*client, MQTTSNPacket* packet)
+{
+    switch (packet->getType())
+    {
+    case MQTTSN_CONNECT:
+        _mqttsnAggrConnection->handleConnect(client, packet);
+        break;
+    case MQTTSN_WILLTOPIC:
+        _mqttsnConnection->handleWilltopic(client, packet);
+        break;
+    case MQTTSN_WILLMSG:
+        _mqttsnAggrConnection->handleWillmsg(client, packet);
+        break;
+    case MQTTSN_DISCONNECT:
+        _mqttsnAggrConnection->handleDisconnect(client, packet);
+        break;
+    case MQTTSN_WILLTOPICUPD:
+        _mqttsnConnection->handleWilltopicupd(client, packet);
+        break;
+    case MQTTSN_WILLMSGUPD:
+        _mqttsnConnection->handleWillmsgupd(client, packet);
+        break;
+    case MQTTSN_PINGREQ:
+        _mqttsnAggrConnection->handlePingreq(client, packet);
+        break;
+    case MQTTSN_PUBLISH:
+        _mqttsnPublish->handleAggregatePublish(client, packet);
+        break;
+    case MQTTSN_PUBACK:
+        _mqttsnPublish->handleAggregateAck(client, packet, MQTTSN_PUBACK);
+        break;
+    case MQTTSN_PUBREC:
+        _mqttsnPublish->handleAggregateAck(client, packet, MQTTSN_PUBREC);
+        break;
+    case MQTTSN_PUBREL:
+        _mqttsnPublish->handleAggregateAck(client, packet, MQTTSN_PUBREL);
+        break;
+    case MQTTSN_PUBCOMP:
+        _mqttsnPublish->handleAggregateAck(client, packet, MQTTSN_PUBCOMP);
+        break;
+    case MQTTSN_REGISTER:
+        _mqttsnPublish->handleRegister(client, packet);
+        break;
+    case MQTTSN_REGACK:
+        _mqttsnPublish->handleRegAck(client, packet);
+        break;
+    case MQTTSN_SUBSCRIBE:
+        _mqttsnSubscribe->handleAggregateSubscribe(client, packet);
+        break;
+    case MQTTSN_UNSUBSCRIBE:
+        _mqttsnSubscribe->handleAggregateUnsubscribe(client, packet);
+        break;
+    default:
+        break;
+    }
+}
+
+void PacketHandleTask::aggregatePacketHandler(Client*client, MQTTGWPacket* packet)
+{
+    switch (packet->getType())
+    {
+    case CONNACK:
+        _mqttConnection->handleConnack(client, packet);
+        break;
+    case PINGRESP:
+        _mqttConnection->handlePingresp(client, packet);
+        break;
+    case PUBLISH:
+        _mqttPublish->handleAggregatePublish(client, packet);
+        break;
+    case PUBACK:
+        _mqttPublish->handleAggregatePuback(client, packet);
+        break;
+    case PUBREC:
+        _mqttPublish->handleAggregateAck(client, packet, PUBREC);
+        break;
+    case PUBREL:
+        _mqttPublish->handleAggregatePubrel(client, packet);
+        break;
+    case PUBCOMP:
+        _mqttPublish->handleAggregateAck(client, packet, PUBCOMP);
+        break;
+    case SUBACK:
+        _mqttSubscribe->handleAggregateSuback(client, packet);
+        break;
+    case UNSUBACK:
+        _mqttSubscribe->handleAggregateUnsuback(client, packet);
+        break;
+    default:
+        break;
+    }
+}
+
+void PacketHandleTask::transparentPacketHandler(Client*client, MQTTSNPacket* packet)
+{
+    switch (packet->getType())
+    {
+    case MQTTSN_CONNECT:
+        _mqttsnConnection->handleConnect(client, packet);
+        break;
+    case MQTTSN_WILLTOPIC:
+        _mqttsnConnection->handleWilltopic(client, packet);
+        break;
+    case MQTTSN_WILLMSG:
+        _mqttsnConnection->handleWillmsg(client, packet);
+        break;
+    case MQTTSN_DISCONNECT:
+        _mqttsnConnection->handleDisconnect(client, packet);
+        break;
+    case MQTTSN_WILLTOPICUPD:
+        _mqttsnConnection->handleWilltopicupd(client, packet);
+        break;
+    case MQTTSN_WILLMSGUPD:
+        _mqttsnConnection->handleWillmsgupd(client, packet);
+        break;
+    case MQTTSN_PINGREQ:
+        _mqttsnConnection->handlePingreq(client, packet);
+        break;
+    case MQTTSN_PUBLISH:
+        _mqttsnPublish->handlePublish(client, packet);
+        break;
+    case MQTTSN_PUBACK:
+        _mqttsnPublish->handlePuback(client, packet);
+        break;
+    case MQTTSN_PUBREC:
+        _mqttsnPublish->handleAck(client, packet, PUBREC);
+        break;
+    case MQTTSN_PUBREL:
+        _mqttsnPublish->handleAck(client, packet, PUBREL);
+        break;
+    case MQTTSN_PUBCOMP:
+        _mqttsnPublish->handleAck(client, packet, PUBCOMP);
+        break;
+    case MQTTSN_REGISTER:
+        _mqttsnPublish->handleRegister(client, packet);
+        break;
+    case MQTTSN_REGACK:
+        _mqttsnPublish->handleRegAck(client, packet);
+        break;
+    case MQTTSN_SUBSCRIBE:
+        _mqttsnSubscribe->handleSubscribe(client, packet);
+        break;
+    case MQTTSN_UNSUBSCRIBE:
+        _mqttsnSubscribe->handleUnsubscribe(client, packet);
+        break;
+    default:
+        break;
+    }
+}
+
+void PacketHandleTask::transparentPacketHandler(Client*client, MQTTGWPacket* packet)
+{
+    switch (packet->getType())
+    {
+    case CONNACK:
+        _mqttConnection->handleConnack(client, packet);
+        break;
+    case PINGRESP:
+        _mqttConnection->handlePingresp(client, packet);
+        break;
+    case PUBLISH:
+        _mqttPublish->handlePublish(client, packet);
+        break;
+    case PUBACK:
+        _mqttPublish->handlePuback(client, packet);
+        break;
+    case PUBREC:
+        _mqttPublish->handleAck(client, packet, PUBREC);
+        break;
+    case PUBREL:
+        _mqttPublish->handleAck(client, packet, PUBREL);
+        break;
+    case PUBCOMP:
+        _mqttPublish->handleAck(client, packet, PUBCOMP);
+        break;
+    case SUBACK:
+        _mqttSubscribe->handleSuback(client, packet);
+        break;
+    case UNSUBACK:
+        _mqttSubscribe->handleUnsuback(client, packet);
+        break;
+    case DISCONNECT:
+        client->disconnected(); // Just change Client's status to "Disconnected"
+        break;
+    default:
+        break;
+    }
 }

--- a/Show More
+++ b/Show More