/** @brief Request to retrieve an audio sample
Data is stored in wav_data */
int playerc_audio_sample_retrieve(playerc_audio_t *device, int index)
{
int result = 0;
player_audio_sample_t req;
player_audio_sample_t * resp;
req.sample.data_count = 0;
req.index = index;
if((result = playerc_client_request(device->info.client, &device->info,
PLAYER_AUDIO_REQ_SAMPLE_RETRIEVE,
&req, (void**)&resp)) < 0)
return result;
device->wav_data.data_count = resp->sample.data_count;
if (device->wav_data.data != NULL)
free (device->wav_data.data);
if ((device->wav_data.data = (uint8_t*) malloc (resp->sample.data_count)) == NULL)
{
player_audio_sample_t_free(resp);
PLAYERC_ERR("Failed to allocate space to store wave data locally");
return -1;
}
memcpy(device->wav_data.data, resp->sample.data, resp->sample.data_count * sizeof(device->wav_data.data[0]));
device->wav_data.format = resp->sample.format;
player_audio_sample_t_free(resp);
return 0;
}
// Get the list of waypoints. The writes the result into the proxy
// rather than returning it to the caller.
int playerc_planner_get_waypoints(playerc_planner_t *device)
{
int i;
int len;
player_planner_waypoints_req_t config;
memset(&config, 0, sizeof(config));
config.subtype = PLAYER_PLANNER_GET_WAYPOINTS_REQ;
len = playerc_client_request(device->info.client, &device->info,
&config, sizeof(config), &config,
sizeof(config));
if (len < 0)
return -1;
if (len == 0)
{
PLAYERC_ERR("got unexpected zero-length reply");
return -1;
}
device->waypoint_count = (int)ntohs(config.count);
for(i=0;i<device->waypoint_count;i++)
{
device->waypoints[i][0] = ((int)ntohl(config.waypoints[i].x)) / 1e3;
device->waypoints[i][1] = ((int)ntohl(config.waypoints[i].y)) / 1e3;
device->waypoints[i][2] = ((int)ntohl(config.waypoints[i].a)) * M_PI / 180;
}
return 0;
}
// Push a packet onto the incoming queue.
void playerc_client_push(playerc_client_t *client,
player_msghdr_t *header, void *data)
{
playerc_client_item_t *item;
// Check for queue overflow; this will leak mem.
if (client->qlen == client->qsize)
{
PLAYERC_ERR("queue overflow; discarding packets");
client->qfirst = (client->qfirst + 1) % client->qsize;
client->qlen -=1;
}
item = client->qitems + (client->qfirst + client->qlen) % client->qsize;
item->header = *header;
if (header->size && data)
{
item->data = malloc(header->size);
memcpy(item->data, data, header->size);
}
else
{
item->data = NULL;
}
client->qlen +=1;
return;
}
// this method performs a select before the read so we can have a timeout
// this stops the client hanging forever if the target disappears from the network
int timed_recv(int s, void *buf, size_t len, int flags, int timeout)
{
struct pollfd ufd;
int ret;
ufd.fd = s;
ufd.events = POLLIN | POLLPRI | POLLERR | POLLHUP | POLLNVAL;
ret = poll (&ufd, 1, timeout);
if (ret <= 0)
{
if(errno == EINTR)
return(0);
else if (ret == 0)
{
PLAYERC_ERR("poll call timed out with no data to recieve");
return ret;
}
else
{
PLAYERC_ERR2("poll call failed with error [%d:%s]", errno, strerror(errno));
return ret;
}
}
return recv(s,buf,len,flags);
}
// Subscribe to a device
int playerc_client_subscribe(playerc_client_t *client, int code, int index,
int access, char *drivername, size_t len)
{
player_device_req_t req, *resp;
resp=NULL;
req.addr.host = 0;
req.addr.robot = 0;
req.addr.interf = code;
req.addr.index = index;
req.access = access;
req.driver_name_count = 0;
if (playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_DEV,
(void*)&req, (void**)&resp) < 0)
{
PLAYERC_ERR("failed to get response");
return -1;
}
if (req.access != access)
{
PLAYERC_ERR2("requested [%d] access, but got [%d] access", access, req.access);
return -1;
}
// Copy the driver name
strncpy(drivername, resp->driver_name, len);
player_device_req_t_free(resp);
return 0;
}
// Get the driver info for all devices. The data is written into the
// proxy structure rather than returned to the caller.
int playerc_client_get_driverinfo(playerc_client_t *client)
{
int i;
player_device_driverinfo_t req, *resp;
for (i = 0; i < client->devinfo_count; i++)
{
memset(&req,0,sizeof(req));
req.addr = client->devinfos[i].addr;
if(playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_DRIVERINFO,
&req, (void**)&resp) < 0)
{
PLAYERC_ERR("failed to get response");
return(-1);
}
strncpy(client->devinfos[i].drivername, resp->driver_name,
resp->driver_name_count);
client->devinfos[i].drivername[resp->driver_name_count] = '\0';
player_device_driverinfo_t_free(resp);
}
return 0;
}
// Change filename
int playerc_log_set_filename(playerc_log_t* device, const char* fname)
{
player_log_set_filename_t req;
req.subtype = PLAYER_LOG_SET_FILENAME;
if(strlen(fname) > (sizeof(req.filename)-1))
{
PLAYERC_ERR("filename too long");
return(-1);
}
strcpy((char *) req.filename,fname);
if(playerc_client_request(device->info.client, &device->info,
&req, sizeof(req), NULL, 0) < 0)
{
PLAYERC_ERR("failed to set logfile name");
return(-1);
}
return(0);
}
// Add a device proxy
int playerc_client_adddevice(playerc_client_t *client, playerc_device_t *device)
{
if (client->device_count >= sizeof(client->device) / sizeof(client->device[0]))
{
PLAYERC_ERR("too many devices");
return -1;
}
device->fresh = 0;
client->device[client->device_count++] = device;
return 0;
}
// Add a client to this multi-client
int playerc_mclient_addclient(playerc_mclient_t *mclient, playerc_client_t *client)
{
if (mclient->client_count >= sizeof(mclient->client) / sizeof(mclient->client[0]))
{
PLAYERC_ERR("too many clients in multi-client; ignoring new client");
return -1;
}
mclient->client[mclient->client_count] = client;
mclient->client_count++;
return 0;
}
// Copy geometry to the device
void playerc_ranger_copy_geom(playerc_ranger_t *device, player_ranger_geom_t *geom)
{
device->device_pose = geom->pose;
device->device_size = geom->size;
if(device->element_poses != NULL)
{
free(device->element_poses);
device->element_poses = NULL;
}
if(device->element_sizes != NULL)
{
free(device->element_sizes);
device->element_sizes = NULL;
}
device->element_count = 0;
if(geom->element_poses_count > 0)
{
if((device->element_poses = (player_pose3d_t *) malloc(geom->element_poses_count * sizeof(player_pose3d_t))) == NULL)
{
PLAYERC_ERR("Failed to allocate space to store sensor poses");
return;
}
memcpy(device->element_poses, geom->element_poses, geom->element_poses_count * sizeof(player_pose3d_t));
}
if (geom->element_sizes_count > 0)
{
if((device->element_sizes = (player_bbox3d_t *) malloc(geom->element_sizes_count * sizeof(player_bbox3d_t))) == NULL)
{
PLAYERC_ERR("Failed to allocate space to store sensor sizes");
return;
}
memcpy(device->element_sizes, geom->element_sizes, geom->element_sizes_count * sizeof(player_bbox3d_t));
}
device->element_count = geom->element_poses_count;
}
// Register a callback. Will be called when after data has been read
// by the indicated device.
int playerc_client_addcallback(playerc_client_t *client, playerc_device_t *device,
playerc_callback_fn_t callback, void *data)
{
if (device->callback_count >= sizeof(device->callback) / sizeof(device->callback[0]))
{
PLAYERC_ERR("too many registered callbacks; ignoring new callback");
return -1;
}
device->callback[device->callback_count] = callback;
device->callback_data[device->callback_count] = data;
device->callback_count++;
return 0;
}
// Rewind playback
int playerc_log_set_read_rewind(playerc_log_t* device)
{
player_log_set_read_rewind_t req;
req.subtype = PLAYER_LOG_SET_READ_REWIND_REQ;
if(playerc_client_request(device->info.client, &device->info,
&req, sizeof(req),
&req, sizeof(req)) < 0)
{
PLAYERC_ERR("failed to rewind data playback");
return(-1);
}
return(0);
}
// Calculate scan points
void playerc_ranger_calculate_points(playerc_ranger_t *device)
{
double b, r, s;
uint32_t ii;
device->points_count = device->ranges_count;
if (device->points_count == 0 && device->points != NULL)
{
free(device->points);
device->points = NULL;
}
else
{
if((device->points = (player_point_3d_t *) realloc(device->points, device->points_count * sizeof(player_point_3d_t))) == NULL)
{
device->points_count = 0;
PLAYERC_ERR("Failed to allocate space to store points");
return;
}
if (device->points_count >= device->element_count)
{
if (device->element_count == 1)
{
b = device->min_angle;
for (ii = 0; ii < device->points_count; ii++)
{
r = device->ranges[ii];
device->points[ii].px = r * cos(b);
device->points[ii].py = r * sin(b);
device->points[ii].pz = 0.0;
b += device->angular_res;
}
}
else
{
for (ii = 0; ii < device->element_count; ii++)
{
r = device->ranges[ii];
s = r * cos(device->element_poses[ii].ppitch);
device->points[ii].px = s * cos(device->element_poses[ii].pyaw) + device->element_poses[ii].px;
device->points[ii].py = s * sin(device->element_poses[ii].pyaw) + device->element_poses[ii].py;
device->points[ii].pz = r * sin(device->element_poses[ii].ppitch) + device->element_poses[ii].pz;
}
}
}
}
}
// Start/stop playback
int playerc_log_set_read_state(playerc_log_t* device, int state)
{
player_log_set_read_state_t req;
req.subtype = PLAYER_LOG_SET_READ_STATE_REQ;
req.state = (uint8_t)state;
if(playerc_client_request(device->info.client, &device->info,
&req, sizeof(req),
&req, sizeof(req)) < 0)
{
PLAYERC_ERR("failed to start/stop data playback");
return(-1);
}
return(0);
}
// Copy intensity data to the device
void playerc_ranger_copy_intns_data(playerc_ranger_t *device, player_ranger_data_intns_t *data)
{
if (device->intensities_count != data->intensities_count || device->intensities == NULL)
{
// Allocate memory for the new data
if((device->intensities = (double *) realloc(device->intensities, data->intensities_count * sizeof(double))) == NULL)
{
device->intensities_count = 0;
PLAYERC_ERR("Failed to allocate space to store intensity data");
return;
}
}
// Copy the range data
if (data->intensities_count > 0) memcpy(device->intensities, data->intensities, data->intensities_count * sizeof(data->intensities[0]));
device->intensities_count = data->intensities_count;
}
// Remove a device proxy
int playerc_client_deldevice(playerc_client_t *client, playerc_device_t *device)
{
int i;
for (i = 0; i < client->device_count; i++)
{
if (client->device[i] == device)
{
memmove(client->device + i, client->device + i + 1,
(client->device_count - i - 1) * sizeof(client->device[0]));
client->device_count--;
return 0;
}
}
PLAYERC_ERR("unknown device");
return -1;
}
// Get logging/playback state; the result is written into the proxy
int playerc_log_get_state(playerc_log_t* device)
{
player_log_get_state_t req;
req.subtype = PLAYER_LOG_GET_STATE_REQ;
if(playerc_client_request(device->info.client, &device->info,
&req, sizeof(req),
&req, sizeof(req)) < 0)
{
PLAYERC_ERR("failed to get logging/playback state");
return(-1);
}
device->type = req.type;
device->state = req.state;
return(0);
}
void playerc_audio_putmsg(playerc_audio_t *device,
player_msghdr_t *header,
uint8_t *data, size_t len)
{
if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_AUDIO_DATA_WAV_REC))
{
player_audio_wav_t * wdata = (player_audio_wav_t *) data;
assert(header->size > 0);
device->wav_data.data_count = wdata->data_count;
if (device->wav_data.data != NULL)
free (device->wav_data.data);
if ((device->wav_data.data = (uint8_t*) malloc (wdata->data_count)) == NULL)
PLAYERC_ERR("Failed to allocate space to store wave data locally");
else
{
memcpy(device->wav_data.data, wdata->data, wdata->data_count * sizeof(device->wav_data.data[0]));
device->wav_data.format = wdata->format;
}
}
else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_AUDIO_DATA_SEQ))
{
player_audio_seq_t * sdata = (player_audio_seq_t *) data;
assert(header->size > 0);
device->seq_data.tones_count = sdata->tones_count;
memcpy(device->seq_data.tones, sdata->tones, sdata->tones_count * sizeof(device->seq_data.tones[0]));
}
else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_AUDIO_DATA_MIXER_CHANNEL))
{
player_audio_mixer_channel_list_t * wdata = (player_audio_mixer_channel_list_t *) data;
assert(header->size > 0);
device->mixer_data.channels_count = wdata->channels_count;
memcpy(device->mixer_data.channels, wdata->channels, wdata->channels_count * sizeof(device->mixer_data.channels[0]));
}
else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_AUDIO_DATA_STATE))
{
player_audio_state_t *sdata = (player_audio_state_t *) data;
assert(header->size > 0);
device->state = sdata->state;
}
else
PLAYERC_WARN2("skipping audio message with unknown type/subtype: %s/%d\n",
msgtype_to_str(header->type), header->subtype);
}
// Calculate bearings
void playerc_ranger_calculate_bearings(playerc_ranger_t *device)
{
double b;
uint32_t ii;
device->bearings_count = device->ranges_count;
if (device->bearings_count == 0 && device->bearings != NULL)
{
free(device->bearings);
device->bearings = NULL;
}
else
{
if((device->bearings = (double *) realloc(device->bearings, device->bearings_count * sizeof(double))) == NULL)
{
device->bearings_count = 0;
PLAYERC_ERR("Failed to allocate space to store bearings");
return;
}
if (device->bearings_count >= device->element_count)
{
if (device->element_count == 1)
{
b = device->min_angle;
for (ii = 0; ii < device->bearings_count; ii++)
{
device->bearings[ii] = b + device->device_pose.pyaw;
b += device->angular_res;
}
}
else
{
for (ii = 0; ii < device->element_count; ii++)
{
device->bearings[ii] = device->element_poses[ii].pyaw;
}
}
}
}
}
// Get the list of available device ids. The data is written into the
// proxy structure rather than returned to the caller.
int playerc_client_get_devlist(playerc_client_t *client)
{
uint32_t i;
player_device_devlist_t *rep_config;
if(playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_DEVLIST,
NULL, (void**)&rep_config) < 0)
{
PLAYERC_ERR("failed to get response");
return(-1);
}
for (i = 0; i < rep_config->devices_count; i++)
client->devinfos[i].addr = rep_config->devices[i];
client->devinfo_count = rep_config->devices_count;
player_device_devlist_t_free(rep_config);
// Now get the driver info
return playerc_client_get_driverinfo(client);
}
int playerc_device_get_strprop(playerc_device_t *device, char *property, char **value)
{
int result = 0;
player_strprop_req_t req, *resp;
req.key = property;
req.key_count = strlen (property) + 1;
req.value = NULL;
req.value_count = 0;
if((result = playerc_client_request(device->client, device,
PLAYER_GET_STRPROP_REQ, &req, (void**)&resp)) < 0)
return result;
if (((*value) = strdup (resp->value)) == NULL)
{
player_strprop_req_t_free(resp);
PLAYERC_ERR ("Failed to allocate memory to store property value");
return -1;
}
player_strprop_req_t_free(resp);
return 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;
}
// Read a raw packet
int playerc_client_readpacket(playerc_client_t *client,
player_msghdr_t *header,
char *data)
{
int nbytes;
player_pack_fn_t packfunc;
int decode_msglen;
if (client->sock < 0)
{
PLAYERC_WARN("no socket to read from");
return -1;
}
while(client->read_xdrdata_len < PLAYERXDR_MSGHDR_SIZE)
{
nbytes = timed_recv(client->sock,
client->read_xdrdata + client->read_xdrdata_len,
PLAYERXDR_MSGHDR_SIZE - client->read_xdrdata_len,
0, (int) client->request_timeout * 1000);
if (nbytes <= 0)
{
if(nbytes == 0)
return -1;
if(errno == EINTR)
continue;
else
{
STRERROR (PLAYERC_ERR2, "recv failed with error [%d: %s]");
//playerc_client_disconnect(client);
if(playerc_client_disconnect_retry(client) < 0)
return(-1);
else
continue;
}
}
client->read_xdrdata_len += nbytes;
}
// Unpack the header
if(player_msghdr_pack(client->read_xdrdata,
PLAYERXDR_MSGHDR_SIZE,
header, PLAYERXDR_DECODE) < 0)
{
PLAYERC_ERR("failed to unpack header");
return -1;
}
if (header->size > PLAYERXDR_MAX_MESSAGE_SIZE - PLAYERXDR_MSGHDR_SIZE)
{
PLAYERC_WARN1("packet is too large, %d bytes", header->size);
}
// Slide over the header
memmove(client->read_xdrdata,
client->read_xdrdata + PLAYERXDR_MSGHDR_SIZE,
client->read_xdrdata_len - PLAYERXDR_MSGHDR_SIZE);
client->read_xdrdata_len -= PLAYERXDR_MSGHDR_SIZE;
while(client->read_xdrdata_len < header->size)
{
nbytes = timed_recv(client->sock,
client->read_xdrdata + client->read_xdrdata_len,
header->size - client->read_xdrdata_len,
0, (int) client->request_timeout*1000);
if (nbytes <= 0)
{
if(errno == EINTR)
continue;
{
STRERROR (PLAYERC_ERR2, "recv failed with error [%d: %s]");
//playerc_client_disconnect(client);
if(playerc_client_disconnect_retry(client) < 0)
return(-1);
else
{
/* Need to start over; the easiest way is to recursively call
* myself. Might be problematic... */
return(playerc_client_readpacket(client,header,data));
}
}
}
client->read_xdrdata_len += nbytes;
}
if (header->size)
{
// Locate the appropriate unpacking function for the message body
if(!(packfunc = playerxdr_get_packfunc(header->addr.interf, header->type,
header->subtype)))
{
// TODO: Allow the user to register a callback to handle unsupported
// messages
PLAYERC_ERR4("skipping message from %s:%u with unsupported type %s:%u",
interf_to_str(header->addr.interf), header->addr.index, msgtype_to_str(header->type), header->subtype);
// Slide over the body
memmove(client->read_xdrdata,
client->read_xdrdata + header->size,
client->read_xdrdata_len - header->size);
client->read_xdrdata_len -= header->size;
return(-1);
}
// Unpack the body
if((decode_msglen = (*packfunc)(client->read_xdrdata,
header->size, data, PLAYERXDR_DECODE)) < 0)
{
PLAYERC_ERR4("decoding failed on message from %s:%u with type %s:%u",
interf_to_str(header->addr.interf), header->addr.index, msgtype_to_str(header->type), header->subtype);
return(-1);
}
}
else
{
decode_msglen = 0;
}
// Slide over the body
memmove(client->read_xdrdata,
client->read_xdrdata + header->size,
client->read_xdrdata_len - header->size);
client->read_xdrdata_len -= header->size;
// Rewrite the header with the decoded message length
header->size = decode_msglen;
return 0;
}
// Write a raw packet
int playerc_client_writepacket(playerc_client_t *client,
player_msghdr_t *header, const char *data)
{
int bytes, ret, length;
player_pack_fn_t packfunc;
int encode_msglen;
struct timeval curr;
if (client->sock < 0)
{
PLAYERC_WARN("no socket to write to");
return -1;
}
// Encode the body first, if it's non-NULL
if(data)
{
// Locate the appropriate packing function for the message body
if(!(packfunc = playerxdr_get_packfunc(header->addr.interf,
header->type,
header->subtype)))
{
// TODO: Allow the user to register a callback to handle unsupported
// messages
PLAYERC_ERR4("skipping message to %s:%u with unsupported type %s:%u",
interf_to_str(header->addr.interf), header->addr.index, msgtype_to_str(header->type), header->subtype);
return(-1);
}
if((encode_msglen =
(*packfunc)(client->write_xdrdata + PLAYERXDR_MSGHDR_SIZE,
PLAYER_MAX_MESSAGE_SIZE - PLAYERXDR_MSGHDR_SIZE,
(void*) data, PLAYERXDR_ENCODE)) < 0)
{
PLAYERC_ERR4("encoding failed on message from %s:%u with type %s:%u",
interf_to_str(header->addr.interf), header->addr.index, msgtype_to_str(header->type), header->subtype);
return(-1);
}
}
else
encode_msglen = 0;
// Write in the encoded size and current time
header->size = encode_msglen;
gettimeofday(&curr,NULL);
header->timestamp = curr.tv_sec + curr.tv_usec / 1e6;
// Pack the header
if(player_msghdr_pack(client->write_xdrdata, PLAYERXDR_MSGHDR_SIZE,
header, PLAYERXDR_ENCODE) < 0)
{
PLAYERC_ERR("failed to pack header");
return -1;
}
// Send the message
length = PLAYERXDR_MSGHDR_SIZE + encode_msglen;
bytes = PLAYERXDR_MSGHDR_SIZE + encode_msglen;
do
{
ret = send(client->sock, &client->write_xdrdata[length-bytes],
bytes, 0);
if (ret > 0)
{
bytes -= ret;
}
#if defined (WIN32)
else if (ret < 0 && (errno != ERRNO_EAGAIN && errno != WSAEINPROGRESS))
#else
else if (ret < 0 && (errno != ERRNO_EAGAIN && errno != EINPROGRESS && errno != EWOULDBLOCK))
#endif
{
STRERROR (PLAYERC_ERR2, "send on body failed with error [%d: %s]");
//playerc_client_disconnect(client);
return(playerc_client_disconnect_retry(client));
}
} while (bytes);
return 0;
}
// Issue request and await reply (blocking).
int playerc_client_request(playerc_client_t *client,
playerc_device_t *deviceinfo,
uint8_t subtype,
const void *req_data, void **rep_data)
{
int peek;
struct timeval start;
struct timeval curr;
player_msghdr_t req_header, rep_header;
memset(&req_header, 0, sizeof(req_header));
if(deviceinfo == NULL)
{
req_header.addr.interf = PLAYER_PLAYER_CODE;
req_header.type = PLAYER_MSGTYPE_REQ;
}
else
{
req_header.addr = deviceinfo->addr;
req_header.type = PLAYER_MSGTYPE_REQ;
}
req_header.subtype = subtype;
if (playerc_client_writepacket(client, &req_header, req_data) < 0)
return -1;
// Read packets until we get a reply. Data packets get queued up
// for later processing.
gettimeofday(&start,NULL);
for(curr = start; tdiff(start,curr)< client->request_timeout; gettimeofday(&curr,NULL))
{
// Peek at the socket
if((peek = playerc_client_internal_peek(client,10)) < 0)
return -1;
else if(peek == 0)
continue;
// There's data on the socket, so read a packet (blocking).
if(playerc_client_readpacket(client, &rep_header, client->data) < 0)
return -1;
if (rep_header.type == PLAYER_MSGTYPE_DATA || rep_header.type == PLAYER_MSGTYPE_SYNCH)
{
// Queue up any incoming data and sync packets for later processing
playerc_client_push(client, &rep_header, client->data);
}
else if(rep_header.type == PLAYER_MSGTYPE_RESP_ACK)
{
// Using TCP, we only need to check the interface and index
if (rep_header.addr.interf != req_header.addr.interf ||
rep_header.addr.index != req_header.addr.index ||
rep_header.subtype != req_header.subtype)
{
PLAYERC_ERR6("got the wrong kind of reply (%d %d %d != %d %d %d).",rep_header.addr.interf,
rep_header.addr.index, rep_header.subtype, req_header.addr.interf, req_header.addr.index,
req_header.subtype);
return -1;
}
if (rep_header.size > 0)
{
if (rep_data)
{
*rep_data = playerxdr_clone_message(client->data,rep_header.addr.interf, rep_header.type, rep_header.subtype);
}
playerxdr_cleanup_message(client->data,rep_header.addr.interf, rep_header.type, rep_header.subtype);
}
return(0);
}
else if (rep_header.type == PLAYER_MSGTYPE_RESP_NACK)
{
// Using TCP, we only need to check the interface and index
if (rep_header.addr.interf != req_header.addr.interf ||
rep_header.addr.index != req_header.addr.index ||
rep_header.subtype != req_header.subtype)
{
PLAYERC_ERR("got the wrong kind of reply (not good).");
return -1;
}
PLAYERC_ERR("got NACK from request");
return -2;
}
}
PLAYERC_ERR4("timed out waiting for server reply to request %s:%d:%s:%d", interf_to_str(req_header.addr.interf), req_header.addr.index, msgtype_to_str(req_header.type), req_header.subtype);
return -1;
}
