From 522a63df8ab78e26645324bcf41b0c689a1fa84e Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 12 Jan 2012 17:41:05 +0100 Subject: [PATCH] Redundant outputs via datagram pairs. --- master/Kbuild.in | 1 + master/Makefile.am | 1 + master/datagram.c | 67 ++++++++++++++++++-- master/datagram.h | 10 +-- master/datagram_pair.c | 71 +++++++++++++++++++++ master/datagram_pair.h | 62 ++++++++++++++++++ master/domain.c | 140 ++++++++++++++++++++++++++--------------- master/domain.h | 3 +- master/master.c | 42 +++++++------ 9 files changed, 317 insertions(+), 80 deletions(-) create mode 100644 master/datagram_pair.c create mode 100644 master/datagram_pair.h diff --git a/master/Kbuild.in b/master/Kbuild.in index c5b059a6..cabfedc7 100644 --- a/master/Kbuild.in +++ b/master/Kbuild.in @@ -36,6 +36,7 @@ obj-m := ec_master.o ec_master-objs := \ cdev.o \ datagram.o \ + datagram_pair.o \ device.o \ domain.o \ fmmu_config.o \ diff --git a/master/Makefile.am b/master/Makefile.am index 0ecf56ab..60fed751 100644 --- a/master/Makefile.am +++ b/master/Makefile.am @@ -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 --git a/master/datagram.c b/master/datagram.c index e64d49d2..db07370b 100644 --- a/master/datagram.c +++ b/master/datagram.c @@ -427,13 +427,70 @@ int ec_datagram_brw( /*****************************************************************************/ /** 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 @@ int ec_datagram_lrd( /*****************************************************************************/ -/** 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 @@ int ec_datagram_lwr( /*****************************************************************************/ -/** 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 --git a/master/datagram.h b/master/datagram.h index a97a11f4..e902fbd4 100644 --- a/master/datagram.h +++ b/master/datagram.h @@ -85,7 +85,6 @@ typedef enum { /** 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_frmw(ec_datagram_t *, uint16_t, uint16_t, size_t); 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 --git a/master/datagram_pair.c b/master/datagram_pair.c new file mode 100644 index 00000000..529ee975 --- /dev/null +++ b/master/datagram_pair.c @@ -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 --git a/master/datagram_pair.h b/master/datagram_pair.h new file mode 100644 index 00000000..f26f7948 --- /dev/null +++ b/master/datagram_pair.h @@ -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 --git a/master/domain.c b/master/domain.c index a5f84e5f..8d47771d 100644 --- a/master/domain.c +++ b/master/domain.c @@ -41,6 +41,7 @@ #include "slave_config.h" #include "domain.h" +#include "datagram_pair.h" /*****************************************************************************/ @@ -63,7 +64,7 @@ void ec_domain_init( 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_init( */ 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 @@ void ec_domain_clear_data( 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 @@ void ec_domain_add_fmmu_config( /*****************************************************************************/ -/** Allocates a domain datagram and appends it to the list. +/** Allocates a domain datagram pair 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. + * 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 @@ int ec_domain_add_datagram( const unsigned int used[] /**< Used by inputs/outputs. */ ) { - ec_datagram_t *datagram; + ec_datagram_pair_t *datagram_pair; int ret; + unsigned int i; - if (!(datagram = kmalloc(sizeof(ec_datagram_t), GFP_KERNEL))) { + 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 @@ int ec_domain_finish( 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 @@ int ec_domain_finish( // 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 @@ int ec_domain_finish( 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 @@ int ec_domain_finish( " %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 @@ const ec_fmmu_config_t *ec_domain_find_fmmu( } /****************************************************************************** - * Realtime interface + * Application interface *****************************************************************************/ int ecrt_domain_reg_pdo_entry_list(ec_domain_t *domain, @@ -342,7 +366,7 @@ int ecrt_domain_reg_pdo_entry_list(ec_domain_t *domain, 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 @@ uint8_t *ecrt_domain_data(ec_domain_t *domain) void ecrt_domain_process(ec_domain_t *domain) { uint16_t working_counter_sum; - ec_datagram_t *datagram; + ec_datagram_pair_t *datagram_pair; + unsigned int i; - 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; + 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_process(ec_domain_t *domain) void ecrt_domain_queue(ec_domain_t *domain) { - ec_datagram_t *datagram; + ec_datagram_pair_t *datagram_pair; + unsigned int i; - list_for_each_entry(datagram, &domain->datagrams, list) { - ec_master_queue_datagram(domain->master, datagram, EC_DEVICE_MAIN); + 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 --git a/master/domain.h b/master/domain.h index 78de3bf0..e4c4104b 100644 --- a/master/domain.h +++ b/master/domain.h @@ -63,7 +63,8 @@ struct ec_domain 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 --git a/master/master.c b/master/master.c index 4dbe86db..eeaf2d58 100644 --- a/master/master.c +++ b/master/master.c @@ -2218,27 +2218,31 @@ void ecrt_master_send(ec_master_t *master) } 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); } }