void
lcm_buf_free_data(lcm_buf_t *lcmb, lcm_ringbuf_t *ringbuf)
{
if(!lcmb->buf)
return;
if (lcmb->ringbuf) {
lcm_ringbuf_dealloc (lcmb->ringbuf, lcmb->buf);
// if the packet was allocated from an obsolete and empty ringbuffer,
// then deallocate the old ringbuffer as well.
if(lcmb->ringbuf != ringbuf && !lcm_ringbuf_used(lcmb->ringbuf)) {
lcm_ringbuf_free(lcmb->ringbuf);
dbg(DBG_LCM, "Destroying unused orphan ringbuffer %p\n",
lcmb->ringbuf);
}
} else {
free (lcmb->buf);
}
lcmb->buf = NULL;
lcmb->buf_size = 0;
lcmb->ringbuf = NULL;
}
static int
_recv_message_fragment (lcm_udpm_t *lcm, lcm_buf_t *lcmb, uint32_t sz)
{
lcm2_header_long_t *hdr = (lcm2_header_long_t*) lcmb->buf;
// any existing fragment buffer for this message source?
lcm_frag_buf_t *fbuf = (lcm_frag_buf_t *) g_hash_table_lookup (lcm->frag_bufs, &lcmb->from);
uint32_t msg_seqno = ntohl (hdr->msg_seqno);
uint32_t data_size = ntohl (hdr->msg_size);
uint32_t fragment_offset = ntohl (hdr->fragment_offset);
// uint16_t fragment_no = ntohs (hdr->fragment_no);
uint16_t fragments_in_msg = ntohs (hdr->fragments_in_msg);
uint32_t frag_size = sz - sizeof (lcm2_header_long_t);
char *data_start = (char*) (hdr + 1);
// discard any stale fragments from previous messages
if (fbuf && ((fbuf->msg_seqno != msg_seqno) ||
(fbuf->data_size != data_size))) {
_destroy_fragment_buffer (lcm, fbuf);
fbuf = NULL;
}
// printf ("fragment %d/%d (offset %d/%d) seq %d packet sz: %d %p\n",
// ntohs(hdr->fragment_no), fragments_in_msg,
// fragment_offset, data_size, msg_seqno, sz, fbuf);
if (data_size > LCM_MAX_MESSAGE_SIZE) {
dbg (DBG_LCM, "rejecting huge message (%d bytes)\n", data_size);
return 0;
}
// create a new fragment buffer if necessary
if (!fbuf && hdr->fragment_no == 0) {
char *channel = (char*) (hdr + 1);
int channel_sz = strlen (channel);
if (channel_sz > LCM_MAX_CHANNEL_NAME_LENGTH) {
dbg (DBG_LCM, "bad channel name length\n");
lcm->udp_discarded_bad++;
return 0;
}
// if the packet has no subscribers, drop the message now.
if (!lcm_has_handlers (lcm->lcm, channel))
return 0;
fbuf = lcm_frag_buf_new (*((struct sockaddr_in*) &lcmb->from),
channel, msg_seqno, data_size, fragments_in_msg,
lcmb->recv_utime);
_add_fragment_buffer (lcm, fbuf);
data_start += channel_sz + 1;
frag_size -= (channel_sz + 1);
}
if (!fbuf) return 0;
#ifdef __linux__
if(lcm->kernel_rbuf_sz < 262145 &&
data_size > lcm->kernel_rbuf_sz &&
! lcm->warned_about_small_kernel_buf) {
fprintf(stderr,
"==== LCM Warning ===\n"
"LCM detected that large packets are being received, but the kernel UDP\n"
"receive buffer is very small. The possibility of dropping packets due to\n"
"insufficient buffer space is very high.\n"
"\n"
"For more information, visit:\n"
" http://lcm.googlecode.com/svn/www/reference/lcm/multicast-setup.html\n\n");
lcm->warned_about_small_kernel_buf = 1;
}
#endif
if (fragment_offset + frag_size > fbuf->data_size) {
dbg (DBG_LCM, "dropping invalid fragment (off: %d, %d / %d)\n",
fragment_offset, frag_size, fbuf->data_size);
_destroy_fragment_buffer (lcm, fbuf);
return 0;
}
// copy data
memcpy (fbuf->data + fragment_offset, data_start, frag_size);
g_get_current_time(&fbuf->last_packet_time);
fbuf->fragments_remaining --;
if (0 == fbuf->fragments_remaining) {
// deallocate the ringbuffer-allocated buffer
g_static_rec_mutex_lock (&lcm->mutex);
lcm_ringbuf_dealloc (lcm->ringbuf, lcmb->buf);
g_static_rec_mutex_unlock (&lcm->mutex);
lcmb->buf_from_ringbuf = 0;
// transfer ownership of the message's payload buffer to the lcm_buf_t
lcmb->buf = fbuf->data;
fbuf->data = NULL;
strcpy (lcmb->channel_name, fbuf->channel);
lcmb->channel_size = strlen (lcmb->channel_name);
lcmb->data_offset = 0;
lcmb->data_size = fbuf->data_size;
lcmb->recv_utime = fbuf->first_packet_utime;
// don't need the fragment buffer anymore
_destroy_fragment_buffer (lcm, fbuf);
return 1;
}
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;
}
