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