Move trace related stuff from controller_block to controller_block_trace

controller/controller/controller_block_trace.c

+/*
+	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2013
+	Copyright Stichting C.A. Muller Radioastronomiestation, 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/>.
+
+ */
+/*
+	controller_block_trace.c
+	
+	This module takes care of tracing block outputs.
+ */
+
+#include <malloc.h>
+#include <string.h>
+
+#include "controller_block.h"
+#include "log.h"
+
+static int nr_traces = 0;
+static struct controller_trace **traces;
+static int max_traces = 0;
+
+static pthread_mutex_t trace_lock;
+static sem_t trace_sync_sem;
+static sem_t trace_waitsem;
+static struct controller_trace *trace_add_ptr = NULL;
+static struct controller_trace *trace_del_ptr = NULL;
+
+void controller_block_trace_init(int max)
+{
+	int i;
+	
+	traces = malloc(sizeof(struct controller_trace *) * max);
+	max_traces = max;
+
+	/* touch entire trace list */
+	for (i = 0; i < max; i++) {
+		traces[i] = NULL;
+	}
+
+	pthread_mutex_init(&trace_lock, NULL);
+	sem_init(&trace_sync_sem, 0, 0);
+	sem_init(&trace_waitsem, 0, 0);
+}
+
+int controller_block_trace_add(char *blockname, char *outterm, 
+    struct controller_trace *trace)
+{
+	void *ptr = NULL;
+	enum controller_block_term_type type = 0;
+	int i, j;
+	int ret = 0;
+	int nr_blocks = controller_block_nr();
+	
+	for (i = 0; i < nr_blocks; i++) {
+		struct controller_block *block;
+		
+		block = controller_block_get(i);
+	
+		if (strcmp(blockname, block->name))
+			continue;
+			
+		for (j = 0; j < block->outputs; j++) {
+			if (!strcmp(outterm, block->output[j].name)){
+				ptr = block->output[j].value.v;
+				type = block->output[j].type;
+			}
+		}
+	}
+
+	trace->rd_pos = 0;
+	trace->wr_pos = 0;
+	trace->type = type;
+	trace->ptr = ptr;
+	
+	/* touch the entire buffer atleast once */
+	for (i = 0; i < trace->len; i++) {
+		trace->buffer[i] = (float)i;
+	}
+
+	pthread_mutex_lock(&trace_lock);
+
+	if (nr_traces < max_traces) {
+		trace_add_ptr = trace;
+
+		log_send(LOG_T_DEBUG, "Addded %s.%s to trace list",
+		    blockname, outterm);
+	} else {
+		ret = 1;
+	}
+	sem_wait(&trace_sync_sem);
+
+	pthread_mutex_unlock(&trace_lock);
+
+	return ret;
+}
+
+void controller_block_trace_del(struct controller_trace *trace)
+{
+	pthread_mutex_lock(&trace_lock);
+
+	trace_del_ptr = trace;
+	sem_wait(&trace_sync_sem);
+
+	pthread_mutex_unlock(&trace_lock);
+
+	return;
+}
+
+void controller_block_trace_wait(void)
+{
+	sem_wait(&trace_waitsem);
+}
+
+void controller_block_trace(void)
+{
+	int i;
+	int semval;
+	
+	if (trace_add_ptr) {
+		traces[nr_traces] = trace_add_ptr;
+		nr_traces++;
+
+		trace_add_ptr = NULL;
+		sem_post(&trace_sync_sem);
+	}
+	if (trace_del_ptr) {
+		for (i = 0; i < nr_traces; i++) {
+			if (traces[i] == trace_del_ptr) {
+				if (i + 1 < nr_traces) {
+					memmove(&traces[i + 0],
+					    &traces[i + 1],
+					    sizeof(struct controller_trace *) *
+					    (nr_traces - i - 1));
+				}
+				break;
+			}
+		}
+		nr_traces--;
+		trace_del_ptr = NULL;
+		sem_post(&trace_sync_sem);
+	}
+	
+	for (i = 0; i < nr_traces; i++) {
+		if (controller_time_samplenr == 0)
+		{
+			traces[i]->wr_pos++;
+			if (traces[i]->wr_pos == traces[i]->len)
+				traces[i]->wr_pos = 0;
+			*((uint32_t *)&traces[i]->buffer[traces[i]->wr_pos]) =
+			    0xffffffff; /* NaN */
+			traces[i]->wr_pos++;
+			if (traces[i]->wr_pos == traces[i]->len)
+				traces[i]->wr_pos = 0;
+			*((uint32_t *)&traces[i]->buffer[traces[i]->wr_pos]) =
+			    controller_time_seconds;
+		}
+		traces[i]->wr_pos++;
+		if (traces[i]->wr_pos == traces[i]->len)
+			traces[i]->wr_pos = 0;
+		switch (traces[i]->type) {
+			case CONTROLLER_BLOCK_TERM_BOOL:
+				traces[i]->buffer[traces[i]->wr_pos] = 
+				    *(bool*)traces[i]->ptr;
+				break;
+			case CONTROLLER_BLOCK_TERM_FLOAT:
+				traces[i]->buffer[traces[i]->wr_pos] = 
+				    *(float*)traces[i]->ptr;
+				break;
+			case CONTROLLER_BLOCK_TERM_UINT32:
+				traces[i]->buffer[traces[i]->wr_pos] = 
+				    *(uint32_t*)traces[i]->ptr;
+				break;
+			default:
+				traces[i]->buffer[traces[i]->wr_pos] = -1;
+		}
+		
+	}
+
+	sem_getvalue(&trace_waitsem, &semval);
+	if (!semval) {
+		sem_post(&trace_waitsem);
+	}
+}
+