diff -r af21f0bdc7c9 -r 2b9c78543663 master/datagram_pair.c --- /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 + +#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; +} + +/*****************************************************************************/