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>

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

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)

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
 
 

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

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

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

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

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));
+        int addrFlg = memcmp(_ipAddress.s6_addr, _gwIpAddress.s6_addr, sizeof(_gwIpAddress.s6_addr));
-        if (isUnicast() && diffAddr && (_portNo != _gwPortNo))
+        if (isUnicast() && addrFlg && (_portNo != _gwPortNo))
         {
             return 0;
         }
@@ -141,11 +141,14 @@ int LNetwork::sslConnect(void)
 LDtls6Port::LDtls6Port()
 {
     _disconReq = false;
-    memset(_pollfds, 0, sizeof(_pollfds));
+    _castStat = STAT_NONE;
-    _sock = 0;
-    _castStat = 0;
     _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);
+            ::close(_sockfdSsl);
-            _pollfds[i].fd = 0;
+            _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);
+    _sockfdMcast = socket(AF_INET6, SOCK_DGRAM, 0);
-    if (sock < 0)
+    if (_sockfdMcast < 0)
     {
         return false;
     }
 
     optval = 1;
-    setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
+    setsockopt(_sockfdMcast, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
-    setsockopt(sock, IPPROTO_IPV6, IPV6_V6ONLY, &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
+    FD_ZERO(&recvfds);
-    if (cnt == 0)
+    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)
+        maxSock = _sockfdMcast;
-        {
-            _castStat = STAT_SSL;
-            _sock = _pollfds[0].fd;
-            return true;
     }
-    }
+    else
-    else if (_pollfds[1].revents & POLLIN)
     {
-        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);
+    _sockfdSsl = socket(AF_INET6, SOCK_DGRAM, 0);
-    if (sock <= 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(_sockfdSsl, IPPROTO_IPV6, IPV6_V6ONLY, &optval, sizeof(optval));
-    setsockopt(sock, SOL_SOCKET, SO_REUSEADDR || SO_REUSEPORT, &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);
+    // Destination is a gateway address and portNo
-    struct sockaddr_in6 dest;
+    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;

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

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

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;

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 the SensorNetwork is not stable, specify "unstableLine".
-#     if Broker's Connection is SSL, write "secureConnection".
+#     if Broker's Connection is TLS, specify "secureConnection".
-#     if the client is a forwarder, "forwarder" is required.
+#     if the client is a forwarder,specify "forwarder".
-#     if the client send PUBLISH QoS-1, "QoS-1" is required.
+#     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

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

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;

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

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;
+#ifdef DEBUG_NW
-
+        int dtls = getUnicastClient(&client);
-        client.clear();
+        D_NWSTACK("Packet type = %d\n", dtls);
-        client.setFamily(_af);
+#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);
+            _senderAddr.sprint(clientaddrBuf);
-            D_NWSTACK("Client %s SSL Accepted. idx=%d\n", clientaddrBuf, index);
+            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)
+                    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
                     {
                         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.setFamily(AF_INET);
-    _multicastAddr->setIpAddress(ipAddress);
+    _multicastAddr.setIpAddress(ipAddress);
-    _multicastAddr->setPort(multiPortNo);
+    _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);
+    _multicastAddr.setPort(multiPortNo);
-    _unicastAddr->setPort(uniPortNo);
+    _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.setFamily(AF_INET6);
-    _multicastAddr->setIpAddress(ipAddress);
+    _multicastAddr.setIpAddress(ipAddress);
-    _multicastAddr->setPort(multiPortNo);
+    _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->setSockaddr4(&sender4);
-    addr->setPort(sender4.sin_port);
+
-    D_NWSTACK("SensorNetwork::getSenderAddress recved from %s:%d length = %d\n", inet_ntoa(sender4.sin_addr),
+    D_NWSTACK("SensorNetwork::getSenderAddress recved from %s:%d length = %d fd=%d\n", inet_ntoa(sender4.sin_addr),
-            ntohs(sender4.sin_port), len);
+            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;
+    char buf[16];
-    len = ::recvfrom(sock, &buf, 1, MSG_PEEK, (sockaddr*) &sender6, &addrlen6);
+    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);
+        D_NWSTACK("errno = %d in SensorNetwork::getSenderAddress\n", errno);
-        return len;
+        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;

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 _senderAddr;
-    SensorNetAddress *_multicastAddr;
+    SensorNetAddress _multicastAddr;
-    SensorNetAddress *_unicastAddr;
+    SensorNetAddress _unicastAddr;
     string _description;
     SSL_CTX *_dtlsctx;
     Connections *_conns;

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