int open_dev(int id)
{
int fd = 0;
switch(devtype) {
#ifdef TAP
case DEV_TAP:
fd = open_tap(id);
if (fd <= 0) {
fd = 0;
return 0;
}
break;
#endif
case DEV_UDP:
fd = open_udp(vpc[id].lport);
if (fd <= 0) {
fd = 0;
return 0;
}
break;
}
return fd;
}
// NPS FDM interface
void nps_fdm_init(double dt)
{
fdm.init_dt = dt;
fdm.curr_dt = dt;
fdm.nan_count = 0;
fdm.pressure = -1;
fdm.pressure_sl = PPRZ_ISA_SEA_LEVEL_PRESSURE;
fdm.total_pressure = -1;
fdm.dynamic_pressure = -1;
fdm.temperature = -1;
init_ltp();
// init circfuf
inputbuf_init(&crrcsim.buf);
printf("Starting to connect to CRRCsim server.\n");
open_udp((char *)(NPS_CRRCSIM_HOST_IP), NPS_CRRCSIM_HOST_PORT, UDP_NONBLOCKING);
if (crrcsim.socket < 0) {
printf("Connection to CRRCsim failed\n");
exit(0);
} else {
printf("Connection to CRRCsim succed\n");
}
// Send something to let crrcsim that we are here
double zero[] = { 0., 0., 0. };
send_servo_cmd(&crrcsim, zero);
}
void *pth_relay(void *dummy)
{
char buf[1500];
int len;
int n = 0;
struct sockaddr_in peeraddr;
struct sockaddr_in addr;
socklen_t size;
struct peerlist *peerhost;
relay_port = vpc[0].lport + MAX_NUM_PTHS;
relay_fd = open_udp(relay_port);
if (relay_fd <= 0)
relay_fd = 0;
/* waiting hub enable */
while (!peerlist)
sleep(1);
while (1) {
len = sizeof(buf);
size = sizeof(struct sockaddr_in);
n = recvfrom(relay_fd, buf, len, 0,
(struct sockaddr *)&peeraddr, &size);
bzero(&addr, sizeof(addr));
addr.sin_family = AF_INET;
peerhost = peerlist;
for (;peerhost;) {
if (peerhost->nodea.port == peeraddr.sin_port) {
if (peerhost->nodea.ip == htonl(INADDR_ANY) ||
peerhost->nodea.ip == peeraddr.sin_addr.s_addr) {
addr.sin_addr.s_addr = peerhost->nodeb.ip;
addr.sin_port = peerhost->nodeb.port;
break;
}
}
if (peerhost->nodeb.port == peeraddr.sin_port) {
if (peerhost->nodeb.ip == htonl(INADDR_ANY) ||
peerhost->nodeb.ip == peeraddr.sin_addr.s_addr) {
addr.sin_addr.s_addr = peerhost->nodea.ip;
addr.sin_port = peerhost->nodea.port;
break;
}
}
peerhost = peerhost->next;
}
if (addr.sin_port) {
sendto(relay_fd, buf, n, 0,
(struct sockaddr *)&addr, sizeof(addr));
}
}
return NULL;
}
/* Start UDP remote server */
int
rem1(
int argc,
char *argv[],
void *p)
{
struct socket sock;
sock.address = INADDR_ANY;
if(argc < 2)
sock.port = IPPORT_REMOTE;
else
sock.port = atoi(argv[1]);
remote_up = open_udp(&sock,uremote);
return 0;
}
void keyboard_thread(void *arg) {
int rfd, ufd;
fd_set rfds;
int maxfd;
int ret;
int pipefd = *( (int *) arg);
short buf;
rfd = open(KEYBOARD, O_RDONLY);
ufd = open_udp();
maxfd = rfd;
if(ufd>maxfd)
maxfd = ufd;
maxfd++;
while(1) {
FD_ZERO(&rfds);
FD_SET(ufd, &rfds);
FD_SET(rfd, &rfds);
ret = select(maxfd, &rfds, NULL, NULL, NULL);
if(FD_ISSET(ufd, &rfds)) {
ret = read(ufd, &buf, sizeof(buf));
if(ret == sizeof(buf))
write(pipefd, &buf, ret);
printf("Got UDP: %d (err=%d)\n", ret, errno);
}
if(FD_ISSET(rfd, &rfds)) {
ret = read(rfd, &buf, sizeof(buf));
if(ret == sizeof(buf))
write(pipefd, &buf, ret);
printf("Got IR: %d (err=%d)\n", ret, errno);
}
}
}
int run_relay(int argc, char **argv)
{
int port;
struct peerlist peer, *tpeer, *peerhost;
struct in_addr in;
char tmp[32];
char *p;
int i, j;
if (argc == 3 && !strcmp(argv[1], "port")) {
port = atoi(argv[2]);
if (port > 1024 && port < 65534)
relay_port = port;
if (relay_fd) {
close(relay_fd);
relay_fd = open_udp(relay_port);
if (relay_fd <= 0) {
printf("Open relay port %d error [%s]\n",
relay_port, strerror(errno));
}
} else
printf("The port is out of range\n");
return 0;
}
if (argc == 2 && !strcmp(argv[1], "show")) {
printf("Relay port: %d\n", relay_port);
peerhost = peerlist;
printf("Relay list");
if (!peerhost || peerhost->nodea.port == 0) {
printf(": none\n");
return 0;
}
printf(":\n");
i = 0;
while (peerhost) {
in.s_addr = peerhost->nodea.ip;
printf(" %2d %s:%d", ++i, inet_ntoa(in), ntohs(peerhost->nodea.port));
in.s_addr = peerhost->nodeb.ip;
printf(" <-> %s:%d\n", inet_ntoa(in), ntohs(peerhost->nodeb.port));
peerhost = peerhost->next;
}
return 0;
}
if (argc == 4 && !strcmp(argv[1], "add")) {
p = strchr(argv[2], ':');
if (p) {
bzero(tmp, sizeof(tmp));
strncpy(tmp, argv[2], p - argv[2]);
peer.nodea.ip = inet_addr(tmp);
i = atoi(p + 1);
if (i < 1024 || i > 65534) {
printf("port %d is out of range\n", i);
return 0;
}
peer.nodea.port = htons(i);
} else {
peer.nodea.ip = htonl(INADDR_ANY);
i = atoi(argv[2]);
if (i < 1024 || i > 65534) {
printf("port %d is out of range\n", i);
return 0;
}
peer.nodea.port = htons(i);
}
p = strchr(argv[3], ':');
if (p) {
bzero(tmp, sizeof(tmp));
strncpy(tmp, argv[3], p - argv[3]);
peer.nodeb.ip = inet_addr(tmp);
i = atoi(p + 1);
if (i < 1024 || i > 65534) {
printf("port %d is out of range\n", i);
return 0;
}
peer.nodeb.port = htons(i);
} else {
peer.nodeb.ip = htonl(INADDR_ANY);
i = atoi(argv[3]);
if (i < 1024 || i > 65534) {
printf("port %d is out of range\n", i);
return 0;
}
peer.nodeb.port = htons(i);
}
/* existed ? */
peerhost = peerlist;
for (j = 0;peerhost;) {
if (((peerhost->nodea.ip == peer.nodea.ip) &&
(peerhost->nodea.port == peer.nodea.port)) ||
((peerhost->nodeb.ip == peer.nodea.ip) &&
(peerhost->nodeb.port == peer.nodea.port))) {
in.s_addr = peer.nodea.ip;
port = peer.nodea.port;
j = 1;
break;
}
if (((peerhost->nodea.ip == peer.nodeb.ip) &&
(peerhost->nodea.port == peer.nodeb.port)) ||
((peerhost->nodeb.ip == peer.nodeb.ip) &&
(peerhost->nodeb.port == peer.nodeb.port))) {
in.s_addr = peer.nodeb.ip;
port = peer.nodeb.port;
j = 1;
break;
}
peerhost = peerhost->next;
}
if (j == 1) {
printf("%s:%d is existed\n", inet_ntoa(in), ntohs(port));
return 0;
}
/* append the rule */
tpeer = (struct peerlist *)malloc(sizeof(struct peerlist));
if (tpeer) {
memcpy(tpeer, &peer, sizeof(peer));
tpeer->next = NULL;
} else
printf("Out of memory\n");
if (peerlist == NULL)
peerlist = tpeer;
else {
peerhost = peerlist;
while (peerhost->next)
peerhost = peerhost->next;
peerhost->next = tpeer;
}
return 0;
}
/* relay del <id> */
if (argc == 3 && !strcmp(argv[1], "del")) {
j = atoi(argv[2]);
tpeer = peerlist;
/* drop the head */
if (j == 1) {
if (peerlist) {
peerlist = peerlist->next;
free(tpeer);
}
return 0;
}
peerhost = tpeer->next;
i = 2;
while (peerhost) {
if (i == j) {
tpeer->next = peerhost->next;
free(peerhost);
break;
}
tpeer = peerhost;
peerhost = peerhost->next;
i++;
}
return 0;
}
/* relay del port port */
if (argc == 4 && !strcmp(argv[1], "del")) {
p = strchr(argv[2], ':');
if (p) {
bzero(tmp, sizeof(tmp));
strncpy(tmp, argv[2], p - argv[2]);
peer.nodea.ip = inet_addr(tmp);
peer.nodea.port = htons(atoi(p + 1));
} else {
peer.nodea.ip = htonl(INADDR_ANY);
peer.nodea.port = htons(atoi(argv[2]));
}
p = strchr(argv[3], ':');
if (p) {
bzero(tmp, sizeof(tmp));
strncpy(tmp, argv[3], p - argv[3]);
peer.nodeb.ip = inet_addr(tmp);
peer.nodeb.port = htons(atoi(p + 1));
} else {
peer.nodeb.ip = htonl(INADDR_ANY);
peer.nodea.port = htons(atoi(argv[3]));
}
tpeer = peerlist;
peerhost = peerlist;
for (;peerhost;) {
if ((peerhost->nodea.ip == peer.nodea.ip) &&
(peerhost->nodea.port == peer.nodea.port) &&
(peerhost->nodeb.ip == peer.nodeb.ip) &&
(peerhost->nodea.port == peer.nodea.port)) {
if (tpeer == peerlist)
peerlist = peerhost->next;
else
tpeer->next = peerhost->next;
free(peerhost);
break;
}
tpeer = peerhost;
peerhost = peerhost->next;
}
return 0;
}
return 0;
}
static void process_client(const char *node, const char *port, int fd)
{
char **temp_files;
char buf[BUFSIZ];
char *option;
int *port_array;
int *pid_array;
int pagesize;
int start_port;
int udp_port;
int options;
int size;
int cpus;
int cpu;
int pid;
int ofd;
int n, s, t, i;
/* Let the client know what we are */
write(fd, "tracecmd", 8);
/* read back the CPU count */
n = read_string(fd, buf, BUFSIZ);
if (n == BUFSIZ)
/** ERROR **/
return;
cpus = atoi(buf);
plog("cpus=%d\n", cpus);
if (cpus < 0)
return;
/* next read the page size */
n = read_string(fd, buf, BUFSIZ);
if (n == BUFSIZ)
/** ERROR **/
return;
pagesize = atoi(buf);
plog("pagesize=%d\n", pagesize);
if (pagesize <= 0)
return;
/* Now the number of options */
n = read_string(fd, buf, BUFSIZ);
if (n == BUFSIZ)
/** ERROR **/
return;
options = atoi(buf);
for (i = 0; i < options; i++) {
/* next is the size of the options */
n = read_string(fd, buf, BUFSIZ);
if (n == BUFSIZ)
/** ERROR **/
return;
size = atoi(buf);
/* prevent a client from killing us */
if (size > MAX_OPTION_SIZE)
return;
option = malloc_or_die(size);
do {
t = size;
s = 0;
s = read(fd, option+s, t);
if (s <= 0)
return;
t -= s;
s = size - t;
} while (t);
s = process_option(option);
free(option);
/* do we understand this option? */
if (!s)
return;
}
if (use_tcp)
plog("Using TCP for live connection\n");
/* Create the client file */
snprintf(buf, BUFSIZ, "%s.%s:%s.dat", output_file, node, port);
ofd = open(buf, O_RDWR | O_CREAT | O_TRUNC, 0644);
if (ofd < 0)
pdie("Can not create file %s", buf);
port_array = malloc_or_die(sizeof(int) * cpus);
pid_array = malloc_or_die(sizeof(int) * cpus);
memset(pid_array, 0, sizeof(int) * cpus);
start_port = START_PORT_SEARCH;
/* Now create a UDP port for each CPU */
for (cpu = 0; cpu < cpus; cpu++) {
udp_port = open_udp(node, port, &pid, cpu, pagesize, start_port);
if (udp_port < 0)
goto out_free;
port_array[cpu] = udp_port;
pid_array[cpu] = pid;
/* due to some bugging finding ports, force search after last port */
start_port = udp_port+1;
}
/* send the client a comma deliminated set of port numbers */
for (cpu = 0; cpu < cpus; cpu++) {
snprintf(buf, BUFSIZ, "%s%d",
cpu ? "," : "", port_array[cpu]);
write(fd, buf, strlen(buf));
}
/* end with null terminator */
write(fd, "\0", 1);
/* Now we are ready to start reading data from the client */
do {
n = read(fd, buf, BUFSIZ);
if (n < 0) {
if (errno == EINTR)
continue;
pdie("reading client");
}
t = n;
s = 0;
do {
s = write(ofd, buf+s, t);
if (s < 0) {
if (errno == EINTR)
break;
pdie("writing to file");
}
t -= s;
s = n - t;
} while (t);
} while (n > 0 && !done);
/* wait a little to let our readers finish reading */
sleep(1);
/* stop our readers */
for (cpu = 0; cpu < cpus; cpu++) {
if (pid_array[cpu] > 0)
kill(pid_array[cpu], SIGUSR1);
}
/* wait a little to have the readers clean up */
sleep(1);
/* Now put together the file */
temp_files = malloc_or_die(sizeof(*temp_files) * cpus);
for (cpu = 0; cpu < cpus; cpu++)
temp_files[cpu] = get_temp_file(node, port, cpu);
tracecmd_attach_cpu_data_fd(ofd, cpus, temp_files);
out_free:
for (cpu = 0; cpu < cpus; cpu++) {
if (pid_array[cpu] > 0) {
kill(pid_array[cpu], SIGKILL);
delete_temp_file(node, port, cpu);
pid_array[cpu] = 0;
}
}
}
