diff --git a/include/villas/nodes/ethercat.h b/include/villas/nodes/ethercat.hpp similarity index 69% rename from include/villas/nodes/ethercat.h rename to include/villas/nodes/ethercat.hpp index 0c9acc787..c44f389b2 100644 --- a/include/villas/nodes/ethercat.h +++ b/include/villas/nodes/ethercat.hpp @@ -3,7 +3,8 @@ * @file * @author Niklas Eiling * @author Steffen Vogel - * @copyright 2018, Institute for Automation of Complex Power Systems, EONERC + * @author Divya Laxetti + * @copyright 2018-2020, Institute for Automation of Complex Power Systems, EONERC * @license GNU General Public License (version 3) * * VILLASnode @@ -30,15 +31,18 @@ #pragma once +#include + #include #include +#include #include #include #include #include // Include hard-coded Ethercat Bus configuration -#include +#include #ifdef __cplusplus extern "C" { @@ -51,28 +55,30 @@ extern "C" { /** Internal data per ethercat node */ struct ethercat { - /* Settings */ + double rate; + struct { - int num_channels; + unsigned num_channels; + double range; + unsigned position; + unsigned product_code; /**< Product ID of EtherCAT slave */ + unsigned vendor_id; /**< Vendor ID of EtherCAT slave */ + + ec_slave_config_t *sc; + unsigned *offsets; /**< Offsets for PDO entries */ } in, out; + ec_domain_t *domain; + ec_pdo_entry_reg_t *domain_regs; + uint8_t *domain_pd; /**< Process data */ + std::thread thread; /**< Cyclic task thread */ + struct task task; /**< Periodic timer */ struct pool pool; struct queue_signalled queue; /**< For samples which are received from WebSockets */ - ec_domain_t *domain; - - ec_slave_config_t *sc_in; - ec_slave_config_t *sc_out; - - uint8_t *domain_pd; - - // Offsets for PDO entries - unsigned int off_out_values[ETHERCAT_NUM_CHANNELS]; - unsigned int off_in_values[ETHERCAT_NUM_CHANNELS]; - - ec_pdo_entry_reg_t domain_regs[2 * ETHERCAT_NUM_CHANNELS + 1]; + std::atomic send; /**< Last sample to be sent via EtherCAT */ }; /* Internal datastructures */ @@ -83,6 +89,21 @@ int ethercat_type_start(struct super_node *sn); /** @see node_type::type_stop */ int ethercat_type_stop(); +/** @see node_type::init */ +int ethercat_init(struct node *n); + +/** @see node_type::destroy */ +int ethercat_destroy(struct node *n); + +/** @see node_type::parse */ +int ethercat_parse(struct node *n, json_t *cfg); + +/** @see node_type::check */ +int ethercat_check(struct node *n); + +/** @see node_type::prepare */ +int ethercat_prepare(struct node *n); + /** @see node_type::open */ int ethercat_start(struct node *n); @@ -95,9 +116,6 @@ int ethercat_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned /** @see node_type::write */ int ethercat_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release); -/** @see node_type::parse */ -int ethercat_parse(struct node *n, json_t *cfg); - /** @} */ diff --git a/include/villas/nodes/ethercat_config.h b/include/villas/nodes/ethercat_config.hpp similarity index 95% rename from include/villas/nodes/ethercat_config.h rename to include/villas/nodes/ethercat_config.hpp index 548595319..6bce8804f 100644 --- a/include/villas/nodes/ethercat_config.h +++ b/include/villas/nodes/ethercat_config.hpp @@ -3,7 +3,8 @@ * @file * @author Niklas Eiling * @author Steffen Vogel - * @copyright 2018, Institute for Automation of Complex Power Systems, EONERC + * @author Divya Laxetti + * @copyright 2018-2020, Institute for Automation of Complex Power Systems, EONERC * @license GNU General Public License (version 3) * * VILLASnode @@ -24,13 +25,7 @@ #pragma once -#define ETHERCAT_NUM_CHANNELS 8 -#define ETHERCAT_VOLTAGE_RANGE 10.0 - -#define ETHERCAT_ALIAS 0 -#define ETHERCAT_POS_COUPLER 0 -#define ETHERCAT_POS_SLAVE_OUT 1 -#define ETHERCAT_POS_SLAVE_IN 2 +#include #define ETHERCAT_VID_BECKHOFF 0x00000002 @@ -46,9 +41,8 @@ #define ETHERCAT_PID_EL3008 0x0bc03052 #define ETHERCAT_PID_FC1100 0x044c0c62 -#include -/*****************************************************************************/ +/** @todo: Make PDO entry tables configurable */ /* Master 0, Slave 3, "EL4038" * Vendor ID: 0x00000002 diff --git a/lib/nodes/CMakeLists.txt b/lib/nodes/CMakeLists.txt index 7acffff67..a2937d13f 100644 --- a/lib/nodes/CMakeLists.txt +++ b/lib/nodes/CMakeLists.txt @@ -20,16 +20,7 @@ # along with this program. If not, see . ################################################################################### -<<<<<<< HEAD set(NODE_SRC) -======= -set(NODE_SRC - influxdb.c - stats.c - signal_generator.c - loopback.c -) ->>>>>>> ethercat: add ethercat plugin to cmake. removed some whitespaces. if(LIBNL3_ROUTE_FOUND) list(APPEND LIBRARIES PkgConfig::LIBNL3_ROUTE) @@ -172,7 +163,7 @@ endif() # Enable Ethercat support if(ETHERLAB_FOUND AND WITH_IO) - list(APPEND NODE_SRC ethercat.c) + list(APPEND NODE_SRC ethercat.cpp) list(APPEND INCLUDE_DIRS ${ETHERLAB_INCLUDE_DIRS}) list(APPEND LIBRARIES ${ETHERLAB_LIBRARIES}) endif() diff --git a/lib/nodes/ethercat.c b/lib/nodes/ethercat.c deleted file mode 100644 index 518aefb72..000000000 --- a/lib/nodes/ethercat.c +++ /dev/null @@ -1,341 +0,0 @@ -/** Node type: Ethercat - * - * @author Niklas Eiling - * @author Steffen Vogel - * @copyright 2018, Institute for Automation of Complex Power Systems, EONERC - * @license GNU General Public License (version 3) - * - * VILLASnode - * - * 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 - * 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 . - *********************************************************************************/ - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -/** TODOs - * - * - Implement ethercat_parse() - * - - "{ s: i, s: { s: i }, s: { } }", - "alias", &w->alias, - "in", - "num_channels", &w->in.num_channels, - "pid", ... - "vid", ... - "out", - - - alias - - coupler_pos - - - in.pos - - in.num_channels - - in.voltage_range - - in.vid - - in.pid - - - out... - - - coupler_vid - - coupler_pid - - * - Implement ethercat_print() - * - Implement ethercat_destroy() - * - Replace printf() by calls to info(), warning(), error() - */ - - -/****************************************************************************/ - -/** Task period in ns. */ -#define PERIOD_NS (1000000) - -/****************************************************************************/ - -/* Constants */ -#define NSEC_PER_SEC (1000000000) -#define FREQUENCY (NSEC_PER_SEC / PERIOD_NS) - -/****************************************************************************/ - -/* Forward declarations */ -static struct plugin p; - -static ec_master_t *master = NULL; - -int ethercat_type_start(struct super_node *sn) -{ - ec_slave_config_t *sc; - struct timespec_wakeup_time; - - master = ecrt_request_master(0); - if (!master) { - return -1; - } - - // Create configuration for bus coupler - sc = ecrt_master_slave_config(master, ETHERCAT_ALIAS, ETHERCAT_POS_COUPLER, ETHERCAT_VID_BECKHOFF, ETHERCAT_PID_EK1100); - if (!sc) { - return -1; - } - - return 0; -} - -int ethercat_start(struct node *n) -{ - struct ethercat *w = (struct ethercat *) n->_vd; - - // int ret = pool_init(&w->pool, DEFAULT_ETHERCAT_QUEUE_LENGTH, SAMPLE_LENGTH(DEFAULT_ETHERCAT_SAMPLE_LENGTH), &memory_hugepage); - // if (ret) - // return ret; - - // ret = queue_signalled_init(&w->queue, DEFAULT_ETHERCAT_QUEUE_LENGTH, &memory_hugepage, 0); - // if (ret) - // return ret; - - w->domain = ecrt_master_create_domain(master); - if (!w->domain) { - return -1; - } - - // Prepare list of domain registers - for (int i = 0; i < ETHERCAT_NUM_CHANNELS; i++) - { - w->off_out_values[i] = 0; - w->off_in_values[i] = 0; - - w->domain_regs[i].alias = ETHERCAT_ALIAS; - w->domain_regs[i].position = ETHERCAT_POS_SLAVE_OUT; - w->domain_regs[i].vendor_id = ETHERCAT_VID_BECKHOFF; - w->domain_regs[i].product_code = ETHERCAT_PID_EL4038; - w->domain_regs[i].index = 0x7000 + i * 0x10; - w->domain_regs[i].subindex = 0x1; - w->domain_regs[i].offset = w->off_out_values + i; - - w->domain_regs[i+ETHERCAT_NUM_CHANNELS].alias = ETHERCAT_ALIAS; - w->domain_regs[i+ETHERCAT_NUM_CHANNELS].position = ETHERCAT_POS_SLAVE_IN; - w->domain_regs[i+ETHERCAT_NUM_CHANNELS].vendor_id = ETHERCAT_VID_BECKHOFF; - w->domain_regs[i+ETHERCAT_NUM_CHANNELS].product_code = ETHERCAT_PID_EL3008; - w->domain_regs[i+ETHERCAT_NUM_CHANNELS].index = 0x6000 + i * 0x10; - w->domain_regs[i+ETHERCAT_NUM_CHANNELS].subindex = 0x11; - w->domain_regs[i+ETHERCAT_NUM_CHANNELS].offset = w->off_in_values + i; - }; - - // End of list delimiter - memset(&w->domain_regs[2*ETHERCAT_NUM_CHANNELS], 0, sizeof(ec_pdo_entry_reg_t)); - - // Configure analog in - info("Configuring PDOs...\n"); - if (!(w->sc_in = ecrt_master_slave_config(master, ETHERCAT_ALIAS, ETHERCAT_POS_SLAVE_IN, ETHERCAT_VID_BECKHOFF, ETHERCAT_PID_EL3008))) { - warning("Failed to get slave configuration.\n"); - return -1; - } - - if (ecrt_slave_config_pdos(w->sc_in, EC_END, slave_4_syncs)) { - error("Failed to configure PDOs.\n"); - return -1; - } - - // Configure analog out - if (!(w->sc_out = ecrt_master_slave_config(master, ETHERCAT_ALIAS, ETHERCAT_POS_SLAVE_OUT, ETHERCAT_VID_BECKHOFF, ETHERCAT_PID_EL4038))) { - warning("Failed to get slave configuration.\n"); - return -1; - } - - if (ecrt_slave_config_pdos(w->sc_out, EC_END, slave_3_syncs)) { - warning("Failed to configure PDOs.\n"); - return -1; - } - - if (ecrt_domain_reg_pdo_entry_list(w->domain, w->domain_regs)) { - error("PDO entry registration failed!\n"); - return -1; - } - - info("Activating master...\n"); - if (ecrt_master_activate(master)) { - return -1; - } - - if (!(w->domain_pd = ecrt_domain_data(w->domain))) { - return -1; - } - - return 0; -} - -int ethercat_stop(struct node *n) -{ - int ret; - struct ethercat *w = (struct ethercat *) n->_vd; - info("releasing master\n"); - ecrt_release_master(master); - info("have a nice day!\n"); - - ret = queue_signalled_destroy(&w->queue); - if (ret) - return ret; - - ret = pool_destroy(&w->pool); - if (ret) - return ret; - - return 0; -} - -int ethercat_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release) -{ - struct ethercat *w = (struct ethercat *) n->_vd; - - // int avail; - // struct sample *cpys[cnt]; - - // avail = queue_signalled_pull_many(&w->queue, (void **) cpys, cnt); - // if (avail < 0) - // return avail; - - // Receive process data - ecrt_master_receive(master); - ecrt_domain_process(w->domain); - - if (cnt < 1) - return 0; - - // Read process data - for (int i = 0; i < MIN(ETHERCAT_NUM_CHANNELS, smps[0]->capacity); ++i) { - int16_t ain_value = EC_READ_S16(w->domain_pd + w->off_in_values[i]); - - smps[0]->data[i].f = ETHERCAT_VOLTAGE_RANGE * (float) ain_value / INT16_MAX; - } - - // sample_copy_many(smps, cpys, avail); - // sample_decref_many(cpys, avail); - - return 1; -} - -int ethercat_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release) -{ - struct ethercat *w = (struct ethercat *) n->_vd; - - /* Make copies of all samples */ - // struct sample *cpys[cnt]; - // int avail = sample_alloc_many(&w->pool, cpys, cnt); - // if (avail < cnt) - // warning("Pool underrun for node %s: avail=%u", node_name(n), avail); - - // sample_copy_many(cpys, smps, avail); - - if (cnt < 1) - return 0; - - // Write process data - for (int i = 0; i < ETHERCAT_NUM_CHANNELS; ++i) { - float aout_voltage = smps[0]->data[i].f; - - int16_t aout_value = aout_voltage / ETHERCAT_VOLTAGE_RANGE * INT16_MAX; - - EC_WRITE_U16(w->domain_pd + w->off_out_values[i], aout_value); - } - - // send process data - ecrt_domain_queue(w->domain); - ecrt_master_send(master); - - //sample_decref_many(cpys, avail); - - return 1; -} - -int ethercat_parse(struct node *n, json_t *cfg) -{ - struct ethercat *w = (struct ethercat *) n->_vd; - - //const char *local, *remote; - //const char *layer = NULL; - const char *format = "villas.binary"; - - int ret; - - json_error_t err; - - - ret = json_unpack_ex(cfg, &err, 0, "{ s?: i, s?: { s: i, s?: i, s?: i }, s?: { s: i, s?: i, s?: i } }", - "alias", &alias, - "out", - "num_channels", &w->in.num_channels, - "pid", &w->out.pid, - "vid", &w->out.vid, - "in", - "num_channels", &w->in.num_channels, - "pid", &w->in.pid, - "vid", &w->in.vid, - ); - if (ret) - jerror(&err, "Failed to parse configuration of node %s", node_name(n)); - - /* Format */ - w->format = format_type_lookup(format); - if (!w->format) - error("Invalid format '%s' for node %s", format, node_name(n)); - - return 0; -} - - -// int ethercat_poll_fds(struct node *n, int *fds) -// { -// struct ethercat *w = (struct ethercat *) n->_vd; - -// fds[0] = queue_signalled_fd(&w->queue); - -// return 1; -// } - -static struct plugin p = { - .name = "ethercat", - .description = "Send and receive samples of an ethercat connection", - .type = PLUGIN_TYPE_NODE, - .node = { - .vectorize = 1, /* we only process a single sample per call */ - .size = sizeof(struct ethercat), - .type.start = ethercat_type_start, - .start = ethercat_start, - .parse = ethercat_parse, - .stop = ethercat_stop, - .read = ethercat_read, - .write = ethercat_write, - // .poll_fds = ethercat_poll_fds - } -}; - -REGISTER_PLUGIN(&p) -LIST_INIT_STATIC(&p.node.instances) diff --git a/lib/nodes/ethercat.cpp b/lib/nodes/ethercat.cpp new file mode 100644 index 000000000..92f59da41 --- /dev/null +++ b/lib/nodes/ethercat.cpp @@ -0,0 +1,484 @@ +/** Node type: Ethercat + * + * @author Niklas Eiling + * @author Steffen Vogel + * @author Divya Laxetti + * @copyright 2018-2020, Institute for Automation of Complex Power Systems, EONERC + * @license GNU General Public License (version 3) + * + * VILLASnode + * + * 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 + * 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 . + *********************************************************************************/ + +#include + +#include +#include +#include +#include +#include +#include + +using namespace villas; + +/* Forward declartions */ +static struct plugin p; + +/* Constants */ +#define NSEC_PER_SEC (1000000000) +#define FREQUENCY (NSEC_PER_SEC / PERIOD_NS) + +/* Global state and config */ +int master_id = 0; +int alias = 0; + +static ec_master_t *master = nullptr; + +struct coupler { + int position; + int vendor_id; + int product_code; + + ec_slave_config_t *sc; +} coupler = { + .position = 0, + .vendor_id = ETHERCAT_VID_BECKHOFF, + .product_code = ETHERCAT_PID_EK1100, + .sc = nullptr +}; + +static void ethercat_cyclic_task(struct node *n) +{ + struct sample *smp; + struct ethercat *w = (struct ethercat *) n->_vd; + + while (true) { + task_wait(&w->task); + + /* Receive process data */ + ecrt_master_receive(master); + ecrt_domain_process(w->domain); + + /* Receive process data */ + smp = sample_alloc(&w->pool); + if (!smp) { + warning("Pool underrun in node %s", node_name(n)); + continue; + } + + smp->length = MIN(w->in.num_channels, smp->capacity); + smp->flags = (int) SampleFlags::HAS_DATA; + smp->signals = &n->in.signals; + + /* Read process data */ + for (unsigned i = 0; i < smp->length; i++) { + int16_t ain_value = EC_READ_S16(w->domain_pd + w->in.offsets[i]); + + smp->data[i].f = w->in.range * (float) ain_value / INT16_MAX; + } + + queue_signalled_push(&w->queue, smp); + + /* Write process data */ + smp = w->send.exchange(nullptr); + + for (unsigned i = 0; i < w->out.num_channels; i++) { + int16_t aout_value = (smp->data[i].f / w->out.range) * INT16_MAX; + + EC_WRITE_S16(w->domain_pd + w->out.offsets[i], aout_value); + } + + sample_decref(smp); + + // send process data + ecrt_domain_queue(w->domain); + ecrt_master_send(master); + } +} + +int ethercat_type_start(villas::node::SuperNode *sn) +{ + int ret; + json_error_t err; + + json_t *cfg = sn->getConfig(); + if (cfg) { + ret = json_unpack_ex(cfg, &err, 0, "{ s?: i, s?:i, s?: { s?: { s?: i, s?: i, s?: i } } }", + "ethernet", + "master", &master_id, + "alias", &alias, + "coupler", + "position", &coupler.position, + "product_code", &coupler.product_code, + "vendor_id", &coupler.vendor_id + ); + if (ret) + jerror(&err, "Failed to parse EtherCAT configuration"); + } + + master = ecrt_request_master(master_id); + if (!master) + return -1; + + /* Create configuration for bus coupler */ + coupler.sc = ecrt_master_slave_config(master, alias, coupler.position, coupler.vendor_id, coupler.product_code); + if (!coupler.sc) + return -1; + + return 0; +} + +int ethercat_type_stop() +{ + info("Releasing EtherCAT master"); + + ecrt_release_master(master); + + return 0; +} + +int ethercat_parse(struct node *n, json_t *cfg) +{ + struct ethercat *w = (struct ethercat *) n->_vd; + + int ret; + json_error_t err; + + ret = json_unpack_ex(cfg, &err, 0, "{ s?: F, s?: { s: i, s?: F, s?: i, s?: i, s?: i }, s?: { s: i, s?: F, s?: i, s?: i, s?: i } }", + "rate", &w->rate, + "out", + "num_channels", &w->out.num_channels, + "range", &w->out.range, + "position", &w->out.position, + "product_code", &w->out.product_code, + "vendor_id", &w->out.vendor_id, + "in", + "num_channels", &w->in.num_channels, + "range", &w->in.range, + "position", &w->in.position, + "product_code", &w->in.product_code, + "vendor_id", &w->in.vendor_id + ); + if (ret) + jerror(&err, "Failed to parse configuration of node %s", node_name(n)); + + return 0; +} + +char * ethercat_print(struct node *n) +{ + struct ethercat *w = (struct ethercat *) n->_vd; + std::stringstream ss; + + ss << "alias=" << alias; + ss << ", in.range=" << w->in.range << ", in.num_channels=" << w->in.num_channels; + ss << ", out.range=" << w->out.range << ", out.num_channels=" << w->out.num_channels; + + return strdup(ss.str().c_str()); +} + +int ethercat_check(struct node *n) +{ + struct ethercat *w = (struct ethercat *) n->_vd; + + /* Some parts of the configuration are still hard-coded for this specific setup */ + if (w->in.product_code != ETHERCAT_PID_EL3008 || + w->in.vendor_id != ETHERCAT_VID_BECKHOFF || + w->out.product_code != ETHERCAT_PID_EL4038 || + w->out.vendor_id != ETHERCAT_VID_BECKHOFF || + coupler.product_code != ETHERCAT_PID_EK1100 || + coupler.vendor_id != ETHERCAT_VID_BECKHOFF + ) + return -1; + + return 0; +} + +int ethercat_prepare(struct node *n) +{ + struct ethercat *w = (struct ethercat *) n->_vd; + + w->in.offsets = new unsigned[w->in.num_channels]; + if (!w->in.offsets) + throw RuntimeError("Failed to allocate memory"); + + w->out.offsets = new unsigned[w->out.num_channels]; + if (!w->out.offsets) + throw RuntimeError("Failed to allocate memory"); + + w->domain_regs = new ec_pdo_entry_reg_t[w->in.num_channels + w->out.num_channels + 1]; + if (!w->domain_regs) + throw RuntimeError("Failed to allocate memory"); + + memset(w->domain_regs, 0, (w->in.num_channels + w->out.num_channels + 1) * sizeof(ec_pdo_entry_reg_t)); + + /* Prepare list of domain registers */ + int o = 0; + for (unsigned i = 0; i < w->out.num_channels; i++) { + w->out.offsets[i] = 0; + + w->domain_regs[o].alias = alias; + w->domain_regs[o].position = w->out.position; + w->domain_regs[o].vendor_id = w->out.vendor_id; + w->domain_regs[o].product_code = w->out.product_code; + w->domain_regs[o].index = 0x7000 + i * 0x10; + w->domain_regs[o].subindex = 0x1; + w->domain_regs[o].offset = w->out.offsets + i; + + o++; + }; + + /* Prepare list of domain registers */ + for (unsigned i = 0; i < w->in.num_channels; i++) { + w->in.offsets[i] = 0; + + w->domain_regs[o].alias = alias; + w->domain_regs[o].position = w->in.position; + w->domain_regs[o].vendor_id = w->in.vendor_id; + w->domain_regs[o].product_code = w->in.product_code; + w->domain_regs[o].index = 0x6000 + i * 0x10; + w->domain_regs[o].subindex = 0x11; + w->domain_regs[o].offset = w->in.offsets + i; + + o++; + }; + + w->domain = ecrt_master_create_domain(master); + if (!w->domain) + return -1; + + return 0; +} + +int ethercat_start(struct node *n) +{ + int ret; + struct ethercat *w = (struct ethercat *) n->_vd; + + /* Configure analog in */ + w->in.sc = ecrt_master_slave_config(master, alias, w->in.position, w->in.vendor_id, w->in.product_code); + if (!w->in.sc) { + warning("Failed to get slave configuration."); + return -1; + } + + ret = ecrt_slave_config_pdos(w->in.sc, EC_END, slave_4_syncs); + if (ret) { + error("Failed to configure PDOs."); + return -1; + } + + /* Configure analog out */ + w->out.sc = ecrt_master_slave_config(master, alias, w->out.position, w->out.vendor_id, w->out.product_code); + if (!w->out.sc) { + warning("Failed to get slave configuration."); + return -1; + } + + ret = ecrt_slave_config_pdos(w->out.sc, EC_END, slave_3_syncs); + if (ret) { + warning("Failed to configure PDOs."); + return -1; + } + + ret = ecrt_domain_reg_pdo_entry_list(w->domain, w->domain_regs); + if (ret) { + error("PDO entry registration failed!"); + return -1; + } + + /** @todo Check that master is not already active... */ + ret = ecrt_master_activate(master); + if (ret) + return -1; + + w->domain_pd = ecrt_domain_data(w->domain); + if (!w->domain_pd) + return -1; + + /* Start cyclic timer */ + ret = task_init(&w->task, w->rate, CLOCK_REALTIME); + if (ret) + return -1; + + /* Start cyclic task */ + w->thread = std::thread(ethercat_cyclic_task, n); + + return 0; +} + +int ethercat_stop(struct node *n) +{ + struct ethercat *w = (struct ethercat *) n->_vd; + + w->thread.join(); + + return 0; +} + +int ethercat_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release) +{ + struct ethercat *w = (struct ethercat *) n->_vd; + + int avail; + struct sample *cpys[cnt]; + + avail = queue_signalled_pull_many(&w->queue, (void **) cpys, cnt); + if (avail < 0) + warning("Pool underrun for node %s: avail=%u", node_name(n), avail); + + sample_copy_many(smps, cpys, avail); + sample_decref_many(cpys, avail); + + return avail; +} + +int ethercat_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release) +{ + struct ethercat *w = (struct ethercat *) n->_vd; + + if (cnt < 1) + return cnt; + + struct sample *smp = smps[0]; + + sample_incref(smp); + + struct sample *old = w->send.exchange(smp); + if (old) + sample_decref(old); + + return 1; +} + +int ethercat_init(struct node *n) +{ + int ret; + struct ethercat *w = (struct ethercat *) n->_vd; + + w->pool.state = State::DESTROYED; + + /* Default values */ + w->rate = 1000; + + w->in.num_channels = 8; + w->in.range = 10.0; + w->in.position = 2; + w->in.product_code = ETHERCAT_PID_EL3008; + w->in.vendor_id = ETHERCAT_VID_BECKHOFF; + w->in.sc = nullptr; + w->in.offsets = nullptr; + + w->out.num_channels = 8; + w->out.range = 10.0; + w->out.position = 1; + w->out.product_code = ETHERCAT_PID_EL4038; + w->out.vendor_id = ETHERCAT_VID_BECKHOFF; + w->out.sc = nullptr; + w->out.offsets = nullptr; + + w->domain = nullptr; + w->domain_pd = nullptr; + w->domain_regs = nullptr; + + /* Placement new for C++ objects */ + new (&w->send) std::atomic(); + new (&w->thread) std::thread(); + + ret = pool_init(&w->pool, DEFAULT_ETHERCAT_QUEUE_LENGTH, SAMPLE_LENGTH(DEFAULT_ETHERCAT_SAMPLE_LENGTH)); + if (ret) + return ret; + + ret = queue_signalled_init(&w->queue, DEFAULT_ETHERCAT_QUEUE_LENGTH); + if (ret) + return ret; + + return 0; +} + +int ethercat_destroy(struct node *n) +{ + int ret; + struct ethercat *w = (struct ethercat *) n->_vd; + + if (w->domain_regs) + delete[] w->domain_regs; + + if (w->in.offsets) + delete[] w->in.offsets; + + if (w->out.offsets) + delete[] w->out.offsets; + + ret = task_destroy(&w->task); + if (ret) + return ret; + + ret = queue_signalled_destroy(&w->queue); + if (ret) + return ret; + + ret = pool_destroy(&w->pool); + if (ret) + return ret; + + /** @todo Destroy domain? */ + + return 0; +} + +int ethercat_poll_fds(struct node *n, int *fds) +{ + struct ethercat *w = (struct ethercat *) n->_vd; + + fds[0] = queue_signalled_fd(&w->queue); + + return 1; +} + +__attribute__((constructor(110))) +static void register_plugin() { + if (plugins.state == State::DESTROYED) + vlist_init(&plugins); + + p.name = "ethercat"; + p.description = "Send and receive samples of an ethercat connection"; + p.type = PluginType::NODE; + p.node.vectorize = 1; /* we only process a single sample per call */ + p.node.size = sizeof(struct ethercat); + p.node.type.start = ethercat_type_start; + p.node.type.stop = ethercat_type_stop; + p.node.parse = ethercat_parse; + p.node.print = ethercat_print; + p.node.check = ethercat_check; + p.node.init = ethercat_init; + p.node.destroy = ethercat_destroy; + p.node.prepare = ethercat_prepare; + p.node.start = ethercat_start; + p.node.stop = ethercat_stop; + p.node.read = ethercat_read; + p.node.write = ethercat_write; + p.node.poll_fds = ethercat_poll_fds; + + vlist_init(&p.node.instances); + vlist_push(&plugins, &p); +} + +__attribute__((destructor(110))) +static void deregister_plugin() { + if (plugins.state != State::DESTROYED) + vlist_remove_all(&plugins, &p); +} +