Move sample timing shell interface to controller_sample

A few improvements from the Vitsch Electronics CVS
Use log_send() for all logging and add context (DEBUG/WARNING/ERROR) info.
A little bit more comments

controller/Makefile

@@ -51,7 +51,7 @@ lib/libethercat.la:
 	@echo " SUBDIR: $@"
 	@$(MAKE) -C ec
 
-lib/libshell.la: lib
+lib/libshell.la: lib/liblog.la lib
 	@echo " SUBDIR: $@"
 	@$(MAKE) -C shell
 

controller/controller/Makefile

 
 IL2C=il2c/il2c
 
-CFLAGS= -O3 -Wall  -I../log/ -I.. -I../../common/include
+CFLAGS= -O3 -Wall  -I../log/ -I../shell/ -I.. -I../../common/include
 BLOCKSRCS= \
 	block_add.c \
 	block_and2.c \

controller/controller/controller_block.c

@@ -26,6 +26,7 @@
 
 #include "controller_block.h"
 #include "controller_sample.h"
+#include "log.h"
 
 int nr_blocks = 0;
 static struct controller_block **blocks = NULL;
@@ -58,7 +59,8 @@ int controller_block_create(char *type, char *name, va_list ap)
 	
 	handle = dlopen(NULL, RTLD_NOW);
 	if (!handle) {
-		printf("Error getting handle from dlopen()\n");
+		log_send(LOG_T_ERROR, 
+		    "Error getting handle from dlopen(): %s", dlerror());
 		return -1;
 	}
 	
@@ -69,11 +71,12 @@ int controller_block_create(char *type, char *name, va_list ap)
 	
 	if (create_func) {
 		if(!create_func(name, ap)) {
-			printf("Error: %s() failed\n", symbol);
+			log_send(LOG_T_ERROR, "Error: %s() failed", symbol);
 			ret = -1;
 		}
 	} else {
-		printf("Error finding function %s for block %s\n", symbol, type);
+		log_send(LOG_T_ERROR, 
+		    "Error finding function %s for block %s", symbol, type);
 		ret = -1;
 	}
 	
@@ -111,7 +114,7 @@ void controller_block_add(struct controller_block *newblock)
 	
 	newblock->context = strdup(controller_block_context);
 
-	printf("Adding a new block: %s (%s) in context %s\n",
+	log_send(LOG_T_DEBUG, "Adding a new block: %s (%s) in context %s",
 	    newblock->name, newblock->type, newblock->context);
 	
 	tmp = realloc(blocks, sizeof(struct controller_block *) * (nr_blocks+1));
@@ -121,28 +124,29 @@ void controller_block_add(struct controller_block *newblock)
 		blocks = tmp;
 		blocks[nr_blocks] = newblock;
 		
-		printf("\tInputs:\n");
+		log_send(LOG_T_DEBUG, "\tInputs:");
 		for (i = 0; i < newblock->inputs; i++) {
 			char *type;
 			
 			type = controller_block_term_str(newblock->input[i].type);
-			printf("\t\t%d\t%s (%s)\n", i, newblock->input[i].name,
+			log_send(LOG_T_DEBUG,
+			    "\t\t%d\t%s (%s)", i, newblock->input[i].name,
 			    type);
 			*newblock->input[i].value.v = NULL;
 			newblock->input[i].otherside = NULL;
 			newblock->input[i].block = newblock;
 		}
-		printf("\tOutputs:\n");
+		log_send(LOG_T_DEBUG, "\tOutputs:");
 		for (i = 0; i < newblock->outputs; i++) {
 			char *type;
 			
 			type = controller_block_term_str(newblock->output[i].type);
-			printf("\t\t%d\t%s (%s)\n", i, newblock->output[i].name,
-			    type);
+			log_send(LOG_T_DEBUG, "\t\t%d\t%s (%s)",
+			    i, newblock->output[i].name, type);
 		}
-		printf("\tParams:\n");
+		log_send(LOG_T_DEBUG, "\tParams:");
 		for (i = 0; i < newblock->params; i++)
-			printf("\t\t%d\t%s%s\n", i, 
+			log_send(LOG_T_DEBUG, "\t\t%d\t%s%s", i, 
 			    newblock->param[i].name,
 			    newblock->param[i].sample ? " (SAMPLE)" : "");
 		
@@ -201,10 +205,31 @@ void controller_block_context_set(char *new_context)
 
 static int sample_running = 0;
 
-void controller_block_sample_init(void)
+int controller_block_sample_init(void)
 {
 	int i, j, k;
 	void *tmp;
+	bool check = true;
+
+	/* Sanity check */
+	for (i = 0; i < nr_blocks; i++) {
+		int j;
+		
+		for (j = 0; j < blocks[i]->inputs; j++) {
+			if (*blocks[i]->input[j].value.v == NULL) {
+				log_send(LOG_T_ERROR,
+				    "%s.%s is not connected!",
+				    blocks[i]->name,
+				    blocks[i]->input[j].name);
+				check = false;
+			}
+		}
+	}
+	if (!check) {
+		log_send(LOG_T_ERROR, 
+		     "Errors found while checking block connections");
+		return -1;
+	}
 
 	while (1) {
 		k = -1;
@@ -229,7 +254,7 @@ void controller_block_sample_init(void)
 		if (i < k) {
 			struct controller_block *dep = blocks[k];
 			
-			printf("Moving %s before %s in chain\n",
+			log_send(LOG_T_DEBUG, "Moving %s before %s in chain",
 			    blocks[k]->name,
 			    blocks[i]->name);
 			memmove(&blocks[i + 1], &blocks[i],
@@ -237,7 +262,7 @@ void controller_block_sample_init(void)
 			blocks[i] = dep;
 		} else {
 			if (i == nr_blocks) {
-				printf("Sorting done\n");
+				log_send(LOG_T_DEBUG, "Sorting done");
 				break;
 			}
 		}
@@ -249,10 +274,10 @@ void controller_block_sample_init(void)
 		for (j = 0; j < blocks[i]->inputs; j++) {
 			if (blocks[i]->input[j].otherside) {
 				if (first)
-					printf("%s chained to:\n",
+					log_send(LOG_T_DEBUG, "%s chained to:",
 					    blocks[i]->name);
 				first = 0;
-				printf("\t%s\n",
+				log_send(LOG_T_DEBUG, "\t%s",
 				    blocks[i]->input[j].otherside->name);
 			}
 		}
@@ -260,7 +285,8 @@ void controller_block_sample_init(void)
 
 	for (i = 0; i < nr_blocks; i++) {
 		if (blocks[i]->calculate) {
-			printf("Block %s has a calculate function\n",
+			log_send(LOG_T_DEBUG,
+			    "Block %s has a calculate function",
 			    blocks[i]->name);
 			tmp = realloc(calculates,
 			     sizeof(struct calculate_func) * (nr_calculates+1));
@@ -270,14 +296,20 @@ void controller_block_sample_init(void)
 				    blocks[i]->calculate;
 				calculates[nr_calculates].block = blocks[i];
 				nr_calculates++;
+			} else {
+				log_send(LOG_T_ERROR,
+				    "Out of memory allocating memory");
 			}
 		} else
-			printf("Block %s has no calculate function\n",
+			log_send(LOG_T_DEBUG, 
+			    "Block %s has no calculate function",
 			    blocks[i]->name);
 	}
-	printf("Found %d calculate functions in %d blocks\n",
+	log_send(LOG_T_DEBUG, "Found %d calculate functions in %d blocks",
 	    nr_calculates, nr_blocks);
 	sample_running = 1;
+
+	return 0;
 }
 
 static pthread_mutex_t param_lock;
@@ -300,30 +332,32 @@ int controller_block_param_set(char *block, char *param, va_list value)
 	int i, j;
 	
 	for (i = 0; i < nr_blocks; i++) {
-		if (!strcmp(blocks[i]->name, block)) {
-			for (j = 0; j < blocks[i]->params; j++) {
-				if (!strcmp(blocks[i]->param[j].name, param)) {
-					if (!sample_running ||
-					    controller_sample_context ||
-					    !blocks[i]->param[j].sample) {
-						blocks[i]->param_set(
-						    blocks[i], j, value);
-						return 0;
-					}
-					
-					pthread_mutex_lock(&param_lock);
+		if (strcmp(blocks[i]->name, block))
+			continue;
+			
+		for (j = 0; j < blocks[i]->params; j++) {
+			if (strcmp(blocks[i]->param[j].name, param))
+				continue;
+			
+			if (!sample_running ||
+			    controller_sample_context() ||
+			    !blocks[i]->param[j].sample) {
+				blocks[i]->param_set(
+				    blocks[i], j, value);
+				return 0;
+			}
+				
+			pthread_mutex_lock(&param_lock);
 
-					param_nr = j;
-					va_copy(param_set_value, value);
-					param_block = blocks[i];
-					param_set_func = blocks[i]->param_set;
-					
-					sem_wait(&param_sync_sem);
+			param_nr = j;
+			va_copy(param_set_value, value);
+			param_block = blocks[i];
+			param_set_func = blocks[i]->param_set;
+				
+			sem_wait(&param_sync_sem);
 
-					pthread_mutex_unlock(&param_lock);
-					return 0;
-				}
-			}
+			pthread_mutex_unlock(&param_lock);
+			return 0;
 		}
 	}
 	return -1;
@@ -334,31 +368,32 @@ int controller_block_param_get(char *block, char *param, void *value)
 	int i, j;
 	
 	for (i = 0; i < nr_blocks; i++) {
-		if (!strcmp(blocks[i]->name, block)) {
-			for (j = 0; j < blocks[i]->params; j++) {
-				if (!strcmp(blocks[i]->param[j].name, param)) {
-					
-					if (!sample_running ||
-					    controller_sample_context ||
-					    !blocks[i]->param[j].sample) {
-						blocks[i]->param_get(
-						    blocks[i], j, value);
-						return 0;
-					}
+		if (strcmp(blocks[i]->name, block))
+			continue;
 
-					pthread_mutex_lock(&param_lock);
+		for (j = 0; j < blocks[i]->params; j++) {
+			if (strcmp(blocks[i]->param[j].name, param))
+				continue;
 					
-					param_nr = j;
-					param_value = value;
-					param_block = blocks[i];
-					param_get_func = blocks[i]->param_get;
+			if (!sample_running ||
+			    controller_sample_context() ||
+			    !blocks[i]->param[j].sample) {
+				blocks[i]->param_get(
+				    blocks[i], j, value);
+				return 0;
+			}
+
+			pthread_mutex_lock(&param_lock);
 					
-					sem_wait(&param_sync_sem);
+			param_nr = j;
+			param_value = value;
+			param_block = blocks[i];
+			param_get_func = blocks[i]->param_get;
+				
+			sem_wait(&param_sync_sem);
 					
-					pthread_mutex_unlock(&param_lock);
-					return 0;
-				}
-			}
+			pthread_mutex_unlock(&param_lock);
+			return 0;
 		}
 	}
 	return -1;
@@ -369,14 +404,15 @@ int controller_block_output_get_float(char *block, char *param, float *value)
 	int i, j, ret = -1;
 	
 	for (i = 0; i < nr_blocks; i++) {
-		if (!strcmp(blocks[i]->name, block)) {
-			for (j = 0; j < blocks[i]->outputs; j++) {
-				if (!strcmp(blocks[i]->output[j].name, param) &&
-				    blocks[i]->output[j].type == CONTROLLER_BLOCK_TERM_FLOAT) {
-					*value = *blocks[i]->output[j].value.f;
-					ret = 0;
-					break;
-				}
+		if (strcmp(blocks[i]->name, block))
+			continue;
+		
+		for (j = 0; j < blocks[i]->outputs; j++) {
+			if (!strcmp(blocks[i]->output[j].name, param) &&
+			    blocks[i]->output[j].type == CONTROLLER_BLOCK_TERM_FLOAT) {
+				*value = *blocks[i]->output[j].value.f;
+				ret = 0;
+				break;
 			}
 		}
 	}
@@ -447,45 +483,50 @@ static int controller_block_resolve_link(struct controller_block_link *link)
 		}
 		if (outptr) {
 			for (i = 0; i < in->inputs; i++) {
-				if (!strcmp(in->input[i].name, link->interm) &&
-				    in->input[i].type == outtype) {
+				if (strcmp(in->input[i].name, link->interm) ||
+				    in->input[i].type != outtype)
+					continue;
 
-				    	link->type = controller_block_term_str(in->input[i].type);
+			    	link->type = controller_block_term_str(in->input[i].type);
 				
-					printf("Linking %s.%s to %s.%s%s\n",
-					    link->outblock, link->outterm, 
-					    link->inblock, link->interm,
-					    link->chain ? " (chained)" : "");
-
-					*in->input[i].value.v = outptr;
+				log_send(LOG_T_DEBUG,
+				    "Linking %s.%s to %s.%s%s",
+				    link->outblock, link->outterm, 
+				    link->inblock, link->interm,
+				    link->chain ? " (chained)" : "");
+
+				*in->input[i].value.v = outptr;
+				if (link->chain)
+					in->input[i].otherside = outsource;
+				else
+					in->input[i].otherside = NULL;
+				for (realin = &in->input[i];
+				    realin->ghostof;
+				    realin = realin->ghostof) {
 					if (link->chain)
-						in->input[i].otherside = outsource;
+						realin->ghostof->otherside = outsource;
 					else
-						in->input[i].otherside = NULL;
-					for (realin = &in->input[i];
-					    realin->ghostof;
-					    realin = realin->ghostof) {
-						if (link->chain)
-							realin->ghostof->otherside = outsource;
-						else
-							realin->ghostof->otherside = NULL;
-					}
-					return 0;
+						realin->ghostof->otherside = NULL;
 				}
+				return 0;
 			}
-			printf("Input terminal %s.%s not found\n",
+			log_send(LOG_T_ERROR, 
+			    "Input terminal %s.%s not found",
 			    link->inblock, link->interm);
 		} else {
-			printf("Output terminal %s.%s not found\n",
+			log_send(LOG_T_ERROR,
+			    "Output terminal %s.%s not found",
 			    link->outblock, link->outterm);
 		}
 	} else {
 		if (!in) {
-			printf("Input block %s does not (yet) exist, will try to link later\n",
+			log_send(LOG_T_DEBUG,
+			    "Input block %s does not (yet) exist, will try to link later",
 			    link->inblock);
 		}
 		if (!out) {
-			printf("Output block %s does not (yet) exist, will try to link later\n",
+			log_send(LOG_T_DEBUG,
+			    "Output block %s does not (yet) exist, will try to link later",
 			    link->outblock);
 		}
 	}
@@ -524,7 +565,6 @@ static struct controller_trace *trace_del_ptr = NULL;
 void controller_block_trace_init(int max)
 {
 	int i;
-	bool check = true;
 	
 	traces = malloc(sizeof(struct controller_trace *) * max);
 	max_traces = max;
@@ -534,25 +574,6 @@ void controller_block_trace_init(int max)
 		traces[i] = NULL;
 	}
 
-	/* Sanity check */
-	for (i = 0; i < nr_blocks; i++) {
-		int j;
-		
-		for (j = 0; j < blocks[i]->inputs; j++) {
-			if (*blocks[i]->input[j].value.v == NULL) {
-				fprintf(stderr,
-				    "Error: %s.%s is not connected!\n",
-				    blocks[i]->name,
-				    blocks[i]->input[j].name);
-				check = false;
-			}
-		}
-	}
-	if (!check) {
-		fprintf(stderr, "Errors found while checking block connections\n");
-		exit(1);
-	}
-
 	pthread_mutex_init(&trace_lock, NULL);
 	sem_init(&trace_sync_sem, 0, 0);
 	sem_init(&trace_waitsem, 0, 0);
@@ -567,12 +588,13 @@ int controller_block_trace_add(char *block, char *outterm,
 	int ret = 0;
 	
 	for (i = 0; i < nr_blocks; i++) {
-		if (!strcmp(block, blocks[i]->name)) {
-			for (j = 0; j < blocks[i]->outputs; j++) {
-				if (!strcmp(outterm, blocks[i]->output[j].name)){
-					ptr = blocks[i]->output[j].value.v;
-					type = blocks[i]->output[j].type;
-				}
+		if (strcmp(block, blocks[i]->name))
+			continue;
+			
+		for (j = 0; j < blocks[i]->outputs; j++) {
+			if (!strcmp(outterm, blocks[i]->output[j].name)){
+				ptr = blocks[i]->output[j].value.v;
+				type = blocks[i]->output[j].type;
 			}
 		}
 	}
@@ -592,7 +614,8 @@ int controller_block_trace_add(char *block, char *outterm,
 	if (nr_traces < max_traces) {
 		trace_add_ptr = trace;
 
-		printf("Addded %s.%s to trace list\n", block, outterm);
+		log_send(LOG_T_DEBUG, "Addded %s.%s to trace list",
+		    block, outterm);
 	} else {
 		ret = 1;
 	}

controller/controller/controller_block.h

@@ -212,7 +212,7 @@ int controller_block_trace_add(char *block, char *outterm,
 void controller_block_trace_del(struct controller_trace *trace);
 void controller_block_trace_wait(void);
 
-void controller_block_sample_init(void);
+int controller_block_sample_init(void);
 
 void controller_block_calculate(void);
 void controller_block_trace(void);

controller/controller/controller_load.c

@@ -28,6 +28,7 @@
 #include "controller_block.h"
 #include "../../common/include/dynarg.h"
 #include <shell/shell.h>
+#include "log.h"
 
 struct controller_load_array {
 	void *array;
@@ -55,7 +56,7 @@ void yyerror(yyscan_t scanner, char const *s)
 {
 	struct controller_load_extra *extra = yyget_extra(scanner);
 	
-	fprintf(stderr, "%s:%d: %s\n", 
+	log_send(LOG_T_ERROR, "%s:%d: %s\n", 
 	    extra->filename,
 	    yyget_lineno(scanner),
 	    s);
@@ -212,9 +213,9 @@ void controller_load_include(char *file_name)
 {
 	int r;
 
-	printf("Include '%s'\n", file_name);
+	log_send(LOG_T_DEBUG, "Include '%s'", file_name);
 	r = controller_load(file_name);
-	printf("End of include '%s': %d\n", file_name, r);
+	log_send(LOG_T_DEBUG, "End of include '%s': %d", file_name, r);
 }
 
 
@@ -261,7 +262,7 @@ static int controller_load_shell(char *args, char *out, int *outlen)
 	extra.stringlen = strlen(args);
 	extra.string = args;
 
-	printf("parsing: %s\n", args);
+	log_send(LOG_T_DEBUG, "parsing: %s", args);
 
 	yylex_init_extra(&extra, &scanner);
 
@@ -295,7 +296,7 @@ static int controller_load_file_shell(char *args, char *out, int *outlen)
 		return *outlen;
 	}
 
-	printf("loading: %s\n", args);
+	log_send(LOG_T_DEBUG, "loading: %s", args);
 
 	ret = controller_load(args);
 	

controller/controller/controller_sample.c

@@ -15,7 +15,11 @@
 	You should have received a copy of the GNU General Public License
 	along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
-
+/*
+	This module starts and maintains the sample thread.
+	It also provides access to sample timing information.
+	
+ */
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
@@ -24,22 +28,19 @@
 #include <time.h>
 #include <string.h>
 #include <sys/mman.h>
+#include <errno.h>
 
 #include "controller_block.h"
 #include "controller_sample.h"
+#include "log.h"
+#include "shell.h"
 
-bool controller_sample_context = false;
 
 static int nsec_interval;
 static int controller_sampleoverruns = 0;
+static double controller_frequency = 1.0;
 
-static inline void tsnorm(struct timespec *ts)
-{
-	while (ts->tv_nsec >= 1000000000) {
-		ts->tv_nsec -= 1000000000;
-		ts->tv_sec++;
-	}
-}
+static pthread_t controller_sample_thread;
 
 struct sample_timing {
 	double min;
@@ -49,6 +50,27 @@ struct sample_timing {
 	char *name;
 };
 
+static struct sample_timing st_start, st_io, st_end, st_sample;
+
+struct controller_sample_start_hook {
+	void (*func)(void *arg);
+	void *arg;
+};
+
+static struct controller_sample_start_hook *controller_sample_start_hooks = NULL;
+static int controller_sample_start_hooks_nr = 0;
+
+
+
+static inline void tsnorm(struct timespec *ts)
+{
+	while (ts->tv_nsec >= 1000000000) {
+		ts->tv_nsec -= 1000000000;
+		ts->tv_sec++;
+	}
+}
+
+
 static void sample_timing_init(struct sample_timing *st, char *name)
 {
 	memset(st, 0, sizeof(struct sample_timing));
@@ -96,12 +118,11 @@ static int sample_timing_diff_nsec(struct timespec *t1, struct timespec *t2)
 static ssize_t sample_timing_snprintf(struct sample_timing *st,
     char *dest, size_t n)
 {
-	return snprintf(dest, n, "%s\t%.3f\t%.3f\t%.3f\n",
+	return snprintf(dest, n, "%s\t%.3f\t%.3f\t%.3f",
 	    st->name,
 	    st->min / 1000.0, st->avg / 1000.0, st->max / 1000.0);
 }
 
-static struct sample_timing st_start, st_io, st_end, st_sample;
 
 /*
 	This thread should be realtime....
@@ -115,12 +136,16 @@ static void *sample_thread(void *arg)
 	struct timespec t_end;
 	param.sched_priority = 99;