fp@39: /******************************************************************************
fp@0:  *
fp@39:  *  $Id$
fp@0:  *
fp@1326:  *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH
fp@197:  *
fp@197:  *  This file is part of the IgH EtherCAT Master.
fp@197:  *
fp@1326:  *  The IgH EtherCAT Master is free software; you can redistribute it and/or
fp@1326:  *  modify it under the terms of the GNU General Public License version 2, as
fp@1326:  *  published by the Free Software Foundation.
fp@1326:  *
fp@1326:  *  The IgH EtherCAT Master is distributed in the hope that it will be useful,
fp@1326:  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
fp@1326:  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
fp@1326:  *  Public License for more details.
fp@1326:  *
fp@1326:  *  You should have received a copy of the GNU General Public License along
fp@1326:  *  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@1363:  *  ---
fp@1363:  *
fp@1363:  *  The license mentioned above concerns the source code only. Using the
fp@1363:  *  EtherCAT technology and brand is only permitted in compliance with the
fp@1363:  *  industrial property and similar rights of Beckhoff Automation GmbH.
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 <linux/module.h>
fp@73: #include <linux/delay.h>
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@624: char *ec_slave_sii_string(ec_slave_t *, unsigned int);
fp@182: 
fp@182: /*****************************************************************************/
fp@182: 
fp@0: /**
fp@195:    Slave constructor.
fp@195:    \return 0 in case of success, else < 0
fp@195: */
fp@195: 
fp@992: void ec_slave_init(
fp@992:         ec_slave_t *slave, /**< EtherCAT slave */
fp@992:         ec_master_t *master, /**< EtherCAT master */
fp@992:         uint16_t ring_position, /**< ring position */
fp@992:         uint16_t station_address /**< station address to configure */
fp@992:         )
fp@73: {
fp@142:     unsigned int i;
martin@1583:     int ret;
fp@142: 
fp@1000:     slave->master = master;
fp@182:     slave->ring_position = ring_position;
fp@182:     slave->station_address = station_address;
fp@1909:     slave->effective_alias = 0x0000;
fp@182: 
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@1338:     slave->configured_rx_mailbox_offset = 0x0000;
fp@1338:     slave->configured_rx_mailbox_size = 0x0000;
fp@1338:     slave->configured_tx_mailbox_offset = 0x0000;
fp@1338:     slave->configured_tx_mailbox_size = 0x0000;
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@1379:     slave->base_sync_count = 0;
fp@325: 
fp@1055:     for (i = 0; i < EC_MAX_PORTS; i++) {
fp@1425:         slave->ports[i].desc = EC_PORT_NOT_IMPLEMENTED;
fp@1425: 
fp@1425:         slave->ports[i].link.link_up = 0;
fp@1425:         slave->ports[i].link.loop_closed = 0;
fp@1425:         slave->ports[i].link.signal_detected = 0;
fp@1000:         slave->sii.physical_layer[i] = 0xFF;
fp@1420: 
fp@1425:         slave->ports[i].receive_time = 0U;
fp@1425: 
fp@1425:         slave->ports[i].next_slave = NULL;
fp@1425:         slave->ports[i].delay_to_next_dc = 0U;
fp@1000:     }
fp@1000: 
fp@1379:     slave->base_fmmu_bit_operation = 0;
fp@1379:     slave->base_dc_supported = 0;
fp@1379:     slave->base_dc_range = EC_DC_32;
fp@1419:     slave->has_dc_system_time = 0;
fp@1426:     slave->transmission_delay = 0U;
fp@1379: 
fp@977:     slave->sii_words = NULL;
fp@977:     slave->sii_nwords = 0;
fp@325: 
fp@1337:     slave->sii.alias = 0x0000;
fp@1337:     slave->sii.vendor_id = 0x00000000;
fp@1337:     slave->sii.product_code = 0x00000000;
fp@1337:     slave->sii.revision_number = 0x00000000;
fp@1337:     slave->sii.serial_number = 0x00000000;
fp@1337:     slave->sii.boot_rx_mailbox_offset = 0x0000;
fp@1337:     slave->sii.boot_rx_mailbox_size = 0x0000;
fp@1337:     slave->sii.boot_tx_mailbox_offset = 0x0000;
fp@1337:     slave->sii.boot_tx_mailbox_size = 0x0000;
fp@1337:     slave->sii.std_rx_mailbox_offset = 0x0000;
fp@1337:     slave->sii.std_rx_mailbox_size = 0x0000;
fp@1337:     slave->sii.std_tx_mailbox_offset = 0x0000;
fp@1337:     slave->sii.std_tx_mailbox_size = 0x0000;
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;
martin@1583: 
martin@1596:     INIT_LIST_HEAD(&slave->slave_sdo_requests);
martin@1596:     init_waitqueue_head(&slave->sdo_queue);
martin@1596: 
martin@1597:     INIT_LIST_HEAD(&slave->foe_requests);
martin@1597:     init_waitqueue_head(&slave->foe_queue);
martin@1597: 
fp@1831:     INIT_LIST_HEAD(&slave->soe_requests);
fp@1831:     init_waitqueue_head(&slave->soe_queue);
fp@1831: 
martin@1583:     // init state machine datagram
martin@1583:     ec_datagram_init(&slave->fsm_datagram);
fp@1921:     snprintf(slave->fsm_datagram.name, EC_DATAGRAM_NAME_SIZE,
fp@1921:             "slave%u-fsm", slave->ring_position);
martin@1583:     ret = ec_datagram_prealloc(&slave->fsm_datagram, EC_MAX_DATA_SIZE);
martin@1583:     if (ret < 0) {
martin@1583:         ec_datagram_clear(&slave->fsm_datagram);
fp@1921:         EC_SLAVE_ERR(slave, "Failed to allocate FSM datagram.\n");
martin@1583:         return;
martin@1583:     }
martin@1583: 
martin@1583:     // create state machine object
martin@1583:     ec_fsm_slave_init(&slave->fsm, slave, &slave->fsm_datagram);
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@992: void ec_slave_clear(ec_slave_t *slave /**< EtherCAT slave */)
fp@448: {
fp@448:     ec_sdo_t *sdo, *next_sdo;
fp@992:     unsigned int i;
fp@992:     ec_pdo_t *pdo, *next_pdo;
fp@448: 
fp@1921:     // abort all pending requests
fp@1921: 
fp@1921:     while (!list_empty(&slave->slave_sdo_requests)) {
fp@1921:         ec_master_sdo_request_t *request =
fp@1921:             list_entry(slave->slave_sdo_requests.next,
fp@1921:                 ec_master_sdo_request_t, list);
fp@1921:         list_del_init(&request->list); // dequeue
ch1010277@2027:         EC_SLAVE_WARN(slave, "Discarding SDO request %p,"
ch1010277@2027:                 " slave about to be deleted.\n",request);
fp@1921:         request->req.state = EC_INT_REQUEST_FAILURE;
ch1010277@2027:         kref_put(&request->refcount,ec_master_sdo_request_release);
fp@1921:         wake_up(&slave->sdo_queue);
fp@1921:     }
fp@1921: 
fp@1921:     while (!list_empty(&slave->foe_requests)) {
fp@1921:         ec_master_foe_request_t *request =
fp@1921:             list_entry(slave->foe_requests.next,
fp@1921:                 ec_master_foe_request_t, list);
fp@1921:         list_del_init(&request->list); // dequeue
fp@1921:         EC_SLAVE_WARN(slave, "Discarding FoE request,"
fp@1921:                 " slave about to be deleted.\n");
fp@1921:         request->req.state = EC_INT_REQUEST_FAILURE;
fp@1921:         wake_up(&slave->foe_queue);
fp@1921:     }
fp@1921: 
fp@1921:     while (!list_empty(&slave->soe_requests)) {
fp@1921:         ec_master_soe_request_t *request =
fp@1921:             list_entry(slave->soe_requests.next,
fp@1921:                 ec_master_soe_request_t, list);
fp@1921:         list_del_init(&request->list); // dequeue
fp@1921:         EC_SLAVE_WARN(slave, "Discarding SoE request,"
fp@1921:                 " slave about to be deleted.\n");
fp@1921:         request->req.state = EC_INT_REQUEST_FAILURE;
fp@1921:         wake_up(&slave->soe_queue);
fp@1921:     }
fp@1921: 
fp@792:     if (slave->config)
fp@792:         ec_slave_config_detach(slave->config);
fp@792: 
fp@1327:     // free all SDOs
fp@448:     list_for_each_entry_safe(sdo, next_sdo, &slave->sdo_dictionary, list) {
fp@448:         list_del(&sdo->list);
fp@992:         ec_sdo_clear(sdo);
fp@992:         kfree(sdo);
fp@992:     }
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@1327:     // 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@977:     if (slave->sii_words)
fp@977:         kfree(slave->sii_words);
martin@1583:     ec_fsm_slave_clear(&slave->fsm);
martin@1583:     ec_datagram_clear(&slave->fsm_datagram);
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@1337:             ec_state_string(slave->current_state, old_state, 0);
fp@1337:             ec_state_string(new_state, cur_state, 0);
fp@1921:             EC_SLAVE_DBG(slave, 0, "%s -> %s.\n", 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@1313:     int i, err;
fp@118:     size_t size;
fp@118:     off_t offset;
fp@624: 
fp@834:     slave->sii.string_count = data[0];
fp@834: 
fp@1313:     if (slave->sii.string_count) {
fp@1313:         if (!(slave->sii.strings =
fp@1313:                     kmalloc(sizeof(char *) * slave->sii.string_count,
fp@1313:                         GFP_KERNEL))) {
fp@1921:             EC_SLAVE_ERR(slave, "Failed to allocate string array memory.\n");
fp@1313:             err = -ENOMEM;
fp@1313:             goto out_zero;
fp@1313:         }
fp@1313: 
fp@1313:         offset = 1;
fp@1313:         for (i = 0; i < slave->sii.string_count; i++) {
fp@1313:             size = data[offset];
fp@1313:             // allocate memory for string structure and data at a single blow
fp@1313:             if (!(slave->sii.strings[i] =
fp@1313:                         kmalloc(sizeof(char) * size + 1, GFP_KERNEL))) {
fp@1921:                 EC_SLAVE_ERR(slave, "Failed to allocate string memory.\n");
fp@1313:                 err = -ENOMEM;
fp@1313:                 goto out_free;
fp@1313:             }
fp@1313:             memcpy(slave->sii.strings[i], data + offset + 1, size);
fp@1313:             slave->sii.strings[i][size] = 0x00; // append binary zero
fp@1313:             offset += 1 + size;
fp@1313:         }
fp@118:     }
fp@118: 
fp@118:     return 0;
fp@624: 
fp@624: out_free:
fp@1313:     for (i--; i >= 0; i--)
fp@1313:         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@1313:     return err;
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@1921:         EC_SLAVE_ERR(slave, "Wrong size of general category (%zu/32).\n",
fp@1921:                 data_size);
fp@1313:         return -EINVAL;
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@1921:         EC_SLAVE_ERR(slave, "Invalid SII sync manager category size %zu.\n",
fp@1921:                 data_size);
fp@1313:         return -EINVAL;
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@1082:         if (total_count > EC_MAX_SYNC_MANAGERS) {
fp@1921:             EC_SLAVE_ERR(slave, "Exceeded maximum number of"
fp@1921:                     " sync managers!\n");
fp@1313:             return -EOVERFLOW;
fp@1055:         }
fp@870:         memsize = sizeof(ec_sync_t) * total_count;
fp@870:         if (!(syncs = kmalloc(memsize, GFP_KERNEL))) {
fp@1921:             EC_SLAVE_ERR(slave, "Failed to allocate %zu bytes"
fp@1921:                     " for sync managers.\n", memsize);
fp@1313:             return -ENOMEM;
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@1055:             ec_sync_init(sync, slave);
fp@870:             sync->physical_start_address = EC_READ_U16(data);
fp@1055:             sync->default_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@1327:    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@1327:         ec_direction_t dir /**< PDO direction. */
fp@620:         )
fp@126: {
fp@1313:     int ret;
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@1921:             EC_SLAVE_ERR(slave, "Failed to allocate PDO memory.\n");
fp@1313:             return -ENOMEM;
fp@126:         }
fp@126: 
fp@627:         ec_pdo_init(pdo);
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@1313:         ret = ec_pdo_set_name(pdo,
fp@1313:                 ec_slave_sii_string(slave, EC_READ_U8(data + 5)));
fp@1313:         if (ret) {
fp@792:             ec_pdo_clear(pdo);
fp@792:             kfree(pdo);
fp@1313:             return ret;
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@1921:                 EC_SLAVE_ERR(slave, "Failed to allocate PDO entry memory.\n");
fp@1313:                 return -ENOMEM;
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@1313:             ret = ec_pdo_entry_set_name(entry,
fp@1313:                     ec_slave_sii_string(slave, EC_READ_U8(data + 3)));
fp@1313:             if (ret) {
fp@792:                 ec_pdo_entry_clear(entry);
fp@792:                 kfree(entry);
fp@1313:                 return ret;
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@1327:         // 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@1055:             if (!(sync = ec_slave_get_sync(slave, pdo->sync_index))) {
fp@1921:                 EC_SLAVE_ERR(slave, "Invalid SM index %i for PDO 0x%04X.",
fp@1921:                         pdo->sync_index, pdo->index);
fp@1313:                 return -ENOENT;
fp@635:             }
fp@635: 
fp@1313:             ret = ec_pdo_list_add_pdo_copy(&sync->pdos, pdo);
fp@1313:             if (ret)
fp@1313:                 return ret;
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@1924:         EC_SLAVE_DBG(slave, 1, "String %u not found.\n", index);
fp@624:         return NULL;
fp@624:     }
fp@624: 
fp@834:     return slave->sii.strings[index];
fp@123: }
fp@123: 
fp@123: /*****************************************************************************/
fp@123: 
fp@1055: /** Get the sync manager given an index.
fp@1055:  *
fp@758:  * \return pointer to sync manager, or NULL.
fp@619:  */
fp@1055: ec_sync_t *ec_slave_get_sync(
fp@912:         ec_slave_t *slave, /**< EtherCAT slave. */
fp@1055:         uint8_t sync_index /**< Sync manager index. */
fp@1055:         )
fp@1055: {
fp@1055:     if (sync_index < slave->sii.sync_count) {
fp@1055:         return &slave->sii.syncs[sync_index];
fp@1055:     } else {
fp@635:         return NULL;
fp@635:     }
fp@619: }
fp@619: 
fp@619: /*****************************************************************************/
fp@619: 
fp@619: /**
fp@1327:    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@1327:                             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@1327:  * Get an SDO from the dictionary.
fp@1327:  * \returns The desired SDO, or NULL.
fp@740:  */
fp@740: 
fp@740: ec_sdo_t *ec_slave_get_sdo(
fp@964:         ec_slave_t *slave, /**< EtherCAT slave */
fp@1327:         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@967:         if (sdo->index != index)
fp@967:             continue;
fp@967:         return sdo;
fp@967:     }
fp@967: 
fp@967:     return NULL;
fp@967: }
fp@967: 
fp@967: /*****************************************************************************/
fp@967: 
fp@967: /**
fp@1327:  * Get an SDO from the dictionary.
fp@967:  *
fp@967:  * const version.
fp@967:  *
fp@1327:  * \returns The desired SDO, or NULL.
fp@967:  */
fp@967: 
fp@967: const ec_sdo_t *ec_slave_get_sdo_const(
fp@967:         const ec_slave_t *slave, /**< EtherCAT slave */
fp@1327:         uint16_t index /**< SDO index */
fp@967:         )
fp@967: {
fp@967:     const ec_sdo_t *sdo;
fp@967: 
fp@967:     list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
fp@967:         if (sdo->index != index)
fp@967:             continue;
fp@740:         return sdo;
fp@740:     }
fp@740: 
fp@740:     return NULL;
fp@740: }
fp@740: 
fp@792: /*****************************************************************************/
fp@799: 
fp@1327: /** Get an SDO from the dictionary, given its position in the list.
fp@1327:  * \returns The desired SDO, or NULL.
fp@964:  */
fp@964: 
fp@964: const ec_sdo_t *ec_slave_get_sdo_by_pos_const(
fp@964:         const ec_slave_t *slave, /**< EtherCAT slave. */
fp@1327:         uint16_t sdo_position /**< SDO list position. */
fp@964:         )
fp@964: {
fp@964:     const ec_sdo_t *sdo;
fp@964: 
fp@964:     list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
fp@964:         if (sdo_position--)
fp@964:             continue;
fp@964:         return sdo;
fp@964:     }
fp@964: 
fp@964:     return NULL;
fp@964: }
fp@964: 
fp@964: /*****************************************************************************/
fp@964: 
fp@1327: /** Get the number of SDOs in the dictionary.
fp@1327:  * \returns SDO count.
fp@964:  */
fp@964: 
fp@964: uint16_t ec_slave_sdo_count(
fp@964:         const ec_slave_t *slave /**< EtherCAT slave. */
fp@964:         )
fp@964: {
fp@964:     const ec_sdo_t *sdo;
fp@964:     uint16_t count = 0;
fp@964: 
fp@964:     list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
fp@964:         count++;
fp@964:     }
fp@964: 
fp@964:     return count;
fp@964: }
fp@964: 
fp@964: /*****************************************************************************/
fp@964: 
fp@1327: /** Finds a mapped PDO.
fp@1327:  * \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@1327:         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: /*****************************************************************************/
fp@1186: 
fp@1327: /** Find name for a PDO and its entries.
fp@1186:  */
fp@1186: void ec_slave_find_names_for_pdo(
fp@1186:         ec_slave_t *slave,
fp@1186:         ec_pdo_t *pdo
fp@1186:         )
fp@1186: {
fp@1186:     const ec_sdo_t *sdo;
fp@1186:     ec_pdo_entry_t *pdo_entry;
fp@1186:     const ec_sdo_entry_t *sdo_entry;
fp@1186: 
fp@1186:     list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
fp@1186:         if (sdo->index == pdo->index) {
fp@1186:             ec_pdo_set_name(pdo, sdo->name);
fp@1186:         } else {
fp@1186:             list_for_each_entry(pdo_entry, &pdo->entries, list) {
fp@1186:                 if (sdo->index == pdo_entry->index) {
fp@1186:                     sdo_entry = ec_sdo_get_entry_const(
fp@1186:                             sdo, pdo_entry->subindex);
fp@1186:                     if (sdo_entry) {
fp@1186:                         ec_pdo_entry_set_name(pdo_entry,
fp@1186:                                 sdo_entry->description);
fp@1186:                     }
fp@1186:                 }
fp@1186:             }
fp@1186:         }
fp@1186:     }
fp@1186: }
fp@1186: 
fp@1186: /*****************************************************************************/
fp@1186: 
fp@1327: /** Attach PDO names.
fp@1186:  */
fp@1186: void ec_slave_attach_pdo_names(
fp@1186:         ec_slave_t *slave
fp@1186:         )
fp@1186: {
fp@1186:     unsigned int i;
fp@1186:     ec_sync_t *sync;
fp@1186:     ec_pdo_t *pdo;
fp@1186:     
fp@1186:     for (i = 0; i < slave->sii.sync_count; i++) {
fp@1186:         sync = slave->sii.syncs + i;
fp@1186:         list_for_each_entry(pdo, &sync->pdos.list, list) {
fp@1186:             ec_slave_find_names_for_pdo(slave, pdo);
fp@1186:         }
fp@1186:     }
fp@1186: }
fp@1186: 
fp@1186: /*****************************************************************************/
fp@1425: 
ch1010277@1998: /** returns the previous connected port of a given port.
ch1010277@1998:  */
ch1010277@1998: 
ch1010277@1998: unsigned int ec_slave_get_previous_port(
ch1010277@1998:     ec_slave_t *slave, /**< EtherCAT slave. */
ch1010277@1998:     unsigned int i /**< Port index */
ch1010277@1998:     )
ch1010277@1998: {
ch1010277@1998:     do
ch1010277@1998:     {
ch1010277@1998:         switch (i)
ch1010277@1998:         {
ch1010277@1998:         case 0: i = 2; break;
ch1010277@1998:         case 1: i = 3; break;
ch1010277@1998:         case 2: i = 1; break;
ch1010277@1998:         case 3:
ch1010277@1998:         default:i = 0; break;
ch1010277@1998:         }
ch1010277@1998:         if (slave->ports[i].next_slave)
ch1010277@1998:             return i;
ch1010277@1998:     } while (i);
ch1010277@1998:     return 0;
ch1010277@1998: }
ch1010277@1998: 
ch1010277@1998: /*****************************************************************************/
ch1010277@1998: 
ch1010277@1998: /** returns the next connected port of a given port.
ch1010277@1998:  */
ch1010277@1998: 
ch1010277@1998: unsigned int ec_slave_get_next_port(
ch1010277@1998:     ec_slave_t *slave, /**< EtherCAT slave. */
ch1010277@1998:     unsigned int i /**< Port index */
ch1010277@1998:     )
ch1010277@1998: {
ch1010277@1998:     do
ch1010277@1998:     {
ch1010277@1998:         switch (i)
ch1010277@1998:         {
ch1010277@1998:         case 0: i = 3; break;
ch1010277@1998:         case 1: i = 2; break;
ch1010277@1998:         case 3: i = 1; break;
ch1010277@1998:         case 2:
ch1010277@1998:         default:i = 0; break;
ch1010277@1998:         }
ch1010277@1998:         if (slave->ports[i].next_slave)
ch1010277@1998:             return i;
ch1010277@1998:     } while (i);
ch1010277@1998:     return 0;
ch1010277@1998: }
ch1010277@1998: 
ch1010277@1998: 
ch1010277@1998: /*****************************************************************************/
ch1010277@1998: 
fp@1425: /** Calculates the sum of round-trip-times of connected ports 1-3.
fp@1425:  */
fp@1425: uint32_t ec_slave_calc_rtt_sum(
fp@1425:         ec_slave_t *slave /**< EtherCAT slave. */
fp@1507:         )
fp@1425: {
fp@1425:     uint32_t rtt_sum = 0, rtt;
ch1010277@1998:     unsigned int i = ec_slave_get_next_port(slave,0);
ch1010277@1998:     while (i != 0) {
ch1010277@1998:         rtt = slave->ports[i].receive_time - slave->ports[ec_slave_get_previous_port(slave,i)].receive_time;
ch1010277@1998:         rtt_sum += rtt;
ch1010277@1998:         i = ec_slave_get_next_port(slave,i);
fp@1425:     }
fp@1425: 
fp@1425:     return rtt_sum;
fp@1425: }
fp@1425: 
fp@1425: /*****************************************************************************/
fp@1425: 
fp@1425: /** Finds the next slave supporting DC delay measurement.
fp@1425:  */
fp@1425: ec_slave_t *ec_slave_find_next_dc_slave(
fp@1425:         ec_slave_t *slave /**< EtherCAT slave. */
fp@1507:         )
fp@1425: {
ch1010277@1998:     unsigned int i;
fp@1425:     ec_slave_t *dc_slave = NULL;
fp@1425: 
fp@1425:     if (slave->base_dc_supported) {
fp@1425:         dc_slave = slave;
fp@1425:     } else {
ch1010277@1998:         i = ec_slave_get_next_port(slave,0);
ch1010277@1998:         while (i != 0) {
fp@1425:             ec_slave_t *next = slave->ports[i].next_slave;
fp@1425:             if (next) {
fp@1425:                 dc_slave = ec_slave_find_next_dc_slave(next);
fp@1425:                 if (dc_slave)
fp@1425:                     break;
fp@1425:             }
ch1010277@1998:             i = ec_slave_get_next_port(slave,i);
fp@1425:         }
fp@1425:     }
fp@1425: 
fp@1425:     return dc_slave;
fp@1425: }
fp@1425: 
fp@1425: /*****************************************************************************/
fp@1425: 
fp@1426: /** Calculates the port transmission delays.
fp@1425:  */
fp@1425: void ec_slave_calc_port_delays(
fp@1425:         ec_slave_t *slave /**< EtherCAT slave. */
fp@1507:         )
fp@1425: {
fp@1425:     unsigned int i;
ch1010277@1998:     ec_slave_t *next_dc;
fp@1425:     uint32_t rtt, next_rtt_sum;
fp@1425: 
fp@1425:     if (!slave->base_dc_supported)
fp@1425:         return;
fp@1425: 
ch1010277@1998:     i = ec_slave_get_next_port(slave,0);
ch1010277@1998:     while (i != 0) {
ch1010277@1998:         next_dc = ec_slave_find_next_dc_slave(slave->ports[i].next_slave);
ch1010277@1998:         if (next_dc) {
ch1010277@1998:             rtt = slave->ports[i].receive_time - slave->ports[ec_slave_get_previous_port(slave,i)].receive_time;
ch1010277@1998:             next_rtt_sum = ec_slave_calc_rtt_sum(next_dc);
ch1010277@1998: 
ch1010277@1998:             slave->ports[i].delay_to_next_dc = (rtt - next_rtt_sum) / 2; // FIXME
ch1010277@1998:             next_dc->ports[0].delay_to_next_dc = (rtt - next_rtt_sum) / 2;
fp@1425: 
fp@1425: #if 0
ch1010277@1998:             EC_SLAVE_DBG(slave, 1, "delay %u:%u rtt=%u"
ch1010277@1998:                     " next_rtt_sum=%u delay=%u\n",
ch1010277@1998:                     slave->ring_position, i, rtt, next_rtt_sum,
ch1010277@1998:                     slave->ports[i].delay_to_next_dc);
fp@1425: #endif
ch1010277@1998:         }
ch1010277@1998:         i = ec_slave_get_next_port(slave,i);
fp@1425:     }
fp@1425: }
fp@1425: 
fp@1425: /*****************************************************************************/
fp@1425: 
fp@1426: /** Recursively calculates transmission delays.
fp@1426:  */
fp@1426: void ec_slave_calc_transmission_delays_rec(
fp@1425:         ec_slave_t *slave, /**< Current slave. */
fp@1425:         uint32_t *delay /**< Sum of delays. */
fp@1507:         )
fp@1425: {
fp@1425:     unsigned int i;
ch1010277@1998:     ec_slave_t *next_dc;
ch1010277@1998: 
ch1010277@1998: #if 1
fp@1921:     EC_SLAVE_DBG(slave, 1, "%u\n", *delay);
fp@1425: #endif
fp@1425: 
fp@1426:     slave->transmission_delay = *delay;
fp@1425: 
ch1010277@1998:     i = ec_slave_get_next_port(slave,0);
ch1010277@1998:     while (i != 0) {
fp@1425:         ec_slave_port_t *port = &slave->ports[i];
ch1010277@1998:         next_dc = ec_slave_find_next_dc_slave(port->next_slave);
ch1010277@1998:         if (next_dc) {
ch1010277@1998:             *delay = *delay + port->delay_to_next_dc;
fp@1425: #if 0
ch1010277@1998:             EC_SLAVE_DBG(slave, 1, "%u:%u %u\n", slave->ring_position, i, *delay);
fp@1425: #endif
ch1010277@1998:             ec_slave_calc_transmission_delays_rec(next_dc, delay);
ch1010277@1998:         }
ch1010277@1998:         i = ec_slave_get_next_port(slave,i);
fp@1425:     }
fp@1425: 
fp@1425:     *delay = *delay + slave->ports[0].delay_to_next_dc;
fp@1425: }
fp@1425: 
fp@1425: /*****************************************************************************/