diff --git a/NEWS b/NEWS index 115aa8de1b966cffb4fed338411d986681cb782b..df9bcb016efc46d647afeeaa05f0836cae6b2984 100644 --- a/NEWS +++ b/NEWS @@ -63,6 +63,7 @@ Changes in version 1.4.0: 'Flags' from general category in slave info file. * Added MODPROBE_FLAGS variable in start script and sysconfig file. * Implemented missing datagram types. +* Allow multiple sync manager categories in SII. ------------------------------------------------------------------------------- diff --git a/master/fsm_slave_scan.c b/master/fsm_slave_scan.c index ba6ab3e2292a694a70e987bd1c3ff71cbf8fcf63..fe1a76d5e8dfcf8609e4a9472a24aad210adae84 100644 --- a/master/fsm_slave_scan.c +++ b/master/fsm_slave_scan.c @@ -464,6 +464,8 @@ void ec_fsm_slave_scan_state_eeprom_data(ec_fsm_slave_scan_t *fsm /**< slave sta } // Evaluate EEPROM contents + + ec_slave_clear_sync_managers(slave); slave->sii.alias = EC_READ_U16(slave->eeprom_data + 2 * 0x0004); diff --git a/master/slave.c b/master/slave.c index 94755786420cce3cbb10c9bcd3eeba98fd78d637..f8a33b7815c9b9004f0181d20f29be971a9642fa 100644 --- a/master/slave.c +++ b/master/slave.c @@ -261,12 +261,7 @@ void ec_slave_clear(struct kobject *kobj /**< kobject of the slave */) } // free all sync managers - if (slave->sii.syncs) { - for (i = 0; i < slave->sii.sync_count; i++) { - ec_sync_clear(&slave->sii.syncs[i]); - } - kfree(slave->sii.syncs); - } + ec_slave_clear_sync_managers(slave); // free all SII Pdos list_for_each_entry_safe(pdo, next_pdo, &slave->sii.pdos, list) { @@ -292,6 +287,23 @@ void ec_slave_sdos_clear(struct kobject *kobj /**< kobject for Sdos */) /*****************************************************************************/ +/** Clear the sync manager array. + */ +void ec_slave_clear_sync_managers(ec_slave_t *slave /**< EtherCAT slave. */) +{ + unsigned int i; + + if (slave->sii.syncs) { + for (i = 0; i < slave->sii.sync_count; i++) { + ec_sync_clear(&slave->sii.syncs[i]); + } + kfree(slave->sii.syncs); + slave->sii.syncs = NULL; + } +} + +/*****************************************************************************/ + /** * Sets the application state of a slave. */ @@ -450,48 +462,70 @@ int ec_slave_fetch_sii_general( /*****************************************************************************/ -/** - Fetches data from a SYNC MANAGER category. - \return 0 in case of success, else < 0 -*/ - +/** Fetches data from a SYNC MANAGER category. + * + * Appends the sync managers described in the category to the existing ones. + * + * \return 0 in case of success, else < 0 + */ int ec_slave_fetch_sii_syncs( - ec_slave_t *slave, /**< EtherCAT slave */ - const uint8_t *data, /**< category data */ - size_t data_size /**< number of bytes */ + ec_slave_t *slave, /**< EtherCAT slave. */ + const uint8_t *data, /**< Category data. */ + size_t data_size /**< Number of bytes. */ ) { - unsigned int i; + unsigned int i, count, total_count; ec_sync_t *sync; size_t memsize; + ec_sync_t *syncs; + uint8_t index; // one sync manager struct is 4 words long if (data_size % 8) { - EC_ERR("Invalid SII sync manager size %u in slave %u.\n", + EC_ERR("Invalid SII sync manager category size %u in slave %u.\n", data_size, slave->ring_position); return -1; } - slave->sii.sync_count = data_size / 8; + count = data_size / 8; - memsize = sizeof(ec_sync_t) * slave->sii.sync_count; - if (!(slave->sii.syncs = kmalloc(memsize, GFP_KERNEL))) { - EC_ERR("Failed to allocate %u bytes for sync managers.\n", - memsize); - slave->sii.sync_count = 0; - return -1; - } + if (slave->master->debug_level) + EC_DBG("Found Sync manager category with %u sync managers.\n", count); - for (i = 0; i < slave->sii.sync_count; i++, data += 8) { - sync = &slave->sii.syncs[i]; + if (count) { + total_count = count + slave->sii.sync_count; + memsize = sizeof(ec_sync_t) * total_count; + if (!(syncs = kmalloc(memsize, GFP_KERNEL))) { + EC_ERR("Failed to allocate %u bytes for sync managers.\n", + memsize); + return -1; + } - ec_sync_init(sync, slave, i); - sync->physical_start_address = EC_READ_U16(data); - sync->length = EC_READ_U16(data + 2); - sync->control_register = EC_READ_U8 (data + 4); - sync->enable = EC_READ_U8 (data + 6); + // copy existing sync managers + memcpy(syncs, slave->sii.syncs, + slave->sii.sync_count * sizeof(ec_sync_t)); + + // initialize new sync managers + for (i = 0; i < count; i++, data += 8) { + index = i + slave->sii.sync_count; + sync = &syncs[index]; + + ec_sync_init(sync, slave, index); + sync->physical_start_address = EC_READ_U16(data); + sync->length = EC_READ_U16(data + 2); + sync->control_register = EC_READ_U8(data + 4); + sync->enable = EC_READ_U8(data + 6); + } + + if (slave->sii.syncs) + kfree(slave->sii.syncs); + slave->sii.syncs = syncs; + slave->sii.sync_count = total_count; } + if (slave->master->debug_level) + EC_DBG("Total sync managers: %u.\n", slave->sii.sync_count); + return 0; } diff --git a/master/slave.h b/master/slave.h index 9d06a908ff599d5207ca74049779cb491aad2877..de2cf4611c44bb2ad6e5cdb4dd6b142dd3a76320 100644 --- a/master/slave.h +++ b/master/slave.h @@ -216,6 +216,8 @@ struct ec_slave int ec_slave_init(ec_slave_t *, ec_master_t *, uint16_t, uint16_t); void ec_slave_destroy(ec_slave_t *); +void ec_slave_clear_sync_managers(ec_slave_t *); + void ec_slave_request_state(ec_slave_t *, ec_slave_state_t); void ec_slave_set_state(ec_slave_t *, ec_slave_state_t); void ec_slave_set_online_state(ec_slave_t *, ec_slave_online_state_t);