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); + } } }