controller_trace.c

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

	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 <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <pthread.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <signal.h>
#include <errno.h>
#include <limits.h>

#include <controller/controller_block.h>
#include <controller/controller_trace.h>
#include <controller/controller_time.h>
#include <controller/controller_mem.h>
#include <log/log.h>
#include <trace/trace.h>

static int nr_traces = 0;
static struct controller_trace_name *trace_list = NULL;
long interval_nsec;

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

struct controller_trace_name *controller_trace_get(int nr)
{
	return &trace_list[nr];
}

int controller_trace_nr(void)
{
	return nr_traces;
}

void controller_trace_list_add(struct controller_trace_name *trace)
{
	struct controller_trace_name *tmp;

	nr_traces++;
	tmp = realloc(trace_list, sizeof(struct controller_trace) * nr_traces);
	if (tmp) {
		trace_list = tmp;
		memcpy(&trace_list[nr_traces-1], trace,
		    sizeof(struct controller_trace));
		log_send(LOG_T_DEBUG, "Added trace %s [%s] using %s.%s",
		    trace->name, trace->unit, 
		    trace->blockname, trace->termname);
	} else
		nr_traces--;
}

#define TRACE_LEN	1024

struct trace_hdl {
	int free;

	struct trace trace;
	bool added;
	enum trace_interval_type type;
	long nsec;

	char tracename[100];
	struct controller_trace ctrace;
	union controller_trace_value last;
	union controller_trace_value buffer[TRACE_LEN];
	bool valuesend;
};

static int nr_trace_hdl;
static struct trace_hdl *trace_hdl;

static int controller_trace_server_port;

/* Thes are created when starting the server and sent to each new client */
struct trace_pkt *controller_trace_pkt_caps;
struct trace_pkt *controller_trace_pkt_interval;
struct trace_pkt *controller_trace_pkt_list;


static enum trace_value_type termtype2tracetype(enum controller_block_term_type type)
{
	switch (type) {
		case CONTROLLER_BLOCK_TERM_FLOAT:
			return TRACE_VALUE_TYPE_FLOAT;
		case CONTROLLER_BLOCK_TERM_BOOL:
			return TRACE_VALUE_TYPE_BOOL;
		case CONTROLLER_BLOCK_TERM_UINT8:
			return TRACE_VALUE_TYPE_UINT8;
		case CONTROLLER_BLOCK_TERM_UINT16:
			return TRACE_VALUE_TYPE_UINT16;
		case CONTROLLER_BLOCK_TERM_UINT32:
			return TRACE_VALUE_TYPE_UINT32;
		case CONTROLLER_BLOCK_TERM_SINT8:
			return TRACE_VALUE_TYPE_SINT8;
		case CONTROLLER_BLOCK_TERM_SINT16:
			return TRACE_VALUE_TYPE_SINT16;
		case CONTROLLER_BLOCK_TERM_SINT32:
			return TRACE_VALUE_TYPE_SINT32;
		default:
			break;
	}
	return -1;
}

static void *controller_trace_handle(void *arg)
{
	int i;
	fd_set fdrx;
	int high;
	struct timeval tv;
	long usec = controller_time_sample_period_get() * 1000000;

	while (1) {
		FD_ZERO(&fdrx);
		high = 0;
		
		for (i = 0; i < nr_trace_hdl; i++) {
			if (trace_hdl[i].free)
				continue;
			
			trace_fd_set(&trace_hdl[i].trace, &fdrx, &high);
		}
		
		tv.tv_sec = 0;
		tv.tv_usec = usec;
	
		select(high + 1, &fdrx, NULL, NULL, &tv);
		
		for (i = 0; i < nr_trace_hdl; i++) {
			if (trace_hdl[i].free)
				continue;
			
			trace_handle(&trace_hdl[i].trace, &fdrx);
		}

		for (i = 0; i < nr_trace_hdl; i++) if (trace_hdl[i].trace.name_set) {
			size_t wr_pos, rd_pos;
			size_t len;
			int j;
		
			wr_pos = trace_hdl[i].ctrace.wr_pos;
			rd_pos = trace_hdl[i].ctrace.rd_pos;
			__sync_synchronize();

			if (wr_pos != rd_pos) {
				struct trace_pkt *pkt;
				
				if (rd_pos >= wr_pos)
					len = trace_hdl[i].ctrace.len - rd_pos;
				else {
					len = wr_pos - rd_pos;
				}
				
				pkt = trace_packet_new();
	
				for (j = 0; j < len; j++) {
					size_t pos = rd_pos+j;
					bool sendtime = false;
					bool sendval = false;
					
					if (trace_hdl[i].nsec < 0) {
						sendval = true;
						trace_hdl[i].nsec = 0;
					}
					if (pos == trace_hdl[i].ctrace.timestamp_pos) {
						trace_hdl[i].nsec = 0;
						sendtime = true;
					}
					
					if (trace_hdl[i].type == TRACE_INTERVAL_TYPE_INTERVAL) {
						struct timespec t;

						t.tv_sec = trace_hdl[i].ctrace.timestamp;
						t.tv_nsec = trace_hdl[i].nsec;
						
						sendval = !timespec_mod(&t, &trace_hdl[i].trace.interval);
						/* No need to send time every second if values are send
						   less than once a second */
						if (&trace_hdl[i].trace.interval.tv_sec)
							sendtime = sendval;
					} else if (trace_hdl[i].type == TRACE_INTERVAL_TYPE_CHANGED) {
						uint32_t bufval = 0;
						uint32_t last = 0;
						
						switch (trace_hdl[i].trace.type) {
							case TRACE_VALUE_TYPE_BOOL:
							case TRACE_VALUE_TYPE_SINT8:
							case TRACE_VALUE_TYPE_UINT8:
								bufval = trace_hdl[i].ctrace.buffer[rd_pos+j].u8;
								last = trace_hdl[i].last.u8;
								trace_hdl[i].last.u8 = bufval;
								break;
							case TRACE_VALUE_TYPE_UINT16:
							case TRACE_VALUE_TYPE_SINT16:
								bufval = trace_hdl[i].ctrace.buffer[rd_pos+j].u16;
								last = trace_hdl[i].last.u16;
								trace_hdl[i].last.u16 = bufval;
								break;
							case TRACE_VALUE_TYPE_UINT32:
							case TRACE_VALUE_TYPE_SINT32:
							case TRACE_VALUE_TYPE_FLOAT:
								bufval = trace_hdl[i].ctrace.buffer[rd_pos+j].u32;
								last = trace_hdl[i].last.u32;
								trace_hdl[i].last.u32 = bufval;
								break;
						}
						if (bufval != last || !trace_hdl[i].valuesend) {
							sendtime = true;
							sendval = true;
							trace_hdl[i].valuesend = true;
						}
					}

					if (sendtime) {
						struct timespec t;
						
						if (pkt->len) {
							trace_packet_write(&trace_hdl[i].trace, pkt);
							trace_packet_put(pkt);
						
							pkt = trace_packet_new();
						}

						t.tv_sec = trace_hdl[i].ctrace.timestamp;
						t.tv_nsec = trace_hdl[i].nsec;
						trace_packet_timestamp_set(
							pkt, &t);
						trace_packet_write(&trace_hdl[i].trace, pkt);
						trace_packet_put(pkt);

						pkt = trace_packet_new();
					}
				
					if (sendval) {
						trace_packet_value_add(pkt,
						    (void*)&trace_hdl[i].ctrace.buffer[rd_pos+j],
						    trace_hdl[i].trace.type);
					}
					trace_hdl[i].nsec += interval_nsec;
				}

				trace_packet_write(&trace_hdl[i].trace, pkt);
				trace_packet_put(pkt);
			
				trace_hdl[i].ctrace.rd_pos += len;
				if (trace_hdl[i].ctrace.rd_pos >= trace_hdl[i].ctrace.len)
					trace_hdl[i].ctrace.rd_pos %= trace_hdl[i].ctrace.len;
			}
		}
	}

	return NULL;
}

static void handler_close(struct trace *trace)
{
	struct trace_hdl *hdl = trace->private;
	
	if (hdl->added)
		controller_block_trace_del(&hdl->ctrace);

	log_send(LOG_T_DEBUG, "Trace connection closed");

	hdl->added = false;
	hdl->free = 1;
}

static void handler_name(struct trace *trace, char *name)
{
	struct trace_hdl *hdl = trace->private;
	int j;
	char *blockname;
	char *termname;
	bool found = false;
	struct trace_pkt *pkt;

	strcpy(hdl->tracename, name);

	log_send(LOG_T_DEBUG, "Starting trace for %s", name);

	/* Check the list of defined traces */
	for (j = 0; j < nr_traces; j++) {
		if (!strcmp(hdl->tracename,
		    trace_list[j].name)) {
			blockname = trace_list[j].blockname;
			termname = trace_list[j].termname;
			found = true;
			break;
		}
	}
	/* If not found, check if it is a "block.term" format */
	if (!found) {
		blockname = hdl->tracename;
		termname = hdl->tracename;
		while (termname[0] && termname[0] != '.')
			termname++;
			if (termname[0] == '.') {
				termname[0] = 0;
				termname++;
		}
	}

	hdl->ctrace.len = TRACE_LEN;
	hdl->ctrace.buffer = hdl->buffer;

	memset(hdl->buffer, 0, sizeof(union controller_trace_value) * TRACE_LEN);

	if (controller_block_trace_add(
	    blockname,
	    termname,
	    &hdl->ctrace)) {
		trace_close(&hdl->trace);
		hdl->free = 1;
		log_send(LOG_T_DEBUG, "Could not add trace");
		return;
	}
	hdl->added = true;

	pkt = trace_packet_new();
	
	hdl->trace.type = termtype2tracetype(hdl->ctrace.type);
	trace_packet_type_set(pkt, termtype2tracetype(hdl->ctrace.type));
	trace_packet_write(trace, pkt);
	trace_packet_put(pkt);
}

static void handler_interval(struct trace *trace, 
    struct timespec *interval, enum trace_interval_type type)
{
	struct trace_hdl *hdl = trace->private;
	struct trace_pkt *pkt;
	
	log_send(LOG_T_DEBUG, "Setting interval %ld.%09ld type %d", 
	    interval->tv_sec, interval->tv_nsec, type);

	hdl->type = 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);

}


static void *controller_trace_server(void *arg)
{
	int listen_sock;
	struct sockaddr_in sockaddr;
	int val;
	
	signal(SIGPIPE, SIG_IGN);
	
	listen_sock = socket(AF_INET, SOCK_STREAM, 0);
	if (listen_sock < 0) {
		perror("socket() failed");
		return NULL;
	}
	
	sockaddr.sin_family = AF_INET;
	sockaddr.sin_addr.s_addr = htonl(INADDR_ANY);
	sockaddr.sin_port = htons(controller_trace_server_port);
	
	val = 1;
	setsockopt(listen_sock, SOL_SOCKET, SO_REUSEADDR, &val, sizeof(int));

	if (bind(listen_sock,
	    (struct sockaddr *)&sockaddr, sizeof(sockaddr)) < 0) {
		perror("bind() failed");
		close(listen_sock);
		return NULL;
	}
	
	if (listen(listen_sock, 8) < 0) {
		perror("listen() failed");
		close(listen_sock);
		return NULL;
	}
	
	while (1) {
		int client;
		struct sockaddr_in sockaddr_client;
		socklen_t len;
		
		len = sizeof(sockaddr_client);
		client = accept(listen_sock,
		    (struct sockaddr *)&sockaddr_client, &len);
		
		if (client >= 0) {
			int i;
			
			for (i = 0; i < nr_trace_hdl; i++) {
				if (!trace_hdl[i].free)
					continue;
				
				trace_initialize_fd(&trace_hdl[i].trace, client);

				/* Send info to client */
				trace_packet_write(&trace_hdl[i].trace,
				    controller_trace_pkt_caps);
				trace_packet_write(&trace_hdl[i].trace,
				    controller_trace_pkt_interval);
				trace_packet_write(&trace_hdl[i].trace,
				    controller_trace_pkt_list);
			
				trace_hdl[i].trace.name_set = false;
				trace_hdl[i].added = false;
				trace_hdl[i].type = TRACE_INTERVAL_TYPE_INTERVAL;
				trace_hdl[i].nsec = -1;
				trace_hdl[i].valuesend = false;
				
				trace_hdl[i].free = 0;
				break;
			}
			if (i == nr_trace_hdl) {
				log_send(LOG_T_ERROR, 
				    "No free trace handlers, closing trace connection");
				close(client);
			}
		} else {
			log_send(LOG_T_ERROR, "accept() failed: %s",
			    strerror(errno));
		}
	}

	return NULL;
}

void controller_trace_server_start(int portnr, int max)
{
	pthread_t thread_id;
	pthread_attr_t attr;
	int i, j;
	double period;
	struct timespec interval;
	
	log_send(LOG_T_DEBUG, "Starting trace server on port %d, max %d clients",
	    portnr, max);

	trace_hdl = controller_mem_calloc(CONTROLLER_MEM_PERIODIC_WRITE,
	     max, sizeof(struct trace_hdl));
	if (!trace_hdl)
		return;

	nr_trace_hdl = max;
	for (i = 0; i < max; i++) {
		trace_hdl[i].free = 1;
		trace_initialize(&trace_hdl[i].trace, 8192);

		trace_hdl[i].trace.private = &trace_hdl[i];
		trace_hdl[i].trace.handler_close = handler_close;
		trace_hdl[i].trace.handler_name = handler_name;
		trace_hdl[i].trace.handler_interval = handler_interval;
	}
	
	controller_trace_pkt_caps = trace_packet_new();
	trace_packet_capabilities_set(controller_trace_pkt_caps, 0);
	
	controller_trace_pkt_interval = trace_packet_new();
	period = controller_time_sample_period_get();
	interval.tv_sec = period;
	period -= interval.tv_sec;
	period *= 1000000000.0;
	period += 0.5;
	interval.tv_nsec = period;
	interval_nsec = period;
	trace_packet_interval_set(controller_trace_pkt_interval,
	    &interval, TRACE_INTERVAL_TYPE_INTERVAL);
	
	controller_trace_pkt_list = trace_packet_new();
	for (i = 0; i < nr_traces; i++) {
		struct controller_block *block;
		struct controller_block_outterm *outterm;
	
		block = controller_block_find(trace_list[i].blockname);
		if (!block)
			continue;
		outterm = controller_block_find_outterm(block, trace_list[i].termname);
		if (!outterm)
			continue;

		trace_packet_list_add(controller_trace_pkt_list,
		    trace_list[i].name,
		    termtype2tracetype(outterm->type),
		    trace_list[i].unit);
	}
	for (i = 0; i < controller_block_nr(); i++) {
		struct controller_block *block = controller_block_get(i);
		for (j = 0; j < block->outputs; j++) {
			char tracename[strlen(block->name) + 1 + strlen(block->output[j].name) + 1];
			
			sprintf(tracename, "%s.%s", block->name, block->output[j].name);
			
			trace_packet_list_add(controller_trace_pkt_list,
			    tracename,
			    termtype2tracetype(block->output[j].type),
			    "");
		}
	}
	
	controller_block_trace_init(max);

	controller_trace_server_port = portnr;

	pthread_attr_init(&attr);
#ifndef __FreeBSD__
	pthread_attr_setstacksize(&attr, PTHREAD_STACK_MIN * 2);
#endif
	pthread_create(&thread_id, &attr,
	    controller_trace_server, NULL);
	pthread_create(&thread_id, &attr,
	    controller_trace_handle, NULL);
}