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; } } }