--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/master/datagram_pair.c Mon Nov 03 15:20:05 2014 +0100
@@ -0,0 +1,201 @@
+/******************************************************************************
+ *
+ * $Id$
+ *
+ * Copyright (C) 2006-2012 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master 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 the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * ---
+ *
+ * The license mentioned above concerns the source code only. Using the
+ * EtherCAT technology and brand is only permitted in compliance with the
+ * industrial property and similar rights of Beckhoff Automation GmbH.
+ *
+ *****************************************************************************/
+
+/**
+ \file
+ EtherCAT datagram pair methods.
+*/
+
+/*****************************************************************************/
+
+#include <linux/slab.h>
+
+#include "master.h"
+#include "datagram_pair.h"
+
+/*****************************************************************************/
+
+/** Datagram pair constructor.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_datagram_pair_init(
+ ec_datagram_pair_t *pair, /**< Datagram pair. */
+ ec_domain_t *domain, /**< Parent domain. */
+ uint32_t logical_offset, /**< Logical offset. */
+ uint8_t *data, /**< Data pointer. */
+ size_t data_size, /**< Data size. */
+ const unsigned int used[] /**< input/output use count. */
+ )
+{
+ ec_device_index_t dev_idx;
+ int ret;
+
+ INIT_LIST_HEAD(&pair->list);
+ pair->domain = domain;
+
+ for (dev_idx = EC_DEVICE_MAIN;
+ dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
+ ec_datagram_init(&pair->datagrams[dev_idx]);
+ snprintf(pair->datagrams[dev_idx].name,
+ EC_DATAGRAM_NAME_SIZE, "domain%u-%u-%s", domain->index,
+ logical_offset, ec_device_names[dev_idx != 0]);
+ pair->datagrams[dev_idx].device_index = dev_idx;
+ }
+
+ pair->expected_working_counter = 0U;
+
+ for (dev_idx = EC_DEVICE_BACKUP;
+ dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
+ /* backup datagrams have their own memory */
+ ret = ec_datagram_prealloc(&pair->datagrams[dev_idx], data_size);
+ if (ret) {
+ goto out_datagrams;
+ }
+ }
+
+#if EC_MAX_NUM_DEVICES > 1
+ if (!(pair->send_buffer = kmalloc(data_size, GFP_KERNEL))) {
+ EC_MASTER_ERR(domain->master,
+ "Failed to allocate domain send buffer!\n");
+ ret = -ENOMEM;
+ goto out_datagrams;
+ }
+#endif
+
+ /* The ec_datagram_lxx() calls below can not fail, because either the
+ * datagram has external memory or it is preallocated. */
+
+ if (used[EC_DIR_OUTPUT] && used[EC_DIR_INPUT]) { // inputs and outputs
+ ec_datagram_lrw_ext(&pair->datagrams[EC_DEVICE_MAIN],
+ logical_offset, data_size, data);
+
+ for (dev_idx = EC_DEVICE_BACKUP;
+ dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
+ ec_datagram_lrw(&pair->datagrams[dev_idx],
+ logical_offset, data_size);
+ }
+
+ // If LRW is used, output FMMUs increment the working counter by 2,
+ // while input FMMUs increment it by 1.
+ pair->expected_working_counter =
+ used[EC_DIR_OUTPUT] * 2 + used[EC_DIR_INPUT];
+ } else if (used[EC_DIR_OUTPUT]) { // outputs only
+ ec_datagram_lwr_ext(&pair->datagrams[EC_DEVICE_MAIN],
+ logical_offset, data_size, data);
+ for (dev_idx = EC_DEVICE_BACKUP;
+ dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
+ ec_datagram_lwr(&pair->datagrams[dev_idx],
+ logical_offset, data_size);
+ }
+
+ pair->expected_working_counter = used[EC_DIR_OUTPUT];
+ } else { // inputs only (or nothing)
+ ec_datagram_lrd_ext(&pair->datagrams[EC_DEVICE_MAIN],
+ logical_offset, data_size, data);
+ for (dev_idx = EC_DEVICE_BACKUP;
+ dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
+ ec_datagram_lrd(&pair->datagrams[dev_idx], logical_offset,
+ data_size);
+ }
+
+ pair->expected_working_counter = used[EC_DIR_INPUT];
+ }
+
+ for (dev_idx = EC_DEVICE_MAIN;
+ dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
+ ec_datagram_zero(&pair->datagrams[dev_idx]);
+ }
+
+ return 0;
+
+out_datagrams:
+ for (dev_idx = EC_DEVICE_MAIN;
+ dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
+ ec_datagram_clear(&pair->datagrams[dev_idx]);
+ }
+
+ return ret;
+}
+
+/*****************************************************************************/
+
+/** Datagram pair destructor.
+ */
+void ec_datagram_pair_clear(
+ ec_datagram_pair_t *pair /**< Datagram pair. */
+ )
+{
+ unsigned int dev_idx;
+
+ for (dev_idx = EC_DEVICE_MAIN;
+ dev_idx < ec_master_num_devices(pair->domain->master);
+ dev_idx++) {
+ ec_datagram_clear(&pair->datagrams[dev_idx]);
+ }
+
+#if EC_MAX_NUM_DEVICES > 1
+ if (pair->send_buffer) {
+ kfree(pair->send_buffer);
+ }
+#endif
+}
+
+/*****************************************************************************/
+
+/** Process received data.
+ *
+ * \return Working counter sum over all devices.
+ */
+uint16_t ec_datagram_pair_process(
+ ec_datagram_pair_t *pair, /**< Datagram pair. */
+ uint16_t wc_sum[] /**< Working counter sums. */
+ )
+{
+ unsigned int dev_idx;
+ uint16_t pair_wc = 0;
+
+ for (dev_idx = 0; dev_idx < ec_master_num_devices(pair->domain->master);
+ dev_idx++) {
+ ec_datagram_t *datagram = &pair->datagrams[dev_idx];
+
+#ifdef EC_RT_SYSLOG
+ ec_datagram_output_stats(datagram);
+#endif
+
+ if (datagram->state == EC_DATAGRAM_RECEIVED) {
+ pair_wc += datagram->working_counter;
+ wc_sum[dev_idx] += datagram->working_counter;
+ }
+ }
+
+ return pair_wc;
+}
+
+/*****************************************************************************/