static void
error_print(void)
{
#if 0
VALUE errat = Qnil; /* OK */
VALUE errinfo = GET_THREAD()->errinfo;
volatile VALUE eclass, e;
const char *einfo;
long elen;
if (NIL_P(errinfo))
return;
PUSH_TAG();
if (EXEC_TAG() == 0) {
errat = get_backtrace(errinfo);
}
else {
errat = Qnil;
}
if (EXEC_TAG())
goto error;
if (NIL_P(errat)) {
const char *file = rb_sourcefile();
int line = rb_sourceline();
if (file)
warn_printf("%s:%d", file, line);
else
warn_printf("%d", line);
}
else if (RARRAY_LEN(errat) == 0) {
error_pos();
}
else {
VALUE mesg = RARRAY_AT(errat, 0);
if (NIL_P(mesg))
error_pos();
else {
warn_print2(RSTRING_PTR(mesg), RSTRING_LEN(mesg));
}
}
eclass = CLASS_OF(errinfo);
if (EXEC_TAG() == 0) {
e = rb_funcall(errinfo, rb_intern("message"), 0, 0);
StringValue(e);
einfo = RSTRING_PTR(e);
elen = RSTRING_LEN(e);
}
else {
einfo = "";
elen = 0;
}
if (EXEC_TAG())
goto error;
if (eclass == rb_eRuntimeError && elen == 0) {
warn_print(": unhandled exception\n");
}
else {
VALUE epath;
epath = rb_class_name(eclass);
if (elen == 0) {
warn_print(": ");
warn_print2(RSTRING_PTR(epath), RSTRING_LEN(epath));
warn_print("\n");
}
else {
char *tail = 0;
long len = elen;
if (RSTRING_PTR(epath)[0] == '#')
epath = 0;
if ((tail = memchr(einfo, '\n', elen)) != 0) {
len = tail - einfo;
tail++; /* skip newline */
}
warn_print(": ");
warn_print2(einfo, len);
if (epath) {
warn_print(" (");
warn_print2(RSTRING_PTR(epath), RSTRING_LEN(epath));
warn_print(")\n");
}
if (tail) {
warn_print2(tail, elen - len - 1);
if (einfo[elen-1] != '\n') warn_print2("\n", 1);
}
}
}
if (!NIL_P(errat)) {
long i;
long len = RARRAY_LEN(errat);
int skip = eclass == rb_eSysStackError;
#define TRACE_MAX (TRACE_HEAD+TRACE_TAIL+5)
#define TRACE_HEAD 8
#define TRACE_TAIL 5
for (i = 1; i < len; i++) {
VALUE v = RARRAY_AT(errat, i);
if (TYPE(v) == T_STRING) {
warn_printf("\tfrom %s\n", RSTRING_PTR(v));
}
if (skip && i == TRACE_HEAD && len > TRACE_MAX) {
warn_printf("\t ... %ld levels...\n",
len - TRACE_HEAD - TRACE_TAIL);
i = len - TRACE_TAIL;
}
}
}
error:
POP_TAG();
#endif
}
int msg_start (Domain_t *dp)
{
Reader_t *rp;
TopicType_t *tp;
int error;
error = DDS_DomainParticipant_register_type ((DDS_DomainParticipant) dp,
dds_participant_msg_ts,
"ParticipantMessageData");
if (error) {
warn_printf ("disc_start: can't register ParticipantMessageData type!");
return (error);
}
if (lock_take (dp->lock)) {
warn_printf ("disc_start: domain lock error (2)");
return (DDS_RETCODE_ERROR);
}
tp = type_lookup (dp, "ParticipantMessageData");
if (tp)
tp->flags |= EF_BUILTIN;
lock_release (dp->lock);
/* Create builtin Participant Message Reader. */
error = create_builtin_endpoint (dp, EPB_PARTICIPANT_MSG_R,
0, 1,
1, 0, 1,
NULL,
dp->participant.p_meta_ucast,
dp->participant.p_meta_mcast,
NULL);
if (error)
return (error);
/* Attach to builtin Participant Message Reader. */
rp = (Reader_t *) dp->participant.p_builtin_ep [EPB_PARTICIPANT_MSG_R];
error = hc_request_notification (rp->r_cache, disc_data_available, (uintptr_t) rp);
if (error) {
fatal_printf ("msg_start: can't register Message Reader!");
return (error);
}
/* Create builtin Participant Message Writer. */
error = create_builtin_endpoint (dp, EPB_PARTICIPANT_MSG_W,
1, 1,
1, 0, 1,
NULL,
dp->participant.p_meta_ucast,
dp->participant.p_meta_mcast,
NULL);
if (error)
return (error);
#if defined (DDS_SECURITY) && defined (DDS_NATIVE_SECURITY)
if (NATIVE_SECURITY (dp)) {
/* Create builtin Participant Secure Message Reader. */
error = create_builtin_endpoint (dp, EPB_PARTICIPANT_MSG_SEC_R,
0, 1,
1, 0, 1,
NULL,
dp->participant.p_meta_ucast,
dp->participant.p_meta_mcast,
NULL);
if (error)
return (error);
/* Attach to builtin Participant Secure Message Reader. */
rp = (Reader_t *) dp->participant.p_builtin_ep [EPB_PARTICIPANT_MSG_SEC_R];
error = hc_request_notification (rp->r_cache, disc_data_available, (uintptr_t) rp);
if (error) {
fatal_printf ("msg_start: can't register secure Message Reader!");
return (error);
}
/* Create builtin Participant Secure Message Writer. */
error = create_builtin_endpoint (dp, EPB_PARTICIPANT_MSG_SEC_W,
1, 1,
1, 0, 1,
NULL,
dp->participant.p_meta_ucast,
dp->participant.p_meta_mcast,
NULL);
if (error)
return (error);
}
#endif
return (DDS_RETCODE_OK);
}
static void tcp_server_accept (SOCKET fd, short revents, void *arg)
{
IP_CX *scxp = (IP_CX *) arg;
TCP_FD *pp;
struct sockaddr_in peer_addr;
#ifdef DDS_IPV6
struct sockaddr_in6 peer_addr_v6;
#endif
struct sockaddr *caddr;
socklen_t size;
uint32_t a4;
int r;
trace_poll_events (fd, revents, arg);
memset (&scxp->dst_addr, 0, sizeof (scxp->dst_addr));
if (scxp->locator->locator.kind == LOCATOR_KIND_TCPv4) {
peer_addr.sin_family = AF_INET;
caddr = (struct sockaddr *) &peer_addr;
size = sizeof (struct sockaddr_in);
}
#ifdef DDS_IPV6
else if (scxp->locator->locator.kind == LOCATOR_KIND_TCPv6) {
peer_addr_v6.sin6_family = AF_INET6;
caddr = (struct sockaddr *) &peer_addr_v6;
size = sizeof (struct sockaddr_in6);
}
#endif
else {
warn_printf ("TCP(S): unknown address family!");
return;
}
#ifdef TCP_ACCEPT_DELAY_MS
usleep (TCP_ACCEPT_DELAY_MS * 1000);
#endif
r = accept (fd, caddr, &size);
ctrc_begind (TCPS_ID, TCPS_SERVER_EV, &fd, sizeof (fd));
ctrc_contd (&revents, sizeof (revents));
ctrc_contd (caddr, size);
ctrc_contd (&r, sizeof (r));
ctrc_endd ();
trace_server ("accept", r, fd);
if (r < 0) {
perror ("tcp_server_accept: accept()");
log_printf (RTPS_ID, 0, "tcp_server_accept: accept() failed - errno = %d.\r\n", ERRNO);
return;
}
#ifdef DDS_TCP_NODELAY
sock_set_tcp_nodelay (r);
#endif
/* Create a new pending TCP connection. */
pp = xmalloc (sizeof (TCP_FD));
if (!pp) {
ctrc_printd (TCPS_ID, TCPS_FD_NOMEM, &r, sizeof (r));
close (r); /* bad reuse of variable in error case */
trace_server ("close", 0, r);
log_printf (RTPS_ID, 0, "TCP(S): allocation failure!\r\n");
return;
}
memset (pp, 0, sizeof (TCP_FD));
pp->fd = r;
if (scxp->locator->locator.kind == LOCATOR_KIND_TCPv4) {
a4 = ntohl (peer_addr.sin_addr.s_addr);
memset (pp->dst_addr, 0, 12);
pp->dst_addr [12] = a4 >> 24;
pp->dst_addr [13] = (a4 >> 16) & 0xff;
pp->dst_addr [14] = (a4 >> 8) & 0xff;
pp->dst_addr [15] = a4 & 0xff;
pp->dst_port = ntohs (peer_addr.sin_port);
}
static int
error_handle(int ex)
{
int status = EXIT_FAILURE;
rb_thread_t *th = GET_THREAD();
if (rb_thread_set_raised(th))
return EXIT_FAILURE;
switch (ex & TAG_MASK) {
case 0:
status = EXIT_SUCCESS;
break;
case TAG_RETURN:
error_pos();
warn_print(": unexpected return\n");
break;
case TAG_NEXT:
error_pos();
warn_print(": unexpected next\n");
break;
case TAG_BREAK:
error_pos();
warn_print(": unexpected break\n");
break;
case TAG_REDO:
error_pos();
warn_print(": unexpected redo\n");
break;
case TAG_RETRY:
error_pos();
warn_print(": retry outside of rescue clause\n");
break;
case TAG_THROW:
/* TODO: fix me */
error_pos();
warn_printf(": unexpected throw\n");
break;
case TAG_RAISE: {
VALUE errinfo = GET_THREAD()->errinfo;
if (rb_obj_is_kind_of(errinfo, rb_eSystemExit)) {
status = sysexit_status(errinfo);
}
else if (rb_obj_is_instance_of(errinfo, rb_eSignal)) {
/* no message when exiting by signal */
}
else {
error_print();
}
break;
}
case TAG_FATAL:
error_print();
break;
default:
rb_bug("Unknown longjmp status %d", ex);
break;
}
rb_thread_reset_raised(th);
return status;
}
/* Write/send (the next part of) a msg (fragment). */
static void tcp_write_message_fragment (IP_CX *cxp)
{
ssize_t n, max;
unsigned char *sp;
TCP_DATA *dp;
ctrc_printd (TCPS_ID, TCPS_TX_FRAG, &cxp->fd, sizeof (cxp->fd));
if ((dp = cxp->sproto) == NULL) {
warn_printf ("tcp_write_message_fragment: no TCP context!");
return;
}
if (!dp->send_msg) {
warn_printf ("tcp_write_message_fragment: no send_msg context!");
#ifdef TRACE_POLL_EVENTS
log_printf (RTPS_ID, 0, "TLS: POLLOUT = 0 [%d]\r\n", cxp->fd);
#endif
sock_fd_event_socket (cxp->fd, POLLOUT, 0);
return;
}
sp = dp->send_msg->buffer + dp->send_msg->used;
while (dp->send_msg->used < dp->send_msg->size) {
max = dp->send_msg->size - dp->send_msg->used;
#ifdef __APPLE__
/* Apple does not support MSG_NOSIGNAL, therefore we set the equivalent Apple-specific
socket option SO_NOSIGPIPE. */
n = send (cxp->fd, sp, max, 0);
#else
/* We're using send() here iso write() so that we can indicate to the kernel *not* to send
a SIGPIPE in case the other already closed this connection. A return code of -1 and ERRNO
to EPIPE is given instead */
n = send (cxp->fd, sp, max, MSG_NOSIGNAL);
#endif
ctrc_begind (TCPS_ID, TCPS_TX_CHUNK, &cxp->fd, sizeof (cxp->fd));
ctrc_contd (&max, sizeof (max));
ctrc_contd (&n, sizeof (n));
ctrc_endd ();
trace_write (n, cxp->fd, max);
if (n < 0) {
if (ERRNO == EINTR)
continue; /* immediately try again */
if ((ERRNO == EAGAIN) || (ERRNO == EWOULDBLOCK))
return; /* Wait for poll() to indicate that we can write another chunk. */
/*perror ("tcp_write_message_fragment");*/
log_printf (RTPS_ID, 0, "%s: error sending data on [%d] (%s).\r\n",
__FUNCTION__, cxp->fd, strerror (ERRNO));
xfree (dp->send_msg->buffer);
xfree (dp->send_msg);
dp->send_msg = NULL;
tcp_cleanup_ctx (cxp);
return;
}
dp->send_msg->used += n;
sp += n;
}
xfree (dp->send_msg->buffer);
xfree (dp->send_msg);
dp->send_msg = NULL;
#ifdef TRACE_POLL_EVENTS
log_printf (RTPS_ID, 0, "TLS: POLLOUT = 0 [%d]\r\n", cxp->fd);
#endif
sock_fd_event_socket (cxp->fd, POLLOUT, 0);
ctrc_printd (TCPS_ID, TCPS_TX_COMPL, &cxp->fd, sizeof (cxp->fd));
if (cxp->stream_cb->on_write_completed)
cxp->stream_cb->on_write_completed (cxp);
else if (cxp->paired &&
cxp->paired->fd == cxp->fd &&
cxp->paired->stream_cb->on_write_completed)
cxp->paired->stream_cb->on_write_completed (cxp);
}
static int tcp_do_connect (TCP_CON_REQ_ST *p)
{
TCP_CON_LIST_ST *hp;
TCP_DATA *dp;
struct sockaddr_in sa_v4;
#ifdef DDS_IPV6
struct sockaddr_in6 sa_v6;
#endif
struct sockaddr *sa;
socklen_t len;
unsigned family;
short events;
int fd, r, err;
#ifdef __APPLE__
int yes = 1;
#endif
trc_con1 ("tcp_do_connect(cp=%p);\r\n", (void *) p);
do {
/* No connect() in progress currently! */
hp = p->head;
if ((hp->locator.kind & LOCATOR_KINDS_IPv4) != 0) {
sa_v4.sin_family = family = AF_INET;
sa_v4.sin_port = htons (hp->locator.port);
sa = (struct sockaddr *) &sa_v4;
len = sizeof (sa_v4);
sa_v4.sin_addr.s_addr = htonl ((hp->locator.address [12] << 24) |
(hp->locator.address [13] << 16) |
(hp->locator.address [14] << 8) |
hp->locator.address [15]);
ctrc_printd (TCPS_ID, TCPS_DO_CON, hp->locator.address + 12, 4);
}
#ifdef DDS_IPV6
else if ((hp->locator.kind & LOCATOR_KINDS_IPv6) != 0) {
sa_v6.sin6_family = family = AF_INET6;
memcpy (sa_v6.sin6_addr.s6_addr, hp->locator.address, 16);
sa_v6.sin6_port = htons (hp->locator.port);
sa = (struct sockaddr *) &sa_v6;
len = sizeof (sa_v6);
ctrc_printd (TCPS_ID, TCPS_DO_CON, hp->locator.address, 16);
}
#endif
else {
log_printf (RTPS_ID, 0, "tcp_do_connect: invalid locator kind!\r\n");
return (-2);
}
dp = xmalloc (sizeof (TCP_DATA));
if (!dp) {
warn_printf ("tcp_connect: out of memory for TCP context!");
return (-2);
}
dp->send_msg = dp->recv_msg = NULL;
fd = socket (family, SOCK_STREAM, IPPROTO_TCP);
trace_client ("socket", fd, -1);
if (fd < 0) {
xfree (dp);
/*perror ("tcp_do_connect: socket()");*/
log_printf (RTPS_ID, 0, "tcp_do_connect: socket() failed - errno = %d.\r\n", ERRNO);
return (-2);
}
p->cxp->fd = fd;
p->cxp->fd_owner = 1;
p->cxp->sproto = dp;
#ifdef __APPLE__
/* MSG_NOSIGNAL does not exist for Apple OS, but a equivalent socket option is available */
if (setsockopt (fd, SOL_SOCKET, SO_NOSIGPIPE, &yes, sizeof (yes)) < 0)
perror ("tcp_do_connect: setsockopt()");
#endif
#ifdef DDS_TCP_NODELAY
sock_set_tcp_nodelay (fd);
#endif
sock_set_socket_nonblocking (fd);
events = POLLIN | POLLPRI | POLLHUP | POLLNVAL;
for (;;) {
r = connect (fd, sa, len);
ctrc_begind (TCPS_ID, TCPS_SYS_CON, &fd, sizeof (fd));
ctrc_contd (&r, sizeof (r));
ctrc_endd ();
trace_client ("connect", r, fd);
if (r == -1) {
err = ERRNO;
if (err == EINTR)
continue;
if (err != EINPROGRESS) {
ctrc_printd (TCPS_ID, TCPS_CON_ERR, &fd, sizeof (fd));
/*perror ("tcp_do_connect: connect()");*/
log_printf (RTPS_ID, 0, "tcp_do_connect: connect() failed - errno = %d.\r\n", err);
close (fd);
trace_server ("close", r, fd);
p->cxp->cx_state = CXS_WRETRY;
p->cxp->fd = 0;
p->cxp->fd_owner = 0;
}
else {
log_printf (RTPS_ID, 0, "TCP: connecting to server [%d] ...\r\n", fd);
p->cxp->cx_state = CXS_CONNECT;
sock_fd_add_socket (fd, events | POLLOUT, tcp_wait_connect_complete, p, "DDS.TCP-C");
ctrc_printd (TCPS_ID, TCPS_CON_WAIT, &fd, sizeof (fd));
return (-1);
}
}
else {
log_printf (RTPS_ID, 0, "TCP: connected to server [%d]\r\n", fd);
p->cxp->cx_state = CXS_OPEN;
ctrc_printd (TCPS_ID, TCPS_CON_OK, &fd, sizeof (fd));
sock_fd_add_socket (fd, events, tcp_socket_activity, p->cxp, "DDS.TCP-C");
}
break;
}
p = tcp_clear_pending_connect (p);
}
while (p);
return (r);
}
static void
error_print(void)
{
volatile VALUE errat = Qundef;
rb_thread_t *th = GET_THREAD();
VALUE errinfo = th->errinfo;
int raised_flag = th->raised_flag;
volatile VALUE eclass = Qundef, e = Qundef;
const char *volatile einfo;
volatile long elen;
if (NIL_P(errinfo))
return;
rb_thread_raised_clear(th);
TH_PUSH_TAG(th);
if (TH_EXEC_TAG() == 0) {
errat = get_backtrace(errinfo);
}
else if (errat == Qundef) {
errat = Qnil;
}
else if (eclass == Qundef || e != Qundef) {
goto error;
}
else {
goto no_message;
}
if (NIL_P(errat)) {
const char *file = rb_sourcefile();
int line = rb_sourceline();
if (!file)
warn_printf("%d", line);
else if (!line)
warn_printf("%s", file);
else
warn_printf("%s:%d", file, line);
}
else if (RARRAY_LEN(errat) == 0) {
error_pos();
}
else {
VALUE mesg = RARRAY_AREF(errat, 0);
if (NIL_P(mesg))
error_pos();
else {
warn_print_str(mesg);
}
}
eclass = CLASS_OF(errinfo);
if (eclass != Qundef &&
(e = rb_check_funcall(errinfo, rb_intern("message"), 0, 0)) != Qundef &&
(RB_TYPE_P(e, T_STRING) || !NIL_P(e = rb_check_string_type(e)))) {
einfo = RSTRING_PTR(e);
elen = RSTRING_LEN(e);
}
else {
no_message:
einfo = "";
elen = 0;
}
if (eclass == rb_eRuntimeError && elen == 0) {
warn_print(": unhandled exception\n");
}
else {
VALUE epath;
epath = rb_class_name(eclass);
if (elen == 0) {
warn_print(": ");
warn_print_str(epath);
warn_print("\n");
}
else {
char *tail = 0;
long len = elen;
if (RSTRING_PTR(epath)[0] == '#')
epath = 0;
if ((tail = memchr(einfo, '\n', elen)) != 0) {
len = tail - einfo;
tail++; /* skip newline */
}
warn_print(": ");
warn_print_str(tail ? rb_str_subseq(e, 0, len) : e);
if (epath) {
warn_print(" (");
warn_print_str(epath);
warn_print(")\n");
}
if (tail) {
warn_print_str(rb_str_subseq(e, tail - einfo, elen - len - 1));
}
if (tail ? einfo[elen-1] != '\n' : !epath) warn_print2("\n", 1);
}
}
if (!NIL_P(errat)) {
long i;
long len = RARRAY_LEN(errat);
int skip = eclass == rb_eSysStackError;
#define TRACE_MAX (TRACE_HEAD+TRACE_TAIL+5)
#define TRACE_HEAD 8
#define TRACE_TAIL 5
for (i = 1; i < len; i++) {
VALUE line = RARRAY_AREF(errat, i);
if (RB_TYPE_P(line, T_STRING)) {
warn_printf("\tfrom %"PRIsVALUE"\n", line);
}
if (skip && i == TRACE_HEAD && len > TRACE_MAX) {
warn_printf("\t ... %ld levels...\n",
len - TRACE_HEAD - TRACE_TAIL);
i = len - TRACE_TAIL;
}
}
}
error:
TH_POP_TAG();
th->errinfo = errinfo;
rb_thread_raised_set(th, raised_flag);
}
static void sfr_rel_data (RemWriter_t *rwp,
Change_t *cp,
SequenceNumber_t *cpsnr,
const KeyHash_t *hp,
const unsigned char *key,
size_t keylen,
#ifdef RTPS_FRAGMENTS
DataFragSMsg *fragp,
FragInfo_t **finfo,
#endif
int ignore)
{
ENDPOINT *ep = &rwp->rw_reader->endpoint;
CCREF *rp, *gap_rp;
HCI hci;
Change_t *ncp;
InstanceHandle h;
SequenceNumber_t gap_first, gap_last, seqnr_first, seqnr_last;
RejectCause_t cause;
/*unsigned max;*/
int error, ooo;
ctrc_printd (RTPS_ID, RTPS_SFR_REL_DATA, &rwp, sizeof (rwp));
prof_start (rtps_rr_data);
#if defined (RTPS_FRAGMENTS) && defined (EXTRA_STATS)
if (fragp)
STATS_INC (rwp->rw_ndatafrags);
else
#endif
STATS_INC (rwp->rw_ndata);
RW_SIGNAL (rwp, "REL-Data");
#ifdef RTPS_MARKERS
if (rwp->rw_reader->endpoint.mark_data)
rtps_marker_notify (rwp->rw_reader->endpoint.endpoint, EM_DATA, "sfr_rel_data");
#endif
#ifdef RTPS_INIT_ACKNACK
/* If first Data: accept it and become alive. */
if (!rwp->rw_peer_alive)
sfr_rel_alive (rwp);
#endif
rwp->rw_hb_no_data = 0;
/* If seqnr already in cache: ignore sample. */
if (SEQNR_LT (*cpsnr, rwp->rw_seqnr_next)) {
RW_SIGNAL (rwp, "REL-Data: ignore (SNR < hcache.SNR)");
return;
}
ooo = !SEQNR_EQ (rwp->rw_seqnr_next, *cpsnr);
/* Create a new instance already, if possible. */
if (ignore
#ifdef RTPS_FRAGMENTS
|| fragp
#endif
)
hci = NULL;
else if (ep->multi_inst) {
hci = hc_lookup_hash (ep->endpoint->cache, hp, key, keylen,
&h, 1, ooo, &cause);
if (!hci) {
/* Don't see this as rejected -- since there is no ack
given, sample will be retransmitted!
dcps_sample_rejected ((Reader_t *) ep->endpoint,
(DDS_SampleRejectedStatusKind) cause,
h);*/
if (!ooo)
rwp->rw_blocked = 1;
return;
}
}
else {
if (!hc_accepts (ep->endpoint->cache, ooo)) {
/* Don't see this as rejected -- since there is no ack
given, sample will be retransmitted!
dcps_sample_rejected ((Reader_t *) ep->endpoint,
DDS_REJECTED_BY_SAMPLES_LIMIT, 0);*/
if (!ooo)
rwp->rw_blocked = 1;
return;
}
h = 0;
hci = NULL;
}
/* Add to changes list of proxy writer context. */
if (LIST_NONEMPTY (rwp->rw_changes)) { /* Already some samples queued. */
RW_SIGNAL (rwp, "REL-Data: changes-queued");
if (hci)
hc_inst_inform (ep->endpoint->cache, hci);
if (rwp->rw_changes.head->relevant)
seqnr_first = rwp->rw_changes.head->u.c.change->c_seqnr;
else
seqnr_first = rwp->rw_changes.head->u.range.first;
if (rwp->rw_changes.tail->relevant)
seqnr_last = rwp->rw_changes.tail->u.c.change->c_seqnr;
else
seqnr_last = rwp->rw_changes.tail->u.range.last;
if (SEQNR_GT (*cpsnr, seqnr_last)) { /* Seqnr > last in list! */
/* Add new data sample node after tail of list. */
RW_SIGNAL (rwp, "REL-Data: add-missing>last");
gap_first = seqnr_last;
SEQNR_INC (gap_first);
if (!SEQNR_EQ (gap_first, *cpsnr)) {
/* Gap between tail and data: add MISSING sample
node between current tail of list and to be
added data sample node. */
gap_last = *cpsnr;
SEQNR_DEC (gap_last);
if (!rwp->rw_changes.tail->relevant &&
rwp->rw_changes.tail->state == CS_MISSING) {
/* Extend previous gap node. */
rwp->rw_changes.tail->u.range.last = gap_last;
RW_SIGNAL (rwp, "REL-Data: extend-gap");
}
else { /* Add new gap node. */
gap_rp = ccref_add_gap (&rwp->rw_changes,
&gap_first,
&gap_last,
1,
CS_MISSING);
if (!gap_rp)
return;
RW_SIGNAL (rwp, "REL-Data: add-gap");
}
}
/* Append data sample node. */
if (ignore)
rp = ccref_add_gap (&rwp->rw_changes, cpsnr,
cpsnr, 1, CS_RECEIVED);
else
rp = ccref_add_received (&rwp->rw_changes, cp,
cpsnr, hci, h, 1);
if (!rp)
return;
rwp->rw_reader->data_queued++;
#ifdef RTPS_FRAGMENTS
if (fragp) {
sfr_fragment (rwp, rp, fragp, finfo, hp, cp, ooo, ignore);
return;
}
#endif
RANGE_CHECK (&rwp->rw_changes, "REL-Data: >last");
}
else if (SEQNR_LT (*cpsnr, seqnr_first)) { /* Seqnr < first! */
/* Add new data sample node before head of list. */
RW_SIGNAL (rwp, "REL-Data: add-missing<first");
gap_last = seqnr_first;
SEQNR_DEC (gap_last);
if (!SEQNR_EQ (gap_last, *cpsnr)) {
/* Gap between data and head: add MISSING sample
node between to be added data sample and
current head of list. */
gap_first = *cpsnr;
SEQNR_INC (gap_first);
if (!rwp->rw_changes.head->relevant &&
rwp->rw_changes.tail->state == CS_MISSING) {
/* Extended following gap node. */
rwp->rw_changes.head->u.range.first = gap_first;
RW_SIGNAL (rwp, "REL-Data: extend-gap");
}
else { /* Add new gap node. */
gap_rp = ccref_add_gap (&rwp->rw_changes,
&gap_first,
&gap_last,
0,
CS_MISSING);
if (!gap_rp)
return;
RW_SIGNAL (rwp, "REL-Data: add-gap");
}
}
/* Prepend data sample node. */
if (ignore)
rp = ccref_add_gap (&rwp->rw_changes, cpsnr,
cpsnr, 0, CS_RECEIVED);
else
rp = ccref_add_received (&rwp->rw_changes, cp,
cpsnr, hci, h, 0);
if (!rp)
return;
rwp->rw_reader->data_queued++;
#ifdef RTPS_FRAGMENTS
if (fragp) {
sfr_fragment (rwp, rp, fragp, finfo, hp, cp, ooo, ignore);
return;
}
#endif
RANGE_CHECK (&rwp->rw_changes, "REL-Data: <first");
}
else { /* Seqnr somewhere in list - lets find it. */
LIST_FOREACH (rwp->rw_changes, rp)
if (rp->relevant) {
if (SEQNR_EQ (rp->u.c.change->c_seqnr, *cpsnr))
break;
}
else if (!SEQNR_LT (*cpsnr, rp->u.range.first) &&
!SEQNR_GT (*cpsnr, rp->u.range.last))
break;
if (LIST_END (rwp->rw_changes, rp) ||
(rp->state != CS_MISSING
#ifdef RTPS_FRAGMENTS
&& !rp->fragments
#endif
))
return;
#ifdef RTPS_FRAGMENTS
if (rp->fragments) {
sfr_fragment (rwp, rp, fragp, finfo, hp, cp, ooo, ignore);
return;
}
#endif
RW_SIGNAL (rwp, "REL-Data: in-range");
RANGE_CHECK (&rwp->rw_changes, "REL-Data: in-range");
if (SEQNR_GT (*cpsnr, rp->u.range.first)) {
/* Prepend gap node: range.first .. *cpsnr -1 */
gap_first = rp->u.range.first;
gap_last = *cpsnr;
SEQNR_DEC (gap_last);
gap_rp = ccref_insert_gap (&rwp->rw_changes,
rp->prev,
&gap_first,
&gap_last,
CS_MISSING);
if (!gap_rp)
return;
RW_SIGNAL (rwp, "REL-Data: prepend-gap");
}
if (SEQNR_LT (*cpsnr, rp->u.range.last)) {
/* Append gap node: *cpsnr + 1 .. range.last */
gap_first = *cpsnr;
SEQNR_INC (gap_first);
gap_last = rp->u.range.last;
gap_rp = ccref_insert_gap (&rwp->rw_changes,
rp,
&gap_first,
&gap_last,
CS_MISSING);
if (!gap_rp)
return;
RW_SIGNAL (rwp, "REL-Data: append-gap");
}
/* Reuse gap node for data. */
#ifdef RTPS_FRAGMENTS
rp->fragments = NULL;
#endif
if (ignore) {
rp->relevant = 0;
rp->u.range.first = *cpsnr;
rp->u.range.last = *cpsnr;
}
else {
if (cp->c_nrefs > 1) {
ncp = hc_change_clone (cp);
if (!ncp) {
warn_printf ("sfr_rel_data (): out of memory for change clone!\r\n");
return;
}
}
else {
rcl_access (cp);
cp->c_nrefs++;
rcl_done (cp);
ncp = cp;
}
ncp->c_handle = h;
ncp->c_seqnr = *cpsnr;
rp->relevant = 1;
rp->u.c.hci = hci;
rp->u.c.change = ncp;
rwp->rw_reader->data_queued++;
}
rp->state = CS_RECEIVED;
#ifdef RTPS_FRAGMENTS
if (fragp) {
sfr_fragment (rwp, rp, fragp, finfo, hp, cp, ooo, ignore);
return;
}
#endif
RW_SIGNAL (rwp, "REL-Data: missing::received");
RANGE_CHECK (&rwp->rw_changes, "REL-Data: missing::received");
}
/* If the received data sample caused some samples to be valid,
add these samples to the history cache and remove them from
the changes list. */
rp = LIST_HEAD (rwp->rw_changes);
if (rp &&
(rp->state == CS_RECEIVED || rp->state == CS_LOST) &&
#ifdef RTPS_FRAGMENTS
!rp->fragments &&
#endif
rwp->rw_heartbeats) {
RW_SIGNAL (rwp, "REL-Data: process-samples");
RANGE_CHECK (&rwp->rw_changes, "REL-Data: process-samples");
sfr_process_samples (rwp);
}
}
LocatorNode_t *locator_list_add (LocatorList_t *list,
LocatorKind_t kind,
const unsigned char *addr,
uint32_t port,
uint32_t scope_id,
Scope_t scope,
unsigned flags,
unsigned sproto)
{
LocSearchData data;
LocatorNode_t **npp, *np;
LocatorRef_t *rp, *p;
int is_new;
data.kind = kind;
data.addr = addr;
data.port = port;
lock_take (loc_lock);
npp = sl_insert (&loc_list, &data, &is_new, loc_cmp);
if (!npp) {
warn_printf ("locator_list_add: not enough memory for list node!");
lock_release (loc_lock);
return (NULL);
}
if (is_new) {
np = mds_pool_alloc (&mem_blocks [MB_LOCATOR]);
if (!np) {
warn_printf ("locator_list_add: not enough memory for locator node!");
#ifdef LIST_DELETE_NODE
sl_delete_node (&loc_list, npp);
#else
sl_delete (&loc_list, &data, loc_cmp);
#endif
lock_release (loc_lock);
return (NULL);
}
np->users = 0;
np->locator.kind = kind;
np->locator.port = port;
memcpy (np->locator.address, addr, sizeof (np->locator.address));
np->locator.scope_id = scope_id;
np->locator.scope = scope;
np->locator.flags = flags;
np->locator.sproto = sproto;
np->locator.intf = 0;
np->locator.handle = 0;
*npp = np;
}
else {
np = *npp;
if (np->locator.scope_id != scope_id ||
(np->locator.scope && scope && np->locator.scope != scope) ||
/*(np->locator.flags && flags &&
(np->locator.flags & LOCF_MFLAGS) != (flags & LOCF_MFLAGS)) ||*/
(np->locator.sproto && sproto && np->locator.sproto != sproto))
log_printf (LOC_ID, 0, "locator_list_add: incompatible locator attributes for %s, "
"%u:%u, %u:%u, 0x%x:0x%x, %u:%u!\r\n",
locator_str (&np->locator),
np->locator.scope_id, scope_id,
np->locator.scope, scope,
np->locator.flags, flags,
np->locator.sproto, sproto);
if (!np->locator.scope_id && scope_id)
np->locator.scope_id = scope_id;
if (!np->locator.scope && scope)
np->locator.scope = scope;
if (flags && np->locator.flags != flags)
np->locator.flags |= flags;
if (!np->locator.sproto && sproto)
np->locator.sproto = sproto;
/* Check if already in list. */
for (rp = *list; rp; rp = rp->next)
if (rp->data == np) { /* Already there! */
lock_release (loc_lock);
return (0);
}
}
#ifdef LOG_LOCATORS
log_printf (LOC_ID, 0, "LOC: locator_list_add (list=%p, %s)\r\n", (void *) list, locator_str (&np->locator));
#endif
rp = mds_pool_alloc (&mem_blocks [MB_LOCREF]);
if (!rp) {
warn_printf ("locator_list_add: not enough memory for locator reference!\r\n");
if (is_new) {
mds_pool_free (&mem_blocks [MB_LOCATOR], np);
#ifdef LIST_DELETE_NODE
sl_delete_node (&loc_list, npp);
#else
sl_delete (&loc_list, &data, loc_cmp);
#endif
}
lock_release (loc_lock);
return (NULL);
}
rp->next = NULL;
rp->data = np;
np->users++;
lock_release (loc_lock);
if (*list) {
for (p = *list; p->next; p = p->next)
;
p->next = rp;
}
else
*list = rp;
return (np);
}
DDS_ReturnCode_t dcps_write (DDS_DataWriter wp,
const void *instance_data,
int dynamic,
const DDS_InstanceHandle_t handle,
const FTime_t *time,
DDS_InstanceHandleSeq *dests)
{
Change_t *cp;
unsigned char *keys;
unsigned char *dp;
DB *dbp;
HCI hci;
size_t tlen, ofs, size;
InstanceHandle h;
const TypeSupport_t *ts;
unsigned i;
DDS_ReturnCode_t ret;
unsigned char buf [16];
prof_start (dcps_write_p);
if (!writer_ptr (wp, 1, &ret))
return (ret);
if (dests) {
if (!dests->_length || !dests->_buffer) {
ret = DDS_RETCODE_BAD_PARAMETER;
goto done;
}
else if (dests->_length > MAX_DW_DESTS) {
ret = DDS_RETCODE_OUT_OF_RESOURCES;
goto done;
}
}
if (!wp->w_pm_status.current_count &&
wp->w_qos->qos.durability_kind == DDS_VOLATILE_DURABILITY_QOS &&
(wp->w_flags & EF_BUILTIN) == 0) {
ret = DDS_RETCODE_OK;
goto done;
}
h = handle;
ts = wp->w_topic->type->type_support;
if (!handle && ts->ts_keys) {
keys = dcps_key_data_get (wp->w_topic, instance_data, dynamic, ENC_DATA (&wp->w_lep), buf, &size, &ret);
if (!keys) {
warn_printf ("DDS_DataWriter_write: invalid parameters!");
goto done;
}
hci = hc_register (wp->w_cache, keys, size, time, &h);
if (!hci) {
warn_printf ("DDS_DataWriter_write: cache_register_instance() failed!");
if (size > sizeof (buf))
xfree (keys);
ret = DDS_RETCODE_OUT_OF_RESOURCES;
goto done;
}
if (size > sizeof (buf))
xfree (keys);
}
else
hci = NULL;
if (!hc_write_required (wp->w_cache)) {
ret = DDS_RETCODE_OK;
goto done;
}
/* Allocate a new change record. */
cp = hc_change_new ();
if (!cp) {
ret = DDS_RETCODE_OUT_OF_RESOURCES;
goto done;
}
#if defined (DDS_DEBUG) && defined (DUMP_DDATA)
if (dynamic) {
dbg_printf ("dcps_write:\r\n");
xd_dump (1, instance_data);
}
#endif
if (ts->ts_dynamic || ts->ts_length > 512) {
ofs = 0;
tlen = DDS_MarshalledDataSize (instance_data, dynamic, ts, &ret);
if (ret) {
log_printf (DCPS_ID, 0, "DDS_DataWriter_write({%u}): marshalled buffer size could not be determined (%d)!\r\n",
wp->w_handle, ret);
goto free_data;
}
if (tlen > dds_max_sample_size) {
log_printf (DCPS_ID, 0, "DDS_DataWriter_write({u}): marshalled buffer size exceeds system limits (%lu/%lu bytes)\r\n",
(unsigned long) tlen, (unsigned long) dds_max_sample_size);
ret = DDS_RETCODE_OUT_OF_RESOURCES;
goto free_data;
}
}
else {
if (ts->ts_length > dds_max_sample_size) {
log_printf (DCPS_ID, 0, "DDS_DataWriter_write({u}): sample size exceeds system limits (%lu/%lu bytes)\r\n",
(unsigned long) ts->ts_length, (unsigned long) dds_max_sample_size);
ret = DDS_RETCODE_OUT_OF_RESOURCES;
goto free_data;
}
ofs = 4;
tlen = ts->ts_length + 4;
if (tlen <= C_DSIZE) {
cp->c_db = NULL;
dp = cp->c_xdata;
memcpy (dp + ofs, instance_data, ts->ts_length);
dp [0] = dp [2] = dp [3] = 0;
dp [1] = (MODE_RAW << 1) | ENDIAN_CPU;
goto data_copied;
}
}
/* Allocate a container to store the data. */
if ((dbp = db_alloc_data (tlen, 1)) == NULL) {
warn_printf ("DDS_DataWriter_write({%u}): out of memory for data (%lu bytes)!\r\n",
wp->w_handle, (unsigned long) tlen);
ret = DDS_RETCODE_OUT_OF_RESOURCES;
goto free_data;
}
cp->c_db = dbp;
dp = dbp->data;
if (ts->ts_dynamic || ts->ts_length > 512) {
/* Add marshalled data to DB chain, prefixed with marshalling type. */
ret = DDS_MarshallData (dp, instance_data, dynamic, ts);
if (ret) {
log_printf (DCPS_ID, 0, "DDS_DataWriter_write({%u}): error %u marshalling data!\r\n",
wp->w_handle, ret);
goto free_data;
}
}
else {
/* Add raw data to DB chain, prefixed with RAW identifier. */
db_put_data (dbp, 4, instance_data, ts->ts_length);
dp [0] = dp [2] = dp [3] = 0;
dp [1] = (MODE_RAW << 1) | ENDIAN_CPU;
}
data_copied:
cp->c_data = dp;
/* cp->c_wack = 0; */
cp->c_kind = ALIVE;
cp->c_linear = 1;
cp->c_writer = wp->w_handle;
cp->c_time = *time;
cp->c_handle = h;
cp->c_length = tlen;
if (!dests)
cp->c_dests [0] = 0;
else {
for (i = 0; i < dests->_length; i++)
cp->c_dests [i] = dests->_buffer [i];
while (i < MAX_DW_DESTS)
cp->c_dests [i++] = 0;
}
ret = hc_add_inst (wp->w_cache, cp, hci, 0);
goto done;
free_data:
hc_change_free (cp);
done:
lock_release (wp->w_lock);
prof_stop (dcps_write_p, 1);
return (ret);
}
void sedp_publication_event (Reader_t *rp,
NotificationType_t t,
int cdd,
int secure)
{
Domain_t *dp = rp->r_subscriber->domain;
Participant_t *pp;
ChangeData_t change;
DiscoveredWriterData *info = NULL, tinfo;
Topic_t *tp;
DiscoveredWriter_t *dwp;
Reader_t *mrp;
UniQos_t qos;
InfoType_t type;
GUID_t *guidp;
int error;
if (t != NT_DATA_AVAILABLE)
return;
rp->r_status &= ~DDS_DATA_AVAILABLE_STATUS;
for (;;) {
if (info) {
pid_writer_data_cleanup (info);
xfree (info);
info = NULL;
}
/*dtrc_print0 ("SEDP-Pub: get samples ");*/
error = disc_get_data (rp, &change);
if (error) {
/*dtrc_print0 ("- none\r\n");*/
break;
}
/*dtrc_print1 ("- valid(%u)\r\n", change.kind);*/
if (change.kind != ALIVE) {
error = hc_get_key (rp->r_cache, change.h, &tinfo, 0);
if (error)
continue;
guidp = &tinfo.proxy.guid;
type = EI_DELETE;
hc_inst_free (rp->r_cache, change.h);
}
else {
info = change.data;
if (!info->topic_name || !info->type_name) {
hc_inst_free (rp->r_cache, change.h);
continue;
}
type = EI_NEW;
guidp = &info->proxy.guid;
}
pp = entity_participant (change.writer);
if (!pp || /* Not found. */
pp == &dp->participant || /* Own sent info. */
entity_ignored (pp->p_flags)) { /* Ignored. */
if (pp != &dp->participant && !cdd)
warn_printf ("sedp_publication_rx: invalid change.writer field!\r\n");
hc_inst_free (rp->r_cache, change.h);
dtrc_print0 ("SEDP-Pub: unneeded!\r\n");
continue; /* Filter out unneeded info. */
}
/* Publication from remote participant. */
if (type == EI_DELETE) {
dwp = (DiscoveredWriter_t *) endpoint_lookup (pp,
&guidp->entity_id);
if (!dwp) {
dtrc_print0 ("SEDP-Pub: DELETE && doesn't exist!\r\n");
continue; /* If doesn't exist - no action. */
}
if (!dwp->dw_topic) {
endpoint_delete (pp, &dwp->dw_ep);
continue; /* Ignored topic -- only endpoint. */
}
if (sedp_log)
log_printf (SEDP_ID, 0, "SEDP: Deleted %spublication (%s/%s) from peer!\r\n",
(secure) ? "secure " : "",
str_ptr (dwp->dw_topic->name),
str_ptr (dwp->dw_topic->type->type_name));
disc_publication_remove (pp, dwp);
hc_inst_free (rp->r_cache, change.h);
continue;
}
/* Do we know this topic? */
tp = topic_lookup (&dp->participant, str_ptr (info->topic_name));
if (tp && entity_ignored (tp->entity.flags)) {
hc_inst_free (rp->r_cache, change.h);
dtrc_print1 ("SEDP: ignored topic (%s)!\r\n", str_ptr (info->topic_name));
continue; /* Ignored topic. */
}
/* Do we know this endpoint already? */
dwp = (DiscoveredWriter_t *) endpoint_lookup (pp, &guidp->entity_id);
if (dwp) {
if (entity_ignored (dwp->dw_flags) || cdd) {
hc_inst_free (rp->r_cache, change.h);
continue; /* Ignored endpoint. */
}
dtrc_print1 ("Already exists (%s)!\r\n", str_ptr (info->topic_name));
type = EI_UPDATE;
disc_publication_update (pp, dwp, info);
if (sedp_log)
log_printf (SEDP_ID, 0, "SEDP: Updated %spublication (%s/%s) from peer!\r\n",
(secure) ? "secure " : "",
str_ptr (info->topic_name),
str_ptr (info->type_name));
hc_inst_free (rp->r_cache, change.h);
}
else {
/* Get QoS parameters. */
qos_disc_writer_set (&qos, &info->qos);
mrp = NULL;
/* Create new endpoint. */
dwp = (DiscoveredWriter_t *) endpoint_create (pp,
pp, &guidp->entity_id, NULL);
if (!dwp) {
dtrc_print1 ("SEDP: Create endpoint (%s) not possible - exit!\r\n", str_ptr (info->topic_name));
hc_inst_free (rp->r_cache, change.h);
qos_disc_writer_restore (&info->qos, &qos);
continue; /* Can't create -- just ignore. */
}
disc_publication_add (pp, dwp, &qos, tp, mrp, info);
hc_inst_free (rp->r_cache, change.h);
if (sedp_log)
log_printf (SEDP_ID, 0, "SEDP: New %spublication (%s/%s) from %s!\r\n",
(secure) ? "secure " : "",
str_ptr (info->topic_name),
str_ptr (info->type_name),
(cdd) ? "CDD" : "peer");
}
}
if (info) {
pid_writer_data_cleanup (info);
xfree (info);
}
}
