Bugfix of #241, #90

Two clients works fine.

Signed-off-by: tomoaki <tomoaki@tomy-tech.com>

.cproject

@@ -14,30 +14,43 @@
 				</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"/>
+									<listOptionValue builtIn="false" value="DEBUG_NW"/>
 								</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 +60,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 +81,8 @@
 						</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="src/linux/udp6|src/linux/udp|GatewayTester|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="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,8 @@
 						</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="linux/udp6|linux/udp" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="MQTTSNGateway/src"/>
 						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="MQTTSNPacket/src"/>
 					</sourceEntries>
 				</configuration>
@@ -151,28 +166,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>

.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/

.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>

.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}

.settings/org.eclipse.cdt.ui.prefs

@@ -0,0 +1,2 @@
+eclipse.preferences.version=1
+formatter_settings_version=1

.travis.yml

@@ -1,31 +1,24 @@
 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:
     - tomoaki@tomy-tech.com

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)

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

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;

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)

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 +=
+DEF1 :=
+DEF2 :=
+DEFS := -D$(SN) $(DEF1) $(DEF2)
+LIBS += -L/usr/local/lib -L/usr/local/opt/openssl
 LDFLAGS := 
 CXXFLAGS := -Wall -O3 -std=c++11
-LDADD := 
+LDADD := -lbluetooth -lssl -lcrypto
 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) ../../../	
 	

MQTTSNGateway/GatewayTester/README.md

@@ -1,28 +1,7 @@
-###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];
@@ -36,9 +15,35 @@ void test2(void)
     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
  *------------------------------------------------------*/
@@ -52,68 +57,61 @@ TEST_LIST = {// e.g. TEST( Label, Test),
              TEST("Disconnect",         test5),
              END_OF_TEST_LIST
         };
-```` 
         
-### **step1. Build **   
-````
-$ git clone https://github.com/eclipse/paho.mqtt-sn.embedded-c 
-$ cd paho.mqtt-sn.embedded-c/MQTTSNGateway/GatewayTester       
-$ make   
-$ make install   
-$ make clean
 ``` 
-MQTT-SNGatewayTester program is copied into ../../../ directory.
+
+**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
+        };
+```
 
 
-### **step2. Execute Gateway Tester.**     
+## How to Build    
+```
+copy codes from the github.
+$ cd paho.mqtt-sn.embedded-c/MQTTSNGateway/GatewayTester       
+$ ./build.sh [udp | udp6 | dtls | dtls6 | rfcomm]
+```       
 
-````    
-$ 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 ) :  
-
-````    
+```    

MQTTSNGateway/GatewayTester/build.sh

@@ -0,0 +1,22 @@
+#!/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 
+    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
+
+

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 ********************/

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 ********************/

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 ********************/
+

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();
-}
-
-*/

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
- **************************************************************************************/
+ ***************************************************************************/
 
 #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
+ *    UDP,DTLS Configuration    (theNetcon)
  *------------------------------------------------------*/
-UDPCONF  = {
-	"GatewayTester",     // ClientId
-	{225,1,1,1},         // Multicast group IP
+UDPCONF = { "GatewayTestClient",  // ClientId
+		{ 225, 1, 1, 1 },         // Multicast group IP
         1883,                     // Multicast group Port
-	20001,               // Local PortNo
-};
+        20020,                    // Local PortNo
+        };
 
 /*------------------------------------------------------
- *    Client Configuration
+ *    UDP6, DTLS6 Configuration    (theNetcon)
  *------------------------------------------------------*/
-MQTTSNCONF = {
-	300,            //KeepAlive (seconds)
+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
-	0,              //Sleep duration in msecs
-	"willTopic",    //WillTopic
-	"willMessage",  //WillMessage
+        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),
+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);
 }
 
+void asleep(void)
+{
+	DISCONNECT(theMqcon.sleepDuration);
+}
+
+void onconnect(void)
+{
+	ONCONNECT();
+}
+
+void connect(void)
+{
+	CONNECT();
+}
+
+void DisableAutoPingreq(void)
+{
+	SetAutoPingReqMode(false);
+}
+
+void CleanSessionOff(void)
+{
+	SetCleanSession(false);
+}
+
 /*------------------------------------------------------
- *    A List of Test functions
+ *    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 topic2",     publishTopic2),
-			 TEST("Step3:Subscribe topic2",   subscribeTopic2),
+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:Unsubscribe topic2", unsubscribe),
+	TEST("Step5:Subscribe PreDefined topic10. ID is not defined.", subscribeTopic10),
 	TEST("Step6:Publish topic2", publishTopic2),
-			 TEST("Step7:subscribe again",    subscribechangeCallback),
+	TEST("Step7:Unsubscribe topic2", unsubscribe),
 	TEST("Step8:Publish topic2", publishTopic2),
-			 TEST("Step9:Disconnect",         disconnect),
-			 END_OF_TEST_LIST
-			};
+	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
+};
 
 /*------------------------------------------------------
- *    unused for Test
+ *    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(test1, 4);
+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 ********************/

MQTTSNGateway/GatewayTester/src/LGwProxy.cpp

@@ -1,5 +1,5 @@
 /**************************************************************************************
- * Copyright (c) 2016, Tomoaki Yamaguchi
+ * Copyright (c) 2016-2018, Tomoaki Yamaguchi
  *
  * All rights reserved. This program and the accompanying materials
  * are made available under the terms of the Eclipse Public License v1.0
@@ -29,20 +29,17 @@ extern void setUint16(uint8_t* pos, uint16_t val);
 extern uint16_t getUint16(const uint8_t* pos);
 extern LMqttsnClient* theClient;
 extern LScreen* theScreen;
+
 /*=====================================
  Class GwProxy
  ======================================*/
-static const char* packet_names[] =
-{
-	"ADVERTISE", "SEARCHGW", "GWINFO", "RESERVED", "CONNECT", "CONNACK",
-	"WILLTOPICREQ", "WILLTOPIC", "WILLMSGREQ", "WILLMSG", "REGISTER", "REGACK",
-	"PUBLISH", "PUBACK", "PUBCOMP", "PUBREC", "PUBREL", "RESERVED",
-	"SUBSCRIBE", "SUBACK", "UNSUBSCRIBE", "UNSUBACK", "PINGREQ", "PINGRESP",
-	"DISCONNECT", "RESERVED", "WILLTOPICUPD", "WILLTOPICRESP", "WILLMSGUPD",
-	"WILLMSGRESP"
-};
+static const char* packet_names[] = { "ADVERTISE", "SEARCHGW", "GWINFO", "RESERVED", "CONNECT", "CONNACK",
+        "WILLTOPICREQ", "WILLTOPIC", "WILLMSGREQ", "WILLMSG", "REGISTER", "REGACK", "PUBLISH", "PUBACK", "PUBCOMP",
+        "PUBREC", "PUBREL", "RESERVED", "SUBSCRIBE", "SUBACK", "UNSUBSCRIBE", "UNSUBACK", "PINGREQ", "PINGRESP",
+        "DISCONNECT", "RESERVED", "WILLTOPICUPD", "WILLTOPICRESP", "WILLMSGUPD", "WILLMSGRESP" };
 
-LGwProxy::LGwProxy(){
+LGwProxy::LGwProxy()
+{
     _nextMsgId = 0;
     _status = GW_LOST;
     _gwId = 0;
@@ -55,95 +52,142 @@ LGwProxy::LGwProxy(){
     _cleanSession = 0;
     _pingStatus = 0;
     _connectRetry = MQTTSN_RETRY_COUNT;
+    _tSleep = 0;
+    _tWake = 0;
+    _initialized = 0;
+    _isForwarderMode = false;
+    _isQoSMinus1Mode = false;
+	_isPingReqMode = true;
+	_isAutoConnectMode = true;
 }
 
-LGwProxy::~LGwProxy(){
+LGwProxy::~LGwProxy()
+{
     _topicTbl.clearTopic();
 }
 
-void LGwProxy::initialize(LUdpConfig netconf, LMqttsnConfig mqconf){
-	_network.initialize(netconf);
-	_clientId = netconf.clientId;
-    _willTopic = mqconf.willTopic;
-    _willMsg = mqconf.willMsg;
-    _qosWill = mqconf.willQos;
-    _retainWill = mqconf.willRetain;
-    _cleanSession = mqconf.cleanSession;
-    _tkeepAlive = mqconf.keepAlive;
+void LGwProxy::initialize(SENSORNET_CONFIG_t* netconf, LMqttsnConfig* mqconf)
+{
+    if (_network.initialize(netconf) == false)
+    {
+        DISPLAY("Can't open SensorNetwork\n");
+        exit(-1);
+    }
+    _clientId = netconf->clientId;
+    _willTopic = mqconf->willTopic;
+    _willMsg = mqconf->willMsg;
+    _qosWill = mqconf->willQos;
+    _retainWill = mqconf->willRetain;
+    _cleanSession = mqconf->cleanSession;
+    _tkeepAlive = mqconf->keepAlive;
+    _initialized = 1;
 }
 
-void LGwProxy::connect(){
+void LGwProxy::connect()
+{
     char* pos;
 
-	while (_status != GW_CONNECTED){
+    while (_status != GW_CONNECTED)
+    {
         pos = _msg;
 
-		if (_status == GW_SEND_WILLMSG){
-			*pos++ = 2 + (uint8_t)strlen(_willMsg);
-			*pos++ = MQTTSN_TYPE_WILLMSG;
-			strcpy(pos,_willMsg);          // WILLMSG
-			_status = GW_WAIT_CONNACK;
-			writeGwMsg();
-		}else if (_status == GW_SEND_WILLTOPIC){
-			*pos++ = 3 + (uint8_t)strlen(_willTopic);
-			*pos++ = MQTTSN_TYPE_WILLTOPIC;
-			*pos++ = _qosWill | _retainWill;
-			strcpy(pos,_willTopic);        // WILLTOPIC
-			_status = GW_WAIT_WILLMSGREQ;
-			writeGwMsg();
-		}else if (_status == GW_CONNECTING || _status == GW_DISCONNECTED){
-			uint8_t clientIdLen = uint8_t(strlen(_clientId) > 23 ? 23 : strlen(_clientId));
-			*pos++ = 6 + clientIdLen;
-			*pos++ = MQTTSN_TYPE_CONNECT;
-			pos++;
-			if (_cleanSession){
-				_msg[2] = MQTTSN_FLAG_CLEAN;
+        if (!_network.isBroadcastable() && _status == GW_LOST)
+        {
+            _status = GW_CONNECTING;
+            continue;
         }
-			*pos++ = MQTTSN_PROTOCOL_ID;
-			setUint16((uint8_t*)pos, _tkeepAlive);
-			pos += 2;
-			strncpy(pos, _clientId, clientIdLen);
-			_msg[ 6 + clientIdLen] = 0;
-			_status = GW_WAIT_CONNACK;
-			if (_willMsg && _willTopic){
-				if (strlen(_willMsg) && strlen(_willTopic)){
-					_msg[2] = _msg[2] | MQTTSN_FLAG_WILL;   // CONNECT
-					_status = GW_WAIT_WILLTOPICREQ;
-				}
-			}
-			writeGwMsg();
-			_connectRetry = MQTTSN_RETRY_COUNT;
-		}else if (_status == GW_LOST){
+
+        if (_status == GW_LOST)
+        {
 
             *pos++ = 3;
             *pos++ = MQTTSN_TYPE_SEARCHGW;
             *pos = 0;                        // SERCHGW
             _status = GW_SEARCHING;
             writeGwMsg();
-
+        }
+        else if (_status == GW_SEND_WILLMSG)
+        {
+            *pos++ = 2 + (uint8_t) strlen(_willMsg);
+            *pos++ = MQTTSN_TYPE_WILLMSG;
+            strcpy(pos, _willMsg);          // WILLMSG
+            _status = GW_WAIT_CONNACK;
+            writeGwMsg();
+        }
+        else if (_status == GW_SEND_WILLTOPIC)
+        {
+            *pos++ = 3 + (uint8_t) strlen(_willTopic);
+            *pos++ = MQTTSN_TYPE_WILLTOPIC;
+            *pos++ = _qosWill | _retainWill;
+            strcpy(pos, _willTopic);        // WILLTOPIC
+            _status = GW_WAIT_WILLMSGREQ;
+            writeGwMsg();
+        }
+        else if (_status == GW_CONNECTING || _status == GW_DISCONNECTED || _status == GW_SLEPT)
+        {
+            uint8_t clientIdLen = uint8_t(strlen(_clientId) > 23 ? 23 : strlen(_clientId));
+            if (_isQoSMinus1Mode)
+            {
+                _status = GW_CONNECTED;
+            }
+            else
+            {
+                *pos++ = 6 + clientIdLen;
+                *pos++ = MQTTSN_TYPE_CONNECT;
+                pos++;
+                if (_cleanSession)
+                {
+                    _msg[2] = MQTTSN_FLAG_CLEAN;
+                }
+                *pos++ = MQTTSN_PROTOCOL_ID;
+                setUint16((uint8_t*) pos, _tkeepAlive);
+                pos += 2;
+                strncpy(pos, _clientId, clientIdLen);
+                _msg[6 + clientIdLen] = 0;
+                _status = GW_WAIT_CONNACK;
+                if (_willMsg && _willTopic && _status != GW_SLEPT)
+                {
+                    if (strlen(_willMsg) && strlen(_willTopic))
+                    {
+                        _msg[2] = _msg[2] | MQTTSN_FLAG_WILL;   // CONNECT
+                        _status = GW_WAIT_WILLTOPICREQ;
+                    }
+                }
+                writeGwMsg();
+                _connectRetry = MQTTSN_RETRY_COUNT;
+            }
         }
         getConnectResponce();
     }
     return;
 }
 
-int LGwProxy::getConnectResponce(void){
+int LGwProxy::getConnectResponce(void)
+{
     int len = readMsg();
 
-	if (len == 0){
-		if (_sendUTC + MQTTSN_TIME_RETRY < time(NULL)){
+    if (len == 0)
+    {
+        if (_sendUTC + MQTTSN_TIME_RETRY < time(NULL))
+        {
             if (_msg[1] == MQTTSN_TYPE_CONNECT)
             {
                 _connectRetry--;
             }
-			if (--_retryCount > 0){
-				writeMsg((const uint8_t*)_msg);  // Not writeGwMsg() : not to reset the counter.
+            if (--_retryCount > 0)
+            {
+                writeMsg((const uint8_t*) _msg);  // Not writeGwMsg() : not to reset the counter.
                 _sendUTC = time(NULL);
-			}else{
+            }
+            else
+            {
                 _sendUTC = 0;
-				if ( _status > GW_SEARCHING && _connectRetry > 0){
+                if (_status > GW_SEARCHING && _connectRetry > 0)
+                {
                     _status = GW_CONNECTING;
-				}else{
+                }
+                else
+                {
                     _status = GW_LOST;
                     _gwId = 0;
                 }
@@ -151,55 +195,107 @@ int LGwProxy::getConnectResponce(void){
             }
         }
         return 0;
-	}else if (_mqttsnMsg[0] == MQTTSN_TYPE_GWINFO && _status == GW_SEARCHING){
+    }
+    else if (_mqttsnMsg[0] == MQTTSN_TYPE_GWINFO && _status == GW_SEARCHING)
+    {
         _network.setGwAddress();
         _gwId = _mqttsnMsg[1];
+
+#if defined(DTLS) || defined(DTLS6)
+        for (int i = 0; i < MQTTSN_RETRY_COUNT; i++)
+        {
+            if (_network.sslConnect() > 0)
+            {
                 _status = GW_CONNECTING;
-	}else if (_mqttsnMsg[0] == MQTTSN_TYPE_WILLTOPICREQ && _status == GW_WAIT_WILLTOPICREQ){
+                DISPLAY("\033[0m\033[0;32m\n\nLGwProxy::getConnectResponce SSL connection established.\033[0m\033[0;37m\n\n");
+                break;
+            }
+            else
+            {
+                DISPLAY("\033[0m\033[0;32m\n\nLGwProxy::getConnectResponce SSL connection failed.\033[0m\033[0;37m\n\n");
+            }
+        }
+#else
+        _status = GW_CONNECTING;
+#endif
+    }
+    else if (_mqttsnMsg[0] == MQTTSN_TYPE_WILLTOPICREQ && _status == GW_WAIT_WILLTOPICREQ)
+    {
         _status = GW_SEND_WILLTOPIC;
-	}else if (_mqttsnMsg[0] == MQTTSN_TYPE_WILLMSGREQ && _status == GW_WAIT_WILLMSGREQ){
+    }
+    else if (_mqttsnMsg[0] == MQTTSN_TYPE_WILLMSGREQ && _status == GW_WAIT_WILLMSGREQ)
+    {
         _status = GW_SEND_WILLMSG;
-	}else if (_mqttsnMsg[0] == MQTTSN_TYPE_CONNACK && _status == GW_WAIT_CONNACK){
-		if (_mqttsnMsg[1] == MQTTSN_RC_ACCEPTED){
+    }
+    else if (_mqttsnMsg[0] == MQTTSN_TYPE_CONNACK && _status == GW_WAIT_CONNACK)
+    {
+        if (_mqttsnMsg[1] == MQTTSN_RC_ACCEPTED)
+        {
             _status = GW_CONNECTED;
             _connectRetry = MQTTSN_RETRY_COUNT;
-			_keepAliveTimer.start(_tkeepAlive * 1000);
-			_topicTbl.clearTopic();
+            setPingReqTimer();
+            if (_tSleep)
+            {
+                _tSleep = 0;
+            }
+            else
+            {
                 DISPLAY("\033[0m\033[0;32m\n\n Connected to the Broker\033[0m\033[0;37m\n\n");
+
+                if (_cleanSession || _initialized == 1)
+                {
+                    _topicTbl.clearTopic();
+                    _initialized = 0;
                     theClient->onConnect();  // SUBSCRIBEs are conducted
-		}else{
+                }
+            }
+        }
+        else
+        {
             _status = GW_CONNECTING;
         }
     }
     return 1;
 }
 
-void LGwProxy::reconnect(void){
+void LGwProxy::reconnect(void)
+{
+	if (_isAutoConnectMode)
+	{
 		D_MQTTLOG("...Gateway reconnect\r\n");
 		_status = GW_DISCONNECTED;
 		connect();
+	}
 }
 
-void LGwProxy::disconnect(uint16_t secs){
+void LGwProxy::disconnect(uint16_t secs)
+{
     _tSleep = secs;
-    _status = GW_DISCONNECTING;
+    _tWake = 0;
 
     _msg[1] = MQTTSN_TYPE_DISCONNECT;
 
-	if (secs){
+    if (secs)
+    {
         _msg[0] = 4;
         setUint16((uint8_t*) _msg + 2, secs);
-	}else{
+        _status = GW_SLEEPING;
+    }
+    else
+    {
         _msg[0] = 2;
         _keepAliveTimer.stop();
+        _status = GW_DISCONNECTING;
     }
 
     _retryCount = MQTTSN_RETRY_COUNT;
-	writeMsg((const uint8_t*)_msg);
+    writeMsg((const uint8_t*) _msg);
     _sendUTC = time(NULL);
 
-	while ( _status != GW_DISCONNECTED && _status != GW_SLEPT){
-		if (getDisconnectResponce() < 0){
+    while (_status != GW_DISCONNECTED && _status != GW_SLEPT)
+    {
+        if (getDisconnectResponce() < 0)
+        {
             _status = GW_LOST;
             DISPLAY("\033[0m\033[0;31m\n\n!!!!!! DISCONNECT  Error !!!!!\033[0m\033[0;37m \n\n");
             return;
@@ -207,44 +303,63 @@ void LGwProxy::disconnect(uint16_t secs){
     }
 }
 
-int LGwProxy::getDisconnectResponce(void){
+int LGwProxy::getDisconnectResponce(void)
+{
     int len = readMsg();
 
-	if (len == 0){
-		if (_sendUTC + MQTTSN_TIME_RETRY < time(NULL)){
-			if (--_retryCount >= 0){
-				writeMsg((const uint8_t*)_msg);
+    if (len == 0)
+    {
+        if (_sendUTC + MQTTSN_TIME_RETRY < time(NULL))
+        {
+            if (--_retryCount >= 0)
+            {
+                writeMsg((const uint8_t*) _msg);
                 _sendUTC = time(NULL);
-			}else{
+            }
+            else
+            {
                 _status = GW_LOST;
                 _gwId = 0;
                 return -1;
             }
         }
         return 0;
-	}else if (_mqttsnMsg[0] == MQTTSN_TYPE_DISCONNECT){
-		if (_tSleep){
-			_status = GW_SLEEPING;
-			_keepAliveTimer.start(_tSleep);
-		}else{
+    }
+    else if (_mqttsnMsg[0] == MQTTSN_TYPE_DISCONNECT)
+    {
+        if (_status == GW_SLEEPING)
+        {
+            _status = GW_SLEPT;
+            uint32_t remain = _keepAliveTimer.getRemain();
+            theClient->setSleepMode(remain);
+
+            /* Wake up and starts from this point. */
+
+        }
+        else
+        {
             _status = GW_DISCONNECTED;
         }
     }
     return 0;
 }
 
-int LGwProxy::getMessage(void){
+int LGwProxy::getMessage(void)
+{
     int len = readMsg();
-	if (len < 0){
+    if (len < 0)
+    {
         return len;   //error
     }
 #ifdef DEBUG_MQTTSN
-	if (len){
+    if (len)
+    {
         D_MQTTLOG(" recved msgType %x\n", _mqttsnMsg[0]);
     }
 #endif
 
-	if (len == 0){
+    if (len == 0)
+    {
         // Check PINGREQ required
         checkPingReq();
 
@@ -260,166 +375,277 @@ int LGwProxy::getMessage(void){
         // Check Timeout of SUBSCRIBEs,
         theClient->getSubscribeManager()->checkTimeout();
 
-	}else if (_mqttsnMsg[0] == MQTTSN_TYPE_PUBLISH){
+    }
+    else if (_mqttsnMsg[0] == MQTTSN_TYPE_PUBLISH)
+    {
         theClient->getPublishManager()->published(_mqttsnMsg, len);
 
-    }else if (_mqttsnMsg[0] == MQTTSN_TYPE_PUBACK || _mqttsnMsg[0] == MQTTSN_TYPE_PUBCOMP ||
-			_mqttsnMsg[0] == MQTTSN_TYPE_PUBREC || _mqttsnMsg[0] == MQTTSN_TYPE_PUBREL ){
-        theClient->getPublishManager()->responce(_mqttsnMsg, (uint16_t)len);
+    }
+    else if (_mqttsnMsg[0] == MQTTSN_TYPE_PUBACK || _mqttsnMsg[0] == MQTTSN_TYPE_PUBCOMP
+            || _mqttsnMsg[0] == MQTTSN_TYPE_PUBREC || _mqttsnMsg[0] == MQTTSN_TYPE_PUBREL)
+    {
+        theClient->getPublishManager()->responce(_mqttsnMsg, (uint16_t) len);
 
-    }else if (_mqttsnMsg[0] == MQTTSN_TYPE_SUBACK || _mqttsnMsg[0] == MQTTSN_TYPE_UNSUBACK){
+    }
+    else if (_mqttsnMsg[0] == MQTTSN_TYPE_SUBACK || _mqttsnMsg[0] == MQTTSN_TYPE_UNSUBACK)
+    {
         theClient->getSubscribeManager()->responce(_mqttsnMsg);
 
-    }else if (_mqttsnMsg[0] == MQTTSN_TYPE_REGISTER){
+    }
+    else if (_mqttsnMsg[0] == MQTTSN_TYPE_REGISTER)
+    {
         _regMgr.responceRegister(_mqttsnMsg, len);
 
-    }else if (_mqttsnMsg[0] == MQTTSN_TYPE_REGACK){
+    }
+    else if (_mqttsnMsg[0] == MQTTSN_TYPE_REGACK)
+    {
         _regMgr.responceRegAck(getUint16(_mqttsnMsg + 3), getUint16(_mqttsnMsg + 1));
 
-    }else if (_mqttsnMsg[0] == MQTTSN_TYPE_PINGRESP){
-        if (_pingStatus == GW_WAIT_PINGRESP){
-            _pingStatus = 0;
-            resetPingReqTimer();
     }
-    }else if (_mqttsnMsg[0] == MQTTSN_TYPE_DISCONNECT){
-    	_status = GW_LOST;
+    else if (_mqttsnMsg[0] == MQTTSN_TYPE_PINGRESP)
+    {
+        if (_pingStatus == GW_WAIT_PINGRESP)
+        {
+            _pingStatus = 0;
+            setPingReqTimer();
+
+            if (_tSleep > 0)
+            {
+                _tWake += _tkeepAlive;
+                if (_tWake < _tSleep)
+                {
+                    theClient->setSleepMode(_tkeepAlive * 1000UL);
+                }
+                else
+                {
+                    DISPLAY("\033[0m\033[0;32m\n\n Get back to ACTIVE.\033[0m\033[0;37m\n\n");
+                    _tWake = 0;
+                    connect();
+                }
+            }
+        }
+    }
+    else if (_mqttsnMsg[0] == MQTTSN_TYPE_DISCONNECT)
+    {
+		_status = GW_DISCONNECTED;
         _gwAliveTimer.stop();
         _keepAliveTimer.stop();
-    }else if (_mqttsnMsg[0] == MQTTSN_TYPE_ADVERTISE){
-    	if (getUint16((const uint8_t*)(_mqttsnMsg + 2)) < 61){
-    		_tAdv = getUint16((const uint8_t*)(_mqttsnMsg + 2)) * 1500;
-    	}else{
-    		_tAdv = getUint16((const uint8_t*)(_mqttsnMsg + 2)) * 1100;
+    }
+    else if (_mqttsnMsg[0] == MQTTSN_TYPE_ADVERTISE)
+    {
+        if (getUint16((const uint8_t*) (_mqttsnMsg + 2)) < 61)
+        {
+            _tAdv = getUint16((const uint8_t*) (_mqttsnMsg + 2)) * 1500;
+        }
+        else
+        {
+            _tAdv = getUint16((const uint8_t*) (_mqttsnMsg + 2)) * 1100;
         }
         _gwAliveTimer.start(_tAdv);
     }
     return 0;
 }
 
-
-
-
-uint16_t LGwProxy::registerTopic(char* topicName, uint16_t topicId){
+uint16_t LGwProxy::registerTopic(char* topicName, uint16_t topicId)
+{
     uint16_t id = topicId;
-    if (id == 0){
+    if (id == 0)
+    {
         id = _topicTbl.getTopicId(topicName);
         _regMgr.registerTopic(topicName);
     }
     return id;
 }
 
-
-int LGwProxy::writeMsg(const uint8_t* msg){
+int LGwProxy::writeMsg(const uint8_t* msg)
+{
     uint16_t len;
     uint8_t pos;
-	uint8_t  rc;
+    uint8_t rc = 0;
 
-	if (msg[0] == 0x01){
+    if (msg[0] == 0x01)
+    {
         len = getUint16(msg + 1);
         pos = 2;
-	}else{
+    }
+    else
+    {
         len = msg[0];
         pos = 1;
     }
 
-	if (msg[0] == 3 && msg[1] == MQTTSN_TYPE_SEARCHGW){
-		rc = _network.broadcast(msg,len);
-	}else{
-		rc = _network.unicast(msg,len);
+    if (msg[0] == 3 && msg[1] == MQTTSN_TYPE_SEARCHGW)
+    {
+        rc = _network.broadcast(msg, len);
+    }
+    else
+    {
+        if (_isForwarderMode)
+        {
+            // create a forwarder encapsulation message  WirelessNodeId is a 4bytes fake data
+            uint8_t* buf = (uint8_t*) malloc(len + 7);
+            buf[0] = 7;
+            buf[1] = MQTTSN_TYPE_ENCAPSULATED;
+            buf[2] = 1;
+            buf[3] = 'w';
+            buf[4] = 'n';
+            buf[5] = 'I';
+            buf[6] = 'd';
+            memcpy(buf + 7, msg, len);
+            if (buf)
+                rc = _network.unicast(buf, len + 7);
+            free(buf);
+            DISPLAY("  Encapsulated\n ");
+        }
+        else
+        {
+            rc = _network.unicast(msg, len);
         }
 
-	if (rc > 0){
-		if ( msg[pos] >= MQTTSN_TYPE_ADVERTISE && msg[pos] <= MQTTSN_TYPE_WILLMSGRESP )
+        if (rc > 0)
+        {
+            if (msg[pos] >= MQTTSN_TYPE_ADVERTISE && msg[pos] <= MQTTSN_TYPE_WILLMSGRESP)
             {
                 DISPLAY("  send %s\n", packet_names[msg[pos]]);
             }
-		return rc;
         }
-	//_status = GW_LOST;
-	//_gwId = 0;
+    }
     return rc;
 }
 
-void LGwProxy::writeGwMsg(void){
+void LGwProxy::writeGwMsg(void)
+{
     _retryCount = MQTTSN_RETRY_COUNT;
-	writeMsg((const uint8_t*)_msg);
+    writeMsg((const uint8_t*) _msg);
     _sendUTC = time(NULL);
 }
 
-int LGwProxy::readMsg(void){
+int LGwProxy::readMsg(void)
+{
     int len = 0;
-	_mqttsnMsg = _network.getMessage(&len);
-	if (len == 0){
+    uint8_t* msg = _network.getMessage(&len);
+    _mqttsnMsg = msg;
+
+    if (len == 0)
+    {
         return 0;
     }
 
-	if (_mqttsnMsg[0] == 0x01){
-		int msgLen = (int) getUint16((const uint8_t*)_mqttsnMsg + 1);
-		if (len != msgLen){
+    if (_mqttsnMsg[0] == 0x01)
+    {
+        int msgLen = (int) getUint16((const uint8_t*) _mqttsnMsg + 1);
+        if (len != msgLen)
+        {
             _mqttsnMsg += 3;
             len = msgLen - 3;
         }
-	}else{
+    }
+    else
+    {
         _mqttsnMsg += 1;
         len -= 1;
     }
-	if ( *_mqttsnMsg >= MQTTSN_TYPE_ADVERTISE && *_mqttsnMsg <= MQTTSN_TYPE_WILLMSGRESP )
+
+    if (*_mqttsnMsg == MQTTSN_TYPE_ENCAPSULATED)
+    {
+        int lenEncap = len + 1;
+
+        if (msg[lenEncap] == 0x01)
+        {
+            int msgLen = (int) getUint16((const uint8_t*) (msg + lenEncap + 1));
+            msg += (lenEncap + 3);
+            len = msgLen - 3;
+        }
+        else
+        {
+            msg += (lenEncap + 1);
+            len = *(msg - 1);
+        }
+        _mqttsnMsg = msg;
+        DISPLAY("  recv encapslated message\n");
+    }
+
+    if (*_mqttsnMsg >= MQTTSN_TYPE_ADVERTISE && *_mqttsnMsg <= MQTTSN_TYPE_WILLMSGRESP)
     {
         DISPLAY("  recv %s\n", packet_names[*_mqttsnMsg]);
     }
     return len;
 }
 
-void LGwProxy::setWillTopic(const char* willTopic, uint8_t qos, bool retain){
+void LGwProxy::setWillTopic(const char* willTopic, uint8_t qos, bool retain)
+{
     _willTopic = willTopic;
     _retainWill = _qosWill = 0;
-	if (qos == 1){
+    if (qos == 1)
+    {
         _qosWill = MQTTSN_FLAG_QOS_1;
-	}else if (qos == 2){
+    }
+    else if (qos == 2)
+    {
         _qosWill = MQTTSN_FLAG_QOS_2;
     }
-	if (retain){
+    if (retain)
+    {
         _retainWill = MQTTSN_FLAG_RETAIN;
     }
 }
-void LGwProxy::setWillMsg(const char* willMsg){
+void LGwProxy::setWillMsg(const char* willMsg)
+{
     _willMsg = willMsg;
 }
 
-
-void LGwProxy::setCleanSession(bool flg){
-	if (flg){
+void LGwProxy::setCleanSession(bool flg)
+{
+    if (flg)
+    {
         _cleanSession = MQTTSN_FLAG_CLEAN;
-	}else{
+    }
+    else
+    {
         _cleanSession = 0;
     }
 }
 
-uint16_t LGwProxy::getNextMsgId(void){
+uint16_t LGwProxy::getNextMsgId(void)
+{
     _nextMsgId++;
-    if (_nextMsgId == 0){
+    if (_nextMsgId == 0)
+    {
         _nextMsgId = 1;
     }
     return _nextMsgId;
 }
 
-void LGwProxy::checkPingReq(void){
+void LGwProxy::checkPingReq(void)
+{
+	if (_isQoSMinus1Mode || _isPingReqMode == false)
+    {
+        return;
+    }
+
     uint8_t msg[2];
     msg[0] = 0x02;
     msg[1] = MQTTSN_TYPE_PINGREQ;
 
-	if (_status == GW_CONNECTED && _keepAliveTimer.isTimeUp() && _pingStatus != GW_WAIT_PINGRESP){
+    if ((_status == GW_CONNECTED || _status == GW_SLEPT) && isPingReqRequired() && _pingStatus != GW_WAIT_PINGRESP)
+    {
         _pingStatus = GW_WAIT_PINGRESP;
         _pingRetryCount = MQTTSN_RETRY_COUNT;
 
-		writeMsg((const uint8_t*)msg);
+        writeMsg((const uint8_t*) msg);
         _pingSendUTC = time(NULL);
-	}else if (_pingStatus == GW_WAIT_PINGRESP){
-        if (_pingSendUTC + MQTTSN_TIME_RETRY < time(NULL)){
-    		if (--_pingRetryCount > 0){
-				writeMsg((const uint8_t*)msg);
+    }
+    else if (_pingStatus == GW_WAIT_PINGRESP)
+    {
+        if (_pingSendUTC + MQTTSN_TIME_RETRY < time(NULL))
+        {
+            if (--_pingRetryCount > 0)
+            {
+                writeMsg((const uint8_t*) msg);
                 _pingSendUTC = time(NULL);
-			}else{
+            }
+            else
+            {
                 _status = GW_LOST;
                 _gwId = 0;
                 _pingStatus = 0;
@@ -430,8 +656,10 @@ void LGwProxy::checkPingReq(void){
     }
 }
 
-void LGwProxy::checkAdvertise(void){
-	if ( _gwAliveTimer.isTimeUp()){
+void LGwProxy::checkAdvertise(void)
+{
+    if (_gwAliveTimer.isTimeUp())
+    {
         _status = GW_LOST;
         _gwId = 0;
         _pingStatus = 0;
@@ -441,18 +669,57 @@ void LGwProxy::checkAdvertise(void){
     }
 }
 
-LTopicTable* LGwProxy::getTopicTable(void){
+LTopicTable* LGwProxy::getTopicTable(void)
+{
     return &_topicTbl;
 }
 
-LRegisterManager* LGwProxy::getRegisterManager(void){
+LRegisterManager* LGwProxy::getRegisterManager(void)
+{
     return &_regMgr;
 }
 
-void LGwProxy::resetPingReqTimer(void){
-	_keepAliveTimer.start(_tkeepAlive * 1000);
+bool LGwProxy::isPingReqRequired(void)
+{
+    return _keepAliveTimer.isTimeUp(_tkeepAlive * 1000UL);
 }
 
-const char* LGwProxy::getClientId(void) {
+void LGwProxy::setPingReqTimer(void)
+{
+    _keepAliveTimer.start(_tkeepAlive * 1000UL);
+}
+
+const char* LGwProxy::getClientId(void)
+{
     return _clientId;
 }
+
+void LGwProxy::setForwarderMode(bool valid)
+{
+    _isForwarderMode = valid;
+}
+
+void LGwProxy::setQoSMinus1Mode(bool valid)
+{
+    _isQoSMinus1Mode = valid;
+}
+
+void LGwProxy::setPingReqMode(bool valid)
+{
+	_isPingReqMode = valid;
+}
+
+void LGwProxy::setAutoConnectMode(bool valid)
+{
+	_isAutoConnectMode = valid;
+}
+
+void LGwProxy::setSessionMode(bool valid)
+{
+	_cleanSession = valid;
+}
+
+uint8_t LGwProxy::getStatus(void)
+{
+	return _status;
+}

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
 
@@ -53,7 +59,7 @@ public:
     LGwProxy();
     ~LGwProxy();
 
-	void     initialize(LUdpConfig netconf, LMqttsnConfig mqconf);
+    void initialize(SENSORNET_CONFIG_t* netconf, LMqttsnConfig* mqconf);
     void     connect(void);
     void     disconnect(uint16_t sec = 0);
     int      getMessage(void);
@@ -64,13 +70,19 @@ public:
     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     resetPingReqTimer(void);
+    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);
@@ -78,6 +90,7 @@ private:
     void     checkAdvertise(void);
     int      getConnectResponce(void);
     int      getDisconnectResponce(void);
+    bool     isPingReqRequired(void);
 
     LNetwork     _network;
     uint8_t*    _mqttsnMsg;
@@ -86,16 +99,17 @@ private:
     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;
-	uint32_t    _sendUTC;
+    time_t      _sendUTC;
     int         _retryCount;
     int         _connectRetry;
     uint8_t     _status;
-	uint32_t    _pingSendUTC;
+    time_t    _pingSendUTC;
     uint8_t     _pingRetryCount;
     uint8_t     _pingStatus;
     LRegisterManager _regMgr;
@@ -103,6 +117,11 @@ private:
     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];
 };
 

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,20 +40,36 @@ 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)
@@ -73,11 +90,13 @@ int run(void)
 			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()
 {
+	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)
 {
+	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);
+}
 

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;
 };
 
 

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_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,14 +249,10 @@ 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.0.0"
 
 #define PAHO_COPYRIGHT0 " * MQTT-SN Gateway Tester"
 #define PAHO_COPYRIGHT1 " * Part of Project Paho in Eclipse"

MQTTSNGateway/GatewayTester/src/LNetworkDtls.cpp

@@ -0,0 +1,547 @@
+/**************************************************************************************
+ * 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");
+        DISPLAY("\033[0m\033[0;31m\nerror 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");
+        DISPLAY("\033[0m\033[0;31m\nerror 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);
+                D_NWLOG("errno == %d in LDtlsPort::unicast\n", 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 = 0;
+    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
+

MQTTSNGateway/GatewayTester/src/LNetworkDtls.h

@@ -0,0 +1,117 @@
+/**************************************************************************************
+ * 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
+
+#define STAT_UNICAST   1
+#define STAT_MULTICAST 2
+#define STAT_SSL 3
+
+using namespace std;
+
+namespace linuxAsyncClient {
+/*========================================
+ 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_ */

MQTTSNGateway/GatewayTester/src/LNetworkDtls6.cpp

@@ -0,0 +1,533 @@
+/**************************************************************************************
+ * 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 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)
+{
+    _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;
+    memset(_pollfds, 0, sizeof(_pollfds));
+    _sock = 0;
+    _castStat = 0;
+    _ifIndex = 0;
+    _gIpAddrStr = NULL;
+}
+
+LDtls6Port::~LDtls6Port()
+{
+    close();
+    if (_gIpAddrStr)
+    {
+        free(_gIpAddrStr);
+    }
+}
+
+void LDtls6Port::close()
+{
+    for (int i = 0; i < 2; i++)
+    {
+        if (_pollfds[i].fd > 0)
+        {
+            ::close(_pollfds[i].fd);
+            _pollfds[i].fd = 0;
+        }
+    }
+}
+
+bool LDtls6Port::open(LUdp6Config *config)
+{
+    int optval = 1;
+    int sock = 0;
+    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 */
+    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 = 0;
+    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;
+}
+
+bool LDtls6Port::isUnicast()
+{
+    return (_sock == _pollfds[0].fd && _sock > 0);
+}
+
+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 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 LDtls6Port::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 LDtls6Port::checkRecvBuf()
+{
+    uint8_t buf[2];
+
+    int cnt = poll(_pollfds, 2, 2000);  // Timeout 2secs
+    if (cnt == 0)
+    {
+        return false;
+    }
+
+    if (_pollfds[0].revents & POLLIN)
+    {
+        if (::recv(_pollfds[0].fd, buf, 1, MSG_DONTWAIT | MSG_PEEK) > 0)
+        {
+            _castStat = STAT_SSL;
+            _sock = _pollfds[0].fd;
+            return true;
+        }
+    }
+    else if (_pollfds[1].revents & POLLIN)
+    {
+        if (::recv(_pollfds[1].fd, buf, 1, MSG_DONTWAIT | MSG_PEEK) > 0)
+        {
+            _castStat = STAT_MULTICAST;
+            _sock = _pollfds[1].fd;
+            return true;
+        }
+    }
+    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(_sock, 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, uint16_t portNo)
+{
+    int optval = 1;
+    int sock = _pollfds[0].fd;
+
+    if (_ssl != 0)
+    {
+        SSL_free(_ssl);
+    }
+
+    if (sock > 0)
+    {
+        ::close(sock);
+    }
+
+    sock = socket(AF_INET6, SOCK_DGRAM, 0);
+    if (sock <= 0)
+    {
+        return -1;
+    }
+    optval = 1;
+    setsockopt(sock, IPPROTO_IPV6, IPV6_V6ONLY, &optval, sizeof(optval));
+    setsockopt(sock, SOL_SOCKET, SO_REUSEADDR || SO_REUSEPORT, &optval, sizeof(optval));
+
+    if (_ifIndex > 0)
+    {
+#ifdef __APPLE__
+        setsockopt(sock, IPPROTO_IP, IP_BOUND_IF, &_ifIndex, sizeof(_ifIndex));
+#else
+        setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, _interfaceName.c_str(), _interfaceName.size());
+#endif
+    }
+
+    struct sockaddr_in6 addr;
+    addr.sin6_family = AF_INET6;
+    addr.sin6_port = _uPortNo;
+    addr.sin6_addr = in6addr_any;
+
+    if (::bind(sock, (struct sockaddr*) &addr, sizeof(addr)) < 0)
+    {
+        return -1;
+    }
+    _pollfds[0].fd = sock;
+    _pollfds[0].events = POLLIN;
+
+    uint16_t listenPort = htons(portNo);
+    struct sockaddr_in6 dest;
+    dest.sin6_family = AF_INET6;
+    dest.sin6_port = htons(listenPort);
+    memcpy(dest.sin6_addr.s6_addr, (const void*) ipAddress.s6_addr, sizeof(ipAddress.s6_addr));
+
+    int rc = 0;
+
+    BIO *cbio = BIO_new_dgram(sock, BIO_NOCLOSE);
+    connect(sock, (sockaddr*) &dest, sizeof(sockaddr_in6));
+    BIO_ctrl(cbio, BIO_CTRL_DGRAM_SET_CONNECTED, 0, &dest);
+    _ssl = SSL_new(_ctx);
+    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
+
+    errno = 0;
+    int stat = SSL_connect(_ssl);
+    if (stat != 1)
+    {
+        rc = -1;
+        D_NWLOG("SSL fail to connect %s\n",strerror(errno));
+    }
+    else
+    {
+        D_NWLOG("SSL connected\n");
+    }
+    return rc;
+}
+
+int verify_cert(int ok, X509_STORE_CTX *ctx)
+{
+    return 1;
+}
+
+#endif
+

MQTTSNGateway/GatewayTester/src/LNetworkDtls6.h

@@ -0,0 +1,121 @@
+/**************************************************************************************
+ * 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_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 );
+
+    SSL_CTX *_ctx;
+    SSL *_ssl;
+    pollfd _pollfds[2];
+    in_port_t _gPortNo;
+    in_port_t _uPortNo;
+    sockaddr_in6 _gIpAddr;
+    char *_gIpAddrStr;
+    uint32_t _ifIndex;
+    string _interfaceName;
+    uint8_t  _castStat;
+    int _sock;
+    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_ */

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
+

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_ */

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,176 +24,198 @@
 #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
  =========================================*/
-LNetwork::LNetwork(){
+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){
+int LNetwork::unicast(const uint8_t *xmitData, uint16_t dataLen)
+{
     return LUdpPort::unicast(xmitData, dataLen, _gwIpAddress, _gwPortNo);
 }
 
-
-uint8_t*  LNetwork::getMessage(int* len){
+uint8_t* LNetwork::getMessage(int *len)
+{
     *len = 0;
-	if (checkRecvBuf()){
+    if (checkRecvBuf())
+    {
         uint16_t recvLen = LUdpPort::recv(_rxDataBuf, MQTTSN_MAX_PACKET_SIZE, false, &_ipAddress, &_portNo);
-		if(_gwIpAddress && isUnicast() && (_ipAddress != _gwIpAddress) && (_portNo != _gwPortNo)){
+        if (_gwIpAddress && isUnicast() && (_ipAddress != _gwIpAddress) && (_portNo != _gwPortNo))
+        {
             return 0;
         }
 
-		if(recvLen < 0){
+        if (recvLen < 0)
+        {
             *len = recvLen;
             return 0;
-		}else{
-			if(_rxDataBuf[0] == 0x01){
-				*len = getUint16(_rxDataBuf + 1 );
-			}else{
+        }
+        else
+        {
+            if (_rxDataBuf[0] == 0x01)
+            {
+                *len = getUint16(_rxDataBuf + 1);
+            }
+            else
+            {
                 *len = _rxDataBuf[0];
             }
-			if(recvLen != *len){
-				*len = 0;
-				return 0;
-			}else{
+            //if(recvLen != *len){
+            //	*len = 0;
+            //	return 0;
+            //}else{
             return _rxDataBuf;
-			}
+            //}
         }
     }
     return 0;
 }
 
-void LNetwork::setGwAddress(void){
+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){
+void LNetwork::resetGwAddress(void)
+{
     _gwIpAddress = 0;
     _gwPortNo = 0;
 }
 
-
-bool LNetwork::initialize(LUdpConfig  config){
+bool LNetwork::initialize(LUdpConfig *config)
+{
     return LUdpPort::open(config);
 }
 
-void LNetwork::setSleep(){
+void LNetwork::setSleep()
+{
     _sleepflg = true;
 }
 
+bool LNetwork::isBroadcastable()
+{
+    return true;
+}
 /*=========================================
  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);
+void LUdpPort::close()
+{
+    if (_sockfdMcast > 0)
+    {
+        ::close(_sockfdMcast);
         _sockfdMcast = -1;
-	if(_sockfdUcast > 0){
-		::close( _sockfdUcast);
+        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){
+    if (_gPortNo == 0 || _gIpAddr == 0 || _uPortNo == 0)
+    {
         return false;
     }
 
     _sockfdUcast = socket(AF_INET, SOCK_DGRAM, 0);
-	if (_sockfdUcast < 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;
+    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){
+    if (::bind(_sockfdUcast, (struct sockaddr*) &addr, sizeof(addr)) < 0)
+    {
         return false;
     }
 
     _sockfdMcast = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
-	if (_sockfdMcast < 0){
+    if (_sockfdMcast < 0)
+    {
         return false;
     }
 
-	struct sockaddr_in addrm;
+    sockaddr_in addrm;
     addrm.sin_family = AF_INET;
     addrm.sin_port = _gPortNo;
-	addrm.sin_addr.s_addr = htonl(INADDR_ANY);
+    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(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();
+    if (::bind(_sockfdMcast, (struct sockaddr*) &addrm, sizeof(addrm)) < 0)
+    {
         return false;
     }
 
@@ -200,88 +223,101 @@ bool LUdpPort::open(LUdpConfig config){
     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){
+    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;
+    }
+
+    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  ){
+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){
+    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");
+    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 )
+        if (!theClientMode)
         {
             char sbuf[SCREEN_BUFF_SIZE];
             int pos = 0;
-			sprintf(sbuf,"\033[0;34msendto %-15s:%-6u",inet_ntoa(dest.sin_addr),htons(port));
+            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++){
+            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
+                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 LUdpPort::multicast( const uint8_t* buf, uint32_t length ){
+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){
+    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{
+    }
+    else
+    {
         D_NWLOG("sendto %-15s:%-6u",inet_ntoa(dest.sin_addr),htons(_gPortNo));
 
-		for(uint16_t i = 0; i < length ; i++){
+        for (uint16_t i = 0; i < length; i++)
+        {
             D_NWLOG(" %02x", *(buf + i));
-			DISPLAY(" %02x", *(buf + i));
-		}
-		D_NWLOG("\n");
+        } D_NWLOG("\n");
 
-		if ( !theClientMode )
+        if (!theClientMode)
         {
             char sbuf[SCREEN_BUFF_SIZE];
             int pos = 0;
-			sprintf(sbuf,"\033[0;34msendto %-15s:%-6u",inet_ntoa(dest.sin_addr),htons(_gPortNo));
+            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++){
+            for (uint16_t i = 0; i < length; i++)
+            {
                 sprintf(sbuf + pos, " %02x", *(buf + i));
-				if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20 )
+                if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20)
                 {
                     break;
                 }
@@ -295,7 +331,8 @@ int LUdpPort::multicast( const uint8_t* buf, uint32_t length ){
 
 }
 
-bool LUdpPort::checkRecvBuf(){
+bool LUdpPort::checkRecvBuf()
+{
     struct timeval timeout;
     timeout.tv_sec = 0;
     timeout.tv_usec = 50000;    // 50 msec
@@ -308,21 +345,29 @@ bool LUdpPort::checkRecvBuf(){
     FD_SET(_sockfdUcast, &recvfds);
     FD_SET(_sockfdMcast, &recvfds);
 
-	if(_sockfdMcast > _sockfdUcast){
+    if (_sockfdMcast > _sockfdUcast)
+    {
         maxSock = _sockfdMcast;
-	}else{
+    }
+    else
+    {
         maxSock = _sockfdUcast;
     }
 
     select(maxSock + 1, &recvfds, 0, 0, &timeout);
 
-	if(FD_ISSET(_sockfdUcast, &recvfds)){
-		if( ::recv(_sockfdUcast, buf, 1,  MSG_DONTWAIT | MSG_PEEK) > 0){
+    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){
+    }
+    else if (FD_ISSET(_sockfdMcast, &recvfds))
+    {
+        if (::recv(_sockfdMcast, buf, 1, MSG_DONTWAIT | MSG_PEEK) > 0)
+        {
             _castStat = STAT_MULTICAST;
             return true;
         }
@@ -331,46 +376,58 @@ bool LUdpPort::checkRecvBuf(){
     return false;
 }
 
-int LUdpPort::recv(uint8_t* buf, uint16_t len, bool flg, uint32_t* ipAddressPtr, uint16_t* 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 );
+    return recvfrom(buf, len, flags, ipAddressPtr, portPtr);
 }
 
-int LUdpPort::recvfrom (uint8_t* buf, uint16_t length, int flags, uint32_t* ipAddressPtr, uint16_t* 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);
 
-	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{
+    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)	{
+    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){
+    }
+    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("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");
+        } D_NWLOG("\n");
 
-		if ( !theClientMode )
+        if (!theClientMode)
         {
             char sbuf[SCREEN_BUFF_SIZE];
             int pos = 0;
-			sprintf(sbuf, "\033[0;34mrecved %-15s:%-6u",inet_ntoa(sender.sin_addr), htons(*portPtr));
+            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++){
+            for (uint16_t i = 0; i < status; i++)
+            {
                 sprintf(sbuf + pos, " %02x", *(buf + i));
-				if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20 )
+                if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20)
                 {
                     break;
                 }
@@ -380,11 +437,13 @@ int LUdpPort::recvfrom (uint8_t* buf, uint16_t length, int flags, uint32_t* ipAd
             theScreen->display(sbuf);
         }
         return status;
-	}else{
+    }
+    else
+    {
         return 0;
     }
     return status;
 }
 
-
+#endif
 

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_ */

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, 2000);  // Timeout 2secs
+    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 */
+

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_ */

MQTTSNGateway/GatewayTester/src/LPublishManager.cpp

@@ -44,6 +44,7 @@ LPublishManager::LPublishManager()
     _last = 0;
     _elmCnt = 0;
     _publishedFlg = SAVE_TASK_INDEX;
+	_autoConnectFlg = false;
 }
 
 LPublishManager::~LPublishManager()
@@ -66,25 +67,22 @@ void LPublishManager::publish(const char* topicName, Payload* payload, uint8_t q
     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);
-}
-
-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 )
+    uint8_t topicType = MQTTSN_TOPIC_TYPE_SHORT;
+    if ( strlen(topicName) > 2 )
+    {
+        topicType = MQTTSN_TOPIC_TYPE_NORMAL;
+    }
+
+    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);
@@ -103,7 +101,7 @@ void LPublishManager::publish(uint16_t topicId, Payload* payload, uint8_t qos, b
 void LPublishManager::publish(uint16_t topicId, uint8_t* payload, uint16_t len, uint8_t qos, bool retain)
 {
     uint16_t msgId = 0;
-	if ( qos > 0 )
+    if ( qos > 0 && qos < 3 )
     {
         msgId = theClient->getGwProxy()->getNextMsgId();
     }
@@ -118,11 +116,14 @@ void LPublishManager::sendPublish(PubElement* elm)
         return;
     }
 
+	if (_autoConnectFlg)
+	{
 		theClient->getGwProxy()->connect();
+	}
 
     uint8_t msg[MQTTSN_MAX_MSG_LENGTH + 1];
     uint8_t org = 0;
-	if (elm->payloadlen > 128)
+    if (elm->payloadlen > 248)
     {
         msg[0] = 0x01;
         setUint16(msg + 1, elm->payloadlen + 9);
@@ -150,8 +151,8 @@ void LPublishManager::sendPublish(PubElement* elm)
     memcpy(msg + org + 7, elm->payload, elm->payloadlen);
 
     theClient->getGwProxy()->writeMsg(msg);
-	theClient->getGwProxy()->resetPingReqTimer();
-	if ((elm->flag & 0x60) == MQTTSN_FLAG_QOS_0)
+    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
@@ -286,7 +287,7 @@ void LPublishManager::published(uint8_t* msg, uint16_t msglen)
     }
 
     _publishedFlg = NEG_TASK_INDEX;
-	theClient->getTopicTable()->execCallback(topicId, msg + 6, msglen - 6, msg[1] & 0x03);
+    theClient->getTopicTable()->execCallback(topicId, msg + 6, msglen - 6,  (MQTTSN_topicTypes)(msg[1] & MQTTSN_TOPIC_TYPE));
     _publishedFlg = SAVE_TASK_INDEX;
 }
 
@@ -313,6 +314,11 @@ void LPublishManager::checkTimeout(void)
     }
 }
 
+void LPublishManager::setAutoConnectMode(bool flg)
+{
+	_autoConnectFlg = flg;
+}
+
 PubElement* LPublishManager::getElement(uint16_t msgId)
 {
     PubElement* elm = _first;
@@ -431,6 +437,10 @@ PubElement* LPublishManager::add(const char* topicName, uint16_t topicId, uint8_
     {
         elm->flag |= MQTTSN_FLAG_QOS_2;
     }
+    else if (qos == 3)
+    {
+        elm->flag |= MQTTSN_FLAG_QOS_M1;
+    }
     if (retain)
     {
         elm->flag |= MQTTSN_FLAG_RETAIN;
@@ -441,6 +451,15 @@ PubElement* LPublishManager::add(const char* topicName, uint16_t topicId, uint8_
         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;

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,6 +85,7 @@ private:
 	PubElement* _last;
 	uint8_t     _elmCnt;
 	uint8_t     _publishedFlg;
+	uint8_t _autoConnectFlg;
 };
 
 } /* tomyAsyncClient */

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

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;

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"

MQTTSNGateway/GatewayTester/src/LSubscribeManager.cpp

@@ -64,10 +64,19 @@ void LSubscribeManager::onConnect(void)
     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;
@@ -115,17 +124,18 @@ void LSubscribeManager::send(SubElement* elm)
     {
         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
+    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)
@@ -142,37 +152,50 @@ void LSubscribeManager::send(SubElement* elm)
 
     theClient->getGwProxy()->connect();
     theClient->getGwProxy()->writeMsg(msg);
-	theClient->getGwProxy()->resetPingReqTimer();
+    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)
+    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, 0, topicType, qos, onPublish);
+    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);
+    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);
+    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);
+    SubElement* elm = add(MQTTSN_TYPE_UNSUBSCRIBE, 0, MQTTSN_TOPIC_TYPE_PREDEFINED, topicId, 0, 0);
     send(elm);
 }
 
@@ -214,20 +237,19 @@ void LSubscribeManager::responce(const uint8_t* msg)
         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);
+                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
             {
-				remove(elm);
                 DISPLAY("\033[0m\033[0;31m SUBACK Invalid messageId. %s\033[0m\033[0;37m\n\n", getElement(msgId)->topicName);
+                remove(elm);
             }
         }
     }
@@ -237,22 +259,22 @@ void LSubscribeManager::responce(const uint8_t* msg)
         SubElement* elm = getElement(msgId);
         if (elm)
         {
-			LTopicTable* tt = theClient->getGwProxy()->getTopicTable();
-			tt->setCallback(elm->topicName, 0);
+            //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(getElement(msgId));
+            remove(elm);
         }
         else
         {
-			DISPLAY("\033[0m\033[0;31m UNSUBACK Invalid messageId. %s\033[0m\033[0;37m\n\n", getElement(msgId)->topicName);
-			remove(getElement(msgId));
+			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,
+SubElement* LSubscribeManager::add(uint8_t msgType, const char* topicName, MQTTSN_topicTypes topicType, uint16_t topicId,
         uint8_t qos, TopicCallback callback)
 {
     SubElement* elm = 0;
@@ -264,6 +286,7 @@ SubElement* LSubscribeManager::add(uint8_t msgType, const char* topicName, uint1
     {
         elm = getElement(topicId, topicType);
     }
+
     if ( elm  == 0 )
     {
         elm = (SubElement*) calloc(1, sizeof(SubElement));
@@ -358,7 +381,7 @@ SubElement* LSubscribeManager::getElement(const char* topicName, uint8_t msgType
     SubElement* elm = _first;
     while (elm)
     {
-		if (strcmp(elm->topicName, topicName) == 0 && elm->msgType == msgType)
+        if ( elm->msgType == msgType &&  strncmp(elm->topicName, topicName, strlen(topicName)) == 0 )
         {
             return elm;
         }
@@ -370,7 +393,7 @@ SubElement* LSubscribeManager::getElement(const char* topicName, uint8_t msgType
     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)

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;

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>
 
@@ -34,41 +33,48 @@ extern bool theClientMode;
 /*=====================================
  TaskManager
  ======================================*/
-LTaskManager::LTaskManager(void){
+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){
+void LTaskManager::run(void)
+{
     int i = 0;
     char c = 0;
     bool cancelFlg = false;
+    TestList test = { 0 };
+    TaskList task = { 0 };
 
-	if ( !theClientMode )
+    if (!theClientMode)
     {
         theClient->getGwProxy()->getMessage();
 
-			for (i = 0; _tests[i].testTask > 0; i++)
+        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' )
+                    if (toupper(c) == 'N')
                     {
 
                         DISPLAY("\033[0m\033[0;32m\n**** %s is canceled ****\033[0m\033[0;37m\n\n", _tests[i].testLabel);
@@ -76,7 +82,7 @@ void LTaskManager::run(void){
                         cancelFlg = true;
                         break;
                     }
-						else if ( toupper(c) == 'Y' )
+                    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("");
@@ -91,22 +97,21 @@ void LTaskManager::run(void){
                 }
             }
 
-				while ( true )
+            while (true)
             {
                 do
                 {
                     theClient->getGwProxy()->getMessage();
                 }
-					while(theClient->getPublishManager()->isMaxFlight() ||
-						   !theClient->getSubscribeManager()->isDone() ||
-						   !theClient->getRegisterManager()->isDone());
+                while (theClient->getPublishManager()->isMaxFlight() || !theClient->getSubscribeManager()->isDone()
+                        || !theClient->getRegisterManager()->isDone());
 
                 if (theClient->getPublishManager()->isDone())
                 {
                     break;
                 }
             }
-				if ( !cancelFlg )
+            if (!cancelFlg)
             {
                 DISPLAY("\033[0m\033[0;32m\n**** %s complete ****\033[0m\033[0;37m\n\n", _tests[i].testLabel);
             }
@@ -118,10 +123,9 @@ void LTaskManager::run(void){
         while (true)
         {
             theClient->getGwProxy()->getMessage();
-			for (_index = 0; _tasks[_index].callback > 0; _index++)
+            for (_index = 0; _tasks[_index].callback > task.callback; _index++)
             {
-				if ((_tasks[_index].prevTime + _tasks[_index].interval <= time(NULL)) &&
-					 _tasks[_index].count == 0)
+                if ((_tasks[_index].prevTime + _tasks[_index].interval <= time(NULL)) && _tasks[_index].count == 0)
                 {
                     _tasks[_index].prevTime = time(NULL);
                     (_tasks[_index].callback)();
@@ -132,9 +136,8 @@ void LTaskManager::run(void){
             {
                 theClient->getGwProxy()->getMessage();
             }
-			while(theClient->getPublishManager()->isMaxFlight() ||
-				   !theClient->getSubscribeManager()->isDone() ||
-				   !theClient->getRegisterManager()->isDone());
+            while (theClient->getPublishManager()->isMaxFlight() || !theClient->getSubscribeManager()->isDone()
+                    || !theClient->getRegisterManager()->isDone());
 
             if (theClient->getPublishManager()->isDone())
             {
@@ -144,19 +147,21 @@ void LTaskManager::run(void){
     }
 }
 
-uint8_t LTaskManager::getIndex(void){
+uint8_t LTaskManager::getIndex(void)
+{
     return _index;
 }
 
-void LTaskManager::done(uint8_t index){
-	if (_tasks )
+void LTaskManager::done(uint8_t index)
+{
+    if (_tasks)
     {
         if (_tasks[index].count > 0)
         {
             _tasks[index].count--;
         }
     }
-	if (_tests )
+    if (_tests)
     {
         if (_tests[index].count > 0)
         {
@@ -165,12 +170,13 @@ void LTaskManager::done(uint8_t index){
     }
 }
 
-void LTaskManager::suspend(uint8_t index){
-	if ( _tasks )
+void LTaskManager::suspend(uint8_t index)
+{
+    if (_tasks)
     {
         _tasks[index].count++;
     }
-	if ( _tests )
+    if (_tests)
     {
         _tests[index].count++;
     }

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;
 };
 

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;
+    }
+}

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;

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)

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;

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
-
-	

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).   
-
-### **step1. Build the gateway**   
-````
-$ git clone https://github.com/eclipse/paho.mqtt-sn.embedded-c   
+# 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 
-$ make   
-$ make install   
-$ make clean    
-````      
-MQTT-SNGateway, MQTT-SNLogmonitor and param.conf are copied into ../ directory.
+```
+In order to build a gateway, one sensor network argument is required. 
+```
+$ ./build.sh [udp|udp6|xbee|loralink|rfcomm]  
+```     
 
+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]
-````   
+``` 
+$ 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: 
 
-### **How to Change the configuration of the gateway**    
-**../gateway.conf**   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
-BrokerPortNo=1883
-BrokerSecurePortNo=8883    
-
-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    
+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
+```
+**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.    
+```
+#
+# CertsKey for TLS connections to a broker
+#
 
+#RootCAfile=/etc/ssl/certs/ca-certificates.crt
+#RootCApath=/etc/ssl/certs/
+#CertsKey=/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.        
+**CertsKey** 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
+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
-````    
+```
+**Baudrate** is a baudrate of xbee.    
+```
+#
+# LoRaLink
+#
 
-**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.    
+BaudrateLoRaLink=115200
+DeviceRxLoRaLink=/dev/loralinkRx
+DeviceTxLoRaLink=/dev/loralinkTx
+```
+https://github.com/ty4tw/MQTT-SN-LoRa    
 
+```
+#
+# Bluetooth RFCOMM
+#
 
+RFCOMMAddress=60:57:18:06:8B:72.*
+```
+**RFCOMMAddress** is a bluetooth mac address and channel. channel should be * for the gateway.
+```
+#
+# LOG
+#
 
-### ** How to monitor the gateway from remote. **
+ShearedMemory=NO
+```
 
-Uncomment line32 in MQTTSNGWDefined.h.
-
-`//#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
+```

MQTTSNGateway/build.sh

@@ -0,0 +1,42 @@
+#!/bin/bash
+
+build () {
+    echo "Start building MQTT-SN Gateway  $1"
+    
+    cd $SCRIPT_DIR/..
+    BDIR='build.gateway'
+    if [ ! -d ./$BDIR ]; then
+        mkdir $BDIR
+    fi
+    cd $BDIR
+    cmake .. -DSENSORNET=$1 -DDEFS="${2} ${3}"
+    make MQTTSNPacket
+    make MQTT-SNGateway
+    make MQTT-SNLogmonitor
+    cd  ../MQTTSNGateway
+    cp *.conf ./bin/
+}
+
+SCRIPT_DIR=$(cd $(dirname $0); pwd)
+
+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
+    rm -rf ../builg.gateway
+else
+    echo "Usage: build.sh  [ udp | udp6 | xbee | loralink | rfcomm | dtls | dtls6 | clean]"
+fi
+
+

MQTTSNGateway/clients.conf

@@ -11,8 +11,37 @@
 #   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, write "unstableLine".
+#     if Broker's Connection is SSL, write "secureConnection".
+#     if the client is a forwarder, "forwarder" is required.
+#     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.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
+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
+

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/
+#CertsKey=/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
 

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

MQTTSNGateway/src/CMakeLists.txt

@@ -0,0 +1,150 @@
+#*******************************************************************************
+#  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")
+
+
+TARGET_INCLUDE_DIRECTORIES(mqtt-sngateway_common
+       PUBLIC
+       .
+       ${OS}
+       ${OS}/${SENSORNET}
+       ../../MQTTSNPacket/src
+       /usr/local/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)

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
@@ -49,8 +49,7 @@ void MQTTGWConnectionHandler::handleConnack(Client* client, MQTTGWPacket* packet
     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());
+        WRITELOG(" ClientID : %s ClientID is collect UTF-8 but not allowed by the Server.\n", client->getClientId());
     }
     else if (resp.rc == MQTT_SERVER_UNAVAILABLE)
     {
@@ -76,14 +75,6 @@ void MQTTGWConnectionHandler::handleConnack(Client* client, MQTTGWPacket* packet
     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);
-	}
 }
 
 void MQTTGWConnectionHandler::handlePingresp(Client* client, MQTTGWPacket* packet)
@@ -92,6 +83,7 @@ void MQTTGWConnectionHandler::handlePingresp(Client* client, MQTTGWPacket* packe
     snPacket->setPINGRESP();
     Event* ev1 = new Event();
     ev1->setClientSendEvent(client, snPacket);
+    client->updateStatus(snPacket);
     _gateway->getClientSendQue()->post(ev1);
 }
 

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
@@ -46,7 +48,8 @@ int MQTTPacket_encode(char* buf, int length)
         if (length > 0)
             d |= 0x80;
         buf[rc++] = d;
-	} while (length > 0);
+    }
+    while (length > 0);
     return rc;
 }
 
@@ -84,8 +87,8 @@ char* readUTFlen(char** pptr, char* enddata, int* len)
         *len = readInt(pptr);
         if (&(*pptr)[*len] <= enddata)
         {
-			string = (char*)calloc(*len + 1, 1);
-			memcpy(string, *pptr, (size_t)*len);
+            string = (char*) calloc(*len + 1, 1);
+            memcpy(string, *pptr, (size_t) *len);
             string[*len] = '\0';
             *pptr += *len;
         }
@@ -139,9 +142,9 @@ void writeChar(unsigned char** pptr, char c)
  */
 void writeInt(unsigned char** pptr, int anInt)
 {
-	**pptr = (unsigned char)(anInt / 256);
+    **pptr = (unsigned char) (anInt / 256);
     (*pptr)++;
-	**pptr = (unsigned char)(anInt % 256);
+    **pptr = (unsigned char) (anInt % 256);
     (*pptr)++;
 }
 
@@ -152,9 +155,9 @@ void writeInt(unsigned char** pptr, int anInt)
  */
 void writeUTF(unsigned char** pptr, const char* string)
 {
-	int len = (int)strlen(string);
+    int len = (int) strlen(string);
     writeInt(pptr, len);
-	memcpy(*pptr, string, (size_t)len);
+    memcpy(*pptr, string, (size_t) len);
     *pptr += len;
 }
 
@@ -184,8 +187,8 @@ int MQTTGWPacket::recv(Network* network)
     unsigned char c;
 
     /* read First Byte of Packet */
-	int rc = network->recv((unsigned char*)&_header.byte, 1);
-	if ( rc <= 0)
+    int rc = network->recv((unsigned char*) &_header.byte, 1);
+    if (rc <= 0)
     {
         return rc;
     }
@@ -202,11 +205,14 @@ int MQTTGWPacket::recv(Network* network)
         }
         _remainingLength += (c & 127) * multiplier;
         multiplier *= 128;
-	} while ((c & 128) != 0);
+    }
+    while ((c & 128) != 0);
 
+    if (_remainingLength > 0)
+    {
         /* allocate buffer */
-	_data = (unsigned char*)calloc(_remainingLength, 1);
-	if ( !_data )
+        _data = (unsigned char*) calloc(_remainingLength, 1);
+        if (!_data)
         {
             return -3;
         }
@@ -214,14 +220,15 @@ int MQTTGWPacket::recv(Network* network)
         /* read Payload */
         int remlen = network->recv(_data, _remainingLength);
 
-	if (remlen == -1 )
+        if (remlen == -1)
         {
             return -1;
         }
-	else if ( remlen != _remainingLength )
+        else if (remlen != _remainingLength)
         {
             return -2;
         }
+    }
     return 1 + len + _remainingLength;
 }
 
@@ -302,21 +309,21 @@ int MQTTGWPacket::setCONNECT(Connect* connect, unsigned char* username, unsigned
     clearData();
     _header = connect->header;
 
-	_remainingLength = ((connect->version == 3) ? 12 : 10) + (int)strlen(connect->clientID) + 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;
+        _remainingLength += (int) strlen(connect->willTopic) + 2 + (int) strlen(connect->willMsg) + 2;
     }
-	if ( connect->flags.bits.username )
+    if (connect->flags.bits.username)
     {
-		_remainingLength += (int)strlen((char*) username) + 2;
+        _remainingLength += (int) strlen((char*) username) + 2;
     }
     if (connect->flags.bits.password)
     {
-		_remainingLength += (int)strlen((char*) password) + 2;
+        _remainingLength += (int) strlen((char*) password) + 2;
     }
 
-	_data = (unsigned char*)calloc(_remainingLength, 1);
+    _data = (unsigned char*) calloc(_remainingLength, 1);
     unsigned char* ptr = _data;
 
     if (connect->version == 3)
@@ -360,8 +367,8 @@ int MQTTGWPacket::setSUBSCRIBE(const char* topic, unsigned char qos, unsigned sh
     _header.byte = 0;
     _header.bits.type = SUBSCRIBE;
     _header.bits.qos = 1;          // Reserved
-	_remainingLength = (int)strlen(topic) + 5;
-	_data = (unsigned char*)calloc(_remainingLength, 1);
+    _remainingLength = (int) strlen(topic) + 5;
+    _data = (unsigned char*) calloc(_remainingLength, 1);
     if (_data)
     {
         unsigned char* ptr = _data;
@@ -380,8 +387,8 @@ int MQTTGWPacket::setUNSUBSCRIBE(const char* topic, unsigned short msgid)
     _header.byte = 0;
     _header.bits.type = UNSUBSCRIBE;
     _header.bits.qos = 1;
-	_remainingLength = (int)strlen(topic) + 4;
-	_data = (unsigned char*)calloc(_remainingLength, 1);
+    _remainingLength = (int) strlen(topic) + 4;
+    _data = (unsigned char*) calloc(_remainingLength, 1);
     if (_data)
     {
         unsigned char* ptr = _data;
@@ -400,14 +407,14 @@ int MQTTGWPacket::setPUBLISH(Publish* pub)
     _header.byte = pub->header.byte;
     _header.bits.type = PUBLISH;
     _remainingLength = 4 + pub->topiclen + pub->payloadlen;
-	_data = (unsigned char*)calloc(_remainingLength, 1);
+    _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 )
+        if (_header.bits.qos > 0)
         {
             writeInt(&ptr, pub->msgId);
         }
@@ -432,7 +439,7 @@ int MQTTGWPacket::setAck(unsigned char msgType, unsigned short msgid)
     _header.bits.type = msgType;
     _header.bits.qos = (msgType == PUBREL) ? 1 : 0;
 
-	_data = (unsigned char*)calloc(_remainingLength, 1);
+    _data = (unsigned char*) calloc(_remainingLength, 1);
     if (_data)
     {
         unsigned char* data = _data;
@@ -467,7 +474,7 @@ int MQTTGWPacket::getPacketData(unsigned char* buf)
 {
     unsigned char* ptr = buf;
     *ptr++ = _header.byte;
-	int len = MQTTPacket_encode((char*)ptr, _remainingLength);
+    int len = MQTTPacket_encode((char*) ptr, _remainingLength);
     ptr += len;
     memcpy(ptr, _data, _remainingLength);
     return 1 + len + _remainingLength;
@@ -493,12 +500,13 @@ char* MQTTGWPacket::getMsgId(char* pbuf)
 {
     int type = getType();
 
-	switch ( type )
+    switch (type)
     {
     case PUBLISH:
         Publish pub;
+        pub.msgId = 0;
         getPUBLISH(&pub);
-		if ( _header.bits.dup )
+        if (_header.bits.dup)
         {
             sprintf(pbuf, "+%04X", pub.msgId);
         }
@@ -507,6 +515,40 @@ char* MQTTGWPacket::getMsgId(char* pbuf)
             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:
@@ -515,13 +557,47 @@ char* MQTTGWPacket::getMsgId(char* pbuf)
     case UNSUBSCRIBE:
     case SUBACK:
     case UNSUBACK:
-		sprintf(pbuf, " %02X%02X", _data[0], _data[1]);
+        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:
-		sprintf(pbuf, "    ");
         break;
     }
-	return pbuf;
 }
 
 char* MQTTGWPacket::print(char* pbuf)
@@ -530,8 +606,8 @@ char* MQTTGWPacket::print(char* pbuf)
     char* ptr = pbuf;
     char** pptr = &pbuf;
     int len = getPacketData(packetData);
-
-	for (int i = 0; i < len; i++)
+    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;
@@ -540,3 +616,31 @@ char* MQTTGWPacket::print(char* pbuf)
     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;
+}

MQTTSNGateway/src/MQTTGWPacket.h

@@ -31,18 +31,29 @@ 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
     {
@@ -50,25 +61,25 @@ typedef union
         bool dup : 1; /**< DUP flag bit */
         unsigned int qos : 2; /**< QoS value, 0, 1 or 2 */
         bool retain : 1; /**< retained flag bit */
-	} bits;
+    }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 */
+        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 ,
+enum MQTT_connackCodes
+{
+    MQTT_CONNECTION_ACCEPTED,
     MQTT_UNACCEPTABLE_PROTOCOL_VERSION,
     MQTT_IDENTIFIER_REJECTED,
     MQTT_SERVER_UNAVAILABLE,
@@ -92,17 +103,17 @@ typedef struct
             bool will : 1; /**< will flag */
             bool cleanstart : 1; /**< cleansession flag */
             int : 1; /**< unused */
-		} bits;
+        }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 */
+            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 */
@@ -116,6 +127,11 @@ typedef struct
     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.
  */
@@ -125,7 +141,7 @@ typedef struct
     char* msg;
     int retained;
     int qos;
-}willMessages;
+} willMessages;
 
 /**
  * Data for a connack packet.
@@ -141,19 +157,18 @@ typedef struct
         {
             unsigned int reserved : 7; /**< message type nibble */
             bool sessionPresent : 1; /**< was a session found on the server? */
-		} bits;
+        }bits;
 #else
         struct
         {
-			bool sessionPresent : 1;    /**< was a session found on the server? */
-			unsigned int reserved : 7;	/**< message type nibble */
+            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 */
 } Connack;
 
-
 /**
  * Data for a publish packet.
  */
@@ -167,6 +182,7 @@ typedef struct
     int payloadlen; /**< payload length */
 } Publish;
 
+#define MQTTPacket_Publish_Initializer {{0}, nullptr, 0, 0, nullptr, 0}
 
 /**
  * Data for one of the ack packets.
@@ -177,6 +193,15 @@ typedef struct
     int msgId; /**< MQTT message id */
 } Ack;
 
+/**
+ * UTF8String.
+ */
+typedef struct
+{
+    unsigned char len;
+    char* data;
+} UTF8String;
+
 /**
  * Class MQTT Packet
  */
@@ -197,14 +222,21 @@ public:
     int getSUBACK(unsigned short* msgId, unsigned char* rc);
     int getPUBLISH(Publish* pub);
 
-	int setCONNECT(Connect* conect, unsigned char* username, unsigned char* password);
+    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 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);

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"
@@ -36,9 +37,41 @@ MQTTGWPublishHandler::~MQTTGWPublishHandler()
 
 void MQTTGWPublishHandler::handlePublish(Client* client, MQTTGWPacket* packet)
 {
-	if ( !client->isActive() && !client->isSleep() )
+    if (!client->isActive() && !client->isSleep() && !client->isAwake())
     {
-		WRITELOG("     The client is neither active nor sleep %s\n", client->getStatus());
+        WRITELOG("%s     The client is neither active nor sleep %s%s\n",
+        ERRMSG_HEADER, client->getStatus(), ERRMSG_FOOTER);
+        return;
+    }
+
+    /* client is sleeping. save PUBLISH */
+    if (client->isSleep())
+    {
+        Publish pub;
+        packet->getPUBLISH(&pub);
+
+        WRITELOG(FORMAT_Y_G_G, currentDateTime(), packet->getName(),
+        RIGHTARROW, client->getClientId(), "is sleeping. a message was saved.");
+
+        if (pub.header.bits.qos == 1)
+        {
+            replyACK(client, &pub, PUBACK);
+        }
+        else if (pub.header.bits.qos == 2)
+        {
+            replyACK(client, &pub, PUBREC);
+        }
+
+        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;
     }
 
@@ -51,7 +84,7 @@ void MQTTGWPublishHandler::handlePublish(Client* client, MQTTGWPacket* packet)
     MQTTSN_topicid topicId;
     uint16_t id = 0;
 
-	if (pub.topiclen == 2)
+    if (pub.topiclen <= 2)
     {
         topicId.type = MQTTSN_TOPIC_TYPE_SHORT;
         *(topicId.data.short_name) = *pub.topic;
@@ -59,120 +92,104 @@ void MQTTGWPublishHandler::handlePublish(Client* client, MQTTGWPacket* packet)
     }
     else
     {
-		topicId.type = MQTTSN_TOPIC_TYPE_NORMAL;
         topicId.data.long_.len = pub.topiclen;
         topicId.data.long_.name = pub.topic;
-		id = client->getTopics()->getTopicId(&topicId);
+        Topic* tp = client->getTopics()->getTopicByName(&topicId);
 
-		if ( id > 0 )
+        if (tp)
         {
-			topicId.data.id = id;
+            topicId.type = tp->getType();
+            topicId.data.long_.len = pub.topiclen;
+            topicId.data.long_.name = pub.topic;
         }
         else
         {
-			/* This message might be subscribed with wild card. */
+            /* This message might be subscribed with wild card or not cleanSession*/
+            topicId.type = MQTTSN_TOPIC_TYPE_NORMAL;
             Topic* topic = client->getTopics()->match(&topicId);
-			if (topic == 0)
+
+            if (topic == nullptr && client->isCleanSession())
             {
-				WRITELOG(" Invalid Topic. PUBLISH message is canceled.\n");
+                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 )
+                else if (pub.header.bits.qos == 2)
                 {
                     replyACK(client, &pub, PUBREC);
                 }
+
+                delete snPacket;
                 return;
             }
 
+            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 REGACK */
+                /* create REGISTER */
                 MQTTSNPacket* regPacket = new MQTTSNPacket();
 
-				MQTTSNString topicName;
+                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);
 
-				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);
-				}
 
                 /* 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);
+                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("\x1b[0m\x1b[31mMQTTGWPublishHandler Can't create a Topic.\n");
+                WRITELOG("%sMQTTGWPublishHandler Can't create a Topic. PUBLISH message is discarded.%s\n",
+                ERRMSG_HEADER, ERRMSG_FOOTER);
                 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);
-	}
 
 }
 
 void MQTTGWPublishHandler::replyACK(Client* client, Publish* pub, int type)
 {
     MQTTGWPacket* pubAck = new MQTTGWPacket();
-	pubAck->setAck(type, (uint16_t)pub->msgId);
+    pubAck->setAck(type, (uint16_t) pub->msgId);
     Event* ev1 = new Event();
     ev1->setBrokerSendEvent(client, pubAck);
     _gateway->getBrokerSendQue()->post(ev1);
@@ -182,25 +199,29 @@ void MQTTGWPublishHandler::handlePuback(Client* client, MQTTGWPacket* packet)
 {
     Ack ack;
     packet->getAck(&ack);
-	uint16_t topicId = client->getWaitedPubTopicId((uint16_t)ack.msgId);
+    TopicIdMapElement* topicId = client->getWaitedPubTopicId((uint16_t) ack.msgId);
     if (topicId)
     {
         MQTTSNPacket* mqttsnPacket = new MQTTSNPacket();
-		mqttsnPacket->setPUBACK(topicId, (uint16_t)ack.msgId, 0);
+        mqttsnPacket->setPUBACK(topicId->getTopicId(), (uint16_t) ack.msgId, 0);
 
-		client->eraseWaitedPubTopicId((uint16_t)ack.msgId);
+        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());
+    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);
+
+    if (client->isActive() || client->isAwake())
+    {
         MQTTSNPacket* mqttsnPacket = new MQTTSNPacket();
         if (type == PUBREC)
         {
@@ -218,5 +239,81 @@ void MQTTGWPublishHandler::handleAck(Client* client, MQTTGWPacket* packet, int t
         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();
+    }
 }
 

MQTTSNGateway/src/MQTTGWPublishHandler.h

@@ -32,6 +32,11 @@ public:
     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);
 
@@ -40,6 +45,4 @@ private:
 
 }
 
-
-
 #endif /* MQTTGWPUBLISHHANDLER_H_ */

MQTTSNGateway/src/MQTTGWSubscribeHandler.cpp

@@ -38,12 +38,11 @@ void MQTTGWSubscribeHandler::handleSuback(Client* client, MQTTGWPacket* packet)
     int qos = 0;
 
     packet->getSUBACK(&msgId, &rc);
-	uint16_t topicId = client->getWaitedSubTopicId(msgId);
+    TopicIdMapElement* topicId = client->getWaitedSubTopicId(msgId);
 
     if (topicId)
     {
         MQTTSNPacket* snPacket = new MQTTSNPacket();
-		client->eraseWaitedSubTopicId(msgId);
 
         if (rc == 0x80)
         {
@@ -54,10 +53,11 @@ void MQTTGWSubscribeHandler::handleSuback(Client* client, MQTTGWPacket* packet)
             returnCode = MQTTSN_RC_ACCEPTED;
             qos = rc;
         }
-		snPacket->setSUBACK(qos, topicId, msgId, returnCode);
+        snPacket->setSUBACK(qos, topicId->getTopicId(), msgId, returnCode);
         Event* evt = new Event();
         evt->setClientSendEvent(client, snPacket);
         _gateway->getClientSendQue()->post(evt);
+        client->eraseWaitedSubTopicId(msgId);
     }
 }
 
@@ -72,3 +72,27 @@ void MQTTGWSubscribeHandler::handleUnsuback(Client* client, MQTTGWPacket* packet
     _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);
+    }
+}
+

MQTTSNGateway/src/MQTTGWSubscribeHandler.h

@@ -31,6 +31,8 @@ public:
     ~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;

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);
+    }
+}
+

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_ */

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);
+    }
+}
+

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_ */

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)
+ {
+
+ }
+ */
+

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_ */

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");
+}

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_ */

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;
+}
+

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_ */

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;
@@ -29,13 +31,13 @@ char* currentDateTime(void);
 BrokerRecvTask::BrokerRecvTask(Gateway* gateway)
 {
     _gateway = gateway;
-	_gateway->attach((Thread*)this);
-	_light = 0;
+    _gateway->attach((Thread*) this);
+    _light = nullptr;
+    setTaskName("BrokerRecvTask");
 }
 
 BrokerRecvTask::~BrokerRecvTask()
 {
-
 }
 
 /**
@@ -52,9 +54,9 @@ void BrokerRecvTask::initialize(int argc, char** argv)
 void BrokerRecvTask::run(void)
 {
     struct timeval timeout;
-	MQTTGWPacket* packet = 0;
+    MQTTGWPacket* packet = nullptr;
     int rc;
-	Event* ev = 0;
+    Event* ev = nullptr;
     fd_set rset;
     fd_set wset;
 
@@ -63,7 +65,7 @@ void BrokerRecvTask::run(void)
         _light->blueLight(false);
         if (CHK_SIGINT)
         {
-			WRITELOG("%s BrokerRecvTask   stopped.\n", currentDateTime());
+            WRITELOG("%s %s stopped.\n", currentDateTime(), getTaskName());
             return;
         }
         timeout.tv_sec = 0;
@@ -74,9 +76,9 @@ void BrokerRecvTask::run(void)
         int sockfd = 0;
 
         /* Prepare sockets list to read */
-		Client* client = _gateway->getClientList()->getClient();
+        Client* client = _gateway->getClientList()->getClient(0);
 
-		while (client > 0)
+        while (client)
         {
             if (client->getNetwork()->isValid())
             {
@@ -99,11 +101,12 @@ void BrokerRecvTask::run(void)
         {
             /* Check sockets is ready to read */
             int activity = select(maxSock + 1, &rset, 0, 0, &timeout);
+
             if (activity > 0)
             {
-				client = _gateway->getClientList()->getClient();
+                client = _gateway->getClientList()->getClient(0);
 
-				while (client > 0)
+                while (client)
                 {
                     _light->blueLight(false);
                     if (client->getNetwork()->isValid())
@@ -116,9 +119,9 @@ void BrokerRecvTask::run(void)
                             /* read sockets */
                             _light->blueLight(true);
                             rc = packet->recv(client->getNetwork());
-							if ( rc > 0 )
+                            if (rc > 0)
                             {
-								if ( log(client, packet) == -1 )
+                                if (log(client, packet) == -1)
                                 {
                                     delete packet;
                                     goto nextClient;
@@ -131,49 +134,45 @@ void BrokerRecvTask::run(void)
                             }
                             else
                             {
-								if ( rc == 0 )  // Disconnected
+                                if (rc == 0)  // Disconnected
                                 {
+                                    WRITELOG("%s BrokerRecvTask %s is disconnected by the broker.%s\n",
+                                    ERRMSG_HEADER, client->getClientId(),
+                                    ERRMSG_FOOTER);
                                     client->getNetwork()->close();
-									delete packet;
-
-									/* delete client when the client is not authorized & session is clean */
-									_gateway->getClientList()->erase(client);
-
-									if ( client )
-									{
-										client = client->getNextClient();
-									}
-									continue;
+                                    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);
+                                    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 )
+                                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);
+                                    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 )
+                                else if (rc == -3)
                                 {
-									WRITELOG("%s BrokerRecvTask can't get memories for the packet %s%s\n", ERRMSG_HEADER, client->getClientId(), ERRMSG_FOOTER);
+                                    WRITELOG("%s BrokerRecvTask can't allocate memories for the packet %s%s\n",
+                                    ERRMSG_HEADER, client->getClientId(),
+                                    ERRMSG_FOOTER);
                                 }
 
                                 delete packet;
 
-								if ( (rc == -1 || rc == -2) && client->isActive() )
+                                if ((rc == -1 || rc == -2) && (client->isActive() || client->isSleep() || client->isAwake()))
                                 {
-									/* disconnect the client */
-									packet = new MQTTGWPacket();
-									packet->setHeader(DISCONNECT);
-									ev = new Event();
-									ev->setBrokerRecvEvent(client, packet);
-									_gateway->getPacketEventQue()->post(ev);
+                                    client->getNetwork()->close();
+                                    client->disconnected();
                                 }
                             }
                         }
                     }
-					nextClient:
-					client = client->getNextClient();
+                    nextClient: client = client->getNextClient();
                 }
             }
         }
@@ -185,30 +184,33 @@ void BrokerRecvTask::run(void)
  */
 int BrokerRecvTask::log(Client* client, MQTTGWPacket* packet)
 {
-	char pbuf[(SIZE_OF_LOG_PACKET + 5 )* 3];
+    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));
+        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), LEFTARROW, client->getClientId(), packet->print(pbuf));
+        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), LEFTARROW, client->getClientId(), packet->print(pbuf));
+        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), LEFTARROW, client->getClientId(), packet->print(pbuf));
+        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(), LEFTARROW, client->getClientId(), packet->print(pbuf));
+        WRITELOG(FORMAT_Y_Y_W, currentDateTime(), packet->getName(), LEFTARROWB, client->getClientId(), packet->print(pbuf));
         break;
     default:
         WRITELOG("Type=%x\n", packet->getType());

MQTTSNGateway/src/MQTTSNGWBrokerRecvTask.h

@@ -28,7 +28,7 @@ namespace MQTTSNGW
 class BrokerRecvTask: public Thread
 {
 MAGIC_WORD_FOR_THREAD;
-	;
+
 public:
     BrokerRecvTask(Gateway* gateway);
     ~BrokerRecvTask();
@@ -44,5 +44,4 @@ private:
 
 }
 
-
 #endif /* MQTTSNGWBROKERRECVTASK_H_ */

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"
@@ -33,14 +34,14 @@ char* currentDateTime();
 BrokerSendTask::BrokerSendTask(Gateway* gateway)
 {
     _gateway = gateway;
-	_gateway->attach((Thread*)this);
-	_gwparams = 0;
-	_light = 0;
+    _gateway->attach((Thread*) this);
+    _gwparams = nullptr;
+    _light = nullptr;
+    setTaskName("BrokerSendTask");
 }
 
 BrokerSendTask::~BrokerSendTask()
 {
-
 }
 
 /**
@@ -57,51 +58,56 @@ void BrokerSendTask::initialize(int argc, char** argv)
  */
 void BrokerSendTask::run()
 {
-	Event* ev = 0;
-	MQTTGWPacket* packet = 0;
-	Client* client = 0;
+    Event* ev = nullptr;
+    MQTTGWPacket* packet = nullptr;
+    Client* client = nullptr;
+    AdapterManager* adpMgr = _gateway->getAdapterManager();
     int rc = 0;
 
     while (true)
     {
         ev = _gateway->getBrokerSendQue()->wait();
 
-		if ( ev->getEventType() == EtStop )
+        if (ev->getEventType() == EtStop)
         {
-			WRITELOG("%s BrokerSendTask   stopped.\n", currentDateTime());
+            WRITELOG("%s %s stopped.\n", currentDateTime(), getTaskName());
             delete ev;
             return;
         }
 
-		if ( ev->getEventType() == EtBrokerSend)
+        if (ev->getEventType() == EtBrokerSend)
         {
             client = ev->getClient();
             packet = ev->getMQTTGWPacket();
 
-			if ( packet->getType() == CONNECT && client->getNetwork()->isValid() )
+            /* Check Client is managed by Adapters */
+            client = adpMgr->getClient(client);
+
+            if (packet->getType() == CONNECT && client->getNetwork()->isValid())
             {
                 client->getNetwork()->close();
             }
 
-			if ( !client->getNetwork()->isValid() )
+            if (!client->getNetwork()->isValid())
             {
                 /* connect to the broker and send a packet */
 
                 if (client->isSecureNetwork())
                 {
-					rc = client->getNetwork()->connect(_gwparams->brokerName, _gwparams->portSecure, _gwparams->rootCApath,
-							  _gwparams->rootCAfile, _gwparams->certKey, _gwparams->privateKey);
+                    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(_gwparams->brokerName, _gwparams->port);
+                    rc = client->getNetwork()->connect((const char*) _gwparams->brokerName, (const char*) _gwparams->port);
                 }
 
-				if ( !rc )
+                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);
+                    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;
@@ -110,19 +116,27 @@ void BrokerSendTask::run()
 
             /* send a packet */
             _light->blueLight(true);
-			if ( (rc = packet->send(client->getNetwork())) > 0 )
+            if ((rc = packet->send(client->getNetwork())) > 0)
             {
-				if ( packet->getType() == CONNECT )
+                if (packet->getType() == CONNECT)
                 {
                     client->connectSended();
                 }
+                else if (packet->getType() == DISCONNECT)
+                {
+                    client->getNetwork()->close();
+                    client->disconnected();
+                }
                 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);
+                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();
+                }
 
                 /* Disconnect the client */
                 packet = new MQTTGWPacket();
@@ -138,22 +152,23 @@ void BrokerSendTask::run()
     }
 }
 
-
 /**
  *  write message content into stdout or Ringbuffer
  */
 void BrokerSendTask::log(Client* client, MQTTGWPacket* packet)
 {
-	char pbuf[(SIZE_OF_LOG_PACKET + 5 )* 3];
+    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));
+        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), RIGHTARROW, client->getClientId(), packet->print(pbuf));
+        WRITELOG(FORMAT_W_MSGID_Y_W, currentDateTime(), packet->getName(), packet->getMsgId(msgId), RIGHTARROWB,
+                client->getClientId(), packet->print(pbuf));
         break;
     case SUBSCRIBE:
     case UNSUBSCRIBE:
@@ -161,13 +176,16 @@ void BrokerSendTask::log(Client* client, MQTTGWPacket* packet)
     case PUBREC:
     case PUBREL:
     case PUBCOMP:
-		WRITELOG(FORMAT_W_MSGID_Y_W, currentDateTime(), packet->getName(), packet->getMsgId(msgId), RIGHTARROW, client->getClientId(), packet->print(pbuf));
+        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(), RIGHTARROW, client->getClientId(), packet->print(pbuf));
+        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(), RIGHTARROW, client->getClientId(), packet->print(pbuf));
+        WRITELOG(FORMAT_Y_Y_W, currentDateTime(), packet->getName(),
+        RIGHTARROWB, client->getClientId(), packet->print(pbuf));
         break;
     default:
         break;

MQTTSNGateway/src/MQTTSNGWBrokerSendTask.h

@@ -22,12 +22,15 @@
 
 namespace MQTTSNGW
 {
+class Adapter;
+
 /*=====================================
  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();

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
 {
@@ -65,11 +71,13 @@ public:
         }
     }
 
-	void post(T* packet)
+    int post(T* packet)
     {
+        int rc;
         _mutex.lock();
-		_que->post(packet);
+        rc = _que->post(packet);
         _mutex.unlock();
+        return rc;
     }
 
     void pop()
@@ -93,89 +101,16 @@ public:
         _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;
-};
-
 /*=====================================
  Class WaitREGACKPacket
  =====================================*/
@@ -204,8 +139,10 @@ public:
     int setPacket(MQTTSNPacket* packet, uint16_t REGACKMsgId);
     MQTTSNPacket* getPacket(uint16_t REGACKMsgId);
     void erase(uint16_t REGACKMsgId);
+    uint8_t getCount(void);
 
 private:
+    uint8_t _cnt;
     waitREGACKPacket* _first;
     waitREGACKPacket* _end;
 };
@@ -213,26 +150,47 @@ private:
 /*=====================================
  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 ClientsPool;
 public:
     Client(bool secure = false);
     Client(uint8_t maxInflightMessages, bool secure);
     ~Client();
 
     Connect* getConnectData(void);
-	uint16_t getWaitedPubTopicId(uint16_t msgId);
-	uint16_t getWaitedSubTopicId(uint16_t msgId);
-	MQTTSNPacket* getClientSleepPacket();
+    TopicIdMapElement* getWaitedPubTopicId(uint16_t msgId);
+    TopicIdMapElement* getWaitedSubTopicId(uint16_t msgId);
+    MQTTGWPacket* getClientSleepPacket(void);
+    void deleteFirstClientSleepPacket(void);
+
+    MQTTSNPacket* getProxyPacket(void);
+    void deleteFirstProxyPacket(void);
     WaitREGACKPacketList* getWaitREGACKPacketList(void);
 
     void eraseWaitedPubTopicId(uint16_t msgId);
@@ -240,9 +198,10 @@ public:
     void clearWaitedPubTopicId(void);
     void clearWaitedSubTopicId(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);
+    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);
 
     bool checkTimeover(void);
     void updateStatus(MQTTSNPacket*);
@@ -251,6 +210,8 @@ public:
     void connackSended(int rc);
     void disconnected(void);
     bool isConnectSendable(void);
+    void tryConnect(void);
+    ClientStatus getClientStatus(void);
 
     uint16_t getNextPacketId(void);
     uint8_t getNextSnMsgId(void);
@@ -263,6 +224,19 @@ public:
     void setClientAddress(SensorNetAddress* sensorNetAddr);
     void setSensorNetType(bool stable);
 
+    Forwarder* getForwarder(void);
+    void setForwarder(Forwarder* forwader);
+
+    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);
@@ -275,18 +249,25 @@ public:
     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);
-	Client* getOTAClient(void);
-	void    setOTAClient(Client* cl);
 
 private:
-	PacketQue<MQTTSNPacket> _clientSleepPacketQue;
+    PacketQue<MQTTGWPacket> _clientSleepPacketQue;
+    PacketQue<MQTTSNPacket> _proxyPacketQue;
+
     WaitREGACKPacketList _waitREGACKList;
 
     Topics* _topics;
@@ -300,6 +281,8 @@ private:
     char* _willTopic;
     char* _willMsg;
 
+    bool _holdPingRequest;
+
     Timer _keepAliveTimer;
     uint32_t _keepAliveMsec;
 
@@ -314,36 +297,14 @@ private:
     bool _sensorNetype;     // false: unstable network like a G3
     SensorNetAddress _sensorNetAddr;
 
+    Forwarder* _forwarder;
+    ClientType _clientType;
+
     bool _sessionStatus;
+    bool _hasPredefTopic;
 
     Client* _nextClient;
     Client* _prevClient;
-	Client* _otaClient;
-};
-
-/*=====================================
- 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;
 };
 
 }

MQTTSNGateway/src/MQTTSNGWClientList.cpp

@@ -0,0 +1,516 @@
+/**************************************************************************************
+ * 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);    //  <=== BUGFIX
+        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++;
+    }
+}

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_ */

MQTTSNGateway/src/MQTTSNGWClientRecvTask.cpp

@@ -15,8 +15,14 @@
  **************************************************************************************/
 
 #include "MQTTSNGWClientRecvTask.h"
-#include "MQTTSNGateway.h"
-#include <string.h>
+#include "MQTTSNPacket.h"
+#include "MQTTSNGWQoSm1Proxy.h"
+#include "MQTTSNGWEncapsulatedPacket.h"
+#include <cstring>
+
+//#include "MQTTSNGWForwarder.h"
+
+using namespace MQTTSNGW;
 char* currentDateTime(void);
 /*=====================================
  Class ClientRecvTask
@@ -24,24 +30,17 @@ char* currentDateTime(void);
 ClientRecvTask::ClientRecvTask(Gateway* gateway)
 {
     _gateway = gateway;
-	_gateway->attach((Thread*)this);
+    _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");
-	}
 }
 
 /*
@@ -51,54 +50,127 @@ void ClientRecvTask::initialize(int argc, char** argv)
  */
 void ClientRecvTask::run()
 {
-	Event* ev = 0;
-	Client* client = 0;
+    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();
+
     char buf[128];
 
     while (true)
     {
+        Client* client = nullptr;
+        Forwarder* fwd = nullptr;
+        WirelessNodeId nodeId;
+
         MQTTSNPacket* packet = new MQTTSNPacket();
         int packetLen = packet->recv(_sensorNetwork);
 
         if (CHK_SIGINT)
         {
-			WRITELOG("%s ClientRecvTask   stopped.\n", currentDateTime());
+            WRITELOG("%s %s stopped.\n", currentDateTime(), getTaskName());
             delete packet;
             return;
         }
 
-		if (packetLen < 2 )
+        if (packetLen < 2)
         {
             delete packet;
             continue;
         }
 
-		if ( packet->getType() <= MQTTSN_ADVERTISE || packet->getType() == MQTTSN_GWINFO )
+        if (packet->getType() <= MQTTSN_ADVERTISE || packet->getType() == MQTTSN_GWINFO)
         {
             delete packet;
             continue;
         }
 
-		if ( packet->getType() == MQTTSN_SEARCHGW )
+        if (packet->getType() == MQTTSN_SEARCHGW)
         {
             /* write log and post Event */
-			log(0, packet);
+            log(0, packet, 0);
             ev = new Event();
             ev->setBrodcastEvent(packet);
-			_gateway->getPacketEventQue()->post(ev);
+            packetEventQue->post(ev);
             continue;
         }
 
-		/* get client from the ClientList of Gateway by sensorNetAddress. */
-		client = _gateway->getClientList()->getClient(_sensorNetwork->getSenderAddress());
+        SensorNetAddress* senderAddr = _gateway->getSensorNetwork()->getSenderAddress();
 
-		if ( client )
+        if (packet->getType() == MQTTSN_ENCAPSULATED)
         {
-			/* write log and post Event */
-			log(client, packet);
+            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->setClientRecvEvent(client,packet);
-			_gateway->getPacketEventQue()->post(ev);
+                ev->setClientSendEvent(client, snPacket);
+                clientsendQue->post(ev);
+                delete packet;
+                continue;
+            }
+            else
+            {
+                ev = new Event();
+                ev->setClientRecvEvent(client, packet);
+                packetEventQue->post(ev);
+            }
         }
         else
         {
@@ -107,62 +179,103 @@ void ClientRecvTask::run()
             {
                 MQTTSNPacket_connectData data;
                 memset(&data, 0, sizeof(MQTTSNPacket_connectData));
-				if ( !packet->getCONNECT(&data) )
+                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);
+                    WRITELOG("%s CONNECT message form %s is incorrect.%s\n",
+                    ERRMSG_HEADER, senderAddr->sprint(buf),
+                    ERRMSG_FOOTER);
                     delete packet;
                     continue;
                 }
 
-				/* create a client */
-				client = _gateway->getClientList()->createClient(_sensorNetwork->getSenderAddress(), &data.clientID, false, false);
-				log(client, packet, &data.clientID);
-				if (!client)
+                client = clientList->getClient(&data.clientID);
+
+                if (fwd != nullptr)
                 {
-					WRITELOG("%s Client(%s) was rejected. CONNECT message has been discarded.%s\n", ERRMSG_HEADER, _sensorNetwork->getSenderAddress()->sprint(buf), ERRMSG_FOOTER);
-					delete packet;
-					continue;
+                    if (client == nullptr)
+                    {
+                        /* create a new client */
+                        client = clientList->createClient(0, &data.clientID, clientType);
                     }
-
-				/* set sensorNetAddress & post Event */
-				client->setClientAddress(_sensorNetwork->getSenderAddress());
-				ev = new Event();
-				ev->setClientRecvEvent(client, packet);
-				_gateway->getPacketEventQue()->post(ev);
+                    /* Add to a forwarded client list of forwarder. */
+                    fwd->addClient(client, &nodeId);
                 }
                 else
                 {
-				log(client, packet);
+                    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;
-				/* 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;
                 }
+
+                /* 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];
 
-	if ( id )
+    if (id)
     {
-		memset((void*)cstr, 0, id->lenstring.len + 1);
-		strncpy(cstr, id->lenstring.data, id->lenstring.len) ;
+        if (id->cstring)
+        {
+            strncpy(cstr, id->cstring, strlen(id->cstring));
             clientId = cstr;
         }
-	else if ( client )
+        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();
     }
@@ -171,14 +284,24 @@ void ClientRecvTask::log(Client* client, MQTTSNPacket* packet, MQTTSNString* id)
         clientId = UNKNOWNCL;
     }
 
+    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:
-	case MQTTSN_PINGREQ:
-		WRITELOG(FORMAT_Y_G_G_NL, currentDateTime(), packet->getName(), LEFTARROW, CLIENT, packet->print(pbuf));
+        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));
+    case MQTTSN_PINGREQ:
+        WRITELOG(FORMAT_Y_G_G_NL, currentDateTime(), packet->getName(),
+        LEFTARROW, clientId, packet->print(pbuf));
         break;
     case MQTTSN_DISCONNECT:
     case MQTTSN_WILLTOPICUPD:
@@ -191,14 +314,19 @@ void ClientRecvTask::log(Client* client, MQTTSNPacket* packet, MQTTSNString* id)
     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));
+        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));
+        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));

MQTTSNGateway/src/MQTTSNGWClientRecvTask.h

@@ -21,21 +21,24 @@
 
 namespace MQTTSNGW
 {
+class AdapterManager;
 
 /*=====================================
  Class ClientRecvTask
  =====================================*/
-class ClientRecvTask:public Thread
+class ClientRecvTask: public Thread
 {
-	MAGIC_WORD_FOR_THREAD;
+MAGIC_WORD_FOR_THREAD;
+    friend AdapterManager;
 public:
     ClientRecvTask(Gateway*);
-	~ClientRecvTask();
+    ~ClientRecvTask(void);
     virtual void initialize(int argc, char** argv);
-	void run();
+    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;

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;
@@ -27,43 +29,51 @@ char* currentDateTime(void);
 ClientSendTask::ClientSendTask(Gateway* gateway)
 {
     _gateway = gateway;
-	_gateway->attach((Thread*)this);
+    _gateway->attach((Thread*) this);
     _sensorNetwork = _gateway->getSensorNetwork();
+    setTaskName("ClientSendTask");
 }
 
 ClientSendTask::~ClientSendTask()
 {
-
 }
 
 void ClientSendTask::run()
 {
-	Client* client = 0;
-	MQTTSNPacket* packet = 0;
+    Client* client = nullptr;
+    MQTTSNPacket* packet = nullptr;
+    AdapterManager* adpMgr = _gateway->getAdapterManager();
     int rc = 0;
 
     while (true)
     {
         Event* ev = _gateway->getClientSendQue()->wait();
 
-		if (ev->getEventType() == EtStop)
+        if (ev->getEventType() == EtStop || _gateway->IsStopping())
         {
-			WRITELOG("%s ClientSendTask   stopped.\n", currentDateTime());
+            WRITELOG("%s %s stopped.\n", currentDateTime(), getTaskName());
             delete ev;
             break;
         }
+
+        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();
-			log(client, packet);
-			rc = packet->unicast(_sensorNetwork, client->getSensorNetAddress());
-		}
-		else if (ev->getEventType() == EtBroadcast)
-		{
-			packet = ev->getMQTTSNPacket();
-			log(client, packet);
-			rc = packet->broadcast(_sensorNetwork);
+                rc = adpMgr->unicastToClient(client, packet, this);
             }
             else if (ev->getEventType() == EtSensornetSend)
             {
@@ -72,10 +82,12 @@ void ClientSendTask::run()
                 rc = packet->unicast(_sensorNetwork, ev->getSensorNetAddress());
             }
 
-		if ( rc < 0 )
+            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);
+                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;
     }
@@ -83,16 +95,16 @@ void ClientSendTask::run()
 
 void ClientSendTask::log(Client* client, MQTTSNPacket* packet)
 {
-	char pbuf[SIZE_OF_LOG_PACKET * 3];
+    char pbuf[SIZE_OF_LOG_PACKET * 3 + 1];
     char msgId[6];
-	const char* clientId = client ? (const char*)client->getClientId() : UNKNOWNCL ;
+    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));
+        WRITELOG(FORMAT_Y_W_G, currentDateTime(), packet->getName(), RIGHTARROW,
+        CLIENTS, packet->print(pbuf));
         break;
     case MQTTSN_CONNACK:
     case MQTTSN_DISCONNECT:
@@ -100,11 +112,13 @@ void ClientSendTask::log(Client* client, MQTTSNPacket* packet)
     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));
+        WRITELOG(FORMAT_W_MSGID_W_G, currentDateTime(), packet->getName(), packet->getMsgId(msgId), RIGHTARROW, clientId,
+                packet->print(pbuf));
         break;
     case MQTTSN_REGACK:
     case MQTTSN_PUBACK:
@@ -113,9 +127,11 @@ void ClientSendTask::log(Client* client, MQTTSNPacket* packet)
     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));
+        WRITELOG(FORMAT_W_MSGID_W_G, currentDateTime(), packet->getName(), packet->getMsgId(msgId), RIGHTARROW, clientId,
+                packet->print(pbuf));
         break;
     default:
         break;
     }
 }
+

MQTTSNGateway/src/MQTTSNGWClientSendTask.h

@@ -21,20 +21,22 @@
 
 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(void);
+    void run(void);
 
 private:
-	void log(Client*, MQTTSNPacket*);
+    void log(Client* client, MQTTSNPacket* packet);
 
     Gateway* _gateway;
     SensorNetwork* _sensorNetwork;

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
@@ -69,15 +69,31 @@ void MQTTSNConnectionHandler::handleSearchgw(MQTTSNPacket* packet)
 void MQTTSNConnectionHandler::handleConnect(Client* client, MQTTSNPacket* packet)
 {
     MQTTSNPacket_connectData data;
-	if ( packet->getCONNECT(&data) == 0 )
+    if (packet->getCONNECT(&data) == 0)
     {
         return;
     }
 
-	/* clear ConnectData of Client */
+    /* 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));
+    if (!client->isAdapter())
+    {
         client->disconnected();
+    }
 
     Topics* topics = client->getTopics();
 
@@ -88,10 +104,14 @@ void MQTTSNConnectionHandler::handleConnect(Client* client, MQTTSNPacket* packet
     connectData->keepAliveTimer = data.duration;
     connectData->flags.bits.will = data.willFlag;
 
-	if ((const char*) _gateway->getGWParams()->loginId != 0 && (const char*) _gateway->getGWParams()->password != 0)
+    if ((const char*) _gateway->getGWParams()->loginId != nullptr)
+    {
+        connectData->flags.bits.username = 1;
+    }
+
+    if ((const char*) _gateway->getGWParams()->password != 0)
     {
         connectData->flags.bits.password = 1;
-		connectData->flags.bits.username = 1;
     }
 
     client->setSessionStatus(false);
@@ -105,10 +125,9 @@ void MQTTSNConnectionHandler::handleConnect(Client* client, MQTTSNPacket* packet
         /* renew the TopicList */
         if (topics)
         {
-			delete topics;
+            topics->eraseNormal();
+            ;
         }
-		topics = new Topics();
-		client->setTopics(topics);
         client->setSessionStatus(true);
     }
 
@@ -125,11 +144,11 @@ void MQTTSNConnectionHandler::handleConnect(Client* client, MQTTSNPacket* packet
     }
     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);
+        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);
@@ -143,9 +162,9 @@ void MQTTSNConnectionHandler::handleWilltopic(Client* client, MQTTSNPacket* pack
 {
     int willQos;
     uint8_t willRetain;
-	MQTTSNString willTopic;
+    MQTTSNString willTopic = MQTTSNString_initializer;
 
-	if ( packet->getWILLTOPIC(&willQos, &willRetain, &willTopic) == 0 )
+    if (packet->getWILLTOPIC(&willQos, &willRetain, &willTopic) == 0)
     {
         return;
     }
@@ -171,19 +190,19 @@ void MQTTSNConnectionHandler::handleWilltopic(Client* client, MQTTSNPacket* pack
  */
 void MQTTSNConnectionHandler::handleWillmsg(Client* client, MQTTSNPacket* packet)
 {
-	if ( !client->isWaitWillMsg() )
+    if (!client->isWaitWillMsg())
     {
         DEBUGLOG("     MQTTSNConnectionHandler::handleWillmsg  WaitWillMsgFlg is off.\n");
         return;
     }
 
-	MQTTSNString willmsg;
+    MQTTSNString willmsg = MQTTSNString_initializer;
     Connect* connectData = client->getConnectData();
 
-	if( client->isConnectSendable() )
+    if (client->isConnectSendable())
     {
         /* save WillMsg in the client */
-		if ( packet->getWILLMSG(&willmsg) == 0 )
+        if (packet->getWILLMSG(&willmsg) == 0)
         {
             return;
         }
@@ -192,7 +211,8 @@ void MQTTSNConnectionHandler::handleWillmsg(Client* client, MQTTSNPacket* packet
         /* create CONNECT message */
         MQTTGWPacket* mqttPacket = new MQTTGWPacket();
         connectData->willMsg = client->getWillMsg();
-		mqttPacket->setCONNECT(connectData, (unsigned char*)_gateway->getGWParams()->loginId, (unsigned char*)_gateway->getGWParams()->password);
+        mqttPacket->setCONNECT(connectData, (unsigned char*) _gateway->getGWParams()->loginId,
+                (unsigned char*) _gateway->getGWParams()->password);
 
         /* Send CONNECT to the broker */
         Event* evt = new Event();
@@ -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;
-	if ( packet->getDISCONNECT(&duration) == 0 )
+
+    if (packet->getDISCONNECT(&duration) != 0)
     {
-		return;
-	}
-	if ( duration == 0 )
+        if (duration == 0)
         {
             MQTTGWPacket* mqMsg = new MQTTGWPacket();
             mqMsg->setHeader(DISCONNECT);
-		ev = new Event();
+            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);
 }
 
 /*
@@ -259,10 +279,34 @@ void MQTTSNConnectionHandler::handleWillmsgupd(Client* client, MQTTSNPacket* pac
  */
 void MQTTSNConnectionHandler::handlePingreq(Client* client, MQTTSNPacket* packet)
 {
+    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);
+    }
 }

MQTTSNGateway/src/MQTTSNGWConnectionHandler.h

@@ -37,7 +37,8 @@ public:
     void handleWillmsgupd(Client* client, MQTTSNPacket* packet);
     void handlePingreq(Client* client, MQTTSNPacket* packet);
 private:
-	char _pbuf[MQTTSNGW_MAX_PACKET_SIZE * 3];
+    void sendStoredPublish(Client* client);
+
     Gateway* _gateway;
 };
 

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,12 +37,19 @@ 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
  ==================================*/
@@ -51,14 +60,19 @@ 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_ */

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;
+}
+

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_ */

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);
+}

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_ */

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>
@@ -37,7 +35,7 @@ void Logmonitor::run()
     while (true)
     {
         const char* data = getLog();
-		if ( *data == 0 )
+        if (*data == 0)
         {
             break;
         }

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)
+{
+
+}

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_ */

MQTTSNGateway/src/MQTTSNGWPacket.cpp

@@ -24,12 +24,30 @@
 
 using namespace std;
 using namespace MQTTSNGW;
+int readInt(char** pptr);
+void writeInt(unsigned char** pptr, int msgId);
 
-MQTTSNPacket::MQTTSNPacket()
+MQTTSNPacket::MQTTSNPacket(void)
 {
-	_buf = 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)
@@ -56,13 +74,13 @@ int MQTTSNPacket::serialize(uint8_t* buf)
 
 int MQTTSNPacket::desirialize(unsigned char* buf, unsigned short len)
 {
-	if ( _buf )
+    if (_buf)
     {
         free(_buf);
     }
 
-	_buf = (unsigned char*)calloc(len, sizeof(unsigned char));
-	if ( _buf )
+    _buf = (unsigned char*) calloc(len, sizeof(unsigned char));
+    if (_buf)
     {
         memcpy(_buf, buf, len);
         _bufLen = len;
@@ -82,17 +100,13 @@ int MQTTSNPacket::recv(SensorNetwork* network)
     {
         len = desirialize(buf, len);
     }
-	else
-	{
-		len = 0;
-	}
     return len;
 
 }
 
 int MQTTSNPacket::getType(void)
 {
-	if ( _bufLen == 0 )
+    if (_bufLen == 0)
     {
         return 0;
     }
@@ -101,6 +115,17 @@ int MQTTSNPacket::getType(void)
     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;
@@ -119,142 +144,178 @@ const char* MQTTSNPacket::getName()
 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);
+    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);
+    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];
+    int buflen = sizeof(buf);
     MQTTSNPacket_connectData data;
-	data.clientID.cstring = (char*)"client01";
-	int len = MQTTSNSerialize_connect(buf, 40, &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);
+    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);
+    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);
+    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);
+    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);
+    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);
+    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);
+    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);
+    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);
+    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);
+    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);
+    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);
+    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);
+    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);
+    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);
+    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);
+    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);
 }
 
@@ -285,26 +346,26 @@ int MQTTSNPacket::getWILLMSG(MQTTSNString* willmsg)
 
 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)
 {
-	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)
@@ -325,7 +386,7 @@ int MQTTSNPacket::getUNSUBSCRIBE(uint16_t* msgId, MQTTSN_topicid* topicFilter)
 
 int MQTTSNPacket::getPINGREQ(void)
 {
-	if (getType() == MQTTSN_PINGRESP && _bufLen > 2 )
+    if (getType() == MQTTSN_PINGRESP && _bufLen > 2)
     {
         return _bufLen - 2;
     }
@@ -336,7 +397,7 @@ int MQTTSNPacket::getDISCONNECT(uint16_t* duration)
 {
     int dur = 0;
     int rc = MQTTSNDeserialize_disconnect(&dur, _buf, _bufLen);
-	*duration = (uint16_t)dur;
+    *duration = (uint16_t) dur;
     return rc;
 }
 
@@ -354,12 +415,9 @@ char* MQTTSNPacket::print(char* pbuf)
 {
     char* ptr = pbuf;
     char** pptr = &pbuf;
-	int value = 0;
-
-	int i = MQTTSNPacket_decode(_buf, _bufLen, &value);
     int size = _bufLen > SIZE_OF_LOG_PACKET ? SIZE_OF_LOG_PACKET : _bufLen;
 
-	for (; i < size; i++)
+    for (int i = 0; i < size; i++)
     {
         sprintf(*pptr, " %02X", *(_buf + i));
         *pptr += 3;
@@ -373,11 +431,11 @@ char* MQTTSNPacket::getMsgId(char* pbuf)
     int value = 0;
     int p = 0;
 
-	switch ( getType() )
+    switch (getType())
     {
     case MQTTSN_PUBLISH:
         p = MQTTSNPacket_decode(_buf, _bufLen, &value);
-		if ( _buf[p + 1] & 0x80 )
+        if (_buf[p + 1] & 0x80)
         {
             sprintf(pbuf, "+%02X%02X", _buf[p + 4], _buf[p + 5]);
         }
@@ -409,5 +467,110 @@ char* MQTTSNPacket::getMsgId(char* pbuf)
         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);
+}

MQTTSNGateway/src/MQTTSNGWPacket.h

@@ -22,12 +22,14 @@
 
 namespace MQTTSNGW
 {
+class SensorNetwork;
 
 class MQTTSNPacket
 {
 public:
-	MQTTSNPacket();
-	~MQTTSNPacket();
+    MQTTSNPacket(void);
+    MQTTSNPacket(MQTTSNPacket &packet);
+    ~MQTTSNPacket(void);
     int unicast(SensorNetwork* network, SensorNetAddress* sendTo);
     int broadcast(SensorNetwork* network);
     int recv(SensorNetwork* network);
@@ -52,31 +54,45 @@ public:
     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 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(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 getWILLTOPIC(int* willQoS, uint8_t* willRetain,
+            MQTTSNString* willTopic);
     int getWILLMSG(MQTTSNString* willmsg);
-	int getREGISTER(uint16_t* topicId, uint16_t* msgId, MQTTSNString* topicName);
+    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 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 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:

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,6 +38,7 @@ using namespace MQTTSNGW;
 
 #define EVENT_QUE_TIME_OUT  2000      // 2000 msecs
 char* currentDateTime(void);
+
 /*=====================================
  Class PacketHandleTask
  =====================================*/
@@ -40,13 +46,16 @@ char* currentDateTime(void);
 PacketHandleTask::PacketHandleTask(Gateway* gateway)
 {
     _gateway = gateway;
-	_gateway->attach((Thread*)this);
+    _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,39 +63,46 @@ PacketHandleTask::PacketHandleTask(Gateway* gateway)
  */
 PacketHandleTask::~PacketHandleTask()
 {
-	if ( _mqttConnection )
+    if (_mqttConnection)
     {
         delete _mqttConnection;
     }
-	if ( _mqttPublish )
+    if (_mqttPublish)
     {
         delete _mqttPublish;
     }
-	if ( _mqttSubscribe )
+    if (_mqttSubscribe)
     {
         delete _mqttSubscribe;
     }
-	if ( _mqttsnConnection )
+    if (_mqttsnConnection)
     {
         delete _mqttsnConnection;
     }
-	if ( _mqttsnPublish )
+    if (_mqttsnPublish)
     {
         delete _mqttsnPublish;
     }
-	if ( _mqttsnSubscribe )
+    if (_mqttsnSubscribe)
     {
         delete _mqttsnSubscribe;
     }
+
+    if (_mqttsnAggrConnection)
+    {
+        delete _mqttsnAggrConnection;
+    }
 }
 
 void PacketHandleTask::run()
 {
-	Event* ev = 0;
+    Event* ev = nullptr;
     EventQue* eventQue = _gateway->getPacketEventQue();
-	Client* client = 0;
-	MQTTSNPacket* snPacket = 0;
-	MQTTGWPacket* brPacket = 0;
+    AdapterManager* adpMgr = _gateway->getAdapterManager();
+
+    Client* client = nullptr;
+    MQTTSNPacket* snPacket = nullptr;
+    MQTTGWPacket* brPacket = nullptr;
     char msgId[6];
     memset(msgId, 0, 6);
 
@@ -99,7 +115,7 @@ void PacketHandleTask::run()
 
         if (ev->getEventType() == EtStop)
         {
-			WRITELOG("%s PacketHandleTask stopped.\n", currentDateTime());
+            WRITELOG("%s %s stopped.\n", currentDateTime(), getTaskName());
             delete ev;
             return;
         }
@@ -112,6 +128,9 @@ void PacketHandleTask::run()
                 _mqttsnConnection->sendADVERTISE();
                 _advertiseTimer.start(_gateway->getGWParams()->keepAlive * 1000UL);
             }
+
+            /*------ Check Adapters   Connect or PINGREQ ------*/
+            adpMgr->checkConnection();
         }
 
         /*------    Handle SEARCHGW Message     ---------*/
@@ -129,100 +148,224 @@ void PacketHandleTask::run()
 
             DEBUGLOG("     PacketHandleTask gets %s %s from the client.\n", snPacket->getName(), snPacket->getMsgId(msgId));
 
-			switch (snPacket->getType())
+            if (adpMgr->isAggregatedClient(client))
             {
-			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;
+                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));
-			switch (brPacket->getType())
+
+            if (client->isAggregater())
             {
-			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;
+                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;
+    }
+}
+

MQTTSNGateway/src/MQTTSNGWPacketHandleTask.h

@@ -17,49 +17,62 @@
 #ifndef MQTTSNGWPACKETHANDLETASK_H_
 #define MQTTSNGWPACKETHANDLETASK_H_
 
-#include <Timer.h>
-#include "MQTTSNGWDefines.h"
-#include "MQTTSNGateway.h"
-#include "MQTTSNGWClient.h"
-#include "MQTTSNGWPacket.h"
-#include "MQTTGWPacket.h"
-#include "MQTTGWConnectionHandler.h"
-#include "MQTTGWPublishHandler.h"
-#include "MQTTGWSubscribeHandler.h"
-#include "MQTTSNGWConnectionHandler.h"
-#include "MQTTSNGWPublishHandler.h"
-#include "MQTTSNGWSubscribeHandler.h"
-#include "SensorNetwork.h"
-
-
+#include "Timer.h"
+#include "MQTTSNGWProcess.h"
 namespace MQTTSNGW
 {
+class Gateway;
+class Client;
+class MQTTSNPacket;
+class MQTTGWPacket;
+class Timer;
+class MQTTGWConnectionHandler;
+class MQTTGWPublishHandler;
+class MQTTGWSubscribeHandler;
+class MQTTSNConnectionHandler;
+class MQTTSNPublishHandler;
+class MQTTSNSubscribeHandler;
+
+class MQTTSNAggregateConnectionHandler;
 
 #define ERRNO_APL_01  11    // Task Initialize Error
 
+class Thread;
+class Timer;
 /*=====================================
  Class PacketHandleTask
  =====================================*/
-class PacketHandleTask : public Thread
+class PacketHandleTask: public Thread
 {
-	MAGIC_WORD_FOR_THREAD;
+MAGIC_WORD_FOR_THREAD;
+    friend class MQTTGWAggregatePublishHandler;
+    friend class MQTTGWAggregateSubscribeHandler;
+    friend class MQTTSNAggregateConnectionHandler;
+    friend class MQTTSNAggregatePublishHandler;
+    friend class MQTTSNAggregateSubscribeHandler;
 public:
     PacketHandleTask(Gateway* gateway);
     ~PacketHandleTask();
     void run();
 private:
-	Gateway* _gateway;
+    void aggregatePacketHandler(Client*client, MQTTSNPacket* packet);
+    void aggregatePacketHandler(Client*client, MQTTGWPacket* packet);
+    void transparentPacketHandler(Client*client, MQTTSNPacket* packet);
+    void transparentPacketHandler(Client*client, MQTTGWPacket* packet);
+
+    Gateway* _gateway
+    { nullptr };
     Timer _advertiseTimer;
     Timer _sendUnixTimer;
-	MQTTGWConnectionHandler* _mqttConnection;
-	MQTTGWPublishHandler*    _mqttPublish;
-	MQTTGWSubscribeHandler*  _mqttSubscribe;
-	MQTTSNConnectionHandler* _mqttsnConnection;
-	MQTTSNPublishHandler*    _mqttsnPublish;
-	MQTTSNSubscribeHandler*  _mqttsnSubscribe;
+    MQTTGWConnectionHandler* _mqttConnection { nullptr };
+    MQTTGWPublishHandler* _mqttPublish { nullptr };
+    MQTTGWSubscribeHandler* _mqttSubscribe { nullptr };
+    MQTTSNConnectionHandler* _mqttsnConnection { nullptr };
+    MQTTSNPublishHandler* _mqttsnPublish { nullptr };
+    MQTTSNSubscribeHandler* _mqttsnSubscribe { nullptr };
+    MQTTSNAggregateConnectionHandler* _mqttsnAggrConnection { nullptr };
 };
 
-
 }
 
 #endif /* MQTTSNGWPACKETHANDLETASK_H_ */

MQTTSNGateway/src/MQTTSNGWProcess.cpp

@@ -13,10 +13,10 @@
  * Contributors:
  *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
  **************************************************************************************/
-
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <string>
 #include <stdarg.h>
 #include <signal.h>
 #include <Timer.h>
@@ -34,8 +34,8 @@ char* currentDateTime(void);
 /*=====================================
  Global Variables & Functions
  ======================================*/
-Process* MQTTSNGW::theProcess = 0;
-MultiTaskProcess* MQTTSNGW::theMultiTaskProcess = 0;
+Process* MQTTSNGW::theProcess = nullptr;
+MultiTaskProcess* MQTTSNGW::theMultiTaskProcess = nullptr;
 
 /*
  *  Save the type of signal
@@ -57,15 +57,17 @@ Process::Process()
     _configDir = CONFIG_DIRECTORY;
     _configFile = CONFIG_FILE;
     _log = 0;
+    _rbsem = NULL;
+    _rb = NULL;
 }
 
 Process::~Process()
 {
-	if (_rb )
+    if (_rb)
     {
         delete _rb;
     }
-	if ( _rbsem )
+    if (_rbsem)
     {
         delete _rbsem;
     }
@@ -88,22 +90,22 @@ void Process::initialize(int argc, char** argv)
     int opt;
     while ((opt = getopt(_argc, _argv, "f:")) != -1)
     {
-		if ( opt == 'f' )
+        if (opt == 'f')
         {
             string config = string(optarg);
             size_t pos = 0;
-			if ( (pos = config.find_last_of("/")) == string::npos )
+            if ((pos = config.find_last_of("/")) == string::npos)
             {
                 _configFile = optarg;
             }
             else
             {
-				_configFile = config.substr(pos + 1, config.size() - pos - 1);;
+                _configFile = config.substr(pos + 1, config.size() - pos - 1);
                 _configDir = config.substr(0, pos + 1);
             }
         }
     }
-	_rbsem = new Semaphore(MQTTSNGW_RB_SEMAPHOR_NAME, 0);
+    _rbsem = new NamedSemaphore(MQTTSNGW_RB_SEMAPHOR_NAME, 0);
     _rb = new RingBuffer(_configDir.c_str());
 
     if (getParam("ShearedMemory", param) == 0)
@@ -128,7 +130,7 @@ void Process::putLog(const char* format, ...)
     va_end(arg);
     if (strlen(_rbdata))
     {
-		if ( _log > 0 )
+        if (_log > 0)
         {
             _rb->put(_rbdata);
             _rbsem->post();
@@ -155,6 +157,8 @@ int Process::getParam(const char* parameter, char* value)
 {
     char str[MQTTSNGW_PARAM_MAX];
     char param[MQTTSNGW_PARAM_MAX];
+    memset(str, 0, sizeof(str));
+    memset(param, 0, sizeof(param));
     FILE *fp;
 
     int i = 0, j = 0;
@@ -162,28 +166,41 @@ int Process::getParam(const char* parameter, char* value)
 
     if ((fp = fopen(configPath.c_str(), "r")) == NULL)
     {
-		WRITELOG("No config file:[%s]\n", configPath.c_str());
-		return -1;
+        throw Exception("Config file not found:\n\nUsage: Command -f path/config_file_name\n", 0);
     }
 
+    int paramlen = strlen(parameter);
+
     while (true)
     {
+        int pos = 0;
+        int len = 0;
         if (fgets(str, MQTTSNGW_PARAM_MAX - 1, fp) == NULL)
         {
             fclose(fp);
             return -3;
         }
-		if (!strncmp(str, parameter, strlen(parameter)))
+        if (str[0] == '#' || str[0] == '\n')
         {
-			while (str[i++] != '=')
-			{
-				;
+            continue;
         }
-			while (str[i] != '\n')
+
+        len = strlen(str);
+        for (pos = 0; i < len; pos++)
         {
-				param[j++] = str[i++];
+            if (str[pos] == '=')
+            {
+                break;
             }
-			param[j] = '\0';
+        }
+
+        if (pos == paramlen)
+        {
+            if (strncmp(str, parameter, paramlen) == 0)
+            {
+                strcpy(param, str + pos + 1);
+                param[len - pos - 2] = '\0';
+
 
                 for (i = strlen(param) - 1; i >= 0 && isspace(param[i]); i--)
                     ;
@@ -202,6 +219,7 @@ int Process::getParam(const char* parameter, char* value)
                 return 0;
             }
         }
+    }
     fclose(fp);
     return -2;
 }
@@ -213,7 +231,7 @@ const char* Process::getLog()
     while ((len = _rb->get(_rbdata, PROCESS_LOG_BUFFER_SIZE)) == 0)
     {
         _rbsem->timedwait(1000);
-		if ( checkSignal() == SIGINT)
+        if (checkSignal() == SIGINT)
         {
             break;
         }
@@ -278,9 +296,7 @@ void MultiTaskProcess::run(void)
         _threadList[i]->start();
     }
 
-	try
-	{
-		while(true)
+    while (true)
     {
         if (theProcess->checkSignal() == SIGINT)
         {
@@ -288,15 +304,6 @@ void MultiTaskProcess::run(void)
         }
         sleep(1);
     }
-	}
-	catch(Exception* ex)
-	{
-		ex->writeMessage();
-	}
-	catch(...)
-	{
-		throw;
-	}
 }
 
 void MultiTaskProcess::waitStop(void)
@@ -307,7 +314,7 @@ void MultiTaskProcess::waitStop(void)
     }
 }
 
-void MultiTaskProcess::threadStoped(void)
+void MultiTaskProcess::threadStopped(void)
 {
     _mutex.lock();
     _stopCount++;
@@ -315,6 +322,11 @@ void MultiTaskProcess::threadStoped(void)
 
 }
 
+void MultiTaskProcess::abort(void)
+{
+    signalHandler(SIGINT);
+}
+
 void MultiTaskProcess::attach(Thread* thread)
 {
     _mutex.lock();
@@ -326,7 +338,7 @@ void MultiTaskProcess::attach(Thread* thread)
     else
     {
         _mutex.unlock();
-		throw Exception("Full of Threads");
+        throw Exception("The maximum number of threads has been exceeded.", -1);
     }
     _mutex.unlock();
 }
@@ -336,40 +348,26 @@ int MultiTaskProcess::getParam(const char* parameter, char* value)
     _mutex.lock();
     int rc = Process::getParam(parameter, value);
     _mutex.unlock();
-	if (rc == -1)
-	{
-		throw Exception("No config file.");
-	}
     return rc;
 }
 
 /*=====================================
  Class Exception
  ======================================*/
-Exception::Exception(const string& message)
+Exception::Exception(const char* message, const int errNo)
 {
     _message = message;
-	_exNo = 0;
-	_fileName = 0;
-	_functionName = 0;
+    _errNo = errNo;
+    _fileName = nullptr;
+    _functionName = nullptr;
     _line = 0;
 }
-
-Exception::Exception(const int exNo, const string& message)
+Exception::Exception(const char* message, const int errNo, const char* file, const char* function, const int line)
 {
     _message = message;
-	_exNo = exNo;
-	_fileName = 0;
-	_functionName = 0;
-	_line = 0;
-}
-
-Exception::Exception(const int exNo, const string& message, const char* file,
-		const char* function, const int line)
-{
-	_message = message;
-	_exNo = exNo;
-	_fileName = file;
+    _errNo = errNo;
+    _fileName = getFileName(file);
+    ;
     _functionName = function;
     _line = line;
 }
@@ -381,7 +379,7 @@ Exception::~Exception() throw ()
 
 const char* Exception::what() const throw ()
 {
-	return _message.c_str();
+    return _message;
 }
 
 const char* Exception::getFileName()
@@ -399,20 +397,49 @@ const int Exception::getLineNo()
     return _line;
 }
 
-const int Exception::getExceptionNo()
+const int Exception::getErrNo()
 {
-	return _exNo;
+    return _errNo;
 }
 
 void Exception::writeMessage()
 {
-	if (getExceptionNo() == 0 )
+    if (_fileName == nullptr)
     {
-		WRITELOG("%s %s\n", currentDateTime(), what());
+        if (_errNo == 0)
+        {
+            WRITELOG("%s%s %s%s\n", currentDateTime(), RED_HDR, _message, CLR_HDR);
         }
         else
         {
-		WRITELOG("%s:%-6d   %s  line %-4d %s() : %s\n", currentDateTime(), getExceptionNo(),
-			getFileName(), getLineNo(), getFunctionName(), what());
+            WRITELOG("%s%s %s.\n                    errno=%d : %s%s\n", currentDateTime(), RED_HDR, _message, _errNo,
+                    strerror(_errNo), CLR_HDR);
+        }
+    }
+    else
+    {
+        if (_errNo == 0)
+        {
+            WRITELOG("%s%s %s.  %s line %-4d %s()%s\n", currentDateTime(), RED_HDR, _message, _fileName, _line, _functionName,
+            CLR_HDR);
+        }
+        else
+        {
+            WRITELOG("%s%s %s.  %s line %-4d %s()\n                    errno=%d : %s%s\n", currentDateTime(), RED_HDR, _message,
+                    _fileName, _line, _functionName, _errNo, strerror(_errNo), CLR_HDR);
+        }
     }
 }
+
+const char* Exception::getFileName(const char* file)
+{
+    for (int len = strlen(file); len > 0; len--)
+    {
+        if (*(file + len) == '/')
+        {
+            return file + len + 1;
+        }
+    }
+    return file;
+}
+

MQTTSNGateway/src/MQTTSNGWProcess.h

@@ -41,6 +41,8 @@ namespace MQTTSNGW
 #define WRITELOG theProcess->putLog
 #define CHK_SIGINT (theProcess->checkSignal() == SIGINT)
 #define UNUSED(x) ((void)(x))
+#define EXCEPTION(...)   Exception(__VA_ARGS__, __FILE__, __func__, __LINE__)
+
 /*=================================
  Class Process
  ==================================*/
@@ -66,7 +68,7 @@ private:
     string _configDir;
     string _configFile;
     RingBuffer* _rb;
-	Semaphore*  _rbsem;
+    NamedSemaphore* _rbsem;
     Mutex _mt;
     int _log;
     char _rbdata[PROCESS_LOG_BUFFER_SIZE + 1];
@@ -84,8 +86,9 @@ public:
     int getParam(const char* parameter, char* value);
     void run(void);
     void waitStop(void);
-	void threadStoped(void);
+    void threadStopped(void);
     void attach(Thread* thread);
+    void abort(void);
 
 private:
     Thread* _threadList[MQTTSNGW_MAX_TASK];
@@ -100,27 +103,25 @@ private:
 class Exception: public exception
 {
 public:
-	Exception(const string& message);
-	Exception(const int exNo, const string& message);
-	Exception(const int exNo, const string& message,
-			const char* file, const char* func, const int line);
+    Exception(const char* message, const int errNo);
+    Exception(const char* message, const int errNo, const char* file, const char* func, int line);
     virtual ~Exception() throw ();
     const char* getFileName();
     const char* getFunctionName();
     const int getLineNo();
-	const int getExceptionNo();
+    const int getErrNo();
     virtual const char* what() const throw ();
     void writeMessage();
 
 private:
-	int _exNo;
-	string _message;
+    const char* getFileName(const char* file);
+    int _errNo;
+    const char* _message;
     const char* _fileName;
     const char* _functionName;
     int _line;
 };
 
-
 /*=====================================
  Class QueElement
  ====================================*/
@@ -132,8 +133,8 @@ public:
     QueElement(T* t)
     {
         _element = t;
-		_next = 0;
-		_prev = 0;
+        _next = nullptr;
+        _prev = nullptr;
     }
 
     ~QueElement()
@@ -155,8 +156,8 @@ class Que
 public:
     Que()
     {
-		_head = 0;
-		_tail = 0;
+        _head = nullptr;
+        _tail = nullptr;
         _cnt = 0;
         _maxSize = 0;
     }
@@ -175,17 +176,17 @@ public:
 
     void pop(void)
     {
-		if ( _head )
+        if (_head)
         {
             QueElement<T>* head = _head;
-			if ( _head == _tail )
+            if (_head == _tail)
             {
-				_head = _tail = 0;
+                _head = _tail = nullptr;
             }
             else
             {
                 _head = head->_next;
-				head->_prev = 0;
+                head->_prev = nullptr;
             }
             delete head;
             _cnt--;
@@ -195,7 +196,7 @@ public:
     T* front(void)
     {
         {
-			if ( _head )
+            if (_head)
             {
                 return _head->_element;
             }
@@ -208,12 +209,12 @@ public:
 
     int post(T* t)
     {
-		if ( t && ( _maxSize == 0 || _cnt < _maxSize ))
+        if (t && (_maxSize == 0 || _cnt < _maxSize))
         {
             QueElement<T>* elm = new QueElement<T>(t);
-			if ( _head )
+            if (_head)
             {
-				if ( _tail == _head )
+                if (_tail == _head)
                 {
                     elm->_prev = _tail;
                     _tail = elm;
@@ -262,8 +263,9 @@ private:
 #define TREE23_BI_NODE        (2)
 #define TREE23_TRI_NODE       (3)
 
-template <typename K, typename V>
-class Tree23Elm{
+template<typename K, typename V>
+class Tree23Elm
+{
     template<typename T, typename U> friend class Tree23;
 public:
     Tree23Elm()
@@ -292,9 +294,9 @@ private:
     V* _val;
 };
 
-
-template <typename K, typename V>
-class Tree23Node{
+template<typename K, typename V>
+class Tree23Node
+{
     template<typename S, typename W> friend class Tree23;
 public:
     Tree23Node(const int type)
@@ -320,7 +322,8 @@ public:
         _left = _midle = _right = NULL;
     }
 
-	Tree23Node(const int type, Tree23Elm<K, V>* telm, Tree23Node<K, V>* left, Tree23Node<K, V>* right)
+    Tree23Node(const int type, Tree23Elm<K, V>* telm, Tree23Node<K, V>* left,
+            Tree23Node<K, V>* right)
     {
         _type = type;
         _telm0 = telm;
@@ -330,7 +333,9 @@ public:
         _right = right;
     }
 
-	Tree23Node(const int type, Tree23Elm<K, V>* telm0, Tree23Elm<K, V>* telm1, Tree23Node<K, V>* left, Tree23Node<K, V>* midle, Tree23Node<K, V>* right)
+    Tree23Node(const int type, Tree23Elm<K, V>* telm0, Tree23Elm<K, V>* telm1,
+            Tree23Node<K, V>* left, Tree23Node<K, V>* midle,
+            Tree23Node<K, V>* right)
     {
         _type = type;
         _telm0 = telm0;
@@ -354,8 +359,9 @@ private:
     Tree23Node<K, V>* _right;
 };
 
-template <typename K, typename V>
-class Tree23{
+template<typename K, typename V>
+class Tree23
+{
 public:
     Tree23()
     {
@@ -364,7 +370,7 @@ public:
 
     ~Tree23()
     {
-		if ( _root )
+        if (_root)
         {
             delete _root;
         }
@@ -372,28 +378,28 @@ public:
 
     void add(K* key, V* val)
     {
-		_root = add( _root, new Tree23Elm<K, V>(key, val));
+        _root = add(_root, new Tree23Elm<K, V>(key, val));
         _root->_type = abs(_root->_type);
     }
 
     Tree23Node<K, V>* add(Tree23Node<K, V>* n, Tree23Elm<K, V>* elm)
     {
-		if ( n == 0 )
+        if (n == 0)
         {
             return new Tree23Node<K, V>(TREE23_INSERT_ACTIVE, elm);
         }
 
         int cmp0 = elm->compare(n->_telm0);
         int cmp1 = 0;
-		switch ( n->_type )
+        switch (n->_type)
         {
         case 2:
-			if ( cmp0 < 0 )
+            if (cmp0 < 0)
             {
                 n->_left = add(n->_left, elm);
                 return addLeft2(n);
             }
-			else if ( cmp0 == 0 )
+            else if (cmp0 == 0)
             {
                 n->_telm0 = elm;
                 return n;
@@ -406,22 +412,22 @@ public:
             break;
         case 3:
             cmp1 = elm->compare(n->_telm1);
-			if ( cmp0 < 0 )
+            if (cmp0 < 0)
             {
                 n->_left = add(n->_left, elm);
                 return addLeft3(n);
             }
-			else if ( cmp0 == 0 )
+            else if (cmp0 == 0)
             {
                 n->_telm0 = elm;
                 return n;
             }
-			else if ( cmp1 < 0 )
+            else if (cmp1 < 0)
             {
                 n->_midle = add(n->_midle, elm);
                 return addMidle3(n);
             }
-			else if ( cmp1 == 0 )
+            else if (cmp1 == 0)
             {
                 n->_telm1 = elm;
                 return n;
@@ -441,7 +447,7 @@ public:
     void remove(K* k)
     {
         _root = remove(_root, k);
-		if ( _root != NULL && _root->_type == TREE23_DELETE_ACTIVE )
+        if (_root != NULL && _root->_type == TREE23_DELETE_ACTIVE)
         {
             _root = _root->_midle;
         }
@@ -449,23 +455,23 @@ public:
 
     Tree23Node<K, V>* remove(Tree23Node<K, V>* node, K* k)
     {
-		if ( node == NULL )
+        if (node == NULL)
         {
             return NULL;
         }
         int cmp0 = k->compare(node->_telm0->_key);
         int cmp1 = 0;
-		switch ( node->_type )
+        switch (node->_type)
         {
         case 2:
-			if ( cmp0 < 0 )
+            if (cmp0 < 0)
             {
-				node->_left = remove( node->_left, k);
+                node->_left = remove(node->_left, k);
                 return removeLeft2(node);
             }
-			else if ( cmp0 == 0 )
+            else if (cmp0 == 0)
             {
-				if ( node->_left == NULL)
+                if (node->_left == NULL)
                 {
                     return new Tree23Node<K, V>(TREE23_DELETE_ACTIVE);
                 }
@@ -481,14 +487,14 @@ public:
             }
         case 3:
             cmp1 = k->compare(node->_telm1->_key);
-			if ( cmp0 < 0 )
+            if (cmp0 < 0)
             {
                 node->_left = remove(node->_left, k);
                 return removeLeft3(node);
             }
-			else if ( cmp0 == 0 )
+            else if (cmp0 == 0)
             {
-				if ( node->_left == NULL )
+                if (node->_left == NULL)
                 {
                     return new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm1);
                 }
@@ -497,14 +503,14 @@ public:
                 node->_telm0 = maxLeft;
                 return removeLeft3(node);
             }
-			else if ( cmp1 < 0 )
+            else if (cmp1 < 0)
             {
                 node->_midle = remove(node->_midle, k);
                 return removeMidle3(node);
             }
-			else if ( cmp1 == 0 )
+            else if (cmp1 == 0)
             {
-				if ( node->_midle == NULL )
+                if (node->_midle == NULL)
                 {
                     return new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0);
                 }
@@ -534,8 +540,9 @@ public:
             switch (node->_type)
             {
             case 2:
-				if ( cmp0 <  0 ) node = node->_left;
-				else if ( cmp0 == 0 )
+                if (cmp0 < 0)
+                    node = node->_left;
+                else if (cmp0 == 0)
                 {
                     return true;
                 }
@@ -546,19 +553,19 @@ public:
                 break;
             case 3:
                 cmp1 = key->compare(node->_telm1->_key);
-				if ( cmp0 <  0 )
+                if (cmp0 < 0)
                 {
                     node = node->_left;
                 }
-				else if ( cmp0 == 0 )
+                else if (cmp0 == 0)
                 {
                     return true;
                 }
-				else if ( cmp1 <  0 )
+                else if (cmp1 < 0)
                 {
                     node = node->_midle;
                 }
-				else if ( cmp1 == 0 )
+                else if (cmp1 == 0)
                 {
                     return true;
                 }
@@ -574,7 +581,6 @@ public:
         return false;
     }
 
-
     V* getVal(K* key)
     {
         Tree23Node<K, V>* node = _root;
@@ -585,11 +591,11 @@ public:
             switch (node->_type)
             {
             case 2:
-				if ( cmp0 <  0 )
+                if (cmp0 < 0)
                 {
                     node = node->_left;
                 }
-				else if ( cmp0 == 0 )
+                else if (cmp0 == 0)
                 {
                     return node->_telm0->_val;
                 }
@@ -600,19 +606,19 @@ public:
                 break;
             case 3:
                 cmp1 = key->compare(node->_telm1->_key);
-				if ( cmp0 < 0 )
+                if (cmp0 < 0)
                 {
                     node = node->_left;
                 }
-				else if ( cmp0 == 0 )
+                else if (cmp0 == 0)
                 {
                     return node->_telm0->_val;
                 }
-				else if ( cmp1 <  0 )
+                else if (cmp1 < 0)
                 {
                     node = node->_midle;
                 }
-				else if ( cmp1 == 0 )
+                else if (cmp1 == 0)
                 {
                     return node->_telm1->_val;
                 }
@@ -632,9 +638,10 @@ private:
     Tree23Node<K, V>* addLeft2(Tree23Node<K, V>* node)
     {
         Tree23Node<K, V>* n = node->_left;
-		if ( n != NULL && n->_type == TREE23_INSERT_ACTIVE )
+        if (n != NULL && n->_type == TREE23_INSERT_ACTIVE)
         {
-			return new Tree23Node<K, V>(TREE23_TRI_NODE, n->_telm0, node->_telm0, n->_left, n->_right, node->_right);
+            return new Tree23Node<K, V>(TREE23_TRI_NODE, n->_telm0,
+                    node->_telm0, n->_left, n->_right, node->_right);
         }
         return node;
     }
@@ -642,11 +649,13 @@ private:
     Tree23Node<K, V>* addLeft3(Tree23Node<K, V>* node)
     {
         Tree23Node<K, V>* n = node->_left;
-		if ( n != NULL && n->_type == TREE23_INSERT_ACTIVE)
+        if (n != NULL && n->_type == TREE23_INSERT_ACTIVE)
         {
             n->_type = TREE23_BI_NODE;
-			Tree23Node<K, V>* nn = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm1, node->_midle, node->_right);
-			return new Tree23Node<K, V>(TREE23_INSERT_ACTIVE, node->_telm0, n, nn);
+            Tree23Node<K, V>* nn = new Tree23Node<K, V>(TREE23_BI_NODE,
+                    node->_telm1, node->_midle, node->_right);
+            return new Tree23Node<K, V>(TREE23_INSERT_ACTIVE, node->_telm0, n,
+                    nn);
         }
         return node;
     }
@@ -656,7 +665,8 @@ private:
         Tree23Node<K, V>* n = node->_right;
         if (n != NULL && n->_type == TREE23_INSERT_ACTIVE)
         {
-			return new Tree23Node<K, V>(TREE23_TRI_NODE, node->_telm0, n->_telm0, node->_left, n->_left, n->_right);
+            return new Tree23Node<K, V>(TREE23_TRI_NODE, node->_telm0,
+                    n->_telm0, node->_left, n->_left, n->_right);
         }
         return node;
     }
@@ -664,10 +674,13 @@ private:
     Tree23Node<K, V>* addRight3(Tree23Node<K, V>* node)
     {
         Tree23Node<K, V>* n = node->_right;
-		if (n != NULL &&  n->_type == TREE23_INSERT_ACTIVE) {
+        if (n != NULL && n->_type == TREE23_INSERT_ACTIVE)
+        {
             n->_type = TREE23_BI_NODE;
-			Tree23Node<K, V>* nn = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0, node->_left, node->_midle);
-			return new Tree23Node<K, V>(TREE23_INSERT_ACTIVE, node->_telm1, nn, n);
+            Tree23Node<K, V>* nn = new Tree23Node<K, V>(TREE23_BI_NODE,
+                    node->_telm0, node->_left, node->_midle);
+            return new Tree23Node<K, V>(TREE23_INSERT_ACTIVE, node->_telm1, nn,
+                    n);
         }
         return node;
     }
@@ -675,16 +688,17 @@ private:
     Tree23Node<K, V>* addMidle3(Tree23Node<K, V>* node)
     {
         Tree23Node<K, V>* n = node->_midle;
-		if ( n != NULL && n->_type == TREE23_INSERT_ACTIVE )
+        if (n != NULL && n->_type == TREE23_INSERT_ACTIVE)
         {
-			n->_left = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0, node->_left, n->_left);
-			n->_right = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm1, n->_right, node->_right);
+            n->_left = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0,
+                    node->_left, n->_left);
+            n->_right = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm1,
+                    n->_right, node->_right);
             return n;
         }
         return node;
     }
 
-
     Tree23Node<K, V>* removeMax(Tree23Node<K, V>* node, Tree23Elm<K, V>* elm)
     {
         if (node->_right == NULL)
@@ -719,26 +733,29 @@ private:
         return NULL;
     }
 
-
     Tree23Node<K, V>* removeLeft2(Tree23Node<K, V>* node)
     {
         Tree23Node<K, V>* n = node->_left;
-		if ( n != NULL && n->_type == TREE23_DELETE_ACTIVE )
+        if (n != NULL && n->_type == TREE23_DELETE_ACTIVE)
         {
             Tree23Node<K, V>* r = node->_right;
             Tree23Node<K, V>* midle;
             Tree23Node<K, V>* left;
             Tree23Node<K, V>* right;
 
-			switch ( r->_type )
+            switch (r->_type)
             {
             case 2:
-				midle = new Tree23Node<K, V>(TREE23_TRI_NODE, node->_telm0, r->_telm0, n->_midle, r->_left, r->_right);
+                midle = new Tree23Node<K, V>(TREE23_TRI_NODE, node->_telm0,
+                        r->_telm0, n->_midle, r->_left, r->_right);
                 return new Tree23Node<K, V>(TREE23_DELETE_ACTIVE, midle);
             case 3:
-				left = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0, n->_midle, r->_left);
-				right = new Tree23Node<K, V>(TREE23_BI_NODE, r->_telm1, r->_midle, r->_right);
-				return new Tree23Node<K, V>(TREE23_BI_NODE, r->_telm0, left, right);
+                left = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0,
+                        n->_midle, r->_left);
+                right = new Tree23Node<K, V>(TREE23_BI_NODE, r->_telm1,
+                        r->_midle, r->_right);
+                return new Tree23Node<K, V>(TREE23_BI_NODE, r->_telm0, left,
+                        right);
             default:
                 break;
             }
@@ -749,7 +766,7 @@ private:
     Tree23Node<K, V>* removeRight2(Tree23Node<K, V>* node)
     {
         Tree23Node<K, V>* n = node->_right;
-		if ( n != NULL && n->_type == TREE23_DELETE_ACTIVE )
+        if (n != NULL && n->_type == TREE23_DELETE_ACTIVE)
         {
             Tree23Node<K, V>* l = node->_left;
             Tree23Node<K, V>* midle;
@@ -759,12 +776,16 @@ private:
             switch (l->_type)
             {
             case 2:
-				midle = new Tree23Node<K, V>(TREE23_TRI_NODE, l->_telm0, node->_telm0, l->_left, l->_right, n->_midle);
+                midle = new Tree23Node<K, V>(TREE23_TRI_NODE, l->_telm0,
+                        node->_telm0, l->_left, l->_right, n->_midle);
                 return new Tree23Node<K, V>(-1, midle);
             case 3:
-				right = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0, l->_right, n->_midle);
-				left = new Tree23Node<K, V>(TREE23_BI_NODE, l->_telm0, l->_left, l->_midle);
-				return new Tree23Node<K, V>(TREE23_BI_NODE, l->_telm1, left, right);
+                right = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0,
+                        l->_right, n->_midle);
+                left = new Tree23Node<K, V>(TREE23_BI_NODE, l->_telm0, l->_left,
+                        l->_midle);
+                return new Tree23Node<K, V>(TREE23_BI_NODE, l->_telm1, left,
+                        right);
             default:
                 break;
             }
@@ -775,21 +796,27 @@ private:
     Tree23Node<K, V>* removeLeft3(Tree23Node<K, V>* node)
     {
         Tree23Node<K, V>* n = node->_left;
-		if ( n != NULL && n->_type == TREE23_DELETE_ACTIVE )
+        if (n != NULL && n->_type == TREE23_DELETE_ACTIVE)
         {
             Tree23Node<K, V>* m = node->_midle;
             Tree23Node<K, V>* r = node->_right;
             Tree23Node<K, V>* left;
             Tree23Node<K, V>* midle;
 
-			switch (m->_type) {
+            switch (m->_type)
+            {
             case 2:
-				left = new Tree23Node<K, V>(TREE23_TRI_NODE, node->_telm0, m->_telm0, n->_midle, m->_left, m->_right);
-				return new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm1, left, r);
+                left = new Tree23Node<K, V>(TREE23_TRI_NODE, node->_telm0,
+                        m->_telm0, n->_midle, m->_left, m->_right);
+                return new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm1, left,
+                        r);
             case 3:
-				left = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0, n->_midle, m->_left);
-				midle = new Tree23Node<K, V>(TREE23_BI_NODE, m->_telm1, m->_midle, m->_right);
-				return new Tree23Node<K, V>(TREE23_TRI_NODE, m->_telm0, node->_telm1, left, midle, r);
+                left = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0,
+                        n->_midle, m->_left);
+                midle = new Tree23Node<K, V>(TREE23_BI_NODE, m->_telm1,
+                        m->_midle, m->_right);
+                return new Tree23Node<K, V>(TREE23_TRI_NODE, m->_telm0,
+                        node->_telm1, left, midle, r);
             default:
                 break;
             }
@@ -800,7 +827,7 @@ private:
     Tree23Node<K, V>* removeMidle3(Tree23Node<K, V>* node)
     {
         Tree23Node<K, V>* n = node->_midle;
-	        if ( n != NULL && n->_type == TREE23_DELETE_ACTIVE )
+        if (n != NULL && n->_type == TREE23_DELETE_ACTIVE)
         {
             Tree23Node<K, V>* l = node->_left;
             Tree23Node<K, V>* r = node->_right;
@@ -809,12 +836,17 @@ private:
             switch (r->_type)
             {
             case 2:
-					right = new Tree23Node<K, V>(TREE23_TRI_NODE, node->_telm1, r->_telm0, n->_midle, r->_left, r->_right);
-					return new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0, l, right);
+                right = new Tree23Node<K, V>(TREE23_TRI_NODE, node->_telm1,
+                        r->_telm0, n->_midle, r->_left, r->_right);
+                return new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0, l,
+                        right);
             case 3:
-					midle = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm1, n->_midle, r->_left);
-					right = new Tree23Node<K, V>(TREE23_BI_NODE, r->_telm1, r->_midle, r->_right);
-					return new Tree23Node<K, V>(TREE23_TRI_NODE, node->_telm0, r->_telm0, l, midle, right);
+                midle = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm1,
+                        n->_midle, r->_left);
+                right = new Tree23Node<K, V>(TREE23_BI_NODE, r->_telm1,
+                        r->_midle, r->_right);
+                return new Tree23Node<K, V>(TREE23_TRI_NODE, node->_telm0,
+                        r->_telm0, l, midle, right);
             default:
                 break;
             }
@@ -825,7 +857,7 @@ private:
     Tree23Node<K, V>* removeRight3(Tree23Node<K, V>* node)
     {
         Tree23Node<K, V>* n = node->_right;
-	        if ( n != NULL && n->_type == TREE23_DELETE_ACTIVE )
+        if (n != NULL && n->_type == TREE23_DELETE_ACTIVE)
         {
             Tree23Node<K, V>* l = node->_left;
             Tree23Node<K, V>* m = node->_midle;
@@ -834,12 +866,17 @@ private:
             switch (m->_type)
             {
             case 2:
-					right = new Tree23Node<K, V>(TREE23_TRI_NODE, m->_telm0, node->_telm1, m->_left, m->_right, n->_midle);
-					return new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0, l, right);
+                right = new Tree23Node<K, V>(TREE23_TRI_NODE, m->_telm0,
+                        node->_telm1, m->_left, m->_right, n->_midle);
+                return new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0, l,
+                        right);
             case 3:
-					right = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm1, m->_right, n->_midle);
-					midle = new Tree23Node<K, V>(TREE23_BI_NODE, m->_telm0, m->_left, m->_midle);
-					return new Tree23Node<K, V>(TREE23_TRI_NODE, node->_telm0, m->_telm1, l, midle, right);
+                right = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm1,
+                        m->_right, n->_midle);
+                midle = new Tree23Node<K, V>(TREE23_BI_NODE, m->_telm0,
+                        m->_left, m->_midle);
+                return new Tree23Node<K, V>(TREE23_TRI_NODE, node->_telm0,
+                        m->_telm1, l, midle, right);
             default:
                 break;
             }
@@ -847,48 +884,52 @@ private:
         return node;
     }
 
-
     Tree23Node<K, V>* _root;
 };
 
 /*=====================================
  Class List
  =====================================*/
-template <typename T>
+template<typename T>
 class ListElm
 {
     template<typename U> friend class List;
 public:
     ListElm()
     {
-		_elm = 0;
-		_prev = _next = 0;
+        _elm = nullptr;
+        _prev = _next = nullptr;
     }
     ListElm(T* elm)
     {
         _elm = elm;
-		_prev = _next = 0;
+        _prev = _next = nullptr;
     }
     T* getContent(void)
     {
         return _elm;
     }
-	~ListElm(){}
+    ~ListElm()
+    {
+    }
 
 private:
-	ListElm<T>* getNext(void){return _next;}
+    ListElm<T>* getNext(void)
+    {
+        return _next;
+    }
     T* _elm;
     ListElm<T>* _prev;
     ListElm<T>* _next;
 };
 
-
-template <typename T>
-class List{
+template<typename T>
+class List
+{
 public:
     List()
     {
-		_head = _tail = 0;
+        _head = _tail = nullptr;
         _size = 0;
     }
     ~List()
@@ -899,11 +940,11 @@ public:
     int add(T* t)
     {
         ListElm<T>* elm = new ListElm<T>(t);
-		if ( elm == 0 )
+        if (elm == nullptr)
         {
             return 0;
         }
-		if ( _head == 0 )
+        if (_head == nullptr)
         {
             _head = elm;
             _tail = elm;
@@ -920,16 +961,16 @@ public:
 
     void erase(ListElm<T>* elm)
     {
-		if ( _head == elm )
+        if (_head == elm)
         {
             _head = elm->_next;
             _size--;
             delete elm;
         }
-		else if ( _tail == elm )
+        else if (_tail == elm)
         {
             _tail = elm->_prev;
-			elm->_prev->_next = 0;
+            elm->_prev->_next = nullptr;
             _size--;
             delete elm;
         }
@@ -944,14 +985,14 @@ public:
     void clear(void)
     {
         ListElm<T>* p = _head;
-		while ( p )
+        while (p)
         {
             ListElm<T>* q = p->_next;
             delete p;
             p = q;
         }
-		_head = 0;
-		_tail = 0;
+        _head = nullptr;
+        _tail = nullptr;
         _size = 0;
     }
 
@@ -970,14 +1011,12 @@ public:
         return _size;
     }
 
-
 private:
     ListElm<T>* _head;
     ListElm<T>* _tail;
     int _size;
 };
 
-
 extern Process* theProcess;
 extern MultiTaskProcess* theMultiTaskProcess;
 

MQTTSNGateway/src/MQTTSNGWPublishHandler.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 "MQTTSNGWPublishHandler.h"
@@ -19,6 +20,7 @@
 #include "MQTTGWPacket.h"
 #include "MQTTSNGateway.h"
 #include "MQTTSNGWClient.h"
+#include "MQTTSNGWQoSm1Proxy.h"
 #include <string.h>
 using namespace std;
 using namespace MQTTSNGW;
@@ -33,143 +35,114 @@ MQTTSNPublishHandler::~MQTTSNPublishHandler()
 
 }
 
-void MQTTSNPublishHandler::handlePublish(Client* client, MQTTSNPacket* packet)
+MQTTGWPacket* MQTTSNPublishHandler::handlePublish(Client* client, MQTTSNPacket* packet)
 {
     uint8_t dup;
     int qos;
     uint8_t retained;
     uint16_t msgId;
-	MQTTSN_topicid topicid;
+    uint16_t tid;
     uint8_t* payload;
+    MQTTSN_topicid topicid;
     int payloadlen;
-	Publish pub;
+    Publish pub = MQTTPacket_Publish_Initializer;
+
     char shortTopic[2];
 
-	if ( !client->isActive() )
+    if (!_gateway->getAdapterManager()->getQoSm1Proxy()->isActive())
     {
-		/* Reply DISCONNECT to the client */
-		Event* ev = new Event();
-		MQTTSNPacket* disconnect = new MQTTSNPacket();
-		disconnect->setDISCONNECT(0);
-		ev->setClientSendEvent(client, disconnect);
-		_gateway->getClientSendQue()->post(ev);
-		return;
+        if (client->isQoSm1())
+        {
+            _gateway->getAdapterManager()->getQoSm1Proxy()->savePacket(client, packet);
+
+            return nullptr;
+        }
     }
 
-	if ( packet->getPUBLISH(&dup, &qos, &retained, &msgId, &topicid, &payload, &payloadlen) ==0 )
+    if (packet->getPUBLISH(&dup, &qos, &retained, &msgId, &topicid, &payload, &payloadlen) == 0)
     {
-		return;
+        return nullptr;
     }
     pub.msgId = msgId;
     pub.header.bits.dup = dup;
-	pub.header.bits.qos = qos;
+    pub.header.bits.qos = (qos == 3 ? 0 : qos);
     pub.header.bits.retain = retained;
+    tid = topicid.data.id;
 
-	Topic* topic = 0;
+    Topic* topic = nullptr;
 
-	if ( topicid.type == MQTTSN_TOPIC_TYPE_PREDEFINED)
-	{
-		if(msgId)
-		{
-			/* Reply PubAck to the client */
-			MQTTSNPacket* pubAck = new MQTTSNPacket();
-			pubAck->setPUBACK( topicid.data.id, msgId, MQTTSN_RC_ACCEPTED);
-			Event* ev1 = new Event();
-			ev1->setClientSendEvent(client, pubAck);
-			_gateway->getClientSendQue()->post(ev1);
-		}
-
-#ifdef OTA_CLIENTS
-		if ( topicid.data.id == PREDEFINEDID_OTA_REQ )
-		{
-			uint8_t clientId[MAX_CLIENTID_LENGTH + 1];
-
-			if ( payloadlen <= MAX_CLIENTID_LENGTH )
-			{
-				memcpy(clientId, payload, payloadlen);
-				clientId[payloadlen] = 0;
-				Client* cl = _gateway->getClientList()->getClient(clientId);
-
-				if ( cl )
-				{
-					WRITELOG("\033[0m\033[0;33m OTA Client : %s\033[0m\033[0;37m\n",cl->getClientId());
-					MQTTSNPacket* pubota = new MQTTSNPacket();
-					pubota->setPUBLISH(0, 0, 0, 0, topicid, 0, 0);
-					cl->setOTAClient(client);
-					Event* evt = new Event();
-					evt->setClientSendEvent(cl, pubota);
-					_gateway->getClientSendQue()->post(evt);
-				}
-				else
-				{
-					MQTTSNPacket* publish = new MQTTSNPacket();
-					topicid.data.id = PREDEFINEDID_OTA_NO_CLIENT;
-					publish->setPUBLISH(0, 0, 0, 0, topicid, clientId, (uint16_t)strlen((const char*)clientId));
-					Event* evt = new Event();
-					evt->setClientSendEvent(client, publish);
-					_gateway->getClientSendQue()->post(evt);
-				}
-			}
-		}
-		else if ( topicid.data.id == PREDEFINEDID_OTA_READY )
-		{
-			Client* cl = client->getOTAClient();
-			if ( cl )
-			{
-				WRITELOG("\033[0m\033[0;33m OTA Manager : %s\033[0m\033[0;37m\n",cl->getClientId());
-				MQTTSNPacket* pubota = new MQTTSNPacket();
-				pubota->setPUBLISH(0, 0, 0, 0, topicid, payload, payloadlen);
-				client->setOTAClient(0);
-				Event* evt = new Event();
-				evt->setClientSendEvent(cl, pubota);
-				_gateway->getClientSendQue()->post(evt);
-			}
-		}
-#endif
-		return;
-	}
-
-	if( topicid.type ==  MQTTSN_TOPIC_TYPE_SHORT )
+    if (topicid.type == MQTTSN_TOPIC_TYPE_SHORT)
     {
         shortTopic[0] = topicid.data.short_name[0];
         shortTopic[1] = topicid.data.short_name[1];
         pub.topic = shortTopic;
         pub.topiclen = 2;
     }
-
-	if ( topicid.type == MQTTSN_TOPIC_TYPE_NORMAL )
+    else
     {
-		topic = client->getTopics()->getTopic(topicid.data.id);
-		if( !topic && msgId && qos > 0 )
+        topic = client->getTopics()->getTopicById(&topicid);
+        if (!topic)
+        {
+            topic = _gateway->getTopics()->getTopicById(&topicid);
+            if (topic)
+            {
+                topic = client->getTopics()->add(topic->getTopicName()->c_str(), topic->getTopicId());
+            }
+        }
+
+        if (!topic && qos == 3)
+        {
+            WRITELOG("%s Invalid TopicId.%s %s\n", ERRMSG_HEADER, client->getClientId(), ERRMSG_FOOTER);
+            return nullptr;
+        }
+
+        if ((qos == 0 || qos == 3) && msgId > 0)
+        {
+            WRITELOG("%s Invalid MsgId.%s %s\n", ERRMSG_HEADER, client->getClientId(), ERRMSG_FOOTER);
+            return nullptr;
+        }
+
+        if (!topic && msgId && qos > 0 && qos < 3)
         {
             /* Reply PubAck with INVALID_TOPIC_ID to the client */
             MQTTSNPacket* pubAck = new MQTTSNPacket();
-			pubAck->setPUBACK( topicid.data.id, msgId, MQTTSN_RC_REJECTED_INVALID_TOPIC_ID);
+            pubAck->setPUBACK(topicid.data.id, msgId, MQTTSN_RC_REJECTED_INVALID_TOPIC_ID);
             Event* ev1 = new Event();
             ev1->setClientSendEvent(client, pubAck);
             _gateway->getClientSendQue()->post(ev1);
-			return;
+            return nullptr;
         }
-		if ( topic )
+        if (topic)
         {
-			pub.topic = (char*)topic->getTopicName()->data();
+            pub.topic = (char*) topic->getTopicName()->data();
             pub.topiclen = topic->getTopicName()->length();
+            topicid.data.long_.name = pub.topic;
+            topicid.data.long_.len = pub.topiclen;
         }
     }
     /* Save a msgId & a TopicId pare for PUBACK */
-	if( msgId && qos > 0 )
+    if (msgId && qos > 0 && qos < 3)
     {
-		client->setWaitedPubTopicId(msgId, topicid.data.id, topicid.type);
+        client->setWaitedPubTopicId(msgId, tid, &topicid);
     }
 
-	pub.payload = (char*)payload;
+    pub.payload = (char*) payload;
     pub.payloadlen = payloadlen;
 
     MQTTGWPacket* publish = new MQTTGWPacket();
     publish->setPUBLISH(&pub);
+
+    if (_gateway->getAdapterManager()->isAggregaterActive() && client->isAggregated())
+    {
+        return publish;
+    }
+    else
+    {
         Event* ev1 = new Event();
         ev1->setBrokerSendEvent(client, publish);
         _gateway->getBrokerSendQue()->post(ev1);
+        return nullptr;
+    }
 }
 
 void MQTTSNPublishHandler::handlePuback(Client* client, MQTTSNPacket* packet)
@@ -178,39 +151,38 @@ void MQTTSNPublishHandler::handlePuback(Client* client, MQTTSNPacket* packet)
     uint16_t msgId;
     uint8_t rc;
 
-	if ( !client->isActive() )
+    if (client->isActive())
+    {
+        if (packet->getPUBACK(&topicId, &msgId, &rc) == 0)
         {
             return;
         }
+
+        if (rc == MQTTSN_RC_ACCEPTED)
+        {
+            if (!_gateway->getAdapterManager()->getAggregater()->isActive())
+            {
                 MQTTGWPacket* pubAck = new MQTTGWPacket();
-
-	if ( packet->getPUBACK(&topicId, &msgId, &rc) == 0 )
-	{
-		return;
-	}
-
-	if ( rc == MQTTSN_RC_ACCEPTED)
-	{
                 pubAck->setAck(PUBACK, msgId);
                 Event* ev1 = new Event();
                 ev1->setBrokerSendEvent(client, pubAck);
                 _gateway->getBrokerSendQue()->post(ev1);
             }
-	else if ( rc == MQTTSN_RC_REJECTED_INVALID_TOPIC_ID)
+        }
+        else if (rc == MQTTSN_RC_REJECTED_INVALID_TOPIC_ID)
         {
             WRITELOG("  PUBACK   %d : Invalid Topic ID\n", msgId);
         }
+    }
 }
 
 void MQTTSNPublishHandler::handleAck(Client* client, MQTTSNPacket* packet, uint8_t packetType)
 {
     uint16_t msgId;
 
-	if ( !client->isActive() )
+    if (client->isActive())
     {
-		return;
-	}
-	if ( packet->getACK(&msgId) == 0 )
+        if (packet->getACK(&msgId) == 0)
         {
             return;
         }
@@ -219,22 +191,20 @@ void MQTTSNPublishHandler::handleAck(Client* client, MQTTSNPacket* packet, uint8
         Event* ev1 = new Event();
         ev1->setBrokerSendEvent(client, ackPacket);
         _gateway->getBrokerSendQue()->post(ev1);
+    }
 }
 
 void MQTTSNPublishHandler::handleRegister(Client* client, MQTTSNPacket* packet)
 {
     uint16_t id;
     uint16_t msgId;
-	MQTTSNString topicName;
+    MQTTSNString topicName = MQTTSNString_initializer;
+    ;
     MQTTSN_topicid topicid;
 
-
-	if ( !client->isActive() )
+    if (client->isActive() || client->isAwake())
     {
-		return;
-	}
-	MQTTSNPacket* regAck = new MQTTSNPacket();
-	if ( packet->getREGISTER(&id, &msgId, &topicName) == 0 )
+        if (packet->getREGISTER(&id, &msgId, &topicName) == 0)
         {
             return;
         }
@@ -244,9 +214,90 @@ void MQTTSNPublishHandler::handleRegister(Client* client, MQTTSNPacket* packet)
         topicid.data.long_.name = topicName.lenstring.data;
 
         id = client->getTopics()->add(&topicid)->getTopicId();
+
+        MQTTSNPacket* regAck = new MQTTSNPacket();
         regAck->setREGACK(id, msgId, MQTTSN_RC_ACCEPTED);
         Event* ev = new Event();
         ev->setClientSendEvent(client, regAck);
         _gateway->getClientSendQue()->post(ev);
+    }
+}
+
+void MQTTSNPublishHandler::handleRegAck(Client* client, MQTTSNPacket* packet)
+{
+    uint16_t id;
+    uint16_t msgId;
+    uint8_t rc;
+    if (client->isActive() || client->isAwake())
+    {
+        if (packet->getREGACK(&id, &msgId, &rc) == 0)
+        {
+            return;
+        }
+
+        /* get PUBLISH message */
+        MQTTSNPacket* regAck = client->getWaitREGACKPacketList()->getPacket(msgId);
+
+        if (regAck != nullptr)
+        {
+            client->getWaitREGACKPacketList()->erase(msgId);
+            Event* ev = new Event();
+            ev->setClientSendEvent(client, regAck);
+            _gateway->getClientSendQue()->post(ev);
+        }
+
+        if (client->isHoldPingReqest() && client->getWaitREGACKPacketList()->getCount() == 0)
+        {
+            /* send PINGREQ to the broker */
+            client->resetPingRequest();
+            MQTTGWPacket* pingreq = new MQTTGWPacket();
+            pingreq->setHeader(PINGREQ);
+            Event* evt = new Event();
+            evt->setBrokerSendEvent(client, pingreq);
+            _gateway->getBrokerSendQue()->post(evt);
+        }
+    }
 
 }
+
+void MQTTSNPublishHandler::handleAggregatePublish(Client* client, MQTTSNPacket* packet)
+{
+    int msgId = 0;
+    MQTTGWPacket* publish = handlePublish(client, packet);
+    if (publish != nullptr)
+    {
+        if (publish->getMsgId() > 0)
+        {
+            if (packet->isDuplicate())
+            {
+                msgId = _gateway->getAdapterManager()->getAggregater()->getMsgId(client, packet->getMsgId());
+            }
+            else
+            {
+                msgId = _gateway->getAdapterManager()->getAggregater()->addMessageIdTable(client, packet->getMsgId());
+            }
+            publish->setMsgId(msgId);
+        }
+        Event* ev1 = new Event();
+        ev1->setBrokerSendEvent(client, publish);
+        _gateway->getBrokerSendQue()->post(ev1);
+    }
+}
+
+void MQTTSNPublishHandler::handleAggregateAck(Client* client, MQTTSNPacket* packet, int type)
+{
+    if (type == MQTTSN_PUBREC)
+    {
+        uint16_t msgId;
+
+        if (packet->getACK(&msgId) == 0)
+        {
+            return;
+        }
+        MQTTSNPacket* ackPacket = new MQTTSNPacket();
+        ackPacket->setPUBREL(msgId);
+        Event* ev = new Event();
+        ev->setClientSendEvent(client, ackPacket);
+        _gateway->getClientSendQue()->post(ev);
+    }
+}