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