etherlabmaster: comparison master/domain.c

equal deleted inserted replaced

-:f859d567f94e
+:42b62867574d
 #include "datagram_pair.h"
 /** Extra debug output for redundancy functions.
 */
 #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 *);
 }
 domain->expected_working_counter = 0x0000;
 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;
 }
 /*****************************************************************************/
 /** Domain destructor.
 void ec_domain_add_fmmu_config(
 ec_domain_t *domain, /**< EtherCAT domain. */
 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;
+sc = fmmu->sc;
-domain->data_size += fmmu->data_size;
+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",
+" Added %u bytes at %u.\n",
-domain->index, fmmu->data_size, domain->data_size);
+domain->index, fmmu->data_size, logical_domain_offset);
 }
 /*****************************************************************************/
 /** Allocates a domain datagram pair and appends it to the list.
 * Walks through the list of all FMMU configurations for the current datagram
 * and ends before the current datagram.
 *
 * \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. */
 )
 {
 return 1;
 }
 /*****************************************************************************/
+/** 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.
 */
 int ec_domain_finish(
 ec_domain_t *domain, /**< EtherCAT domain. */
 uint32_t base_address /**< Logical base address. */
 )
 {
-uint32_t datagram_offset;
+uint32_t datagram_offset = 0;
-size_t datagram_size;
+unsigned int datagram_count = 0;
-unsigned int datagram_count;
-unsigned int datagram_used[EC_DIR_COUNT];
 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;
 domain->logical_base_address = base_address;
 // Cycle through all domain FMMUs and
 // - 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) {
+if (fmmu->data_size > EC_MAX_DATA_SIZE) {
-// Correct logical FMMU address
+EC_MASTER_ERR(domain->master,
-fmmu->logical_start_address += base_address;
+"FMMU size %u bytes exceeds maximum data size %u",
+fmmu->data_size, EC_MAX_DATA_SIZE);
-// Increment Input/Output counter to determine datagram types
+return -EINVAL;
-// and calculate expected working counters
+}
-if (shall_count(fmmu, datagram_first_fmmu)) {
+if (fmmu->logical_domain_offset >= candidate_start) {
-datagram_used[fmmu->dir]++;
+// 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.
-// If the current FMMU's data do not fit in the current datagram,
+// All FMMUs prior to this point approved for next datagram
-// allocate a new one.
+valid_fmmu = fmmu;
-if (datagram_size + fmmu->data_size > EC_MAX_DATA_SIZE) {
+valid_start = candidate_start;
-ret = ec_domain_add_datagram_pair(domain,
+if (fmmu->logical_domain_offset + fmmu->data_size - datagram_offset
-domain->logical_base_address + datagram_offset,
+> EC_MAX_DATA_SIZE) {
-datagram_size, domain->data + datagram_offset,
+// yet the new candidate exceeds the datagram size, so we
-datagram_used);
+// use the last known valid candidate to create the datagram
-if (ret < 0)
+ret = emplace_datagram(domain, datagram_offset, valid_start,
-return ret;
+datagram_first_fmmu, valid_fmmu);
+if (ret < 0)
-datagram_offset += datagram_size;
+return ret;
-datagram_size = 0;
-datagram_count++;
+datagram_offset = valid_start;
-datagram_used[EC_DIR_OUTPUT] = 0;
+datagram_count++;
-datagram_used[EC_DIR_INPUT] = 0;
+datagram_first_fmmu = fmmu;
-datagram_first_fmmu = fmmu;
+}
 }
+if (fmmu->logical_domain_offset + fmmu->data_size > candidate_start) {
-datagram_size += fmmu->data_size;
+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) {
+if (domain->data_size > datagram_offset) {
-ret = ec_domain_add_datagram_pair(domain,
+ret = emplace_datagram(domain, datagram_offset, domain->data_size,
-domain->logical_base_address + datagram_offset,
+datagram_first_fmmu, fmmu);
-datagram_size, domain->data + datagram_offset,
-datagram_used);
 if (ret < 0)
 return ret;
 datagram_count++;
 }
 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,
+" %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;
 }
 if (fmmu->dir != EC_DIR_INPUT) {
 continue;
 }
-if (fmmu->logical_start_address >=
+if (fmmu->logical_domain_offset >= datagram_size) {
-logical_datagram_address + datagram_size) {
 // fmmu data contained in next datagram pair
 break;
 }
-datagram_offset =
+datagram_offset = fmmu->logical_domain_offset;
-fmmu->logical_start_address - logical_datagram_address;
 #if DEBUG_REDUNDANCY
 EC_MASTER_DBG(domain->master, 1,
-"input fmmu log=%u size=%u offset=%u\n",
+"input fmmu log_off=%u size=%u offset=%u\n",
-fmmu->logical_start_address, fmmu->data_size,
+fmmu->logical_domain_offset, fmmu->data_size,
 datagram_offset);
 if (domain->master->debug_level > 0) {
 ec_print_data(pair->send_buffer + datagram_offset,
 fmmu->data_size);
 ec_print_data(main_datagram->data + datagram_offset,

changeset 2635	42b62867574d
parent 2627	04c83089bac2