Bugfix of DTLS6 #241

This fix doesn't work on OpenSSL version 1.1.1 11.sep 2018 Upgrade to version 1.1.1k 25 Mar. 2021 Signed-off-by: tomoaki <tomoaki@tomy-tech.com>
2025-12-12 15:16:50 +01:00 · 2021-08-05 13:18:01 +09:00
parent a136f50c75
commit 4dcfa98303
19 changed files with 285 additions and 180 deletions
--- a/.cproject
+++ b/.cproject
@@ -38,7 +38,6 @@
 								<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>
@@ -81,7 +80,8 @@
 						</toolChain>
 					</folderInfo>
 					<sourceEntries>
-						<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="GatewayTester|src/linux/udp6|src/linux/udp|src/linux/dtls/SensorNetSubTask.cpp|GatewayTester/samples/ClientSub|GatewayTester/samples/ClientPubQoS-1|GatewayTester/samples/ClientPub|src/linux/rfcomm|src/tests|src/linux/xbee|src/mainLogmonitor.cpp|src/linux/loralink" flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="MQTTSNGateway"/>
+						<entry excluding="mainTest.cpp|ClientSub|ClientPubQoS-1|ClientPub" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="MQTTSNGateway/GatewayTester/samples"/>
 						<entry excluding="samples|test" flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="MQTTSNPacket"/>
 					</sourceEntries>
 				</configuration>
@@ -153,7 +153,8 @@
 					</folderInfo>
 					<sourceEntries>
 						<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 excluding="mainTest.cpp|ClientSub|ClientPubQoS-1|ClientPub" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="MQTTSNGateway/GatewayTester/samples"/>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="MQTTSNGateway/src"/>
 						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="MQTTSNPacket/src"/>
 					</sourceEntries>
 				</configuration>
--- a/MQTTSNGateway/GatewayTester/Makefile
+++ b/MQTTSNGateway/GatewayTester/Makefile
@@ -40,14 +40,14 @@ $(SUBDIR)/Util.cpp \
 CXX := g++ 
 CPPFLAGS += 

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

 PROG := $(OUTDIR)/$(PROGTEST)
--- a/MQTTSNGateway/GatewayTester/build.sh
+++ b/MQTTSNGateway/GatewayTester/build.sh
@@ -8,6 +8,7 @@ if [ $1 == "udp" ] ; then
 elif [ $1 == "udp6" ] ; then 
    make SN=UDP6 $DEF1 $DEF2
 elif [ $1 == "rfcomm" ] ; then 
+	export LDADDBLT=-lbluetooth
    make SN=RFCOMM $DEF1 $DEF2
 elif [ $1 == "dtls" ] ; then
    make SN=DTLS $DEF1 $DEF2
@@ -16,7 +17,7 @@ elif [ $1 == "dtls6" ] ; then
 elif [ $1 == "clean" ] ; then
    make clean
 else
-    echo "Usage: build.sh  [ udp | udp6 | rfcomm | dtls | dtls6] | clean"
+    echo "Usage: build.sh  [ udp | udp6 | rfcomm | dtls | dtls6 | clean]"
 fi


--- a/MQTTSNGateway/GatewayTester/samples/mainTest.cpp
+++ b/MQTTSNGateway/GatewayTester/samples/mainTest.cpp
@@ -52,7 +52,7 @@ extern LScreen* theScreen;
 *    UDP,DTLS Configuration    (theNetcon)
 *------------------------------------------------------*/
 UDPCONF = { "GatewayTestClient",  // ClientId
-		{ 225, 1, 1, 1 },         // Multicast group IP
+        { 225, 1, 1, 1 },         // Multicast group IP
        1883,                     // Multicast group Port
        20020,                    // Local PortNo
        };
--- a/MQTTSNGateway/GatewayTester/src/LGwProxy.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LGwProxy.cpp
@@ -207,12 +207,12 @@ int LGwProxy::getConnectResponce(void)
            if (_network.sslConnect() > 0)
            {
                _status = GW_CONNECTING;
-                DISPLAY("\033[0m\033[0;32m\n\nLGwProxy::getConnectResponce SSL connection established.\033[0m\033[0;37m\n\n");
+                DISPLAY("\033[0m\033[0;32m\n\nLGwProxy::getConnectResponce DTLS 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");
+                DISPLAY("\033[0m\033[0;32m\n\nLGwProxy::getConnectResponce DTLS connection failed.\033[0m\033[0;37m\n\n");
            }
        }
 #else
--- a/MQTTSNGateway/GatewayTester/src/LMqttsnClientApp.h
+++ b/MQTTSNGateway/GatewayTester/src/LMqttsnClientApp.h
@@ -252,11 +252,11 @@ typedef enum
 /*=================================
 *    Starting prompt
 ==================================*/
-#define TESTER_VERSION " * Version: 2.0.0"
+#define TESTER_VERSION " * Version: 2.1.0"

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

--- a/MQTTSNGateway/GatewayTester/src/LNetworkDtls.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LNetworkDtls.cpp
@@ -224,7 +224,6 @@ bool LDtlsPort::open(LUdpConfig *config)
    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;
    }
@@ -236,7 +235,6 @@ bool LDtlsPort::open(LUdpConfig *config)
    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;
    }
@@ -256,7 +254,6 @@ int LDtlsPort::unicast(const uint8_t *buf, uint32_t length)
    {
        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
@@ -386,7 +383,7 @@ bool LDtlsPort::checkRecvBuf()
            return true;
        }
    }
-    _castStat = 0;
+    _castStat = STAT_NONE;
    return false;
 }

--- a/MQTTSNGateway/GatewayTester/src/LNetworkDtls.h
+++ b/MQTTSNGateway/GatewayTester/src/LNetworkDtls.h
@@ -36,13 +36,14 @@
 #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 {
+#define STAT_NONE   0
+#define STAT_UNICAST   1
+#define STAT_MULTICAST 2
+#define STAT_SSL 3
 /*========================================
 Class LDtlsPort
 =======================================*/
--- a/MQTTSNGateway/GatewayTester/src/LNetworkDtls6.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LNetworkDtls6.cpp
@@ -75,8 +75,8 @@ uint8_t* LNetwork::getMessage(int *len)
    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))
+        int addrFlg = memcmp(_ipAddress.s6_addr, _gwIpAddress.s6_addr, sizeof(_gwIpAddress.s6_addr));
+        if (isUnicast() && addrFlg && (_portNo != _gwPortNo))
        {
            return 0;
        }
@@ -141,11 +141,14 @@ int LNetwork::sslConnect(void)
 LDtls6Port::LDtls6Port()
 {
    _disconReq = false;
-    memset(_pollfds, 0, sizeof(_pollfds));
-    _sock = 0;
-    _castStat = 0;
+    _castStat = STAT_NONE;
    _ifIndex = 0;
-    _gIpAddrStr = NULL;
+    _gIpAddrStr = nullptr;
+    _sockfdMcast = 0;
+    _sockfdSsl = 0;
+    _ctx = nullptr;
+    _ssl = nullptr;
+    _gPortNo = _uPortNo = 0;
 }

 LDtls6Port::~LDtls6Port()
@@ -159,12 +162,14 @@ LDtls6Port::~LDtls6Port()

 void LDtls6Port::close()
 {
-    for (int i = 0; i < 2; i++)
+    if (_sockfdMcast > 0)
    {
-        if (_pollfds[i].fd > 0)
+        ::close(_sockfdMcast);
+        _sockfdMcast = 0;
+        if (_sockfdSsl > 0)
        {
-            ::close(_pollfds[i].fd);
-            _pollfds[i].fd = 0;
+            ::close(_sockfdSsl);
+            _sockfdSsl = 0;
        }
    }
 }
@@ -172,7 +177,6 @@ void LDtls6Port::close()
 bool LDtls6Port::open(LUdp6Config *config)
 {
    int optval = 1;
-    int sock = 0;
    sockaddr_in6 addr6;
    char errmsg[256];

@@ -205,22 +209,22 @@ bool LDtls6Port::open(LUdp6Config *config)
    }

    /* create a multicast socket */
-    sock = socket(AF_INET6, SOCK_DGRAM, 0);
-    if (sock < 0)
+    _sockfdMcast = socket(AF_INET6, SOCK_DGRAM, 0);
+    if (_sockfdMcast < 0)
    {
        return false;
    }

    optval = 1;
-    setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
-    setsockopt(sock, IPPROTO_IPV6, IPV6_V6ONLY, &optval, sizeof(optval));
+    setsockopt(_sockfdMcast, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
+    setsockopt(_sockfdMcast, IPPROTO_IPV6, IPV6_V6ONLY, &optval, sizeof(optval));

    memset(&addr6, 0, sizeof(addr6));
    addr6.sin6_family = AF_INET6;
    addr6.sin6_port = _gPortNo;
    addr6.sin6_addr = in6addr_any;

-    if (::bind(sock, (sockaddr*) &addr6, sizeof(addr6)) < 0)
+    if (::bind(_sockfdMcast, (sockaddr*) &addr6, sizeof(addr6)) < 0)
    {
        D_NWLOG("\033[0m\033[0;31merror %s ::bind() in Udp6Port::open\033[0m\033[0;37m\n", strerror(errno));
        return false;
@@ -229,21 +233,21 @@ bool LDtls6Port::open(LUdp6Config *config)
    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)
+    if (setsockopt(_sockfdMcast, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, &addrm, sizeof(addrm)) < 0)
    {
        D_NWLOG("\033[0m\033[0;31merror %s IPV6_ADD_MEMBERSHIP in Udp6Port::open\033[0m\033[0;37m\n", strerror(errno));
        close();
        return false;
    }
-    optval = 0;
-    if (setsockopt(sock, IPPROTO_IPV6, IPV6_MULTICAST_LOOP, &optval, sizeof(optval)) < 0)
+
+    optval = 1;
+    if (setsockopt(_sockfdMcast, IPPROTO_IPV6, IPV6_MULTICAST_LOOP, &optval, sizeof(optval)) < 0)
    {
        D_NWLOG("\033[0m\033[0;31merror %s IPV6_MULTICAST_LOOP in Udp6Port::open\033[0m\033[0;37m\n", strerror(errno));
        close();
        return false;
    }
-    _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));
@@ -253,7 +257,7 @@ bool LDtls6Port::open(LUdp6Config *config)

 bool LDtls6Port::isUnicast()
 {
-    return (_sock == _pollfds[0].fd && _sock > 0);
+    return (_castStat == STAT_UNICAST);
 }

 int LDtls6Port::unicast(const uint8_t *buf, uint32_t length)
@@ -267,11 +271,13 @@ int LDtls6Port::unicast(const uint8_t *buf, uint32_t length)
    }
    else
    {
-        D_NWLOG("sendto gateway via DTLS ");
+        D_NWLOG("sendto gateway via DTLS6 ");
        for (uint16_t i = 0; i < length; i++)
        {
            D_NWLOG(" %02x", *(buf + i));
-        }D_NWLOG("\n");
+        }
+
+        D_NWLOG("\n");

        if (!theClientMode)
        {
@@ -299,12 +305,12 @@ int LDtls6Port::multicast(const uint8_t *buf, uint32_t length)
 {
    char sbuf[SCREEN_BUFF_SIZE];
    char portStr[8];
-    sprintf(portStr, "%d", ntohs(_gPortNo));
+    sprintf(portStr, "%d", ntohs(_gIpAddr.sin6_port));

-    int status = ::sendto(_pollfds[1].fd, buf, length, 0, (sockaddr*) &_gIpAddr, sizeof(_gIpAddr));
+    int status = ::sendto(_sockfdMcast, 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));
+        DISPLAY("\033[0m\033[0;31merrno == %d in LDtls6Port::multicast\033[0m\033[0;37m\n", errno);
        return errno;
    }
    else
@@ -313,7 +319,9 @@ int LDtls6Port::multicast(const uint8_t *buf, uint32_t length)
        for (uint16_t i = 0; i < length; i++)
        {
            D_NWLOG(" %02x", *(buf + i));
-        }D_NWLOG("\n");
+        }
+
+        D_NWLOG("\n");

        if (!theClientMode)
        {
@@ -339,32 +347,52 @@ int LDtls6Port::multicast(const uint8_t *buf, uint32_t length)

 bool LDtls6Port::checkRecvBuf()
 {
+    timeval timeout;
+    timeout.tv_sec = 0;
+    timeout.tv_usec = 50000;    // 50 msec
+
    uint8_t buf[2];
+    fd_set recvfds;
+    int maxSock = 0;

-    int cnt = poll(_pollfds, 2, 2000);  // Timeout 2secs
-    if (cnt == 0)
+    FD_ZERO(&recvfds);
+    if (_sockfdMcast)
    {
-        return false;
+        FD_SET(_sockfdMcast, &recvfds);
+    }
+    if (_sockfdSsl)
+    {
+        FD_SET(_sockfdSsl, &recvfds);
    }

-    if (_pollfds[0].revents & POLLIN)
+    if (_sockfdMcast > _sockfdSsl)
    {
-        if (::recv(_pollfds[0].fd, buf, 1, MSG_DONTWAIT | MSG_PEEK) > 0)
-        {
-            _castStat = STAT_SSL;
-            _sock = _pollfds[0].fd;
-            return true;
-        }
+        maxSock = _sockfdMcast;
    }
-    else if (_pollfds[1].revents & POLLIN)
+    else
    {
-        if (::recv(_pollfds[1].fd, buf, 1, MSG_DONTWAIT | MSG_PEEK) > 0)
+        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;
-            _sock = _pollfds[1].fd;
            return true;
        }
    }
+    else if (FD_ISSET(_sockfdSsl, &recvfds))
+    {
+        if (::recv(_sockfdSsl, buf, 1, MSG_DONTWAIT | MSG_PEEK) > 0)
+        {
+            _castStat = STAT_SSL;
+            return true;
+        }
+    }
+    _castStat = STAT_NONE;
    return false;
 }

@@ -393,7 +421,7 @@ int LDtls6Port::recvfrom(uint8_t *buf, uint16_t length, int flags, in6_addr *ipA
    else if (_castStat == STAT_MULTICAST)
    {
        D_NWLOG("Mcast ");
-        status = ::recvfrom(_sock, buf, length, flags, (sockaddr*) &sender, &addrlen);
+        status = ::recvfrom(_sockfdMcast, buf, length, flags, (sockaddr*) &sender, &addrlen);
    }
    else
    {
@@ -445,63 +473,82 @@ int LDtls6Port::recvfrom(uint8_t *buf, uint16_t length, int flags, in6_addr *ipA
    return status;
 }

-int LDtls6Port::sslConnect(in6_addr ipAddress, uint16_t portNo)
+int LDtls6Port::sslConnect(in6_addr ipAddress, in_port_t portNo)
 {
    int optval = 1;
-    int sock = _pollfds[0].fd;

    if (_ssl != 0)
    {
+        D_NWLOG("LDtls6Port::sslConnect SSL exists.\n");
+        SSL_shutdown(_ssl);
        SSL_free(_ssl);
+        _sockfdSsl = 0;
+        _ssl = 0;
    }

-    if (sock > 0)
+    if (_sockfdSsl > 0)
    {
-        ::close(sock);
+        D_NWLOG("LDtls6Port::sslConnect socket exists.\n");
+        ::close(_sockfdSsl);
    }

-    sock = socket(AF_INET6, SOCK_DGRAM, 0);
-    if (sock <= 0)
+    _sockfdSsl = socket(AF_INET6, SOCK_DGRAM, 0);
+    if (_sockfdSsl <= 0)
    {
+        D_NWLOG("LDtls6Port::sslConnect Can't create a socket\n");
        return -1;
    }
    optval = 1;
-    setsockopt(sock, IPPROTO_IPV6, IPV6_V6ONLY, &optval, sizeof(optval));
-    setsockopt(sock, SOL_SOCKET, SO_REUSEADDR || SO_REUSEPORT, &optval, sizeof(optval));
+    setsockopt(_sockfdSsl, IPPROTO_IPV6, IPV6_V6ONLY, &optval, sizeof(optval));
+    setsockopt(_sockfdSsl, SOL_SOCKET, SO_REUSEADDR || SO_REUSEPORT, &optval, sizeof(optval));

    if (_ifIndex > 0)
    {
 #ifdef __APPLE__
-        setsockopt(sock, IPPROTO_IP, IP_BOUND_IF, &_ifIndex, sizeof(_ifIndex));
+     setsockopt(_sockfdSsl, IPPROTO_IP, IP_BOUND_IF, &_ifIndex, sizeof(_ifIndex));
 #else
-        setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, _interfaceName.c_str(), _interfaceName.size());
+        setsockopt(_sockfdSsl, SOL_SOCKET, SO_BINDTODEVICE, _interfaceName.c_str(), _interfaceName.size());
 #endif
    }

-    struct sockaddr_in6 addr;
+    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)
+    if (::bind(_sockfdSsl, (struct sockaddr*) &addr, sizeof(addr)) < 0)
    {
+        ::close(_sockfdSsl);
+        D_NWLOG("LDtlsPort::sslConnect Can't bind a socket\n");
        return -1;
    }
-    _pollfds[0].fd = sock;
-    _pollfds[0].events = POLLIN;

-    uint16_t listenPort = htons(portNo);
-    struct sockaddr_in6 dest;
+    // Destination is a gateway address and portNo
+    int rc = 0;
+    sockaddr_in6 dest;
    dest.sin6_family = AF_INET6;
-    dest.sin6_port = htons(listenPort);
+    dest.sin6_port = portNo;
    memcpy(dest.sin6_addr.s6_addr, (const void*) ipAddress.s6_addr, sizeof(ipAddress.s6_addr));

-    int rc = 0;
+    BIO *cbio = BIO_new_dgram(_sockfdSsl, BIO_NOCLOSE);
+    if (connect(_sockfdSsl, (sockaddr*) &dest, sizeof(sockaddr_in6)) < 0)
+    {
+        D_NWLOG("socket can't connect %s\n",strerror(errno));
+        return -1;
+    }
+
+    if (BIO_ctrl(cbio, BIO_CTRL_DGRAM_SET_CONNECTED, 0, &dest) <0)
+    {
+        D_NWLOG("BIO_ctrl %s\n",strerror(errno));
+        return -1;
+    }

-    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);
+    if (_ssl == nullptr)
+    {
+        D_NWLOG("SSL_new  %s\n",strerror(errno));
+       return -1;
+    }
    SSL_set_bio(_ssl, cbio, cbio);

 #ifdef DEBUG_NW
@@ -510,7 +557,12 @@ int LDtls6Port::sslConnect(in6_addr ipAddress, uint16_t portNo)
    D_NWLOG("connect to %-15s:%-6u\n", addrBuf, ntohs(dest.sin6_port));
 #endif

+    timeval timeout;
+    timeout.tv_sec = 5;
+    timeout.tv_usec = 0;
+    BIO_ctrl(cbio, BIO_CTRL_DGRAM_SET_RECV_TIMEOUT, 0, &timeout);
    errno = 0;
+
    int stat = SSL_connect(_ssl);
    if (stat != 1)
    {
@@ -519,6 +571,7 @@ int LDtls6Port::sslConnect(in6_addr ipAddress, uint16_t portNo)
    }
    else
    {
+        rc = 1;
        D_NWLOG("SSL connected\n");
    }
    return rc;
--- a/MQTTSNGateway/GatewayTester/src/LNetworkDtls6.h
+++ b/MQTTSNGateway/GatewayTester/src/LNetworkDtls6.h
@@ -37,6 +37,7 @@
 #define SOCKET_MAXRECV  500
 #define SOCKET_MAXBUFFER_LENGTH 500 // buffer size

+#define STAT_NONE   0
 #define STAT_UNICAST   1
 #define STAT_MULTICAST 2
 #define STAT_SSL 3
@@ -68,9 +69,10 @@ private:
    void close();
    int recvfrom ( uint8_t* buf, uint16_t len, int flags, in6_addr* ipaddress, in_port_t* port );

+    int _sockfdMcast;
+    int _sockfdSsl;
    SSL_CTX *_ctx;
    SSL *_ssl;
-    pollfd _pollfds[2];
    in_port_t _gPortNo;
    in_port_t _uPortNo;
    sockaddr_in6 _gIpAddr;
@@ -78,7 +80,6 @@ private:
    uint32_t _ifIndex;
    string _interfaceName;
    uint8_t  _castStat;
-    int _sock;
    bool   _disconReq;

 };
--- a/MQTTSNGateway/GatewayTester/src/LNetworkUdp.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LNetworkUdp.cpp
@@ -197,6 +197,7 @@ bool LUdpPort::open(LUdpConfig *config)

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

@@ -216,6 +217,7 @@ bool LUdpPort::open(LUdpConfig *config)

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

--- a/MQTTSNGateway/GatewayTester/src/LNetworkUdp6.cpp
+++ b/MQTTSNGateway/GatewayTester/src/LNetworkUdp6.cpp
@@ -350,7 +350,7 @@ bool LUdp6Port::checkRecvBuf()
 {
    uint8_t buf[2];

-    int cnt = poll(_pollfds, 2, 2000);  // Timeout 2secs
+    int cnt = poll(_pollfds, 2, 50);  // Timeout 50m secs
    if (cnt == 0)
    {
        return false;
--- a/MQTTSNGateway/clients.conf
+++ b/MQTTSNGateway/clients.conf
@@ -15,10 +15,10 @@
 #     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.
+#     if the SensorNetwork is not stable, specify "unstableLine".
+#     if Broker's Connection is TLS, specify "secureConnection".
+#     if the client is a forwarder,specify "forwarder".
+#     if the client send PUBLISH QoS-1, specify "QoS-1".
 #
 # Ex:
 #     #Client List
@@ -31,6 +31,17 @@
 #
 # SensorNetwork address format is defined by SensorNetAddress::setAddress(string* data) function.
 #
+# UDP6 (IPv6 UDP) [IPv6 address]:PortNo
+# RFCOMM          Device_address.channel (1-30)
+# XBee            FFFFFFFFFFFFFFFF　8bytes Hex
+# LoRaLink        1-254 
+#
+#
+# This is a sample of UDP. 
+#
+# REWRITE ALL ACCORDING TO YOUR CLIENTS.
+#
+
 GatewayTester, 172.16.1.11:20020
 ClientPUB,172.16.1.11:2010
 Client01,172.16.1.11:12001
--- a/MQTTSNGateway/src/CMakeLists.txt
+++ b/MQTTSNGateway/src/CMakeLists.txt
@@ -81,7 +81,7 @@ ADD_LIBRARY(mqtt-sngateway_common
 link_directories("/usr/local/lib")

 # Mac
-link_directories("/usr/local/opt/openssl")
+link_directories("/usr/local/opt/openssl/lib")


 TARGET_INCLUDE_DIRECTORIES(mqtt-sngateway_common
@@ -91,6 +91,7 @@ TARGET_INCLUDE_DIRECTORIES(mqtt-sngateway_common
       ${OS}/${SENSORNET}
       ../../MQTTSNPacket/src
       /usr/local/include
+       /usr/local/opt/openssl/include
       )

 IF(SENSORNET MATCHES "rfcomm")
--- a/MQTTSNGateway/src/MQTTSNGWClientRecvTask.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWClientRecvTask.cpp
@@ -98,11 +98,11 @@ void ClientRecvTask::run()
            continue;
        }

-        SensorNetAddress* senderAddr = _gateway->getSensorNetwork()->getSenderAddress();
+        SensorNetAddress senderAddr = *_gateway->getSensorNetwork()->getSenderAddress();

        if (packet->getType() == MQTTSN_ENCAPSULATED)
        {
-            fwd = _gateway->getAdapterManager()->getForwarderList()->getForwarder(senderAddr);
+            fwd = _gateway->getAdapterManager()->getForwarderList()->getForwarder(&senderAddr);

            if (fwd != nullptr)
            {
@@ -124,7 +124,7 @@ void ClientRecvTask::run()

            if (qosm1Proxy->isActive())
            {
-                const char* clientName = qosm1Proxy->getClientId(senderAddr);
+                const char *clientName = qosm1Proxy->getClientId(&senderAddr);

                if (clientName != nullptr)
                {
@@ -134,7 +134,7 @@ void ClientRecvTask::run()
                    {
                        log(clientName, packet);
                        WRITELOG("%s %s  %s can send only PUBLISH with QoS-1.%s\n",
-                        ERRMSG_HEADER, clientName, senderAddr->sprint(buf), ERRMSG_FOOTER);
+                        ERRMSG_HEADER, clientName, senderAddr.sprint(buf), ERRMSG_FOOTER);
                        delete packet;
                        continue;
                    }
@@ -143,7 +143,7 @@ void ClientRecvTask::run()

            if (client == nullptr)
            {
-                client = _gateway->getClientList()->getClient(senderAddr);
+                client = _gateway->getClientList()->getClient(&senderAddr);
            }
        }

@@ -183,7 +183,7 @@ void ClientRecvTask::run()
                {
                    log(0, packet, &data.clientID);
                    WRITELOG("%s CONNECT message form %s is incorrect.%s\n",
-                    ERRMSG_HEADER, senderAddr->sprint(buf),
+                    ERRMSG_HEADER, senderAddr.sprint(buf),
                    ERRMSG_FOOTER);
                    delete packet;
                    continue;
@@ -208,13 +208,13 @@ void ClientRecvTask::run()
                        /* Authentication is not required */
                        if (_gateway->getGWParams()->clientAuthentication == false)
                        {
-                            client->setClientAddress(senderAddr);
+                            client->setClientAddress(&senderAddr);
                        }
                    }
                    else
                    {
                        /* create a new client */
-                        client = clientList->createClient(senderAddr, &data.clientID, clientType);
+                        client = clientList->createClient(&senderAddr, &data.clientID, clientType);
                    }
                }

@@ -223,7 +223,7 @@ void ClientRecvTask::run()
                if (client == nullptr)
                {
                    WRITELOG("%s Client(%s) was rejected. CONNECT message has been discarded.%s\n",
-                    ERRMSG_HEADER, senderAddr->sprint(buf),
+                    ERRMSG_HEADER, senderAddr.sprint(buf),
                    ERRMSG_FOOTER);
                    delete packet;
                    continue;
@@ -247,7 +247,7 @@ void ClientRecvTask::run()
                else
                {
                    WRITELOG("%s MQTTSNGWClientRecvTask  Client(%s) is not connecting. message has been discarded.%s\n",
-                    ERRMSG_HEADER, senderAddr->sprint(buf),
+                    ERRMSG_HEADER, senderAddr.sprint(buf),
                    ERRMSG_FOOTER);
                }
                delete packet;
--- a/MQTTSNGateway/src/MQTTSNGateway.h
+++ b/MQTTSNGateway/src/MQTTSNGateway.h
@@ -27,9 +27,9 @@ namespace MQTTSNGW
 /*=================================
 *    Starting prompt
 ==================================*/
-#define PAHO_COPYRIGHT0 " * "
+#define PAHO_COPYRIGHT0 " * MQTT-SN Gateway"
 #define PAHO_COPYRIGHT1 " * Part of Project Paho in Eclipse"
-#define PAHO_COPYRIGHT2 " * (http://git.eclipse.org/c/paho/org.eclipse.paho.mqtt-sn.embedded-c.git/)"
+#define PAHO_COPYRIGHT2 " * (https://github.com/eclipse/paho.mqtt-sn.embedded-c.git)"
 #define PAHO_COPYRIGHT3 " * Author : Tomoaki YAMAGUCHI"
 #define PAHO_COPYRIGHT4 " ***************************************************************************"
 /*==========================================================
--- a/MQTTSNGateway/src/linux/dtls/SensorNetwork.cpp
+++ b/MQTTSNGateway/src/linux/dtls/SensorNetwork.cpp
@@ -61,6 +61,7 @@ unsigned char cookie_secret[COOKIE_SECRET_LENGTH];
 SensorNetAddress::SensorNetAddress()
 {
    _portNo = 0;
+    _pfdsIndex = 0;
    memset(&_ipAddr, 0, sizeof(_ipAddr));
 }

@@ -223,7 +224,6 @@ void SensorNetAddress::setSockaddr4(sockaddr_in *sockaddr)
    _ipAddr.af = sockaddr->sin_family;
    _portNo = sockaddr->sin_port;
    memcpy((void*) &_ipAddr.addr.ad4, (void*) &sockaddr->sin_addr, sizeof(_ipAddr.addr.ad4));
-    this->_pfdsIndex = 0;
 }

 void SensorNetAddress::setSockaddr6(sockaddr_in6 *sockaddr)
@@ -231,7 +231,6 @@ void SensorNetAddress::setSockaddr6(sockaddr_in6 *sockaddr)
    _ipAddr.af = sockaddr->sin6_family;
    _portNo = sockaddr->sin6_port;
    memcpy((void*) &_ipAddr.addr.ad6, (void*) &sockaddr->sin6_addr, sizeof(_ipAddr.addr.ad6));
-    this->_pfdsIndex = 0;
 }

 void SensorNetAddress::cpyAddr4(sockaddr_in *sockaddr)
@@ -244,8 +243,15 @@ void SensorNetAddress::cpyAddr4(sockaddr_in *sockaddr)
 void SensorNetAddress::cpyAddr6(sockaddr_in6 *sockaddr)
 {
    sockaddr->sin6_family = _ipAddr.af;
-    memcpy((void*) &sockaddr->sin6_addr, (void*) &_ipAddr.addr.ad6, sizeof(_ipAddr.addr.ad6));
    sockaddr->sin6_port = _portNo;
+    memcpy((void*) &sockaddr->sin6_addr, (void*) &_ipAddr.addr.ad6, sizeof(_ipAddr.addr.ad6));
+}
+
+void SensorNetAddress::cpyAddr(SensorNetAddress *addr)
+{
+    addr->_portNo = _portNo;
+    memcpy((void*) &addr->_ipAddr, (const void*) &_ipAddr, sizeof(_ipAddr));
+    addr->_pfdsIndex = _pfdsIndex;
 }

 char* SensorNetAddress::sprint(char *buf)
@@ -269,6 +275,7 @@ char* SensorNetAddress::sprint(char *buf)
        return buf;
    }
    sprintf(buf + strlen(buf), "%d", ntohs(_portNo));
+    sprintf(buf + strlen(buf), " index=%d", _pfdsIndex);
    return buf;
 }

@@ -392,6 +399,7 @@ int Connections::getSockClient(int index)

 void Connections::close(int index)
 {
+    D_NWSTACK("Connection %d closed\n", index);
    int idx = index + POLL_SSL;
    _mutex.lock();
    int sock = _pollfds[idx].fd;
@@ -435,6 +443,7 @@ int Connections::addClientSSL(SSL *ssl, int sock)
    int rc = _numfds - POLL_SSL;
    _numfds++;
    _mutex.unlock();
+    D_NWSTACK("Add client connection index=%d, ssl=%ld, sock=%d\n", rc, (long int )ssl, sock);
    return rc;
 }

@@ -453,6 +462,14 @@ SSL* Connections::getClientSSL(int index)
    return _ssls[index + POLL_SSL];
 }

+void Connections::print(void)
+{
+    for (int i = 0; i < _numfds; i++)
+    {
+        printf("index=%d  fd=%d   ssl=%ld \n", i, _pollfds[i].fd, (long int) _ssls[i]);
+    }
+}
+
 /*================================================================
 Class  SensorNetwork

@@ -468,6 +485,8 @@ SSL* Connections::getClientSSL(int index)
 #define DTLS_APPL         23
 #define DTLS_OTHERS       100

+#define DTLS_TIMEOUT      4
+
 /* Certificate verification. Returns 1 if trusted, else 0 */
 int verify_cert(int ok, X509_STORE_CTX *ctx);

@@ -479,9 +498,6 @@ int verify_cookie(SSL *ssl, const unsigned char *cookie, unsigned int cookie_len

 SensorNetwork::SensorNetwork()
 {
-    _senderAddr = new SensorNetAddress();
-    _multicastAddr = new SensorNetAddress();
-    _unicastAddr = new SensorNetAddress();
    _conns = new Connections();
    _dtlsctx = nullptr;
    _af = 0;
@@ -493,24 +509,18 @@ SensorNetwork::~SensorNetwork()
    {
        delete _conns;
    }
-    if (_senderAddr != nullptr)
-    {
-        delete _senderAddr;
-    }
-    if (_multicastAddr != nullptr)
-    {
-        delete _multicastAddr;
-    }
-    if (_unicastAddr != nullptr)
-    {
-        delete _unicastAddr;
-    }
 }

 int SensorNetwork::unicast(const uint8_t *payload, uint16_t payloadLength, SensorNetAddress *sendToAddr)
 {
-    _mutex.lock();
+#ifdef DEBUG_NW
+    char buf[256];
+    _conns->print();
+    sendToAddr->sprint(buf);
+    D_NWSTACK("sendto %s\n", buf);
+#endif

+    _mutex.lock();
    SSL *ssl = _conns->getClientSSL(sendToAddr->getIndex());
    int len = SSL_write(ssl, payload, payloadLength);
    int rc = SSL_get_error(ssl, len);
@@ -530,7 +540,7 @@ int SensorNetwork::broadcast(const uint8_t *payload, uint16_t payloadLength)
    int status;
 #ifndef DTLS6
    sockaddr_in dest;
-    _multicastAddr->cpyAddr4(&dest);
+    _multicastAddr.cpyAddr4(&dest);

    status = ::sendto(_conns->getSockUnicast(), payload, payloadLength, 0, (const sockaddr*) &dest, sizeof(dest));
    if (status < 0)
@@ -542,7 +552,7 @@ int SensorNetwork::broadcast(const uint8_t *payload, uint16_t payloadLength)

 #else
    sockaddr_in6 dest;
-    _multicastAddr->cpyAddr6(&dest);
+    _multicastAddr.cpyAddr6(&dest);

    status = ::sendto(_conns->getSockUnicast(), payload, payloadLength, 0, (const sockaddr*) &dest, sizeof(dest));
    if (status < 0)
@@ -577,7 +587,7 @@ int SensorNetwork::read(uint8_t *buf, uint16_t bufLen)
    client.setFamily(_af);

    // Check POLL_IN
-    int cnt = _conns->poll(2000);  // Timeout 2secs
+    int cnt = _conns->poll(6000);  // Timeout 6secs
    if (cnt == 0)
    {
        // Timeout
@@ -588,21 +598,21 @@ int SensorNetwork::read(uint8_t *buf, uint16_t bufLen)
        return -1;
    }

-
    _mutex.lock();

    //  Check Unicast Port

    if (_conns->getEventUnicast() & POLLIN)
    {
-        D_NWSTACK("Connect RECV\n");
+        D_NWSTACK("RECV Unicast SSL_connect\n");
+
        // SSL connection request from a client
-        optval = 1;
-
-        client.clear();
-        client.setFamily(_af);
-
+#ifdef DEBUG_NW
+        int dtls = getUnicastClient(&client);
+        D_NWSTACK("Packet type = %d\n", dtls);
+#else
        getUnicastClient(&client);
+#endif
        sockListen = _conns->getSockUnicast();

        // Listen Connection
@@ -615,7 +625,7 @@ int SensorNetwork::read(uint8_t *buf, uint16_t bufLen)
        int rc = 0;

        // SSL Listen
-        D_NWSTACK("Listen SSL\n");
+        D_NWSTACK("DTLSv1_listen\n");

        rc = DTLSv1_listen(ssl, (BIO_ADDR*) &client_addr);

@@ -631,6 +641,7 @@ int SensorNetwork::read(uint8_t *buf, uint16_t bufLen)
 #ifndef DTLS6
        // DTLS over IPv4
        int client_fd = socket(AF_INET, SOCK_DGRAM, 0);
+        optval = 1;
        setsockopt(client_fd, SOL_SOCKET, SO_REUSEADDR, (const void*) &optval, sizeof(optval));
        // Bind to Dtls PortNo
        bind(client_fd, (sockaddr*) &_serverAddr4, sizeof(sockaddr_in));
@@ -639,6 +650,7 @@ int SensorNetwork::read(uint8_t *buf, uint16_t bufLen)
 #else
        // DTLS over IPv6
        int client_fd = socket(AF_INET6, SOCK_DGRAM, 0);
+        optval = 1;
        setsockopt(client_fd, SOL_SOCKET, SO_REUSEADDR, (const void*) &optval, sizeof(optval));
        // Bind to Dtls PortNo
        bind(client_fd, (sockaddr*) &_serverAddr6, sizeof(sockaddr_in6));
@@ -650,12 +662,18 @@ int SensorNetwork::read(uint8_t *buf, uint16_t bufLen)
        BIO_set_fd(cbio, client_fd, BIO_NOCLOSE);
        BIO_ctrl(cbio, BIO_CTRL_DGRAM_SET_CONNECTED, 0, &client_addr);

+        // set timeout
+        timeval timeout;
+        timeout.tv_sec = DTLS_TIMEOUT;
+        timeout.tv_usec = 0;
+        BIO_ctrl(cbio, BIO_CTRL_DGRAM_SET_RECV_TIMEOUT, 0, &timeout);
+
        // Finish handshake
        int ret = SSL_accept(ssl);
        if (ret <= 0)
        {
            ERR_error_string_n(ERR_get_error(), errmsg, sizeof(errmsg));
-            WRITELOG("SSL_accept ret=%d %s\n", ret, errmsg);
+            WRITELOG("SSL_accept %s\n", errmsg);
            SSL_shutdown(ssl);
            SSL_free(ssl);
            ::close(client_fd);
@@ -667,11 +685,13 @@ int SensorNetwork::read(uint8_t *buf, uint16_t bufLen)

            // save SensorNetworkAddress of Client
            client.setIndex(index);
-            _senderAddr = &client;
+            client.cpyAddr(&_senderAddr);
+
 #ifdef DEBUG_NW
            char clientaddrBuf[128];
-            client.sprint(clientaddrBuf);
-            D_NWSTACK("Client %s SSL Accepted. idx=%d\n", clientaddrBuf, index);
+            _senderAddr.sprint(clientaddrBuf);
+            D_NWSTACK("DTLS accepted client is %s   index=%d client_fd=%d\n", clientaddrBuf, _senderAddr.getIndex(),
+                    client_fd);
 #endif
        }
        _mutex.unlock();
@@ -716,22 +736,23 @@ int SensorNetwork::read(uint8_t *buf, uint16_t bufLen)
                    // The packet is a MQTT-SN message
                    ssl = _conns->getClientSSL(i);
                    int len = SSL_read_ex(ssl, (void*) buf, (size_t) bufLen, &recvlen);
-                    if (SSL_get_error(ssl, len) >= 0)
-                    {
-#ifdef DEBUG_NW
-                        char clientaddrBuf[128];
-                        client.sprint(clientaddrBuf);
-                        D_NWSTACK("Client %s SSL Accepted. idx=%d\n", clientaddrBuf, i);
-#endif
-                        _senderAddr = &client;
-                        _senderAddr->setIndex(i);
-                    }
-                    else
+                    if (SSL_get_error(ssl, len) < 0)
                    {
                        D_NWSTACK("SSL RECV Error\n");
                        _conns->close(i);
                        recvlen = -1;
                    }
+                    else
+                    {
+                        client.cpyAddr(&_senderAddr);
+                        _senderAddr.setIndex(i);
+
+#ifdef DEBUG_NW
+                        char clientaddrBuf[128];
+                        _senderAddr.sprint(clientaddrBuf);
+                        D_NWSTACK("Client %s ssl=%ld Received. idx=%d\n", clientaddrBuf, (long int )ssl, i);
+#endif
+                    }
                    _mutex.unlock();
                    return recvlen;
                }
@@ -880,7 +901,7 @@ const char* SensorNetwork::getDescription(void)

 SensorNetAddress* SensorNetwork::getSenderAddress(void)
 {
-    return _senderAddr;
+    return &_senderAddr;
 }

 int SensorNetwork::openV4(string *ipAddress, uint16_t multiPortNo, uint16_t uniPortNo, uint32_t ttl)
@@ -966,9 +987,9 @@ int SensorNetwork::openV4(string *ipAddress, uint16_t multiPortNo, uint16_t uniP
        D_NWSTACK("error %d IP_MULTICAST_LOOP in UDP4_6Port::openV4 %s\n", errno, strerror(errno));
        return -1;
    }
-    _multicastAddr->setFamily(AF_INET);
-    _multicastAddr->setIpAddress(ipAddress);
-    _multicastAddr->setPort(multiPortNo);
+    _multicastAddr.setFamily(AF_INET);
+    _multicastAddr.setIpAddress(ipAddress);
+    _multicastAddr.setPort(multiPortNo);
    _conns->setSockMulticast(sock);
    return 0;
 }
@@ -987,10 +1008,10 @@ int SensorNetwork::openV6(string *ipAddress, string *interface, uint16_t multiPo
        return -1;
    }

-    _multicastAddr->setPort(multiPortNo);
-    _unicastAddr->setPort(uniPortNo);
+    _multicastAddr.setPort(multiPortNo);
+    _unicastAddr.setPort(uniPortNo);

-    if (_multicastAddr->setIpAddress(ipAddress) < 0)
+    if (_multicastAddr.setIpAddress(ipAddress) < 0)
    {
        D_NWSTACK("Incorrect IPV6 address in SensorNetwork::openV6 error %s\n", strerror(errno));
        return -1;
@@ -1073,12 +1094,12 @@ int SensorNetwork::openV6(string *ipAddress, string *interface, uint16_t multiPo
    }

    struct ipv6_mreq mreq;
-    mreq.ipv6mr_multiaddr = _multicastAddr->getIpAddress()->addr.ad6;
+    mreq.ipv6mr_multiaddr = _multicastAddr.getIpAddress()->addr.ad6;
    mreq.ipv6mr_interface = ifindex;

-    if (setsockopt(sock, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0)
+    if (setsockopt(sock, IPPROTO_IPV6, IPV6_JOIN_GROUP, &mreq, sizeof(mreq)) < 0)
    {
-        D_NWSTACK("Multicast IPV6_ADD_MEMBERSHIP in SensorNetwork::openV6 error %s\n", strerror(errno));
+        D_NWSTACK("Multicast IPV6_JOIN_GROUP in SensorNetwork::openV6 error %s\n", strerror(errno));
        return -1;
    }

@@ -1099,9 +1120,9 @@ int SensorNetwork::openV6(string *ipAddress, string *interface, uint16_t multiPo
        D_NWSTACK("Multicast IPV6_MULTICAST_HOPS in SensorNetwork::openV6 error %s\n", strerror(errno));
        return -1;
    }
-    _multicastAddr->setFamily(AF_INET6);
-    _multicastAddr->setIpAddress(ipAddress);
-    _multicastAddr->setPort(multiPortNo);
+    _multicastAddr.setFamily(AF_INET6);
+    _multicastAddr.setIpAddress(ipAddress);
+    _multicastAddr.setPort(multiPortNo);
    return 0;
 }

@@ -1169,14 +1190,14 @@ int SensorNetwork::getSenderAddress(int sock, SensorNetAddress *addr)

    if (len < 0 && errno != EAGAIN)
    {
-        D_NWSTACK("errno = %d in UDPPort::getSender\n", errno);
+        D_NWSTACK("errno = %d in SensorNetwork::getSenderAddress\n", errno);
        return -1;
    }
-    addr->setFamily(AF_INET);
-    addr->getIpAddress()->addr.ad4 = sender4.sin_addr;
-    addr->setPort(sender4.sin_port);
-    D_NWSTACK("SensorNetwork::getSenderAddress recved from %s:%d length = %d\n", inet_ntoa(sender4.sin_addr),
-            ntohs(sender4.sin_port), len);
+
+    addr->setSockaddr4(&sender4);
+
+    D_NWSTACK("SensorNetwork::getSenderAddress recved from %s:%d length = %d fd=%d\n", inet_ntoa(sender4.sin_addr),
+            ntohs(addr->getPort()), len, sock);

    if (len >= 13)
    {
@@ -1191,27 +1212,34 @@ int SensorNetwork::getSenderAddress(int sock, SensorNetAddress *addr)
    //AF_INET6
    sockaddr_in6 sender6 = { 0 };
    socklen_t addrlen6 = sizeof(sender6);
-    unsigned long int buf = 0;
-    len = ::recvfrom(sock, &buf, 1, MSG_PEEK, (sockaddr*) &sender6, &addrlen6);
+    char buf[16];
+    int rc = DTLS_OTHERS;
+
+    len = ::recvfrom(sock, &buf, 15, MSG_PEEK, (sockaddr*) &sender6, &addrlen6);

    if (len < 0 && errno != EAGAIN)
    {
-        D_NWSTACK("errno = %d in SensorNetwork::getSender\n", errno);
-        return len;
+        D_NWSTACK("errno = %d in SensorNetwork::getSenderAddress\n", errno);
+        return -1;
    }

-    addr->setFamily(AF_INET6);
    addr->setSockaddr6(&sender6);
-    addr->setPort(sender4.sin_port);

 #ifdef DEBUG_NW
    char senderstr[INET6_ADDRSTRLEN];
    inet_ntop(AF_INET6, &sender6.sin6_addr,senderstr,INET6_ADDRSTRLEN);
-    D_NWSTACK("recved from %s:%d length = %d\n",senderstr ,ntohs(sender6.sin6_port), len);
+    D_NWSTACK("recved from %s:%d length = %d fd=%d\n",senderstr ,ntohs(addr->getPort()), len, sock);
 #endif
 #endif

-    return len;
+    if (len >= 13)
+    {
+        if (buf[0] == DTLS_CLIENTHELLO || buf[0] == DTLS_APPL)
+        {
+            rc = buf[0];
+        }
+    }
+    return rc;
 }

 void SensorNetwork::clearRecvData(int sock)
@@ -1220,6 +1248,12 @@ void SensorNetwork::clearRecvData(int sock)
    ::recv(sock, buf, MQTTSNGW_MAX_PACKET_SIZE, 0);
 }

+Connections* SensorNetwork::getConnections(void)
+{
+    return _conns;
+}
+
+
 int verify_cert(int ok, X509_STORE_CTX *ctx)
 {
    return 1;
--- a/MQTTSNGateway/src/linux/dtls/SensorNetwork.h
+++ b/MQTTSNGateway/src/linux/dtls/SensorNetwork.h
@@ -57,6 +57,7 @@ public:
    void setSockaddr6(sockaddr_in6 *sockaddr);
    void cpyAddr4(sockaddr_in *sockaddr);
    void cpyAddr6(sockaddr_in6 *sockaddr);
+    void cpyAddr(SensorNetAddress *addr);
    in_port_t getPort(void);
    ipAddr_t* getIpAddress(void);
    void setIndex(int index);
@@ -109,6 +110,7 @@ public:
    int getEventUnicast(void);
    int getEventListen(void);
    void closeSSL(int index);
+    void print(void);
 private:
    pollfd *_pollfds;
    SSL **_ssls;
@@ -133,6 +135,7 @@ public:
    void initialize(void);
    const char* getDescription(void);
    SensorNetAddress* getSenderAddress(void);
+    Connections* getConnections(void);
    void close();

 private:
@@ -145,9 +148,9 @@ private:
    void clearRecvData(int sock);

    Mutex _mutex;
-    SensorNetAddress *_senderAddr;
-    SensorNetAddress *_multicastAddr;
-    SensorNetAddress *_unicastAddr;
+    SensorNetAddress _senderAddr;
+    SensorNetAddress _multicastAddr;
+    SensorNetAddress _unicastAddr;
    string _description;
    SSL_CTX *_dtlsctx;
    Connections *_conns;
--- a/MQTTSNGateway/src/linux/udp6/SensorNetwork.cpp
+++ b/MQTTSNGateway/src/linux/udp6/SensorNetwork.cpp
@@ -360,9 +360,9 @@ int UDPPort6::open(uint16_t uniPortNo, uint16_t multiPortNo, const char *multica
    ipv6_mreq addrm;
    addrm.ipv6mr_interface = ifindex;
    inet_pton(AF_INET6, multicastAddr, &addrm.ipv6mr_multiaddr);
-    if (setsockopt(sock, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, &addrm, sizeof(addrm)) < 0)
+    if (setsockopt(sock, IPPROTO_IPV6, IPV6_JOIN_GROUP, &addrm, sizeof(addrm)) < 0)
    {
-        D_NWSTACK("\033[0m\033[0;31m error %d IPV6_ADD_MEMBERSHIP in Udp6Port::open\033[0m\033[0;37m\n", errno);
+        D_NWSTACK("\033[0m\033[0;31m error %d IPV6_JOIN_GROUP in Udp6Port::open\033[0m\033[0;37m\n", errno);
        close();
        return false;
    }