Commit 5517c99d authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

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
parent 774b1787
......@@ -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
......
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 \
......
......@@ -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;
}
......
......@@ -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);
......
......@@ -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);
......
......@@ -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;