trace_proxy.c

/*
	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2008, 2009, 2013
	Copyright Stichting C.A. Muller Radioastronomiestation, 2008, 2009

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.

 */

#include "dt_port_numbers.h"
#include "dt_host.h"

#include <trace.h>
#include "tcp_listen.h"

#include <stdio.h>
#include <signal.h>
#include <sys/select.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <string.h>
#include <stdlib.h>
#include <errno.h>

#define TIMEOUT 10
#define VALCLIENT_UPDATE_NSEC (100*1000*1000)

static char *tr_host;
static int tr_port;

static struct trace_pkt *interval_pkt;
static struct timespec base_interval;

struct private {
	int nr_clients;
	struct trace **clients;

	time_t lastclient;
	struct trace_value lastvalue;
};


static int nr_client_traces = 0;
static struct trace **client_traces;

static int nr_server_traces = 0;
static struct trace **server_traces;

static void common_denom(struct timespec *common, struct timespec *int1, struct timespec *int2)
{
	uint64_t i1, i2, b;
	uint64_t d1, d2;
	uint64_t inew;
	
	i1 = int1->tv_sec * 1000000000 + int1->tv_nsec;
	i2 = int2->tv_sec * 1000000000 + int2->tv_nsec;
	b = base_interval.tv_sec * 1000000000 + base_interval.tv_nsec;
	
	d1 = i1 / b;
	d2 = i2 / b;
	
	if (d2 && d1 > d2 && !(d1 % d2)) {
		inew = d2;
	} else if (d1 && d2 > d1 && !(d2 % d1)) {
		inew = d1;
	} else {
		inew = 1;
	}
	
	common->tv_sec = base_interval.tv_sec * inew;
	common->tv_nsec = base_interval.tv_nsec * inew;
	common->tv_sec += common->tv_nsec / 1000000000;
	common->tv_nsec %= 1000000000;
}

static void server_interval_check(struct trace *server)
{
	struct private *priv = server->private;
	enum trace_interval_type type = 0;
	struct timespec interval;
	int i;
	bool has_valclient;
	bool fullrate = false;
	
	interval.tv_sec = 0;
	interval.tv_nsec = 0;
	
	has_valclient = priv->lastclient + TIMEOUT > time(NULL);
	
	if (has_valclient) {
		interval.tv_sec = 0;
		interval.tv_nsec = VALCLIENT_UPDATE_NSEC;
	}

	for (i = 0; i < priv->nr_clients; i++) {
		switch (priv->clients[i]->interval_type) {
			case TRACE_INTERVAL_TYPE_INTERVAL:
				if (type) {
					fullrate = true;
				} else {
					type = TRACE_INTERVAL_TYPE_INTERVAL;
				}
				
				/* Search for a common interval for all clients */
				common_denom(&interval, 
				    &interval, &priv->clients[i]->interval);

				break;
			case TRACE_INTERVAL_TYPE_CHANGED:
				if (type) {
					fullrate = true;
				} else {
					type = TRACE_INTERVAL_TYPE_CHANGED;
				}
				break;
			default:
				break;
		}
		
	}

	if (!type && has_valclient) {
		type = TRACE_INTERVAL_TYPE_INTERVAL;
		interval.tv_sec = 0;
		interval.tv_nsec = VALCLIENT_UPDATE_NSEC;
	}

	if (fullrate) {
		type = TRACE_INTERVAL_TYPE_INTERVAL;
		interval.tv_sec = base_interval.tv_sec;
		interval.tv_nsec = base_interval.tv_nsec;
	}

	if ((server->interval_type != type) ||
	    (server->interval.tv_sec != interval.tv_sec) ||
	    (server->interval.tv_nsec != interval.tv_nsec)) {
		struct trace_pkt *pkt;
		
		printf("Changing server trace %s from %ld.%09ld, %d to %ld.%09ld, %d\n",
		    server->name,
		    server->interval.tv_sec, server->interval.tv_nsec, 
		    server->interval_type,
		    interval.tv_sec, interval.tv_nsec, type);
		
		pkt = trace_packet_new();
		trace_packet_interval_set(pkt, 
		    &interval, type);
		trace_packet_write(server, pkt);
		trace_packet_put(pkt);
	}
}

static struct trace *server_find_client(struct trace *trace)
{
	int i, j;
	
	for (i = 0; i < nr_server_traces; i++) {
		struct trace *server;
		struct private *priv;
		
		server = server_traces[i];
		priv = server->private;
		
		for (j = 0; j < priv->nr_clients; j++) {
			if (priv->clients[j] == trace)
				return server;
		}
	}
	
	return NULL;
}

static void handler_interval(struct trace *trace, 
    struct timespec *interval, enum trace_interval_type type)
{
	base_interval = *interval;
	
	interval_pkt = trace_packet_new();
	trace_packet_interval_set(interval_pkt, 
	    interval, TRACE_INTERVAL_TYPE_INTERVAL);
}

static void client_interval(struct trace *trace, 
    struct timespec *interval, enum trace_interval_type type)
{
	struct trace_pkt *pkt;
	struct trace *server;

	printf("Client %s requests %ld.%09ld type %d\n",
	    trace->name,
	    interval->tv_sec, interval->tv_nsec, type);
	/* Send back interval to client (as ACK) */
	pkt = trace_packet_new();
	trace_packet_interval_set(pkt, 
	    interval, type);
	trace_packet_write(trace, pkt);
	trace_packet_put(pkt);

	server = server_find_client(trace);
	if (!server)
		return;
		
	server_interval_check(server);
}

static struct trace_pkt *list_pkt;

static void handler_list_entry(struct trace *trace, 
    char *name, enum trace_value_type type, char *unit)
{
	trace_packet_list_add(list_pkt, name, type, unit);

	trace_autorecover(trace, false);
}

static bool check_client(struct private *priv, int nr)
{
	if (trace_state_get(priv->clients[nr]) == TRACE_STATE_DISCONNECTED) {
		trace_free(priv->clients[nr]);
		memmove(&priv->clients[nr], &priv->clients[nr+1],
		    (priv->nr_clients-nr-1)*sizeof(struct trace *));
		priv->nr_clients--;
		printf("Trace lost a client, %d left\n",
		    priv->nr_clients);
		return true;
	}
	return false;
}


static bool timespec_mod(struct timespec *t, struct timespec *tdiv)
{
	/* TODO: make a proper mod taking both fields into account */
	if (tdiv->tv_sec) {
		return t->tv_sec % tdiv->tv_sec || t->tv_nsec;
	} else if (tdiv->tv_nsec) {
		return t->tv_nsec % tdiv->tv_nsec;
	}
	return false;
}

static void server_value(struct trace *trace,
    struct trace_value *value)
{
	struct trace_pkt *pkt;
	struct private *priv = trace->private;
	int i;
	
	if (priv->nr_clients) {
		bool changed;
		
		if (priv->lastvalue.value.u32 != value->value.u32)
			changed = true;
		else
			changed = false;
		priv->lastvalue.value.u32 = value->value.u32;
		
		pkt = trace_packet_new();
		trace_packet_value_add(pkt, value, trace->type);
	
		for (i = 0; i < priv->nr_clients; i++) {
			bool send = false;
			
			switch (priv->clients[i]->interval_type) {
				case TRACE_INTERVAL_TYPE_CHANGED:
					send = changed;
					break;
				case TRACE_INTERVAL_TYPE_INTERVAL:
					send =!timespec_mod(&value->t,
					    &priv->clients[i]->interval);
					break;
				default:
					send = true;
					break;
			}
				
			if (!send)
				continue;

			trace_packet_write(priv->clients[i], pkt);
			if (check_client(priv, i)) {
				server_interval_check(trace);
				i--;
				continue;
			}
		}
		trace_packet_put(pkt);
	}
}

static void server_timestamp(struct trace *trace, 
    struct timespec *timestamp)
{
	struct trace_pkt *pkt;
	struct private *priv = trace->private;
	int i;
	
	if (priv->nr_clients) {
		pkt = trace_packet_new();
		trace_packet_timestamp_set(pkt, timestamp);
	
		for (i = 0; i < priv->nr_clients; i++) {
			trace_packet_write(priv->clients[i], pkt);
			if (check_client(priv, i)) {
				server_interval_check(trace);
				i--;
				continue;
			}
		}
		trace_packet_put(pkt);
	}
	
	if (!priv->nr_clients && trace->fd >= 0 && priv->lastclient + TIMEOUT < time(NULL)) {
		printf("Timeout, closing trace\n");
		trace_close(trace);
		trace_autorecover(trace, false);
	}
}


static void server_type(struct trace *trace, enum trace_value_type type)
{
	struct trace_pkt *pkt;
	struct private *priv = trace->private;
	int i;
	
	pkt = trace_packet_new();
	trace_packet_type_set(pkt, type);
	
	for (i = 0; i < priv->nr_clients; i++) {
		trace_packet_write(priv->clients[i], pkt);
	}
	
	trace_packet_put(pkt);
}

static void client_accept(int fd_listen)
{
	int fd;
	struct trace *client;
	
	fd = tcp_accept(fd_listen);
	if (fd < 0)
		return;
	
	printf("New client\n");
	
	client = trace_alloc();
	if (!client) {
		close(fd);
		return;
	}
	
	trace_initialize_fd(client, fd);

	trace_packet_write(client, interval_pkt);
	trace_packet_write(client, list_pkt);
	client->handler_interval = client_interval;

	client_traces = realloc(client_traces, 
	    sizeof(struct trace *) * (nr_client_traces + 1));

	client_traces[nr_client_traces] = client;
	nr_client_traces++;
}

static void free_client(int i)
{
	memmove(&client_traces[i], &client_traces[i+1],
	    (nr_client_traces-i-1)*sizeof(struct trace *));
	nr_client_traces--;
}


static void free_server(int i)
{
	memmove(&server_traces[i], &server_traces[i+1],
	    (nr_server_traces-i-1)*sizeof(struct trace *));
	nr_server_traces--;
	
	printf("Freed server, %d left\n", nr_server_traces);
}

static struct trace *server_find(char *name)
{
	struct trace *trace;
	struct private *priv;
	int i;
	
	for (i = 0; i < nr_server_traces; i++) {
		if (!strcmp(server_traces[i]->name, name))
			break;
	}
	
	if (i == nr_server_traces) {
		struct trace_pkt *pkt;
		
		trace = trace_open(tr_host, tr_port);
		trace_name_set(trace, name);
		
		trace->interval_type = TRACE_INTERVAL_TYPE_INTERVAL;
		
		pkt = trace_packet_new();
		trace_packet_interval_set(pkt, 
		    &base_interval, trace->interval_type);
		trace_packet_write(trace, pkt);
		trace_packet_put(pkt);
		
		server_traces = realloc(server_traces, 
		    sizeof(struct trace *) * (nr_server_traces + 1));
		
		priv = calloc(sizeof(struct private), 1);
		trace->private = priv;
		
		server_traces[nr_server_traces] = trace;
		
		nr_server_traces++;		
		
		trace->handler_value = server_value;
		trace->handler_timestamp = server_timestamp;
		trace->handler_type = server_type;
		trace_autorecover(trace, true);
	}

	return server_traces[i];
}

static void client_add(struct trace *client)
{
	struct trace *trace;
	struct private *priv;
	
	printf("Going to add client to trace '%s'\n", client->name);
	
	trace = server_find(client->name);
	
	priv = trace->private;

	if (trace->type_set) {
		struct trace_pkt *pkt;
		
		pkt = trace_packet_new();
		trace_packet_type_set(pkt, trace->type);
		trace_packet_write(client, pkt);
		trace_packet_put(pkt);
	}
	if (trace->value_set) {
		struct trace_pkt *pkt;

		pkt = trace_packet_new();
		trace_packet_value_add(pkt, &trace->value, trace->type);
		trace_packet_write(client, pkt);
		trace_packet_put(pkt);
	}
	
	priv->clients = realloc(priv->clients, 
	    sizeof(struct trace *) * (priv->nr_clients + 1));
	priv->clients[priv->nr_clients] = client;
	priv->nr_clients++;

	server_interval_check(trace);

	printf("Trace now has %d clients\n", priv->nr_clients);
}

struct valclient {
	int fd;
	
	size_t len;
	char request[1024];
};

int nr_valclients;
struct valclient **valclients;

static void free_val(int i)
{
	free(valclients[i]);

	memmove(&valclients[i], &valclients[i+1],
	    (nr_valclients-i-1)*sizeof(struct valclient *));
	nr_valclients--;
}

static void val_accept(fd_listenval)
{
	int fd;
	struct valclient *valclient;
	
	fd = tcp_accept(fd_listenval);
	if (fd < 0)
		return;
	
	ioctl(fd, FIONBIO, &(int){ 1 });
	
	valclient = calloc(1, sizeof(struct valclient));
	
	valclient->fd = fd;
	valclient->len = 0;
	
	valclients = realloc(valclients, 
	    sizeof(struct valclient*)*(nr_valclients + 1));

	valclients[nr_valclients] = valclient;
	nr_valclients++;
}

static void val_handle(struct valclient *valclient)
{
	ssize_t r;
	int i;
	char reply[1024];
	
	r = read(valclient->fd, &valclient->request[valclient->len],
	    1024 - valclient->len);
	
	if ((r < 0 && errno != EAGAIN) || r == 0) {
		close(valclient->fd);
		valclient->fd = -1;
		return;
	}
	valclient->len += r;
	
	for (i = 0; i < valclient->len - 1; i++) {
		if (!valclient->request[i] && !valclient->request[i+1]) {
			char *name;
			char *replyp;
			time_t now = time(NULL);

			replyp = reply;
			
			for (name = valclient->request; name[0]; 
			    name += strlen(name) + 1) {
				struct trace *trace;
				struct private *priv;
				bool int_check;

				trace = server_find(name);
				priv = trace->private;
				
				int_check = priv->lastclient + TIMEOUT < now;
				
				priv->lastclient = now;

				if (int_check);
					server_interval_check(trace);

				switch(trace->type) {
					case TRACE_VALUE_TYPE_FLOAT:
						replyp += sprintf(replyp, "%g ",
						    trace->value.value.f);
						break;
					case TRACE_VALUE_TYPE_BOOL:
						replyp += sprintf(replyp, "%d ",
						    trace->value.value.b);
						break;
					case TRACE_VALUE_TYPE_UINT8:
						replyp += sprintf(replyp, "%d ",
						    trace->value.value.u8);
						break;
					case TRACE_VALUE_TYPE_UINT16:
						replyp += sprintf(replyp, "%d ",
						    trace->value.value.u16);
						break;
					case TRACE_VALUE_TYPE_UINT32:
						replyp += sprintf(replyp, "%d ",
						    trace->value.value.u32);
						break;
					case TRACE_VALUE_TYPE_SINT8:
						replyp += sprintf(replyp, "%d ",
						    trace->value.value.s8);
						break;
					case TRACE_VALUE_TYPE_SINT16:
						replyp += sprintf(replyp, "%d ",
						    trace->value.value.s16);
						break;
					case TRACE_VALUE_TYPE_SINT32:
						replyp += sprintf(replyp, "%d ",
						    trace->value.value.s32);
						break;
				}
			}
			sprintf(replyp, "\n");
			write(valclient->fd, reply, strlen(reply));
			close(valclient->fd);
			valclient->fd = -1;
		}
	}
}

int main (int argc, char **argv)
{
	int fd_listen, fd_listenval;
	fd_set fd_rd;
	struct trace *list;
	int high, i, j;
	
	fd_listen = tcp_listen(CONSOLE_TRACE_PORT, 0, 100);
	if (fd_listen < 0) {
		printf("Could not open listen port %d\n", CONSOLE_TRACE_PORT);
		return 1;
	}

	fd_listenval = tcp_listen(CONSOLE_TRACE_VAL_PORT, 0, 100);
	if (fd_listenval < 0) {
		printf("Could not open listen port %d\n", CONSOLE_TRACE_VAL_PORT);
		return 1;
	}
	
	signal(SIGPIPE, SIG_IGN);

        /* determine socket on dt_ctrl */
	tr_host = dt_host_controller();
	tr_port = CTRL_TRACE_PORT;

	/* Get list of all traces */
	printf("Going to retrieve list of traces\n");

	list = trace_open(tr_host, tr_port);
	list->handler_interval = handler_interval;
	list->handler_list_entry = handler_list_entry;
	trace_autorecover(list, true);

	list_pkt = trace_packet_new();

	while (trace_state_get(list) != TRACE_STATE_READY) {
		struct timeval timeout;
		high = 0;

		timeout.tv_sec = 1;
		timeout.tv_usec = 10;
		
		FD_ZERO(&fd_rd);
		trace_fd_set(list, &fd_rd, &high);	
		select(high + 1, &fd_rd, NULL, NULL, &timeout);
		trace_handle(list, &fd_rd);
	
		/* Prevent cpu hogging */
		if (trace_state_get(list) != TRACE_STATE_READY)
			sleep(1);
		printf("state: %d\n", trace_state_get(list));
	}
	trace_close(list);
	trace_free(list);

	printf("Recieved list from server, now waiting for clients\n");

	while (1) {
		struct timeval timeout;
		
		FD_ZERO(&fd_rd);
		
		timeout.tv_sec = 10;
		timeout.tv_usec = 0;
		
		high = fd_listen;
		FD_SET(fd_listen, &fd_rd);
		FD_SET(fd_listenval, &fd_rd);
		if (fd_listenval > high)
			high = fd_listenval;

		for (i = 0; i < nr_client_traces; i++) {
			trace_fd_set(client_traces[i], &fd_rd, &high);
		}
		for (i = 0; i < nr_server_traces; i++) {
			struct private *priv = server_traces[i]->private;
			
			trace_fd_set(server_traces[i], &fd_rd, &high);
			for (j = 0; j < priv->nr_clients; j++) {
				trace_fd_set(priv->clients[j], &fd_rd, &high);
			}
		}
		for (i = 0; i < nr_valclients; i++) {
			FD_SET(valclients[i]->fd, &fd_rd);
			if (valclients[i]->fd > high)
				high = valclients[i]->fd;
		}
		
		select(high + 1, &fd_rd, NULL, NULL, &timeout);
		
		if (FD_ISSET(fd_listen, &fd_rd)) {
			client_accept(fd_listen);
		}
		if (FD_ISSET(fd_listenval, &fd_rd)) {
			val_accept(fd_listenval);
		}
		for (i = 0; i < nr_valclients; i++) {
			if (FD_ISSET(valclients[i]->fd, &fd_rd)) {
				val_handle(valclients[i]);
			}
			if (valclients[i]->fd < 0) {
				free_val(i);
				i--;
			}
		}
		
		for (i = 0; i < nr_client_traces; i++) {
			if(trace_handle(client_traces[i], &fd_rd))
				continue;
			
			if (client_traces[i]->name_set) {
				client_add(client_traces[i]);
				free_client(i);
				i--;
				continue;
			}
			
			if (trace_state_get(client_traces[i]) ==
			    TRACE_STATE_DISCONNECTED) {
				trace_free(client_traces[i]);
				free_client(i);
				printf("Freeing client %d\n", i);
				i--;
				continue;
			}
		}
		for (i = 0; i < nr_server_traces; i++) {
			struct private *priv = server_traces[i]->private;
			
			if (trace_handle(server_traces[i], &fd_rd))
				continue;
			for (j = 0; j < priv->nr_clients; j++) {
				trace_handle(priv->clients[j], &fd_rd);
			}

			if (trace_state_get(server_traces[i]) ==
			    TRACE_STATE_DISCONNECTED &&
			    !server_traces[i]->recover) {
			    	free(server_traces[i]->private);
				trace_free(server_traces[i]);
				free_server(i);
				printf("Freeing server trace %d\n", i);
				i--;
				continue;
			}
		}
	}

	return 0;
}