From fc4ee4d699a977e655b1daef02c7caf3578f96cd Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Fri, 5 Mar 2010 18:05:35 +0100 Subject: [PATCH] Added ecrt_slave_config_idn() method to store SoE IDN configurations. --- NEWS | 1 + include/ecrt.h | 22 +++++++++++ master/fsm_slave_config.c | 80 ++++++++++++++++++++++++++++++++++++++- master/fsm_slave_config.h | 2 + master/slave_config.c | 50 ++++++++++++++++++++++++ master/slave_config.h | 1 + 6 files changed, 155 insertions(+), 1 deletion(-) diff --git a/NEWS b/NEWS index d9cda749..f9535498 100644 --- a/NEWS +++ b/NEWS @@ -13,6 +13,7 @@ Changes since 1.4.0: * Added distributed clocks support. * Added watchdog configuration via application interface (thanks to J. Mohre). * Added VoE mailbox protocol support. +* Added SoE mailbox protocol support. * Improved the callback mechanism. ecrt_master_callbacks() now takes two callback functions for sending and receiving datagrams. ecrt_master_send_ext() is used to execute the sending of non-application diff --git a/include/ecrt.h b/include/ecrt.h index d75298a8..6a049fc3 100644 --- a/include/ecrt.h +++ b/include/ecrt.h @@ -75,6 +75,7 @@ * - Removed 'const' from argument of ecrt_sdo_request_state(), because the * userspace library has to modify object internals. * - Added 64-bit data access macros. + * - Added ecrt_slave_config_idn() method for storing SoE IDN configurations. * * @{ */ @@ -1134,6 +1135,27 @@ void ecrt_slave_config_state( ec_slave_config_state_t *state /**< State object to write to. */ ); +/** Add an SoE IDN configuration. + * + * A configuration for a Sercos-over-EtherCAT IDN is stored in the slave + * configuration object and is written to the slave whenever the slave is + * being configured by the master. This usually happens once on master + * activation, but can be repeated subsequently, for example after the slave's + * power supply failed. + * + * Please note that the this function does not do any endianess correction. + * Data have to be passed in EtherCAT endianess (little-endian). + * + * \retval 0 Success. + * \retval <0 Error code. + */ +int ecrt_slave_config_idn( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t idn, /**< SoE IDN. */ + const uint8_t *data, /**< Pointer to the data. */ + size_t size /**< Size of the \a data. */ + ); + /****************************************************************************** * Domain methods *****************************************************************************/ diff --git a/master/fsm_slave_config.c b/master/fsm_slave_config.c index aa3f1ed7..6ea04b1d 100644 --- a/master/fsm_slave_config.c +++ b/master/fsm_slave_config.c @@ -54,6 +54,7 @@ void ec_fsm_slave_config_state_dc_write_offset(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *); +void ec_fsm_slave_config_state_soe_conf(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_watchdog_divider(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_watchdog(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *); @@ -71,6 +72,7 @@ void ec_fsm_slave_config_enter_dc_clear_assign(ec_fsm_slave_config_t *); void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *); void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *); void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *); +void ec_fsm_slave_config_enter_soe_conf(ec_fsm_slave_config_t *); void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *); void ec_fsm_slave_config_enter_watchdog_divider(ec_fsm_slave_config_t *); void ec_fsm_slave_config_enter_watchdog(ec_fsm_slave_config_t *); @@ -113,6 +115,7 @@ void ec_fsm_slave_config_clear( ) { ec_sdo_request_clear(&fsm->request_copy); + ec_soe_request_clear(&fsm->soe_request_copy); } /*****************************************************************************/ @@ -759,7 +762,7 @@ void ec_fsm_slave_config_enter_sdo_conf( // No CoE configuration to be applied? if (list_empty(&slave->config->sdo_configs)) { // skip SDO configuration - ec_fsm_slave_config_enter_pdo_conf(fsm); + ec_fsm_slave_config_enter_soe_conf(fsm); return; } @@ -807,6 +810,81 @@ void ec_fsm_slave_config_state_sdo_conf( return; } + // All SDOs are now configured. + ec_fsm_slave_config_enter_soe_conf(fsm); +} + +/*****************************************************************************/ + +/** Check for SoE configurations to be applied. + */ +void ec_fsm_slave_config_enter_soe_conf( + ec_fsm_slave_config_t *fsm /**< slave state machine */ + ) +{ + ec_slave_t *slave = fsm->slave; + ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; + + if (!slave->config) { + ec_fsm_slave_config_enter_pdo_sync(fsm); + return; + } + + // No SoE configuration to be applied? + if (list_empty(&slave->config->soe_configs)) { // skip configuration + ec_fsm_slave_config_enter_pdo_conf(fsm); + return; + } + + // start SoE configuration + fsm->state = ec_fsm_slave_config_state_soe_conf; + fsm->soe_request = list_entry(fsm->slave->config->soe_configs.next, + ec_soe_request_t, list); + ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); + ec_soe_request_write(&fsm->soe_request_copy); + ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); + ec_fsm_soe_exec(fsm_soe); // execute immediately +} + +/*****************************************************************************/ + +/** Slave configuration state: SOE_CONF. + */ +void ec_fsm_slave_config_state_soe_conf( + ec_fsm_slave_config_t *fsm /**< slave state machine */ + ) +{ + ec_slave_t *slave = fsm->slave; + ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; + + if (ec_fsm_soe_exec(fsm_soe)) { + return; + } + + if (!ec_fsm_soe_success(fsm_soe)) { + EC_ERR("SoE configuration failed for slave %u.\n", + fsm->slave->ring_position); + fsm->slave->error_flag = 1; + fsm->state = ec_fsm_slave_config_state_error; + return; + } + + if (!fsm->slave->config) { // config removed in the meantime + ec_fsm_slave_config_reconfigure(fsm); + return; + } + + // Another IDN to configure? + if (fsm->soe_request->list.next != &fsm->slave->config->soe_configs) { + fsm->soe_request = list_entry(fsm->soe_request->list.next, + ec_soe_request_t, list); + ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); + ec_soe_request_write(&fsm->soe_request_copy); + ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); + ec_fsm_soe_exec(fsm_soe); // execute immediately + return; + } + // All SDOs are now configured. ec_fsm_slave_config_enter_pdo_conf(fsm); } diff --git a/master/fsm_slave_config.h b/master/fsm_slave_config.h index a7668810..15370f1a 100644 --- a/master/fsm_slave_config.h +++ b/master/fsm_slave_config.h @@ -63,6 +63,8 @@ struct ec_fsm_slave_config unsigned int retries; /**< Retries on datagram timeout. */ ec_sdo_request_t *request; /**< SDO request for SDO configuration. */ ec_sdo_request_t request_copy; /**< Copied SDO request. */ + ec_soe_request_t *soe_request; /**< SDO request for SDO configuration. */ + ec_soe_request_t soe_request_copy; /**< Copied SDO request. */ unsigned long jiffies_start; /**< For timeout calculations. */ unsigned int take_time; /**< Store jiffies after datagram reception. */ }; diff --git a/master/slave_config.c b/master/slave_config.c index a30dd1ee..df77c088 100644 --- a/master/slave_config.c +++ b/master/slave_config.c @@ -86,6 +86,7 @@ void ec_slave_config_init( INIT_LIST_HEAD(&sc->sdo_configs); INIT_LIST_HEAD(&sc->sdo_requests); INIT_LIST_HEAD(&sc->voe_handlers); + INIT_LIST_HEAD(&sc->soe_configs); } /*****************************************************************************/ @@ -101,6 +102,7 @@ void ec_slave_config_clear( unsigned int i; ec_sdo_request_t *req, *next_req; ec_voe_handler_t *voe, *next_voe; + ec_soe_request_t *soe, *next_soe; ec_slave_config_detach(sc); @@ -128,6 +130,13 @@ void ec_slave_config_clear( ec_voe_handler_clear(voe); kfree(voe); } + + // free all SoE configurations + list_for_each_entry_safe(soe, next_soe, &sc->soe_configs, list) { + list_del(&soe->list); + ec_soe_request_clear(soe); + kfree(soe); + } } /*****************************************************************************/ @@ -950,6 +959,46 @@ void ecrt_slave_config_state(const ec_slave_config_t *sc, /*****************************************************************************/ +int ecrt_slave_config_idn(ec_slave_config_t *sc, uint16_t idn, + const uint8_t *data, size_t size) +{ + ec_slave_t *slave = sc->slave; + ec_soe_request_t *req; + int ret; + + if (sc->master->debug_level) + EC_DBG("ecrt_slave_config_idn(sc = 0x%p, idn = 0x%04X, " + "data = 0x%p, size = %zu)\n", sc, idn, data, size); + + if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { + EC_ERR("Slave %u does not support SoE!\n", slave->ring_position); + return -EPROTONOSUPPORT; // protocol not supported + } + + if (!(req = (ec_soe_request_t *) + kmalloc(sizeof(ec_soe_request_t), GFP_KERNEL))) { + EC_ERR("Failed to allocate memory for IDN configuration!\n"); + return -ENOMEM; + } + + ec_soe_request_init(req); + ec_soe_request_set_idn(req, idn); + + ret = ec_soe_request_copy_data(req, data, size); + if (ret < 0) { + ec_soe_request_clear(req); + kfree(req); + return ret; + } + + down(&sc->master->master_sem); + list_add_tail(&req->list, &sc->soe_configs); + up(&sc->master->master_sem); + return 0; +} + +/*****************************************************************************/ + /** \cond */ EXPORT_SYMBOL(ecrt_slave_config_sync_manager); @@ -969,6 +1018,7 @@ EXPORT_SYMBOL(ecrt_slave_config_complete_sdo); EXPORT_SYMBOL(ecrt_slave_config_create_sdo_request); EXPORT_SYMBOL(ecrt_slave_config_create_voe_handler); EXPORT_SYMBOL(ecrt_slave_config_state); +EXPORT_SYMBOL(ecrt_slave_config_idn); /** \endcond */ diff --git a/master/slave_config.h b/master/slave_config.h index 599c1b38..c2a3091c 100644 --- a/master/slave_config.h +++ b/master/slave_config.h @@ -77,6 +77,7 @@ struct ec_slave_config { struct list_head sdo_configs; /**< List of SDO configurations. */ struct list_head sdo_requests; /**< List of SDO requests. */ struct list_head voe_handlers; /**< List of VoE handlers. */ + struct list_head soe_configs; /**< List of SoE configurations. */ }; /*****************************************************************************/