/**
* Attempts to write a series of iovec buffers to a socket in *one* system call so that
* they are sent as one packet.
* @param socket the socket to write to
* @param iovecs an array of buffers to write
* @param count number of buffers in iovecs
* @param bytes number of bytes actually written returned
* @return completion code, especially TCPSOCKET_INTERRUPTED
*/
int Socket_writev(int socket, iobuf* iovecs, int count, unsigned long* bytes)
{
int rc;
FUNC_ENTRY;
#if defined(WIN32) || defined(WIN64)
rc = WSASend(socket, iovecs, count, (LPDWORD)bytes, 0, NULL, NULL);
if (rc == SOCKET_ERROR)
{
int err = Socket_error("WSASend - putdatas", socket);
if (err == EWOULDBLOCK || err == EAGAIN)
rc = TCPSOCKET_INTERRUPTED;
}
#else
*bytes = 0L;
rc = writev(socket, iovecs, count);
if (rc == SOCKET_ERROR)
{
int err = Socket_error("writev - putdatas", socket);
if (err == EWOULDBLOCK || err == EAGAIN)
rc = TCPSOCKET_INTERRUPTED;
}
else
*bytes = rc;
#endif
FUNC_EXIT_RC(rc);
return rc;
}
int read(unsigned char* buffer, int len, int timeout_ms)
{
struct timeval interval = {timeout_ms / 1000, (timeout_ms % 1000) * 1000};
if (interval.tv_sec < 0 || (interval.tv_sec == 0 && interval.tv_usec <= 0))
{
interval.tv_sec = 0;
interval.tv_usec = 100;
}
setsockopt(mysock, SOL_SOCKET, SO_RCVTIMEO, (char *)&interval, sizeof(struct timeval));
int bytes = 0;
while (bytes < len)
{
int rc = ::recv(mysock, &buffer[bytes], (size_t)(len - bytes), 0);
if (rc == -1)
{
if (Socket_error("read") != 0)
{
bytes = -1;
break;
}
}
else
bytes += rc;
}
return bytes;
}
/**
* Convert a numeric address to character string
* @param sa socket numerical address
* @param sock socket
* @return the peer information
*/
char* Socket_getaddrname(struct sockaddr* sa, int sock)
{
/**
* maximum length of the address string
*/
#define ADDRLEN INET6_ADDRSTRLEN+1
/**
* maximum length of the port string
*/
#define PORTLEN 10
static char addr_string[ADDRLEN + PORTLEN];
#if defined(WIN32) || defined(WIN64)
int buflen = ADDRLEN*2;
wchar_t buf[ADDRLEN*2];
if (WSAAddressToString(sa, sizeof(struct sockaddr_in6), NULL, buf, (LPDWORD)&buflen) == SOCKET_ERROR)
Socket_error("WSAAddressToString", sock);
else
wcstombs(addr_string, buf, sizeof(addr_string));
/* TODO: append the port information - format: [00:00:00::]:port */
/* strcpy(&addr_string[strlen(addr_string)], "what?"); */
#else
struct sockaddr_in *sin = (struct sockaddr_in *)sa;
inet_ntop(sin->sin_family, &sin->sin_addr, addr_string, ADDRLEN);
sprintf(&addr_string[strlen(addr_string)], ":%d", ntohs(sin->sin_port));
#endif
return addr_string;
}
/**
* Reads one byte from a socket
* @param socket the socket to read from
* @param c the character read, returned
* @return completion code
*/
int Socket_getch(int socket, char* c)
{
int rc = SOCKET_ERROR;
FUNC_ENTRY;
if ((rc = SocketBuffer_getQueuedChar(socket, c)) != SOCKETBUFFER_INTERRUPTED)
goto exit;
if ((rc = recv(socket, c, (size_t)1, 0)) == SOCKET_ERROR)
{
int err = Socket_error("recv - getch", socket);
if (err == EWOULDBLOCK || err == EAGAIN)
{
rc = TCPSOCKET_INTERRUPTED;
SocketBuffer_interrupted(socket, 0);
}
}
else if (rc == 0)
rc = SOCKET_ERROR; /* The return value from recv is 0 when the peer has performed an orderly shutdown. */
else if (rc == 1)
{
SocketBuffer_queueChar(socket, *c);
rc = TCPSOCKET_COMPLETE;
}
exit:
FUNC_EXIT_RC(rc);
return rc;
}
/**
return >=0 for a socket descriptor, <0 for an error code
*/
int lowlevel_open()
{
mysock = socket(AF_INET, SOCK_DGRAM, 0);
if (mysock == INVALID_SOCKET)
return Socket_error("socket", mysock);
return mysock;
}
int MQTTSPacket_send(const Clients *client, MQTTSHeader header, char* buffer, int buflen)
{
int rc = 0;
char *data = NULL;
uint8_t *ptr = NULL;
PacketBuffer buf;
FUNC_ENTRY;
if (header.len > 256)
{
header.len += 2;
buflen += 2;
data = malloc(header.len);
ptr = data;
*ptr++ = 0x01;
writeInt(&ptr, header.len);
}
else
{
data = malloc(header.len);
ptr = data;
*ptr++ = (uint8_t)header.len;
}
*ptr++ = header.type;
memcpy(ptr, buffer, buflen);
buf.data = data;
buf.len = buflen + 2;
char *colon ;
if ( client->wirelessNodeId != NULL )
{
buf = MQTTSPacketSerialize_forwarder_encapsulation(client , buf) ;
// Temporary shorten client->addr until the colon before wireless node ID
colon = strrchr(client->addr, ':');
*(colon) = '\0';
}
rc = MQTTSPacket_sendPacketBuffer( client->socket, client->addr, buf);
if ( client->wirelessNodeId != NULL )
*(colon) = ':';
if (rc == SOCKET_ERROR)
{
Socket_error("sendto", client->socket);
/* if (err == EWOULDBLOCK || err == EAGAIN)
rc = TCPSOCKET_INTERRUPTED;
*/
}
else
rc = 0;
free(buf.data);
FUNC_EXIT_RC(rc);
return rc;
}
/**
* Close a socket without removing it from the select list.
* @param socket the socket to close
* @return completion code
*/
int Socket_close_only(int socket)
{
int rc;
FUNC_ENTRY;
#if defined(WIN32) || defined(WIN64)
if (shutdown(socket, SD_BOTH) == SOCKET_ERROR)
Socket_error("shutdown", socket);
if ((rc = closesocket(socket)) == SOCKET_ERROR)
Socket_error("close", socket);
#else
if (shutdown(socket, SHUT_WR) == SOCKET_ERROR)
Socket_error("shutdown", socket);
if ((rc = recv(socket, NULL, (size_t)0, 0)) == SOCKET_ERROR)
Socket_error("shutdown", socket);
if ((rc = close(socket)) == SOCKET_ERROR)
Socket_error("close", socket);
#endif
FUNC_EXIT_RC(rc);
return rc;
}
int MQTTSPacket_sendPacketBuffer(int socket, char* addr, PacketBuffer buf)
{
char *port;
int rc = 0;
FUNC_ENTRY;
port = strrchr(addr, ':') + 1;
*(port - 1) = '\0';
if (strchr(addr, ':'))
{
struct sockaddr_in6 cliaddr6;
memset(&cliaddr6, '\0', sizeof(cliaddr6));
cliaddr6.sin6_family = AF_INET6;
if (inet_pton(cliaddr6.sin6_family, addr, &cliaddr6.sin6_addr) == 0)
Socket_error("inet_pton", socket);
cliaddr6.sin6_port = htons(atoi(port));
if ((rc = sendto(socket, buf.data, buf.len, 0, (const struct sockaddr*)&cliaddr6, sizeof(cliaddr6))) == SOCKET_ERROR)
Socket_error("sendto", socket);
else
rc = 0;
}
else
{
struct sockaddr_in cliaddr;
cliaddr.sin_family = AF_INET;
if (inet_pton(cliaddr.sin_family, addr, &cliaddr.sin_addr.s_addr) == 0)
Socket_error("inet_pton", socket);
cliaddr.sin_port = htons(atoi(port));
if ((rc = sendto(socket, buf.data, buf.len, 0, (const struct sockaddr*)&cliaddr, sizeof(cliaddr))) == SOCKET_ERROR)
Socket_error("sendto", socket);
else
rc = 0;
}
*(port - 1) = ':';
FUNC_EXIT_RC(rc);
return rc;
}
/**
* Get information about the other end connected to a socket
* @param sock the socket to inquire on
* @return the peer information
*/
char* Socket_getpeer(int sock)
{
struct sockaddr_in6 sa;
socklen_t sal = sizeof(sa);
int rc;
if ((rc = getpeername(sock, (struct sockaddr*)&sa, &sal)) == SOCKET_ERROR)
{
Socket_error("getpeername", sock);
return "unknown";
}
return Socket_getaddrname((struct sockaddr*)&sa, sock);
}
int lowlevel_sendPacketBuffer(char* host, int port, unsigned char* buf, int buflen)
{
struct sockaddr_in cliaddr;
int rc = 0;
memset(&cliaddr, 0, sizeof(cliaddr));
cliaddr.sin_family = AF_INET;
cliaddr.sin_addr.s_addr = inet_addr(host);
cliaddr.sin_port = htons(port);
if ((rc = sendto(mysock, buf, buflen, 0, (const struct sockaddr*)&cliaddr, sizeof(cliaddr))) == SOCKET_ERROR)
Socket_error("sendto", mysock);
else
rc = 0;
return rc;
}
int MQTTSPacket_send(int socket, char* addr, MQTTSHeader header, char* buffer, int buflen)
{
int rc = 0;
char *data = NULL;
char *ptr = NULL;
PacketBuffer buf;
FUNC_ENTRY;
if (header.len > 256)
{
header.len += 2;
buflen += 2;
data = malloc(header.len);
ptr = data;
*ptr++ = 0x01;
writeInt(&ptr, header.len);
}
else
{
data = malloc(header.len);
ptr = data;
*ptr++ = header.len;
}
*ptr++ = header.type;
memcpy(ptr, buffer, buflen);
buf.data = data;
buf.len = buflen + 2;
rc = MQTTSPacket_sendPacketBuffer(socket, addr, buf);
if (rc == SOCKET_ERROR)
{
Socket_error("sendto", socket);
/* if (err == EWOULDBLOCK || err == EAGAIN)
rc = TCPSOCKET_INTERRUPTED;
*/
}
else
rc = 0;
free(data);
FUNC_EXIT_RC(rc);
return rc;
}
/**
* Attempts to read a number of bytes from a socket, non-blocking. If a previous read did not
* finish, then retrieve that data.
* @param socket the socket to read from
* @param bytes the number of bytes to read
* @param actual_len the actual number of bytes read
* @return completion code
*/
char *Socket_getdata(int socket, int bytes, int* actual_len)
{
int rc;
char* buf;
FUNC_ENTRY;
if (bytes == 0)
{
buf = SocketBuffer_complete(socket);
goto exit;
}
buf = SocketBuffer_getQueuedData(socket, bytes, actual_len);
if ((rc = recv(socket, buf + (*actual_len), (size_t)(bytes - (*actual_len)), 0)) == SOCKET_ERROR)
{
rc = Socket_error("recv - getdata", socket);
if (rc != EAGAIN && rc != EWOULDBLOCK)
{
buf = NULL;
goto exit;
}
}
else if (rc == 0) /* rc 0 means the other end closed the socket, albeit "gracefully" */
{
buf = NULL;
goto exit;
}
else
*actual_len += rc;
if (*actual_len == bytes)
SocketBuffer_complete(socket);
else /* we didn't read the whole packet */
{
SocketBuffer_interrupted(socket, *actual_len);
Log(TRACE_MAX, -1, "%d bytes expected but %d bytes now received", bytes, *actual_len);
}
exit:
FUNC_EXIT;
return buf;
}
Clients* MQTTSProtocol_create_multicast(char* ip_address, char* clientID, int loopback)
{ /* outgoing connection */
int i, port, rc;
char* addr;
Clients* newc = NULL;
char* intface = NULL;
int ipv6 = 0;
FUNC_ENTRY;
newc = malloc(sizeof(Clients));
memset(newc, '\0', sizeof(Clients));
newc->clientID = clientID;
newc->outbound = 1;
newc->good = 1;
newc->connected = 1;
newc->outboundMsgs = ListInitialize();
newc->inboundMsgs = ListInitialize();
for (i = 0; i < PRIORITY_MAX; ++i)
newc->queuedMsgs[i] = ListInitialize();
newc->registrations = ListInitialize();
newc->protocol = PROTOCOL_MQTTS_MULTICAST;
/* if there is a space in the ip_address string, it means we have address plus interface specified */
if ((intface = strchr(ip_address, ' ')) != NULL)
{
*intface = '\0';
++intface;
}
addr = MQTTProtocol_addressPort(ip_address, &port);
newc->addr = malloc(strlen(ip_address) + 1);
strcpy(newc->addr, ip_address);
ipv6 = (newc->addr[0] == '[');
rc = Socket_new_udp(&(newc->socket), ipv6);
if (setsockopt(newc->socket, IPPROTO_IP, IP_MULTICAST_LOOP, (const char*)&loopback, sizeof(loopback)) == SOCKET_ERROR)
Socket_error("set bridge IP_MULTICAST_LOOP", newc->socket);
if (intface)
{
if (ipv6)
{
int index = 0;
if ((index = if_nametoindex(intface)) == 0)
Socket_error("get interface index", newc->socket);
else if (setsockopt(newc->socket, IPPROTO_IPV6, IPV6_MULTICAST_IF, (const char*)&index, sizeof(index)) == SOCKET_ERROR)
Socket_error("set bridge IP_MULTICAST_IF", newc->socket);
}
else
{
struct in_addr interface_addr;
#if defined(WIN32)
if ((rc = win_inet_pton(AF_INET, intface, &interface_addr)) == SOCKET_ERROR)
Socket_error("WSAStringToAddress interface", newc->socket);
else
{
#else
/* get address of the interface */
struct ifreq ifreq;
strncpy(ifreq.ifr_name, intface, IFNAMSIZ);
if (ioctl(newc->socket, SIOCGIFADDR, &ifreq) == SOCKET_ERROR)
Socket_error("get interface address", newc->socket);
else
{
memcpy(&interface_addr, &((struct sockaddr_in *)&ifreq.ifr_addr)->sin_addr, sizeof(struct in_addr));
#endif
if (setsockopt(newc->socket, IPPROTO_IP, IP_MULTICAST_IF, &interface_addr, sizeof(interface_addr)) == SOCKET_ERROR)
Socket_error("set bridge IP_MULTICAST_IF", newc->socket);
}
}
}
TreeAdd(bstate->disconnected_mqtts_clients, newc, sizeof(Clients) + strlen(newc->clientID)+1 + 3*sizeof(List));
FUNC_EXIT;
return newc;
}
Clients* MQTTSProtocol_connect(char* ip_address, char* clientID, int cleansession, int try_private, int keepalive, willMessages* willMessage)
{ /* outgoing connection */
int rc, port;
char* addr;
Clients* newc = NULL;
FUNC_ENTRY;
newc = TreeRemoveKeyIndex(bstate->disconnected_clients, clientID, 1); /* must be a dummy client */
if (newc == NULL)
newc = TreeRemoveKeyIndex(bstate->clients, clientID, 1);
if (newc)
{
free(clientID);
newc->connected = newc->ping_outstanding = newc->connect_state = newc->msgID = newc->discardedMsgs = 0;
}
else
{
int i;
newc = malloc(sizeof(Clients));
memset(newc, '\0', sizeof(Clients));
newc->outboundMsgs = ListInitialize();
newc->inboundMsgs = ListInitialize();
for (i = 0; i < PRIORITY_MAX; ++i)
newc->queuedMsgs[i] = ListInitialize();
newc->clientID = clientID;
}
newc->cleansession = cleansession;
newc->outbound = newc->good = 1;
newc->keepAliveInterval = keepalive;
newc->registrations = ListInitialize();
newc->will = willMessage;
newc->noLocal = try_private; /* try private connection first */
time(&(newc->lastContact));
newc->pendingRegistration = NULL;
newc->protocol = PROTOCOL_MQTTS;
addr = MQTTProtocol_addressPort(ip_address, &port);
newc->addr = malloc(strlen(ip_address));
strcpy(newc->addr, ip_address);
rc = Socket_new_type(addr, port, &(newc->socket), SOCK_DGRAM);
if (rc == EINPROGRESS || rc == EWOULDBLOCK)
newc->connect_state = 1; /* UDP connect called - improbable, but possible on obscure platforms */
else if (rc == 0)
{
newc->connect_state = 2;
rc = MQTTSPacket_send_connect(newc);
}
TreeAdd(bstate->clients, newc, sizeof(Clients) + strlen(newc->clientID)+1 + 3*sizeof(List));
FUNC_EXIT;
return newc;
}
void* MQTTSPacket_Factory(int sock, char** clientAddr, struct sockaddr* from, uint8_t** wlnid , uint8_t *wlnid_len , int* error)
{
static MQTTSHeader header;
void* pack = NULL;
/*struct sockaddr_in cliAddr;*/
int n;
char* data = msg;
socklen_t len = sizeof(struct sockaddr_in6);
*wlnid = NULL ;
*wlnid_len = 0 ;
FUNC_ENTRY;
/* #if !defined(NO_BRIDGE)
client = Protocol_getoutboundclient(sock);
FUNC_ENTRY;
if (client!=NULL)
n = recv(sock,msg,512,0);
else
#endif */
/* max message size from global parameters, as we lose the packet if we don't receive it. Default is
* 65535, so the parameter can be used to decrease the memory usage.
* The message memory area must be allocated on the heap so that this memory can be not allocated
* on reduced-memory systems.
*/
n = recvfrom(sock, msg, max_packet_size, 0, from, &len);
if (n == SOCKET_ERROR)
{
int en = Socket_error("UDP read error", sock);
if (en == EINVAL)
Log(LOG_WARNING, 0, "EINVAL");
*error = SOCKET_ERROR;
goto exit;
}
*clientAddr = Socket_getaddrname(from, sock);
/*
printf("%d bytes of data on socket %d from %s\n",n,sock,*clientAddr);
if (n>0) {
for (i=0;i<n;i++) {
printf("%d ",msg[i]);
}
printf("\n");
}
*/
*error = SOCKET_ERROR; // indicate whether an error occurred, or not
if (n < 2)
goto exit;
data = MQTTSPacket_parse_header( &header, data ) ;
/* In case of Forwarder Encapsulation packet, Length: 1-octet long, specifies the number of octets up to the end
* of the “Wireless Node Id” field (incl. the Length octet itself). Length does not include length of payload
* (encapsulated MQTT-SN message itself).
*/
if (header.type != MQTTS_FRWDENCAP && header.len != n)
{
*error = UDPSOCKET_INCOMPLETE;
goto exit;
}
else
{
// Forwarder Encapsulation packet. Extract Wireless Node Id and MQTT-SN message
if ( header.type == MQTTS_FRWDENCAP )
{
// Skip Crt(1) field
data++ ;
// Wireless Node Id
*wlnid = data ;
// Wireless Node Id length is packet length - 3 octet (Length(1) + MsgType(1) + Crt(1))
*wlnid_len = header.len - 3 ;
data += *wlnid_len ;
// Read encapsulated packet and set header and shift data to beginning of payload
data = MQTTSPacket_parse_header( &header, data ) ;
}
uint8_t ptype = header.type;
if (ptype < MQTTS_ADVERTISE || ptype > MQTTS_WILLMSGRESP || new_mqtts_packets[ptype] == NULL)
Log(TRACE_MAX, 17, NULL, ptype);
else if ((pack = (*new_mqtts_packets[ptype])(header, data)) == NULL)
*error = BAD_MQTTS_PACKET;
}
exit:
FUNC_EXIT_RC(*error);
return pack;
}
/**
* Fully initialize the socket module for in and outbound communications.
* In single-listener mode.
* @param anAddress IP address string
* @param port number
* @param ipv6 flag to indicate ipv6 should be used
* @return completion code
*/
int Socket_initialize(char* anAddress, int aPort, int ipv6)
{
int flag = 1;
int rc = SOCKET_ERROR;
int ipv4 = 1; /* yes we can drop down to ipv4 */
FUNC_ENTRY;
s.port = aPort;
if (anAddress == NULL || strcmp(anAddress, "INADDR_ANY") == 0)
{
s.addr.sin_addr.s_addr = htonl(INADDR_ANY);
s.addr6.sin6_addr = in6addr_any;
}
else
{
if (anAddress[0] == '[')
{
int changed = ipv6_format(anAddress);
inet_pton(AF_INET6, &anAddress[1], &(s.addr6.sin6_addr));
ipv4 = 0;
if (changed)
(anAddress)[changed] = ']';
}
else
{
inet_pton(AF_INET, anAddress, &(s.addr.sin_addr.s_addr));
ipv6 = 0;
}
}
Socket_outInitialize();
s.mySocket = -1;
if (ipv6)
s.mySocket = socket(AF_INET6, SOCK_STREAM, 0);
if (s.mySocket < 0 && ipv4)
{
s.mySocket = socket(AF_INET, SOCK_STREAM, 0);
ipv6 = 0;
}
s.maxfdp1 = 0;
Log(TRACE_MAX, 6, NULL, FD_SETSIZE);
if (s.mySocket < 0)
{
Socket_error("socket", s.mySocket);
Log(LOG_WARNING, 77, NULL);
rc = s.mySocket;
goto exit;
}
#if !defined(WIN32)
if (setsockopt(s.mySocket, SOL_SOCKET, SO_REUSEADDR, (const char*)&flag, sizeof(int)) != 0)
Log(LOG_WARNING, 109, NULL, aPort);
#endif
if (ipv6)
{
s.addr6.sin6_family = AF_INET6;
s.addr6.sin6_port = htons(s.port);
rc = bind(s.mySocket, (struct sockaddr *)&(s.addr6), sizeof(s.addr6));
}
else
{
s.addr.sin_family = AF_INET;
s.addr.sin_port = htons(s.port);
memset(s.addr.sin_zero, 0, sizeof(s.addr.sin_zero));
rc = bind(s.mySocket, (struct sockaddr *)&(s.addr), sizeof(s.addr));
}
if (rc == SOCKET_ERROR)
{
Socket_error("bind", s.mySocket);
Log(LOG_WARNING, 78, NULL, aPort);
goto exit;
}
if (listen(s.mySocket, SOMAXCONN) == SOCKET_ERROR) /* second parm is max no of connections */
{
Socket_error("listen", s.mySocket);
Log(LOG_WARNING, 79, NULL, aPort);
goto exit;
}
if (Socket_setnonblocking(s.mySocket) == SOCKET_ERROR)
{
Socket_error("setnonblocking", s.mySocket);
goto exit;
}
FD_SET((u_int)s.mySocket, &(s.rset)); /* Add the current socket descriptor */
s.maxfdp1 = s.mySocket + 1;
memcpy((void*)&(s.rset_saved), (void*)&(s.rset), sizeof(s.rset_saved));
rc = 0;
exit:
FUNC_EXIT_RC(rc);
return rc;
}
/**
* Create the server socket in multi-listener mode.
* @param list pointer to a listener structure
* @return completion code
*/
int Socket_addServerSocket(Listener* list)
{
int flag = 1;
int rc = SOCKET_ERROR;
int ipv4 = 1; /* yes we can drop down to ipv4 */
FUNC_ENTRY;
if (!list->address || strcmp(list->address, "INADDR_ANY") == 0)
{
struct in6_addr in6addr_any = IN6ADDR_ANY_INIT;
list->addr.sin_addr.s_addr = htonl(INADDR_ANY);
list->addr6.sin6_addr = in6addr_any;
}
else
{
if (list->address[0] == '[')
{
int changed = ipv6_format(list->address);
#if defined(WIN32)
/*wchar_t buf[(INET6_ADDRSTRLEN+1)*2];
int buflen = sizeof(list->addr6);
mbstowcs(buf, &list->address[1], sizeof(buf));
rc = WSAStringToAddress(buf, AF_INET6, NULL, (struct sockaddr*)&list->addr6, &buflen);*/
rc = win_inet_pton(AF_INET6, &list->address[1], &(list->addr6.sin6_addr));
#else
rc = inet_pton(AF_INET6, &list->address[1], &(list->addr6.sin6_addr));
#endif
ipv4 = 0;
if (changed)
(list->address)[changed] = ']';
}
else
{
#if defined(WIN32)
/*wchar_t buf[(INET6_ADDRSTRLEN+1)*2];
int buflen = sizeof(list->addr);
mbstowcs(buf, list->address, sizeof(buf));
rc = WSAStringToAddress(buf, AF_INET, NULL, (struct sockaddr*)&list->addr, &buflen);*/
rc = win_inet_pton(AF_INET, list->address, &(list->addr.sin_addr.s_addr));
#else
rc = inet_pton(AF_INET, list->address, &(list->addr.sin_addr.s_addr));
#endif
list->ipv6 = 0;
}
#if defined(WIN32)
if (rc != 0)
#else
if (rc != 1)
#endif
{
Log(LOG_WARNING, 67, NULL, list->address);
rc = -1;
goto exit;
}
}
list->socket = -1;
if (list->protocol == PROTOCOL_MQTT)
{
if (list->ipv6)
list->socket = socket(AF_INET6, SOCK_STREAM, 0);
if (list->socket < 0 && ipv4)
{
list->socket = socket(AF_INET, SOCK_STREAM, 0);
list->ipv6 = 0;
}
}
#if defined(MQTTS)
else if (list->protocol == PROTOCOL_MQTTS)
{
if (list->ipv6)
list->socket = socket(AF_INET6, SOCK_DGRAM, 0);
if (list->socket < 0 && ipv4)
{
list->socket = socket(AF_INET, SOCK_DGRAM, 0);
list->ipv6 = 0;
}
}
#endif
Log(TRACE_MAX, 6, NULL, FD_SETSIZE);
if (list->socket < 0)
{
Socket_error("socket", list->socket);
Log(LOG_WARNING, 77, NULL);
rc = list->socket;
goto exit;
}
#if !defined(WIN32)
if (setsockopt(list->socket, SOL_SOCKET, SO_REUSEADDR, (const char*)&flag, sizeof(int)) != 0)
Log(LOG_WARNING, 109, NULL, list->port);
#endif
if (list->ipv6)
{
list->addr6.sin6_family = AF_INET6;
list->addr6.sin6_port = htons(list->port);
rc = bind(list->socket, (struct sockaddr *)&(list->addr6), sizeof(list->addr6));
}
else
{
list->addr.sin_family = AF_INET;
list->addr.sin_port = htons(list->port);
memset(list->addr.sin_zero, 0, sizeof(list->addr.sin_zero));
rc = bind(list->socket, (struct sockaddr *)&(list->addr), sizeof(list->addr));
}
if (rc == SOCKET_ERROR)
{
Socket_error("bind", list->socket);
Log(LOG_WARNING, 78, NULL, list->port);
goto exit;
}
/* Only listen if this is mqtt/tcp */
if (list->protocol == PROTOCOL_MQTT &&
listen(list->socket, SOMAXCONN) == SOCKET_ERROR) /* second parm is max no of connections */
{
Socket_error("listen", list->socket);
Log(LOG_WARNING, 79, NULL, list->port);
goto exit;
}
if (Socket_setnonblocking(list->socket) == SOCKET_ERROR)
{
Socket_error("setnonblocking", list->socket);
goto exit;
}
#if defined(MQTTS)
/* If mqtts/udp, add to the list of clientsds to test for reading */
if (list->protocol == PROTOCOL_MQTTS)
{
ListElement* current = NULL;
int loopback = list->loopback;
#if !defined(USE_POLL)
int* pnewSd = (int*)malloc(sizeof(list->socket));
*pnewSd = list->socket;
ListAppend(s.clientsds, pnewSd, sizeof(list->socket));
#endif
Log(LOG_INFO, 300, NULL, list->port);
if (setsockopt(list->socket, IPPROTO_IP, IP_MULTICAST_LOOP, (const char*)&loopback, sizeof(loopback)) == SOCKET_ERROR)
Socket_error("set listener IP_MULTICAST_LOOP", list->socket);
/* join any multicast groups */
while (ListNextElement(list->multicast_groups, ¤t))
Socket_joinMulticastGroup(list->socket, list->ipv6, (char*)(current->content));
}
else
#endif
Log(LOG_INFO, 14, NULL, list->port);
#if defined(USE_POLL)
/* add new socket to the epoll descriptor with epoll_ctl */
struct socket_info* si = malloc(sizeof(struct socket_info));
si->listener = list;
si->fd = list->socket;
si->connect_pending = 0;
si->event.events = EPOLLIN;
si->event.data.ptr = si;
TreeAdd(s.fds_tree, si, sizeof(struct socket_info));
if (epoll_ctl(s.epoll_fds, EPOLL_CTL_ADD, list->socket, &si->event) != 0)
Socket_error("epoll_ctl add", list->socket);
#else
FD_SET((u_int)list->socket, &(s.rset)); /* Add the current socket descriptor */
s.maxfdp1 = max(s.maxfdp1+1, list->socket+1);
memcpy((void*)&(s.rset_saved), (void*)&(s.rset), sizeof(s.rset_saved));
#endif
rc = 0;
exit:
FUNC_EXIT_RC(rc);
return rc;
}
int Socket_joinMulticastGroup(int sock, int ipv6, char* mcast)
{
int rc = 0;
char* mcast_addr = mcast;
char* mcast_interface = NULL;
/* if there is a space in the mcast string, it means we have address plus interface specified */
if ((mcast_interface = strchr(mcast, ' ')) != NULL)
{
*mcast_interface = '\0';
++mcast_interface;
}
else
mcast_interface = "INADDR_ANY";
if (ipv6)
{
struct ipv6_mreq mreq6;
#if defined(WIN32)
if ((rc = win_inet_pton(AF_INET6, mcast_addr, &mreq6.ipv6mr_multiaddr.s6_addr)) == SOCKET_ERROR)
Socket_error("WSAStringToAddress", sock);
#else
if ((rc = inet_pton(AF_INET6, mcast_addr, &mreq6.ipv6mr_multiaddr.s6_addr)) != 1)
{
if (rc == 0)
Log(LOG_WARNING, 67, NULL, mcast_addr);
else if (rc == SOCKET_ERROR)
Socket_error("inet_pton", sock);
}
#endif
if (strcmp(mcast_interface, "INADDR_ANY") == 0)
mreq6.ipv6mr_interface = 0;
else
{
#if defined(WIN32)
/* on Windows, the if_nametoindex function requires Vista, i.e. will not work on XP */
/* Alternative functions are not short running. So for now, we can take an interface number */
mreq6.ipv6mr_interface = atoi(mcast_interface);
#else
mreq6.ipv6mr_interface = if_nametoindex(mcast_interface);
#endif
if (mreq6.ipv6mr_interface == 0)
Log(LOG_WARNING, 302, NULL, mcast_interface);
}
rc = setsockopt(sock, IPPROTO_IPV6, IPV6_JOIN_GROUP, (const char*)&mreq6, sizeof(mreq6));
}
else
{
struct ip_mreq mreq;
#if defined(WIN32)
if ((rc = win_inet_pton(AF_INET, mcast_addr, &mreq.imr_multiaddr.s_addr)) == SOCKET_ERROR)
Socket_error("WSAStringToAddress", sock);
#else
if ((rc = inet_pton(AF_INET, mcast_addr, &mreq.imr_multiaddr.s_addr)) != 1)
{
if (rc == 0)
Log(LOG_WARNING, 67, NULL, mcast_addr);
else if (rc == -1)
Socket_error("inet_pton", sock);
}
#endif
if (strcmp(mcast_interface, "INADDR_ANY") == 0)
mreq.imr_interface.s_addr = htonl(INADDR_ANY);
else
{
/* find an address corresponding to the named interface */
#if defined(WIN32)
/* on Windows, the GetAdaptersAddresses is suggested as a method for getting an address for an adapter.
However that function is not short running. So for now, we can just take an address */
if ((rc = win_inet_pton(AF_INET, mcast_interface, &mreq.imr_interface.s_addr)) == SOCKET_ERROR)
Socket_error("WSAStringToAddress interface", sock);
//mreq.imr_interface.s_addr = inet_addr(mcast_interface); /* can use inet_addr as this code is IPv4 specific */
#else
struct ifreq ifreq;
strncpy(ifreq.ifr_name, mcast_interface, IFNAMSIZ);
if (ioctl(sock, SIOCGIFADDR, &ifreq) >= 0)
memcpy(&mreq.imr_interface,
&((struct sockaddr_in *) &ifreq.ifr_addr)->sin_addr,
sizeof(struct in_addr));
else
Socket_error("ioctl SIOCGIFADDR", sock);
#endif
}
rc = setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, (const char*)&mreq, sizeof(mreq));
}
Log(LOG_INFO, 303, NULL, mcast_addr, mcast_interface);
if (rc != 0)
Socket_error("add to multicast group", sock);
return rc;
}
void* MQTTSPacket_Factory(int sock, char** clientAddr, struct sockaddr* from, int* error)
{
static MQTTSHeader header;
int ptype;
void* pack = NULL;
/*struct sockaddr_in cliAddr;*/
int n;
char* data = &msg[0];
socklen_t len = sizeof(struct sockaddr_in6);
FUNC_ENTRY;
/* #if !defined(NO_BRIDGE)
client = Protocol_getoutboundclient(sock);
FUNC_ENTRY;
if (client!=NULL)
n = recv(sock,msg,512,0);
else
#endif */
/* max message size from global parameters, as we lose the packet if we don't receive it. Default is
* 65535, so the parameter can be used to decrease the memory usage.
* The message memory area must be allocated on the heap so that this memory can be not allocated
* on reduced-memory systems.
*/
n = recvfrom(sock, msg, max_packet_size, 0, from, &len);
if (n == SOCKET_ERROR)
{
int en = Socket_error("UDP read error", sock);
if (en == EINVAL)
Log(LOG_WARNING, 0, "EINVAL");
*error = SOCKET_ERROR;
goto exit;
}
*clientAddr = Socket_getaddrname(from, sock);
/*
printf("%d bytes of data on socket %d from %s\n",n,sock,*clientAddr);
if (n>0) {
for (i=0;i<n;i++) {
printf("%d ",msg[i]);
}
printf("\n");
}
*/
*error = SOCKET_ERROR; // indicate whether an error occurred, or not
if (n < 2)
goto exit;
if (msg[0] == 1)
{
++data;
header.len = readInt(&data);
}
else
header.len = *(unsigned char*)data++;
header.type = *data++;
//printf("header.type is %d, header.len is %d, n is %d\n", header.type, header.len, n);
if (header.len != n)
{
*error = UDPSOCKET_INCOMPLETE;
goto exit;
}
else
{
ptype = header.type;
if (ptype < MQTTS_ADVERTISE || ptype > MQTTS_WILLMSGRESP || new_mqtts_packets[ptype] == NULL)
Log(TRACE_MAX, 17, NULL, ptype);
else if ((pack = (*new_mqtts_packets[ptype])(header, data)) == NULL)
*error = BAD_MQTTS_PACKET;
}
exit:
FUNC_EXIT_RC(*error);
return pack;
}
/**
* Returns the next socket ready for communications as indicated by select
* @param more_work flag to indicate more work is waiting, and thus a timeout value of 0 should
* be used for the select
* @param tp the timeout to be used for the select, unless overridden
* @return the socket next ready, or 0 if none is ready
*/
int Socket_getReadySocket(int more_work, struct timeval *tp)
{
int rc = 0;
static struct timeval zero = {0L, 0L}; /* 0 seconds */
static struct timeval one = {1L, 0L}; /* 1 second */
struct timeval timeout = one;
FUNC_ENTRY;
if (s.clientsds->count == 0)
goto exit;
if (more_work)
timeout = zero;
else if (tp)
timeout = *tp;
while (s.cur_clientsds != NULL)
{
if (isReady(*((int*)(s.cur_clientsds->content)), &(s.rset), &wset))
break;
ListNextElement(s.clientsds, &s.cur_clientsds);
}
if (s.cur_clientsds == NULL)
{
int rc1;
fd_set pwset;
memcpy((void*)&(s.rset), (void*)&(s.rset_saved), sizeof(s.rset));
memcpy((void*)&(pwset), (void*)&(s.pending_wset), sizeof(pwset));
if ((rc = select(s.maxfdp1, &(s.rset), &pwset, NULL, &timeout)) == SOCKET_ERROR)
{
Socket_error("read select", 0);
goto exit;
}
Log(TRACE_MAX, -1, "Return code %d from read select", rc);
if (Socket_continueWrites(&pwset) == SOCKET_ERROR)
{
rc = 0;
goto exit;
}
memcpy((void*)&wset, (void*)&(s.rset_saved), sizeof(wset));
if ((rc1 = select(s.maxfdp1, NULL, &(wset), NULL, &zero)) == SOCKET_ERROR)
{
Socket_error("write select", 0);
rc = rc1;
goto exit;
}
Log(TRACE_MAX, -1, "Return code %d from write select", rc1);
if (rc == 0 && rc1 == 0)
goto exit; /* no work to do */
s.cur_clientsds = s.clientsds->first;
while (s.cur_clientsds != NULL)
{
int cursock = *((int*)(s.cur_clientsds->content));
if (isReady(cursock, &(s.rset), &wset))
break;
ListNextElement(s.clientsds, &s.cur_clientsds);
}
}
if (s.cur_clientsds == NULL)
rc = 0;
else
{
rc = *((int*)(s.cur_clientsds->content));
ListNextElement(s.clientsds, &s.cur_clientsds);
}
exit:
FUNC_EXIT_RC(rc);
return rc;
} /* end getReadySocket */
/**
* Create a new socket and TCP connect to an address/port
* @param addr the address string
* @param port the TCP port
* @param sock returns the new socket
* @return completion code
*/
int Socket_new(char* addr, int port, int* sock)
{
int type = SOCK_STREAM;
struct sockaddr_in address;
#if defined(AF_INET6)
struct sockaddr_in6 address6;
#endif
int rc = SOCKET_ERROR;
#if defined(WIN32) || defined(WIN64)
short family;
#else
sa_family_t family = AF_INET;
#endif
struct addrinfo *result = NULL;
struct addrinfo hints = {0, AF_UNSPEC, SOCK_STREAM, IPPROTO_TCP, 0, NULL, NULL, NULL};
FUNC_ENTRY;
*sock = -1;
if (addr[0] == '[')
++addr;
if ((rc = getaddrinfo(addr, NULL, &hints, &result)) == 0)
{
struct addrinfo* res = result;
/* prefer ip4 addresses */
while (res)
{
if (res->ai_family == AF_INET)
{
result = res;
break;
}
res = res->ai_next;
}
if (result == NULL)
rc = -1;
else
#if defined(AF_INET6)
if (result->ai_family == AF_INET6)
{
address6.sin6_port = htons(port);
address6.sin6_family = family = AF_INET6;
address6.sin6_addr = ((struct sockaddr_in6*)(result->ai_addr))->sin6_addr;
}
else
#endif
if (result->ai_family == AF_INET)
{
address.sin_port = htons(port);
address.sin_family = family = AF_INET;
address.sin_addr = ((struct sockaddr_in*)(result->ai_addr))->sin_addr;
}
else
rc = -1;
freeaddrinfo(result);
}
else
Log(TRACE_MIN, -1, "getaddrinfo failed for addr %s with rc %d", addr, rc);
if (rc != 0)
Log(LOG_ERROR, -1, "%s is not a valid IP address", addr);
else
{
*sock = socket(family, type, 0);
if (*sock == INVALID_SOCKET)
rc = Socket_error("socket", *sock);
else
{
#if defined(NOSIGPIPE)
int opt = 1;
if (setsockopt(*sock, SOL_SOCKET, SO_NOSIGPIPE, (void*)&opt, sizeof(opt)) != 0)
Log(TRACE_MIN, -1, "Could not set SO_NOSIGPIPE for socket %d", *sock);
#endif
Log(TRACE_MIN, -1, "New socket %d for %s, port %d", *sock, addr, port);
if (Socket_addSocket(*sock) == SOCKET_ERROR)
rc = Socket_error("setnonblocking", *sock);
else
{
/* this could complete immmediately, even though we are non-blocking */
if (family == AF_INET)
rc = connect(*sock, (struct sockaddr*)&address, sizeof(address));
#if defined(AF_INET6)
else
rc = connect(*sock, (struct sockaddr*)&address6, sizeof(address6));
#endif
if (rc == SOCKET_ERROR)
rc = Socket_error("connect", *sock);
if (rc == EINPROGRESS || rc == EWOULDBLOCK)
{
int* pnewSd = (int*)malloc(sizeof(int));
*pnewSd = *sock;
ListAppend(s.connect_pending, pnewSd, sizeof(int));
Log(TRACE_MIN, 15, "Connect pending");
}
}
}
}
FUNC_EXIT_RC(rc);
return rc;
}
