diff --git a/master/domain.c b/master/domain.c index 87e4c230c8e91af5e0d1d783bf1dd6d110b45618..13b57eef965acaca6b99e6ecc321c1b1e1a37704 100644 --- a/master/domain.c +++ b/master/domain.c @@ -439,16 +439,9 @@ ec_slave_t *ecrt_domain_register_pdo(ec_domain_t *domain, master = domain->master; - // translate address + // translate address and validate slave if (!(slave = ecrt_master_get_slave(master, address))) return NULL; - - if (vendor_id != slave->sii_vendor_id || - product_code != slave->sii_product_code) { - EC_ERR("Invalid slave type at position %i - Requested: 0x%08X 0x%08X," - " found: 0x%08X 0x%08X\".\n", slave->ring_position, vendor_id, - product_code, slave->sii_vendor_id, slave->sii_product_code); - return NULL; - } + if (ec_slave_validate(slave, vendor_id, product_code)) return NULL; if (!data_ptr) { // data_ptr is NULL => mark slave as "registered" (do not warn) diff --git a/master/slave.c b/master/slave.c index 7087ea91d071175b432b8e69269f92f13424268f..b4412d45b626939cd1c2fa88bf8e1893eee775aa 100644 --- a/master/slave.c +++ b/master/slave.c @@ -903,6 +903,27 @@ int ec_slave_conf_sdo(ec_slave_t *slave, /**< EtherCAT slave */ return 0; } +/*****************************************************************************/ + +/** + \return 0 in case of success, else < 0 +*/ + +int ec_slave_validate(const ec_slave_t *slave, /**< EtherCAT slave */ + uint32_t vendor_id, /**< vendor ID */ + uint32_t product_code /**< product code */ + ) +{ + if (vendor_id != slave->sii_vendor_id || + product_code != slave->sii_product_code) { + EC_ERR("Invalid slave type at position %i - Requested: 0x%08X 0x%08X," + " found: 0x%08X 0x%08X\".\n", slave->ring_position, vendor_id, + product_code, slave->sii_vendor_id, slave->sii_product_code); + return -1; + } + return 0; +} + /****************************************************************************** * Realtime interface *****************************************************************************/ diff --git a/master/slave.h b/master/slave.h index 1ad7cd9888352c36a061d3a0e22d0b802ebe6029..e53a4fea010f444e6080401b8e21c3071ff01f42 100644 --- a/master/slave.h +++ b/master/slave.h @@ -271,6 +271,8 @@ uint16_t ec_slave_calc_sync_size(const ec_slave_t *, int ec_slave_is_coupler(const ec_slave_t *); int ec_slave_has_subbus(const ec_slave_t *); +int ec_slave_validate(const ec_slave_t *, uint32_t, uint32_t); + /*****************************************************************************/ #endif