Introduce controller_sample_task for executing sample synchronized tasks.

Use it for block parameters.
Use it for ethercat frames. (non pdo)
Use it for vesp commands.

controller/controller/build.mk

@@ -12,6 +12,7 @@ CONTROLLER_SRCS= \
 	$(DIR)/controller_time.c \
 	$(DIR)/controller_trace.c \
 	$(DIR)/controller_sample.c \
+	$(DIR)/controller_sample_task.c \
 	$(DIR)/controller_dumpdot.c \
 	$(DIR)/controller_load.c \
 	$(DIR)/controller_load_variable.c \

controller/controller/controller_block.h

@@ -237,7 +237,6 @@ int controller_block_param_list_add(struct controller_block *block,
     struct controller_block_param_list *list);
 int controller_block_param_set(char *block, char *param, int argc, va_list,
     char **arg_types);
-void controller_block_param_handle(void);
 
 /*
 	controller_block_trace

controller/controller/controller_block_param.c

@@ -30,27 +30,28 @@
 #include <controller/controller_block.h>
 #include <controller/controller_sample.h>
 #include <controller/controller_load.h>
+#include <controller/controller_sample_task.h>
 #include <log/log.h>
 
-static pthread_mutex_t param_lock;
-static sem_t param_sync_sem;
-static controller_block_param_set_func_t param_set_func;
-static int param_set_argc;
-static va_list param_set_value;
-static int *param_set_ret;
-static struct controller_block *param_block;
-static char *param_name;
+struct param_set {
+	struct controller_block *block;
+	char *name;
+	int argc;
+	va_list value;
+	controller_block_param_set_func_t func;
+};
 
-void controller_block_param_init(void)
+int param_set_sync(void *arg)
 {
-	pthread_mutex_init(&param_lock, NULL);
-	sem_init(&param_sync_sem, 0, 0);
+	struct param_set *param = arg;
+	
+	return param->func(param->block, param->name, param->argc, param->value);
 }
 
 int controller_block_param_set(char *blockname, char *param, int argc,
     va_list value, char **arg_types)
 {
-	int i, j;
+	int i;
 	int nr_blocks = controller_block_nr();
 	struct controller_block *block;
 	
@@ -69,45 +70,26 @@ int controller_block_param_set(char *blockname, char *param, int argc,
 			    argc, arg_types))
 				return -1;
 			
-			if (!controller_sample_running() ||
-			    controller_sample_context() ||
-			    !entry->sample) {
+			if(!entry->sample) {
 				return entry->func_set(block, param, argc,
 				    value);
 			}
-				
-			pthread_mutex_lock(&param_lock);
+			
+			struct param_set param_data;
 
-			va_copy(param_set_value, value);
-			param_block = block;
-			param_name = param;
-			param_set_argc = argc;
-			param_set_func = entry->func_set;
-			param_set_ret = &j;
+			va_copy(param_data.value, value);
+			param_data.block = block;
+			param_data.name = param;
+			param_data.argc = argc;
+			param_data.func = entry->func_set;
 				
-			sem_wait(&param_sync_sem);
-			va_end(param_set_value);
-
-			pthread_mutex_unlock(&param_lock);
-			return j;
+			return controller_sample_task(param_set_sync, &param_data);
 		}
 	}
 	return -1;
 }
 
 
-void controller_block_param_handle(void)
-{
-	if (param_set_func) {
-		*param_set_ret = param_set_func(param_block, param_name,
-		    param_set_argc, param_set_value);
-		param_set_func = NULL;
-		param_set_ret = NULL;
-		param_set_argc = 0;
-		sem_post(&param_sync_sem);
-	}
-}    
-
 /* helper function to initialize param lists */
 int controller_block_param_list_add(struct controller_block *block,
     struct controller_block_param_list *list)

controller/controller/controller_sample.c

@@ -39,6 +39,7 @@
 
 #include <controller/controller_block.h>
 #include <controller/controller_sample.h>
+#include <controller/controller_sample_task.h>
 #include <controller/controller_time.h>
 #include <controller/controller_trigger.h>
 #include <controller/controller_load.h>
@@ -262,7 +263,7 @@ static void *sample_thread(void *arg)
 		t_io = timestamp();
 		
 		controller_block_trace();
-		controller_block_param_handle();
+		controller_sample_task_exec();
 		
 		t_end = timestamp();
 		
@@ -283,7 +284,7 @@ bool controller_sample_context(void)
 
 bool controller_sample_running(void)
 {
-	return false;
+	return controller_sample_thread_running;
 }
 
 

controller/controller/controller_sample_task.c

+/*
+	Copyright Jeroen Vreeken (jeroen@vreeken.net), 2015
+
+	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 <controller/controller_sample_task.h>
+#include <controller/controller_sample.h>
+
+#include <stdlib.h>
+#include <semaphore.h>
+
+struct task_data {
+	struct task_data *next;
+	
+	int (*task)(void *);
+	void *arg;
+	int ret;
+	
+	sem_t sem;
+};
+
+static struct task_data *tasklist = NULL;
+
+int controller_sample_task(int (*task)(void *), void *arg)
+{
+	struct task_data data;
+	 
+	if (controller_sample_context() ||
+	    !controller_sample_running())
+		return task(arg);
+	
+	data.task = task;
+	data.arg = arg;
+	sem_init(&data.sem, 0, 0);
+
+	do {
+		data.next = tasklist;
+	} while (!__sync_bool_compare_and_swap(&tasklist, data.next, &data));
+
+	sem_wait(&data.sem);
+
+	return data.ret;
+}
+
+void controller_sample_task_exec(void)
+{
+	if (tasklist) {
+		struct task_data *data;
+		
+		do {
+			data = tasklist;
+		} while (!__sync_bool_compare_and_swap(&tasklist, data, data->next));
+		
+		data->ret = data->task(data->arg);
+		
+		sem_post(&data->sem);
+	}
+}

controller/controller/controller_sample_task.h

+/*
+	Copyright Jeroen Vreeken (jeroen@vreeken.net), 2015
+
+	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/>.
+ */
+
+#ifndef _INCLUDE_CONTROLLER_SAMPLE_TASK_H_
+#define _INCLUDE_CONTROLLER_SAMPLE_TASK_H_
+
+int controller_sample_task(int (*task)(void *), void *arg);
+void controller_sample_task_exec(void);
+
+#endif /* _INCLUDE_CONTROLLER_SAMPLE_TASK_H_ */

controller/dt_ctrl.c

@@ -65,8 +65,6 @@ int main(int argc, char **argv)
 
 	log_server_start(CTRL_LOG_PORT, LOG_T_DEBUG, LOG_T_INFO);
 
-	controller_block_param_init();
-	
 	if (argc < 2) {
 		printf("Usage: %s <controllerfile>\n", argv[0]);
 		printf("\n");

controller/ec/block_ec.c

@@ -51,7 +51,6 @@ static void calculate(struct controller_block *ec)
 static void calculate_tx(struct controller_block *ec)
 {
 	ec_tx_pdo();
-	ec_datagram_sample();
 }
 
 static int param_set(struct controller_block *ec, char *param, int argc,

controller/ec/ec.c

@@ -36,6 +36,7 @@
 
 #include <controller/controller_time.h>
 #include <controller/controller_sample.h>
+#include <controller/controller_sample_task.h>
 #include <controller/controller_block.h>
 #include <ec/ec.h>
 #include <ec/ec_int.h>
@@ -574,16 +575,12 @@ int ec_slave_dc_count(void)
 	return dc_slaves;
 }
 
-static struct ec_dgram_addr *datagram_addr;
-static void *datagram_buffer;
-static size_t datagram_len;
-static ssize_t datagram_ret;
-static bool datagram_read = false;
-static bool datagram_write = false;
-
-static bool sample_running = false;
-static pthread_mutex_t datagram_lock;
-static sem_t datagram_sync_sem;
+struct ec_dgram_read_data {
+	struct ec_dgram_addr *addr;
+	void *buffer;
+	size_t len;
+	ssize_t ret;
+};
 
 static void ec_datagram_sample_init(void *arg)
 {
@@ -632,12 +629,6 @@ static void ec_datagram_sample_init(void *arg)
 	    controller_time_period_get(ec_bus->owner->time));
 
 	eth_timeout_set(ec_sock, &timeout, &timeout);
-
-	log_send(LOG_T_INFO, "Switching ethercat datagrams to sample-synchronised mode");
-	pthread_mutex_init(&datagram_lock, NULL);
-	sem_init(&datagram_sync_sem, 0, 0);
-	
-	sample_running = true;
 }
 
 static ssize_t ec_datagram_read_real(struct ec_dgram_addr *addr,
@@ -701,32 +692,27 @@ err_sendrecv:
 	return -1;
 }
 
-ssize_t ec_datagram_read(struct ec_dgram_addr *addr,
-    void *buffer, size_t len)
+int ec_datagram_read_sync(void *arg)
 {
-	ssize_t ret;
+	struct ec_dgram_read_data *data = arg;
 	
-	if (controller_sample_context())
-		return ec_datagram_read_real(addr, buffer, len);
-
-	pthread_mutex_lock(&datagram_lock);
-
-	if (!sample_running) {
-		ret = ec_datagram_read_real(addr, buffer, len);
-	} else {
-		datagram_addr = addr;
-		datagram_buffer = buffer;
-		datagram_len = len;
-		datagram_read = true;
+	data->ret = ec_datagram_read_real(data->addr, data->buffer, data->len);
 	
-		sem_wait(&datagram_sync_sem);
+	return 0;
+}
+
+ssize_t ec_datagram_read(struct ec_dgram_addr *addr,
+    void *buffer, size_t len)
+{
+	struct ec_dgram_read_data data;
 	
-		ret = datagram_ret;
-	}
+	data.addr = addr;
+	data.buffer = buffer;
+	data.len = len;
 	
-	pthread_mutex_unlock(&datagram_lock);
+	controller_sample_task(ec_datagram_read_sync, &data);
 
-	return ret;
+	return data.ret;
 }
 
 static ssize_t ec_datagram_write_real(struct ec_dgram_addr *addr,
@@ -793,55 +779,29 @@ err_sendrecv:
 	return -1;
 }
 
-ssize_t ec_datagram_write(struct ec_dgram_addr *addr,
-    void *buffer, size_t len)
+int ec_datagram_write_sync(void *arg)
 {
-	ssize_t ret;
+	struct ec_dgram_read_data *data = arg;
 	
-	if (controller_sample_context())
-		return ec_datagram_write_real(addr, buffer, len);
+	data->ret = ec_datagram_write_real(data->addr, data->buffer, data->len);
 	
-	pthread_mutex_lock(&datagram_lock);
+	return 0;
+}
 
-	if (!sample_running || controller_sample_context()) {
-		ret = ec_datagram_write_real(addr, buffer, len);
-	} else {
-		datagram_addr = addr;
-		datagram_buffer = buffer;
-		datagram_len = len;
-		datagram_write = true;
-	
-		sem_wait(&datagram_sync_sem);
+ssize_t ec_datagram_write(struct ec_dgram_addr *addr,
+    void *buffer, size_t len)
+{
+	struct ec_dgram_read_data data;
 	
-		ret = datagram_ret;
-	}
+	data.addr = addr;
+	data.buffer = buffer;
+	data.len = len;
 	
-	pthread_mutex_unlock(&datagram_lock);
+	controller_sample_task(ec_datagram_write_sync, &data);
 
-	return ret;
+	return data.ret;
 }
 
-void ec_datagram_sample(void)
-{
-	if (datagram_write) {
-		datagram_ret = ec_datagram_write_real(
-		    datagram_addr,
-		    datagram_buffer,
-		    datagram_len);
-		datagram_write = false;
-		sem_post(&datagram_sync_sem);
-	} else if (datagram_read) {
-		datagram_ret = ec_datagram_read_real(
-		    datagram_addr,
-		    datagram_buffer,
-		    datagram_len);
-		datagram_read = false;
-		sem_post(&datagram_sync_sem);
-	}
-}
-
-
-
 static bool init_done = false;
 
 bool ec_initialized(void)

controller/ec/ec.h

@@ -73,7 +73,6 @@ do { \
 
 ssize_t ec_datagram_read(struct ec_dgram_addr *addr, void *buffer, size_t len);
 ssize_t ec_datagram_write(struct ec_dgram_addr *addr, void *buffer, size_t len);
-void ec_datagram_sample(void);
 
 int ec_rx_pdo_add(struct ec_dgram_addr *addr, unsigned char **data, size_t len);
 int ec_tx_pdo_add(struct ec_dgram_addr *addr, unsigned char **data, size_t len);

controller/vesp/vesp.c

@@ -20,6 +20,7 @@
 #include <vesp/vesp_bus.h>
 
 #include <controller/controller_mem.h>
+#include <controller/controller_sample_task.h>
 
 #include <log/log.h>
 
@@ -324,7 +325,7 @@ int vesp_output_clients(struct controller_bus *bus)
 	return ret;
 }
 
-ssize_t vesp_command(struct controller_bus *bus, 
+ssize_t vesp_command_real(struct controller_bus *bus, 
     uint8_t address, uint8_t command, 
     void *arg, size_t arg_size,
     void *res, size_t res_size)
@@ -386,6 +387,47 @@ err:
 	return -1;
 }
 
+struct command_data {
+	struct controller_bus *bus;
+	uint8_t address;
+	uint8_t command;
+	void *arg;
+	size_t arg_size;
+	void *res;
+	size_t res_size;
+	ssize_t ret;
+};
+
+int vesp_command_sync(void *argp)
+{
+	struct command_data *data = argp;
+	
+	data->ret = vesp_command_real(data->bus, data->address, data->command, 
+	    data->arg, data->arg_size, data->res, data->res_size);
+	
+	return 0;
+}
+
+ssize_t vesp_command(struct controller_bus *bus, 
+    uint8_t address, uint8_t command, 
+    void *arg, size_t arg_size,
+    void *res, size_t res_size)
+{
+	struct command_data data;
+	
+	data.bus = bus;
+	data.address = address;
+	data.command = command;
+	data.arg = arg;
+	data.arg_size = arg_size;
+	data.res = res;
+	data.res_size = res_size;
+	
+	controller_sample_task(vesp_command_sync, &data);
+	
+	return data.ret;
+}
+
 int vesp_command_get_details(struct controller_bus *bus,
     uint8_t address, uint8_t *vid, uint8_t *dev)
 {