io.cpp

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  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 Willow Garage, Inc. 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.
 */
/*****************************************************************************
** Includes
*****************************************************************************/
 
#include "config.h"
 
#include <ros/io.h>
#include <ros/assert.h> // don't need if we dont call the pipe functions.
#include <ros/file_log.h>
#include <errno.h> // for EFAULT and co.
#include <iostream>
#include <sstream>
#ifdef WIN32
#else
  #include <cstring> // strerror
  #include <fcntl.h> // for non-blocking configuration
#endif
 
#ifdef HAVE_EPOLL
  #include <sys/epoll.h>
#endif
 
/*****************************************************************************
** Macros
*****************************************************************************/
 
#define UNUSED(expr) do { (void)(expr); } while (0)
 
/*****************************************************************************
** Namespaces
*****************************************************************************/
 
namespace ros {
 
int last_socket_error() {
        #ifdef WIN32
                return WSAGetLastError();
        #else
                return errno;
        #endif
}
 
const char* socket_error_string(int err) {
#ifdef WIN32
    // could fix this to use FORMAT_MESSAGE and print a real string later,
    // but not high priority.
    std::stringstream ostream;
    ostream << "WSA Error: " << err;
    return ostream.str().c_str();
#else
    return strerror(err);
#endif
}
 
const char* last_socket_error_string() {
        #ifdef WIN32
        return socket_error_string(WSAGetLastError());
        #else
        return socket_error_string(errno);
        #endif
}
 
bool last_socket_error_is_would_block() {
#if defined(WIN32)
        if ( WSAGetLastError() == WSAEWOULDBLOCK ) {
                return true;
        } else {
                return false;
        }
#else
        if ( ( errno == EAGAIN  ) || ( errno == EWOULDBLOCK ) ) { // posix permits either
                return true;
        } else {
                return false;
        }
#endif
}
 
int create_socket_watcher() {
  int epfd = -1;
#if defined(HAVE_EPOLL)
  epfd = ::epoll_create1(0);
  if (epfd < 0)
  {
    ROS_ERROR("Unable to create epoll watcher: %s", strerror(errno));
  }
#endif
  return epfd;
}
 
void close_socket_watcher(int fd) {
  if (fd >= 0)
    ::close(fd);
}
 
void add_socket_to_watcher(int epfd, int fd) {
#if defined(HAVE_EPOLL)
  struct epoll_event ev;
  bzero(&ev, sizeof(ev));
 
  ev.events = 0;
  ev.data.fd = fd;
 
  if (::epoll_ctl(epfd, EPOLL_CTL_ADD, fd, &ev))
  {
    ROS_ERROR("Unable to add FD to epoll: %s", strerror(errno));
  }
#else
  UNUSED(epfd);
  UNUSED(fd);
#endif
}
 
void del_socket_from_watcher(int epfd, int fd) {
#if defined(HAVE_EPOLL)
  if (::epoll_ctl(epfd, EPOLL_CTL_DEL, fd, NULL))
  {
    ROS_ERROR("Unable to remove FD to epoll: %s", strerror(errno));
  }
#else
  UNUSED(epfd);
  UNUSED(fd);
#endif
}
 
void set_events_on_socket(int epfd, int fd, int events) {
#if defined(HAVE_EPOLL)
  struct epoll_event ev;
  bzero(&ev, sizeof(ev));
 
  ev.events = events;
  ev.data.fd = fd;
  if (::epoll_ctl(epfd, EPOLL_CTL_MOD, fd, &ev))
  {
    ROS_ERROR("Unable to modify FD epoll: %s", strerror(errno));
  }
#else
  UNUSED(epfd);
  UNUSED(fd);
  UNUSED(events);
#endif
}
 
 
 
/*****************************************************************************
** Service Robotics/Libssh Functions
*****************************************************************************/
pollfd_vector_ptr poll_sockets(int epfd, socket_pollfd *fds, nfds_t nfds, int timeout) {
#if defined(WIN32)
        fd_set readfds, writefds, exceptfds;
        struct timeval tv, *ptv;
        socket_fd_t max_fd;
        int rc;
        nfds_t i;
        boost::shared_ptr<std::vector<socket_pollfd> > ofds;
 
        UNUSED(epfd);
 
        if (fds == NULL) {
                errno = EFAULT;
                return ofds;
        }
 
        FD_ZERO (&readfds);
        FD_ZERO (&writefds);
        FD_ZERO (&exceptfds);
 
        /*********************
        ** Compute fd sets
        **********************/
        // also find the largest descriptor.
        for (rc = -1, max_fd = 0, i = 0; i < nfds; i++) {
                if (fds[i].fd == INVALID_SOCKET) {
                        continue;
                }
                if (fds[i].events & (POLLIN | POLLRDNORM)) {
                        FD_SET (fds[i].fd, &readfds);
                }
                if (fds[i].events & (POLLOUT | POLLWRNORM | POLLWRBAND)) {
                        FD_SET (fds[i].fd, &writefds);
                }
                if (fds[i].events & (POLLPRI | POLLRDBAND)) {
                        FD_SET (fds[i].fd, &exceptfds);
                }
                if (fds[i].fd > max_fd &&
                          (fds[i].events & (POLLIN | POLLOUT | POLLPRI |
                                                                POLLRDNORM | POLLRDBAND |
                                                                POLLWRNORM | POLLWRBAND))) {
                        max_fd = fds[i].fd;
                        rc = 0;
                }
        }
 
        if (rc == -1) {
                errno = EINVAL;
                return ofds;
        }
        /*********************
        ** Setting the timeout
        **********************/
        if (timeout < 0) {
                ptv = NULL;
        } else {
                ptv = &tv;
                if (timeout == 0) {
                        tv.tv_sec = 0;
                        tv.tv_usec = 0;
                } else {
                        tv.tv_sec = timeout / 1000;
                        tv.tv_usec = (timeout % 1000) * 1000;
                }
        }
 
        rc = select (max_fd + 1, &readfds, &writefds, &exceptfds, ptv);
        if (rc < 0) {
                return ofds;
        }
        ofds.reset(new std::vector<socket_pollfd>);
        if ( rc == 0 ) {
                return ofds;
        }
 
        for (rc = 0, i = 0; i < nfds; i++) {
                if (fds[i].fd != INVALID_SOCKET) {
                        fds[i].revents = 0;
 
                        if (FD_ISSET(fds[i].fd, &readfds)) {
                                int save_errno = errno;
                                char data[64] = {0};
                                int ret;
 
                                /* support for POLLHUP */
                                // just check if there's incoming data, without removing it from the queue.
                                ret = recv(fds[i].fd, data, 64, MSG_PEEK);
                                #ifdef WIN32
                                if ((ret == -1) &&
                                                (errno == WSAESHUTDOWN || errno == WSAECONNRESET ||
                                                (errno == WSAECONNABORTED) || errno == WSAENETRESET))
                                #else
                                if ((ret == -1) &&
                                                (errno == ESHUTDOWN || errno == ECONNRESET ||
                                                (errno == ECONNABORTED) || errno == ENETRESET))
                                #endif
                                {
                                        fds[i].revents |= POLLHUP;
                                } else {
                                        fds[i].revents |= fds[i].events & (POLLIN | POLLRDNORM);
                                }
                                errno = save_errno;
                        }
                        if (FD_ISSET(fds[i].fd, &writefds)) {
                                fds[i].revents |= fds[i].events & (POLLOUT | POLLWRNORM | POLLWRBAND);
                        }
 
                        if (FD_ISSET(fds[i].fd, &exceptfds)) {
                                fds[i].revents |= fds[i].events & (POLLPRI | POLLRDBAND);
                        }
 
                        if (fds[i].revents & ~POLLHUP) {
                                rc++;
                        }
                } else {
                                fds[i].revents = POLLNVAL;
                }
                ofds->push_back(fds[i]);
        }
        return ofds;
#elif defined (HAVE_EPOLL)
        UNUSED(nfds);
        UNUSED(fds);
        struct epoll_event ev[nfds];
        pollfd_vector_ptr ofds;
 
        int fd_cnt = ::epoll_wait(epfd, ev, nfds, timeout);
 
        if (fd_cnt < 0)
        {
        // EINTR means that we got interrupted by a signal, and is not an error
                if(errno != EINTR) {
                        ROS_ERROR("Error in epoll_wait! %s", strerror(errno));
                        ofds.reset();
                }
        }
        else
        {
                ofds.reset(new std::vector<socket_pollfd>);
                for (int i = 0; i < fd_cnt; i++)
                {
                        socket_pollfd pfd;
                        pfd.fd = ev[i].data.fd;
                        pfd.revents = ev[i].events;
                        ofds->push_back(pfd);
                }
        }
        return ofds;
#else
        UNUSED(epfd);
        pollfd_vector_ptr ofds(new std::vector<socket_pollfd>);
        // Clear the `revents` fields
        for (nfds_t i = 0; i < nfds; i++)
        {
                fds[i].revents = 0;
        }
 
        // use an existing poll implementation
        int result = poll(fds, nfds, timeout);
        if ( result < 0 )
        {
                // EINTR means that we got interrupted by a signal, and is not an error
                if(errno != EINTR)
                {
                        ROS_ERROR("Error in poll! %s", strerror(errno));
                        ofds.reset();
                }
        } else {
                for (nfds_t i = 0; i < nfds; i++)
                {
                        if (fds[i].revents)
                        {
                                ofds->push_back(fds[i]);
                                fds[i].revents = 0;
                        }
                }
        }
        return ofds;
#endif // poll_sockets functions
}
/*****************************************************************************
** Socket Utilities
*****************************************************************************/
int is_async_connected(socket_fd_t &socket, int &err) {
#ifdef HAVE_EPOLL
    int nfds = 1;
    int epfd = create_socket_watcher();
    add_socket_to_watcher(epfd, socket);
    set_events_on_socket(epfd, socket, EPOLLIN | EPOLLOUT);
    struct epoll_event ev[nfds];
    int fd_cnt = ::epoll_wait(epfd, ev, nfds, 0);
    close_socket_watcher(epfd);
    if (fd_cnt < 0) {
      if (errno != EINTR) {
        ROS_ERROR("Error in epoll_wait! %s", strerror(errno));
        err = errno;
        return -1;
      }
    } else if (fd_cnt == 0) {
      err = 0;
      return 0;
    } else if (ev[0].events & EPOLLERR || ev[0].events & EPOLLHUP) {
      // attempt to retrieve the error on the file descriptor epoll is waiting on
      err = 0;
      socklen_t errlen = sizeof(err);
      getsockopt(socket, SOL_SOCKET, SO_ERROR, (void *)&err, &errlen);
      return -1;
    }
#else // HAVE_EPOLL
    // use zero-timeout select to check if async socket
    fd_set wfds, exceptfds;
    FD_ZERO(&wfds);
    FD_ZERO(&exceptfds);
    FD_SET(socket, &wfds);
    FD_SET(socket, &exceptfds);
    struct timeval tv;
    tv.tv_sec = 0;
    tv.tv_usec = 0;
    int ret = select(socket + 1, NULL, &wfds, &exceptfds, &tv);
    if (ret == -1)
    {
      // select() error
      ROSCPP_CONN_LOG_DEBUG("select() on socket[%d] failed with error [%s]",
                            socket, last_socket_error_string());
      err = last_socket_error();
      return -1;
    }
    else if (ret == 0)
    {
      // select() timeout, socket is still connecting
      err = 0;
      return 0;
    }
    // select() fake wakeup?
    ROS_ASSERT(FD_ISSET(socket, &wfds) || FD_ISSET(socket, &exceptfds));
 
    // check connection is success or failure
#ifdef WIN32
    // In Windows:
    //    Success is reported in the writefds set and failure is reported in the exceptfds set.
    //    If failure, we must then call getsockopt SO_ERROR to determine the error value to describe why the failure occurred
    ROS_ASSERT((FD_ISSET(socket, &wfds) && !FD_ISSET(socket, &exceptfds)) ||
               (!FD_ISSET(socket, &wfds) && FD_ISSET(socket, &exceptfds)));
    if (FD_ISSET(socket, &exceptfds)) {
      // an error occurred during connection
      int errinfo = 0;
      socklen_t errlen = sizeof(int);
      if (getsockopt(socket, SOL_SOCKET, SO_ERROR, reinterpret_cast<char*>(&errinfo), &errlen) == -1)
      {
        // getsockopt() error
        ROSCPP_CONN_LOG_DEBUG("getsockopt() on socket[%d] failed with error [%s]",
                              socket, last_socket_error_string());
        err = last_socket_error();
        return -1;
      }
      ROS_ASSERT(errlen == sizeof(int));
      ROS_ASSERT(errinfo != 0);
      ROSCPP_CONN_LOG_DEBUG("Async connect on socket[%d] failed with error [%s]",
                            socket, socket_error_string(errinfo));
      err = errinfo;
      return -1;
    }
#else // WIN32
    // In Linux:
    //    Both success and failure is reported in the writefds set.
    //    We must use getsockopt() to read the SO_ERROR option at level SOL_SOCKET to determine whether
    //    connect() completed successfully (SO_ERROR is zero) or unsuccessfully (SO_ERROR is error code).
    if (!FD_ISSET(socket, &wfds)) {
      // wfds is not set, socket is still connecting
      err = 0;
      return 0;
    }
    // use getsockopt() to check connection is success or failure
    int errinfo;
    socklen_t errlen = sizeof(int);
    if (getsockopt(socket, SOL_SOCKET, SO_ERROR, &errinfo, &errlen) == -1)
    {
      // getsockopt() error
      ROSCPP_CONN_LOG_DEBUG("getsockopt() on socket[%d] failed with error [%s]",
                            socket, last_socket_error_string());
      err = last_socket_error();
      return -1;
    }
    ROS_ASSERT(errlen == sizeof(int));
    if (errinfo != 0)
    {
      // an error occurred during connection
      ROSCPP_CONN_LOG_DEBUG("Async connect on socket[%d] failed with error [%s]",
                            socket, socket_error_string(errinfo));
      err = errinfo;
      return -1;
    }
#endif // WIN32
#endif // HAVE_EPOLL
    err = 0;
    return 1;
}
 
int set_non_blocking(socket_fd_t &socket) {
#ifdef WIN32
    u_long non_blocking = 1;
        if(ioctlsocket( socket, FIONBIO, &non_blocking ) != 0 )
        {
      return WSAGetLastError();
        }
#else
    if(fcntl(socket, F_SETFL, O_NONBLOCK) == -1)
    {
      return errno;
    }
#endif
    return 0;
}
 
int close_socket(socket_fd_t &socket) {
#ifdef WIN32
        if(::closesocket(socket) == SOCKET_ERROR ) {
                return -1;
        } else {
                return 0;
        }
#else
        if (::close(socket) < 0) {
                return -1;
        } else {
                return 0;
        }
#endif //WIN32
}
 
/*****************************************************************************
** Signal Pair
*****************************************************************************/
int create_signal_pair(signal_fd_t signal_pair[2]) {
#ifdef WIN32 // use a socket pair
        signal_pair[0] = INVALID_SOCKET;
        signal_pair[1] = INVALID_SOCKET;
 
    union {
       struct sockaddr_in inaddr;
       struct sockaddr addr;
    } a;
    socklen_t addrlen = sizeof(a.inaddr);
 
    /*********************
        ** Listen Socket
        **********************/
        socket_fd_t listen_socket = INVALID_SOCKET;
    listen_socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
        if (listen_socket == INVALID_SOCKET) {
                return -1;
        }
 
    // allow it to be bound to an address already in use - do we actually need this?
    int reuse = 1;
    if (setsockopt(listen_socket, SOL_SOCKET, SO_REUSEADDR, (char*) &reuse, (socklen_t) sizeof(reuse)) == SOCKET_ERROR ) {
        ::closesocket(listen_socket);
                return -1;
    }
 
    memset(&a, 0, sizeof(a));
    a.inaddr.sin_family = AF_INET;
    a.inaddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
    // For TCP/IP, if the port is specified as zero, the service provider assigns
    // a unique port to the application from the dynamic client port range.
    a.inaddr.sin_port = 0;
 
    if  (bind(listen_socket, &a.addr, sizeof(a.inaddr)) == SOCKET_ERROR) {
        ::closesocket(listen_socket);
                return -1;
    }
    // we need this below because the system auto filled in some entries, e.g. port #
    if  (getsockname(listen_socket, &a.addr, &addrlen) == SOCKET_ERROR) {
        ::closesocket(listen_socket);
                return -1;
    }
    // max 1 connection permitted
    if (listen(listen_socket, 1) == SOCKET_ERROR) {
        ::closesocket(listen_socket);
                return -1;
    }
 
    /*********************
        ** Connection
        **********************/
    // do we need io overlapping?
    // DWORD flags = (make_overlapped ? WSA_FLAG_OVERLAPPED : 0);
    DWORD overlapped_flag = 0;
    signal_pair[0] = WSASocket(AF_INET, SOCK_STREAM, 0, NULL, 0, overlapped_flag);
    if (signal_pair[0] == INVALID_SOCKET) {
        ::closesocket(listen_socket);
        ::closesocket(signal_pair[0]);
        return -1;
    }
    // reusing the information from above to connect to the listener
    if (connect(signal_pair[0], &a.addr, sizeof(a.inaddr)) == SOCKET_ERROR) {
        ::closesocket(listen_socket);
        ::closesocket(signal_pair[0]);
        return -1;
    }
    /*********************
        ** Accept
        **********************/
    signal_pair[1] = accept(listen_socket, NULL, NULL);
    if (signal_pair[1] == INVALID_SOCKET) {
        ::closesocket(listen_socket);
        ::closesocket(signal_pair[0]);
        ::closesocket(signal_pair[1]);
        return -1;
    }
        /*********************
        ** Nonblocking
        **********************/
    // should we do this or should we set io overlapping?
    if ( (set_non_blocking(signal_pair[0]) != 0) || (set_non_blocking(signal_pair[1]) != 0)  ) {
                ::closesocket(listen_socket);
                ::closesocket(signal_pair[0]);
                ::closesocket(signal_pair[1]);
        return -1;
    }
        /*********************
        ** Cleanup
        **********************/
    ::closesocket(listen_socket);  // the listener has done its job.
    return 0;
#else // use a pipe pair
        // initialize
        signal_pair[0] = -1;
        signal_pair[1] = -1;
 
        if(pipe(signal_pair) != 0) {
                ROS_FATAL( "pipe() failed");
                return -1;
        }
        if(fcntl(signal_pair[0], F_SETFL, O_NONBLOCK) == -1) {
                ROS_FATAL( "fcntl() failed");
                return -1;
        }
        if(fcntl(signal_pair[1], F_SETFL, O_NONBLOCK) == -1) {
                ROS_FATAL( "fcntl() failed");
                return -1;
        }
        return 0;
#endif // create_pipe
}
 
} // namespace ros