bool frudp_add_ucast_rx(const uint16_t port)
{
FREERTPS_INFO("add ucast rx port %d\n", port);
// we may have already added this when searching for our participant ID
// so, let's spin through and see if it's already there
for (int i = 0; i < g_frudp_rx_socks_used; i++)
if (g_frudp_rx_socks[i].port == port)
{
FREERTPS_INFO(" found port match in slot %d\n", i);
return true; // it's already here
}
int s = frudp_create_sock();
if (s < 0)
return false;
struct sockaddr_in rx_bind_addr;
memset(&rx_bind_addr, 0, sizeof(rx_bind_addr));
rx_bind_addr.sin_family = AF_INET;
rx_bind_addr.sin_addr.s_addr = g_frudp_tx_addr.sin_addr.s_addr;
rx_bind_addr.sin_port = htons(port);
int result = bind(s, (struct sockaddr *)&rx_bind_addr, sizeof(rx_bind_addr));
if (result < 0)
{
FREERTPS_ERROR("couldn't bind to unicast port %d\n", port);
close(s);
return false;
}
frudp_rx_sock_t *rxs = &g_frudp_rx_socks[g_frudp_rx_socks_used];
rxs->sock = s;
rxs->port = port;
rxs->addr = rx_bind_addr.sin_addr.s_addr;
//FREERTPS_INFO(" added in rx sock slot %d\n", g_frudp_rx_socks_used);
g_frudp_rx_socks_used++;
return true;
}
bool frudp_init_participant_id()
{
FREERTPS_INFO("frudp_init_participant_id()\n");
for (int pid = 0; pid < 100; pid++) // todo: hard upper bound is bad
{
// see if we can open the port; if so, let's say we have a unique PID
g_frudp_config.participant_id = pid;
const uint16_t port = frudp_ucast_builtin_port();
if (frudp_add_ucast_rx(port))
{
FREERTPS_INFO("using RTPS/DDS PID %d\n", pid);
return true;
}
}
return false; // couldn't find an available PID
}
void frudp_fini()
{
frudp_disco_fini();
FREERTPS_INFO("udp fini\n");
for (int i = 0; i < g_frudp_rx_socks_used; i++)
{
close(g_frudp_rx_socks[i].sock);
g_frudp_rx_socks[i].sock = -1;
}
}
void frudp_sedp_init()
{
FREERTPS_INFO("sedp init\r\n");
for (int i = 0; i < FRUDP_MAX_SUBS; i++)
{
frudp_submsg_data_t *d =
(frudp_submsg_data_t *)&g_sedp_sub_writer_data_buf[i * SEDP_MSG_BUF_LEN];
d->writer_sn = g_frudp_sn_unknown;
g_sedp_sub_writer_data_submsgs[i] = d;
}
for (int i = 0; i < FRUDP_MAX_PUBS; i++)
{
frudp_submsg_data_t *d =
(frudp_submsg_data_t *)&g_sedp_pub_writer_data_buf[i * SEDP_MSG_BUF_LEN];
d->writer_sn = g_frudp_sn_unknown;
g_sedp_pub_writer_data_submsgs[i] = d;
}
g_sedp_sub_pub = frudp_create_pub(
NULL, // no topic name
NULL, // no type name
g_sedp_sub_writer_id,
g_sedp_sub_writer_data_submsgs,
FRUDP_MAX_SUBS);
g_sedp_pub_pub = frudp_create_pub(
NULL, // no topic name
NULL, // no type name
g_sedp_pub_writer_id,
g_sedp_pub_writer_data_submsgs,
FRUDP_MAX_PUBS);
frudp_sub_t sedp_sub_sub;
sedp_sub_sub.topic_name = NULL;
sedp_sub_sub.type_name = NULL;
sedp_sub_sub.reader_eid = g_sedp_sub_reader_id;
sedp_sub_sub.data_cb = frudp_sedp_rx_sub_data;
sedp_sub_sub.msg_cb = NULL;
sedp_sub_sub.reliable = true;
frudp_add_sub(&sedp_sub_sub);
frudp_sub_t sedp_pub_sub; // subscribe to the publisher announcers
sedp_pub_sub.topic_name = NULL;
sedp_pub_sub.type_name = NULL;
sedp_pub_sub.reader_eid = g_sedp_pub_reader_id;
sedp_pub_sub.data_cb = frudp_sedp_rx_pub_data;
sedp_pub_sub.msg_cb = NULL;
sedp_pub_sub.reliable = true;
frudp_add_sub(&sedp_pub_sub);
}
bool frudp_add_mcast_rx(in_addr_t group, uint16_t port) //,
//const freertps_udp_rx_callback_t rx_cb)
{
FREERTPS_INFO("add mcast rx port %d\n", port);
int s = frudp_create_sock();
if (s < 0)
return false;
if (!set_sock_reuse(s))
return false;
int result;
struct sockaddr_in rx_bind_addr;
memset(&rx_bind_addr, 0, sizeof(rx_bind_addr));
rx_bind_addr.sin_family = AF_INET;
rx_bind_addr.sin_addr.s_addr = htonl(INADDR_ANY);
rx_bind_addr.sin_port = htons(port);
result = bind(s, (struct sockaddr *)&rx_bind_addr, sizeof(rx_bind_addr));
if (result < 0)
{
FREERTPS_ERROR("couldn't bind rx sock to port %d, failed with errno %d\n",
port, errno);
return false;
}
struct ip_mreq mreq;
mreq.imr_multiaddr.s_addr = group;
mreq.imr_interface.s_addr = g_frudp_tx_addr.sin_addr.s_addr;
result = setsockopt(s, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
if (result < 0)
{
FREERTPS_ERROR("couldn't add rx sock to multicast group, errno = %d\n",
errno);
return false;
}
frudp_rx_sock_t *rxs = &g_frudp_rx_socks[g_frudp_rx_socks_used];
rxs->sock = s;
rxs->port = port;
rxs->addr = g_frudp_tx_addr.sin_addr.s_addr;
//g_freertps_udp_rx_socks[g_freertps_udp_rx_socks_used].cb = rx_cb;
//FREERTPS_INFO(" added in rx sock slot %d\n", g_frudp_rx_socks_used);
g_frudp_rx_socks_used++;
return true;
}
void frudp_spdp_init()
{
FREERTPS_INFO("sdp init\r\n");
frudp_spdp_last_bcast.seconds = 0;
frudp_spdp_last_bcast.fraction = 0;
frudp_reader_t spdp_reader;
spdp_reader.writer_guid = g_frudp_guid_unknown;
spdp_reader.reader_eid = g_spdp_reader_id;
spdp_reader.max_rx_sn.low = 0;
spdp_reader.max_rx_sn.high = 0;
spdp_reader.data_cb = frudp_spdp_rx_data;
spdp_reader.msg_cb = NULL;
spdp_reader.reliable = false;
frudp_add_reader(&spdp_reader);
/*
frudp_subscribe(g_frudp_entity_id_unknown,
g_spdp_writer_id,
frudp_spdp_rx_data,
NULL);
*/
}
bool frudp_init()
{
enet_init();
FREERTPS_INFO("metal udp init()\r\n");
FREERTPS_INFO("using address %d.%d.%d.%d for unicast\r\n",
(FRUDP_IP4_ADDR >> 24) & 0xff,
(FRUDP_IP4_ADDR >> 16) & 0xff,
(FRUDP_IP4_ADDR >> 8) & 0xff,
(FRUDP_IP4_ADDR ) & 0xff);
g_frudp_config.unicast_addr = freertps_htonl(FRUDP_IP4_ADDR);
g_frudp_config.guid_prefix.prefix[0] = FREERTPS_VENDOR_ID >> 8;
g_frudp_config.guid_prefix.prefix[1] = FREERTPS_VENDOR_ID & 0xff;
memcpy(&g_frudp_config.guid_prefix.prefix[2], g_enet_mac, 6);
frudp_generic_init();
// not sure about endianness here.
// 4 bytes left. let's use the system time in microseconds since power-up
// todo: init ethernet PHY. after PHY link is up,
// store system time in the guid_prefix.
//memcpy(&g_frudp_config.guid_prefix.prefix[8], &pid, 4);
//frudp_disco_init();
return true;
}
bool frudp_part_create(void)
{
if (g_frudp_participant_init_complete)
{
printf("woah there partner. "
"freertps currently only allows one participant.\r\n");
return false;
}
FREERTPS_INFO("frudp_part_create() on domain_id %d\r\n",
(int)g_frudp_config.domain_id);
//g_frudp_config.domain_id = domain_id;
if (!frudp_init_participant_id())
{
printf("unable to initialize participant ID\r\n");
return false;
}
//frudp_generic_init();
//frudp_disco_init();
g_frudp_participant_init_complete = true;
printf("prefix: ");
frudp_print_guid_prefix(&g_frudp_config.guid_prefix);
printf("\n");
return true;
}
void frudp_disco_fini(void)
{
FREERTPS_INFO("discovery fini\r\n");
frudp_spdp_fini();
frudp_sedp_fini();
}
void frudp_disco_start(void)
{
FREERTPS_INFO("discovery start\r\n");
frudp_spdp_start();
frudp_sedp_start();
}
void frudp_disco_init(void)
{
FREERTPS_INFO("discovery init\r\n");
frudp_spdp_init();
frudp_sedp_init();
}
bool frudp_init_participant_id(void)
{
FREERTPS_INFO("frudp_init_participant_id()\r\n");
g_frudp_config.participant_id = 0;
return true;
}
void FreeRtps::freertps_start(void)
{
FREERTPS_INFO("FreeRTPS Start\r\n");
frudp_spdp_start();
frudp_sedp_start();
}
void frudp_fini()
{
frudp_disco_fini();
FREERTPS_INFO("metal udp fini\r\n");
}
static void frudp_spdp_rx_data(frudp_receiver_state_t *rcvr,
const frudp_submsg_t *submsg,
const uint16_t scheme,
const uint8_t *data)
{
#ifdef SPDP_VERBOSE
FREERTPS_INFO(" spdp_rx\n");
#endif
if (scheme != FRUDP_SCHEME_PL_CDR_LE)
{
FREERTPS_ERROR("expected spdp data to be PL_CDR_LE. bailing...\n");
return;
}
frudp_part_t *part = &g_frudp_spdp_rx_part;
// todo: spin through this param list and save it
frudp_parameter_list_item_t *item = (frudp_parameter_list_item_t *)data;
while ((uint8_t *)item < submsg->contents + submsg->header.len)
{
const frudp_parameterid_t pid = item->pid;
if (pid == FRUDP_PID_SENTINEL)
break;
const uint8_t *pval = item->value;
/*
FREERTPS_INFO(" unhandled spdp rx param 0x%x len %d\n",
(unsigned)pid, item->len);
*/
if (pid == FRUDP_PID_PROTOCOL_VERSION)
{
#ifdef SPDP_VERBOSE
FREERTPS_INFO(" spdp proto version 0x%04x\n",
*((uint16_t *)(pval)));
#endif
part->pver = *((frudp_pver_t *)(pval)); // todo: what about alignment?
}
else if (pid == FRUDP_PID_VENDOR_ID)
{
part->vid = freertps_htons(*((frudp_vid_t *)pval));
#ifdef SPDP_VERBOSE
FREERTPS_INFO(" spdp vendor_id 0x%04x = %s\n",
part->vid, frudp_vendor(part->vid));
#endif
}
else if (pid == FRUDP_PID_DEFAULT_UNICAST_LOCATOR)
{
frudp_locator_t *loc = (frudp_locator_t *)pval;
part->default_unicast_locator = *loc; // todo: worry about alignment
if (loc->kind == FRUDP_LOCATOR_KIND_UDPV4)
{
#ifdef SPDP_VERBOSE
FREERTPS_INFO(" spdp unicast locator udpv4: %s:%d\n",
frudp_ip4_ntoa(loc->addr.udp4.addr),
loc->port);
#endif
}
}
else if (pid == FRUDP_PID_DEFAULT_MULTICAST_LOCATOR)
{
frudp_locator_t *loc = (frudp_locator_t *)pval;
part->default_multicast_locator = *loc; // todo: worry about alignment
if (loc->kind == FRUDP_LOCATOR_KIND_UDPV4)
{
#ifdef SPDP_VERBOSE
FREERTPS_INFO(" spdp multicast locator udpv4: %s:%d\n",
frudp_ip4_ntoa(loc->addr.udp4.addr),
loc->port);
#endif
}
else
FREERTPS_INFO(" spdp unknown mcast locator kind: %d\n", (int)loc->kind);
}
else if (pid == FRUDP_PID_METATRAFFIC_UNICAST_LOCATOR)
{
frudp_locator_t *loc = (frudp_locator_t *)pval;
part->metatraffic_unicast_locator = *loc; // todo: worry about alignment
if (loc->kind == FRUDP_LOCATOR_KIND_UDPV4)
{
#ifdef SPDP_VERBOSE
FREERTPS_INFO(" spdp metatraffic unicast locator udpv4: %s:%d\n",
frudp_ip4_ntoa(loc->addr.udp4.addr),
loc->port);
#endif
}
else if (loc->kind == FRUDP_LOCATOR_KIND_UDPV6)
{
// ignore ip6 for now...
}
else
FREERTPS_INFO(" spdp unknown metatraffic mcast locator kind: %d\n", (int)loc->kind);
}
else if (pid == FRUDP_PID_METATRAFFIC_MULTICAST_LOCATOR)
{
frudp_locator_t *loc = (frudp_locator_t *)pval;
part->metatraffic_multicast_locator = *loc; // todo: worry about alignment
if (loc->kind == FRUDP_LOCATOR_KIND_UDPV4)
{
#ifdef SPDP_VERBOSE
FREERTPS_INFO(" spdp metatraffic multicast locator udpv4: %s:%d\n",
frudp_ip4_ntoa(loc->addr.udp4.addr),
loc->port);
#endif
}
else if (loc->kind == FRUDP_LOCATOR_KIND_UDPV6)
{
// ignore ip6 for now...
}
else
FREERTPS_INFO(" spdp unknown metatraffic mcast locator kind: %d\n", (int)loc->kind);
}
else if (pid == FRUDP_PID_PARTICIPANT_LEASE_DURATION)
{
frudp_duration_t *dur = (frudp_duration_t *)pval;
part->lease_duration = *dur;
#ifdef SPDP_VERBOSE
FREERTPS_INFO(" spdp lease duration: %d.%09d\n",
dur->sec, dur->nanosec);
#endif
}
else if (pid == FRUDP_PID_PARTICIPANT_GUID)
{
frudp_guid_t *guid = (frudp_guid_t *)pval;
memcpy(&part->guid_prefix, &guid->prefix, FRUDP_GUID_PREFIX_LEN);
#ifdef SPDP_VERBOSE
uint8_t *p = guid->prefix.prefix;
FREERTPS_INFO(" guid 0x%02x%02x%02x%02x"
"%02x%02x%02x%02x"
"%02x%02x%02x%02x\n",
p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7],
p[8], p[9], p[10], p[11]);
#endif
}
else if (pid == FRUDP_PID_BUILTIN_ENDPOINT_SET)
{
part->builtin_endpoints = *((frudp_builtin_endpoint_set_t *)pval);
#ifdef SPDP_VERBOSE
FREERTPS_INFO(" builtin endpoints: 0x%08x\n",
part->builtin_endpoints);
#endif
}
else if (pid == FRUDP_PID_PROPERTY_LIST)
{
// todo
}
else if (pid & 0x8000)
{
// ignore vendor-specific PID's
}
else
{
FREERTPS_ERROR(" unhandled spdp rx param 0x%x len %d\n",
(unsigned)pid, item->len);
}
// now, advance to next item in list...
item = (frudp_parameter_list_item_t *)(((uint8_t *)item) + 4 + item->len);
}
// now that we have stuff the "part" buffer, spin through our
// participant list and see if we already have this one
bool found = false;
for (int i = 0; !found && i < g_frudp_disco_num_parts; i++)
{
frudp_part_t *p = &g_frudp_disco_parts[i];
if (frudp_guid_prefix_identical(&p->guid_prefix,
&part->guid_prefix))
{
#ifdef SPDP_VERBOSE
printf("found match at participant slot %d\n", i);
#endif
found = true;
// TODO: see if anything has changed. update if needed
}
}
if (!found)
{
#ifdef SPDP_VERBOSE
printf("didn't have this participant already.\n");
#endif
if (g_frudp_disco_num_parts < FRUDP_DISCO_MAX_PARTS)
{
const int p_idx = g_frudp_disco_num_parts; // save typing
frudp_part_t *p = &g_frudp_disco_parts[p_idx];
*p = *part; // save everything plz
//printf(" saved new participant in slot %d\n", p_idx);
g_frudp_disco_num_parts++;
sedp_add_builtin_endpoints(p);
// push this new participant our SEDP data to speed up the process
//frudp_send_sedp_
}
else
printf("not enough room to save the new participant.\n");
}
}
void frudp_sedp_fini()
{
FREERTPS_INFO("sedp fini\r\n");
}
bool frudp_init()
{
FREERTPS_INFO("udp init()\n");
for (int i = 0; i < FRUDP_MAX_RX_SOCKS; i++)
{
g_frudp_rx_socks[i].sock = -1;
//g_freertps_udp_rx_socks[i].cb = NULL;
}
struct ifaddrs *ifaddr;
if (getifaddrs(&ifaddr) == -1)
{
FREERTPS_FATAL("couldn't call getifaddrs");
return false;
}
char *tx_addr_str = "127.0.0.1"; // use loopback if nothing else is found
for (struct ifaddrs *ifa = ifaddr; ifa; ifa = ifa->ifa_next)
{
if (!ifa->ifa_addr)
continue;
int family = ifa->ifa_addr->sa_family;
if (family != AF_INET)
continue;
char host[NI_MAXHOST];
if (getnameinfo(ifa->ifa_addr, sizeof(struct sockaddr_in),
host, NI_MAXHOST, NULL, 0, NI_NUMERICHOST))
continue;
FREERTPS_INFO("found address %s on interface %s\n", host, ifa->ifa_name);
if (0 == strcmp(host, "127.0.0.1"))
continue; // boring
tx_addr_str = host; // save this one for now
}
FREERTPS_INFO("using address %s for unicast\n", tx_addr_str);
g_frudp_tx_addr.sin_addr.s_addr = inet_addr(tx_addr_str);
g_frudp_config.unicast_addr = (uint32_t)g_frudp_tx_addr.sin_addr.s_addr;
freeifaddrs(ifaddr);
g_frudp_tx_sock = socket(AF_INET, SOCK_DGRAM, 0);
if (g_frudp_tx_sock < 0)
{
FREERTPS_FATAL("couldn't create tx sock\n");
return false;
}
int result;
result = setsockopt(g_frudp_tx_sock, IPPROTO_IP, IP_MULTICAST_IF,
(char *)&g_frudp_tx_addr.sin_addr.s_addr,
sizeof(g_frudp_tx_addr));
if (result < 0)
{
FREERTPS_FATAL("couldn't set tx sock to allow multicast\n");
return false;
}
// because we may have multiple freertps processes listening to the
// multicast traffic on this host, we need to enable multicast loopback
// todo: I assume this is enabled by default, but let's set it anyway
int loopback = 1;
result = setsockopt(g_frudp_tx_sock, IPPROTO_IP, IP_MULTICAST_LOOP,
&loopback, sizeof(loopback));
if (result < 0)
{
FREERTPS_FATAL("couldn't enable outbound tx multicast loopback\n");
return false;
}
//if (!frudp_init_participant_id())
// return false;
// some of the following stuff has been moved to frudp_part_create()
//frudp_generic_init();
// not sure about endianness here.
g_frudp_config.guid_prefix.prefix[0] = FREERTPS_VENDOR_ID >> 8;
g_frudp_config.guid_prefix.prefix[1] = FREERTPS_VENDOR_ID & 0xff;
// todo: actually get mac address
const uint8_t mac[6] = { 0x01, 0x23, 0x45, 0x67, 0x89, 0xab };
memcpy(&g_frudp_config.guid_prefix.prefix[2], mac, 6);
// 4 bytes left. let's use the POSIX process ID
uint32_t pid = (uint32_t)getpid(); // on linux, this will be 4 bytes
memcpy(&g_frudp_config.guid_prefix.prefix[8], &pid, 4);
//frudp_disco_init();
frudp_generic_init();
return true;
}
void frudp_spdp_fini()
{
FREERTPS_INFO("sdp fini\n");
}
void FreeRtps::freertps_init(void)
{
FREERTPS_INFO("FreeRTPS Init\r\n");
frudp_spdp_init();
frudp_sedp_init();
}
