From a7ebd65bb3e3dd1c319a8850ac615f7e0d654a6c Mon Sep 17 00:00:00 2001 From: tomoaki Date: Mon, 12 Jun 2017 16:32:31 +0900 Subject: [PATCH] Added specific interface to send UDP6 packets Signed-off-by: tomoaki --- .../src/linux/udp6/SensorNetwork.cpp | 439 ++++++++++++++++++ MQTTSNGateway/src/linux/udp6/SensorNetwork.h | 112 +++++ 2 files changed, 551 insertions(+) create mode 100644 MQTTSNGateway/src/linux/udp6/SensorNetwork.cpp create mode 100644 MQTTSNGateway/src/linux/udp6/SensorNetwork.h diff --git a/MQTTSNGateway/src/linux/udp6/SensorNetwork.cpp b/MQTTSNGateway/src/linux/udp6/SensorNetwork.cpp new file mode 100644 index 0000000..eff397d --- /dev/null +++ b/MQTTSNGateway/src/linux/udp6/SensorNetwork.cpp @@ -0,0 +1,439 @@ +/************************************************************************************** + * Copyright (c) 2017, Benjamin Aigner + * Copyright (c) 2016, Tomoaki Yamaguchi (original UDPv4 implementation) + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Benjamin Aigner - Adaption of the UDPv4 code to use UDPv6 + * Tomoaki Yamaguchi - initial API and implementation and/or initial documentation + **************************************************************************************/ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "SensorNetwork.h" +#include "MQTTSNGWProcess.h" + +//using namespace std; +using namespace MQTTSNGW; + +/*=========================================== + Class SensorNetAddreess + ============================================*/ +SensorNetAddress::SensorNetAddress() +{ + _portNo = 0; + memset((void *)&_IpAddr,0,sizeof(_IpAddr)); +} + +SensorNetAddress::~SensorNetAddress() +{ +} + +struct sockaddr_in6 *SensorNetAddress::getIpAddress(void) +{ + return &_IpAddr; +} + +uint16_t SensorNetAddress::getPortNo(void) +{ + return _portNo; +} + +void SensorNetAddress::setAddress(struct sockaddr_in6 *IpAddr, uint16_t port) +{ + memcpy((void *)&_IpAddr,IpAddr,sizeof(_IpAddr)); + _portNo = port; +} + +/** + * convert Text data to SensorNetAddress + * @param buf is pointer of IP_Address:PortNo format text + * @return success = 0, Invalid format = -1 + */ +int SensorNetAddress::setAddress(string* data) +{ + const char *cstr = data->c_str(); + inet_pton(AF_INET6, cstr, &(_IpAddr.sin6_addr)); + return 0; +} +/** + * convert Text data to SensorNetAddress + * @param buf is pointer of IP_Address:PortNo format text + * @return success = 0, Invalid format = -1 + */ +int SensorNetAddress::setAddress(const char* data) +{ + inet_pton(AF_INET6, data, &(_IpAddr.sin6_addr)); + return 0; +} + +char* SensorNetAddress::getAddress(void) +{ + inet_ntop(AF_INET6, &(_IpAddr.sin6_addr), _addrString, INET6_ADDRSTRLEN); + return _addrString; +} + +bool SensorNetAddress::isMatch(SensorNetAddress* addr) +{ + return ((this->_portNo == addr->_portNo) && \ + (this->_IpAddr.sin6_addr.__in6_u.__u6_addr32[0] == addr->_IpAddr.sin6_addr.__in6_u.__u6_addr32[0]) && \ + (this->_IpAddr.sin6_addr.__in6_u.__u6_addr32[1] == addr->_IpAddr.sin6_addr.__in6_u.__u6_addr32[1]) && \ + (this->_IpAddr.sin6_addr.__in6_u.__u6_addr32[2] == addr->_IpAddr.sin6_addr.__in6_u.__u6_addr32[2]) && \ + (this->_IpAddr.sin6_addr.__in6_u.__u6_addr32[3] == addr->_IpAddr.sin6_addr.__in6_u.__u6_addr32[3])); +} + +SensorNetAddress& SensorNetAddress::operator =(SensorNetAddress& addr) +{ + this->_portNo = addr._portNo; + memcpy(&this->_IpAddr.sin6_addr, &addr._IpAddr.sin6_addr, sizeof(this->_IpAddr.sin6_addr)); + return *this; +} + + +char* SensorNetAddress::sprint(char* buf) +{ + char ip[INET6_ADDRSTRLEN]; + inet_ntop(AF_INET6, &(_IpAddr.sin6_addr), ip, INET6_ADDRSTRLEN); + sprintf( buf, "%s:", ip); + sprintf( buf + strlen(buf), "%d", ntohs(_portNo)); + return buf; +} + +/*=========================================== + Class SensorNetwork + ============================================*/ +SensorNetwork::SensorNetwork() +{ +} + +SensorNetwork::~SensorNetwork() +{ +} + +int SensorNetwork::unicast(const uint8_t* payload, uint16_t payloadLength, SensorNetAddress* sendToAddr) +{ + return UDPPort6::unicast(payload, payloadLength, sendToAddr); +} + +int SensorNetwork::broadcast(const uint8_t* payload, uint16_t payloadLength) +{ + return UDPPort6::broadcast(payload, payloadLength); +} + +int SensorNetwork::read(uint8_t* buf, uint16_t bufLen) +{ + return UDPPort6::recv(buf, bufLen, &_clientAddr); +} + +int SensorNetwork::initialize(void) +{ + char param[MQTTSNGW_PARAM_MAX]; + uint16_t unicastPortNo = 0; + string ip; + string broadcast; + string interface; + + if (theProcess->getParam("GatewayUDP6Bind", param) == 0) + { + ip = param; + _description = "GatewayUDP6Bind: "; + _description += param; + } + if (theProcess->getParam("GatewayUDP6Port", param) == 0) + { + unicastPortNo = atoi(param); + _description += " Gateway Port: "; + _description += param; + } + if (theProcess->getParam("GatewayUDP6Broadcast", param) == 0) + { + broadcast = param; + _description += " Broadcast Address: "; + _description += param; + } + if (theProcess->getParam("GatewayUDP6If", param) == 0) + { + interface = param; + _description += " Interface: "; + _description += param; + } + + return UDPPort6::open(ip.c_str(), unicastPortNo, broadcast.c_str(), interface.c_str()); +} + +const char* SensorNetwork::getDescription(void) +{ + return _description.c_str(); +} + +SensorNetAddress* SensorNetwork::getSenderAddress(void) +{ + return &_clientAddr; +} + +/*========================================= + Class udpStack + =========================================*/ + +UDPPort6::UDPPort6() +{ + _disconReq = false; + _sockfdUnicast = -1; + _sockfdMulticast = -1; +} + +UDPPort6::~UDPPort6() +{ + close(); +} + +void UDPPort6::close(void) +{ + if (_sockfdUnicast > 0) + { + ::close(_sockfdUnicast); + _sockfdUnicast = -1; + } + if (_sockfdMulticast > 0) + { + ::close(_sockfdMulticast); + _sockfdMulticast = -1; + } +} + +int UDPPort6::open(const char* ipAddress, uint16_t uniPortNo, const char* broadcastAddr, const char* interfaceName) +{ + struct addrinfo hints, *res; + int errnu; + const int reuse = 1; + + if (uniPortNo == 0) + { + cout << "error portNo undefined in UDPPort::open\n"; + return -1; + } + + + memset(&hints, 0, sizeof hints); + hints.ai_family = AF_INET6; // use IPv6 + hints.ai_socktype = SOCK_DGRAM; + hints.ai_flags = AI_PASSIVE; //use local IF address + + getaddrinfo(NULL, std::to_string(uniPortNo).c_str(), &hints, &res); + + _sockfdMulticast = socket(res->ai_family, res->ai_socktype, res->ai_protocol); + if(_sockfdMulticast <0) + { + perror("UDP6::open - multicast:"); + return errno; + } + + //select the interface + unsigned int ifindex; + ifindex = if_nametoindex(interfaceName); + errnu = setsockopt(_sockfdMulticast, IPPROTO_IPV6, IPV6_MULTICAST_IF, &ifindex,sizeof(ifindex)); + if(errnu <0) + { + perror("UDP6::open - limit IF:"); + return errnu; + } + + strcpy(_interfaceName,interfaceName); + + //restrict the socket to IPv6 only + int on = 1; + errnu = setsockopt(_sockfdMulticast, IPPROTO_IPV6, IPV6_V6ONLY, (char *)&on, sizeof(on)); + if(errnu <0) + { + perror("UDP6::open - limit IPv6:"); + return errnu; + } + + _uniPortNo = uniPortNo; + freeaddrinfo(res); + + //init the structs for getaddrinfo + //according to: https://beej.us/guide/bgnet/output/html/multipage/ + memset(&hints, 0, sizeof hints); + hints.ai_family = AF_INET6; // use IPv6, whichever + hints.ai_socktype = SOCK_DGRAM; + hints.ai_flags = AI_PASSIVE; // fill in my IP for me + + //no specific address, bind to available ones... + getaddrinfo(NULL, std::to_string(uniPortNo).c_str(), &hints, &res); + + //create the socket + _sockfdUnicast = socket(res->ai_family, res->ai_socktype, res->ai_protocol); + if (_sockfdUnicast < 0) + { + perror("UDP6::open - unicast socket:"); + return -1; + } + + //if given, set a given device name to bind to + if(strlen(interfaceName) > 0) + { + //socket option: bind to a given interface name + setsockopt(_sockfdUnicast, SOL_SOCKET, SO_BINDTODEVICE, interfaceName, strlen(interfaceName)); + } + + //socket option: reuse address + setsockopt(_sockfdUnicast, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); + + //finally: bind... + if (::bind(_sockfdUnicast, res->ai_addr, res->ai_addrlen) < 0) + { + perror("bind: "); + cout << "error can't bind unicast socket in UDPPort::open\n"; + return -1; + } + + //if given, set a broadcast address; otherwise it will be :: + if(strlen(broadcastAddr) > 0) + { + _grpAddr.setAddress(broadcastAddr); + } else { + _grpAddr.setAddress("::"); + } + //everything went fine... + freeaddrinfo(res); + return 0; +} + +//TODO: test if unicast is working too.... +int UDPPort6::unicast(const uint8_t* buf, uint32_t length, SensorNetAddress* addr) +{ + char destStr[INET6_ADDRSTRLEN+10]; + struct addrinfo hints, *res; + memset(&hints, 0, sizeof hints); + hints.ai_family = AF_INET6; // use IPv6 + hints.ai_socktype = SOCK_DGRAM; + + string portStr; + if(addr->getPortNo() != 0) + { + portStr = to_string(addr->getPortNo()); + } else { + portStr = to_string(_uniPortNo); + } + + if(strlen(_interfaceName) != 0) + { + strcpy(destStr, addr->getAddress()); + strcat(destStr,"%"); + strcat(destStr,_interfaceName); + getaddrinfo(destStr, portStr.c_str(), &hints, &res); + } else { + getaddrinfo(addr->getAddress(), portStr.c_str(), &hints, &res); + } + + int status = ::sendto(_sockfdUnicast, buf, length, 0, res->ai_addr, res->ai_addrlen); + + if (status < 0) + { + perror("UDP6::unicast: "); + cout << "errno in UDPPort::sendto:" << errno << "\n"; + } + + cout << "unicast sendto " << destStr << ", port: " << portStr << " length = " << status << "\n"; + + return status; +} + +int UDPPort6::broadcast(const uint8_t* buf, uint32_t length) +{ + struct addrinfo hint,*info; + int err; + memset( &hint, 0, sizeof( hint ) ); + + hint.ai_family = AF_INET6; + hint.ai_socktype = SOCK_DGRAM; + hint.ai_protocol = 0; + + + + if(strlen(_interfaceName) != 0) + { + char destStr[80]; + strcpy(destStr, _grpAddr.getAddress()); + strcat(destStr,"%"); + strcat(destStr,_interfaceName); + err = getaddrinfo(destStr, std::to_string(_uniPortNo).c_str(), &hint, &info ); + } else { + err = getaddrinfo(_grpAddr.getAddress(), std::to_string(_uniPortNo).c_str(), &hint, &info ); + } + + if( err != 0 ) { + perror( "UDP6::broadcast - getaddrinfo: " ); + return err; + } + + err = sendto(_sockfdMulticast, buf, length, 0, info->ai_addr, info->ai_addrlen ); + + if(err < 0 ) { + perror( "UDP6::broadcast - sendto: " ); + return errno; + } + + return 0; +} + +//TODO: test if this is working properly (GW works, but this function is not completely tested) +int UDPPort6::recv(uint8_t* buf, uint16_t len, SensorNetAddress* addr) +{ + struct timeval timeout; + fd_set recvfds; + + timeout.tv_sec = 0; + timeout.tv_usec = 1000000; // 1 sec + FD_ZERO(&recvfds); + FD_SET(_sockfdUnicast, &recvfds); + + int rc = 0; + if ( select(_sockfdUnicast + 1, &recvfds, 0, 0, &timeout) > 0 ) + { + if (FD_ISSET(_sockfdUnicast, &recvfds)) + { + rc = recvfrom(_sockfdUnicast, buf, len, 0, addr); + } + } + return rc; +} + +//TODO: test if this is working properly (GW works, but this function is not completely tested) +int UDPPort6::recvfrom(int sockfd, uint8_t* buf, uint16_t len, uint8_t flags, SensorNetAddress* addr) +{ + sockaddr_in6 sender; + socklen_t addrlen = sizeof(sender); + memset(&sender, 0, addrlen); + + int status = ::recvfrom(sockfd, buf, len, flags, (sockaddr*) &sender, &addrlen); + + if (status < 0 && errno != EAGAIN) + { + cout << "errno == " << errno << " in UDPPort::recvfrom\n"; + return -1; + } + addr->setAddress(&sender, (uint16_t)sender.sin6_port); + //D_NWSTACK("recved from %s:%d length = %d\n", inet_ntoa(sender.sin_addr),ntohs(sender.sin_port), status); + return status; +} diff --git a/MQTTSNGateway/src/linux/udp6/SensorNetwork.h b/MQTTSNGateway/src/linux/udp6/SensorNetwork.h new file mode 100644 index 0000000..62c297f --- /dev/null +++ b/MQTTSNGateway/src/linux/udp6/SensorNetwork.h @@ -0,0 +1,112 @@ +/************************************************************************************** + * Copyright (c) 2017, Benjamin Aigner + * based on UDP implementation (Copyright (c) 2016 Tomoaki Yamaguchi) + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Benjamin Aigner - port to UDPv6, used by RFC7668 (6lowpan over Bluetooth LE) + * Tomoaki Yamaguchi - initial API and implementation and/or initial documentation + **************************************************************************************/ + +#ifndef SENSORNETWORK_H_ +#define SENSORNETWORK_H_ + +#include "MQTTSNGWDefines.h" +#include +#include + +using namespace std; + +namespace MQTTSNGW +{ + +#ifdef DEBUG_NWSTACK + #define D_NWSTACK(...) printf(__VA_ARGS__) +#else + #define D_NWSTACK(...) +#endif + +/*=========================================== + Class SensorNetAddreess + ============================================*/ +class SensorNetAddress +{ +public: + SensorNetAddress(); + ~SensorNetAddress(); + void setAddress(struct sockaddr_in6 *IpAddr, uint16_t port); + int setAddress(string* data); + int setAddress(const char* data); + uint16_t getPortNo(void); + struct sockaddr_in6 *getIpAddress(void); + char* getAddress(void); + bool isMatch(SensorNetAddress* addr); + SensorNetAddress& operator =(SensorNetAddress& addr); + char* sprint(char* buf); +private: + uint16_t _portNo; + char _addrString[INET6_ADDRSTRLEN+1]; + struct sockaddr_in6 _IpAddr; +}; + +/*======================================== + Class UpdPort + =======================================*/ +class UDPPort6 +{ +public: + UDPPort6(); + virtual ~UDPPort6(); + + int open(const char* ipAddress, uint16_t uniPortNo, const char* broadcastAddr, const char* interfaceName); + void close(void); + int unicast(const uint8_t* buf, uint32_t length, SensorNetAddress* sendToAddr); + int broadcast(const uint8_t* buf, uint32_t length); + int recv(uint8_t* buf, uint16_t len, SensorNetAddress* addr); + +private: + void setNonBlocking(const bool); + int recvfrom(int sockfd, uint8_t* buf, uint16_t len, uint8_t flags, SensorNetAddress* addr); + + int _sockfdUnicast; + int _sockfdMulticast; + char _interfaceName[10]; + + SensorNetAddress _grpAddr; + SensorNetAddress _clientAddr; + uint16_t _uniPortNo; + bool _disconReq; + +}; + +/*=========================================== + Class SensorNetwork + ============================================*/ +class SensorNetwork: public UDPPort6 +{ +public: + SensorNetwork(); + ~SensorNetwork(); + + int unicast(const uint8_t* payload, uint16_t payloadLength, SensorNetAddress* sendto); + int broadcast(const uint8_t* payload, uint16_t payloadLength); + int read(uint8_t* buf, uint16_t bufLen); + int initialize(void); + const char* getDescription(void); + SensorNetAddress* getSenderAddress(void); + +private: + SensorNetAddress _clientAddr; // Sender's address. not gateway's one. + string _description; +}; + +} +#endif /* SENSORNETWORK6_H_ */