int DTrack::init(dtrack_init_type* ini)
{
// init udp socket:
_udpsock = udp_init(ini->udpport);
if(_udpsock < 0){
return DTRACK_ERR_UDP;
}
_udptimeout_us = ini->udptimeout_us;
// init udp buffer:
_udpbufsize = ini->udpbufsize;
_udpbuf = (char *)malloc(_udpbufsize);
if(!_udpbuf){
udp_exit(_udpsock);
return DTRACK_ERR_MEM;
}
// DTrack remote control parameters:
_remote_ip = udp_inet_atoh(ini->remote_ip);
_remote_port = ini->remote_port;
return DTRACK_ERR_NONE;
}
bool vrpn_Tracker_DTrack::dtrack_exit(void)
{
// release buffer:
if(d_udpbuf != NULL){
free(d_udpbuf);
}
// release UDP socket:
if(d_udpsock != INVALID_SOCKET){
udp_exit(d_udpsock);
}
return true;
}
bool vrpn_Tracker_DTrack::dtrack_init(int udpport)
{
d_udpbuf = NULL;
d_lasterror = DTRACK_ERR_NONE;
// create UDP socket:
if(udpport <= 0 || udpport > 65535){
fprintf(stderr, "vrpn_Tracker_DTrack: Illegal UDP port %d.\n", udpport);
return false;
}
d_udpsock = udp_init((unsigned short )udpport);
if(d_udpsock == INVALID_SOCKET){
fprintf(stderr, "vrpn_Tracker_DTrack: Cannot Initialize UDP Socket.\n");
return false;
}
d_udptimeout_us = 0;
// create UDP buffer:
d_udpbufsize = UDPRECEIVE_BUFSIZE;
d_udpbuf = (char *)malloc(d_udpbufsize);
if(d_udpbuf == NULL){
udp_exit(d_udpsock);
d_udpsock = INVALID_SOCKET;
fprintf(stderr, "vrpn_Tracker_DTrack: Cannot Allocate Memory for UDP Buffer.\n");
return false;
}
// reset actual DTrack data:
act_framecounter = 0;
act_timestamp = -1;
act_num_marker = act_num_body = act_num_flystick = 0;
act_has_bodycal_format = false;
act_has_old_flystick_format = false;
return true;
}
int DTrack::exit(void)
{
// exit udp buffer:
if(_udpbuf){
free(_udpbuf);
}
// exit udp socket:
if(_udpsock > 0){
if(udp_exit(_udpsock)){
return DTRACK_ERR_UDP;
}
}
return DTRACK_ERR_NONE;
}
DTrack2::~DTrack2(void)
{
// release buffer:
if(d_udpbuf){
free(d_udpbuf);
}
// release sockets:
if(d_udpsock){
udp_exit(d_udpsock);
d_udpsock = NULL;
}
if(d_tcpsock){
tcp_exit(d_tcpsock);
d_tcpsock = NULL;
}
}
DTrack2::DTrack2(
const std::string& server_host, unsigned short server_port, unsigned short data_port,
int data_bufsize, int data_timeout_us, int server_timeout_us
)
{
int err;
unsigned int ip;
d_udpsock = NULL;
d_tcpsock = NULL;
d_udpbuf = NULL;
set_data_noerror();
set_server_noerror();
cmd_storelasterror(NULL);
d_udptimeout_us = data_timeout_us;
d_tcptimeout_us = server_timeout_us;
// creat UDP socket:
d_udpport = data_port;
err = udp_init(&d_udpsock, &d_udpport);
if(err){
d_udpport = 0;
return;
}
// creat UDP buffer:
d_udpbufsize = data_bufsize;
d_udpbuf = (char *)malloc(data_bufsize);
if(!d_udpbuf){
udp_exit(d_udpsock);
d_udpsock = NULL;
d_udpport = 0;
return;
}
// access DTrack2 server:
if(!server_host.empty()){
ip = ip_name2ip(server_host.c_str());
if(ip == 0){ // resolving of hostname was not possible
free(d_udpbuf);
d_udpbuf = NULL;
udp_exit(d_udpsock);
d_udpsock = NULL;
d_udpport = 0;
return;
}
err = tcp_client_init(&d_tcpsock, ip, (server_port == 0) ? DTRACK2_PROT_TCP_PORT : server_port);
if(err){ // no connection to DTrack2 server
free(d_udpbuf);
d_udpbuf = NULL;
udp_exit(d_udpsock);
d_udpsock = NULL;
d_udpport = 0;
return;
}
}
// reset actual DTrack data:
act_framecounter = 0;
act_timestamp = -1;
act_num_body = act_num_flystick = act_num_meatool = act_num_hand = 0;
act_num_marker = 0;
d_message_origin = "";
d_message_status = "";
d_message_framenr = 0;
d_message_errorid = 0;
d_message_msg = "";
}
int test_net_udp(void)
{
struct timeval timeout;
socket_udp *s1, *s2;
char buf1[BUFSIZE], buf2[BUFSIZE];
const char *hname;
int rc, i;
#ifndef WIN32
/* "BSD" bug test that appears in this function uses fork and
* exec, that are not present on WIN32. Since the original
* bug has not been seen on Win32 it's probably not worth
* converting to use CreateProcess() */
int status_parent, status_child;
#endif /* WIN32 */
srand48(time(NULL));
/**********************************************************************/
/* The first test is to loopback a packet to ourselves... */
printf
("Testing UDP/IP networking (IPv4 loopback) ................................ ");
fflush(stdout);
s1 = udp_init("127.0.0.1", 5004, 5004, 1);
if (s1 == NULL) {
printf("FAIL\n");
printf(" Cannot initialize socket\n");
return 1;
}
randomize(buf1, BUFSIZE);
randomize(buf2, BUFSIZE);
if (udp_send(s1, buf1, BUFSIZE) < 0) {
printf("FAIL\n");
perror(" Cannot send packet");
goto abort_loopback;
}
timeout.tv_sec = 1;
timeout.tv_usec = 0;
udp_fd_zero();
udp_fd_set(s1);
rc = udp_select(&timeout);
if (rc < 0) {
printf("FAIL\n");
perror(" Select failed");
goto abort_loopback;
}
if (rc == 0) {
printf("FAIL\n");
printf(" No data waiting\n");
goto abort_loopback;
}
if (!udp_fd_isset(s1)) {
printf("FAIL\n");
printf(" No data on file descriptor\n");
goto abort_loopback;
}
if (udp_recv(s1, buf2, BUFSIZE) < 0) {
printf("FAIL\n");
perror(" Receive failed");
goto abort_loopback;
}
if (memcmp(buf1, buf2, BUFSIZE) != 0) {
printf("FAIL\n");
printf(" Buffer corrupt\n");
goto abort_loopback;
}
printf("Ok\n");
abort_loopback:
hname = udp_host_addr(s1); /* we need this for the unicast test... */
udp_exit(s1);
/**********************************************************************/
/* Now we send a packet to ourselves via our real network address... */
printf
("Testing UDP/IP networking (IPv4 unicast) ................................. ");
fflush(stdout);
s1 = udp_init(hname, 5004, 5005, 1);
if (s1 == NULL) {
printf("FAIL\n");
printf(" Cannot initialize socket\n");
return 1;
}
s2 = udp_init(hname, 5005, 5004, 1);
if (s2 == NULL) {
printf("FAIL\n");
printf(" Cannot initialize socket\n");
return 1;
}
randomize(buf1, BUFSIZE);
randomize(buf2, BUFSIZE);
if (udp_send(s1, buf1, BUFSIZE) < 0) {
printf("FAIL\n");
perror(" Cannot send");
goto abort_unicast;
}
timeout.tv_sec = 1;
timeout.tv_usec = 0;
udp_fd_zero();
udp_fd_set(s1);
udp_fd_set(s2);
rc = udp_select(&timeout);
if (rc < 0) {
printf("FAIL\n");
perror(" Select failed");
goto abort_unicast;
}
if (rc == 0) {
printf("FAIL\n");
printf(" No data waiting (no route to %s?)\n", hname);
goto abort_unicast;
}
if (!udp_fd_isset(s2)) {
printf("FAIL\n");
printf(" No data on file descriptor\n");
goto abort_unicast;
}
if (udp_recv(s2, buf2, BUFSIZE) < 0) {
printf("FAIL\n");
perror(" Cannot receive");
goto abort_unicast;
}
if (memcmp(buf1, buf2, BUFSIZE) != 0) {
printf("FAIL\n");
printf(" Buffer corrupt\n");
goto abort_unicast;
}
printf("Ok\n");
abort_unicast:
udp_exit(s1);
udp_exit(s2);
/**********************************************************************/
/* Loopback a packet to ourselves via multicast... */
printf
("Testing UDP/IP networking (IPv4 multicast) ............................... ");
fflush(stdout);
s1 = udp_init("224.2.0.1", 5004, 5004, 1);
if (s1 == NULL) {
printf("FAIL\n");
printf(" Cannot initialize socket\n");
return 1;
}
randomize(buf1, BUFSIZE);
randomize(buf2, BUFSIZE);
if (udp_send(s1, buf1, BUFSIZE) < 0) {
printf("FAIL\n");
perror(" Cannot send");
goto abort_multicast;
}
timeout.tv_sec = 1;
timeout.tv_usec = 0;
udp_fd_zero();
udp_fd_set(s1);
rc = udp_select(&timeout);
if (rc < 0) {
printf("FAIL\n");
perror(" Select failed");
goto abort_multicast;
}
if (rc == 0) {
printf("FAIL\n");
printf(" No data waiting (no multicast loopback route?)\n");
goto abort_multicast;
}
if (!udp_fd_isset(s1)) {
printf("FAIL\n");
printf(" No data on file descriptor\n");
goto abort_multicast;
}
if (udp_recv(s1, buf2, BUFSIZE) < 0) {
printf("FAIL\n");
perror(" Cannot receive");
goto abort_multicast;
}
if (memcmp(buf1, buf2, BUFSIZE) != 0) {
printf("FAIL\n");
printf(" Buffer corrupt\n");
goto abort_multicast;
}
printf("Ok\n");
abort_multicast:
udp_exit(s1);
/**********************************************************************/
/* Loopback a packet to ourselves via multicast, checking lengths... */
printf
("Testing UDP/IP networking (IPv4 length check) ............................ ");
fflush(stdout);
s1 = udp_init("224.2.0.1", 5004, 5004, 1);
if (s1 == NULL) {
printf("FAIL\n");
printf(" Cannot initialize socket\n");
return 1;
}
for (i = 1; i < BUFSIZE; i++) {
randomize(buf1, i);
randomize(buf2, i);
if (udp_send(s1, buf1, i) < 0) {
printf("FAIL\n");
perror(" Cannot send");
goto abort_length;
}
timeout.tv_sec = 1;
timeout.tv_usec = 0;
udp_fd_zero();
udp_fd_set(s1);
rc = udp_select(&timeout);
if (rc < 0) {
printf("FAIL\n");
perror(" Select failed");
goto abort_length;
}
if (rc == 0) {
printf("FAIL\n");
printf
(" No data waiting (no multicast loopback route?)\n");
goto abort_length;
}
if (!udp_fd_isset(s1)) {
printf("FAIL\n");
printf(" No data on file descriptor\n");
goto abort_length;
}
if (udp_recv(s1, buf2, BUFSIZE) != i) {
printf("FAIL\n");
perror(" Cannot receive");
goto abort_length;
}
if (memcmp(buf1, buf2, i) != 0) {
printf("FAIL\n");
printf(" Buffer corrupt\n");
goto abort_length;
}
}
printf("Ok\n");
abort_length:
udp_exit(s1);
#ifdef HAVE_IPv6
/**********************************************************************/
/* The first test is to loopback a packet to ourselves... */
printf
("Testing UDP/IP networking (IPv6 loopback) ................................ ");
fflush(stdout);
s1 = udp_init("::1", 5004, 5004, 1);
if (s1 == NULL) {
printf("FAIL\n");
printf(" Cannot initialize socket\n");
return 1;
}
randomize(buf1, BUFSIZE);
randomize(buf2, BUFSIZE);
if (udp_send(s1, buf1, BUFSIZE) < 0) {
printf("FAIL\n");
perror(" Cannot send");
goto abort_loopback_ipv6;
}
timeout.tv_sec = 1;
timeout.tv_usec = 0;
udp_fd_zero();
udp_fd_set(s1);
rc = udp_select(&timeout);
if (rc < 0) {
printf("FAIL\n");
perror(" Select failed");
goto abort_loopback_ipv6;
}
if (rc == 0) {
printf("FAIL\n");
printf(" No data waiting\n");
goto abort_loopback_ipv6;
}
if (!udp_fd_isset(s1)) {
printf("FAIL\n");
printf(" No data on file descriptor\n");
goto abort_loopback_ipv6;
}
if (udp_recv(s1, buf2, BUFSIZE) < 0) {
printf("FAIL\n");
perror(" Cannot receive");
goto abort_loopback_ipv6;
}
if (memcmp(buf1, buf2, BUFSIZE) != 0) {
printf("FAIL\n");
printf(" Buffer corrupt\n");
goto abort_loopback_ipv6;
}
printf("Ok\n");
abort_loopback_ipv6:
udp_exit(s1);
/**********************************************************************/
/* Loopback a packet to ourselves via multicast. The address is the */
/* SAP address, but we use a different port. */
printf
("Testing UDP/IP networking (IPv6 multicast) ............................... ");
fflush(stdout);
s1 = udp_init("ff01::2:7ffe", 5004, 5004, 1);
if (s1 == NULL) {
printf("FAIL\n");
printf(" Cannot initialize socket\n");
return 1;
}
randomize(buf1, BUFSIZE);
randomize(buf2, BUFSIZE);
if (udp_send(s1, buf1, BUFSIZE) < 0) {
printf("FAIL\n");
perror(" Cannot send");
goto abort_multicast_ipv6;
}
timeout.tv_sec = 1;
timeout.tv_usec = 0;
udp_fd_zero();
udp_fd_set(s1);
rc = udp_select(&timeout);
if (rc < 0) {
printf("FAIL\n");
perror(" Select failed");
goto abort_multicast_ipv6;
}
if (rc == 0) {
printf("FAIL\n");
printf(" No data waiting (no multicast loopback route?)\n");
goto abort_multicast_ipv6;
}
if (!udp_fd_isset(s1)) {
printf("FAIL\n");
printf(" No data on file descriptor\n");
goto abort_multicast_ipv6;
}
if (udp_recv(s1, buf2, BUFSIZE) < 0) {
printf("FAIL\n");
perror(" Cannot receive");
goto abort_multicast_ipv6;
}
if (memcmp(buf1, buf2, BUFSIZE) != 0) {
printf("FAIL\n");
printf(" Buffer corrupt\n");
goto abort_multicast_ipv6;
}
hname = udp_host_addr(s1); /* we need this for the unicast test... */
printf("Ok\n");
abort_multicast_ipv6:
udp_exit(s1);
#else
printf
("Testing UDP/IP networking (IPv6 loopback) ................................ --\n");
printf
("Testing UDP/IP networking (IPv6 unicast) ................................. --\n");
printf
("Testing UDP/IP networking (IPv6 multicast) ............................... --\n");
#endif
/**********************************************************************/
#ifdef WIN32
printf
("Testing UDP/IP networking (FreeBSD bug) .................................. --\n");
#else
printf
("Testing UDP/IP networking (FreeBSD bug) .................................. ");
fflush(stdout);
status_parent = 0;
randomize(buf1, 64);
s1 = udp_init("224.2.0.1", 5004, 5004, 1);
if (s1 == NULL) {
printf("fail (parent): cannot initialize socket\n");
return 1;
}
rc = fork();
if (rc == -1) {
printf("fail: cannot fork\n");
goto abort_bsd;
} else if (rc == 0) {
/* child */
s2 = udp_init("224.2.0.1", 5004, 5004, 1);
if (s2 == NULL) {
printf("FAIL\n");
printf(" Child cannot initialize socket\n");
exit(0);
}
if (udp_send(s2, buf1, 64) < 0) {
printf("FAIL\n");
perror(" Child cannot send");
exit(0);
}
timeout.tv_sec = 10;
timeout.tv_usec = 0;
udp_fd_zero();
udp_fd_set(s2);
rc = udp_select(&timeout);
if (rc < 0) {
printf("FAIL\n");
perror(" Child select");
exit(0);
}
if (rc == 0) {
printf("FAIL\n");
printf
(" Child: no data waiting (no multicast loopback route?)\n");
exit(0);
}
if (!udp_fd_isset(s2)) {
printf("FAIL\n");
printf(" Child: no data on file descriptor\n");
exit(0);
}
rc = udp_recv(s2, buf2, BUFSIZE);
if (rc < 0) {
printf("FAIL\n");
perror(" Child cannot receive");
exit(0);
}
if (rc != 64) {
printf("FAIL\n");
printf(" Child: read size incorrect (%d != %d)\n", rc,
64);
exit(0);
}
if (memcmp(buf1, buf2, 64) != 0) {
printf("FAIL\n");
printf(" Child: buffer corrupt\n");
exit(0);
}
udp_exit(s2);
exit(1);
} else {
/* parent */
timeout.tv_sec = 10;
timeout.tv_usec = 0;
udp_fd_zero();
udp_fd_set(s1);
rc = udp_select(&timeout);
if (rc < 0) {
printf("FAIL\n");
perror(" Parent select");
goto abort_bsd;
}
if (rc == 0) {
printf("FAIL\n");
printf
(" Parent: no data waiting (no multicast loopback route?)\n");
goto abort_bsd;
}
if (!udp_fd_isset(s1)) {
printf("FAIL\n");
printf(" Parent: no data on file descriptor\n");
goto abort_bsd;
}
rc = udp_recv(s1, buf2, BUFSIZE);
if (rc < 0) {
printf("FAIL\n");
perror(" Parent cannot receive");
goto abort_bsd;
}
if (rc != 64) {
printf("FAIL\n");
printf(" Parent: read size incorrect (%d != %d)\n", rc,
64);
goto abort_bsd;
}
if (memcmp(buf1, buf2, 64) != 0) {
printf("FAIL\n");
printf(" Parent: buffer corrupt\n");
goto abort_bsd;
}
status_parent = 1;
}
abort_bsd:
wait(&status_child);
if (status_parent && status_child) {
printf("Ok\n");
}
udp_exit(s1);
#endif /* WIN32 */
return 0;
}
void umtp_done(umtp_t umtp)
{
void *thr_retval;
if( umtp->flag == MASTER && umtp_error.command ) // notify user about error
umtp_error_notify(umtp, TRUE);
umtp_end = 1;
if( umtp->active_groups != NULL )
{
umtp_group *g, *temp = NULL;
fprintf(umtp->log, " [umtp__done] REMOVE GROUPS\n");
g = umtp->active_groups;
while( g != NULL )
{
temp = g->next;
fprintf(umtp->log, " [umtp__done] REMOVE GROUP (%s/%d)\n", umtp_ntoa(g->group), g->port);
umtp_destroy_group(umtp, g);
g = temp;
}
}
if( umtp->allowed_endpoints != NULL )
{
int bufLen = 0;
umtp_packet pkt;
umtp_header header;
unsigned char *packet_ptr = NULL;
struct in_addr saddr;
// send TEAR_DOWN to every user or server
umtp_endpoint *temp = NULL, *ep = umtp->allowed_endpoints;
fprintf(umtp->log, " [umtp__done] REMOVE ENDPOINTS\n");
while( ep != NULL )
{
if( ep->host != inet_addr(umtp->addr) && ep->remote_cookie != 0)
{
fprintf(umtp->log, " [umtp__done] TEAR_DOWN to %s\n", umtp_ntoa(ep->host));
saddr.s_addr = (in_addr_t)ep->host;
pkt.payload = NULL;
make_udp_send_header( &header, TEAR_DOWN, 0, 0, 0,
ep->remote_cookie,
umtp->local_cookie);
pkt.header = &header;
pkt.source = inet_addr(umtp->addr);
packet_ptr = create_udp_payload(&pkt, 0, &bufLen);
umtp->s->addr4.s_addr = ep->host;
umtp->s->tx_port = ep->port;
udp_send(umtp->s, packet_ptr, bufLen);
if(packet_ptr != NULL ) free(packet_ptr);
}
temp = ep->next;
umtp_remove_endpoint_(umtp, ep->host, ep->port);
ep = temp;
}
}
fprintf(umtp->log, " [umtp__done] REMOVE SOCKETS...\n");
fprintf(umtp->log, " [umtp__done] 1. UNICAST SOCKET\n");
fprintf(umtp->log, " [umtp__done] WAIT FOR UNICAST SOCKET CLOSE...\n");
pthread_join(umtp->u_id, &thr_retval);
if( umtp->s1 != NULL ) udp_exit(umtp->s1);
fprintf(umtp->log, " [umtp__done] DONE.\n");
if(umtp->mstate == MPROBE_START )
{
fprintf(umtp->log, " [umtp__done] 2. MULTICAST CHECK SOCKET\n");
fprintf(umtp->log, " [umtp__done] WAIT FOR MULTICAST CHECK SOCKET CLOSE...\n");
pthread_join(umtp->m_id, &thr_retval);
if( umtp->m != NULL ) udp_exit(umtp->m);
fprintf(umtp->log, " [umtp__done] DONE.\n");
}
fprintf(umtp->log, " [umtp__done] 3. UMTP SOCKET\n");
fprintf(umtp->log, " [umtp__done] WAIT FOR UMTPSOCKET SOCKET CLOSE...\n");
if( umtp->s != NULL ) udp_exit(umtp->s);
fprintf(umtp->log, " [umtp__done] DONE.\n");
if(umtp->addr != NULL ) free(umtp->addr);
if(umtp->log != NULL) fclose(umtp->log);
if(umtp != NULL ) free(umtp);
ring_buf_close();
printf(" [umtp__done] UMTP ENDS.\n");
}
/**
* \brief Initialize UDP socket.
*
* @param[out] sock socket number
* @param[in,out] port port number, 0 if to be chosen by the OS
* @param[in] ip multicast ip to listen
* @return 0 if ok, < 0 if error occured
*/
int udp_init(void **sock, unsigned short *port, unsigned int ip)
{
struct _ip_socket_struct *s;
struct sockaddr_in addr;
#ifdef OS_UNIX
socklen_t addrlen;
#endif
#ifdef OS_WIN
int addrlen;
#endif
s = (struct _ip_socket_struct *)malloc(sizeof(struct _ip_socket_struct));
if (s == NULL)
{
return -11;
}
// create socket:
#ifdef OS_UNIX
s->ossock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (s->ossock < 0)
{
free(s);
return -2;
}
#endif
#ifdef OS_WIN
s->ossock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (s->ossock == INVALID_SOCKET)
{
free(s);
return -2;
}
#endif
if (ip != 0)
{
// set reuse port to on to allow multiple binds per host
int flag_on = 1;
if ((setsockopt(s->ossock, SOL_SOCKET, SO_REUSEADDR, (char *)&flag_on, sizeof(flag_on))) < 0)
{
perror("setsockopt() failed1");
udp_exit(s);
return -4;
}
}
// name socket:
addr.sin_family = AF_INET;
addr.sin_port = htons(*port);
addr.sin_addr.s_addr = htonl(INADDR_ANY);
addrlen = sizeof(addr);
if (bind(s->ossock, (struct sockaddr *)&addr, addrlen) < 0)
{
udp_exit(s);
return -3;
}
if (*port == 0)
{
// port number was chosen by the OS
if (getsockname(s->ossock, (struct sockaddr *)&addr, &addrlen))
{
udp_exit(s);
return -3;
}
*port = ntohs(addr.sin_port);
}
if (ip != 0)
{
// construct an IGMP join request structure
struct ip_mreq ipmreq;
ipmreq.imr_multiaddr.s_addr = htonl(ip);
ipmreq.imr_interface.s_addr = htonl(INADDR_ANY);
// send an ADD MEMBERSHIP message via setsockopt
if ((setsockopt(s->ossock, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *)&ipmreq, sizeof(ipmreq))) < 0)
{
perror("setsockopt() failed2");
udp_exit(s);
return -5;
}
}
*sock = s;
return 0;
}
