SensorNetwork.cpp

00001 /**************************************************************************************
00002  * Copyright (c) 2017, Benjamin Aigner
00003  * Copyright (c) 2016, Tomoaki Yamaguchi (original UDPv4 implementation)
00004  *
00005  * All rights reserved. This program and the accompanying materials
00006  * are made available under the terms of the Eclipse Public License v1.0
00007  * and Eclipse Distribution License v1.0 which accompany this distribution.
00008  *
00009  * The Eclipse Public License is available at
00010  *    http://www.eclipse.org/legal/epl-v10.html
00011  * and the Eclipse Distribution License is available at
00012  *   http://www.eclipse.org/org/documents/edl-v10.php.
00013  *
00014  * Contributors:
00015  *    Benjamin Aigner - Adaption of the UDPv4 code to use UDPv6
00016  *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
00017  *    Tieto Poland Sp. z o.o. - improve portability
00018  **************************************************************************************/
00019 #include <stdio.h>
00020 #include <unistd.h>
00021 #include <sys/types.h>
00022 #include <sys/socket.h>
00023 #include <netinet/ip.h>
00024 #include <net/if.h>
00025 #include <arpa/inet.h>
00026 #include <netdb.h>
00027 #include <string.h>
00028 #include <iostream>
00029 #include <regex>
00030 #include <string>
00031 #include <stdlib.h>
00032 #include "SensorNetwork.h"
00033 #include "MQTTSNGWProcess.h"
00034 
00035 //using namespace std;
00036 using namespace MQTTSNGW;
00037 
00038 /*===========================================
00039  Class  SensorNetAddreess
00040  ============================================*/
00041 SensorNetAddress::SensorNetAddress()
00042 {
00043     _portNo = 0;
00044     memset((void *)&_IpAddr,0,sizeof(_IpAddr));
00045 }
00046 
00047 SensorNetAddress::~SensorNetAddress()
00048 {
00049 }
00050 
00051 struct sockaddr_in6 *SensorNetAddress::getIpAddress(void)
00052 {
00053     return &_IpAddr;
00054 }
00055 
00056 uint16_t SensorNetAddress::getPortNo(void)
00057 {
00058     return _portNo;
00059 }
00060 
00061 void SensorNetAddress::setAddress(struct sockaddr_in6 *IpAddr, uint16_t port)
00062 {
00063     memcpy((void *)&_IpAddr,IpAddr,sizeof(_IpAddr));
00064     _portNo = port;
00065 }
00066 
00067 /**
00068  *  convert Text data to SensorNetAddress
00069  *  @param  buf is pointer of IP_Address:PortNo format text
00070  *  @return success = 0,  Invalid format = -1
00071  */
00072 int SensorNetAddress::setAddress(string* data)
00073 {
00074     const char *cstr = data->c_str();
00075     inet_pton(AF_INET6, cstr, &(_IpAddr.sin6_addr));
00076     return 0;
00077 }
00078 /**
00079  *  convert Text data to SensorNetAddress
00080  *  @param  buf is pointer of IP_Address:PortNo format text
00081  *  @return success = 0,  Invalid format = -1
00082  */
00083 int SensorNetAddress::setAddress(const char* data)
00084 {
00085     inet_pton(AF_INET6, data, &(_IpAddr.sin6_addr));
00086     return 0;
00087 }
00088 
00089 char* SensorNetAddress::getAddress(void)
00090 {
00091     inet_ntop(AF_INET6, &(_IpAddr.sin6_addr), _addrString, INET6_ADDRSTRLEN);
00092     return _addrString;
00093 }
00094 
00095 bool SensorNetAddress::isMatch(SensorNetAddress* addr)
00096 {
00097     return ((this->_portNo == addr->_portNo) && \
00098     (this->_IpAddr.sin6_addr.s6_addr32[0] == addr->_IpAddr.sin6_addr.s6_addr32[0]) && \
00099     (this->_IpAddr.sin6_addr.s6_addr32[1] == addr->_IpAddr.sin6_addr.s6_addr32[1]) && \
00100     (this->_IpAddr.sin6_addr.s6_addr32[2] == addr->_IpAddr.sin6_addr.s6_addr32[2]) && \
00101     (this->_IpAddr.sin6_addr.s6_addr32[3] == addr->_IpAddr.sin6_addr.s6_addr32[3]));
00102 }
00103 
00104 SensorNetAddress& SensorNetAddress::operator =(SensorNetAddress& addr)
00105 {
00106     this->_portNo = addr._portNo;
00107     memcpy(&this->_IpAddr.sin6_addr, &addr._IpAddr.sin6_addr, sizeof(this->_IpAddr.sin6_addr));
00108     return *this;
00109 }
00110 
00111 
00112 char* SensorNetAddress::sprint(char* buf)
00113 {
00114     char ip[INET6_ADDRSTRLEN];
00115     inet_ntop(AF_INET6, &(_IpAddr.sin6_addr), ip, INET6_ADDRSTRLEN);
00116     sprintf( buf, "%s:", ip);
00117     sprintf( buf + strlen(buf), "%d", ntohs(_portNo));
00118     return buf;
00119 }
00120 
00121 /*===========================================
00122  Class  SensorNetwork
00123  ============================================*/
00124 SensorNetwork::SensorNetwork()
00125 {
00126 }
00127 
00128 SensorNetwork::~SensorNetwork()
00129 {
00130 }
00131 
00132 int SensorNetwork::unicast(const uint8_t* payload, uint16_t payloadLength, SensorNetAddress* sendToAddr)
00133 {
00134     return UDPPort6::unicast(payload, payloadLength, sendToAddr);
00135 }
00136 
00137 int SensorNetwork::broadcast(const uint8_t* payload, uint16_t payloadLength)
00138 {
00139     return UDPPort6::broadcast(payload, payloadLength);
00140 }
00141 
00142 int SensorNetwork::read(uint8_t* buf, uint16_t bufLen)
00143 {
00144     return UDPPort6::recv(buf, bufLen, &_clientAddr);
00145 }
00146 
00147 int SensorNetwork::initialize(void)
00148 {
00149     char param[MQTTSNGW_PARAM_MAX];
00150     uint16_t unicastPortNo = 0;
00151     string ip;
00152     string broadcast;
00153     string interface;
00154 
00155     if (theProcess->getParam("GatewayUDP6Bind", param) == 0)
00156     {
00157         ip = param;
00158         _description = "GatewayUDP6Bind: ";
00159         _description += param;
00160     }
00161     if (theProcess->getParam("GatewayUDP6Port", param) == 0)
00162     {
00163         unicastPortNo = atoi(param);
00164         _description += " Gateway Port: ";
00165         _description += param;
00166     }
00167     if (theProcess->getParam("GatewayUDP6Broadcast", param) == 0)
00168     {
00169         broadcast = param;
00170         _description += " Broadcast Address: ";
00171         _description += param;
00172     }
00173     if (theProcess->getParam("GatewayUDP6If", param) == 0)
00174     {
00175         interface = param;
00176         _description += " Interface: ";
00177         _description += param;
00178     }
00179 
00180     return UDPPort6::open(ip.c_str(), unicastPortNo, broadcast.c_str(), interface.c_str());
00181 }
00182 
00183 const char* SensorNetwork::getDescription(void)
00184 {
00185     return _description.c_str();
00186 }
00187 
00188 SensorNetAddress* SensorNetwork::getSenderAddress(void)
00189 {
00190     return &_clientAddr;
00191 }
00192 
00193 /*=========================================
00194  Class udpStack
00195  =========================================*/
00196 
00197 UDPPort6::UDPPort6()
00198 {
00199     _disconReq = false;
00200     _sockfdUnicast = -1;
00201     _sockfdMulticast = -1;
00202 }
00203 
00204 UDPPort6::~UDPPort6()
00205 {
00206     close();
00207 }
00208 
00209 void UDPPort6::close(void)
00210 {
00211     if (_sockfdUnicast > 0)
00212     {
00213         ::close(_sockfdUnicast);
00214         _sockfdUnicast = -1;
00215     }
00216     if (_sockfdMulticast > 0)
00217     {
00218         ::close(_sockfdMulticast);
00219         _sockfdMulticast = -1;
00220     }
00221 }
00222 
00223 int UDPPort6::open(const char* ipAddress, uint16_t uniPortNo, const char* broadcastAddr, const char* interfaceName)
00224 {
00225     struct addrinfo hints, *res;
00226     int errnu;
00227     const int reuse = 1;
00228 
00229     if (uniPortNo == 0)
00230     {
00231         WRITELOG("error portNo undefined in UDPPort::open\n");
00232         return -1;
00233     }
00234 
00235 
00236     memset(&hints, 0, sizeof hints);
00237     hints.ai_family = AF_INET6;  // use IPv6
00238     hints.ai_socktype = SOCK_DGRAM;
00239     hints.ai_flags = AI_PASSIVE; //use local IF address
00240 
00241     getaddrinfo(NULL, std::to_string(uniPortNo).c_str(), &hints, &res);
00242 
00243     _sockfdMulticast = socket(res->ai_family, res->ai_socktype, res->ai_protocol);
00244     if(_sockfdMulticast <0)
00245     {
00246         WRITELOG("UDP6::open - multicast: %s",strerror(_sockfdMulticast));
00247         return errno;
00248     }
00249 
00250     //select the interface
00251     unsigned int ifindex;
00252     ifindex = if_nametoindex(interfaceName);
00253     errnu = setsockopt(_sockfdMulticast, IPPROTO_IPV6, IPV6_MULTICAST_IF, &ifindex,sizeof(ifindex));
00254     if(errnu <0)
00255     {
00256         WRITELOG("UDP6::open - limit IF: %s",strerror(errnu));
00257         return errnu;
00258     }
00259 
00260     strcpy(_interfaceName,interfaceName);
00261 
00262     //restrict the socket to IPv6 only
00263     int on = 1;
00264     errnu = setsockopt(_sockfdMulticast, IPPROTO_IPV6, IPV6_V6ONLY, (char *)&on, sizeof(on));
00265     if(errnu <0)
00266     {
00267         WRITELOG("UDP6::open - limit IPv6: %s",strerror(errnu));
00268         return errnu;
00269     }
00270 
00271     _uniPortNo = uniPortNo;
00272     freeaddrinfo(res);
00273 
00274     //init the structs for getaddrinfo
00275     //according to: https://beej.us/guide/bgnet/output/html/multipage/
00276     memset(&hints, 0, sizeof hints);
00277     hints.ai_family = AF_INET6;  // use IPv6, whichever
00278     hints.ai_socktype = SOCK_DGRAM;
00279     hints.ai_flags = AI_PASSIVE;     // fill in my IP for me
00280 
00281     //no specific address, bind to available ones...
00282     getaddrinfo(NULL, std::to_string(uniPortNo).c_str(), &hints, &res);
00283 
00284     //create the socket
00285     _sockfdUnicast = socket(res->ai_family, res->ai_socktype, res->ai_protocol);
00286     if (_sockfdUnicast < 0)
00287     {
00288         WRITELOG("UDP6::open - unicast socket: %s",strerror(_sockfdUnicast));
00289         return -1;
00290     }
00291 
00292     //if given, set a given device name to bind to
00293     if(strlen(interfaceName) > 0)
00294     {
00295         //socket option: bind to a given interface name
00296         setsockopt(_sockfdUnicast, SOL_SOCKET, SO_BINDTODEVICE, interfaceName, strlen(interfaceName));
00297     }
00298 
00299     //socket option: reuse address
00300     setsockopt(_sockfdUnicast, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
00301 
00302     //finally: bind...
00303     errnu = ::bind(_sockfdUnicast, res->ai_addr, res->ai_addrlen);
00304     if (errnu  < 0)
00305     {
00306         WRITELOG("error can't bind unicast socket in UDPPort::open: %s\n",strerror(errnu));
00307         return -1;
00308     }
00309 
00310     //if given, set a broadcast address; otherwise it will be ::
00311     if(strlen(broadcastAddr) > 0)
00312     {
00313         _grpAddr.setAddress(broadcastAddr);
00314     } else {
00315         _grpAddr.setAddress("::");
00316     }
00317     //everything went fine...
00318     freeaddrinfo(res);
00319     return 0;
00320 }
00321 
00322 //TODO: test if unicast is working too....
00323 int UDPPort6::unicast(const uint8_t* buf, uint32_t length, SensorNetAddress* addr)
00324 {
00325     char destStr[INET6_ADDRSTRLEN+10];
00326     struct addrinfo hints, *res;
00327     memset(&hints, 0, sizeof hints);
00328     hints.ai_family = AF_INET6;  // use IPv6
00329     hints.ai_socktype = SOCK_DGRAM;
00330 
00331     int port = 0;
00332     string portStr;
00333     if(addr->getPortNo() != 0)
00334     {
00335         port = htons(addr->getPortNo());
00336         portStr = to_string(port);
00337     } else {
00338         port = _uniPortNo;
00339         portStr = to_string(port);
00340     }
00341 
00342     if(strlen(_interfaceName) != 0)
00343     {
00344         strcpy(destStr, addr->getAddress());
00345         strcat(destStr,"%");
00346         strcat(destStr,_interfaceName);
00347         if(IN6_IS_ADDR_LINKLOCAL(addr->getAddress()))
00348         {
00349             getaddrinfo(destStr, portStr.c_str(), &hints, &res);
00350         }
00351         else
00352         {
00353             getaddrinfo(addr->getAddress(), portStr.c_str(), &hints, &res);
00354         }
00355     } else {
00356         strcpy(destStr, addr->getAddress());
00357         getaddrinfo(addr->getAddress(), portStr.c_str(), &hints, &res);
00358     }
00359 
00360     int status = ::sendto(_sockfdUnicast, buf, length, 0, res->ai_addr, res->ai_addrlen);
00361 
00362     if (status < 0)
00363     {
00364         WRITELOG("errno in UDPPort::unicast(sendto): %d, %s\n",status,strerror(status));
00365     }
00366 
00367     WRITELOG("unicast sendto %s, port: %d length = %d\n", destStr,port,status);
00368 
00369     return status;
00370 }
00371 
00372 int UDPPort6::broadcast(const uint8_t* buf, uint32_t length)
00373 {
00374     struct addrinfo hint,*info;
00375     int err;
00376     memset( &hint, 0, sizeof( hint ) );
00377 
00378     hint.ai_family = AF_INET6;
00379     hint.ai_socktype = SOCK_DGRAM;
00380     hint.ai_protocol = 0;
00381 
00382 
00383 
00384     if(strlen(_interfaceName) != 0)
00385     {
00386         char destStr[80];
00387         strcpy(destStr, _grpAddr.getAddress());
00388         strcat(destStr,"%");
00389         strcat(destStr,_interfaceName);
00390         if(IN6_IS_ADDR_MC_NODELOCAL(_grpAddr.getAddress()) ||
00391            IN6_IS_ADDR_MC_LINKLOCAL(_grpAddr.getAddress()))
00392         {
00393             err = getaddrinfo(destStr, std::to_string(_uniPortNo).c_str(), &hint, &info );
00394         }
00395         else
00396         {
00397             err = getaddrinfo(_grpAddr.getAddress(), std::to_string(_uniPortNo).c_str(), &hint, &info );
00398         }
00399     } else {
00400         err = getaddrinfo(_grpAddr.getAddress(), std::to_string(_uniPortNo).c_str(), &hint, &info );
00401     }
00402 
00403     if( err != 0 ) {
00404         WRITELOG("UDP6::broadcast - getaddrinfo: %s",strerror(err));
00405         return err;
00406     }
00407 
00408     err = sendto(_sockfdMulticast, buf, length, 0, info->ai_addr, info->ai_addrlen );
00409 
00410     if(err < 0 ) {
00411         WRITELOG("UDP6::broadcast - sendto: %s",strerror(err));
00412         return errno;
00413     }
00414 
00415     return 0;
00416 }
00417 
00418 //TODO: test if this is working properly (GW works, but this function is not completely tested)
00419 int UDPPort6::recv(uint8_t* buf, uint16_t len, SensorNetAddress* addr)
00420 {
00421     struct timeval timeout;
00422     fd_set recvfds;
00423 
00424     timeout.tv_sec = 0;
00425     timeout.tv_usec = 1000000;    // 1 sec
00426     FD_ZERO(&recvfds);
00427     FD_SET(_sockfdUnicast, &recvfds);
00428 
00429     int rc = 0;
00430     if ( select(_sockfdUnicast + 1, &recvfds, 0, 0, &timeout) > 0 )
00431     {
00432         if (FD_ISSET(_sockfdUnicast, &recvfds))
00433         {
00434             rc = recvfrom(_sockfdUnicast, buf, len, 0, addr);
00435         }
00436     }
00437     return rc;
00438 }
00439 
00440 //TODO: test if this is working properly (GW works, but this function is not completely tested)
00441 int UDPPort6::recvfrom(int sockfd, uint8_t* buf, uint16_t len, uint8_t flags, SensorNetAddress* addr)
00442 {
00443     sockaddr_in6 sender;
00444     socklen_t addrlen = sizeof(sender);
00445     memset(&sender, 0, addrlen);
00446 
00447     int status = ::recvfrom(sockfd, buf, len, flags, (sockaddr*) &sender, &addrlen);
00448 
00449     if (status < 0 && errno != EAGAIN)
00450     {
00451         WRITELOG("errno == %d in UDPPort::recvfrom: %s\n",errno,strerror(errno));
00452         return -1;
00453     }
00454     addr->setAddress(&sender, (uint16_t)sender.sin6_port);
00455     //D_NWSTACK("recved from %s:%d length = %d\n", inet_ntoa(sender.sin_addr),ntohs(sender.sin_port), status);
00456     return status;
00457 }