udp_server.h

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia (IIT)
 *  Copyright (c) 2014 Norwegian University of Science and Technology
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Norwegian University of Science and
 *     Technology, nor the names of its contributors may be used to
 *     endorse or promote products derived from this software without
 *     specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

#ifndef IDJL_RSI_UDP_SERVER
#define IDJL_RSI_UDP_SERVER

#include <stdexcept>
#include <string>

#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <cstring>

// Socket includes

// For info about the differences between Winsock and standard posix sockets, see :
// * https://stackoverflow.com/questions/28027937/cross-platform-sockets
// * https://docs.microsoft.com/en-us/windows/desktop/winsock/porting-socket-applications-to-winsock

#ifdef _WIN32
#include <winsock2.h>
#else
#ifdef __VXWORKS__
#include <sys/times.h> // for struct timeval
#else
#include <sys/time.h>
#endif

#include <arpa/inet.h>
#include <netdb.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>
#endif

#ifdef __VXWORKS__
#include <selectLib.h>
#include <sockLib.h>
#endif

#include <stdio.h>

#define RSI_BUFSIZE 1024

#if !defined(_WIN32)
#define INVALID_SOCKET -1
#endif


namespace IDJL {

/**
 * Simple UDP server for the RSI driver.
 *
 * The implemented server  works on VxWorks, Windows and Linux
 * and listens by default on all available network interfaces.
 */
class UDPRSIServer
{
public:
    UDPRSIServer(unsigned short serverPort)
        : m_serverPort(serverPort)
        , timeout_(false)
    {
    }

    bool init()
    {
        // On Windows, start the winsocket interface
#ifdef _WIN32
        WSADATA wsa_data;
        WSAStartup(MAKEWORD(1, 1), &wsa_data);
#endif

        sockfd_ = socket(AF_INET, SOCK_DGRAM, 0);
        if (sockfd_ == INVALID_SOCKET) {
            fprintf(stderr, "UDPRSIServer Error opening socket: %s \n", strerror(errno));
            return false;
        }
        optval = 1;
#if defined(__VXWORKS__) || defined(_WIN32)
        setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&optval, sizeof(int));
#else
        setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (const void*)&optval, sizeof(int));
#endif
        memset(&serveraddr_, 0, sizeof(serveraddr_));
        serveraddr_.sin_family = AF_INET;
#ifdef __VXWORKS__
        serveraddr_.sin_addr.s_addr = htonl(INADDR_ANY);
#else
        serveraddr_.sin_addr.s_addr = htonl(INADDR_ANY);
#endif
        serveraddr_.sin_port = htons(m_serverPort);
        if (bind(sockfd_, (struct sockaddr*)&serveraddr_, sizeof(serveraddr_)) < 0) {
            fprintf(stderr, "UDPRSIServer: Error binding socket: %s \n", strerror(errno));
            return false;
        }
        clientlen_ = sizeof(clientaddr_);
        return true;
    }

    ~UDPRSIServer()
    {
#if defined(_WIN32)
        closesocket(sockfd_);
        WSACleanup();
#else
        close(sockfd_);
#endif
    }

    bool set_timeout(int millisecs)
    {
        if (millisecs != 0) {
            tv_.tv_sec = millisecs / 1000;
            tv_.tv_usec = (millisecs % 1000) * 1000;
            timeout_ = true;
            return timeout_;
        } else {
            return timeout_;
        }
    }

    long int send(std::string& buffer)
    {
        long int bytes = 0;
        // memcpy(write_buffer_, buffer.data(), buffer.size());
#if defined(__VXWORKS__) || defined(_WIN32)
        bytes = sendto(sockfd_,
                       const_cast<char*>(buffer.data()),
                       buffer.size(),
                       0,
                       (struct sockaddr*)&clientaddr_,
                       clientlen_);
#else
        bytes = sendto(
            sockfd_, buffer.data(), buffer.size(), 0, (struct sockaddr*)&clientaddr_, clientlen_);
#endif
        if (bytes < 0) {
            fprintf(stderr, "UDPRSIServer: ERROR in sendto, bytes: %ld\n", bytes);
        }

        return bytes;
    }

    long int recv(std::string& buffer)
    {
        long int bytes = 0;

        if (timeout_) {
            fd_set read_fds;
            FD_ZERO(&read_fds);
            FD_SET(sockfd_, &read_fds);

            struct timeval tv;
            tv.tv_sec = tv_.tv_sec;
            tv.tv_usec = tv_.tv_usec;

            if (select(sockfd_ + 1, &read_fds, NULL, NULL, &tv) < 0) {
                return 0;
            }

            if (FD_ISSET(sockfd_, &read_fds)) {
                // memset(buffer_, 0, RSI_BUFSIZE);
#if defined(__VXWORKS__) || defined(_WIN32)
                bytes = recvfrom(sockfd_,
                                 buffer_,
                                 RSI_BUFSIZE,
                                 0,
                                 (struct sockaddr*)&clientaddr_,
                                 reinterpret_cast<int*>(&clientlen_));
#else
                bytes = recvfrom(
                    sockfd_, buffer_, RSI_BUFSIZE, 0, (struct sockaddr*)&clientaddr_, &clientlen_);
#endif
                if (bytes < 0) {
                    fprintf(stderr, "UDPRSIServer: ERROR in sendto, bytes: %ld\n", bytes);
                }
                // Transform the buffer in a 0-terminated string
                buffer_[bytes] = 0;
            } else {
                return 0;
            }

        } else {
            // memset(buffer_, 0, RSI_BUFSIZE);
#if defined(__VXWORKS__) || defined(_WIN32)
            bytes = recvfrom(sockfd_,
                             buffer_,
                             RSI_BUFSIZE,
                             0,
                             (struct sockaddr*)&clientaddr_,
                             reinterpret_cast<int*>(&clientlen_));
#else
            bytes = recvfrom(
                sockfd_, buffer_, RSI_BUFSIZE, 0, (struct sockaddr*)&clientaddr_, &clientlen_);
#endif
            if (bytes < 0) {
                fprintf(stderr, "UDPRSIServer: ERROR in sendto, bytes: %ld\n", bytes);
            }

            // Transform the buffer in a 0-terminated string
            buffer_[bytes] = 0;
        }

        buffer = std::string(buffer_);

        return bytes;
    }

private:
    unsigned short m_serverPort;
    bool timeout_;
    struct timeval tv_;

    unsigned int sockfd_;
    unsigned int clientlen_;
    struct sockaddr_in serveraddr_;
    struct sockaddr_in clientaddr_;
    char buffer_[RSI_BUFSIZE];
    char write_buffer_[RSI_BUFSIZE];
    int optval;
};
} // namespace IDJL

#endif