diff --git a/master/domain.c b/master/domain.c index 843e6787..e832c79f 100644 --- a/master/domain.c +++ b/master/domain.c @@ -266,22 +266,12 @@ int ec_domain_reg_pdo_range(ec_domain_t *domain, /**< EtherCAT domain */ { ec_data_reg_t *data_reg; ec_sii_sync_t *sync; - unsigned int sync_index; uint16_t sync_length; - switch (dir) { // FIXME - case EC_DIR_OUTPUT: sync_index = 2; break; - case EC_DIR_INPUT: sync_index = 3; break; - default: - EC_ERR("Invalid direction!\n"); - return -1; - } - - if (sync_index >= slave->sii_sync_count) { + if (!(sync = ec_slave_get_pdo_sync(slave, dir))) { EC_ERR("No sync manager found for PDO range.\n"); return -1; } - sync = &slave->sii_syncs[sync_index]; // Allocate memory for data registration object if (!(data_reg = diff --git a/master/slave.c b/master/slave.c index 0213e11d..5e2e7932 100644 --- a/master/slave.c +++ b/master/slave.c @@ -1113,6 +1113,36 @@ uint16_t ec_slave_calc_sync_size(const ec_slave_t *slave, /*****************************************************************************/ +/** + */ + +ec_sii_sync_t *ec_slave_get_pdo_sync( + ec_slave_t *slave, /**< EtherCAT slave */ + ec_direction_t dir /**< input or output */ + ) +{ + unsigned int sync_index; + + switch (dir) { + case EC_DIR_OUTPUT: sync_index = 0; break; + case EC_DIR_INPUT: sync_index = 1; break; + default: + EC_ERR("Invalid direction!\n"); + return NULL; + } + + if (slave->sii_mailbox_protocols) sync_index += 2; + + if (sync_index >= slave->sii_sync_count) { + EC_ERR("No appropriate sync manager found.\n"); + return NULL; + } + + return &slave->sii_syncs[sync_index]; +} + +/*****************************************************************************/ + /** Initializes a sync manager configuration page with EEPROM data. The referenced memory (\a data) must be at least EC_SYNC_SIZE bytes. diff --git a/master/slave.h b/master/slave.h index c53ec8d8..578757f5 100644 --- a/master/slave.h +++ b/master/slave.h @@ -284,6 +284,7 @@ int ec_slave_fetch_pdo(ec_slave_t *, const uint8_t *, size_t, int ec_slave_locate_string(ec_slave_t *, unsigned int, char **); // misc. +ec_sii_sync_t *ec_slave_get_pdo_sync(ec_slave_t *, ec_direction_t); void ec_slave_sync_config(const ec_slave_t *, const ec_sii_sync_t *, uint8_t *); void ec_slave_fmmu_config(const ec_slave_t *, const ec_fmmu_t *, uint8_t *);