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;
}
static int
_setup_recv_parts (lcm_udpm_t *lcm)
{
g_static_rec_mutex_lock(&lcm->mutex);
// some thread synchronization code to ensure that only one thread sets up the
// receive thread, and that all threads entering this function after the thread
// setup begins wait for it to finish.
if(lcm->creating_read_thread) {
// check if this thread is the one creating the receive thread.
// If so, just return.
if(g_static_private_get(&CREATE_READ_THREAD_PKEY)) {
g_static_rec_mutex_unlock(&lcm->mutex);
return 0;
}
// ugly bit with two mutexes because we can't use a GStaticRecMutex with a GCond
g_mutex_lock(lcm->create_read_thread_mutex);
g_static_rec_mutex_unlock(&lcm->mutex);
// wait for the thread creating the read thread to finish
while(lcm->creating_read_thread) {
g_cond_wait(lcm->create_read_thread_cond, lcm->create_read_thread_mutex);
}
g_mutex_unlock(lcm->create_read_thread_mutex);
g_static_rec_mutex_lock(&lcm->mutex);
// if we've gotten here, then either the read thread is created, or it
// was not possible to do so. Figure out which happened, and return.
int result = lcm->thread_created ? 0 : -1;
g_static_rec_mutex_unlock(&lcm->mutex);
return result;
} else if(lcm->thread_created) {
g_static_rec_mutex_unlock(&lcm->mutex);
return 0;
}
// no other thread is trying to create the read thread right now. claim that task.
lcm->creating_read_thread = 1;
lcm->create_read_thread_mutex = g_mutex_new();
lcm->create_read_thread_cond = g_cond_new();
// mark this thread as the one creating the read thread
g_static_private_set(&CREATE_READ_THREAD_PKEY, GINT_TO_POINTER(1), NULL);
dbg (DBG_LCM, "allocating resources for receiving messages\n");
// allocate the fragment buffer hashtable
lcm->frag_bufs = lcm_frag_buf_store_new(MAX_FRAG_BUF_TOTAL_SIZE,
MAX_NUM_FRAG_BUFS);
// allocate multicast socket
lcm->recvfd = socket (AF_INET, SOCK_DGRAM, 0);
if (lcm->recvfd < 0) {
perror ("allocating LCM recv socket");
goto setup_recv_thread_fail;
}
struct sockaddr_in addr;
memset (&addr, 0, sizeof (addr));
addr.sin_family = AF_INET;
addr.sin_addr.s_addr = INADDR_ANY;
addr.sin_port = lcm->params.mc_port;
// allow other applications on the local machine to also bind to this
// multicast address and port
int opt=1;
dbg (DBG_LCM, "LCM: setting SO_REUSEADDR\n");
if (setsockopt (lcm->recvfd, SOL_SOCKET, SO_REUSEADDR,
(char*)&opt, sizeof (opt)) < 0) {
perror ("setsockopt (SOL_SOCKET, SO_REUSEADDR)");
goto setup_recv_thread_fail;
}
#ifdef USE_REUSEPORT
/* Mac OS and FreeBSD require the REUSEPORT option in addition
* to REUSEADDR or it won't let multiple processes bind to the
* same port, even if they are using multicast. */
dbg (DBG_LCM, "LCM: setting SO_REUSEPORT\n");
if (setsockopt (lcm->recvfd, SOL_SOCKET, SO_REUSEPORT,
(char*)&opt, sizeof (opt)) < 0) {
perror ("setsockopt (SOL_SOCKET, SO_REUSEPORT)");
goto setup_recv_thread_fail;
}
#endif
#if 0
// set loopback option so that packets sent out on the multicast socket
// are also delivered to it
unsigned char lo_opt = 1;
dbg (DBG_LCM, "LCM: setting multicast loopback option\n");
status = setsockopt (lcm->recvfd, IPPROTO_IP, IP_MULTICAST_LOOP,
&lo_opt, sizeof (lo_opt));
if (status < 0) {
perror ("setting multicast loopback");
return -1;
}
#endif
#ifdef WIN32
// Windows has small (8k) buffer by default
// Increase it to a default reasonable amount
int recv_buf_size = 2048 * 1024;
setsockopt(lcm->recvfd, SOL_SOCKET, SO_RCVBUF,
(char*)&recv_buf_size, sizeof(recv_buf_size));
#endif
// debugging... how big is the receive buffer?
unsigned int retsize = sizeof (int);
getsockopt (lcm->recvfd, SOL_SOCKET, SO_RCVBUF,
(char*)&lcm->kernel_rbuf_sz, (socklen_t *) &retsize);
dbg (DBG_LCM, "LCM: receive buffer is %d bytes\n", lcm->kernel_rbuf_sz);
if (lcm->params.recv_buf_size) {
if (setsockopt (lcm->recvfd, SOL_SOCKET, SO_RCVBUF,
(char *) &lcm->params.recv_buf_size,
sizeof (lcm->params.recv_buf_size)) < 0) {
perror ("setsockopt(SOL_SOCKET, SO_RCVBUF)");
fprintf (stderr, "Warning: Unable to set recv buffer size\n");
}
getsockopt (lcm->recvfd, SOL_SOCKET, SO_RCVBUF,
(char*)&lcm->kernel_rbuf_sz, (socklen_t *) &retsize);
dbg (DBG_LCM, "LCM: receive buffer is %d bytes\n", lcm->kernel_rbuf_sz);
if (lcm->params.recv_buf_size > lcm->kernel_rbuf_sz) {
g_warning ("LCM UDP receive buffer size (%d) \n"
" is smaller than reqested (%d). "
"For more info:\n"
" http://lcm-proj.github.io/multicast_setup.html\n",
lcm->kernel_rbuf_sz, lcm->params.recv_buf_size);
}
}
/* Enable per-packet timestamping by the kernel, if available */
#ifdef SO_TIMESTAMP
opt = 1;
setsockopt (lcm->recvfd, SOL_SOCKET, SO_TIMESTAMP, &opt, sizeof (opt));
#endif
if (bind (lcm->recvfd, (struct sockaddr*)&addr, sizeof (addr)) < 0) {
perror ("bind");
goto setup_recv_thread_fail;
}
struct ip_mreq mreq;
mreq.imr_multiaddr = lcm->params.mc_addr;
mreq.imr_interface.s_addr = INADDR_ANY;
// join the multicast group
dbg (DBG_LCM, "LCM: joining multicast group\n");
if (setsockopt (lcm->recvfd, IPPROTO_IP, IP_ADD_MEMBERSHIP,
(char*)&mreq, sizeof (mreq)) < 0) {
perror ("setsockopt (IPPROTO_IP, IP_ADD_MEMBERSHIP)");
goto setup_recv_thread_fail;
}
lcm->inbufs_empty = lcm_buf_queue_new ();
lcm->inbufs_filled = lcm_buf_queue_new ();
lcm->ringbuf = lcm_ringbuf_new (LCM_RINGBUF_SIZE);
int i;
for (i = 0; i < LCM_DEFAULT_RECV_BUFS; i++) {
/* We don't set the receive buffer's data pointer yet because it
* will be taken from the ringbuffer at receive time. */
lcm_buf_t * lcmb = (lcm_buf_t *) calloc (1, sizeof (lcm_buf_t));
lcm_buf_enqueue (lcm->inbufs_empty, lcmb);
}
// setup a pipe for notifying the reader thread when to quit
if(0 != lcm_internal_pipe_create(lcm->thread_msg_pipe)) {
perror(__FILE__ " pipe(setup)");
goto setup_recv_thread_fail;
}
fcntl (lcm->thread_msg_pipe[1], F_SETFL, O_NONBLOCK);
/* Start the reader thread */
lcm->read_thread = g_thread_create (recv_thread, lcm, TRUE, NULL);
if (!lcm->read_thread) {
fprintf (stderr, "Error: LCM failed to start reader thread\n");
goto setup_recv_thread_fail;
}
lcm->thread_created = 1;
g_static_rec_mutex_unlock(&lcm->mutex);
// conduct a self-test just to make sure everything is working.
dbg (DBG_LCM, "LCM: conducting self test\n");
int self_test_results = udpm_self_test(lcm);
g_static_rec_mutex_lock(&lcm->mutex);
if (0 == self_test_results) {
dbg (DBG_LCM, "LCM: self test successful\n");
} else {
// self test failed. destroy the read thread
fprintf (stderr, "LCM self test failed!!\n"
"Check your routing tables and firewall settings\n");
_destroy_recv_parts (lcm);
}
// notify threads waiting for the read thread to be created
g_mutex_lock(lcm->create_read_thread_mutex);
lcm->creating_read_thread = 0;
g_cond_broadcast(lcm->create_read_thread_cond);
g_mutex_unlock(lcm->create_read_thread_mutex);
g_static_rec_mutex_unlock(&lcm->mutex);
return self_test_results;
setup_recv_thread_fail:
_destroy_recv_parts (lcm);
g_static_rec_mutex_unlock(&lcm->mutex);
return -1;
}
// read continuously until a complete message arrives
static lcm_buf_t *
udp_read_packet (lcm_udpm_t *lcm)
{
lcm_buf_t *lcmb = NULL;
int sz = 0;
g_static_rec_mutex_lock (&lcm->mutex);
double buf_avail = lcm_ringbuf_available(lcm->ringbuf);
g_static_rec_mutex_unlock (&lcm->mutex);
if (buf_avail < lcm->udp_low_watermark)
lcm->udp_low_watermark = buf_avail;
GTimeVal tv;
g_get_current_time(&tv);
int elapsedsecs = tv.tv_sec - lcm->udp_last_report_secs;
if (elapsedsecs > 2) {
uint32_t total_bad = lcm->udp_discarded_lcmb +
lcm->udp_discarded_buf +
lcm->udp_discarded_bad;
if (total_bad > 0 || lcm->udp_low_watermark < 0.5) {
fprintf(stderr,
"%d.%03d LCM loss %4.1f%% : %5d lcmb, %5d buf, %5d err, "
"buf avail %4.1f%%\n",
(int) tv.tv_sec, (int) tv.tv_usec/1000,
total_bad * 100.0 / (lcm->udp_rx + total_bad),
lcm->udp_discarded_lcmb,
lcm->udp_discarded_buf,
lcm->udp_discarded_bad,
100.0 * lcm->udp_low_watermark);
lcm->udp_rx = 0;
lcm->udp_discarded_lcmb = 0;
lcm->udp_discarded_buf = 0;
lcm->udp_discarded_bad = 0;
lcm->udp_last_report_secs = tv.tv_sec;
lcm->udp_low_watermark = HUGE;
}
}
int got_complete_message = 0;
while (!got_complete_message) {
// wait for either incoming UDP data, or for an abort message
fd_set fds;
FD_ZERO (&fds);
FD_SET (lcm->recvfd, &fds);
FD_SET (lcm->thread_msg_pipe[0], &fds);
SOCKET maxfd = MAX(lcm->recvfd, lcm->thread_msg_pipe[0]);
if (select (maxfd + 1, &fds, NULL, NULL, NULL) <= 0) {
perror ("udp_read_packet -- select:");
continue;
}
if (FD_ISSET (lcm->thread_msg_pipe[0], &fds)) {
// received an exit command.
dbg (DBG_LCM, "read thread received exit command\n");
if (lcmb) {
// lcmb is not on one of the memory managed buffer queues. We could
// either put it back on one of the queues, or just free it here. Do the
// latter.
//
// Can also just free its lcm_buf_t here. Its data buffer is
// managed either by the ring buffer or the fragment buffer, so
// we can ignore it.
free (lcmb);
}
return NULL;
}
// there is incoming UDP data ready.
assert (FD_ISSET (lcm->recvfd, &fds));
if (!lcmb) {
// try to allocate space on the ringbuffer for the new data
// first allocate a buffer struct
g_static_rec_mutex_lock (&lcm->mutex);
lcmb = lcm_buf_dequeue (lcm->inbufs_empty);
if (!lcmb) {
g_static_rec_mutex_unlock (&lcm->mutex);
udp_discard_packet (lcm);
continue;
}
lcmb->buf_from_ringbuf = 1;
// next allocate space on the ringbuffer.
// give it the maximum possible size for an unfragmented packet
lcmb->buf = lcm_ringbuf_alloc(lcm->ringbuf, 65536);
g_static_rec_mutex_unlock (&lcm->mutex);
if (!lcmb->buf) {
// ringbuffer is full. discard the packet, put lcmb back on the
// empty queue, and start waiting again
udp_discard_packet (lcm);
lcm_buf_enqueue (lcm->inbufs_empty, lcmb);
lcmb = NULL;
continue;
}
// zero the last byte so that strlen never segfaults
lcmb->buf[65535] = 0;
}
struct iovec vec;
vec.iov_base = lcmb->buf;
vec.iov_len = 65535;
#ifdef MSG_EXT_HDR
// operating systems that provide SO_TIMESTAMP allow us to obtain more
// accurate timestamps by having the kernel produce timestamps as soon
// as packets are received.
char controlbuf[64];
#endif
struct msghdr msg;
msg.msg_name = &lcmb->from;
msg.msg_namelen = sizeof (struct sockaddr);
msg.msg_iov = &vec;
msg.msg_iovlen = 1;
#ifdef MSG_EXT_HDR
msg.msg_control = controlbuf;
msg.msg_controllen = sizeof (controlbuf);
msg.msg_flags = 0;
#endif
sz = recvmsg (lcm->recvfd, &msg, 0);
if (sz < 0) {
perror ("udp_read_packet -- recvmsg");
lcm->udp_discarded_bad++;
continue;
}
if (sz < sizeof(lcm2_header_short_t)) {
// packet too short to be LCM
lcm->udp_discarded_bad++;
continue;
}
lcmb->fromlen = msg.msg_namelen;
int got_utime = 0;
#ifdef SO_TIMESTAMP
struct cmsghdr * cmsg = CMSG_FIRSTHDR (&msg);
/* Get the receive timestamp out of the packet headers if possible */
while (!lcmb->recv_utime && cmsg) {
if (cmsg->cmsg_level == SOL_SOCKET &&
cmsg->cmsg_type == SCM_TIMESTAMP) {
// GTimeVal is identical to struct timeval, so this cast is ok
GTimeVal * t = (GTimeVal*) CMSG_DATA (cmsg);
lcmb->recv_utime = (int64_t) t->tv_sec * 1000000 + t->tv_usec;
got_utime = 1;
break;
}
cmsg = CMSG_NXTHDR (&msg, cmsg);
}
#endif
if (!got_utime)
lcmb->recv_utime = _timestamp_now ();
lcm2_header_short_t *hdr2 = (lcm2_header_short_t*) lcmb->buf;
uint32_t rcvd_magic = ntohl(hdr2->magic);
if (rcvd_magic == LCM2_MAGIC_SHORT)
got_complete_message = _recv_short_message (lcm, lcmb, sz);
else if (rcvd_magic == LCM2_MAGIC_LONG)
got_complete_message = _recv_message_fragment (lcm, lcmb, sz);
else {
dbg (DBG_LCM, "LCM: bad magic\n");
lcm->udp_discarded_bad++;
continue;
}
}
// if the newly received packet is a short packet, then resize the space
// allocated to it on the ringbuffer to exactly match the amount of space
// required. That way, we do not use 64k of the ringbuffer for every
// incoming message.
if (lcmb->buf_from_ringbuf) {
g_static_rec_mutex_lock (&lcm->mutex);
lcm_ringbuf_shrink_last(lcm->ringbuf, lcmb->buf, sz);
g_static_rec_mutex_unlock (&lcm->mutex);
}
return lcmb;
}
static int
_setup_recv_thread (lcm_udpm_t *lcm)
{
assert (!lcm->thread_created);
dbg (DBG_LCM, "allocating resources for receiving messages\n");
// allocate multicast socket
lcm->recvfd = socket (AF_INET, SOCK_DGRAM, 0);
if (lcm->recvfd < 0) {
perror ("allocating LCM recv socket");
_destroy_recv_parts (lcm);
return -1;
}
struct sockaddr_in addr;
memset (&addr, 0, sizeof (addr));
addr.sin_family = AF_INET;
addr.sin_addr.s_addr = INADDR_ANY;
addr.sin_port = lcm->params.mc_port;
// allow other applications on the local machine to also bind to this
// multicast address and port
int opt=1;
dbg (DBG_LCM, "LCM: setting SO_REUSEADDR\n");
if (setsockopt (lcm->recvfd, SOL_SOCKET, SO_REUSEADDR,
(char*)&opt, sizeof (opt)) < 0) {
perror ("setsockopt (SOL_SOCKET, SO_REUSEADDR)");
_destroy_recv_parts (lcm);
return -1;
}
#ifdef USE_REUSEPORT
/* Mac OS and FreeBSD require the REUSEPORT option in addition
* to REUSEADDR or it won't let multiple processes bind to the
* same port, even if they are using multicast. */
dbg (DBG_LCM, "LCM: setting SO_REUSEPORT\n");
if (setsockopt (lcm->recvfd, SOL_SOCKET, SO_REUSEPORT,
(char*)&opt, sizeof (opt)) < 0) {
perror ("setsockopt (SOL_SOCKET, SO_REUSEPORT)");
_destroy_recv_parts (lcm);
return -1;
}
#endif
#if 0
// set loopback option so that packets sent out on the multicast socket
// are also delivered to it
unsigned char lo_opt = 1;
dbg (DBG_LCM, "LCM: setting multicast loopback option\n");
status = setsockopt (lcm->recvfd, IPPROTO_IP, IP_MULTICAST_LOOP,
&lo_opt, sizeof (lo_opt));
if (status < 0) {
perror ("setting multicast loopback");
return -1;
}
#endif
#ifdef WIN32
// Windows has small (8k) buffer by default
// Increase it to a default reasonable amount
int recv_buf_size = 2048 * 1024;
setsockopt(lcm->recvfd, SOL_SOCKET, SO_RCVBUF,
(char*)&recv_buf_size, sizeof(recv_buf_size));
#endif
// debugging... how big is the receive buffer?
unsigned int retsize = sizeof (int);
getsockopt (lcm->recvfd, SOL_SOCKET, SO_RCVBUF,
(char*)&lcm->kernel_rbuf_sz, (socklen_t *) &retsize);
dbg (DBG_LCM, "LCM: receive buffer is %d bytes\n", lcm->kernel_rbuf_sz);
if (lcm->params.recv_buf_size) {
if (setsockopt (lcm->recvfd, SOL_SOCKET, SO_RCVBUF,
(char *) &lcm->params.recv_buf_size,
sizeof (lcm->params.recv_buf_size)) < 0) {
perror ("setsockopt(SOL_SOCKET, SO_RCVBUF)");
fprintf (stderr, "Warning: Unable to set recv buffer size\n");
}
getsockopt (lcm->recvfd, SOL_SOCKET, SO_RCVBUF,
(char*)&lcm->kernel_rbuf_sz, (socklen_t *) &retsize);
dbg (DBG_LCM, "LCM: receive buffer is %d bytes\n", lcm->kernel_rbuf_sz);
if (lcm->params.recv_buf_size > lcm->kernel_rbuf_sz) {
g_warning ("LCM UDP receive buffer size (%d) \n"
" is smaller than reqested (%d). "
"For more info:\n"
" http://lcm.googlecode.com/svn/www/reference/lcm/multicast-setup.html\n",
lcm->kernel_rbuf_sz, lcm->params.recv_buf_size);
}
}
/* Enable per-packet timestamping by the kernel, if available */
#ifdef SO_TIMESTAMP
opt = 1;
setsockopt (lcm->recvfd, SOL_SOCKET, SO_TIMESTAMP, &opt, sizeof (opt));
#endif
if (bind (lcm->recvfd, (struct sockaddr*)&addr, sizeof (addr)) < 0) {
perror ("bind");
_destroy_recv_parts (lcm);
return -1;
}
struct ip_mreq mreq;
mreq.imr_multiaddr = lcm->params.mc_addr;
mreq.imr_interface.s_addr = INADDR_ANY;
// join the multicast group
dbg (DBG_LCM, "LCM: joining multicast group\n");
if (setsockopt (lcm->recvfd, IPPROTO_IP, IP_ADD_MEMBERSHIP,
(char*)&mreq, sizeof (mreq)) < 0) {
perror ("setsockopt (IPPROTO_IP, IP_ADD_MEMBERSHIP)");
_destroy_recv_parts (lcm);
return -1;
}
lcm->inbufs_empty = lcm_buf_queue_new ();
lcm->inbufs_filled = lcm_buf_queue_new ();
lcm->ringbuf = lcm_ringbuf_new (LCM_RINGBUF_SIZE);
int i;
for (i = 0; i < LCM_DEFAULT_RECV_BUFS; i++) {
/* We don't set the receive buffer's data pointer yet because it
* will be taken from the ringbuffer at receive time. */
lcm_buf_t * lcmb = (lcm_buf_t *) calloc (1, sizeof (lcm_buf_t));
lcm_buf_enqueue (lcm->inbufs_empty, lcmb);
}
// setup a pipe for notifying the reader thread when to quit
if(0 != lcm_internal_pipe_create(lcm->thread_msg_pipe)) {
perror(__FILE__ " pipe(setup)");
_destroy_recv_parts (lcm);
return -1;
}
fcntl (lcm->thread_msg_pipe[1], F_SETFL, O_NONBLOCK);
/* Start the reader thread */
lcm->read_thread = g_thread_create (recv_thread, lcm, TRUE, NULL);
if (!lcm->read_thread) {
fprintf (stderr, "Error: LCM failed to start reader thread\n");
_destroy_recv_parts (lcm);
return -1;
}
lcm->thread_created = 1;
dbg (DBG_LCM, "LCM: conducting self test\n");
if (udpm_self_test (lcm) < 0) {
fprintf (stderr, "LCM self test failed!!\n"
"Check your routing tables and firewall settings\n");
_destroy_recv_parts (lcm);
return -1;
}
dbg (DBG_LCM, "LCM: self test successful\n");
return 0;
}
static int
lcm_udpm_handle (lcm_udpm_t *lcm)
{
int status;
char ch;
if (! lcm->thread_created) {
if (0 != _setup_recv_thread (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 (!is_buf_queue_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;
lcm_dispatch_handlers (lcm->lcm, &rbuf, lcmb->channel_name);
g_static_rec_mutex_lock (&lcm->mutex);
if (lcmb->buf_from_ringbuf)
lcm_ringbuf_dealloc (lcm->ringbuf, lcmb->buf);
else
free (lcmb->buf);
lcmb->buf = NULL;
lcmb->buf_size = 0;
lcm_buf_enqueue (lcm->inbufs_empty, lcmb);
g_static_rec_mutex_unlock (&lcm->mutex);
return 0;
}
