fp@39: /****************************************************************************** fp@0: * fp@39: * $Id$ fp@0: * fp@197: * Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH fp@197: * fp@197: * This file is part of the IgH EtherCAT Master. fp@197: * fp@197: * The IgH EtherCAT Master is free software; you can redistribute it fp@197: * and/or modify it under the terms of the GNU General Public License fp@246: * as published by the Free Software Foundation; either version 2 of the fp@246: * License, or (at your option) any later version. fp@197: * fp@197: * The IgH EtherCAT Master is distributed in the hope that it will be fp@197: * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of fp@197: * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the fp@197: * GNU General Public License for more details. fp@197: * fp@197: * You should have received a copy of the GNU General Public License fp@197: * along with the IgH EtherCAT Master; if not, write to the Free Software fp@197: * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA fp@197: * fp@246: * The right to use EtherCAT Technology is granted and comes free of fp@246: * charge under condition of compatibility of product made by fp@246: * Licensee. People intending to distribute/sell products based on the fp@246: * code, have to sign an agreement to guarantee that products using fp@246: * software based on IgH EtherCAT master stay compatible with the actual fp@246: * EtherCAT specification (which are released themselves as an open fp@246: * standard) as the (only) precondition to have the right to use EtherCAT fp@246: * Technology, IP and trade marks. fp@246: * fp@39: *****************************************************************************/ fp@0: fp@199: /** fp@199: \file fp@199: EtherCAT slave methods. fp@199: */ fp@199: fp@199: /*****************************************************************************/ fp@199: fp@24: #include fp@73: #include fp@0: fp@54: #include "globals.h" fp@293: #include "datagram.h" fp@98: #include "master.h" fp@792: #include "slave_config.h" fp@792: fp@792: #include "slave.h" fp@0: fp@39: /*****************************************************************************/ fp@0: fp@251: extern const ec_code_msg_t al_status_messages[]; fp@251: fp@251: /*****************************************************************************/ fp@251: fp@404: void ec_slave_clear(struct kobject *); fp@482: void ec_slave_sdos_clear(struct kobject *); fp@182: ssize_t ec_show_slave_attribute(struct kobject *, struct attribute *, char *); fp@256: ssize_t ec_store_slave_attribute(struct kobject *, struct attribute *, fp@256: const char *, size_t); fp@624: char *ec_slave_sii_string(ec_slave_t *, unsigned int); fp@182: fp@182: /*****************************************************************************/ fp@182: fp@199: /** \cond */ fp@199: fp@325: EC_SYSFS_READ_ATTR(info); fp@256: EC_SYSFS_READ_WRITE_ATTR(state); fp@872: EC_SYSFS_READ_WRITE_ATTR(sii); fp@607: EC_SYSFS_READ_WRITE_ATTR(alias); fp@182: fp@182: static struct attribute *def_attrs[] = { fp@325: &attr_info, fp@256: &attr_state, fp@872: &attr_sii, fp@607: &attr_alias, fp@182: NULL, fp@182: }; fp@182: fp@182: static struct sysfs_ops sysfs_ops = { fp@256: .show = ec_show_slave_attribute, fp@256: .store = ec_store_slave_attribute fp@182: }; fp@182: fp@182: static struct kobj_type ktype_ec_slave = { fp@182: .release = ec_slave_clear, fp@182: .sysfs_ops = &sysfs_ops, fp@182: .default_attrs = def_attrs fp@182: }; fp@118: fp@482: static struct kobj_type ktype_ec_slave_sdos = { fp@482: .release = ec_slave_sdos_clear fp@482: }; fp@419: fp@199: /** \endcond */ fp@199: fp@118: /*****************************************************************************/ fp@118: fp@0: /** fp@195: Slave constructor. fp@195: \return 0 in case of success, else < 0 fp@195: */ fp@195: fp@195: int ec_slave_init(ec_slave_t *slave, /**< EtherCAT slave */ fp@195: ec_master_t *master, /**< EtherCAT master */ fp@195: uint16_t ring_position, /**< ring position */ fp@195: uint16_t station_address /**< station address to configure */ fp@182: ) fp@73: { fp@142: unsigned int i; fp@142: fp@182: slave->ring_position = ring_position; fp@182: slave->station_address = station_address; fp@182: fp@73: slave->master = master; fp@325: fp@792: slave->config = NULL; fp@656: slave->requested_state = EC_SLAVE_STATE_PREOP; fp@325: slave->current_state = EC_SLAVE_STATE_UNKNOWN; fp@325: slave->error_flag = 0; fp@908: slave->force_config = 0; fp@325: fp@73: slave->base_type = 0; fp@73: slave->base_revision = 0; fp@73: slave->base_build = 0; fp@73: slave->base_fmmu_count = 0; fp@325: fp@872: slave->sii_data = NULL; fp@872: slave->sii_size = 0; fp@325: fp@834: slave->sii.alias = 0; fp@834: slave->sii.vendor_id = 0; fp@834: slave->sii.product_code = 0; fp@834: slave->sii.revision_number = 0; fp@834: slave->sii.serial_number = 0; fp@834: slave->sii.rx_mailbox_offset = 0; fp@834: slave->sii.rx_mailbox_size = 0; fp@834: slave->sii.tx_mailbox_offset = 0; fp@834: slave->sii.tx_mailbox_size = 0; fp@834: slave->sii.mailbox_protocols = 0; fp@835: fp@835: slave->sii.strings = NULL; fp@835: slave->sii.string_count = 0; fp@835: fp@835: slave->sii.has_general = 0; fp@834: slave->sii.group = NULL; fp@834: slave->sii.image = NULL; fp@834: slave->sii.order = NULL; fp@834: slave->sii.name = NULL; fp@835: memset(&slave->sii.coe_details, 0x00, sizeof(ec_sii_coe_details_t)); fp@836: memset(&slave->sii.general_flags, 0x00, sizeof(ec_sii_general_flags_t)); fp@834: slave->sii.current_on_ebus = 0; fp@834: fp@834: slave->sii.syncs = NULL; fp@834: slave->sii.sync_count = 0; fp@835: fp@834: INIT_LIST_HEAD(&slave->sii.pdos); fp@835: fp@135: INIT_LIST_HEAD(&slave->sdo_dictionary); fp@145: fp@419: slave->sdo_dictionary_fetched = 0; fp@419: slave->jiffies_preop = 0; fp@419: fp@183: for (i = 0; i < 4; i++) { fp@183: slave->dl_link[i] = 0; fp@183: slave->dl_loop[i] = 0; fp@183: slave->dl_signal[i] = 0; fp@834: slave->sii.physical_layer[i] = 0xFF; fp@145: } fp@182: fp@484: // init kobject and add it to the hierarchy fp@484: memset(&slave->kobj, 0x00, sizeof(struct kobject)); fp@484: kobject_init(&slave->kobj); fp@484: slave->kobj.ktype = &ktype_ec_slave; fp@484: slave->kobj.parent = &master->kobj; fp@484: if (kobject_set_name(&slave->kobj, "slave%03i", slave->ring_position)) { fp@484: EC_ERR("Failed to set kobject name.\n"); fp@484: goto out_slave_put; fp@484: } fp@484: if (kobject_add(&slave->kobj)) { fp@484: EC_ERR("Failed to add slave's kobject.\n"); fp@484: goto out_slave_put; fp@484: } fp@484: fp@814: // init Sdo kobject and add it to the hierarchy fp@484: memset(&slave->sdo_kobj, 0x00, sizeof(struct kobject)); fp@484: kobject_init(&slave->sdo_kobj); fp@484: slave->sdo_kobj.ktype = &ktype_ec_slave_sdos; fp@484: slave->sdo_kobj.parent = &slave->kobj; fp@484: if (kobject_set_name(&slave->sdo_kobj, "sdos")) { fp@484: EC_ERR("Failed to set kobject name.\n"); fp@484: goto out_sdo_put; fp@484: } fp@484: if (kobject_add(&slave->sdo_kobj)) { fp@814: EC_ERR("Failed to add Sdos kobject.\n"); fp@484: goto out_sdo_put; fp@484: } fp@484: fp@182: return 0; fp@484: fp@484: out_sdo_put: fp@484: kobject_put(&slave->sdo_kobj); fp@484: kobject_del(&slave->kobj); fp@484: out_slave_put: fp@484: kobject_put(&slave->kobj); fp@484: return -1; fp@73: } fp@73: fp@73: /*****************************************************************************/ fp@73: fp@73: /** fp@195: Slave destructor. fp@448: Clears and frees a slave object. fp@448: */ fp@448: fp@448: void ec_slave_destroy(ec_slave_t *slave /**< EtherCAT slave */) fp@448: { fp@448: ec_sdo_t *sdo, *next_sdo; fp@448: fp@792: if (slave->config) fp@792: ec_slave_config_detach(slave->config); fp@792: fp@814: // free all Sdos fp@448: list_for_each_entry_safe(sdo, next_sdo, &slave->sdo_dictionary, list) { fp@448: list_del(&sdo->list); fp@448: ec_sdo_destroy(sdo); fp@448: } fp@448: fp@814: // free Sdo kobject fp@484: kobject_del(&slave->sdo_kobj); fp@448: kobject_put(&slave->sdo_kobj); fp@448: fp@448: // destroy self fp@448: kobject_del(&slave->kobj); fp@448: kobject_put(&slave->kobj); fp@448: } fp@448: fp@448: /*****************************************************************************/ fp@448: fp@448: /** fp@448: Clear and free slave. fp@448: This method is called by the kobject, fp@448: once there are no more references to it. fp@195: */ fp@195: fp@195: void ec_slave_clear(struct kobject *kobj /**< kobject of the slave */) fp@182: { fp@182: ec_slave_t *slave; fp@625: ec_pdo_t *pdo, *next_pdo; fp@624: unsigned int i; fp@118: fp@182: slave = container_of(kobj, ec_slave_t, kobj); fp@182: fp@624: // free all strings fp@834: if (slave->sii.strings) { fp@834: for (i = 0; i < slave->sii.string_count; i++) fp@834: kfree(slave->sii.strings[i]); fp@834: kfree(slave->sii.strings); fp@118: } fp@121: fp@195: // free all sync managers fp@870: ec_slave_clear_sync_managers(slave); fp@126: fp@814: // free all SII Pdos fp@834: list_for_each_entry_safe(pdo, next_pdo, &slave->sii.pdos, list) { fp@126: list_del(&pdo->list); fp@627: ec_pdo_clear(pdo); fp@126: kfree(pdo); fp@126: } fp@126: fp@872: if (slave->sii_data) fp@872: kfree(slave->sii_data); fp@418: fp@418: kfree(slave); fp@121: } fp@121: fp@121: /*****************************************************************************/ fp@121: fp@121: /** fp@814: * Sdo kobject clear method. fp@758: */ fp@482: fp@814: void ec_slave_sdos_clear(struct kobject *kobj /**< kobject for Sdos */) fp@482: { fp@482: } fp@482: fp@482: /*****************************************************************************/ fp@482: fp@870: /** Clear the sync manager array. fp@870: */ fp@870: void ec_slave_clear_sync_managers(ec_slave_t *slave /**< EtherCAT slave. */) fp@870: { fp@870: unsigned int i; fp@870: fp@870: if (slave->sii.syncs) { fp@870: for (i = 0; i < slave->sii.sync_count; i++) { fp@870: ec_sync_clear(&slave->sii.syncs[i]); fp@870: } fp@870: kfree(slave->sii.syncs); fp@870: slave->sii.syncs = NULL; fp@870: } fp@870: } fp@870: fp@870: /*****************************************************************************/ fp@870: fp@482: /** fp@610: * Sets the application state of a slave. fp@610: */ fp@610: fp@610: void ec_slave_set_state(ec_slave_t *slave, /**< EtherCAT slave */ fp@610: ec_slave_state_t new_state /**< new application state */ fp@610: ) fp@610: { fp@610: if (new_state != slave->current_state) { fp@610: if (slave->master->debug_level) { fp@610: char old_state[EC_STATE_STRING_SIZE], fp@610: cur_state[EC_STATE_STRING_SIZE]; fp@610: ec_state_string(slave->current_state, old_state); fp@610: ec_state_string(new_state, cur_state); fp@784: EC_DBG("Slave %u: %s -> %s.\n", fp@610: slave->ring_position, old_state, cur_state); fp@610: } fp@610: slave->current_state = new_state; fp@610: } fp@610: } fp@610: fp@610: /*****************************************************************************/ fp@610: fp@610: /** fp@758: * Request a slave state and resets the error flag. fp@446: */ fp@446: fp@635: void ec_slave_request_state(ec_slave_t *slave, /**< EtherCAT slave */ fp@446: ec_slave_state_t state /**< new state */ fp@446: ) fp@446: { fp@446: slave->requested_state = state; fp@446: slave->error_flag = 0; fp@446: } fp@446: fp@446: /*****************************************************************************/ fp@446: fp@446: /** fp@195: Fetches data from a STRING category. fp@742: \todo range checking fp@195: \return 0 in case of success, else < 0 fp@195: */ fp@195: fp@620: int ec_slave_fetch_sii_strings( fp@620: ec_slave_t *slave, /**< EtherCAT slave */ fp@742: const uint8_t *data, /**< category data */ fp@742: size_t data_size /**< number of bytes */ fp@620: ) fp@118: { fp@624: int i; fp@118: size_t size; fp@118: off_t offset; fp@624: fp@834: slave->sii.string_count = data[0]; fp@834: fp@834: if (!slave->sii.string_count) fp@624: return 0; fp@624: fp@834: if (!(slave->sii.strings = fp@834: kmalloc(sizeof(char *) * slave->sii.string_count, fp@624: GFP_KERNEL))) { fp@624: EC_ERR("Failed to allocate string array memory.\n"); fp@624: goto out_zero; fp@624: } fp@624: fp@118: offset = 1; fp@834: for (i = 0; i < slave->sii.string_count; i++) { fp@118: size = data[offset]; fp@195: // allocate memory for string structure and data at a single blow fp@834: if (!(slave->sii.strings[i] = fp@624: kmalloc(sizeof(char) * size + 1, GFP_KERNEL))) { fp@118: EC_ERR("Failed to allocate string memory.\n"); fp@624: goto out_free; fp@624: } fp@834: memcpy(slave->sii.strings[i], data + offset + 1, size); fp@834: slave->sii.strings[i][size] = 0x00; // append binary zero fp@118: offset += 1 + size; fp@118: } fp@118: fp@118: return 0; fp@624: fp@624: out_free: fp@834: for (i--; i >= 0; i--) kfree(slave->sii.strings[i]); fp@834: kfree(slave->sii.strings); fp@834: slave->sii.strings = NULL; fp@624: out_zero: fp@834: slave->sii.string_count = 0; fp@624: return -1; fp@118: } fp@118: fp@118: /*****************************************************************************/ fp@118: fp@118: /** fp@195: Fetches data from a GENERAL category. fp@195: \return 0 in case of success, else < 0 fp@195: */ fp@195: fp@742: int ec_slave_fetch_sii_general( fp@620: ec_slave_t *slave, /**< EtherCAT slave */ fp@742: const uint8_t *data, /**< category data */ fp@742: size_t data_size /**< size in bytes */ fp@620: ) fp@123: { fp@190: unsigned int i; fp@876: uint8_t flags; fp@190: fp@742: if (data_size != 32) { fp@742: EC_ERR("Wrong size of general category (%u/32) in slave %u.\n", fp@742: data_size, slave->ring_position); fp@742: return -1; fp@742: } fp@742: fp@834: slave->sii.group = ec_slave_sii_string(slave, data[0]); fp@834: slave->sii.image = ec_slave_sii_string(slave, data[1]); fp@834: slave->sii.order = ec_slave_sii_string(slave, data[2]); fp@834: slave->sii.name = ec_slave_sii_string(slave, data[3]); fp@123: fp@190: for (i = 0; i < 4; i++) fp@834: slave->sii.physical_layer[i] = fp@195: (data[4] & (0x03 << (i * 2))) >> (i * 2); fp@499: fp@876: // read CoE details fp@876: flags = EC_READ_U8(data + 5); fp@876: slave->sii.coe_details.enable_sdo = (flags >> 0) & 0x01; fp@876: slave->sii.coe_details.enable_sdo_info = (flags >> 1) & 0x01; fp@876: slave->sii.coe_details.enable_pdo_assign = (flags >> 2) & 0x01; fp@876: slave->sii.coe_details.enable_pdo_configuration = (flags >> 3) & 0x01; fp@876: slave->sii.coe_details.enable_upload_at_startup = (flags >> 4) & 0x01; fp@876: slave->sii.coe_details.enable_sdo_complete_access = (flags >> 5) & 0x01; fp@876: fp@876: // read general flags fp@876: flags = EC_READ_U8(data + 0x000B); fp@876: slave->sii.general_flags.enable_safeop = (flags >> 0) & 0x01; fp@876: slave->sii.general_flags.enable_not_lrw = (flags >> 1) & 0x01; fp@876: fp@834: slave->sii.current_on_ebus = EC_READ_S16(data + 0x0C); fp@835: slave->sii.has_general = 1; fp@742: return 0; fp@118: } fp@118: fp@118: /*****************************************************************************/ fp@118: fp@870: /** Fetches data from a SYNC MANAGER category. fp@870: * fp@870: * Appends the sync managers described in the category to the existing ones. fp@870: * fp@870: * \return 0 in case of success, else < 0 fp@870: */ fp@620: int ec_slave_fetch_sii_syncs( fp@870: ec_slave_t *slave, /**< EtherCAT slave. */ fp@870: const uint8_t *data, /**< Category data. */ fp@870: size_t data_size /**< Number of bytes. */ fp@870: ) fp@870: { fp@870: unsigned int i, count, total_count; fp@626: ec_sync_t *sync; fp@742: size_t memsize; fp@870: ec_sync_t *syncs; fp@870: uint8_t index; fp@742: fp@742: // one sync manager struct is 4 words long fp@742: if (data_size % 8) { fp@870: EC_ERR("Invalid SII sync manager category size %u in slave %u.\n", fp@742: data_size, slave->ring_position); fp@742: return -1; fp@742: } fp@742: fp@870: count = data_size / 8; fp@870: fp@870: if (count) { fp@870: total_count = count + slave->sii.sync_count; fp@870: memsize = sizeof(ec_sync_t) * total_count; fp@870: if (!(syncs = kmalloc(memsize, GFP_KERNEL))) { fp@870: EC_ERR("Failed to allocate %u bytes for sync managers.\n", fp@870: memsize); fp@870: return -1; fp@870: } fp@870: fp@873: for (i = 0; i < slave->sii.sync_count; i++) fp@873: ec_sync_init_copy(syncs + i, slave->sii.syncs + i); fp@870: fp@870: // initialize new sync managers fp@870: for (i = 0; i < count; i++, data += 8) { fp@870: index = i + slave->sii.sync_count; fp@870: sync = &syncs[index]; fp@870: fp@870: ec_sync_init(sync, slave, index); fp@870: sync->physical_start_address = EC_READ_U16(data); fp@870: sync->length = EC_READ_U16(data + 2); fp@870: sync->control_register = EC_READ_U8(data + 4); fp@870: sync->enable = EC_READ_U8(data + 6); fp@870: } fp@870: fp@870: if (slave->sii.syncs) fp@870: kfree(slave->sii.syncs); fp@870: slave->sii.syncs = syncs; fp@870: slave->sii.sync_count = total_count; fp@870: } fp@870: fp@126: return 0; fp@118: } fp@118: fp@118: /*****************************************************************************/ fp@118: fp@118: /** fp@814: Fetches data from a [RT]XPdo category. fp@195: \return 0 in case of success, else < 0 fp@195: */ fp@195: fp@620: int ec_slave_fetch_sii_pdos( fp@620: ec_slave_t *slave, /**< EtherCAT slave */ fp@620: const uint8_t *data, /**< category data */ fp@742: size_t data_size, /**< number of bytes */ fp@814: ec_direction_t dir /**< Pdo direction. */ fp@620: ) fp@126: { fp@625: ec_pdo_t *pdo; fp@625: ec_pdo_entry_t *entry; fp@126: unsigned int entry_count, i; fp@126: fp@742: while (data_size >= 8) { fp@625: if (!(pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { fp@814: EC_ERR("Failed to allocate Pdo memory.\n"); fp@126: return -1; fp@126: } fp@126: fp@627: ec_pdo_init(pdo); fp@792: pdo->dir = dir; fp@325: pdo->index = EC_READ_U16(data); fp@325: entry_count = EC_READ_U8(data + 2); fp@325: pdo->sync_index = EC_READ_U8(data + 3); fp@792: if (ec_pdo_set_name(pdo, fp@792: ec_slave_sii_string(slave, EC_READ_U8(data + 5)))) { fp@792: ec_pdo_clear(pdo); fp@792: kfree(pdo); fp@792: return -1; fp@792: } fp@834: list_add_tail(&pdo->list, &slave->sii.pdos); fp@126: fp@742: data_size -= 8; fp@126: data += 8; fp@126: fp@126: for (i = 0; i < entry_count; i++) { fp@625: if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { fp@814: EC_ERR("Failed to allocate Pdo entry memory.\n"); fp@126: return -1; fp@126: } fp@126: fp@792: ec_pdo_entry_init(entry); fp@325: entry->index = EC_READ_U16(data); fp@325: entry->subindex = EC_READ_U8(data + 2); fp@792: if (ec_pdo_entry_set_name(entry, fp@792: ec_slave_sii_string(slave, EC_READ_U8(data + 3)))) { fp@792: ec_pdo_entry_clear(entry); fp@792: kfree(entry); fp@792: return -1; fp@792: } fp@325: entry->bit_length = EC_READ_U8(data + 5); fp@126: list_add_tail(&entry->list, &pdo->entries); fp@126: fp@742: data_size -= 8; fp@126: data += 8; fp@126: } fp@635: fp@814: // if sync manager index is positive, the Pdo is mapped by default fp@635: if (pdo->sync_index >= 0) { fp@762: ec_sync_t *sync; fp@635: fp@834: if (pdo->sync_index >= slave->sii.sync_count) { fp@814: EC_ERR("Invalid SM index %i for Pdo 0x%04X in slave %u.", fp@635: pdo->sync_index, pdo->index, slave->ring_position); fp@635: return -1; fp@635: } fp@834: sync = &slave->sii.syncs[pdo->sync_index]; fp@635: fp@879: if (ec_pdo_list_add_pdo_copy(&sync->pdos, pdo)) fp@635: return -1; fp@792: fp@879: sync->assign_source = EC_ASSIGN_SII; fp@635: } fp@126: } fp@126: fp@126: return 0; fp@114: } fp@114: fp@114: /*****************************************************************************/ fp@114: fp@114: /** fp@784: Searches the string list for an index. fp@195: \return 0 in case of success, else < 0 fp@197: */ fp@197: fp@624: char *ec_slave_sii_string( fp@620: ec_slave_t *slave, /**< EtherCAT slave */ fp@624: unsigned int index /**< string index */ fp@620: ) fp@123: { fp@624: if (!index--) fp@624: return NULL; fp@624: fp@834: if (index >= slave->sii.string_count) { fp@624: if (slave->master->debug_level) fp@784: EC_WARN("String %u not found in slave %u.\n", fp@624: index, slave->ring_position); fp@624: return NULL; fp@624: } fp@624: fp@834: return slave->sii.strings[index]; fp@123: } fp@123: fp@123: /*****************************************************************************/ fp@123: fp@792: /** Outputs all information about a certain slave. fp@630: */ fp@772: ssize_t ec_slave_info(const ec_slave_t *slave, /**< EtherCAT slave */ fp@772: char *buffer /**< Output buffer */ fp@772: ) fp@772: { fp@871: const ec_sync_t *sync; fp@871: const ec_pdo_t *pdo; fp@871: const ec_pdo_entry_t *pdo_entry; fp@142: int first, i; fp@772: char *large_buffer, *buf; fp@772: unsigned int size; fp@772: fp@772: if (!(large_buffer = (char *) kmalloc(PAGE_SIZE * 2, GFP_KERNEL))) { fp@772: return -ENOMEM; fp@772: } fp@772: fp@772: buf = large_buffer; fp@772: fp@784: buf += sprintf(buf, "Ring position: %u\n", fp@763: slave->ring_position); fp@772: buf += sprintf(buf, "State: "); fp@772: buf += ec_state_string(slave->current_state, buf); fp@772: buf += sprintf(buf, " ("); fp@772: buf += ec_state_string(slave->requested_state, buf); fp@772: buf += sprintf(buf, ")\n"); fp@792: buf += sprintf(buf, "Flags: %s\n\n", slave->error_flag ? "ERROR" : "ok"); fp@325: fp@772: buf += sprintf(buf, "Data link status:\n"); fp@183: for (i = 0; i < 4; i++) { fp@772: buf += sprintf(buf, " Port %u: Phy %u (", fp@834: i, slave->sii.physical_layer[i]); fp@834: switch (slave->sii.physical_layer[i]) { fp@190: case 0x00: fp@772: buf += sprintf(buf, "EBUS"); fp@190: break; fp@190: case 0x01: fp@772: buf += sprintf(buf, "100BASE-TX"); fp@190: break; fp@190: case 0x02: fp@772: buf += sprintf(buf, "100BASE-FX"); fp@190: break; fp@190: default: fp@772: buf += sprintf(buf, "unknown"); fp@772: } fp@772: buf += sprintf(buf, "), Link %s, Loop %s, %s\n", fp@325: slave->dl_link[i] ? "up" : "down", fp@325: slave->dl_loop[i] ? "closed" : "open", fp@325: slave->dl_signal[i] ? "Signal detected" : "No signal"); fp@325: } fp@772: buf += sprintf(buf, "\n"); fp@763: fp@834: if (slave->sii.alias) fp@772: buf += sprintf(buf, "Configured station alias:" fp@834: " 0x%04X (%u)\n\n", slave->sii.alias, slave->sii.alias); fp@763: fp@772: buf += sprintf(buf, "Identity:\n"); fp@772: buf += sprintf(buf, " Vendor ID: 0x%08X (%u)\n", fp@834: slave->sii.vendor_id, slave->sii.vendor_id); fp@772: buf += sprintf(buf, " Product code: 0x%08X (%u)\n", fp@834: slave->sii.product_code, slave->sii.product_code); fp@772: buf += sprintf(buf, " Revision number: 0x%08X (%u)\n", fp@834: slave->sii.revision_number, slave->sii.revision_number); fp@772: buf += sprintf(buf, " Serial number: 0x%08X (%u)\n\n", fp@834: slave->sii.serial_number, slave->sii.serial_number); fp@834: fp@834: if (slave->sii.mailbox_protocols) { fp@772: buf += sprintf(buf, "Mailboxes:\n"); fp@772: buf += sprintf(buf, " RX: 0x%04X/%u, TX: 0x%04X/%u\n", fp@834: slave->sii.rx_mailbox_offset, slave->sii.rx_mailbox_size, fp@834: slave->sii.tx_mailbox_offset, slave->sii.tx_mailbox_size); fp@772: buf += sprintf(buf, " Supported protocols: "); fp@147: fp@147: first = 1; fp@834: if (slave->sii.mailbox_protocols & EC_MBOX_AOE) { fp@772: buf += sprintf(buf, "AoE"); fp@147: first = 0; fp@147: } fp@834: if (slave->sii.mailbox_protocols & EC_MBOX_EOE) { fp@772: if (!first) buf += sprintf(buf, ", "); fp@772: buf += sprintf(buf, "EoE"); fp@147: first = 0; fp@147: } fp@834: if (slave->sii.mailbox_protocols & EC_MBOX_COE) { fp@772: if (!first) buf += sprintf(buf, ", "); fp@772: buf += sprintf(buf, "CoE"); fp@147: first = 0; fp@147: } fp@834: if (slave->sii.mailbox_protocols & EC_MBOX_FOE) { fp@772: if (!first) buf += sprintf(buf, ", "); fp@772: buf += sprintf(buf, "FoE"); fp@147: first = 0; fp@147: } fp@834: if (slave->sii.mailbox_protocols & EC_MBOX_SOE) { fp@772: if (!first) buf += sprintf(buf, ", "); fp@772: buf += sprintf(buf, "SoE"); fp@147: first = 0; fp@147: } fp@834: if (slave->sii.mailbox_protocols & EC_MBOX_VOE) { fp@772: if (!first) buf += sprintf(buf, ", "); fp@772: buf += sprintf(buf, "VoE"); fp@772: } fp@772: buf += sprintf(buf, "\n\n"); fp@772: } fp@772: fp@835: if (slave->sii.has_general) { fp@772: buf += sprintf(buf, "General:\n"); fp@763: fp@834: if (slave->sii.group) fp@834: buf += sprintf(buf, " Group: %s\n", slave->sii.group); fp@834: if (slave->sii.image) fp@834: buf += sprintf(buf, " Image: %s\n", slave->sii.image); fp@834: if (slave->sii.order) fp@772: buf += sprintf(buf, " Order number: %s\n", fp@834: slave->sii.order); fp@834: if (slave->sii.name) fp@834: buf += sprintf(buf, " Name: %s\n", slave->sii.name); fp@835: if (slave->sii.mailbox_protocols & EC_MBOX_COE) { fp@835: buf += sprintf(buf, " CoE details:\n"); fp@835: buf += sprintf(buf, " Enable Sdo: %s\n", fp@835: slave->sii.coe_details.enable_sdo ? "yes" : "no"); fp@835: buf += sprintf(buf, " Enable Sdo Info: %s\n", fp@835: slave->sii.coe_details.enable_sdo_info ? "yes" : "no"); fp@835: buf += sprintf(buf, " Enable Pdo Assign: %s\n", fp@835: slave->sii.coe_details.enable_pdo_assign ? "yes" : "no"); fp@835: buf += sprintf(buf, " Enable Pdo Configuration: %s\n", fp@835: slave->sii.coe_details.enable_pdo_configuration ? fp@835: "yes" : "no"); fp@835: buf += sprintf(buf, " Enable Upload at startup: %s\n", fp@835: slave->sii.coe_details.enable_upload_at_startup ? fp@835: "yes" : "no"); fp@835: buf += sprintf(buf, " Enable Sdo complete access: %s\n", fp@835: slave->sii.coe_details.enable_sdo_complete_access fp@835: ? "yes" : "no"); fp@835: } fp@835: fp@836: buf += sprintf(buf, " Flags:\n"); fp@836: buf += sprintf(buf, " Enable SafeOp: %s\n", fp@836: slave->sii.general_flags.enable_safeop ? "yes" : "no"); fp@836: buf += sprintf(buf, " Enable notLRW: %s\n", fp@836: slave->sii.general_flags.enable_not_lrw ? "yes" : "no"); fp@835: buf += sprintf(buf, " Current consumption: %i mA\n\n", fp@835: slave->sii.current_on_ebus); fp@325: } fp@325: fp@834: if (slave->sii.sync_count) { fp@879: buf += sprintf(buf, "Sync managers / assigned Pdos:\n"); fp@763: fp@834: for (i = 0; i < slave->sii.sync_count; i++) { fp@834: sync = &slave->sii.syncs[i]; fp@772: buf += sprintf(buf, fp@784: " SM%u: addr 0x%04X, size %u, control 0x%02X, %s\n", fp@763: sync->index, sync->physical_start_address, fp@792: sync->length, sync->control_register, fp@763: sync->enable ? "enable" : "disable"); fp@763: fp@879: if (list_empty(&sync->pdos.list)) { fp@879: buf += sprintf(buf, " No Pdos assigned.\n"); fp@879: } else if (sync->assign_source != EC_ASSIGN_NONE) { fp@879: buf += sprintf(buf, " Pdo assignment from "); fp@879: switch (sync->assign_source) { fp@879: case EC_ASSIGN_SII: fp@879: buf += sprintf(buf, "SII"); fp@879: break; fp@879: case EC_ASSIGN_COE: fp@879: buf += sprintf(buf, "CoE"); fp@879: break; fp@879: case EC_ASSIGN_CUSTOM: fp@879: buf += sprintf(buf, "application"); fp@879: break; fp@879: default: fp@879: buf += sprintf(buf, "?"); fp@879: break; fp@879: } fp@879: buf += sprintf(buf, ".\n"); fp@763: } fp@763: fp@879: list_for_each_entry(pdo, &sync->pdos.list, list) { fp@772: buf += sprintf(buf, " %s 0x%04X \"%s\"\n", fp@792: pdo->dir == EC_DIR_OUTPUT ? "RxPdo" : "TxPdo", fp@763: pdo->index, pdo->name ? pdo->name : "???"); fp@763: fp@763: list_for_each_entry(pdo_entry, &pdo->entries, list) { fp@772: buf += sprintf(buf, fp@784: " 0x%04X:%X \"%s\", %u bit\n", fp@763: pdo_entry->index, pdo_entry->subindex, fp@763: pdo_entry->name ? pdo_entry->name : "???", fp@763: pdo_entry->bit_length); fp@763: } fp@763: } fp@763: } fp@772: buf += sprintf(buf, "\n"); fp@763: } fp@763: fp@763: // type-cast to avoid warnings on some compilers fp@834: if (!list_empty((struct list_head *) &slave->sii.pdos)) { fp@814: buf += sprintf(buf, "Available Pdos from SII:\n"); fp@763: fp@834: list_for_each_entry(pdo, &slave->sii.pdos, list) { fp@772: buf += sprintf(buf, " %s 0x%04X \"%s\"", fp@792: pdo->dir == EC_DIR_OUTPUT ? "RxPdo" : "TxPdo", fp@635: pdo->index, pdo->name ? pdo->name : "???"); fp@763: if (pdo->sync_index >= 0) fp@879: buf += sprintf(buf, ", default assignment: SM%u.\n", fp@763: pdo->sync_index); fp@763: else fp@879: buf += sprintf(buf, ", no default assignment.\n"); fp@635: fp@635: list_for_each_entry(pdo_entry, &pdo->entries, list) { fp@784: buf += sprintf(buf, " 0x%04X:%X \"%s\", %u bit\n", fp@635: pdo_entry->index, pdo_entry->subindex, fp@635: pdo_entry->name ? pdo_entry->name : "???", fp@635: pdo_entry->bit_length); fp@635: } fp@635: } fp@772: buf += sprintf(buf, "\n"); fp@325: } fp@325: fp@772: size = buf - large_buffer; fp@772: if (size >= PAGE_SIZE) { fp@772: const char trunc[] = "\n---TRUNCATED---\n"; fp@772: unsigned int len = strlen(trunc); fp@772: memcpy(large_buffer + PAGE_SIZE - len, trunc, len); fp@772: } fp@772: fp@772: size = min(size, (unsigned int) PAGE_SIZE); fp@772: memcpy(buffer, large_buffer, size); fp@772: kfree(large_buffer); fp@772: return size; fp@325: } fp@325: fp@325: /*****************************************************************************/ fp@325: fp@133: /** fp@872: * Schedules an SII write request. fp@607: * \return 0 case of success, otherwise error code. fp@607: */ fp@607: fp@872: int ec_slave_schedule_sii_writing( fp@872: ec_sii_write_request_t *request /**< SII write request */ fp@758: ) fp@607: { fp@607: ec_master_t *master = request->slave->master; fp@607: fp@649: request->state = EC_REQUEST_QUEUED; fp@607: fp@872: // schedule SII write request. fp@872: down(&master->sii_sem); fp@872: list_add_tail(&request->list, &master->sii_requests); fp@872: up(&master->sii_sem); fp@607: fp@607: // wait for processing through FSM fp@872: if (wait_event_interruptible(master->sii_queue, fp@649: request->state != EC_REQUEST_QUEUED)) { fp@607: // interrupted by signal fp@872: down(&master->sii_sem); fp@649: if (request->state == EC_REQUEST_QUEUED) { fp@607: list_del(&request->list); fp@872: up(&master->sii_sem); fp@607: return -EINTR; fp@607: } fp@607: // request already processing: interrupt not possible. fp@872: up(&master->sii_sem); fp@607: } fp@607: fp@607: // wait until master FSM has finished processing fp@872: wait_event(master->sii_queue, fp@861: request->state != EC_REQUEST_BUSY); fp@861: fp@861: return request->state == EC_REQUEST_SUCCESS ? 0 : -EIO; fp@607: } fp@607: fp@607: /*****************************************************************************/ fp@607: fp@607: /** fp@872: * Calculates the SII checksum field. fp@735: * fp@735: * The checksum is generated with the polynom x^8+x^2+x+1 (0x07) and an fp@735: * initial value of 0xff (see IEC 61158-6-12 ch. 5.4). fp@735: * fp@735: * The below code was originally generated with PYCRC fp@735: * http://www.tty1.net/pycrc fp@735: * fp@735: * ./pycrc.py --width=8 --poly=0x07 --reflect-in=0 --xor-in=0xff fp@735: * --reflect-out=0 --xor-out=0 --generate c --algorithm=bit-by-bit fp@735: * fp@735: * \return CRC8 fp@735: */ fp@735: fp@872: uint8_t ec_slave_sii_crc( fp@758: const uint8_t *data, /**< pointer to data */ fp@758: size_t length /**< number of bytes in \a data */ fp@758: ) fp@735: { fp@735: unsigned int i; fp@735: uint8_t bit, byte, crc = 0x48; fp@735: fp@735: while (length--) { fp@735: byte = *data++; fp@735: for (i = 0; i < 8; i++) { fp@735: bit = crc & 0x80; fp@735: crc = (crc << 1) | ((byte >> (7 - i)) & 0x01); fp@735: if (bit) crc ^= 0x07; fp@735: } fp@735: } fp@735: fp@735: for (i = 0; i < 8; i++) { fp@735: bit = crc & 0x80; fp@735: crc <<= 1; fp@735: if (bit) crc ^= 0x07; fp@735: } fp@735: fp@735: return crc; fp@735: } fp@735: fp@735: /*****************************************************************************/ fp@735: fp@735: /** fp@872: * Writes complete SII contents to a slave. fp@607: * \return data size written in case of success, otherwise error code. fp@607: */ fp@269: fp@872: ssize_t ec_slave_write_sii(ec_slave_t *slave, /**< EtherCAT slave */ fp@872: const uint8_t *data, /**< new SII data */ fp@607: size_t size /**< size of data in bytes */ fp@607: ) fp@269: { fp@872: ec_sii_write_request_t request; fp@601: const uint16_t *cat_header; fp@601: uint16_t cat_type, cat_size; fp@607: int ret; fp@735: uint8_t crc; fp@601: fp@601: if (slave->master->mode != EC_MASTER_MODE_IDLE) { // FIXME fp@872: EC_ERR("Writing SIIs only allowed in idle mode!\n"); fp@599: return -EBUSY; fp@269: } fp@269: fp@269: if (size % 2) { fp@872: EC_ERR("SII data size is odd (%u bytes)! SII data must be" fp@856: " word-aligned. Dropping.\n", size); fp@599: return -EINVAL; fp@269: } fp@269: fp@872: // init SII write request fp@601: INIT_LIST_HEAD(&request.list); fp@601: request.slave = slave; fp@754: request.data = data; fp@754: request.word_offset = 0; fp@754: request.word_size = size / 2; fp@754: fp@754: if (request.word_size < 0x0041) { fp@872: EC_ERR("SII data too short (%u words)! Mimimum is" fp@856: " 40 fixed words + 1 delimiter. Dropping.\n", fp@856: request.word_size); fp@599: return -EINVAL; fp@269: } fp@269: fp@735: // calculate checksum fp@872: crc = ec_slave_sii_crc(data, 14); // CRC over words 0 to 6 fp@735: if (crc != data[14]) { fp@872: EC_WARN("SII CRC incorrect. Must be 0x%02x.\n", crc); fp@735: } fp@735: fp@754: cat_header = (const uint16_t *) request.data fp@872: + EC_FIRST_SII_CATEGORY_OFFSET; fp@601: cat_type = EC_READ_U16(cat_header); fp@601: while (cat_type != 0xFFFF) { // cycle through categories fp@754: if (cat_header + 1 > fp@754: (const uint16_t *) request.data + request.word_size) { fp@872: EC_ERR("SII data corrupted! Dropping.\n"); fp@599: return -EINVAL; fp@269: } fp@601: cat_size = EC_READ_U16(cat_header + 1); fp@754: if (cat_header + cat_size + 2 > fp@754: (const uint16_t *) request.data + request.word_size) { fp@872: EC_ERR("SII data corrupted! Dropping.\n"); fp@601: return -EINVAL; fp@601: } fp@601: cat_header += cat_size + 2; fp@601: cat_type = EC_READ_U16(cat_header); fp@601: } fp@601: fp@872: // SII data ok. schedule writing. fp@872: if ((ret = ec_slave_schedule_sii_writing(&request))) fp@607: return ret; // error code fp@607: fp@607: return size; // success fp@607: } fp@607: fp@607: /*****************************************************************************/ fp@607: fp@607: /** fp@872: * Writes the Secondary slave address (alias) to the slave's SII. fp@607: * \return data size written in case of success, otherwise error code. fp@607: */ fp@607: fp@607: ssize_t ec_slave_write_alias(ec_slave_t *slave, /**< EtherCAT slave */ fp@607: const uint8_t *data, /**< alias string */ fp@607: size_t size /**< size of data in bytes */ fp@607: ) fp@607: { fp@872: ec_sii_write_request_t request; fp@607: char *remainder; fp@754: uint16_t alias; fp@607: int ret; fp@872: uint8_t sii_data[16], crc; fp@607: fp@607: if (slave->master->mode != EC_MASTER_MODE_IDLE) { // FIXME fp@872: EC_ERR("Writing to SII is only allowed in idle mode!\n"); fp@607: return -EBUSY; fp@607: } fp@607: fp@607: alias = simple_strtoul(data, &remainder, 0); fp@607: if (remainder == (char *) data || (*remainder && *remainder != '\n')) { fp@607: EC_ERR("Invalid alias value! Dropping.\n"); fp@607: return -EINVAL; fp@607: } fp@735: fp@872: if (!slave->sii_data || slave->sii_size < 16) { fp@872: EC_ERR("Failed to read SII contents from slave %u.\n", fp@735: slave->ring_position); fp@735: return -EINVAL; fp@735: } fp@735: fp@872: // copy first 7 words of recent SII contents fp@872: memcpy(sii_data, slave->sii_data, 14); fp@607: fp@754: // write new alias address in word 4 fp@872: EC_WRITE_U16(sii_data + 8, alias); fp@735: fp@735: // calculate new checksum over words 0 to 6 fp@872: crc = ec_slave_sii_crc(sii_data, 14); fp@872: EC_WRITE_U16(sii_data + 14, crc); fp@872: fp@872: // init SII write request fp@607: INIT_LIST_HEAD(&request.list); fp@607: request.slave = slave; fp@872: request.data = sii_data; fp@754: request.word_offset = 0x0000; fp@754: request.word_size = 8; fp@607: fp@872: if ((ret = ec_slave_schedule_sii_writing(&request))) fp@607: return ret; // error code fp@607: fp@834: slave->sii.alias = alias; // FIXME: do this in state machine fp@607: fp@607: return size; // success fp@607: } fp@607: fp@269: /*****************************************************************************/ fp@269: fp@269: /** fp@195: Formats attribute data for SysFS read access. fp@195: \return number of bytes to read fp@195: */ fp@195: fp@195: ssize_t ec_show_slave_attribute(struct kobject *kobj, /**< slave's kobject */ fp@195: struct attribute *attr, /**< attribute */ fp@195: char *buffer /**< memory to store data */ fp@182: ) fp@182: { fp@182: ec_slave_t *slave = container_of(kobj, ec_slave_t, kobj); fp@182: fp@325: if (attr == &attr_info) { fp@325: return ec_slave_info(slave, buffer); fp@185: } fp@256: else if (attr == &attr_state) { fp@256: switch (slave->current_state) { fp@256: case EC_SLAVE_STATE_INIT: fp@256: return sprintf(buffer, "INIT\n"); fp@256: case EC_SLAVE_STATE_PREOP: fp@256: return sprintf(buffer, "PREOP\n"); fp@813: case EC_SLAVE_STATE_SAFEOP: fp@813: return sprintf(buffer, "SAFEOP\n"); fp@256: case EC_SLAVE_STATE_OP: fp@256: return sprintf(buffer, "OP\n"); fp@256: default: fp@256: return sprintf(buffer, "UNKNOWN\n"); fp@256: } fp@256: } fp@872: else if (attr == &attr_sii) { fp@872: if (slave->sii_data) { fp@872: if (slave->sii_size > PAGE_SIZE) { fp@872: EC_ERR("SII contents of slave %u exceed 1 page (%u/%u).\n", fp@872: slave->ring_position, slave->sii_size, fp@266: (int) PAGE_SIZE); fp@266: } fp@266: else { fp@872: memcpy(buffer, slave->sii_data, slave->sii_size); fp@872: return slave->sii_size; fp@266: } fp@266: } fp@266: } fp@607: else if (attr == &attr_alias) { fp@834: return sprintf(buffer, "%u\n", slave->sii.alias); fp@607: } fp@182: fp@182: return 0; fp@182: } fp@182: fp@256: /*****************************************************************************/ fp@256: fp@256: /** fp@256: Formats attribute data for SysFS write access. fp@256: \return number of bytes processed, or negative error code fp@256: */ fp@256: fp@256: ssize_t ec_store_slave_attribute(struct kobject *kobj, /**< slave's kobject */ fp@256: struct attribute *attr, /**< attribute */ fp@256: const char *buffer, /**< memory with data */ fp@256: size_t size /**< size of data to store */ fp@256: ) fp@256: { fp@256: ec_slave_t *slave = container_of(kobj, ec_slave_t, kobj); fp@256: fp@256: if (attr == &attr_state) { fp@403: char state[EC_STATE_STRING_SIZE]; fp@292: if (!strcmp(buffer, "INIT\n")) fp@446: ec_slave_request_state(slave, EC_SLAVE_STATE_INIT); fp@292: else if (!strcmp(buffer, "PREOP\n")) fp@446: ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP); fp@813: else if (!strcmp(buffer, "SAFEOP\n")) fp@813: ec_slave_request_state(slave, EC_SLAVE_STATE_SAFEOP); fp@292: else if (!strcmp(buffer, "OP\n")) fp@446: ec_slave_request_state(slave, EC_SLAVE_STATE_OP); fp@292: else { fp@292: EC_ERR("Invalid slave state \"%s\"!\n", buffer); fp@292: return -EINVAL; fp@292: } fp@292: fp@333: ec_state_string(slave->requested_state, state); fp@784: EC_INFO("Accepted new state %s for slave %u.\n", fp@333: state, slave->ring_position); fp@292: return size; fp@256: } fp@872: else if (attr == &attr_sii) { fp@872: return ec_slave_write_sii(slave, buffer, size); fp@269: } fp@607: else if (attr == &attr_alias) { fp@607: return ec_slave_write_alias(slave, buffer, size); fp@607: } fp@607: fp@607: return -EIO; fp@256: } fp@256: fp@275: /*****************************************************************************/ fp@275: fp@912: /** Get the sync manager for either Rx- or Tx-Pdos. fp@912: * fp@912: * \todo This seems not to be correct in every case... fp@758: * \return pointer to sync manager, or NULL. fp@619: */ fp@626: ec_sync_t *ec_slave_get_pdo_sync( fp@912: ec_slave_t *slave, /**< EtherCAT slave. */ fp@912: ec_direction_t dir /**< Input or output. */ fp@619: ) fp@619: { fp@619: unsigned int sync_index; fp@619: fp@635: if (dir != EC_DIR_INPUT && dir != EC_DIR_OUTPUT) { fp@635: EC_ERR("Invalid direction!\n"); fp@635: return NULL; fp@635: } fp@635: fp@912: if (slave->sii.sync_count != 1) { fp@912: sync_index = (unsigned int) dir; fp@912: if (slave->sii.mailbox_protocols) sync_index += 2; fp@912: fp@912: if (sync_index >= slave->sii.sync_count) fp@912: return NULL; fp@912: } else { // sync_count == 1 fp@912: // A single sync manager may be used for inputs OR outputs! fp@912: if (ec_sync_direction(&slave->sii.syncs[0]) != dir) fp@912: return NULL; fp@912: sync_index = 0; fp@912: } fp@619: fp@834: return &slave->sii.syncs[sync_index]; fp@619: } fp@619: fp@619: /*****************************************************************************/ fp@619: fp@619: /** fp@329: \return 0 in case of success, else < 0 fp@329: */ fp@329: fp@410: int ec_slave_validate(const ec_slave_t *slave, /**< EtherCAT slave */ fp@410: uint32_t vendor_id, /**< vendor ID */ fp@410: uint32_t product_code /**< product code */ fp@410: ) fp@410: { fp@834: if (vendor_id != slave->sii.vendor_id || fp@834: product_code != slave->sii.product_code) { fp@784: EC_ERR("Invalid slave type at position %u:\n", slave->ring_position); fp@643: EC_ERR(" Requested: 0x%08X 0x%08X\n", vendor_id, product_code); fp@643: EC_ERR(" Found: 0x%08X 0x%08X\n", fp@834: slave->sii.vendor_id, slave->sii.product_code); fp@410: return -1; fp@410: } fp@410: return 0; fp@410: } fp@410: fp@423: /*****************************************************************************/ fp@423: fp@423: /** fp@814: Counts the total number of Sdos and entries in the dictionary. fp@423: */ fp@423: fp@423: void ec_slave_sdo_dict_info(const ec_slave_t *slave, /**< EtherCAT slave */ fp@814: unsigned int *sdo_count, /**< number of Sdos */ fp@423: unsigned int *entry_count /**< total number of fp@423: entries */ fp@423: ) fp@423: { fp@423: unsigned int sdos = 0, entries = 0; fp@423: ec_sdo_t *sdo; fp@423: ec_sdo_entry_t *entry; fp@423: fp@423: list_for_each_entry(sdo, &slave->sdo_dictionary, list) { fp@423: sdos++; fp@423: list_for_each_entry(entry, &sdo->entries, list) { fp@423: entries++; fp@423: } fp@423: } fp@423: fp@423: *sdo_count = sdos; fp@423: *entry_count = entries; fp@423: } fp@423: fp@740: /*****************************************************************************/ fp@740: fp@740: /** fp@814: * Get an Sdo from the dictionary. fp@814: * \returns The desired Sdo, or NULL. fp@740: */ fp@740: fp@740: ec_sdo_t *ec_slave_get_sdo( fp@740: ec_slave_t *slave /**< EtherCAT slave */, fp@814: uint16_t index /**< Sdo index */ fp@740: ) fp@740: { fp@740: ec_sdo_t *sdo; fp@740: fp@740: list_for_each_entry(sdo, &slave->sdo_dictionary, list) { fp@740: if (sdo->index != index) continue; fp@740: return sdo; fp@740: } fp@740: fp@740: return NULL; fp@740: } fp@740: fp@792: /*****************************************************************************/ fp@799: fp@799: /** Finds a mapped Pdo. fp@799: * \returns The desired Pdo object, or NULL. fp@799: */ fp@799: const ec_pdo_t *ec_slave_find_pdo( fp@799: const ec_slave_t *slave, /**< Slave. */ fp@799: uint16_t index /**< Pdo index to find. */ fp@799: ) fp@799: { fp@799: unsigned int i; fp@799: const ec_sync_t *sync; fp@799: const ec_pdo_t *pdo; fp@799: fp@834: for (i = 0; i < slave->sii.sync_count; i++) { fp@834: sync = &slave->sii.syncs[i]; fp@799: fp@879: if (!(pdo = ec_pdo_list_find_pdo_const(&sync->pdos, index))) fp@799: continue; fp@799: fp@799: return pdo; fp@799: } fp@799: fp@799: return NULL; fp@799: } fp@799: fp@799: /*****************************************************************************/