# HG changeset patch # User Florian Pose # Date 1331910266 -3600 # Node ID 8527429b613732e1620824f0d661fedb0f04aa8b # Parent 5682b63f826a0f144e2a58669c0e88ceb0803b7a Compare with send buffer; copy changed data. diff -r 5682b63f826a -r 8527429b6137 master/datagram_pair.c --- a/master/datagram_pair.c Thu Aug 11 19:43:48 2011 +0200 +++ b/master/datagram_pair.c Fri Mar 16 16:04:26 2012 +0100 @@ -34,23 +34,94 @@ /*****************************************************************************/ +#include + +#include "master.h" #include "datagram_pair.h" /*****************************************************************************/ /** Datagram pair constructor. */ -void ec_datagram_pair_init( - ec_datagram_pair_t *pair /**< Datagram pair. */ +int ec_datagram_pair_init( + ec_datagram_pair_t *pair, /**< Datagram pair. */ + ec_domain_t *domain, /**< Parent domain. */ + uint32_t logical_offset, + uint8_t *data, + size_t data_size, /**< Data size. */ + const unsigned int used[] /**< input/output use count. */ ) { - unsigned int i; + unsigned int dev_idx; + int ret; INIT_LIST_HEAD(&pair->list); + pair->domain = domain; - for (i = 0; i < EC_NUM_DEVICES; i++) { - ec_datagram_init(&pair->datagrams[i]); + for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; 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, dev_idx ? "backup" : "main"); } + + pair->expected_working_counter = 0U; + + /* backup datagram has its own memory */ + ret = ec_datagram_prealloc(&pair->datagrams[EC_DEVICE_BACKUP], + data_size); + if (ret) { + goto out_datagrams; + } + + 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; + } + + /* 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); + ec_datagram_lrw(&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. + 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); + ec_datagram_lwr(&pair->datagrams[EC_DEVICE_BACKUP], + 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); + ec_datagram_lrd(&pair->datagrams[EC_DEVICE_BACKUP], + logical_offset, data_size); + + pair->expected_working_counter = used[EC_DIR_INPUT]; + } + + for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) { + ec_datagram_zero(&pair->datagrams[dev_idx]); + } + + return 0; + +out_datagrams: + for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) { + ec_datagram_clear(&pair->datagrams[dev_idx]); + } + + return ret; } /*****************************************************************************/ @@ -61,11 +132,62 @@ ec_datagram_pair_t *pair /**< Datagram pair. */ ) { - unsigned int i; + unsigned int dev_idx; - for (i = 0; i < EC_NUM_DEVICES; i++) { - ec_datagram_clear(&pair->datagrams[i]); + for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) { + ec_datagram_clear(&pair->datagrams[dev_idx]); + } + + if (pair->send_buffer) { + kfree(pair->send_buffer); } } /*****************************************************************************/ + +/** Process received data. + */ +unsigned int ec_datagram_pair_process( + ec_datagram_pair_t *pair /**< Datagram pair. */ + ) +{ + unsigned int dev_idx, wc_sum = 0; + + for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) { + ec_datagram_t *datagram = &pair->datagrams[dev_idx]; + + ec_datagram_output_stats(datagram); + + if (datagram->state == EC_DATAGRAM_RECEIVED) { + wc_sum += datagram->working_counter; + } + } + + return wc_sum; +} + +/*****************************************************************************/ + +/** Process received data. + */ +int ec_datagram_pair_data_changed( + const ec_datagram_pair_t *pair, + size_t offset, + size_t size, + ec_device_index_t dev_idx + ) +{ + uint8_t *sent = pair->send_buffer + offset; + uint8_t *recv = pair->datagrams[dev_idx].data + offset; + size_t i; + + for (i = 0; i < size; i++) { + if (recv[i] != sent[i]) { + return 1; + } + } + + return 0; +} + +/*****************************************************************************/ diff -r 5682b63f826a -r 8527429b6137 master/datagram_pair.h --- a/master/datagram_pair.h Thu Aug 11 19:43:48 2011 +0200 +++ b/master/datagram_pair.h Fri Mar 16 16:04:26 2012 +0100 @@ -48,15 +48,23 @@ */ typedef struct { struct list_head list; /**< List header. */ + ec_domain_t *domain; ec_datagram_t datagrams[EC_NUM_DEVICES]; /**< Main and backup datagram. */ + uint8_t *send_buffer; + unsigned int expected_working_counter; /**< Expectord working conter. */ } ec_datagram_pair_t; /*****************************************************************************/ -void ec_datagram_pair_init(ec_datagram_pair_t *); +int ec_datagram_pair_init(ec_datagram_pair_t *, ec_domain_t *, uint32_t, + uint8_t *, size_t, const unsigned int []); void ec_datagram_pair_clear(ec_datagram_pair_t *); +unsigned int ec_datagram_pair_process(ec_datagram_pair_t *); +int ec_datagram_pair_data_changed(const ec_datagram_pair_t *, + size_t, size_t, ec_device_index_t); + /*****************************************************************************/ #endif diff -r 5682b63f826a -r 8527429b6137 master/domain.c --- a/master/domain.c Thu Aug 11 19:43:48 2011 +0200 +++ b/master/domain.c Fri Mar 16 16:04:26 2012 +0100 @@ -43,6 +43,8 @@ #include "domain.h" #include "datagram_pair.h" +#define DEBUG_REDUNDANCY 0 + /*****************************************************************************/ void ec_domain_clear_data(ec_domain_t *); @@ -144,7 +146,6 @@ { ec_datagram_pair_t *datagram_pair; int ret; - unsigned int dev_idx; if (!(datagram_pair = kmalloc(sizeof(ec_datagram_pair_t), GFP_KERNEL))) { EC_MASTER_ERR(domain->master, @@ -152,52 +153,20 @@ return -ENOMEM; } - ec_datagram_pair_init(datagram_pair); - - /* backup datagram has its own memory */ - ret = ec_datagram_prealloc(&datagram_pair->datagrams[EC_DEVICE_BACKUP], - data_size); + ret = ec_datagram_pair_init(datagram_pair, domain, logical_offset, data, + data_size, used); 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 - 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 - 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) - 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]; - } - - for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) { - snprintf(datagram_pair->datagrams[dev_idx].name, - EC_DATAGRAM_NAME_SIZE, "domain%u-%u-%s", domain->index, - logical_offset, dev_idx ? "backup" : "main"); - ec_datagram_zero(&datagram_pair->datagrams[dev_idx]); - } + domain->expected_working_counter += + datagram_pair->expected_working_counter; + + EC_MASTER_DBG(domain->master, 1, + "Adding datagram pair with expected WC %u.\n", + datagram_pair->expected_working_counter); + list_add_tail(&datagram_pair->list, &domain->datagram_pairs); return 0; @@ -449,17 +418,91 @@ void ecrt_domain_process(ec_domain_t *domain) { uint16_t working_counter_sum; - ec_datagram_pair_t *datagram_pair; - unsigned int dev_idx; + ec_datagram_pair_t *datagram_pair = NULL; + ec_fmmu_config_t *fmmu; + uint32_t logical_datagram_address; + unsigned int datagram_offset, datagram_pair_wc = 0; + size_t datagram_size; + ec_datagram_t *main_datagram; + +#if DEBUG_REDUNDANCY + EC_MASTER_DBG(domain->master, 1, "domain %u process\n", domain->index); +#endif working_counter_sum = 0; - list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) { - for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) { - ec_datagram_t *datagram = &datagram_pair->datagrams[dev_idx]; - ec_datagram_output_stats(datagram); - if (datagram->state == EC_DATAGRAM_RECEIVED) { - working_counter_sum += datagram->working_counter; - } + + if (!list_empty(&domain->datagram_pairs)) { + datagram_pair = + list_entry(domain->datagram_pairs.next, ec_datagram_pair_t, list); + main_datagram = &datagram_pair->datagrams[EC_DEVICE_MAIN]; + + logical_datagram_address = EC_READ_U32(main_datagram->address); + datagram_size = main_datagram->data_size; + datagram_offset = + fmmu->logical_start_address - logical_datagram_address; + datagram_pair_wc = ec_datagram_pair_process(datagram_pair); + working_counter_sum += datagram_pair_wc; +#if DEBUG_REDUNDANCY + EC_MASTER_DBG(domain->master, 1, "dgram %s log=%u\n", + main_datagram->name, logical_datagram_address); +#endif + } + + /* Go through all FMMU configs to detect data changes. */ + list_for_each_entry(fmmu, &domain->fmmu_configs, list) { +#if DEBUG_REDUNDANCY + EC_MASTER_DBG(domain->master, 1, "fmmu log=%u size=%u dir=%u\n", + fmmu->logical_start_address, fmmu->data_size, fmmu->dir); +#endif + if (fmmu->dir != EC_DIR_INPUT) { + continue; + } + + logical_datagram_address = + EC_READ_U32(datagram_pair->datagrams[EC_DEVICE_MAIN].address); + datagram_size = datagram_pair->datagrams[EC_DEVICE_MAIN].data_size; + datagram_offset = + fmmu->logical_start_address - logical_datagram_address; + while (datagram_offset >= datagram_size) { + + datagram_pair = list_entry(datagram_pair->list.next, + ec_datagram_pair_t, list); + main_datagram = &datagram_pair->datagrams[EC_DEVICE_MAIN]; + + logical_datagram_address = EC_READ_U32(main_datagram->address); + datagram_size = main_datagram->data_size; + datagram_offset = + fmmu->logical_start_address - logical_datagram_address; + datagram_pair_wc = ec_datagram_pair_process(datagram_pair); + working_counter_sum += datagram_pair_wc; +#if DEBUG_REDUNDANCY + EC_MASTER_DBG(domain->master, 1, "dgram %s log=%u\n", + main_datagram->name, logical_datagram_address); +#endif + } + +#if DEBUG_REDUNDANCY + EC_MASTER_DBG(domain->master, 1, "input fmmu log=%u size=%u" + " using datagram %s offset=%u\n", + fmmu->logical_start_address, fmmu->data_size, + datagram_pair->datagrams[EC_DEVICE_MAIN].name, + datagram_offset); +#endif + + if (ec_datagram_pair_data_changed(datagram_pair, + datagram_offset, fmmu->data_size, EC_DEVICE_MAIN)) { + /* data changed on main link. no copying necessary. */ + } else if (ec_datagram_pair_data_changed(datagram_pair, + datagram_offset, fmmu->data_size, EC_DEVICE_BACKUP) + || (datagram_pair_wc + == datagram_pair->expected_working_counter)) { + /* data changed on backup link or no change and complete WC. + * copy to main memory. */ + uint8_t *target = datagram_pair->datagrams[EC_DEVICE_MAIN].data + + datagram_offset; + uint8_t *source = datagram_pair->datagrams[EC_DEVICE_BACKUP].data + + datagram_offset; + memcpy(target, source, fmmu->data_size); } } @@ -495,6 +538,11 @@ list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) { + /* copy main data to send buffer */ + memcpy(datagram_pair->send_buffer, + datagram_pair->datagrams[EC_DEVICE_MAIN].data, + datagram_pair->datagrams[EC_DEVICE_MAIN].data_size); + /* copy main data to backup datagram */ memcpy(datagram_pair->datagrams[EC_DEVICE_BACKUP].data, datagram_pair->datagrams[EC_DEVICE_MAIN].data,