static int
lcm_udpm_handle (lcm_udpm_t *lcm)
{
int status;
char ch;
if(0 != _setup_recv_parts (lcm))
return -1;
/* Read one byte from the notify pipe. This will block if no packets are
* available yet and wake up when they are. */
status = lcm_internal_pipe_read(lcm->notify_pipe[0], &ch, 1);
if (status == 0) {
fprintf (stderr, "Error: lcm_handle read 0 bytes from notify_pipe\n");
return -1;
}
else if (status < 0) {
fprintf (stderr, "Error: lcm_handle read: %s\n", strerror (errno));
return -1;
}
/* Dequeue the next received packet */
g_static_rec_mutex_lock (&lcm->mutex);
lcm_buf_t * lcmb = lcm_buf_dequeue (lcm->inbufs_filled);
if (!lcmb) {
fprintf (stderr,
"Error: no packet available despite getting notification.\n");
g_static_rec_mutex_unlock (&lcm->mutex);
return -1;
}
/* If there are still packets in the queue, put something back in the pipe
* so that future invocations will get called. */
if (!lcm_buf_queue_is_empty (lcm->inbufs_filled))
if (lcm_internal_pipe_write(lcm->notify_pipe[1], "+", 1) < 0)
perror ("write to notify");
g_static_rec_mutex_unlock (&lcm->mutex);
lcm_recv_buf_t rbuf;
rbuf.data = (uint8_t*) lcmb->buf + lcmb->data_offset;
rbuf.data_size = lcmb->data_size;
rbuf.recv_utime = lcmb->recv_utime;
rbuf.lcm = lcm->lcm;
if(lcm->creating_read_thread) {
// special case: If we're creating the read thread and are in
// self-test mode, then only dispatch the self-test message.
if(!strcmp(lcmb->channel_name, SELF_TEST_CHANNEL))
lcm_dispatch_handlers (lcm->lcm, &rbuf, lcmb->channel_name);
} else {
lcm_dispatch_handlers (lcm->lcm, &rbuf, lcmb->channel_name);
}
g_static_rec_mutex_lock (&lcm->mutex);
lcm_buf_free_data(lcmb, lcm->ringbuf);
lcm_buf_enqueue (lcm->inbufs_empty, lcmb);
g_static_rec_mutex_unlock (&lcm->mutex);
return 0;
}
/* This is the receiver thread that runs continuously to retrieve any incoming
* LCM packets from the network and queues them locally. */
static void *
recv_thread (void * user)
{
#ifdef G_OS_UNIX
// Mask out all signals on this thread.
sigset_t mask;
sigfillset(&mask);
pthread_sigmask(SIG_SETMASK, &mask, NULL);
#endif
lcm_udpm_t * lcm = (lcm_udpm_t *) user;
while (1) {
lcm_buf_t *lcmb = udp_read_packet(lcm);
if (!lcmb) break;
/* If necessary, notify the reading thread by writing to a pipe. We
* only want one character in the pipe at a time to avoid blocking
* writes, so we only do this when the queue transitions from empty to
* non-empty. */
g_static_rec_mutex_lock (&lcm->mutex);
if (lcm_buf_queue_is_empty (lcm->inbufs_filled))
if (lcm_internal_pipe_write(lcm->notify_pipe[1], "+", 1) < 0)
perror ("write to notify");
/* Queue the packet for future retrieval by lcm_handle (). */
lcm_buf_enqueue (lcm->inbufs_filled, lcmb);
g_static_rec_mutex_unlock (&lcm->mutex);
}
dbg (DBG_LCM, "read thread exiting\n");
return NULL;
}
lcm_buf_t *
lcm_buf_allocate_data(lcm_buf_queue_t * inbufs_empty, lcm_ringbuf_t **ringbuf) {
lcm_buf_t * lcmb = NULL;
// first allocate a buffer struct for the packet metadata
if (lcm_buf_queue_is_empty(inbufs_empty)) {
// allocate additional buffer structs if needed
int i;
for (i = 0; i < LCM_DEFAULT_RECV_BUFS; i++) {
lcm_buf_t * nbuf = (lcm_buf_t *) calloc(1, sizeof(lcm_buf_t));
lcm_buf_enqueue(inbufs_empty, nbuf);
}
}
lcmb = lcm_buf_dequeue(inbufs_empty);
assert(lcmb);
// allocate space on the ringbuffer for the packet data.
// give it the maximum possible size for an unfragmented packet
lcmb->buf = lcm_ringbuf_alloc(*ringbuf, LCM_MAX_UNFRAGMENTED_PACKET_SIZE);
if (lcmb->buf == NULL) {
// ringbuffer is full. allocate a larger ringbuffer
// Can't free the old ringbuffer yet because it's in use (i.e., full)
// Must wait until later to free it.
assert(lcm_ringbuf_used(*ringbuf) > 0);
dbg(DBG_LCM, "Orphaning ringbuffer %p\n", *ringbuf);
unsigned int old_capacity = lcm_ringbuf_capacity(*ringbuf);
unsigned int new_capacity = (unsigned int) (old_capacity * 1.5);
// replace the passed in ringbuf with the new one
*ringbuf = lcm_ringbuf_new(new_capacity);
lcmb->buf = lcm_ringbuf_alloc(*ringbuf, 65536);
assert(lcmb->buf);
dbg(DBG_LCM, "Allocated new ringbuffer size %u\n", new_capacity);
}
// save a pointer to the ringbuf, in case it gets replaced by another call
lcmb->ringbuf = *ringbuf;
// zero the last byte so that strlen never segfaults
lcmb->buf[65535] = 0;
return lcmb;
}
