# HG changeset patch # User Florian Pose # Date 1326386465 -3600 # Node ID 1d0711235a61da9140d1626b95733509c67eb07f # Parent 5e1d3c9430e01711861cc1287d101cb924c6419b Redundant outputs via datagram pairs. diff -r 5e1d3c9430e0 -r 1d0711235a61 master/Kbuild.in --- a/master/Kbuild.in Thu Jan 12 13:55:15 2012 +0100 +++ b/master/Kbuild.in Thu Jan 12 17:41:05 2012 +0100 @@ -36,6 +36,7 @@ ec_master-objs := \ cdev.o \ datagram.o \ + datagram_pair.o \ device.o \ domain.o \ fmmu_config.o \ diff -r 5e1d3c9430e0 -r 1d0711235a61 master/Makefile.am --- a/master/Makefile.am Thu Jan 12 13:55:15 2012 +0100 +++ b/master/Makefile.am Thu Jan 12 17:41:05 2012 +0100 @@ -31,6 +31,7 @@ noinst_HEADERS = \ cdev.c cdev.h \ datagram.c datagram.h \ + datagram_pair.c datagram_pair.h \ debug.c debug.h \ device.c device.h \ domain.c domain.h \ diff -r 5e1d3c9430e0 -r 1d0711235a61 master/datagram.c --- a/master/datagram.c Thu Jan 12 13:55:15 2012 +0100 +++ b/master/datagram.c Thu Jan 12 17:41:05 2012 +0100 @@ -428,12 +428,69 @@ /** Initializes an EtherCAT LRD datagram. * + * \return Return value of ec_datagram_prealloc(). + */ +int ec_datagram_lrd( + ec_datagram_t *datagram, /**< EtherCAT datagram. */ + uint32_t offset, /**< Logical address. */ + size_t data_size /**< Number of bytes to read/write. */ + ) +{ + int ret; + EC_FUNC_HEADER; + datagram->type = EC_DATAGRAM_LRD; + EC_WRITE_U32(datagram->address, offset); + EC_FUNC_FOOTER; +} + +/*****************************************************************************/ + +/** Initializes an EtherCAT LWR datagram. + * + * \return Return value of ec_datagram_prealloc(). + */ +int ec_datagram_lwr( + ec_datagram_t *datagram, /**< EtherCAT datagram. */ + uint32_t offset, /**< Logical address. */ + size_t data_size /**< Number of bytes to read/write. */ + ) +{ + int ret; + EC_FUNC_HEADER; + datagram->type = EC_DATAGRAM_LWR; + EC_WRITE_U32(datagram->address, offset); + EC_FUNC_FOOTER; +} + +/*****************************************************************************/ + +/** Initializes an EtherCAT LRW datagram. + * + * \return Return value of ec_datagram_prealloc(). + */ +int ec_datagram_lrw( + ec_datagram_t *datagram, /**< EtherCAT datagram. */ + uint32_t offset, /**< Logical address. */ + size_t data_size /**< Number of bytes to read/write. */ + ) +{ + int ret; + EC_FUNC_HEADER; + datagram->type = EC_DATAGRAM_LRW; + EC_WRITE_U32(datagram->address, offset); + EC_FUNC_FOOTER; +} + +/*****************************************************************************/ + +/** Initializes an EtherCAT LRD datagram with external memory. + * * \attention It is assumed, that the external memory is at least \a data_size * bytes large. * * \return Return value of ec_datagram_prealloc(). */ -int ec_datagram_lrd( +int ec_datagram_lrd_ext( ec_datagram_t *datagram, /**< EtherCAT datagram. */ uint32_t offset, /**< Logical address. */ size_t data_size, /**< Number of bytes to read/write. */ @@ -451,14 +508,14 @@ /*****************************************************************************/ -/** Initializes an EtherCAT LWR datagram. +/** Initializes an EtherCAT LWR datagram with external memory. * * \attention It is assumed, that the external memory is at least \a data_size * bytes large. * * \return Return value of ec_datagram_prealloc(). */ -int ec_datagram_lwr( +int ec_datagram_lwr_ext( ec_datagram_t *datagram, /**< EtherCAT datagram. */ uint32_t offset, /**< Logical address. */ size_t data_size, /**< Number of bytes to read/write. */ @@ -476,14 +533,14 @@ /*****************************************************************************/ -/** Initializes an EtherCAT LRW datagram. +/** Initializes an EtherCAT LRW datagram with external memory. * * \attention It is assumed, that the external memory is at least \a data_size * bytes large. * * \return Return value of ec_datagram_prealloc(). */ -int ec_datagram_lrw( +int ec_datagram_lrw_ext( ec_datagram_t *datagram, /**< EtherCAT datagram. */ uint32_t offset, /**< Logical address. */ size_t data_size, /**< Number of bytes to read/write. */ diff -r 5e1d3c9430e0 -r 1d0711235a61 master/datagram.h --- a/master/datagram.h Thu Jan 12 13:55:15 2012 +0100 +++ b/master/datagram.h Thu Jan 12 17:41:05 2012 +0100 @@ -85,7 +85,6 @@ /** EtherCAT datagram. */ typedef struct { - struct list_head list; /**< Needed by domain datagram lists. */ struct list_head queue; /**< Master datagram queue item. */ struct list_head sent; /**< Master list item for sent datagrams. */ ec_device_index_t device_index; /**< Device via which the datagram shall @@ -132,9 +131,12 @@ int ec_datagram_brd(ec_datagram_t *, uint16_t, size_t); int ec_datagram_bwr(ec_datagram_t *, uint16_t, size_t); int ec_datagram_brw(ec_datagram_t *, uint16_t, size_t); -int ec_datagram_lrd(ec_datagram_t *, uint32_t, size_t, uint8_t *); -int ec_datagram_lwr(ec_datagram_t *, uint32_t, size_t, uint8_t *); -int ec_datagram_lrw(ec_datagram_t *, uint32_t, size_t, uint8_t *); +int ec_datagram_lrd(ec_datagram_t *, uint32_t, size_t); +int ec_datagram_lwr(ec_datagram_t *, uint32_t, size_t); +int ec_datagram_lrw(ec_datagram_t *, uint32_t, size_t); +int ec_datagram_lrd_ext(ec_datagram_t *, uint32_t, size_t, uint8_t *); +int ec_datagram_lwr_ext(ec_datagram_t *, uint32_t, size_t, uint8_t *); +int ec_datagram_lrw_ext(ec_datagram_t *, uint32_t, size_t, uint8_t *); void ec_datagram_print_state(const ec_datagram_t *); void ec_datagram_print_wc_error(const ec_datagram_t *); diff -r 5e1d3c9430e0 -r 1d0711235a61 master/datagram_pair.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/datagram_pair.c Thu Jan 12 17:41:05 2012 +0100 @@ -0,0 +1,71 @@ +/****************************************************************************** + * + * $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 "datagram_pair.h" + +/*****************************************************************************/ + +/** Datagram pair constructor. + */ +void ec_datagram_pair_init( + ec_datagram_pair_t *pair /**< Datagram pair. */ + ) +{ + unsigned int i; + + INIT_LIST_HEAD(&pair->list); + + for (i = 0; i < EC_NUM_DEVICES; i++) { + ec_datagram_init(&pair->datagrams[i]); + } +} + +/*****************************************************************************/ + +/** Datagram pair destructor. + */ +void ec_datagram_pair_clear( + ec_datagram_pair_t *pair /**< Datagram pair. */ + ) +{ + unsigned int i; + + for (i = 0; i < EC_NUM_DEVICES; i++) { + ec_datagram_clear(&pair->datagrams[i]); + } +} + +/*****************************************************************************/ diff -r 5e1d3c9430e0 -r 1d0711235a61 master/datagram_pair.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/datagram_pair.h Thu Jan 12 17:41:05 2012 +0100 @@ -0,0 +1,62 @@ +/****************************************************************************** + * + * $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 structure. +*/ + +/*****************************************************************************/ + +#ifndef __EC_DATAGRAM_PAIR_H__ +#define __EC_DATAGRAM_PAIR_H__ + +#include + +#include "globals.h" +#include "datagram.h" + +/*****************************************************************************/ + +/** Domain datagram pair. + */ +typedef struct { + struct list_head list; /**< List header. */ + ec_datagram_t datagrams[EC_NUM_DEVICES]; /**< Main and backup datagram. + */ +} ec_datagram_pair_t; + +/*****************************************************************************/ + +void ec_datagram_pair_init(ec_datagram_pair_t *); +void ec_datagram_pair_clear(ec_datagram_pair_t *); + +/*****************************************************************************/ + +#endif diff -r 5e1d3c9430e0 -r 1d0711235a61 master/domain.c --- a/master/domain.c Thu Jan 12 13:55:15 2012 +0100 +++ b/master/domain.c Thu Jan 12 17:41:05 2012 +0100 @@ -41,6 +41,7 @@ #include "slave_config.h" #include "domain.h" +#include "datagram_pair.h" /*****************************************************************************/ @@ -63,7 +64,7 @@ domain->data = NULL; domain->data_origin = EC_ORIG_INTERNAL; domain->logical_base_address = 0x00000000; - INIT_LIST_HEAD(&domain->datagrams); + INIT_LIST_HEAD(&domain->datagram_pairs); domain->working_counter = 0x0000; domain->expected_working_counter = 0x0000; domain->working_counter_changes = 0; @@ -76,12 +77,13 @@ */ void ec_domain_clear(ec_domain_t *domain /**< EtherCAT domain */) { - ec_datagram_t *datagram, *next; + ec_datagram_pair_t *datagram_pair, *next_pair; // dequeue and free datagrams - list_for_each_entry_safe(datagram, next, &domain->datagrams, list) { - ec_datagram_clear(datagram); - kfree(datagram); + list_for_each_entry_safe(datagram_pair, next_pair, + &domain->datagram_pairs, list) { + ec_datagram_pair_clear(datagram_pair); + kfree(datagram_pair); } ec_domain_clear_data(domain); @@ -95,8 +97,10 @@ ec_domain_t *domain /**< EtherCAT domain. */ ) { - if (domain->data_origin == EC_ORIG_INTERNAL && domain->data) + if (domain->data_origin == EC_ORIG_INTERNAL && domain->data) { kfree(domain->data); + } + domain->data = NULL; domain->data_origin = EC_ORIG_INTERNAL; } @@ -122,15 +126,15 @@ /*****************************************************************************/ -/** Allocates a domain datagram and appends it to the list. - * - * The datagram type and expected working counters are determined by the - * number of input and output fmmus that share the datagram. +/** Allocates a domain datagram pair and appends it to the list. + * + * The datagrams' types and expected working counters are determined by the + * number of input and output fmmus that share the datagrams. * * \retval 0 Success. * \retval <0 Error code. */ -int ec_domain_add_datagram( +int ec_domain_add_datagram_pair( ec_domain_t *domain, /**< EtherCAT domain. */ uint32_t logical_offset, /**< Logical offset. */ size_t data_size, /**< Size of the data. */ @@ -138,47 +142,64 @@ const unsigned int used[] /**< Used by inputs/outputs. */ ) { - ec_datagram_t *datagram; + ec_datagram_pair_t *datagram_pair; int ret; - - if (!(datagram = kmalloc(sizeof(ec_datagram_t), GFP_KERNEL))) { + unsigned int i; + + if (!(datagram_pair = kmalloc(sizeof(ec_datagram_pair_t), GFP_KERNEL))) { EC_MASTER_ERR(domain->master, - "Failed to allocate domain datagram!\n"); + "Failed to allocate domain datagram pair!\n"); return -ENOMEM; } - ec_datagram_init(datagram); - snprintf(datagram->name, EC_DATAGRAM_NAME_SIZE, - "domain%u-%u", domain->index, logical_offset); + ec_datagram_pair_init(datagram_pair); + + /* backup datagram has its own memory */ + ret = ec_datagram_prealloc(&datagram_pair->datagrams[EC_DEVICE_BACKUP], + data_size); + if (ret) { + ec_datagram_pair_clear(datagram_pair); + kfree(datagram_pair); + return ret; + } + + /* 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 - ret = ec_datagram_lrw(datagram, logical_offset, data_size, data); - if (ret < 0) { - kfree(datagram); - return ret; - } + ec_datagram_lrw_ext(&datagram_pair->datagrams[EC_DEVICE_MAIN], + logical_offset, data_size, data); + ec_datagram_lrw(&datagram_pair->datagrams[EC_DEVICE_BACKUP], + logical_offset, data_size); + // If LRW is used, output FMMUs increment the working counter by 2, // while input FMMUs increment it by 1. domain->expected_working_counter += used[EC_DIR_OUTPUT] * 2 + used[EC_DIR_INPUT]; } else if (used[EC_DIR_OUTPUT]) { // outputs only - ret = ec_datagram_lwr(datagram, logical_offset, data_size, data); - if (ret < 0) { - kfree(datagram); - return ret; - } + ec_datagram_lwr_ext(&datagram_pair->datagrams[EC_DEVICE_MAIN], + logical_offset, data_size, data); + ec_datagram_lwr(&datagram_pair->datagrams[EC_DEVICE_BACKUP], + logical_offset, data_size); + domain->expected_working_counter += used[EC_DIR_OUTPUT]; } else { // inputs only (or nothing) - ret = ec_datagram_lrd(datagram, logical_offset, data_size, data); - if (ret < 0) { - kfree(datagram); - return ret; - } + ec_datagram_lrd_ext(&datagram_pair->datagrams[EC_DEVICE_MAIN], + logical_offset, data_size, data); + ec_datagram_lrd(&datagram_pair->datagrams[EC_DEVICE_BACKUP], + logical_offset, data_size); + domain->expected_working_counter += used[EC_DIR_INPUT]; } - ec_datagram_zero(datagram); - list_add_tail(&datagram->list, &domain->datagrams); + for (i = 0; i < EC_NUM_DEVICES; i++) { + snprintf(datagram_pair->datagrams[i].name, EC_DATAGRAM_NAME_SIZE, + "domain%u-%u-%s", domain->index, logical_offset, + i ? "backup" : "main"); + ec_datagram_zero(&datagram_pair->datagrams[i]); + } + + list_add_tail(&datagram_pair->list, &domain->datagram_pairs); return 0; } @@ -205,7 +226,7 @@ unsigned int datagram_used[EC_DIR_COUNT]; ec_fmmu_config_t *fmmu; ec_fmmu_config_t *fmmu_temp; - const ec_datagram_t *datagram; + const ec_datagram_pair_t *datagram_pair; int ret; domain->logical_base_address = base_address; @@ -250,7 +271,7 @@ // If the current FMMU's data do not fit in the current datagram, // allocate a new one. if (datagram_size + fmmu->data_size > EC_MAX_DATA_SIZE) { - ret = ec_domain_add_datagram(domain, + ret = ec_domain_add_datagram_pair(domain, domain->logical_base_address + datagram_offset, datagram_size, domain->data + datagram_offset, datagram_used); @@ -270,10 +291,10 @@ datagram_size += fmmu->data_size; } - // Allocate last datagram, if data are left (this is also the case if the - // process data fit into a single datagram) + /* Allocate last datagram pair, if data are left (this is also the case if + * the process data fit into a single datagram) */ if (datagram_size) { - ret = ec_domain_add_datagram(domain, + ret = ec_domain_add_datagram_pair(domain, domain->logical_base_address + datagram_offset, datagram_size, domain->data + datagram_offset, datagram_used); @@ -286,13 +307,16 @@ " %zu byte, expected working counter %u.\n", domain->index, domain->logical_base_address, domain->data_size, domain->expected_working_counter); - list_for_each_entry(datagram, &domain->datagrams, list) { + + list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) { + const ec_datagram_t *datagram = + &datagram_pair->datagrams[EC_DEVICE_MAIN]; EC_MASTER_INFO(domain->master, " Datagram %s: Logical offset 0x%08x," " %zu byte, type %s.\n", datagram->name, EC_READ_U32(datagram->address), datagram->data_size, ec_datagram_type_string(datagram)); } - + return 0; } @@ -333,7 +357,7 @@ } /****************************************************************************** - * Realtime interface + * Application interface *****************************************************************************/ int ecrt_domain_reg_pdo_entry_list(ec_domain_t *domain, @@ -342,7 +366,7 @@ const ec_pdo_entry_reg_t *reg; ec_slave_config_t *sc; int ret; - + EC_MASTER_DBG(domain->master, 1, "ecrt_domain_reg_pdo_entry_list(" "domain = 0x%p, regs = 0x%p)\n", domain, regs); @@ -399,13 +423,17 @@ void ecrt_domain_process(ec_domain_t *domain) { uint16_t working_counter_sum; - ec_datagram_t *datagram; - - working_counter_sum = 0x0000; - list_for_each_entry(datagram, &domain->datagrams, list) { - ec_datagram_output_stats(datagram); - if (datagram->state == EC_DATAGRAM_RECEIVED) { - working_counter_sum += datagram->working_counter; + ec_datagram_pair_t *datagram_pair; + unsigned int i; + + working_counter_sum = 0; + list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) { + for (i = 0; i < EC_NUM_DEVICES; i++) { + ec_datagram_t *datagram = &datagram_pair->datagrams[i]; + ec_datagram_output_stats(datagram); + if (datagram->state == EC_DATAGRAM_RECEIVED) { + working_counter_sum += datagram->working_counter; + } } } @@ -435,10 +463,20 @@ void ecrt_domain_queue(ec_domain_t *domain) { - ec_datagram_t *datagram; - - list_for_each_entry(datagram, &domain->datagrams, list) { - ec_master_queue_datagram(domain->master, datagram, EC_DEVICE_MAIN); + ec_datagram_pair_t *datagram_pair; + unsigned int i; + + list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) { + + /* copy main data to backup datagram */ + memcpy(datagram_pair->datagrams[EC_DEVICE_BACKUP].data, + datagram_pair->datagrams[EC_DEVICE_MAIN].data, + datagram_pair->datagrams[EC_DEVICE_MAIN].data_size); + + for (i = 0; i < EC_NUM_DEVICES; i++) { + ec_master_queue_datagram(domain->master, + &datagram_pair->datagrams[i], i); + } } } diff -r 5e1d3c9430e0 -r 1d0711235a61 master/domain.h --- a/master/domain.h Thu Jan 12 13:55:15 2012 +0100 +++ b/master/domain.h Thu Jan 12 17:41:05 2012 +0100 @@ -63,7 +63,8 @@ ec_origin_t data_origin; /**< Origin of the \a data memory. */ uint32_t logical_base_address; /**< Logical offset address of the process data. */ - struct list_head datagrams; /**< Datagrams for process data exchange. */ + struct list_head datagram_pairs; /**< Datagrams pairs (main/backup) for + process data exchange. */ uint16_t working_counter; /**< Last working counter value. */ uint16_t expected_working_counter; /**< Expected working counter. */ diff -r 5e1d3c9430e0 -r 1d0711235a61 master/master.c --- a/master/master.c Thu Jan 12 13:55:15 2012 +0100 +++ b/master/master.c Thu Jan 12 17:41:05 2012 +0100 @@ -2218,27 +2218,31 @@ } ec_master_inject_external_datagrams(master); - if (unlikely(!master->devices[EC_DEVICE_MAIN].link_state)) { - // link is down, no datagram can be sent - list_for_each_entry_safe(datagram, n, - &master->datagram_queue, queue) { - datagram->state = EC_DATAGRAM_ERROR; - list_del_init(&datagram->queue); - } - - // query link state - ec_device_poll(&master->devices[EC_DEVICE_MAIN]); - - // clear frame statistics - ec_device_clear_stats(&master->devices[EC_DEVICE_MAIN]); - return; - } - - // send frames for (i = 0; i < EC_NUM_DEVICES; i++) { - if (master->devices[i].dev) { - ec_master_send_datagrams(master, i); - } + if (unlikely(!master->devices[i].link_state)) { + // link is down, no datagram can be sent + list_for_each_entry_safe(datagram, n, + &master->datagram_queue, queue) { + if (datagram->device_index == i) { + datagram->state = EC_DATAGRAM_ERROR; + list_del_init(&datagram->queue); + } + } + + if (!master->devices[i].dev) { + continue; + } + + // query link state + ec_device_poll(&master->devices[i]); + + // clear frame statistics + ec_device_clear_stats(&master->devices[i]); + return; + } + + // send frames + ec_master_send_datagrams(master, i); } }