nuclear@2: #include <stdio.h>
nuclear@2: #include <stdlib.h>
nuclear@2: #include <string.h>
nuclear@2: #include <errno.h>
nuclear@2: #include <unistd.h>
nuclear@2: #include <sys/types.h>
nuclear@2: #include <sys/socket.h>
nuclear@2: #include <arpa/inet.h>
nuclear@2: #include <sys/time.h>
nuclear@2: #include <sys/select.h>
nuclear@2: #include "inpclient.h"
nuclear@2: #include "proto.h"
nuclear@2: 
nuclear@2: static int discover(struct sockaddr_in *srv_sa);
nuclear@3: static int send_request(int s, struct message *msg, long timeout);
nuclear@2: 
nuclear@2: static int opt_dport = DEF_PORT;	/* discover broadcast port */
nuclear@2: static long opt_timeout = 5000;	/* 10 sec discovery timeout */
nuclear@2: 
nuclear@2: static int sock = -1;
nuclear@2: 
nuclear@2: 
nuclear@2: int netinp_start()
nuclear@2: {
nuclear@2: 	struct message msg;
nuclear@2: 	struct sockaddr_in srv_sa;
nuclear@2: 
nuclear@2: 	if(sock == -1) {
nuclear@2: 		if((sock = discover(&srv_sa)) == -1) {
nuclear@2: 			return -1;
nuclear@2: 		}
nuclear@2: 
nuclear@2: 		if(connect(sock, (struct sockaddr*)&srv_sa, sizeof srv_sa) == -1) {
nuclear@2: 			perror("failed to connect socket");
nuclear@2: 			close(sock);
nuclear@2: 			return -1;
nuclear@2: 		}
nuclear@2: 	}
nuclear@2: 
nuclear@2: 	memset(&msg, 0, sizeof msg);
nuclear@2: 	msg.magic = MAGIC;
nuclear@3: 	msg.type = REQ_START;
nuclear@3: 	return send_request(sock, &msg, opt_timeout);
nuclear@2: }
nuclear@2: 
nuclear@2: int netinp_stop()
nuclear@2: {
nuclear@3: 	struct message msg;
nuclear@3: 
nuclear@3: 	if(sock == -1) {
nuclear@3: 		return -1;
nuclear@3: 	}
nuclear@3: 
nuclear@3: 	memset(&msg, 0, sizeof msg);
nuclear@3: 	msg.magic = MAGIC;
nuclear@3: 	msg.type = REQ_STOP;
nuclear@3: 	return send_request(sock, &msg, opt_timeout);
nuclear@3: }
nuclear@3: 
nuclear@3: int netinp_read(float *pos)
nuclear@3: {
nuclear@3: 	struct message msg;
nuclear@3: 
nuclear@3: 	if(sock == -1) {
nuclear@3: 		return -1;
nuclear@3: 	}
nuclear@3: 
nuclear@3: 	if(recv(sock, &msg, sizeof msg, 0) == -1) {
nuclear@3: 		perror("netinp_read failed");
nuclear@3: 		return -1;
nuclear@3: 	}
nuclear@3: 
nuclear@3: 	pos[0] = ((float*)msg.data)[0];
nuclear@3: 	pos[1] = ((float*)msg.data)[1];
nuclear@3: 	pos[2] = ((float*)msg.data)[3];
nuclear@3: 	return 0;
nuclear@3: }
nuclear@3: 
nuclear@3: int netinp_reset()
nuclear@3: {
nuclear@3: 	return -1;	// TODO
nuclear@2: }
nuclear@2: 
nuclear@2: static int discover(struct sockaddr_in *srv_sa)
nuclear@2: {
nuclear@2: 	int s, true_val = 1;
nuclear@2: 	struct sockaddr_in sa;
nuclear@2: 	long timeout = opt_timeout;
nuclear@2: 	struct message msg;
nuclear@2: 	struct timeval tv0, tv, tv_timeout;
nuclear@2: 
nuclear@2: 	if((s = socket(PF_INET, SOCK_DGRAM, 0)) == -1) {
nuclear@2: 		perror("failed to create discover datagram socket");
nuclear@2: 		return -1;
nuclear@2: 	}
nuclear@2: 	setsockopt(s, SOL_SOCKET, SO_BROADCAST, &true_val, sizeof true_val);
nuclear@2: 
nuclear@2: 	memset(&sa, 0, sizeof sa);
nuclear@2: 	sa.sin_family = AF_INET;
nuclear@2: 	sa.sin_port = htons(opt_dport);
nuclear@2: 	sa.sin_addr.s_addr = htonl(INADDR_ANY);
nuclear@2: 
nuclear@2: 	if(bind(s, (struct sockaddr*)&sa, sizeof sa) == -1) {
nuclear@2: 		perror("failed to bind datagram socket");
nuclear@2: 		close(s);
nuclear@2: 		return -1;
nuclear@2: 	}
nuclear@2: 
nuclear@2: 	do {
nuclear@2: 		fd_set rdset;
nuclear@2: 
nuclear@2: 		msg.magic = MAGIC;
nuclear@2: 		msg.type = REQ_DISCOVER;
nuclear@2: 
nuclear@2: 		memset(&sa, 0, sizeof sa);
nuclear@2: 		sa.sin_family = AF_INET;
nuclear@2: 		sa.sin_port = htons(opt_dport);
nuclear@2: 		sa.sin_addr.s_addr = 0xffffffff;
nuclear@2: 
nuclear@2: 		if(sendto(s, &msg, sizeof msg, 0, (struct sockaddr*)&sa, sizeof sa) == -1) {
nuclear@2: 			perror("failed to send discovery bcast dgram");
nuclear@2: 			close(s);
nuclear@2: 			return -1;
nuclear@2: 		}
nuclear@2: 
nuclear@2: 		tv_timeout.tv_sec = timeout / 1000;
nuclear@2: 		tv_timeout.tv_usec = (timeout % 1000) * 1000;
nuclear@2: 
nuclear@2: 		FD_ZERO(&rdset);
nuclear@2: 		FD_SET(s, &rdset);
nuclear@2: 
nuclear@2: 		gettimeofday(&tv0, 0);
nuclear@2: 
nuclear@2: 		while(select(s + 1, &rdset, 0, 0, &tv_timeout) == -1 && errno == EINTR);
nuclear@2: 
nuclear@2: 		gettimeofday(&tv, 0);
nuclear@2: 		timeout -= (tv.tv_sec - tv0.tv_sec) * 1000 + (tv.tv_usec - tv0.tv_usec) / 1000;
nuclear@2: 
nuclear@2: 		if(FD_ISSET(s, &rdset)) {
nuclear@2: 			socklen_t sa_size = sizeof *srv_sa;
nuclear@2: 			if(recvfrom(s, &msg, sizeof msg, 0, (struct sockaddr*)srv_sa, &sa_size) == -1) {
nuclear@2: 				perror("failed to receive datagram\n");
nuclear@2: 			}
nuclear@2: 			if(msg.magic == MAGIC && msg.type == RSP_OK) {
nuclear@2: 				printf("found input server \"%s\" at: %s:%d\n", msg.data,
nuclear@2: 						inet_ntoa(srv_sa->sin_addr), ntohs(srv_sa->sin_port));
nuclear@2: 				return s;
nuclear@2: 			}
nuclear@2: 		}
nuclear@2: 
nuclear@2: 	} while(timeout > 0);
nuclear@2: 
nuclear@2: 	close(s);
nuclear@2: 	return -1;
nuclear@2: }
nuclear@2: 
nuclear@3: static int send_request(int s, struct message *msg, long timeout)
nuclear@2: {
nuclear@3: 	fd_set rdset;
nuclear@3: 	struct timeval tv, tv0, tv_timeout;
nuclear@3: 	struct message resp;
nuclear@2: 
nuclear@3: 	do {
nuclear@3: 		if(send(s, msg, sizeof *msg, 0) == -1) {
nuclear@3: 			perror("failed to send message");
nuclear@3: 			return -1;
nuclear@3: 		}
nuclear@3: 
nuclear@3: 		tv_timeout.tv_sec = timeout / 1000;
nuclear@3: 		tv_timeout.tv_usec = (timeout % 1000) * 1000;
nuclear@3: 
nuclear@3: 		FD_ZERO(&rdset);
nuclear@3: 		FD_SET(s, &rdset);
nuclear@3: 
nuclear@3: 		gettimeofday(&tv0, 0);
nuclear@3: 		while(select(s + 1, &rdset, 0, 0, &tv_timeout) == -1 && errno == EINTR);
nuclear@3: 		gettimeofday(&tv, 0);
nuclear@3: 
nuclear@3: 		if(FD_ISSET(s, &rdset)) {
nuclear@3: 			if(recv(s, &resp, sizeof resp, 0) == -1) {
nuclear@3: 				perror("failed to recieve message");
nuclear@3: 				return -1;
nuclear@3: 			}
nuclear@3: 			if(resp.magic != MAGIC) {
nuclear@3: 				fprintf(stderr, "got invalid response\n");
nuclear@3: 				return -1;
nuclear@3: 			}
nuclear@3: 			memcpy(msg, &resp, sizeof resp);
nuclear@3: 			return 0;
nuclear@3: 		}
nuclear@3: 
nuclear@3: 		timeout -= (tv.tv_sec - tv0.tv_sec) * 1000 + (tv.tv_usec - tv0.tv_usec) / 1000;
nuclear@3: 	} while(timeout > 0);
nuclear@3: 
nuclear@3: 	return -1;
nuclear@2: }