// Read from a bunch of clients
int playerc_mclient_read(playerc_mclient_t *mclient, int timeout)
{
int i, count;
// Configure poll structure to wait for incoming data
for (i = 0; i < mclient->client_count; i++)
{
mclient->pollfd[i].fd = mclient->client[i]->sock;
mclient->pollfd[i].events = POLLIN;
mclient->pollfd[i].revents = 0;
}
// Wait for incoming data
count = poll(mclient->pollfd, mclient->client_count, timeout);
if (count < 0)
{
PLAYERC_ERR1("poll returned error [%s]", strerror(errno));
return -1;
}
// Now read from each of the waiting sockets
for (i = 0; i < mclient->client_count; i++)
{
if ((mclient->pollfd[i].revents & POLLIN) > 0)
{
if(playerc_client_read(mclient->client[i]))
{
// cache the latest timestamp
if(mclient->client[i]->datatime > mclient->time)
mclient->time = mclient->client[i]->datatime;
}
}
}
return count;
}
// Process incoming data
void playerc_micronsonar_putmsg(playerc_micronsonar_t *device, player_msghdr_t *header,
player_micronsonar_data_t *data, size_t len)
{
if((header->type == PLAYER_MSGTYPE_DATA) &&
(header->subtype == PLAYER_MICRONSONAR_DATA_STATE))
{
device->width = data->width;
device->height = data->height;
device->bpp = data->bpp;
device->format = data->format;
device->image_count = data->image_count;
device->image = realloc(device->image, sizeof(device->image[0])*device->image_count);
device->centreX = data->centreX;
device->centreY = data->centreY;
device->range = data->range;
device->numBins = data->numBins;
device->startAngle = data->startAngle;
device->endAngle = data->endAngle;
if (device->image)
memcpy(device->image, data->image, device->image_count);
else
PLAYERC_ERR1("failed to allocate memory for image, needed %ld bytes\n", sizeof(device->image[0])*device->image_count);
}
else
PLAYERC_WARN2("skipping micronsonar message with unknown type/subtype: %s/%d\n",
msgtype_to_str(header->type), header->subtype);
return;
}
// Disconnect from the server
int playerc_client_disconnect(playerc_client_t *client)
{
#if defined (WIN32)
if (closesocket(client->sock) != 0)
{
FormatMessage(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, NULL,
ErrNo, 0, (LPTSTR) &errBuffer, 0, NULL);
PLAYERC_ERR1("closesocket failed with error [%s]", (LPTSTR) errBuffer);
LocalFree(errBuffer);
}
#else
if (close(client->sock) < 0)
{
PLAYERC_ERR1("close failed with error [%s]", strerror(errno));
client->sock = -1;
return -1;
}
#endif
client->sock = -1;
client->connected = 0;
return 0;
}
// Destroy a player client
void playerc_client_destroy(playerc_client_t *client)
{
player_msghdr_t header;
// Pop everything off the queue.
while (!playerc_client_pop(client, &header, client->data))
{
playerxdr_cleanup_message(client->data,header.addr.interf, header.type, header.subtype);
}
#if defined (WIN32)
// Clean up the Windows sockets API (this can safely be done as many times as we like)
if (WSACleanup () != 0)
PLAYERC_ERR1 ("Failed to clean up Windows sockets API with error %s", WSAGetLastError ());
#endif
free(client->data);
free(client->write_xdrdata);
free(client->read_xdrdata);
free(client->host);
free(client);
return;
}
// Test to see if there is pending data.
// Returns -1 on error, 0 or 1 otherwise.
int playerc_mclient_peek(playerc_mclient_t *mclient, int timeout)
{
int i, count;
// Configure poll structure to wait for incoming data
for (i = 0; i < mclient->client_count; i++)
{
mclient->pollfd[i].fd = mclient->client[i]->sock;
mclient->pollfd[i].events = POLLIN;
mclient->pollfd[i].revents = 0;
}
// Wait for incoming data
count = poll(mclient->pollfd, mclient->client_count, timeout);
if (count < 0)
{
PLAYERC_ERR1("poll returned error [%s]", strerror(errno));
return -1;
}
return (count > 0);
}
// Test to see if there is pending data. Don't send a data request.
int playerc_client_internal_peek(playerc_client_t *client, int timeout)
{
int count;
struct pollfd fd;
if (client->sock < 0)
{
PLAYERC_WARN("no socket to peek at");
return -1;
}
fd.fd = client->sock;
//fd.events = POLLIN | POLLHUP;
fd.events = POLLIN | POLLPRI | POLLERR | POLLHUP | POLLNVAL;
fd.revents = 0;
// Wait for incoming data
count = poll(&fd, 1, timeout);
if (count < 0)
{
if(errno == EINTR)
return(0);
else
{
PLAYERC_ERR1("poll returned error [%s]", strerror(errno));
//playerc_client_disconnect(client);
return(playerc_client_disconnect_retry(client));
}
}
if (count > 0 && (fd.revents & POLLHUP))
{
PLAYERC_ERR("socket disconnected");
//playerc_client_disconnect(client);
return(playerc_client_disconnect_retry(client));
}
return count;
}
// Connect to the server
int playerc_client_connect(playerc_client_t *client)
{
#if defined(HAVE_GETADDRINFO)
struct addrinfo* addr_ptr = NULL;
#else
struct hostent* entp = NULL;
#endif
char banner[PLAYER_IDENT_STRLEN];
int ret;
//double t;
/*
struct timeval last;
struct timeval curr;
*/
#if defined (WIN32)
unsigned long setting = 0;
#else
int old_flags;
struct itimerval timer;
struct sigaction sigact;
#endif
struct sockaddr_in clientaddr;
// Construct socket
if(client->transport == PLAYERC_TRANSPORT_UDP)
{
#if defined (WIN32)
if((client->sock = socket(PF_INET, SOCK_DGRAM, 0)) == INVALID_SOCKET)
#else
if((client->sock = socket(PF_INET, SOCK_DGRAM, 0)) < 0)
#endif
{
STRERROR(PLAYERC_ERR2, "socket() failed with error [%d: %s]");
return -1;
}
/*
* INADDR_ANY indicates that any network interface (IP address)
* for the local host may be used (presumably the OS will choose the
* right one).
*
* Specifying sin_port = 0 allows the system to choose the port.
*/
clientaddr.sin_family = AF_INET;
clientaddr.sin_addr.s_addr = INADDR_ANY;
clientaddr.sin_port = 0;
if(bind(client->sock,
(struct sockaddr*)&clientaddr, sizeof(clientaddr)) < -1)
{
STRERROR(PLAYERC_ERR2, "bind() failed with error [%d: %s]");
return -1;
}
}
else
{
#if defined (WIN32)
if((client->sock = socket(PF_INET, SOCK_STREAM, 0)) == INVALID_SOCKET)
#else
if((client->sock = socket(PF_INET, SOCK_STREAM, 0)) < 0)
#endif
{
STRERROR(PLAYERC_ERR2, "socket() failed with error [%d: %s]");
return -1;
}
}
#if ENABLE_TCP_NODELAY
// Disable Nagel's algorithm for lower latency
{
int yes = 1;
if(setsockopt(client->sock, IPPROTO_TCP, TCP_NODELAY, &yes,
sizeof(int)) == -1)
{
PLAYERC_ERR("failed to enable TCP_NODELAY - setsockopt failed");
return -1;
}
}
#endif
// Construct server address
memset(&client->server, 0, sizeof(client->server));
client->server.sin_family = AF_INET;
client->server.sin_port = htons(client->port);
#if defined(HAVE_GETADDRINFO)
if (getaddrinfo(client->host, NULL, NULL, &addr_ptr) != 0)
{
playerc_client_disconnect(client);
PLAYERC_ERR("getaddrinfo() failed with error");
return -1;
}
assert(addr_ptr);
assert(addr_ptr->ai_addr);
#ifdef AF_INET6
if (((addr_ptr->ai_addr->sa_family) != AF_INET) &&
((addr_ptr->ai_addr->sa_family) != AF_INET6))
#else
if ((addr_ptr->ai_addr->sa_family) != AF_INET)
#endif
{
playerc_client_disconnect(client);
PLAYERC_ERR("unsupported internet address family");
return -1;
}
client->server.sin_addr.s_addr = ((struct sockaddr_in *)(addr_ptr->ai_addr))->sin_addr.s_addr;
freeaddrinfo(addr_ptr);
addr_ptr = NULL;
#else
entp = gethostbyname(client->host);
if (entp == NULL)
{
playerc_client_disconnect(client);
STRERROR(PLAYERC_ERR2, "gethostbyname() failed with error [%d: %s]");
return -1;
}
assert(entp->h_length <= sizeof (client->server.sin_addr));
memcpy(&(client->server.sin_addr), entp->h_addr_list[0], entp->h_length);
#endif
// Connect the socket
/*
t = client->request_timeout;
do
{
if (t <= 0)
{
PLAYERC_ERR2("connect call on [%s:%d] timed out",
client->host, client->port);
return -1;
}
gettimeofday(&last,NULL);
puts("calling connect");
ret = connect(client->sock, (struct sockaddr*)&client->server,
sizeof(client->server));
gettimeofday(&curr,NULL);
t -= ((curr.tv_sec + curr.tv_usec/1e6) -
(last.tv_sec + last.tv_usec/1e6));
} while (ret == -1 && (errno == EALREADY || errno == EAGAIN || errno == EINPROGRESS));
*/
// In Windows, the connect timeout is (apparently) a registry setting.
#if !defined (WIN32)
/* Set up a timer to interrupt the connection process */
timer.it_interval.tv_sec = 0;
timer.it_interval.tv_usec = 0;
timer.it_value.tv_sec = (int)floor(client->request_timeout);
timer.it_value.tv_usec =
(int)rint(fmod(client->request_timeout,timer.it_value.tv_sec)*1e6);
if(setitimer(ITIMER_REAL, &timer, NULL) != 0)
PLAYERC_WARN("failed to set up connection timer; "
"indefinite hang may result");
/* Turn off system call restart so that connect() will terminate when the
* alarm goes off */
if(sigaction(SIGALRM, NULL, &sigact) != 0)
PLAYERC_WARN("failed to get SIGALRM action data; "
"unexpected exit may result");
else
{
#ifdef SA_RESTART
sigact.sa_handler = dummy;
sigact.sa_flags &= ~SA_RESTART;
if(sigaction(SIGALRM, &sigact, NULL) != 0)
#endif
PLAYERC_WARN("failed to set SIGALRM action data; "
"unexpected exit may result");
}
#endif
ret = connect(client->sock, (struct sockaddr*)&client->server,
sizeof(client->server));
#if !defined (WIN32)
/* Turn off timer */
timer.it_value.tv_sec = 0;
timer.it_value.tv_usec = 0;
if(setitimer(ITIMER_REAL, &timer, NULL) != 0)
PLAYERC_WARN("failed to turn off connection timer; "
"unexpected exit may result");
/* Restore normal SIGALRM behavior */
#ifdef SA_RESTART
sigact.sa_handler = SIG_DFL;
sigact.sa_flags |= SA_RESTART;
if(sigaction(SIGALRM, &sigact, NULL) != 0)
#endif
PLAYERC_WARN("failed to reset SIGALRM action data; "
"unexpected behavior may result");
#endif
if (ret < 0)
{
playerc_client_disconnect(client);
#if defined (WIN32)
FormatMessage (FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, NULL,
ErrNo, 0, (LPTSTR) &errBuffer, 0, NULL);
PLAYERC_ERR4 ("connect call on [%s:%d] failed with error [%d:%s]",
client->host, client->port, ErrNo, (LPTSTR) errBuffer);
LocalFree (errBuffer);
#else
PLAYERC_ERR4("connect call on [%s:%d] failed with error [%d:%s]",
client->host, client->port, errno, strerror(ErrNo));
#endif
return -1;
}
// For UDP, send an empty msg to get things going
if(client->transport == PLAYERC_TRANSPORT_UDP)
{
if(send(client->sock, NULL, 0, 0) < 0)
{
STRERROR(PLAYERC_ERR2, "send() failed with error [%d: %s]");
return -1;
}
}
// set socket to be blocking
#if defined (WIN32)
if (ioctlsocket (client->sock, FIONBIO, &setting) == SOCKET_ERROR)
{
FormatMessage(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, NULL,
ErrNo, 0, (LPTSTR) &errBuffer, 0, NULL);
PLAYERC_ERR1("error getting socket flags [%s]", (LPTSTR) errBuffer);
LocalFree(errBuffer);
}
#else
if ((old_flags = fcntl(client->sock, F_GETFL)) < 0)
{
PLAYERC_ERR1("error getting socket flags [%s]", strerror(errno));
return -1;
}
if (fcntl(client->sock, F_SETFL, old_flags & ~O_NONBLOCK) < 0)
{
PLAYERC_ERR1("error setting socket non-blocking [%s]", strerror(errno));
return -1;
}
#endif
// Get the banner
if (timed_recv(client->sock, banner, sizeof(banner), 0, 2000) < sizeof(banner))
{
playerc_client_disconnect(client);
PLAYERC_ERR("incomplete initialization string");
return -1;
}
//set the datamode to pull
playerc_client_datamode(client, PLAYER_DATAMODE_PULL);
PLAYERC_WARN4("[%s] connected on [%s:%d] with sock %d\n", banner, client->host, client->port, client->sock);
client->connected = 1;
return 0;
}
// Create a player client
playerc_client_t *playerc_client_create(playerc_mclient_t *mclient, const char *host, int port)
{
playerc_client_t *client;
#if defined (WIN32)
// Initialise Windows sockets API (this can safely be done as many times as we like)
// Thus must be called once for every client creation, in order to match the calls on
// client destruction (winsocks uses an internal reference counter to ensure only the
// final call to WSACleanup actually does anything).
WSADATA info;
int result;
if ((result = WSAStartup (MAKEWORD (2, 2), &info)) != 0)
{
PLAYERC_ERR1 ("Failed to initialise Windows sockets API with error %d", result);
}
#endif
// Have we done one-time intialization work yet?
if(!init_done)
{
playerxdr_ftable_init();
if (itable_init () != 0)
return NULL;
init_done = 1;
}
client = malloc(sizeof(playerc_client_t));
memset(client, 0, sizeof(playerc_client_t));
client->id = client;
client->host = strdup(host);
client->port = port;
client->connected = 0;
if (mclient)
playerc_mclient_addclient(mclient, client);
// TODO: make this memory allocation more conservative
client->data = (char*)malloc(PLAYER_MAX_MESSAGE_SIZE);
client->write_xdrdata = (char*)malloc(PLAYERXDR_MAX_MESSAGE_SIZE);
client->read_xdrdata = (char*)malloc(PLAYERXDR_MAX_MESSAGE_SIZE);
client->read_xdrdata_len = 0;
assert(client->data);
assert(client->write_xdrdata);
assert(client->read_xdrdata);
client->qfirst = 0;
client->qlen = 0;
client->qsize = sizeof(client->qitems) / sizeof(client->qitems[0]);
client->datatime = 0;
client->lasttime = 0;
/* this is the server's default */
client->mode = PLAYER_DATAMODE_PUSH;
client->transport = PLAYERC_TRANSPORT_TCP;
client->data_requested = 0;
client->data_received = 0;
client->request_timeout = 5.0;
client->retry_limit = 0;
client->retry_time = 2.0;
return client;
}
