char *sys_username (char *buf, size_t length)
{
#ifdef _WIN32
char ts [UNLEN + 1];
DWORD len = sizeof (ts);
if (!GetUserNameA (ts, &len))
fatal_printf ("sys_username(): GetUserNameA() returned error!");
if (len >= length)
fatal_printf ("sys_username(): buffer too short!");
memcpy (buf, ts, len);
buf [len] = '\0';
#elif defined (NUTTX_RTOS)
ARG_NOT_USED (length)
strcpy (buf, "NuttX_username");
#else
char *cp = getlogin ();
if (strlen (cp) >= length) {
memcpy (buf, cp, length - 1);
buf [length - 1] = '\0';
warn_printf ("sys_username(): login name too long ('%s')!", cp);
}
else
strcpy (buf, cp);
#endif
return (buf);
}
char *sys_username (char *buf, size_t length)
{
#ifdef _WIN32
char ts [UNLEN + 1];
DWORD len = sizeof (ts);
if (!GetUserNameA (ts, &len))
fatal_printf ("sys_username(): GetUserNameA() returned error!");
if (len >= length)
fatal_printf ("sys_username(): buffer too short!");
memcpy (buf, ts, len);
buf [len] = '\0';
#else
const char *cp = getlogin ();
if (!cp)
cp = sys_getenv ("LOGNAME");
if (!cp)
return (NULL);
if (strlen (cp) >= length) {
memcpy (buf, cp, length - 1);
buf [length - 1] = '\0';
warn_printf ("sys_username(): login name too long ('%s')!", cp);
}
else
strcpy (buf, cp);
#endif
return (buf);
}
int sys_init (void)
{
#ifdef _WIN32
WSADATA ad;
int err;
if ((err = WSAStartup (MAKEWORD(2, 2), &ad)) != 0)
fatal_printf ("WSAStartup(v2.2) failed with error: %d\n", err);
/* Confirm that the WinSock DLL supports 2.2.*/
/* Note that if the DLL supports versions greater */
/* than 2.2 in addition to 2.2, it will still return */
/* 2.2 in wVersion since that is the version we */
/* requested. */
if (LOBYTE (ad.wVersion) != 2 || HIBYTE (ad.wVersion) != 2) {
/* Tell the user that we could not find a usable */
/* WinSock DLL. */
WSACleanup ();
fatal_printf ("Could not find a usable version of Winsock.dll (> v2.2)\n");
}
#endif
sys_getftime (&sys_startup_time);
return (0 /*DDS_RETCODE_OK*/);
}
CryptoData_t *crypto_create (const SEC_CRYPTO *plugin,
size_t data_size,
void *owner,
int endpoint)
{
CryptoData_t *dp;
Crypto_t h;
unsigned n;
void *nhandles;
h = handle_alloc (handles);
if (!h) {
n = cur_handles + min_handles;
if (n > max_handles)
n = max_handles;
n -= cur_handles;
if (!n) {
warn_printf ("Crypto: max. # of contexts reached (1)!");
return (NULL);
}
nhandles = handle_extend (handles, n);
if (!nhandles) {
warn_printf ("Crypto: max. # of contexts reached (2)!");
return (NULL);
}
handles = nhandles;
h = handle_alloc (handles);
if (!h) {
fatal_printf ("Crypto: can't create a handle!");
return (NULL);
}
crypto = xrealloc (crypto, (cur_handles + 1 + n) * sizeof (CryptoData_t *));
if (!crypto) {
fatal_printf ("Crypto: can't extend crypto table!");
return (NULL);
}
cur_handles += n;
sec_crypt_alloc += n * sizeof (CryptoData_t *);
}
dp = xmalloc (sizeof (CryptoData_t) + data_size);
if (!dp) {
warn_printf ("Crypto: Out of memory for crypto data!");
handle_free (handles, h);
return (NULL);
}
sec_crypt_alloc += sizeof (CryptoData_t) + data_size;
dp->handle = h;
dp->plugin = plugin;
if (endpoint)
dp->parent.endpoint = owner;
else
dp->parent.participant = owner;
dp->endpoint = endpoint;
dp->data = dp + 1;
(*crypto) [h] = dp;
dp->handle = h;
return (dp);
}
TopicHandle_t DDS_SP_add_topic (ParticipantHandle_t participant_handle,
DomainHandle_t domain_handle)
{
MSParticipant_t *p = NULL;
MSDomain_t *d = NULL;
MSUTopic_t *tp = NULL;
MSTopic_t *dtp = NULL;
/* log_printf (SEC_ID, 0, "MSP: Add default 'allow all' policy for topic\r\n"); */
if (participant_handle > 0) {
/*ADD this topic for participant*/
if (!(p = id_handles [participant_handle]))
return (-1);
if ( LIST_HEAD (p->topics) &&
LIST_HEAD (p->topics)->topic.index == 0)
DDS_SP_remove_topic ( participant_handle, 0, 0);
if (!(tp = calloc (1, sizeof (MSUTopic_t))))
fatal_printf ("Out of memory!\r\n");
LIST_ADD_TAIL (p->topics, *tp);
tp->id = ~0; /* all domains */
tp->topic.name = NULL; /* name == '*' */
tp->topic.mode = TA_ALL;
tp->topic.index = ++topic_counter;
log_printf ("MSP: add topic (%d, 0, %d);\r\n", participant_handle, tp->topic.index);
tp->topic.blacklist = 0;
p->ntopics++;
return (tp->topic.index);
}
else if (domain_handle > 0) {
/*ADD this topic for domain*/
if (!(d = domain_handles [domain_handle]))
return (-1);
if (LIST_HEAD (d->topics) &&
LIST_HEAD (d->topics)->index == 0)
DDS_SP_remove_topic (0, domain_handle, 0);
if (!(dtp = calloc (1, sizeof (MSTopic_t))))
fatal_printf ("Out of memory!\r\n");
LIST_ADD_TAIL (d->topics, *dtp);
dtp->name = NULL; /* name == '*' */
dtp->mode = TA_ALL;
dtp->index = ++topic_counter;
log_printf ("MSP: add topic (0, %d, %d);\r\n", participant_handle, tp->topic.index);
dtp->blacklist = 0;
d->ntopics++;
return (dtp->index);
}
return (-1);
}
int sock_fd_add_socket (SOCKET s, short events, RSDATAFCT rx_fct, void *udata, const char *name)
{
SockSocket_t *sp;
void *p;
unsigned e;
WSAEVENT ev;
lock_take (sock_lock);
if ((ev = WSACreateEvent ()) == WSA_INVALID_EVENT) {
lock_release (sock_lock);
return (DDS_RETCODE_OUT_OF_RESOURCES);
}
if (num_socks == max_socks ||
nhandles >= MAXIMUM_WAIT_OBJECTS) {
if (!max_socks ||
max_socks > MAX_SIZE ||
nhandles >= MAXIMUM_WAIT_OBJECTS) {
WSACloseEvent (ev);
lock_release (sock_lock);
return (DDS_RETCODE_OUT_OF_RESOURCES);
}
max_socks += INC_SIZE;
p = xrealloc (sockets, sizeof (SockSocket_t) * max_socks);
if (!p)
fatal_printf ("sock_fd_add_socket: can't realloc()!");
sockets = p;
}
/*printf ("socket added: fd=%d, events=%d\n", s, events);*/
sp = &(*sockets) [num_socks];
sp->is_socket = 1;
sp->index = nhandles;
sp->socket = s;
sp->events = events;
sp->name = name;
sp->handle = ev;
e = 0;
if ((events & POLLIN) != 0)
e = FD_READ;
if ((events & POLLPRI) != 0)
e |= FD_OOB;
if ((events & POLLOUT) != 0)
e |= FD_WRITE;
if ((events & POLLHUP) != 0)
e |= FD_CLOSE;
if (WSAEventSelect (s, ev, e))
fatal_printf ("sock_fd_add_socket(): WSAEventSelect() failed - error = %d", WSAGetLastError ());
sp->sfct = rx_fct;
sp->udata = udata;
num_socks++;
whandles [nhandles] = ev;
wsock [nhandles++] = (Sock_t *) sp;
lock_release (sock_lock);
return (DDS_RETCODE_OK);
}
DomainHandle_t DDS_SP_add_domain (void)
{
MSDomain_t *d;
MSTopic_t *tp;
MSPartition_t *pp;
if (!domains.head)
msp_init ();
/* insert a default, allow all policy. */
/* log_printf (SEC_ID, 0, "MSP: Creating a default 'allow none' policy for all domains\r\n"); */
d = calloc (1, sizeof (MSDomain_t));
if (!d)
fatal_printf ("out-of-memory for domain policy!\r\n");
d->domain_id = ~0; /* all other domains */
d->access = DS_SECRET;
d->exclusive = 0; /* open */
#ifdef DDS_SECURITY
d->transport = TRANS_BOTH_DTLS_UDP;
#else
d->transport = TRANS_BOTH_NONE;
#endif
d->blacklist = 0;
/* allow all topics in the domain */
LIST_INIT (d->topics);
if (!(tp = calloc (1, sizeof (MSTopic_t))))
fatal_printf ("Out of memory!\r\n");
LIST_ADD_TAIL (d->topics, *tp);
tp->name = 0; /* name == '*' */
tp->mode = TA_ALL;
tp->index = 0;
tp->blacklist = 0;
d->ntopics++;
/* allow all partitions in the domain */
LIST_INIT (d->partitions);
if (!(pp = calloc (1, sizeof (MSPartition_t))))
fatal_printf ("Out of memory!\r\n");
LIST_ADD_TAIL (d->partitions, *pp);
pp->name = NULL; /* name == '*' */
pp->mode = TA_ALL;
pp->index = 0;
pp->blacklist = 0;
d->npartitions++;
LIST_ADD_TAIL (domains, *d);
d->handle = ++num_domains;
domain_handles [d->handle] = d;
return (d->handle);
}
TCP_CON_REQ_ST *tcp_clear_pending_connect (TCP_CON_REQ_ST *p)
{
TCP_CON_LIST_ST *xp, *prev;
trc_con1 ("tcp_clear_pending_connect(cp=%p) ", (void *) p);
if (!p->next) {
for (prev = NULL, xp = tcp_cx_pending;
xp;
prev = xp, xp = xp->next)
if (xp == p->head)
break;
if (!xp)
fatal_printf ("tcp_clear_pending: head of list not found!\r\n");
if (prev)
prev->next = xp->next;
else
tcp_cx_pending = xp->next;
xfree (xp);
xfree (p);
p = NULL;
}
else {
p = p->next;
xfree (p->head->reqs);
p->head->reqs = p;
}
trc_con1 ("-> next cp=%p\r\n", (void *) p);
return (p);
}
ParticipantHandle_t DDS_SP_add_participant (void)
{
MSParticipant_t *p;
MSUTopic_t *tp = NULL;
MSUPartition_t *pp = NULL;
msp_init ();
/* log_printf (SEC_ID, 0, "MSP: Creating a default 'allow all' policy for participant\r\n"); */
/* insert a default, allow all policy */
p = calloc (1, sizeof (MSParticipant_t));
if (!p)
fatal_printf ("out-of-memory for participant (user) policy!\r\n");
memcpy (p->name, "*", strlen ("*") + 1);
p->access = DS_SECRET;
p->blacklist = 0;
LIST_INIT (p->topics);
if (!(tp = calloc (1, sizeof (MSUTopic_t))))
fatal_printf ("Out of memory!\r\n");
LIST_ADD_TAIL (p->topics, *tp);
tp->id = ~0; /* all domains */
tp->topic.name = NULL; /* name == '*' */
tp->topic.mode = TA_ALL;
tp->topic.index = 0;
tp->topic.blacklist = 0;
p->ntopics++;
LIST_INIT (p->partitions);
if (!(pp = calloc (1, sizeof (MSUPartition_t))))
fatal_printf ("Out of memory!\r\n");
LIST_ADD_TAIL (p->partitions, *pp);
pp->id = ~0; /* all domains */
pp->partition.name = NULL; /* name == '*' */
pp->partition.mode = TA_ALL;
pp->partition.index = 0;
pp->partition.blacklist = 0;
p->npartitions++;
LIST_ADD_TAIL (participants, *p);
p->handle = ++num_ids;
id_handles [p->handle] = p;
return (p->handle);
}
static void
fatal_if_unsuccessful_child(const char *child, int status)
{
if (status == 0)
return;
if (status == -1)
fatal_eprintf("invoking %s", child);
if (WIFEXITED(status))
fatal_printf("%s: unsuccessful exit %d", child, WEXITSTATUS(status));
if (WIFSIGNALED(status))
fatal_printf("%s: %s%s", child, strsignal(WTERMSIG(status)),
WCOREDUMP(status) ? " (core dumped)" : "");
fatal_printf("%s: unexpected status %04x (neither exit nor fatal signal)",
child, status);
}
void ctt_send (Domain_t *dp,
Participant_t *pp,
Endpoint_t *sep,
Endpoint_t *dep,
DDS_ParticipantVolatileSecureMessage *msg)
{
Writer_t *wp;
DDS_Time_t time;
DDS_InstanceHandleSeq handles;
DDS_InstanceHandle_t h;
int error;
CTT_ASSERT (dp);
memcpy (msg->message_identity.source_guid, dp->participant.p_guid_prefix.prefix, 12);
msg->message_identity.sequence_number = psmp_seqnr++;
if (pp)
memcpy (msg->destination_participant_key, pp->p_guid_prefix.prefix, 12);
if (dep) {
memcpy (msg->destination_endpoint_key, pp->p_guid_prefix.prefix, 12);
memcpy (msg->destination_endpoint_key + 12, dep->entity_id.id, 4);
memcpy (msg->source_endpoint_key, dp->participant.p_guid_prefix.prefix, 12);
memcpy (msg->source_endpoint_key + 12, sep->entity_id.id, 4);
}
CTT_TRACE ('T', msg);
wp = (Writer_t *) dp->participant.p_builtin_ep [EPB_PARTICIPANT_VOL_SEC_W];
/* Send participant data. */
if (pp) {
DDS_SEQ_INIT (handles);
h = pp->p_builtin_ep [EPB_PARTICIPANT_VOL_SEC_R]->entity.handle;
DDS_SEQ_LENGTH (handles) = DDS_SEQ_MAXIMUM (handles) = 1;
DDS_SEQ_DATA (handles) = &h;
sys_gettime ((Time_t *) &time);
error = DDS_DataWriter_write_w_timestamp_directed (wp, msg, 0, &time, &handles);
if (error)
fatal_printf ("ctt_send: error sending crypto tokens message!");
} else {
sys_gettime ((Time_t *) &time);
error = DDS_DataWriter_write_w_timestamp (wp, msg, 0, &time);
if (error)
fatal_printf ("ctt_send: error sending crypto tokens message!");
}
CTT_ASSERT (dp);
}
static const char *
must_getenv(const char *var)
{
const char *val = getenv(var);
if (!val || !*val)
fatal_printf("%s: required variable not set in environment", var);
return val;
}
int msg_init (void)
{
dds_participant_msg_ts = DDS_DynamicType_register (dds_participant_msg_tsm);
if (!dds_participant_msg_ts) {
fatal_printf ("Can't register ParticipantMessageData type!");
return (DDS_RETCODE_BAD_PARAMETER);
}
return (DDS_RETCODE_OK);
}
static int
mask2cidr(const char *netmask)
{
struct in_addr addr;
if (inet_pton(AF_INET, netmask, &addr) != 1)
fatal_printf("%s: not a valid dotted-quad address", netmask);
uint32_t mask = ntohl(addr.s_addr);
/* To be a *netmask*, the number we get from inet_pton must be of
the form 1...10...0, i.e. there must be a single point in the
bit representation where it changes from all-1 to all-0. */
int leading_ones = count_leading_zeros(~mask);
int trailing_zeros = count_trailing_zeros(mask);
if (leading_ones != 32 - trailing_zeros)
fatal_printf("%s (%08x: %dlo, %dtz): not a valid netmask",
netmask, mask, leading_ones, trailing_zeros);
return leading_ones;
}
static long long
xstrtonum(const char *str, long long minval, long long maxval,
const char *msgprefix)
{
if (minval > maxval)
fatal_printf("xstrtonum: misuse: minval(%lld) > maxval(%lld)",
minval, maxval);
long long rv;
char *endp;
errno = 0;
rv = strtoll(str, &endp, 10);
if (endp == str || *endp != '\0')
fatal_printf("%s: '%s': invalid number", msgprefix, str);
else if (errno)
fatal_eprintf("%s: '%s'", msgprefix, str);
else if (rv < minval)
fatal_printf("%s: '%s': too small (minimum %lld)", msgprefix, str, minval);
else if (rv > maxval)
fatal_printf("%s: '%s': too large (maximum %lld)", msgprefix, str, maxval);
return rv;
}
int ctt_start (Domain_t *dp)
{
#if defined (DDS_TRACE) && defined (CTT_TRACE_RTPS)
Writer_t *wp;
#endif
Reader_t *rp;
int error;
CTT_ASSERT (dp);
error = create_builtin_endpoint (dp, EPB_PARTICIPANT_VOL_SEC_W,
1, 1,
1, 1, 0,
NULL,
dp->participant.p_meta_ucast,
dp->participant.p_meta_mcast,
NULL);
if (error)
return (error);
error = create_builtin_endpoint (dp, EPB_PARTICIPANT_VOL_SEC_R,
0, 1,
1, 1, 0,
NULL,
dp->participant.p_meta_ucast,
dp->participant.p_meta_mcast,
NULL);
if (error)
return (error);
rp = (Reader_t *) dp->participant.p_builtin_ep [EPB_PARTICIPANT_VOL_SEC_R];
error = hc_request_notification (rp->r_cache, disc_data_available, (uintptr_t) rp);
if (error) {
fatal_printf ("SPDP: can't register Crypto Token listener!");
return (error);
}
#if defined (DDS_TRACE) && defined (CTT_TRACE_RTPS)
rtps_trace_set (&rp->r_ep, DDS_TRACE_ALL);
wp = (Writer_t *) dp->participant.p_builtin_ep [EPB_PARTICIPANT_VOL_SEC_W];
rtps_trace_set (&wp->w_ep, DDS_TRACE_ALL);
#endif
CTT_ASSERT (dp);
return (DDS_RETCODE_OK);
}
int trc_lock_required (pthread_mutex_t *l, const char *file, int line)
{
LockState_t *lp;
lock_takef (trc_lock);
lp = lock_lookup (l);
#ifdef LOG_LOCKS
if (lp)
ltrc_print1 ("{REQD(%s)}", lp->name);
else
ltrc_print1 ("{REQD(%p)}", (void *) l);
#endif
if (lp && lp->state != LS_TAKEN)
fatal_printf ("trc_lock_required (at %s:%u): lock(%s) not yet taken!", file, line, lp->name);
lock_releasef (trc_lock);
return (0);
}
int sock_fd_add_handle (HANDLE h,
short events,
RHDATAFCT rx_fct,
void *udata,
const char *name)
{
SockHandle_t *hp;
void *p;
if (!max_handles) /* Needed for Debug init ... */
sock_fd_init ();
lock_take (sock_lock);
if (num_handles == max_handles ||
nhandles >= MAXIMUM_WAIT_OBJECTS) {
if (!max_handles ||
max_handles >= MAX_SIZE ||
nhandles >= MAXIMUM_WAIT_OBJECTS) {
lock_release (sock_lock);
return (DDS_RETCODE_OUT_OF_RESOURCES);
}
max_handles += INC_SIZE;
p = xrealloc (handles, sizeof (SockHandle_t) * max_handles);
if (!p)
fatal_printf ("sock_fd_add: can't realloc()!");
handles = p;
}
/*printf ("handle added: fd=%d, events=%d\n", h, events);*/
hp = &(*handles) [num_handles];
hp->is_socket = 0;
hp->index = nhandles;
hp->events = events;
hp->name = name;
hp->handle = h;
hp->hfct = rx_fct;
hp->udata = udata;
num_handles++;
whandles [nhandles] = h;
wsock [nhandles++] = (Sock_t *) hp;
lock_release (sock_lock);
return (DDS_RETCODE_OK);
}
void handle_free (void *table, handle_t handle)
{
HandleTable_t *ht = (HandleTable_t *) table;
Index_t *p;
h_printf1 ("handle_free (t, %u)\r\n", handle);
if (!handle ||
handle > ht->nelements ||
ht->elem [handle].next ||
ht->elem [handle].prev) {
fatal_printf ("handle_free: invalid handle (%u)!", handle);
return;
}
p = &ht->elem [handle];
p->prev = ht->elem [0].prev;
ht->elem [0].prev = handle;
ht->elem [p->prev].next = handle;
p->next = 0;
}
int sock_fd_add (int fd, short events, RSDATAFCT rx_fct, void *udata, const char *name)
{
unsigned n;
if (!max_fds)
sock_fd_init ();
lock_take (sock_lock);
n = atomic_get_w (num_fds);
if (n == atomic_get_w (max_fds)) {
if (max_fds >= fd_max_size) {
lock_release (sock_lock);
return (DDS_RETCODE_OUT_OF_RESOURCES);
}
lock_take (poll_lock);
atomic_add_w (max_fds, FD_INC_SIZE);
#if defined (NUTTX_RTOS)
fds = realloc (fds, sizeof (struct pollfd) * max_fds);
fcts = realloc (fcts, sizeof (RSDATAFCT) * max_fds);
ud = realloc (ud, sizeof (void *) * max_fds);
#else
fds = xrealloc (fds, sizeof (struct pollfd) * max_fds);
fcts = xrealloc (fcts, sizeof (RSDATAFCT) * max_fds);
ud = xrealloc (ud, sizeof (void *) * max_fds);
#endif
lock_release (poll_lock);
if (!fds || !fcts || !ud)
fatal_printf ("rtps_fd_add: can't realloc()!");
}
/*printf ("socket added: fd=%d, events=%d\n", fd, events);*/
(*fds) [n].fd = fd;
(*fds) [n].events = events;
(*fds) [n].revents = 0;
(*fcts) [n] = rx_fct;
(*ud) [n] = udata;
(*names) [n] = name;
atomic_inc_w (num_fds);
lock_release (sock_lock);
return (DDS_RETCODE_OK);
}
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);
}
