GitHub/paho.mqtt-sn.embedded-c
1
0
Fork 0
mirror of https://github.com/eclipse/paho.mqtt-sn.embedded-c.git synced 2025-12-13 07:26:52 +01:00
Code Issues Projects Releases Wiki Activity
Files
5fb4312aad43799921abd88743344a157a0e3221
paho.mqtt-sn.embedded-c/MQTTSNGateway/src/linux/dtls/SensorNetwork.cpp
tomoaki 5fb4312aad This branch for debugging DTLS #90, #150, #195, #227 
The purpose of this branch is to share work in process.
Change sellect() of UDP to poll()
Rewrite UDP6 for DTLS6

Known bug: can't reconnect DTLS

Signed-off-by: tomoaki <tomoaki@tomy-tech.com>
2021-08-01 19:45:58 +09:00

1438 lines
37 KiB
C++
Raw Blame History

/**************************************************************************************
* Copyright (c) 2021, Tomoaki Yamaguchi
*
* All rights reserved. This program and the accompanying materials
* are made available under the terms of the Eclipse Public License v1.0
* and Eclipse Distribution License v1.0 which accompany this distribution.
*
* The Eclipse Public License is available at
* http://www.eclipse.org/legal/epl-v10.html
* and the Eclipse Distribution License is available at
* http://www.eclipse.org/org/documents/edl-v10.php.
*
* Contributors:
* Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
**************************************************************************************/
#include <stdio.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/ip.h>
#include <arpa/inet.h>
#include <net/if.h>
#include <netdb.h>
#include <string.h>
#include <regex>
#include <string>
#include <stdlib.h>
#include <poll.h>
#include <openssl/ssl.h>
#include <openssl/err.h>
#include <openssl/rand.h>
#include <memory>
#include "SensorNetwork.h"
#include "MQTTSNGWProcess.h"
#include "MQTTSNGateway.h"
using namespace std;
using namespace MQTTSNGW;
extern Gateway *theGateway;
#define COOKIE_SECRET_LENGTH 16
int cookie_initialized = 0;
unsigned char cookie_secret[COOKIE_SECRET_LENGTH];
/*===========================================
Class SensorNetAddreess
These 4 methods are minimum requirements for the SensorNetAddress class.
isMatch(SensorNetAddress* )
operator =(SensorNetAddress& )
setAddress(string* )
sprint(char* )
UDPPort class requires these 3 methods.
getIpAddress(void)
getPortNo(void)
setAddress(uint32_t IpAddr, uint16_t port)
============================================*/
SensorNetAddress::SensorNetAddress()
{
_portNo = 0;
memset(&_ipAddr, 0, sizeof(_ipAddr));
}
SensorNetAddress::~SensorNetAddress()
{
}
void SensorNetAddress::setFamily(int type)
{
_ipAddr.af = type;
}
int SensorNetAddress::getFamily(void)
{
return _ipAddr.af;
}
ipAddr_t* SensorNetAddress::getIpAddress(void)
{
return &_ipAddr;
}
in_port_t SensorNetAddress::getPort(void)
{
return _portNo;
}
void SensorNetAddress::setAddress(ipAddr_t *IpAddr, uint16_t port)
{
_ipAddr.addr.ad6 = IpAddr->addr.ad6;
_portNo = htons(port);
_ipAddr.af = IpAddr->af;
}
void SensorNetAddress::setPort(uint16_t port)
{
_portNo = htons(port);
}
/**
* Set Address data to SensorNetAddress
*
* @param *ip_port is "IP_Address:PortNo" format string
* @return success = 0, Invalid format = -1
*
* This function is used in ClientList::authorize(const char* fileName)
* e.g.
* Authorized clients are defined by fileName = "clients.conf"
*
* Client02,172.16.1.7:12002
* Client03,172.16.1.8:13003
* Client01,172.16.1.6:12001
* or
* Client01,[xxxx::xxxx]:11001
* Client02,[xxxx::xxxx]:12001
*
* This definition is necessary when using TLS/DTLS connection.
* Gateway rejects clients are not in the list for security reasons.
*
*/
int SensorNetAddress::setAddress(string *ipAddrPort)
{
string port("");
string ip("");
size_t pos;
int portNo = 0;
_portNo = 0;
memset(&_ipAddr.addr, 0, sizeof(_ipAddr.addr.ad6));
if (*ipAddrPort->c_str() == '[')
{
// AF_INET6
pos = ipAddrPort->find_last_of("]:");
if (pos != string::npos)
{
ip = ipAddrPort->substr(1, pos - 2);
port = ipAddrPort->substr(pos + 1);
}
}
else
{
// AF_INET
pos = ipAddrPort->find_last_of(':');
if (pos != string::npos)
{
ip = ipAddrPort->substr(0, pos);
port = ipAddrPort->substr(pos + 1);
}
}
if (port == "" || ip == "")
{
return -1;
}
if (setIpAddress(&ip) == 0)
{
if ((portNo = atoi(port.c_str())) != 0)
{
_portNo = htons(portNo);
return 0;
}
}
return -1;
}
int SensorNetAddress::setIpAddress(string *ipAddress)
{
if (inet_pton(AF_INET, (const char*) ipAddress->c_str(), (void*) &_ipAddr.addr) == 1)
{
_ipAddr.af = AF_INET;
}
else if (inet_pton(AF_INET6, (const char*) ipAddress->c_str(), (void*) &_ipAddr.addr) == 1)
{
_ipAddr.af = AF_INET6;
}
else
{
_ipAddr.af = 0;
return -1;
}
return 0;
}
bool SensorNetAddress::isMatch(SensorNetAddress *addr)
{
if (this->_portNo != addr->_portNo || this->_ipAddr.af != addr->_ipAddr.af)
{
return false;
}
if (this->_ipAddr.af == AF_INET
&& memcmp((const void*) &this->_ipAddr.addr.ad4, (const void*) &addr->_ipAddr.addr.ad4, sizeof(struct in_addr))
== 0)
{
return true;
}
if (this->_ipAddr.af == AF_INET6
&& memcmp((const void*) &this->_ipAddr.addr.ad6, (const void*) &addr->_ipAddr.addr.ad6, sizeof(struct in6_addr))
== 0)
{
return true;
}
return false;
}
SensorNetAddress& SensorNetAddress::operator =(SensorNetAddress &addr)
{
this->_portNo = addr._portNo;
memcpy((void*) &this->_ipAddr, (const void*) &addr._ipAddr, sizeof(_ipAddr));
this->_pfdsIndex = addr._pfdsIndex;
return *this;
}
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)
{
_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)
{
sockaddr->sin_family = _ipAddr.af;
memcpy((void*) &sockaddr->sin_addr, (void*) &_ipAddr.addr.ad4, sizeof(_ipAddr.addr.ad4));
sockaddr->sin_port = _portNo;
}
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;
}
char* SensorNetAddress::sprint(char *buf)
{
char senderstr[INET6_ADDRSTRLEN];
char *ptr = senderstr;
if (_ipAddr.af == AF_INET)
{
ptr = inet_ntoa(_ipAddr.addr.ad4);
sprintf(buf, "%s:", ptr);
}
else if (_ipAddr.af == AF_INET6)
{
inet_ntop(AF_INET6, (const void*) &_ipAddr.addr.ad6, ptr, INET6_ADDRSTRLEN);
sprintf(buf, "[%s]:", ptr);
}
else
{
*buf = 0;
return buf;
}
sprintf(buf + strlen(buf), "%d", ntohs(_portNo));
return buf;
}
void SensorNetAddress::setIndex(int index)
{
_pfdsIndex = index;
}
int SensorNetAddress::getIndex(void)
{
return _pfdsIndex;
}
void SensorNetAddress::clear(void)
{
memset(&_ipAddr, 0, sizeof(_ipAddr));
_portNo = 0;
}
Connections::Connections()
{
_pollfds = nullptr;
_clientAddr = nullptr;
_ssls = nullptr;
_maxfds = 0;
_numfds = 2;
}
Connections::~Connections()
{
if (_ssls)
{
for (int i = 0; i < _numfds; i++)
{
if (_ssls[i] > 0)
{
SSL_shutdown(_ssls[i]);
SSL_free(_ssls[i]);
}
}
free(_ssls);
}
if (_pollfds)
{
for (int i = 0; i < _numfds; i++)
{
if (_pollfds[i].fd > 0)
{
::close(_pollfds[i].fd);
}
}
free(_pollfds);
}
for (int i = 0; i < _maxfds; i++)
{
delete _clientAddr[i];
}
free(_clientAddr);
}
void Connections::initialize(int maxClient)
{
_maxfds = maxClient + POLL_SSL;
if ((_pollfds = (pollfd*) calloc(_maxfds, sizeof(pollfd))) == NULL)
{
throw EXCEPTION("Can't allocate pollfd.", 0);
}
if ((_ssls = (SSL**) calloc(_maxfds, sizeof(SSL*))) == NULL)
{
throw EXCEPTION("Can't allocate ssls.", 0);
}
_clientAddr = (SensorNetAddress**) malloc(_maxfds * sizeof(unsigned long int));
for (int i = 0; i < _maxfds; i++)
{
_clientAddr[i] = new SensorNetAddress();
}
}
void Connections::closeSSL(int index)
{
index += POLL_SSL;
SSL_shutdown(_ssls[index]);
SSL_free(_ssls[index]);
_ssls[index] = (SSL*) -1;
}
int Connections::getEventUnicast(void)
{
return _pollfds[POLL_UCAST].revents;
}
int Connections::getEventMulticast(void)
{
return _pollfds[POLL_MCAST].revents;
}
int Connections::getEventClient(int index)
{
return _pollfds[index + POLL_SSL].revents;
}
int Connections::getSockMulticast(void)
{
return _pollfds[POLL_MCAST].fd;
}
void Connections::setSockMulticast(int sock)
{
_mutex.lock();
_pollfds[POLL_MCAST].fd = sock;
_pollfds[POLL_MCAST].events = POLLIN;
_mutex.unlock();
}
void Connections::setSockUnicast(int sock)
{
_mutex.lock();
_pollfds[POLL_UCAST].fd = sock;
_pollfds[POLL_UCAST].events = POLLIN;
_mutex.unlock();
}
int Connections::getSockUnicast(void)
{
return _pollfds[POLL_UCAST].fd;
}
int Connections::getSockClient(int index)
{
return _pollfds[index + POLL_SSL].fd;
}
void Connections::close(int index)
{
_mutex.lock();
int idx = index + POLL_SSL;
_mutex.lock();
int sock = _pollfds[idx].fd;
SSL *ssl = _ssls[idx];
SensorNetAddress *addr = _clientAddr[idx];
for (; idx < _numfds; idx++)
{
_ssls[index] = _ssls[idx + 1];
_pollfds[index] = _pollfds[idx + 1];
_clientAddr[index] = _clientAddr[idx + 1];
if (_ssls[idx + 1] == 0)
{
_clientAddr[idx + 1] = new SensorNetAddress();
break;
}
}
if (ssl > 0)
{
_numfds--;
SSL_shutdown(ssl);
SSL_free(ssl);
}
if (sock > 0)
{
close(sock);
}
if (addr != nullptr)
{
delete addr;
}
_mutex.unlock();
}
int Connections::poll(int timeout)
{
return ::poll(_pollfds, _numfds, timeout);
}
int Connections::addClientSSL(SSL *ssl, int sock)
{
_mutex.lock();
_pollfds[_numfds].fd = sock;
_pollfds[_numfds].events = POLLIN;
_ssls[_numfds] = ssl;
int rc = _numfds - POLL_SSL;
_numfds++;
_mutex.unlock();
return rc;
}
int Connections::getNumOfConnections(void)
{
return _numfds;
}
int Connections::getNumOfClients(void)
{
return _numfds - POLL_SSL > 0 ? _numfds - POLL_SSL : 0;
}
SSL* Connections::getClientSSL(int index)
{
return _ssls[index + POLL_SSL];
}
int Connections::searchClient(SensorNetAddress *addr)
{
for (int i = POLL_SSL; i < _numfds; i++)
{
if (_clientAddr[i]->isMatch(addr) == true)
{
return i - POLL_SSL;
}
}
return -1;
}
/*================================================================
Class SensorNetwork
getDescpription( ) is used by Gateway::initialize( )
initialize( ) is used by Gateway::initialize( )
getSenderAddress( ) is used by ClientRecvTask::run( )
broadcast( ) is used by MQTTSNPacket::broadcast( )
unicast( ) is used by MQTTSNPacket::unicast( )
read( ) is used by MQTTSNPacket::recv( )
================================================================*/
#define PACKET_CLIENTHELLO 10000
#define PACKET_APPL 10001
#define PACKET_OTHERS 10002
/* Certificate verification. Returns 1 if trusted, else 0 */
int verify_cert(int ok, X509_STORE_CTX *ctx);
/* Generate cookie. Returns 1 on success, 0 otherwise */
int generate_cookie(SSL *ssl, unsigned char *cookie, unsigned int *cookie_len);
/* Verify cookie. Returns 1 on success, 0 otherwise */
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;
}
SensorNetwork::~SensorNetwork()
{
if (_conns != nullptr)
{
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();
SSL *ssl = _conns->getClientSSL(sendToAddr->getIndex());
int len = SSL_write(ssl, payload, payloadLength);
int rc = SSL_get_error(ssl, len);
if (rc < 0)
{
D_NWSTACK("error %d in SensorNetwork::unicast\n", rc);
len = -1;
}
_mutex.unlock();
return len;
}
int SensorNetwork::broadcast(const uint8_t *payload, uint16_t payloadLength)
{
_mutex.lock();
int status;
#ifndef DTLS6
sockaddr_in dest;
_multicastAddr->cpyAddr4(&dest);
status = ::sendto(_conns->getSockUnicast(), payload, payloadLength, 0, (const sockaddr*) &dest, sizeof(dest));
if (status < 0)
{
WRITELOG("AF_INET errno = %d in UDP4_6Port::sendto\n", errno);
}
D_NWSTACK("sendto %s:%u length = %d\n", inet_ntoa(dest.sin_addr), ntohs(dest.sin_port), status);
#else
sockaddr_in6 dest;
_multicastAddr->cpyAddr6(&dest);
status = ::sendto(_conns->getSockUnicast(), payload, payloadLength, 0, (const sockaddr*) &dest, sizeof(dest));
if (status < 0)
{
WRITELOG("AF_INET6 errno = %d in SensorNetwork::broadcast\n", errno);
}
#ifdef DEBUG_NW
char buff[INET6_ADDRSTRLEN];
inet_ntop(AF_INET6, &dest.sin6_addr, buff, INET6_ADDRSTRLEN);
D_NWSTACK("sendto [%s]:%u length = %d\n", buff, ntohs(dest.sin6_port), status);
#endif
#endif
_mutex.unlock();
return status;
}
int SensorNetwork::read(uint8_t *buf, uint16_t bufLen)
{
int optval;
int clientIndex = -1;
int sockListen = 0;
SensorNetAddress client;
char errmsg[256];
union
{
struct sockaddr_in s4;
struct sockaddr_in6 s6;
} client_addr;
// Ccheck sockets
int cnt = _conns->poll(2000); // Timeout 2secs
if (cnt == 0)
{
return cnt;
}
if (cnt < 0)
{
/* ToDo: close socket */
return -1;
}
int numfds = _conns->getNumOfConnections();
client.clear();
client.setFamily(_af);
size_t recvlen = 0;
SSL *ssl = 0;
_mutex.lock();
// Check Unicast Port
if (_conns->getEventUnicast() & POLLIN)
{
D_NWSTACK("Connect RECV\n");
// SSL connection request from a client
optval = 1;
client.clear();
client.setFamily(_af);
getUnicastClient(&client);
sockListen = _conns->getSockUnicast();
ListenClient_hello:
// Listen Connection
SSL *ssl = SSL_new(_dtlsctx);
BIO *bio = BIO_new_dgram(sockListen, BIO_NOCLOSE);
SSL_set_bio(ssl, bio, bio);
SSL_set_options(ssl, SSL_OP_COOKIE_EXCHANGE);
int rc = 0;
// SSL Listen
D_NWSTACK("Listen SSL\n");
rc = DTLSv1_listen(ssl, (BIO_ADDR*) &client_addr);
if (rc <= 0)
{
ERR_error_string_n(ERR_get_error(), errmsg, sizeof(errmsg));
WRITELOG("Listen rc=%d %s\n", rc, errmsg);
_mutex.unlock();
return 0;
}
// if (clientIndex != -1)
// {
// _conns->close(clientIndex);
// }
// SSL Accept
#ifndef DTLS6
int client_fd = socket(AF_INET, SOCK_DGRAM, 0);
setsockopt(client_fd, SOL_SOCKET, SO_REUSEADDR, (const void*) &optval, sizeof(optval));
// Bind to Dtls PortNo
bind(client_fd, (sockaddr*) &_serverAddr4, sizeof(sockaddr_in));
connect(client_fd, (sockaddr*) &client_addr, sizeof(sockaddr_in));
client.setSockaddr4((sockaddr_in*) &client_addr.s4);
#else
// DTLS over IPv6
int client_fd = socket(AF_INET6, SOCK_DGRAM, 0);
setsockopt(client_fd, SOL_SOCKET, SO_REUSEADDR, (const void*) &optval, sizeof(optval));
// Bind to Dtls PortNo
bind(client_fd, (sockaddr*) &_serverAddr6, sizeof(sockaddr_in6));
connect(client_fd, (sockaddr*) &client_addr, sizeof(sockaddr_in6));
client.setSockaddr6((sockaddr_in6*) &client_addr.s6);
#endif
BIO *cbio = SSL_get_rbio(ssl);
BIO_set_fd(cbio, client_fd, BIO_NOCLOSE);
BIO_ctrl(cbio, BIO_CTRL_DGRAM_SET_CONNECTED, 0, &client_addr);
D_NWSTACK("Accept SSL\n");
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);
SSL_shutdown(ssl);
SSL_free(ssl);
::close(client_fd);
}
else
{
// add ssl & socket to Connections instance
int index = _conns->addClientSSL(ssl, client_fd);
// save SensorNetworkAddress of Client
client.setIndex(index);
_senderAddr = &client;
char clientaddrBuf[128];
client.sprint(clientaddrBuf);
WRITELOG("Client %s SSL Accepted. idx=%d\n", clientaddrBuf, index);
}
_mutex.unlock();
}
// check Multicast
else if (_conns->getEventMulticast() & POLLIN)
{
_mutex.unlock();
return multicastRecv(buf, bufLen);
}
else
{
// Check SSL packet from clients
for (int i = 0; i < numfds - POLL_SSL; i++)
{
if (_conns->getEventClient(i) == POLLIN)
{
D_NWSTACK("SSL RECV\n");
int dtls = getSendClient(i, &client);
if (dtls > 0)
{
D_NWSTACK("DTLT type=%d\n", dtls);
if (dtls == PACKET_CLIENTHELLO)
{
#ifdef DEBUG_NW
char clientaddrBuf[128];
client.sprint(clientaddrBuf);
D_NWSTACK("Client %s SSL reconnect. idx=%d\n", clientaddrBuf, i);
#endif
clientIndex = i;
sockListen = _conns->getSockClient(i);
goto ListenClient_hello;
}
// Recv a MQTT-SN message from a client
ssl = _conns->getClientSSL(i);
int len = SSL_read_ex(ssl, (void*) buf, (size_t) bufLen, &recvlen);
if (SSL_get_error(ssl, len) >= 0)
{
_senderAddr = &client;
_senderAddr->setIndex(i);
char clientaddrBuf[128];
client.sprint(clientaddrBuf);
D_NWSTACK("Client %s SSL Accepted. idx=%d\n", clientaddrBuf, i);
}
else
{
D_NWSTACK("SSL RECV Error\n");
_conns->close(i);
recvlen = -1;
}
_mutex.unlock();
return recvlen;
}
}
}
}
return 0;
}
void SensorNetwork::initialize(void)
{
char param[MQTTSNGW_PARAM_MAX];
char errmsg[256];
uint16_t multicastPortNo = 0;
uint16_t unicastPortNo = 0;
uint16_t dtlsPortNo = 0;
SensorNetAddress add;
sockaddr_in6 soadd;
add.setSockaddr6(&soadd);
#ifndef DTLS6
string ip;
uint32_t ttl = 1;
if (theProcess->getParam("MulticastIP", param) == 0)
{
ip = param;
_description += "IPv4 DTLS Multicast ";
_description += param;
}
if (theProcess->getParam("MulticastPortNo", param) == 0)
{
multicastPortNo = atoi(param);
_description += ":";
_description += param;
}
if (theProcess->getParam("GatewayPortNo", param) == 0)
{
unicastPortNo = atoi(param);
_description += ", Gateway PortNo:";
_description += param;
}
if (theProcess->getParam("DtlsPortNo", param) == 0)
{
dtlsPortNo = atoi(param);
_description += ", SSL PortNo:";
_description += param;
}
if (theProcess->getParam("MulticastTTL", param) == 0)
{
ttl = atoi(param);
_description += ", TTL:";
_description += param;
}
#else
string ip6;
uint32_t hops = 1;
string interface;
if (theProcess->getParam("MulticastIPv6", param) == 0)
{
ip6 = param;
_description += "IPv6 DTLS Multicast [";
_description += param;
}
if (theProcess->getParam("MulticastIPv6PortNo", param) == 0)
{
multicastPortNo = atoi(param);
_description += "]:";
_description += param;
}
if (theProcess->getParam("GatewayIPv6PortNo", param) == 0)
{
unicastPortNo = atoi(param);
_description += ", Gateway PortNo:";
_description += param;
}
if (theProcess->getParam("DtlsPortNo", param) == 0)
{
dtlsPortNo = atoi(param);
_description += ", SSL PortNo:";
_description += param;
}
if (theProcess->getParam("MulticastIPv6If", param) == 0)
{
interface = param;
_description += ", Interface:";
_description += param;
}
if (theProcess->getParam("MulticastHops", param) == 0)
{
hops = atoi(param);
_description += ", Hops:";
_description += param;
}
#endif
if (theGateway->getGWParams()->gwCertskey == nullptr)
{
throw EXCEPTION("DtlsCertsKey is required.", 0);
}
if (theGateway->getGWParams()->gwPrivatekey == nullptr)
{
throw EXCEPTION("DtlsPrivateKey is required.", 0);
}
/* allocate Connections */
_conns->initialize(theGateway->getGWParams()->maxClients);
SSL_load_error_strings();
SSL_library_init();
_dtlsctx = SSL_CTX_new(DTLS_server_method());
if (_dtlsctx == 0)
{
ERR_error_string_n(ERR_get_error(), errmsg, sizeof(errmsg));
D_NWSTACK("SSL_CTX_new() %s\n", errmsg);
throw EXCEPTION("SSL_CTX_new()", 0);
}
SSL_CTX_set_min_proto_version(_dtlsctx, DTLS1_VERSION);
if (SSL_CTX_use_certificate_file(_dtlsctx, theGateway->getGWParams()->gwCertskey, SSL_FILETYPE_PEM) != 1)
{
ERR_error_string_n(ERR_get_error(), errmsg, sizeof(errmsg));
D_NWSTACK("SSL_CTX_use_certificate_file() %s %s\n", theGateway->getGWParams()->gwCertskey, errmsg);
throw EXCEPTION("SSL_CTX_use_certificate_file()", 0);
}
if (SSL_CTX_use_PrivateKey_file(_dtlsctx, theGateway->getGWParams()->gwPrivatekey, SSL_FILETYPE_PEM) != 1)
{
ERR_error_string_n(ERR_get_error(), errmsg, sizeof(errmsg));
D_NWSTACK("SSL_CTX_use_PrivateKey_file() %s %s\n", theGateway->getGWParams()->gwPrivatekey, errmsg);
throw EXCEPTION("SSL_CTX_use_PrivateKey_file()", 0);
}
/* Client certification and cookie are not required */
SSL_CTX_set_verify(_dtlsctx, SSL_VERIFY_NONE, NULL);
SSL_CTX_set_cookie_generate_cb(_dtlsctx, generate_cookie);
SSL_CTX_set_cookie_verify_cb(_dtlsctx, verify_cookie);
/* Prepare UDP and UDP6 sockets for Multicasting and unicasting */
#ifndef DTLS6
if (openV4(&ip, multicastPortNo, unicastPortNo, dtlsPortNo, ttl) < 0)
{
throw EXCEPTION("Can't open a UDP4", errno);
}
#else
if (openV6(&ip6, &interface, multicastPortNo, unicastPortNo, dtlsPortNo, hops) < 0)
{
throw EXCEPTION("Can't open a UDP6", errno);
}
#endif
}
const char* SensorNetwork::getDescription(void)
{
return _description.c_str();
}
SensorNetAddress* SensorNetwork::getSenderAddress(void)
{
return _senderAddr;
}
int SensorNetwork::openV4(string *ipAddress, uint16_t multiPortNo, uint16_t uniPortNo, uint16_t dtlsPortNo, uint32_t ttl)
{
int optval = 0;
int rc = -1;
int sock = 0;
errno = 0;
_af = AF_INET;
if (uniPortNo == 0 || multiPortNo == 0)
{
D_NWSTACK("error portNo undefined in UDP4_6Port::openV4\n");
return rc;
}
/*------ Create unicast socket --------*/
sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (sock < 0)
{
D_NWSTACK("can't create unicast socket in UDP4_6Port::openV4 error %d %s\n", errno, strerror(errno));
return -1;
}
optval = 1;
setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
sockaddr_in addr;
addr.sin_family = AF_INET;
addr.sin_port = htons(uniPortNo);
addr.sin_addr.s_addr = INADDR_ANY;
if (::bind(sock, (sockaddr*) &addr, sizeof(addr)) < 0)
{
D_NWSTACK("can't bind unicast socket in UDP4_6Port::openV4 error %d %s\n", errno, strerror(errno));
return -1;
}
_conns->setSockUnicast(sock);
/*------ Set SSL socket address --------*/
_serverAddr4.sin_family = AF_INET;
_serverAddr4.sin_port = htons(uniPortNo);
_serverAddr4.sin_addr.s_addr = INADDR_ANY;
/*------ Create Multicast socket --------*/
sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (sock < 0)
{
D_NWSTACK("can't create multicast socket in UDP4_6Port::openV4 error %d %s\n", errno, strerror(errno));
return -1;
}
setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
sockaddr_in addrm;
addrm.sin_family = AF_INET;
addrm.sin_port = htons(multiPortNo);
addrm.sin_addr.s_addr = INADDR_ANY;
if (::bind(sock, (sockaddr*) &addrm, sizeof(addrm)) < 0)
{
D_NWSTACK("can't bind multicast socket in UDP4_6Port::openV4 error %d %s\n", errno, strerror(errno));
return -1;
}
ip_mreq mreq;
memset(&mreq, 0, sizeof(mreq));
mreq.imr_interface.s_addr = INADDR_ANY;
mreq.imr_multiaddr.s_addr = inet_addr(ipAddress->c_str());
if (setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0)
{
D_NWSTACK("Multicast IP_ADD_MEMBERSHIP in UDP4_6Port::openV4 error %d %s\n", errno, strerror(errno));
return -1;
}
if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_TTL, &ttl, sizeof(ttl)) < 0)
{
D_NWSTACK("Multicast IP_MULTICAST_TTL in UDP4_6Port::openV4 error %d %s\n", errno, strerror(errno));
return -1;
}
#ifdef DEBUG_NW
optval = 1;
#else
optval = 0;
#endif
if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_LOOP, (char*) &optval, sizeof(optval)) < 0)
{
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);
_conns->setSockMulticast(sock);
return 0;
}
int SensorNetwork::openV6(string *ipAddress, string *interface, uint16_t multiPortNo, uint16_t uniPortNo, uint16_t dtlsPortNo,
uint32_t hops)
{
int optval = 0;
int sock = 0;
uint32_t ifindex = 0;
errno = 0;
if (uniPortNo == 0 || multiPortNo == 0)
{
WRITELOG("error portNo undefined in SensorNetwork::openV6\n");
return -1;
}
_multicastAddr->setPort(multiPortNo);
_unicastAddr->setPort(dtlsPortNo);
if (_multicastAddr->setIpAddress(ipAddress) < 0)
{
D_NWSTACK("Incorrect IPV6 address in SensorNetwork::openV6 error %s\n", strerror(errno));
return -1;
}
/*------ Create unicast socket --------*/
sock = socket(AF_INET6, SOCK_DGRAM, 0);
if (sock < 0)
{
D_NWSTACK("can't create unicast socket in SensorNetwork::openV6 error %s\n", strerror(errno));
return -1;
}
_conns->setSockUnicast(sock);
optval = 1;
setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
optval = 1;
if (setsockopt(sock, IPPROTO_IPV6, IPV6_V6ONLY, &optval, sizeof(optval)) < 0)
{
D_NWSTACK("IPV6_ONLY in SensorNetwork::openV6 error %s\n", strerror(errno));
return -1;
}
sockaddr_in6 addr;
memset(&addr, 0, sizeof(addr));
addr.sin6_family = AF_INET6;
addr.sin6_port = htons(uniPortNo);
addr.sin6_addr = in6addr_any;
if (::bind(sock, (sockaddr*) &addr, sizeof(addr)) < 0)
{
D_NWSTACK("can't bind unicast socket in SensorNetwork::openV6 error %s\n", strerror(errno));
return -1;
}
if (interface->size() > 0)
{
ifindex = if_nametoindex(interface->c_str());
#ifdef __APPLE__
setsockopt(sock, IPPROTO_IP, IP_BOUND_IF, &ifindex, interface->size());
#else
setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, interface->c_str(), interface->size());
#endif
}
/*------ Set SSL socket address --------*/
_serverAddr6.sin6_family = AF_INET6;
_serverAddr6.sin6_port = htons(uniPortNo);
_serverAddr6.sin6_addr = in6addr_any;
if (::bind(sock, (sockaddr*) &_serverAddr6, sizeof(_serverAddr6)) < 0)
{
D_NWSTACK("can't bind listen socket in SensorNetwork::openV6 error %s\n", strerror(errno));
return -1;
}
if (interface->size() > 0)
{
#ifdef __APPLE__
setsockopt(sock, IPPROTO_IP, IP_BOUND_IF, &ifindex, interface->size());
#else
setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, interface->c_str(), interface->size());
#endif
}
// Create Multicast socket
sock = socket(AF_INET6, SOCK_DGRAM, 0);
if (sock < 0)
{
D_NWSTACK("can't create multicast socket in SensorNetwork::openV6 error %s\n", strerror(errno));
return -1;
}
_conns->setSockMulticast(sock);
optval = 1;
setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
if (setsockopt(sock, IPPROTO_IPV6, IPV6_MULTICAST_IF, &ifindex, sizeof(ifindex)) < 0)
{
D_NWSTACK("IPV6_MULTICAST_IF in SensorNetwork::openV6 error %s\n", strerror(errno));
return -1;
}
if (setsockopt(sock, IPPROTO_IPV6, IPV6_V6ONLY, (char*) &optval, sizeof(optval)) < 0)
{
D_NWSTACK("IPV6_ONLY in SensorNetworkSensorNetwork::openV6 error %s\n", strerror(errno));
return -1;
}
sockaddr_in6 addrm;
addrm.sin6_family = AF_INET6;
addrm.sin6_port = htons(multiPortNo);
addrm.sin6_addr = in6addr_any;
if (::bind(sock, (sockaddr*) &addrm, sizeof(addrm)) < 0)
{
D_NWSTACK("can't bind multicast socket in SensorNetwork::openV6 error %s\n", strerror(errno));
return -1;
}
struct ipv6_mreq mreq;
mreq.ipv6mr_multiaddr = _multicastAddr->getIpAddress()->addr.ad6;
mreq.ipv6mr_interface = ifindex;
if (setsockopt(sock, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0)
{
D_NWSTACK("Multicast IPV6_ADD_MEMBERSHIP in SensorNetwork::openV6 error %s\n", strerror(errno));
return -1;
}
#ifdef DEBUG_NW
optval = 1;
#else
optval = 0;
#endif
if (setsockopt(sock, IPPROTO_IPV6, IPV6_MULTICAST_LOOP, &optval, sizeof(optval)) < 0)
{
D_NWSTACK("IPV6_MULTICAST_LOOP in SensorNetwork::openV6 error %s\n", strerror(errno));
return -1;
}
if (setsockopt(sock, IPPROTO_IPV6, IPV6_MULTICAST_HOPS, &hops, sizeof(hops)) < 0)
{
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);
return 0;
}
int SensorNetwork::multicastRecv(uint8_t *buf, uint16_t len)
{
int rc = -1;
#ifndef DTLS6
sockaddr_in sender;
socklen_t addrlen = sizeof(sender);
memset(&sender, 0, addrlen);
rc = ::recvfrom(_conns->getSockMulticast(), buf, len, 0, (sockaddr*) &sender, &addrlen);
if (rc < 0 && errno != EAGAIN)
{
D_NWSTACK("errno %s IPv4 in SensorNetwork::multicastRecv\n", strerror(errno));
return -1;
}
D_NWSTACK("IPv4 multicast recved from %s:%d length = %d\n", inet_ntoa(sender.sin_addr), ntohs(sender.sin_port), rc);
#else
sockaddr_in6 sender;
socklen_t addrlen = sizeof(sender);
memset(&sender, 0, addrlen);
rc = ::recvfrom(_conns->getSockMulticast(), buf, len, 0, (sockaddr*) &sender, &addrlen);
if (rc < 0 && errno != EAGAIN)
{
D_NWSTACK("errno = %d IPv6 in SensorNetwork::multicastRecv\n", errno);
return -1;
}
#ifdef DEBUG_NW
char buff[INET6_ADDRSTRLEN];
inet_ntop(AF_INET6, &sender.sin6_addr, buff, INET6_ADDRSTRLEN);
D_NWSTACK("IPv6 multicast recved from %s:%u length = %d\n", buff, ntohs(sender.sin6_port), rc);
#endif
#endif
return rc;
}
int SensorNetwork::getUnicastClient(SensorNetAddress *addr)
{
return getSenderAddress(_conns->getSockUnicast(), addr);
}
int SensorNetwork::getSendClient(int index, SensorNetAddress *addr)
{
return getSenderAddress(_conns->getSockClient(index), addr);
}
int SensorNetwork::getSenderAddress(int sock, SensorNetAddress *addr)
{
int len = -1;
#ifndef DTLS6
// AF_INET
sockaddr_in sender4 = { 0 };
socklen_t addrlen4 = sizeof(sender4);
char buf[16];
int rc = PACKET_OTHERS;
len = ::recvfrom(sock, buf, 15, MSG_PEEK, (sockaddr*) &sender4, &addrlen4);
if (len < 0 && errno != EAGAIN)
{
D_NWSTACK("errno = %d in UDPPort::getSender\n", errno);
return -1;
}
addr->setFamily(AF_INET);
addr->getIpAddress()->addr.ad4 = sender4.sin_addr;
D_NWSTACK("SensorNetwork::getSenderAddress recved from %s:%d length = %d\n", inet_ntoa(sender4.sin_addr),
ntohs(sender4.sin_port), len);
// if (len >= 13)
{
if (buf[0] == 22)
{
rc = PACKET_CLIENTHELLO;
}
else if (buf[0] == 23)
{
rc = PACKET_APPL;
}
D_NWSTACK("getSenderAddress len=%d Packet type=%d\n", len, buf[0]);
}
return rc;
#else
//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);
if (len < 0 && errno != EAGAIN)
{
D_NWSTACK("errno = %d in SensorNetwork::getSender\n", errno);
return len;
}
addr->setFamily(AF_INET6);
addr->setSockaddr6(&sender6);
#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);
#endif
#endif
return len;
}
void SensorNetwork::clearRecvData(int sock)
{
uint8_t buf[MQTTSNGW_MAX_PACKET_SIZE];
::recv(sock, buf, MQTTSNGW_MAX_PACKET_SIZE, 0);
}
int verify_cert(int ok, X509_STORE_CTX *ctx)
{
return 1;
}
int generate_cookie(SSL *ssl, unsigned char *cookie, unsigned int *cookie_len)
{
unsigned char *buf;
unsigned char result[EVP_MAX_MD_SIZE];
unsigned int len = 0;
unsigned int rsltlen;
union
{
struct sockaddr_storage ss;
struct sockaddr_in6 s6;
struct sockaddr_in s4;
} peer;
if (!cookie_initialized)
{
if (!RAND_bytes(cookie_secret, COOKIE_SECRET_LENGTH))
{
return 0;
}
cookie_initialized = 1;
}
(void) BIO_dgram_get_peer(SSL_get_rbio(ssl), &peer);
len = 0;
switch (peer.ss.ss_family)
{
case AF_INET:
len += sizeof(struct in_addr);
break;
case AF_INET6:
len += sizeof(struct in6_addr);
break;
default:
OPENSSL_assert(0);
break;
}
len += sizeof(in_port_t);
buf = (unsigned char*) OPENSSL_malloc(len);
switch (peer.ss.ss_family)
{
case AF_INET:
memcpy(buf, &peer.s4.sin_port, sizeof(in_port_t));
memcpy(buf + sizeof(peer.s4.sin_port), &peer.s4.sin_addr, sizeof(struct in_addr));
break;
case AF_INET6:
memcpy(buf, &peer.s6.sin6_port, sizeof(in_port_t));
memcpy(buf + sizeof(in_port_t), &peer.s6.sin6_addr, sizeof(struct in6_addr));
break;
default:
OPENSSL_assert(0);
break;
}
HMAC(EVP_sha1(), (const void*) cookie_secret, COOKIE_SECRET_LENGTH, (const unsigned char*) buf, len, result, &rsltlen);
OPENSSL_free(buf);
memcpy(cookie, result, rsltlen);
*cookie_len = rsltlen;
return 1;
}
int verify_cookie(SSL *ssl, const unsigned char *cookie, unsigned int cookie_len)
{
unsigned char *buf;
unsigned char result[EVP_MAX_MD_SIZE];
unsigned int len = 0;
unsigned int rsltlen;
union
{
struct sockaddr_storage ss;
struct sockaddr_in6 s6;
struct sockaddr_in s4;
} peer;
if (!cookie_initialized)
{
return 0;
}
(void) BIO_dgram_get_peer(SSL_get_rbio(ssl), &peer);
len = 0;
switch (peer.ss.ss_family)
{
case AF_INET:
len += sizeof(struct in_addr);
break;
case AF_INET6:
len += sizeof(struct in6_addr);
break;
default:
OPENSSL_assert(0);
break;
}
len += sizeof(in_port_t);
buf = (unsigned char*) OPENSSL_malloc(len);
switch (peer.ss.ss_family)
{
case AF_INET:
memcpy(buf, &peer.s4.sin_port, sizeof(in_port_t));
memcpy(buf + sizeof(in_port_t), &peer.s4.sin_addr, sizeof(struct in_addr));
break;
case AF_INET6:
memcpy(buf, &peer.s6.sin6_port, sizeof(in_port_t));
memcpy(buf + sizeof(in_port_t), &peer.s6.sin6_addr, sizeof(struct in6_addr));
break;
default:
OPENSSL_assert(0);
break;
}
HMAC(EVP_sha1(), (const void*) cookie_secret, COOKIE_SECRET_LENGTH, (const unsigned char*) buf, len, result, &rsltlen);
OPENSSL_free(buf);
if (cookie_len == rsltlen && memcmp(result, cookie, rsltlen) == 0)
{
return 1;
}
return 0;
}
Reference in New Issue View Git Blame Copy Permalink