SOCKET accept(JNIEnv* e, SOCKET sock, long* client_addr, short* client_port)
{
sockaddr_in adr;
SOCKET client_socket = ::accept(sock, (sockaddr*)&adr, NULL);
if (INVALID_SOCKET == client_socket) {
char buf[255];
sprintf(buf,
"Can't accept the incoming connection. System error: %d",
last_socket_error());
throwNew(e, "java/io/IOException", buf);
return INVALID_SOCKET;
}
if (client_addr != NULL) {
#ifdef PLATFORM_WINDOWS
*client_addr = ntohl(adr.sin_addr.S_un.S_addr);
#else
*client_addr = ntohl(adr.sin_addr.s_addr);
#endif
}
if (client_port != NULL) {
*client_port = ntohs(adr.sin_port);
}
return client_socket;
}
void throw_io_failure(const char* what, bool add_errno_failure) {
if (add_errno_failure) {
std::ostringstream oss;
oss << what << ": " << last_socket_error_as_string()
<< " [errno: " << last_socket_error() << "]";
throw std::ios_base::failure(oss.str());
}
throw std::ios_base::failure(what);
}
void abort(JNIEnv* e, SOCKET sock)
{
if (SOCKET_ERROR == ::closesocket(sock)) {
char buf[255];
sprintf(
buf, "Can't close the socket. System error: %d", last_socket_error());
throwNew(e, "java/io/IOException", buf);
}
}
port_t
bind_random_free_listen_port(socket_t socket_fd, ipv4_t ip, port_t low, port_t high) {
/* find a port in the range (inclusive) to bind to
based on searching a random permutation of the numbers
in the range */
assert(low <= high);
bool allocated_socket = false;
if (socket_fd == INVALID_SOCKET) {
allocated_socket = true;
socket_fd = socket(AF_INET, SOCK_STREAM, 0);
if (socket_fd == INVALID_SOCKET) goto fail;
}
/* unsigned has to be bigger than port_t because otherwise the highest
port minus the lowest port plus one would be an overflow */
STATIC_ASSERT(sizeof(unsigned) > sizeof(port_t),
"unsigned is too large or port_t is too small");
unsigned range_size = (unsigned) high - (unsigned) low + 1;
port_t cur_port = low + rand() % range_size;
unsigned adder = high == low
? 0 /* we're just iterating once anyway */
: find_random_smaller_mutually_prime(range_size);
unsigned i;
for (i = 0; i < range_size; ++i) {
struct sockaddr_in listen_addr;
init_sockaddr_in(&listen_addr, ip, cur_port);
int ret_bind = bind(socket_fd,
(struct sockaddr *) &listen_addr,
sizeof(listen_addr));
if (!ret_bind) break;
if (last_socket_error() != SOCKET_EADDRINUSE) goto fail;
cur_port += adder;
/* deal with wraparound
NB: this is unsigned overflow so it's not undefined */
if (cur_port > high) cur_port = low + (cur_port - high - 1);
else if (cur_port < low) cur_port += low;
}
if (i == range_size) {
fail:
cur_port = 0;
}
if (allocated_socket && socket_fd != INVALID_SOCKET) {
/* log if close fails */
closesocket(socket_fd);
}
return cur_port;
}
void handle_io_result(ssize_t res, bool is_nonblock, const char* msg) {
if (res < 0) {
auto err = last_socket_error();
if (is_nonblock && would_block_or_temporarily_unavailable(err)) {
// don't throw for 'failed' non-blocking IO,
// just try again later
}
else throw_io_failure(msg);
}
}
void close_output(JNIEnv* e, SOCKET sock)
{
if (SOCKET_ERROR == ::shutdown(sock, SD_SEND)) {
int errcode = last_socket_error();
if (errcode != ENOTCONN) {
char buf[255];
sprintf(buf, "Can't shutdown the socket. System error: %d", errcode);
throwNew(e, "java/io/IOException", buf);
}
}
}
void send(JNIEnv* e, SOCKET sock, const char* buff_ptr, int buff_size)
{
if (SOCKET_ERROR == ::send(sock, buff_ptr, buff_size, 0)) {
char buf[255];
sprintf(buf,
"Can't send data through the socket. System error: %d",
last_socket_error());
throwNew(e, "java/io/IOException", buf);
return;
}
}
SOCKET create(JNIEnv* e)
{
SOCKET sock;
if (INVALID_SOCKET == (sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP))) {
char buf[255];
sprintf(
buf, "Can't create a socket. System error: %d", last_socket_error());
throwNew(e, "java/io/IOException", buf);
return 0; // This doesn't matter cause we have risen an exception
}
return sock;
}
int recv(JNIEnv* e, SOCKET sock, char* buff_ptr, int buff_size)
{
int length = ::recv(sock, buff_ptr, buff_size, 0);
if (SOCKET_ERROR == length) {
char buf[255];
sprintf(buf,
"Can't receive data through the socket. System error: %d",
last_socket_error());
throwNew(e, "java/io/IOException", buf);
return 0; // This doesn't matter cause we have risen an exception
}
return length;
}
void bind(JNIEnv* e, SOCKET sock, long addr, short port)
{
sockaddr_in adr;
adr.sin_family = AF_INET;
#ifdef PLATFORM_WINDOWS
adr.sin_addr.S_un.S_addr = htonl(addr);
#else
adr.sin_addr.s_addr = htonl(addr);
#endif
adr.sin_port = htons(port);
if (SOCKET_ERROR == ::bind(sock, (sockaddr*)&adr, sizeof(adr))) {
char buf[255];
sprintf(buf, "Can't bind a socket. System error: %d", last_socket_error());
throwNew(e, "java/io/IOException", buf);
return;
}
}
std::string last_socket_error_as_string() {
LPTSTR errorText = NULL;
auto hresult = last_socket_error();
FormatMessage(// use system message tables to retrieve error text
FORMAT_MESSAGE_FROM_SYSTEM
// allocate buffer on local heap for error text
| FORMAT_MESSAGE_ALLOCATE_BUFFER
// Important! will fail otherwise, since we're not
// (and CANNOT) pass insertion parameters
| FORMAT_MESSAGE_IGNORE_INSERTS,
nullptr, // unused with FORMAT_MESSAGE_FROM_SYSTEM
hresult,
MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
(LPTSTR) &errorText, // output
0, // minimum size for output buffer
nullptr); // arguments - see note
std::string result;
if (errorText != nullptr) {
result = errorText;
// release memory allocated by FormatMessage()
LocalFree(errorText);
}
return result;
}
const char *
last_socket_error_message(void) {
return socket_error_message(last_socket_error());
}
bool TransportTCP::connect(const std::string& host, int port)
{
if (!isHostAllowed(host))
return false; // adios amigo
sock_ = socket(s_use_ipv6_ ? AF_INET6 : AF_INET, SOCK_STREAM, 0);
connected_host_ = host;
connected_port_ = port;
if (sock_ == ROS_INVALID_SOCKET)
{
ROS_ERROR("socket() failed with error [%s]", last_socket_error_string());
return false;
}
setNonBlocking();
sockaddr_storage sas;
socklen_t sas_len;
in_addr ina;
in6_addr in6a;
if (inet_pton(AF_INET, host.c_str(), &ina) == 1)
{
sockaddr_in *address = (sockaddr_in*) &sas;
sas_len = sizeof(sockaddr_in);
la_len_ = sizeof(sockaddr_in);
address->sin_family = AF_INET;
address->sin_port = htons(port);
address->sin_addr.s_addr = ina.s_addr;
}
else if (inet_pton(AF_INET6, host.c_str(), &in6a) == 1)
{
sockaddr_in6 *address = (sockaddr_in6*) &sas;
sas_len = sizeof(sockaddr_in6);
la_len_ = sizeof(sockaddr_in6);
address->sin6_family = AF_INET6;
address->sin6_port = htons(port);
memcpy(address->sin6_addr.s6_addr, in6a.s6_addr, sizeof(in6a.s6_addr));
}
else
{
struct addrinfo* addr;
struct addrinfo hints;
memset(&hints, 0, sizeof(hints));
hints.ai_family = AF_UNSPEC;
if (getaddrinfo(host.c_str(), NULL, &hints, &addr) != 0)
{
close();
ROS_ERROR("couldn't resolve publisher host [%s]", host.c_str());
return false;
}
bool found = false;
struct addrinfo* it = addr;
char namebuf[128];
for (; it; it = it->ai_next)
{
if (!s_use_ipv6_ && it->ai_family == AF_INET)
{
sockaddr_in *address = (sockaddr_in*) &sas;
sas_len = sizeof(*address);
memcpy(address, it->ai_addr, it->ai_addrlen);
address->sin_family = it->ai_family;
address->sin_port = htons(port);
strcpy(namebuf, inet_ntoa(address->sin_addr));
found = true;
break;
}
if (s_use_ipv6_ && it->ai_family == AF_INET6)
{
sockaddr_in6 *address = (sockaddr_in6*) &sas;
sas_len = sizeof(*address);
memcpy(address, it->ai_addr, it->ai_addrlen);
address->sin6_family = it->ai_family;
address->sin6_port = htons((u_short) port);
// TODO IPV6: does inet_ntop need other includes for Windows?
inet_ntop(AF_INET6, (void*)&(address->sin6_addr), namebuf, sizeof(namebuf));
found = true;
break;
}
}
freeaddrinfo(addr);
if (!found)
{
ROS_ERROR("Couldn't resolve an address for [%s]\n", host.c_str());
return false;
}
ROSCPP_LOG_DEBUG("Resolved publisher host [%s] to [%s] for socket [%d]", host.c_str(), namebuf, sock_);
}
int ret = ::connect(sock_, (sockaddr*) &sas, sas_len);
// windows might need some time to sleep (input from service robotics hack) add this if testing proves it is necessary.
ROS_ASSERT((flags_ & SYNCHRONOUS) || ret != 0);
if (((flags_ & SYNCHRONOUS) && ret != 0) || // synchronous, connect() should return 0
(!(flags_ & SYNCHRONOUS) && last_socket_error() != ROS_SOCKETS_ASYNCHRONOUS_CONNECT_RETURN)) // asynchronous, connect() should return -1 and WSAGetLastError()=WSAEWOULDBLOCK/errno=EINPROGRESS
{
ROSCPP_LOG_DEBUG("Connect to tcpros publisher [%s:%d] failed with error [%d, %s]", host.c_str(), port, ret, last_socket_error_string());
close();
return false;
}
// from daniel stonier:
#ifdef WIN32
// This is hackish, but windows fails at recv() if its slow to connect (e.g. happens with wireless)
// recv() needs to check if its connected or not when its asynchronous?
Sleep(100);
#endif
std::stringstream ss;
ss << host << ":" << port << " on socket " << sock_;
cached_remote_host_ = ss.str();
if (!initializeSocket())
{
return false;
}
if (flags_ & SYNCHRONOUS)
{
ROSCPP_LOG_DEBUG("connect() succeeded to [%s:%d] on socket [%d]", host.c_str(), port, sock_);
}
else
{
ROSCPP_LOG_DEBUG("Async connect() in progress to [%s:%d] on socket [%d]", host.c_str(), port, sock_);
}
return true;
}
inline bool
Server::ConnectionListener::startListening ()
{
int status;
/* create a nonblocking-socket */
# if _WIN32
sockd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
# else
sockd = socket(AF_INET, SOCK_STREAM, 0);
# endif
# if _WIN32
if (INVALID_SOCKET == sockd)
# else
if (-1 == sockd)
# endif
{
# if _WIN32
last_socket_error();
# else
perror("Socket creation error");
# endif
return false;
}
status = ::bind(sockd, (struct sockaddr*)&_local, sizeof(_local));
if (status == -1)
{
# if _WIN32
last_socket_error();
# else
perror("Binding error");
# endif
return false;
}
uplink_log_info("Server started listening.");
status = listen(sockd, 5);
if (status == -1)
{
# if _WIN32
last_socket_error();
# else
perror("Listening error");
# endif
return false;
}
# if _WIN32
u_long val = 1;
ioctlsocket(sockd, FIONBIO, &val);
# else
// set listen to be non-blocking
int socketFlags = fcntl(sockd, F_GETFL, 0);
fcntl(sockd, F_SETFL, socketFlags | O_NONBLOCK);
# endif
_startedListening = true;
start(); // Start the thread.
return true;
}
bool TransportTCP::connect(const std::string& host, int port)
{
sock_ = socket(AF_INET, SOCK_STREAM, 0);
connected_host_ = host;
connected_port_ = port;
if (sock_ == ROS_INVALID_SOCKET)
{
ROS_ERROR("socket() failed with error [%s]", last_socket_error_string());
return false;
}
setNonBlocking();
sockaddr_in sin;
sin.sin_family = AF_INET;
if (inet_addr(host.c_str()) == INADDR_NONE)
{
struct addrinfo* addr;
if (getaddrinfo(host.c_str(), NULL, NULL, &addr) != 0)
{
close();
ROS_ERROR("couldn't resolve publisher host [%s]", host.c_str());
return false;
}
bool found = false;
struct addrinfo* it = addr;
for (; it; it = it->ai_next)
{
if (it->ai_family == AF_INET)
{
memcpy(&sin, it->ai_addr, it->ai_addrlen);
sin.sin_family = it->ai_family;
sin.sin_port = htons(port);
found = true;
break;
}
}
freeaddrinfo(addr);
if (!found)
{
ROS_ERROR("Couldn't find an AF_INET address for [%s]\n", host.c_str());
return false;
}
ROSCPP_LOG_DEBUG("Resolved publisher host [%s] to [%s] for socket [%d]", host.c_str(), inet_ntoa(sin.sin_addr), sock_);
}
else
{
sin.sin_addr.s_addr = inet_addr(host.c_str()); // already an IP addr
}
sin.sin_port = htons(port);
int ret = ::connect(sock_, (sockaddr *)&sin, sizeof(sin));
// windows might need some time to sleep (input from service robotics hack) add this if testing proves it is necessary.
ROS_ASSERT((flags_ & SYNCHRONOUS) || ret != 0);
if (((flags_ & SYNCHRONOUS) && ret != 0) || // synchronous, connect() should return 0
(!(flags_ & SYNCHRONOUS) && last_socket_error() != ROS_SOCKETS_ASYNCHRONOUS_CONNECT_RETURN)) // asynchronous, connect() should return -1 and WSAGetLastError()=WSAEWOULDBLOCK/errno=EINPROGRESS
{
ROSCPP_LOG_DEBUG("Connect to tcpros publisher [%s:%d] failed with error [%d, %s]", host.c_str(), port, ret, last_socket_error_string());
close();
return false;
}
std::stringstream ss;
ss << host << ":" << port << " on socket " << sock_;
cached_remote_host_ = ss.str();
if (!initializeSocket())
{
return false;
}
if (flags_ & SYNCHRONOUS)
{
ROSCPP_LOG_DEBUG("connect() succeeded to [%s:%d] on socket [%d]", host.c_str(), port, sock_);
}
else
{
ROSCPP_LOG_DEBUG("Async connect() in progress to [%s:%d] on socket [%d]", host.c_str(), port, sock_);
}
return true;
}
