trace.c

/*
	Trace support

	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2011
	Copyright Stichting C.A. Muller Radioastronomiestation, 2011

	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 <trace/trace.h>
#include <trace/trace_def.h>

#include <log/log.h>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <inttypes.h>
#include <sys/ioctl.h>
#include <pthread.h>


static bool timespec_older(struct timespec *t1, struct timespec *t2)
{
	if (t1->tv_sec < t2->tv_sec)
		return true;
	if (t1->tv_sec > t2->tv_sec)
		return false;
	if (t1->tv_nsec < t2->tv_nsec)
		return true;
	return false;
}

static void timespec_normalize(struct timespec *t)
{
	long newsec = t->tv_nsec / 1000000000;
	
	t->tv_sec += newsec;
	t->tv_nsec -= newsec * 1000000000;
}

static void timespec_add(struct timespec *dst, struct timespec *add)
{
	dst->tv_sec += add->tv_sec;
	dst->tv_nsec += add->tv_nsec;
	timespec_normalize(dst);
}

void trace_initialize(struct trace *trace, size_t buffer)
{
	memset(trace, 0, sizeof(struct trace));
	trace->rx_buffer = malloc(buffer);
	trace->rx_buf_size = buffer;
	trace->fd = -1;
}

int trace_interval_set(struct trace *trace, struct timespec *interval, enum trace_interval_type type)
{
	struct trace_pkt *pkt;

	trace->interval_req.tv_sec = interval->tv_sec;
	trace->interval_req.tv_nsec = interval->tv_nsec;
	trace->interval_type_req = type;
	trace->interval_set = true;
	
	pkt = trace_packet_new();
	trace_packet_interval_set(pkt, interval, type);
	trace_packet_write(trace, pkt);
	trace_packet_put(pkt);
	
	return 0;
}

int trace_name_set(struct trace *trace, char *name)
{
	struct trace_pkt *pkt;

	if (trace->name)
		free(trace->name);
	trace->name = strdup(name);
	
	pkt = trace_packet_new();
	trace_packet_name_set(pkt, name);
	trace_packet_write(trace, pkt);
	trace_packet_put(pkt);
	
	return 0;
}

void trace_autorecover(struct trace *trace, bool value)
{
	trace->recover = value;
}


void trace_packet_initialize(struct trace_pkt *pkt)
{
	memset(pkt, 0, sizeof(struct trace_pkt));
}



#define PACKET_BUFLEN 256
#define PACKET_POOL_CHUNK 16

static struct trace_pkt **packet_pool;
static int packet_pool_size = 0;
static int packet_pool_cur = 0;

static pthread_mutex_t trace_pkt_mutex = PTHREAD_MUTEX_INITIALIZER;

struct trace_pkt *trace_packet_new(void)
{
	struct trace_pkt *pkt = NULL;
	
	pthread_mutex_lock(&trace_pkt_mutex);
	if (packet_pool_cur) {
		pkt = packet_pool[packet_pool_cur-1];
		packet_pool_cur--;
	}
	pthread_mutex_unlock(&trace_pkt_mutex);
	
	if (!pkt) {
		pkt = calloc(1, sizeof(struct trace_pkt));
		pkt->data = malloc(PACKET_BUFLEN);
		pkt->buflen = PACKET_BUFLEN;
	}
	
	return pkt;
}

int trace_packet_resize(struct trace_pkt *pkt, size_t len)
{
	if (len <= pkt->buflen)
		return 0;
	
	pkt->data = realloc(pkt->data, len);
	pkt->buflen = len;

	return 0;
}

void trace_packet_put(struct trace_pkt *pkt)
{
	if (pkt->buflen > PACKET_BUFLEN) {
		free(pkt->data);
		free(pkt);
		return;
	}
	
	pthread_mutex_lock(&trace_pkt_mutex);

	if (packet_pool_cur >= packet_pool_size) {
		packet_pool_size += PACKET_POOL_CHUNK;
		packet_pool = realloc(packet_pool,
		    sizeof(struct trace_pkt *) * packet_pool_size);
	}
	packet_pool[packet_pool_cur] = pkt;
	pkt->len = 0;
	packet_pool_cur++;

	pthread_mutex_unlock(&trace_pkt_mutex);
}

int trace_packet_list_add(struct trace_pkt *pkt, 
    char *name, enum trace_value_type type, char *unit)
{
	size_t entry_size = strlen(name)+1 + 1 + strlen(unit)+1;
	unsigned char *entry;

	if (!pkt->len) {
		trace_packet_resize(pkt, 1);
		pkt->len = 1;
		pkt->data[0] = TRACE_PTYPE_LIST;
	}
	
	trace_packet_resize(pkt, pkt->len + entry_size);
	entry = pkt->data + pkt->len;

	strcpy((char *)entry, name);
	entry += strlen(name) + 1;

	*entry = type;
	entry++;

	strcpy((char *)entry, unit);
	
	pkt->len += entry_size;
		
	return 0;
}

int trace_packet_value_add(struct trace_pkt *pkt, struct trace_value *value, enum trace_value_type type)
{
	size_t entry_size;
	struct {
		uint16_t u16;
	} __packed *entry16;
	struct {
		uint32_t u32;
	} __packed *entry32;
	unsigned char *entry;

	switch (type) {
		case TRACE_VALUE_TYPE_BOOL:
		case TRACE_VALUE_TYPE_UINT8:
		case TRACE_VALUE_TYPE_SINT8:
			entry_size = 1;
			break;
		case TRACE_VALUE_TYPE_SINT16:
		case TRACE_VALUE_TYPE_UINT16:
			entry_size = 2;
			break;
		case TRACE_VALUE_TYPE_UINT32:
		case TRACE_VALUE_TYPE_SINT32:
		case TRACE_VALUE_TYPE_FLOAT:
		default:
			entry_size = 4;
			break;
	}

	if (!pkt->len) {
		trace_packet_resize(pkt, 1);
		pkt->len = 1;
		pkt->data[0] = TRACE_PTYPE_DATA;
	}
	
	trace_packet_resize(pkt, pkt->len + entry_size);
	entry = pkt->data + pkt->len;

	switch (type) {
		case TRACE_VALUE_TYPE_BOOL:
		case TRACE_VALUE_TYPE_UINT8:
		case TRACE_VALUE_TYPE_SINT8:
			*entry = value->value.u8;
			break;
		case TRACE_VALUE_TYPE_SINT16:
		case TRACE_VALUE_TYPE_UINT16: {
			entry16 = (void *)entry;
			
			entry16->u16 = htobe16(value->value.u16);
			
			break;
		}
		case TRACE_VALUE_TYPE_UINT32:
		case TRACE_VALUE_TYPE_SINT32:
		case TRACE_VALUE_TYPE_FLOAT:
		default: {
			entry32 = (void *)entry;
			
			entry32->u32 = htobe32(value->value.u32);
			
			break;
		}
	}
	
	
	pkt->len += entry_size;
		
	return 0;
}

int trace_packet_interval_set(struct trace_pkt *pkt, 
    struct timespec *interval, enum trace_interval_type type)
{
	struct trace_ptype_interval *pinterval;
	
	if (pkt->len < sizeof(struct trace_ptype_interval) + sizeof(struct trace_header)) {
		pkt->len = sizeof(struct trace_ptype_interval) + sizeof(struct trace_header);
		trace_packet_resize(pkt, pkt->len);
		pkt->data[0] = TRACE_PTYPE_INTERVAL;
	}
	pinterval = (struct trace_ptype_interval *)&pkt->data[1];
	
	pinterval->sec = htobe64(interval->tv_sec);
	pinterval->nsec = htobe32(interval->tv_nsec);
	pinterval->type = type;
	
	return 0;
}

int trace_packet_timestamp_set(struct trace_pkt *pkt, 
    struct timespec *timestamp)
{
	struct trace_ptype_timestamp *ptimestamp;
	
	if (pkt->len < sizeof(struct trace_ptype_timestamp) + sizeof(struct trace_header)) {
		pkt->len = sizeof(struct trace_ptype_timestamp) + sizeof(struct trace_header);
		trace_packet_resize(pkt, pkt->len);
		pkt->data[0] = TRACE_PTYPE_TIMESTAMP;
	}
	ptimestamp = (struct trace_ptype_timestamp *)&pkt->data[1];
	
	ptimestamp->sec = htobe64(timestamp->tv_sec);
	ptimestamp->nsec = htobe32(timestamp->tv_nsec);
	
	return 0;
}

int trace_packet_name_set(struct trace_pkt *pkt, char *name)
{
	if (pkt->len < sizeof(struct trace_header) + strlen(name) + 1) {
		pkt->len = strlen(name) + 1 + sizeof(struct trace_header);
		trace_packet_resize(pkt, pkt->len);
		pkt->data[0] = TRACE_PTYPE_NAME;
	}
	strcpy((char *)pkt->data + 1, name);
	
	return 0;
}

int trace_packet_type_set(struct trace_pkt *pkt, enum trace_value_type type)
{
	struct trace_ptype_value_type *vt;

	if (pkt->len < sizeof(struct trace_header) + sizeof(struct trace_ptype_value_type)) {
		pkt->len = sizeof(struct trace_header) + sizeof(struct trace_ptype_value_type);
		trace_packet_resize(pkt, pkt->len);
		pkt->data[0] = TRACE_PTYPE_VALUE_TYPE;
	}
	vt = (void *)pkt->data + sizeof(struct trace_header);
	
	vt->type = type;
		
	return 0;
}

bool trace_check(struct trace *trace)
{
	if (trace->fd < 0) {
		if (!trace->recover)
			return false;

		if (trace->recovertime >= time(NULL))
			return false;
		trace->recovertime = time(NULL);

		log_send(LOG_T_DEBUG, "Attempt to recover trace %s", 
		    trace->name);
		trace_connect(trace);
		if (trace->fd >= 0) {
			struct trace_pkt *pkt;
		
			if (trace->name) {
				pkt = trace_packet_new();
				trace_packet_name_set(pkt, trace->name);
				trace_packet_write(trace, pkt);
				trace_packet_put(pkt);
			}
			if (trace->interval_set) {
				pkt = trace_packet_new();
				trace_packet_interval_set(pkt, 
				    &trace->interval_req, 
				    trace->interval_type_req);
				trace_packet_write(trace, pkt);
				trace_packet_put(pkt);
			}
		}
		if (trace->fd < 0) {
			log_send(LOG_T_DEBUG, "Failed to recover");
			return false;
		}
		log_send(LOG_T_DEBUG, "Reconnected trace");
	}
	
	return true;
}

bool trace_fd_set(struct trace *trace, fd_set *set, int *high)
{
	if (!trace_check(trace))
		return false;

	FD_SET(trace->fd, set);

	if (trace->fd > *high)
		*high = trace->fd;
	
	return true;
}

int trace_handle_recv(struct trace *trace)
{
	while (trace_fd_read(trace)) {
		struct trace_pkt *pkt;
		
		pkt = trace_packet_get(trace);
		if (!pkt)
			break;
		switch (pkt->data[0]) {
			case TRACE_PTYPE_INTERVAL: {
				struct trace_ptype_interval *pinterval;
				enum trace_interval_type type;
				
				pinterval = (void*)pkt->data + 1;
				
				trace->interval.tv_sec = be64toh(pinterval->sec);
				trace->interval.tv_nsec = be32toh(pinterval->nsec);
				type = pinterval->type;
				trace->interval_type = type;
				
				if (trace->handler_interval) {
					trace->handler_interval(trace, &trace->interval, type);
				}
				break;
			}
			case TRACE_PTYPE_TIMESTAMP: {
				struct trace_ptype_timestamp *ptimestamp;
									
				ptimestamp = (void*)pkt->data + 1;
				
				trace->timestamp.tv_sec = be64toh(ptimestamp->sec);
				trace->timestamp.tv_nsec = be32toh(ptimestamp->nsec);
				
				if (trace->handler_timestamp) {
					trace->handler_timestamp(trace, &trace->timestamp);
				}
				break;
			}
			case TRACE_PTYPE_LIST: {
				unsigned char *listentry;
				size_t pos = 1;
				
				while (pos < pkt->len) {
					char *name;
					enum trace_value_type type;
					char *unit;
					
					listentry = pkt->data + pos;
					
					name = (char *)listentry;
					type = listentry[strlen(name) + 1];
					unit = (char *)listentry+strlen(name) + 2;
					
					if (trace->handler_list_entry) {
						trace->handler_list_entry(trace,
						    name, type, unit);
					}
					pos += strlen(name) + strlen(unit) + 3;
				}
				trace->list_received = true;
				break;
			}
			case TRACE_PTYPE_VALUE_TYPE: {
				struct trace_ptype_value_type *vt;
				enum trace_value_type type;
				
				vt = (void*)pkt->data + 1;
				type = vt->type;
				
				trace->type = type;
				trace->type_set = type;
				if (trace->handler_type) {
					trace->handler_type(trace, type);
				}
				break;
			}
			case TRACE_PTYPE_NAME: {
				char *name;
				
				name = (void*)pkt->data + 1;
				
				trace->name_set = true;
				trace->name = strdup(name);

				if (trace->handler_name) {
					trace->handler_name(trace, name);
				}
				break;
			}
			case TRACE_PTYPE_DATA: {
				size_t pos = 1;
				struct trace_ptype_value *v;
				
				while (pos < pkt->len) {
					v = (void *)pkt->data + pos;
					switch (trace->type) {
						case TRACE_VALUE_TYPE_FLOAT:
						case TRACE_VALUE_TYPE_UINT32:
						case TRACE_VALUE_TYPE_SINT32:
							trace->value.value.u32 = be32toh(v->u.u32);
							pos+=4;
							break;
						case TRACE_VALUE_TYPE_UINT16:
						case TRACE_VALUE_TYPE_SINT16:
							trace->value.value.u16 = be32toh(v->u.u16);
							pos+=2;
							break;
						case TRACE_VALUE_TYPE_BOOL:
						case TRACE_VALUE_TYPE_UINT8:
						case TRACE_VALUE_TYPE_SINT8:
							trace->value.value.u8 = v->u.u8;
							pos++;
							break;
					}

					if (timespec_older(&trace->value.t, &trace->timestamp)) {
						trace->value.t = trace->timestamp;
					} else {
						timespec_add(&trace->value.t, &trace->interval);
					}
					trace->value_set = true;
					if (trace->handler_value) {
						trace->handler_value(trace, &trace->value);
					}
				}
				break;
			}
		}
		trace_packet_put(pkt);
	}
	return 0;
}

int trace_handle(struct trace *trace, fd_set *set)
{
	if (!set || FD_ISSET(trace->fd, set)) {
		trace_handle_recv(trace);
	}
	return 0;
}

enum trace_state trace_state_get(struct trace *trace)
{
	if (trace->fd < 0)
		return TRACE_STATE_DISCONNECTED;
	
	if (trace->list_received)
		return TRACE_STATE_READY;
	
	return TRACE_STATE_CONNECTED;
}


char *enum_trace_value_type2str(enum trace_value_type type)
{
	switch(type) {
		case TRACE_VALUE_TYPE_FLOAT:
			return "FLOAT";
		case TRACE_VALUE_TYPE_BOOL:
			return "BOOL";
		case TRACE_VALUE_TYPE_UINT8:
			return "UINT8";
		case TRACE_VALUE_TYPE_UINT16:
			return "UINT16";
		case TRACE_VALUE_TYPE_UINT32:
			return "UINT32";
		case TRACE_VALUE_TYPE_SINT8:
			return "SINT8";
		case TRACE_VALUE_TYPE_SINT16:
			return "SINT16";
		case TRACE_VALUE_TYPE_SINT32:
			return "SINT32";
		default:
			return "unknown";
	}
}


char *enum_trace_interval_type2str(enum trace_interval_type type)
{
	switch(type) {
		case TRACE_INTERVAL_TYPE_INTERVAL:
			return "INTERVAL";
		case TRACE_INTERVAL_TYPE_CHANGED:
			return "CHANGED";
		case TRACE_INTERVAL_TYPE_CHANGED_INTERVAL:
			return "CHANGED_INTERVAL";
		default:
			return "unknown";
	}
}


char *enum_trace_state2str(enum trace_state state)
{
	switch(state) {
		case TRACE_STATE_DISCONNECTED:
			return "DISCONECTED";
		case TRACE_STATE_CONNECTED:
			return "CONNECTED";
		case TRACE_STATE_READY:
			return "READY";
		case TRACE_STATE_RECEIVING:
			return "RECEIVING";
		default:
			return "unknown";
	}
}