Beispiel #1
0
static int lcm_memq_handle(lcm_memq_t *self)
{
    char ch;
    int status = lcm_internal_pipe_read(self->notify_pipe[0], &ch, 1);
    if (status == 0) {
        fprintf(stderr, "Error: lcm_memq_handle read 0 bytes from notify_pipe\n");
        return -1;
    }

    g_mutex_lock(self->mutex);
    memq_msg_t *msg = (memq_msg_t *) g_queue_pop_head(self->queue);
    if (!g_queue_is_empty(self->queue)) {
        if (lcm_internal_pipe_write(self->notify_pipe[1], "+", 1) < 0) {
            perror(__FILE__ " - write to notify pipe (lcm_memq_handle)");
        }
    }
    g_mutex_unlock(self->mutex);

    dbg(DBG_LCM, "Dispatching message on channel [%s], size [%d]\n", msg->channel,
        msg->rbuf.data_size);

    if (lcm_try_enqueue_message(self->lcm, msg->channel)) {
        lcm_dispatch_handlers(self->lcm, &msg->rbuf, msg->channel);
    }

    memq_msg_destroy(msg);
    return 0;
}
Beispiel #2
0
static int
_recv_short_message (lcm_udpm_t *lcm, lcm_buf_t *lcmb, int sz)
{
    lcm2_header_short_t *hdr2 = (lcm2_header_short_t*) lcmb->buf;

    // shouldn't have to worry about buffer overflow here because we
    // zeroed out byte #65536, which is never written to by recv
    const char *pkt_channel_str = (char*) (hdr2 + 1);

    lcmb->channel_size = strlen (pkt_channel_str);

    if (lcmb->channel_size > LCM_MAX_CHANNEL_NAME_LENGTH) {
        dbg (DBG_LCM, "bad channel name length\n");
        lcm->udp_discarded_bad++;
        return 0;
    }

    lcm->udp_rx++;

    // if the packet has no subscribers, drop the message now.
    if(!lcm_try_enqueue_message(lcm->lcm, pkt_channel_str))
        return 0;

    strcpy (lcmb->channel_name, pkt_channel_str);

    lcmb->data_offset = 
        sizeof (lcm2_header_short_t) + lcmb->channel_size + 1;

    lcmb->data_size = sz - lcmb->data_offset;
    return 1;
}
Beispiel #3
0
static int
lcm_tcpq_handle(lcm_tcpq_t * self)
{
    if(self->socket < 0 && 0 != _connect_to_server(self)) {
        return -1;
    }

    // read, ignore message type
    uint32_t msg_type;
    if(_recv_uint32(self->socket, &msg_type))
        goto disconnected;

    // read channel length, channel
    uint32_t channel_len;
    if(_recv_uint32(self->socket, &channel_len))
        goto disconnected;
    if(_ensure_buf_capacity((void**)&self->recv_channel_buf,
                &self->recv_channel_buf_len, channel_len+1)) {
        fprintf(stderr, "Memory allocation error\n");
        return -1;
    }
    if(channel_len != _recv_fully(self->socket, self->recv_channel_buf,
                channel_len))
        goto disconnected;
    self->recv_channel_buf[channel_len] = 0;

    // read payload size, payload
    uint32_t data_len;
    if(_recv_uint32(self->socket, &data_len))
        goto disconnected;
    if(_ensure_buf_capacity(&self->data_buf, &self->data_buf_len, data_len)) {
        fprintf(stderr, "Memory allocation error\n");
        return -1;
    }
    if(data_len != _recv_fully(self->socket, self->data_buf, data_len))
        goto disconnected;

    lcm_recv_buf_t rbuf;
    rbuf.data = self->data_buf;
    rbuf.data_size = data_len;
    rbuf.recv_utime = timestamp_now();
    rbuf.lcm = self->lcm;

    if(lcm_try_enqueue_message(self->lcm, self->recv_channel_buf))
        lcm_dispatch_handlers(self->lcm, &rbuf, self->recv_channel_buf);
    return 0;

disconnected:
    _close_socket(self->socket);
    self->socket = -1;
    return -1;
}
Beispiel #4
0
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_store_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))) {
        lcm_frag_buf_store_remove (lcm->frag_bufs, fbuf);
        dbg(DBG_LCM, "Dropping message (missing %d fragments)\n",
            fbuf->fragments_remaining);
        fbuf = NULL;
    }

//    printf ("fragment %d/%d (offset %d/%d) seq %d packet sz: %d %p\n",
//        ntohs(hdr->fragment_no) + 1, 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);
        lcm_frag_buf_store_add (lcm->frag_bufs, 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-proj.github.io/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);
        lcm_frag_buf_store_remove (lcm->frag_bufs, fbuf);
        return 0;
    }

    // copy data
    memcpy (fbuf->data + fragment_offset, data_start, frag_size);
    fbuf->last_packet_utime = lcmb->recv_utime;

    fbuf->fragments_remaining --;

    if (0 == fbuf->fragments_remaining) {
        // complete message received.  Is there a subscriber that still
        // wants it?  (i.e., does any subscriber have space in its queue?)
        if(!lcm_try_enqueue_message(lcm->lcm, fbuf->channel)) {
            // no... sad... free the fragment buffer and return
            lcm_frag_buf_store_remove (lcm->frag_bufs, fbuf);
            return 0;
        }

        // yes, transfer the message into the lcm_buf_t

        // deallocate the ringbuffer-allocated buffer
        g_static_rec_mutex_lock (&lcm->mutex);
        lcm_buf_free_data(lcmb, lcm->ringbuf);
        g_static_rec_mutex_unlock (&lcm->mutex);

        // transfer ownership of the message's payload buffer
        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->last_packet_utime;

        // don't need the fragment buffer anymore
        lcm_frag_buf_store_remove (lcm->frag_bufs, fbuf);

        return 1;
    }

    return 0;
}
Beispiel #5
0
static int
lcm_logprov_handle (lcm_logprov_t * lr)
{
    lcm_recv_buf_t rbuf;

    if (!lr->event)
        return -1;

    char ch;
    int status = lcm_internal_pipe_read(lr->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;
    }

    int64_t now = timestamp_now ();
    /* Initialize the wall clock if this is the first time through */
    if (lr->next_clock_time < 0)
        lr->next_clock_time = now;

//    rbuf.channel = lr->event->channel,
    rbuf.data = (uint8_t*) lr->event->data;
    rbuf.data_size = lr->event->datalen;
    rbuf.recv_utime = lr->next_clock_time;
    rbuf.lcm = lr->lcm;

    if(lcm_try_enqueue_message(lr->lcm, lr->event->channel))
        lcm_dispatch_handlers (lr->lcm, &rbuf, lr->event->channel);

    int64_t prev_log_time = lr->event->timestamp;
    if (load_next_event (lr) < 0) {
        /* end-of-file reached.  This call succeeds, but next call to
         * _handle will fail */
        lr->event = NULL;
        if(lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1) < 0) {
            perror(__FILE__ " - write(notify)");
        }
        return 0;
    }

    /* Compute the wall time for the next event */
    if (lr->speed > 0)
        lr->next_clock_time +=
            (lr->event->timestamp - prev_log_time) / lr->speed;
    else
        lr->next_clock_time = now;

    if (lr->next_clock_time > now) {
        int wstatus = lcm_internal_pipe_write(lr->timer_pipe[1], &lr->next_clock_time, 8);
        if(wstatus < 0) {
            perror(__FILE__ " - write(timer_pipe)");
        }
    } else {
        int wstatus = lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1);
        if(wstatus < 0) {
            perror(__FILE__ " - write(notify_pipe)");
        }
    }

    return 0;
}