static lcm_provider_t *lcm_memq_create(lcm_t *parent, const char *target, const GHashTable *args)
{
lcm_memq_t *self = (lcm_memq_t *) calloc(1, sizeof(lcm_memq_t));
self->lcm = parent;
self->queue = g_queue_new();
self->mutex = g_mutex_new();
dbg(DBG_LCM, "Initializing LCM memq provider context...\n");
if (lcm_internal_pipe_create(self->notify_pipe) != 0) {
perror(__FILE__ " - pipe (notify)");
lcm_memq_destroy(self);
return NULL;
}
return self;
}
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;
}
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;
}
lcm_provider_t *
lcm_udpm_create (lcm_t * parent, const char *network, const GHashTable *args)
{
udpm_params_t params;
memset (¶ms, 0, sizeof (udpm_params_t));
g_hash_table_foreach ((GHashTable*) args, new_argument, ¶ms);
if (parse_mc_addr_and_port (network, ¶ms) < 0) {
return NULL;
}
lcm_udpm_t * lcm = (lcm_udpm_t *) calloc (1, sizeof (lcm_udpm_t));
lcm->lcm = parent;
lcm->params = params;
lcm->recvfd = -1;
lcm->sendfd = -1;
lcm->thread_msg_pipe[0] = lcm->thread_msg_pipe[1] = -1;
lcm->udp_low_watermark = 1.0;
lcm->kernel_rbuf_sz = 0;
lcm->warned_about_small_kernel_buf = 0;
lcm->frag_bufs = NULL;
// synchronization variables used when allocating receive resources
lcm->creating_read_thread = 0;
lcm->create_read_thread_mutex = NULL;
lcm->create_read_thread_cond = NULL;
// internal notification pipe
if(0 != lcm_internal_pipe_create(lcm->notify_pipe)) {
perror(__FILE__ " pipe(create)");
lcm_udpm_destroy (lcm);
return NULL;
}
fcntl (lcm->notify_pipe[1], F_SETFL, O_NONBLOCK);
g_static_rec_mutex_init (&lcm->mutex);
g_static_mutex_init (&lcm->transmit_lock);
dbg (DBG_LCM, "Initializing LCM UDPM context...\n");
dbg (DBG_LCM, "Multicast %s:%d\n", inet_ntoa(params.mc_addr), ntohs (params.mc_port));
// setup destination multicast address
memset (&lcm->dest_addr, 0, sizeof (lcm->dest_addr));
lcm->dest_addr.sin_family = AF_INET;
lcm->dest_addr.sin_addr = params.mc_addr;
lcm->dest_addr.sin_port = params.mc_port;
// test connectivity
SOCKET testfd = socket (AF_INET, SOCK_DGRAM, 0);
if (connect (testfd, (struct sockaddr*) &lcm->dest_addr,
sizeof (lcm->dest_addr)) < 0) {
perror ("connect");
lcm_udpm_destroy (lcm);
#ifdef __linux__
linux_check_routing_table(lcm->dest_addr.sin_addr);
#endif
return NULL;
}
lcm_close_socket(testfd);
// create a transmit socket
//
// don't use connect() on the actual transmit socket, because linux then
// has problems multicasting to localhost
lcm->sendfd = socket (AF_INET, SOCK_DGRAM, 0);
// set multicast TTL
if (params.mc_ttl == 0) {
dbg (DBG_LCM, "LCM multicast TTL set to 0. Packets will not "
"leave localhost\n");
}
dbg (DBG_LCM, "LCM: setting multicast packet TTL to %d\n", params.mc_ttl);
if (setsockopt (lcm->sendfd, IPPROTO_IP, IP_MULTICAST_TTL,
(char *) ¶ms.mc_ttl, sizeof (params.mc_ttl)) < 0) {
perror ("setsockopt(IPPROTO_IP, IP_MULTICAST_TTL)");
lcm_udpm_destroy (lcm);
return NULL;
}
#ifdef WIN32
// Windows has small (8k) buffer by default
// increase the send buffer to a reasonable amount.
int send_buf_size = 256 * 1024;
setsockopt(lcm->sendfd, SOL_SOCKET, SO_SNDBUF,
(char*)&send_buf_size, sizeof(send_buf_size));
#endif
// debugging... how big is the send buffer?
int sockbufsize = 0;
unsigned int retsize = sizeof(int);
getsockopt(lcm->sendfd, SOL_SOCKET, SO_SNDBUF,
(char*)&sockbufsize, (socklen_t *) &retsize);
dbg (DBG_LCM, "LCM: send buffer is %d bytes\n", sockbufsize);
// set loopback option on the send socket
#ifdef __sun__
unsigned char send_lo_opt = 1;
#else
unsigned int send_lo_opt = 1;
#endif
if (setsockopt (lcm->sendfd, IPPROTO_IP, IP_MULTICAST_LOOP,
(char *) &send_lo_opt, sizeof (send_lo_opt)) < 0) {
perror ("setsockopt (IPPROTO_IP, IP_MULTICAST_LOOP)");
lcm_udpm_destroy (lcm);
return NULL;
}
// don't start the receive thread yet. Only allocate resources for
// receiving messages when a subscription is made.
// However, we still need to setup sendfd in multi-cast group
struct ip_mreq mreq;
mreq.imr_multiaddr = lcm->params.mc_addr;
mreq.imr_interface.s_addr = INADDR_ANY;
dbg (DBG_LCM, "LCM: joining multicast group\n");
if (setsockopt (lcm->sendfd, IPPROTO_IP, IP_ADD_MEMBERSHIP,
(char*)&mreq, sizeof (mreq)) < 0) {
#ifdef WIN32
// ignore this error in windows... see issue #60
#else
perror ("setsockopt (IPPROTO_IP, IP_ADD_MEMBERSHIP)");
lcm_udpm_destroy (lcm);
return NULL;
#endif
}
return lcm;
}
static lcm_provider_t *
lcm_logprov_create (lcm_t * parent, const char *target, const GHashTable *args)
{
if (!target || !strlen (target)) {
fprintf (stderr, "Error: Missing filename\n");
return NULL;
}
lcm_logprov_t * lr = (lcm_logprov_t *) calloc (1, sizeof (lcm_logprov_t));
lr->lcm = parent;
lr->filename = strdup(target);
lr->speed = 1;
lr->next_clock_time = -1;
lr->start_timestamp = -1;
g_hash_table_foreach ((GHashTable*) args, new_argument, lr);
dbg (DBG_LCM, "Initializing LCM log provider context...\n");
dbg (DBG_LCM, "Filename %s\n", lr->filename);
if(lcm_internal_pipe_create(lr->notify_pipe) != 0) {
perror(__FILE__ " - pipe (notify)");
lcm_logprov_destroy (lr);
return NULL;
}
if(lcm_internal_pipe_create(lr->timer_pipe) != 0) {
perror(__FILE__ " - pipe (timer)");
lcm_logprov_destroy (lr);
return NULL;
}
//fcntl (lcm->notify_pipe[1], F_SETFL, O_NONBLOCK);
if (!lr->writer) {
lr->log = lcm_eventlog_create (lr->filename, "r");
} else {
lr->log = lcm_eventlog_create (lr->filename, "w");
}
if (!lr->log) {
fprintf (stderr, "Error: Failed to open %s: %s\n", lr->filename,
strerror (errno));
lcm_logprov_destroy (lr);
return NULL;
}
// only start the reader thread if not in write mode
if (!lr->writer){
if (load_next_event (lr) < 0) {
fprintf (stderr, "Error: Failed to read first event from log\n");
lcm_logprov_destroy (lr);
return NULL;
}
/* Start the reader thread */
lr->timer_thread = g_thread_create (timer_thread, lr, TRUE, NULL);
if (!lr->timer_thread) {
fprintf (stderr, "Error: LCM failed to start timer thread\n");
lcm_logprov_destroy (lr);
return NULL;
}
lr->thread_created = 1;
if(lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1) < 0) {
perror(__FILE__ " - write (reader create)");
}
if(lr->start_timestamp > 0){
dbg (DBG_LCM, "Seeking to timestamp: %lld\n", (long long)lr->start_timestamp);
lcm_eventlog_seek_to_timestamp(lr->log, lr->start_timestamp);
}
}
return lr;
}
