diff -r f859d567f94e -r 42b62867574d master/domain.c --- a/master/domain.c Fri Jan 22 10:11:58 2016 +0100 +++ b/master/domain.c Fri Jan 22 13:09:43 2016 +0100 @@ -47,6 +47,11 @@ */ #define DEBUG_REDUNDANCY 0 +#ifndef list_next_entry +#define list_next_entry(pos, member) \ + list_entry((pos)->member.next, typeof(*(pos)), member) +#endif + /*****************************************************************************/ void ec_domain_clear_data(ec_domain_t *); @@ -79,6 +84,10 @@ domain->working_counter_changes = 0; domain->redundancy_active = 0; domain->notify_jiffies = 0; + + /* Used by ec_domain_add_fmmu_config */ + memset(domain->offset_used, 0, sizeof(domain->offset_used)); + domain->sc_in_work = 0; } /*****************************************************************************/ @@ -124,14 +133,45 @@ ec_fmmu_config_t *fmmu /**< FMMU configuration. */ ) { + const ec_slave_config_t *sc; + uint32_t logical_domain_offset; + unsigned fmmu_data_size; + fmmu->domain = domain; - - domain->data_size += fmmu->data_size; + sc = fmmu->sc; + + fmmu_data_size = ec_pdo_list_total_size( + &sc->sync_configs[fmmu->sync_index].pdos); + + if (sc->allow_overlapping_pdos && (sc == domain->sc_in_work)) { + // If we permit overlapped PDOs, and we already have an allocated FMMU + // for this slave, allocate the subsequent FMMU offsets by direction + logical_domain_offset = domain->offset_used[fmmu->dir]; + } else { + // otherwise, allocate to the furthest extent of any allocated + // FMMU on this domain. + logical_domain_offset = max(domain->offset_used[EC_DIR_INPUT], + domain->offset_used[EC_DIR_OUTPUT]); + // rebase the free offsets to the current position + domain->offset_used[EC_DIR_INPUT] = logical_domain_offset; + domain->offset_used[EC_DIR_OUTPUT] = logical_domain_offset; + } + domain->sc_in_work = sc; + + // consume the offset space for this FMMU's direction + domain->offset_used[fmmu->dir] += fmmu_data_size; + + ec_fmmu_set_domain_offset_size(fmmu, logical_domain_offset, fmmu_data_size); + list_add_tail(&fmmu->list, &domain->fmmu_configs); + // Determine domain size from furthest extent of FMMU data + domain->data_size = max(domain->offset_used[EC_DIR_INPUT], + domain->offset_used[EC_DIR_OUTPUT]); + EC_MASTER_DBG(domain->master, 1, "Domain %u:" - " Added %u bytes, total %zu.\n", - domain->index, fmmu->data_size, domain->data_size); + " Added %u bytes at %u.\n", + domain->index, fmmu->data_size, logical_domain_offset); } /*****************************************************************************/ @@ -192,7 +232,7 @@ * * \return Non-zero if slave connfig was already counted. */ -int shall_count( +static int shall_count( const ec_fmmu_config_t *cur_fmmu, /**< Current FMMU with direction to search for. */ const ec_fmmu_config_t *first_fmmu /**< Datagram's first FMMU. */ @@ -213,13 +253,55 @@ /*****************************************************************************/ +/** Domain finish helper function. + * + * Known boundaries for a datagram have been identified. Scans the datagram + * FMMU boundaries for WKC counters and then creates the datagram_pair. + * + * \param domain The parent domain + * \param datagram_begin_offset Datagram's logical beginning offset + * \param datagram_end_offset Logical end offset (one past last byte) + * \param datagram_first_fmmu The begin FMMU in the datagram + * \param datagram_end_fmmu The end (one past last) FMMU + * + * \return Non-zero if error emplacing domain + */ +static int emplace_datagram(ec_domain_t *domain, + uint32_t datagram_begin_offset, + uint32_t datagram_end_offset, + const ec_fmmu_config_t *datagram_first_fmmu, + const ec_fmmu_config_t *datagram_end_fmmu + ) +{ + unsigned int datagram_used[EC_DIR_COUNT]; + const ec_fmmu_config_t *curr_fmmu; + size_t data_size; + + data_size = datagram_end_offset - datagram_begin_offset; + + memset(datagram_used, 0, sizeof(datagram_used)); + for (curr_fmmu = datagram_first_fmmu; + &curr_fmmu->list != &datagram_end_fmmu->list; + curr_fmmu = list_next_entry(curr_fmmu, list)) { + if (shall_count(curr_fmmu, datagram_first_fmmu)) { + datagram_used[curr_fmmu->dir]++; + } + } + + return ec_domain_add_datagram_pair(domain, + domain->logical_base_address + datagram_begin_offset, + data_size, + domain->data + datagram_begin_offset, + datagram_used); +} + +/*****************************************************************************/ + /** Finishes a domain. * * This allocates the necessary datagrams and writes the correct logical * addresses to every configured FMMU. * - * \todo Check for FMMUs that do not fit into any datagram. - * * \retval 0 Success * \retval <0 Error code. */ @@ -228,12 +310,13 @@ uint32_t base_address /**< Logical base address. */ ) { - uint32_t datagram_offset; - size_t datagram_size; - unsigned int datagram_count; - unsigned int datagram_used[EC_DIR_COUNT]; + uint32_t datagram_offset = 0; + unsigned int datagram_count = 0; ec_fmmu_config_t *fmmu; const ec_fmmu_config_t *datagram_first_fmmu = NULL; + const ec_fmmu_config_t *valid_fmmu = NULL; + unsigned candidate_start = 0; + unsigned valid_start = 0; const ec_datagram_pair_t *datagram_pair; int ret; @@ -253,56 +336,49 @@ // - correct the logical base addresses // - set up the datagrams to carry the process data // - calculate the datagrams' expected working counters - datagram_offset = 0; - datagram_size = 0; - datagram_count = 0; - datagram_used[EC_DIR_OUTPUT] = 0; - datagram_used[EC_DIR_INPUT] = 0; - if (!list_empty(&domain->fmmu_configs)) { datagram_first_fmmu = list_entry(domain->fmmu_configs.next, ec_fmmu_config_t, list); } list_for_each_entry(fmmu, &domain->fmmu_configs, list) { - - // Correct logical FMMU address - fmmu->logical_start_address += base_address; - - // Increment Input/Output counter to determine datagram types - // and calculate expected working counters - if (shall_count(fmmu, datagram_first_fmmu)) { - datagram_used[fmmu->dir]++; - } - - // 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_pair(domain, - domain->logical_base_address + datagram_offset, - datagram_size, domain->data + datagram_offset, - datagram_used); - if (ret < 0) - return ret; - - datagram_offset += datagram_size; - datagram_size = 0; - datagram_count++; - datagram_used[EC_DIR_OUTPUT] = 0; - datagram_used[EC_DIR_INPUT] = 0; - datagram_first_fmmu = fmmu; - } - - datagram_size += fmmu->data_size; + if (fmmu->data_size > EC_MAX_DATA_SIZE) { + EC_MASTER_ERR(domain->master, + "FMMU size %u bytes exceeds maximum data size %u", + fmmu->data_size, EC_MAX_DATA_SIZE); + return -EINVAL; + } + if (fmmu->logical_domain_offset >= candidate_start) { + // As FMMU offsets increase monotonically, and candidate start + // offset has never been contradicted, it can now never be + // contradicted, as no future FMMU can cross it. + // All FMMUs prior to this point approved for next datagram + valid_fmmu = fmmu; + valid_start = candidate_start; + if (fmmu->logical_domain_offset + fmmu->data_size - datagram_offset + > EC_MAX_DATA_SIZE) { + // yet the new candidate exceeds the datagram size, so we + // use the last known valid candidate to create the datagram + ret = emplace_datagram(domain, datagram_offset, valid_start, + datagram_first_fmmu, valid_fmmu); + if (ret < 0) + return ret; + + datagram_offset = valid_start; + datagram_count++; + datagram_first_fmmu = fmmu; + } + } + if (fmmu->logical_domain_offset + fmmu->data_size > candidate_start) { + candidate_start = fmmu->logical_domain_offset + fmmu->data_size; + } } /* 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_pair(domain, - domain->logical_base_address + datagram_offset, - datagram_size, domain->data + datagram_offset, - datagram_used); + if (domain->data_size > datagram_offset) { + ret = emplace_datagram(domain, datagram_offset, domain->data_size, + datagram_first_fmmu, fmmu); if (ret < 0) return ret; datagram_count++; @@ -317,9 +393,9 @@ 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, + " %zu byte, type %s at %p.\n", datagram->name, EC_READ_U32(datagram->address), datagram->data_size, - ec_datagram_type_string(datagram)); + ec_datagram_type_string(datagram), datagram); } return 0; @@ -503,19 +579,17 @@ continue; } - if (fmmu->logical_start_address >= - logical_datagram_address + datagram_size) { + if (fmmu->logical_domain_offset >= datagram_size) { // fmmu data contained in next datagram pair break; } - datagram_offset = - fmmu->logical_start_address - logical_datagram_address; + datagram_offset = fmmu->logical_domain_offset; #if DEBUG_REDUNDANCY EC_MASTER_DBG(domain->master, 1, - "input fmmu log=%u size=%u offset=%u\n", - fmmu->logical_start_address, fmmu->data_size, + "input fmmu log_off=%u size=%u offset=%u\n", + fmmu->logical_domain_offset, fmmu->data_size, datagram_offset); if (domain->master->debug_level > 0) { ec_print_data(pair->send_buffer + datagram_offset,