Commit 0a48e37f authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

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.
parent 2fcc7308
......@@ -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 \
......
......@@ -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
......
......@@ -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)
......
......@@ -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;
}
......
/*
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);
}
}
/*
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_ */
......@@ -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");
......
......@@ -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,
......
......@@ -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)
......
......@@ -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);
......
......@@ -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)
{
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment