# HG changeset patch # User Florian Pose # Date 1275856822 -7200 # Node ID 73896ef6d0773037675348d713af0baa92a40b67 # Parent 7e9deec7742de5436ae70386c0bb09b7c788e861 Added state parameter to ecrt_slave_config_idn(). diff -r 7e9deec7742d -r 73896ef6d077 include/ecrt.h --- a/include/ecrt.h Sun Jun 06 21:32:50 2010 +0200 +++ b/include/ecrt.h Sun Jun 06 22:40:22 2010 +0200 @@ -382,6 +382,17 @@ EC_REQUEST_ERROR, /**< Request processing failed. */ } ec_request_state_t; +/*****************************************************************************/ + +/** Application-layer state. + */ +typedef enum { + EC_AL_STATE_INIT = 1, /**< Init. */ + EC_AL_STATE_PREOP = 2, /**< Pre-operational. */ + EC_AL_STATE_SAFEOP = 4, /**< Safe-operational. */ + EC_AL_STATE_OP = 8, /**< Operational. */ +} ec_al_state_t; + /****************************************************************************** * Global functions *****************************************************************************/ @@ -1202,6 +1213,8 @@ int ecrt_slave_config_idn( ec_slave_config_t *sc, /**< Slave configuration. */ uint16_t idn, /**< SoE IDN. */ + ec_al_state_t state, /**< AL state in which to write the IDN (PREOP or + SAFEOP). */ const uint8_t *data, /**< Pointer to the data. */ size_t size /**< Size of the \a data. */ ); diff -r 7e9deec7742d -r 73896ef6d077 lib/slave_config.c --- a/lib/slave_config.c Sun Jun 06 21:32:50 2010 +0200 +++ b/lib/slave_config.c Sun Jun 06 22:40:22 2010 +0200 @@ -473,12 +473,13 @@ /*****************************************************************************/ int ecrt_slave_config_idn(ec_slave_config_t *sc, uint16_t idn, - const uint8_t *data, size_t size) + ec_al_state_t state, const uint8_t *data, size_t size) { ec_ioctl_sc_idn_t io; io.config_index = sc->index; io.idn = idn; + io.state = state; io.data = data; io.size = size; diff -r 7e9deec7742d -r 73896ef6d077 master/cdev.c --- a/master/cdev.c Sun Jun 06 21:32:50 2010 +0200 +++ b/master/cdev.c Sun Jun 06 22:40:22 2010 +0200 @@ -2463,7 +2463,7 @@ up(&master->master_sem); // FIXME - ret = ecrt_slave_config_idn(sc, ioctl.idn, data, ioctl.size); + ret = ecrt_slave_config_idn(sc, ioctl.idn, ioctl.state, data, ioctl.size); kfree(data); return ret; } diff -r 7e9deec7742d -r 73896ef6d077 master/fsm_slave_config.c --- a/master/fsm_slave_config.c Sun Jun 06 21:32:50 2010 +0200 +++ b/master/fsm_slave_config.c Sun Jun 06 22:40:22 2010 +0200 @@ -69,7 +69,7 @@ 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_soe_conf_preop(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 *); @@ -80,6 +80,7 @@ void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *); +void ec_fsm_slave_config_state_soe_conf_safeop(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *); void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *); @@ -88,7 +89,7 @@ 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_soe_conf_preop(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 *); @@ -96,6 +97,8 @@ void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *); void ec_fsm_slave_config_enter_dc_cycle(ec_fsm_slave_config_t *); void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *); +void ec_fsm_slave_config_enter_soe_conf_safeop(ec_fsm_slave_config_t *); +void ec_fsm_slave_config_enter_op(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *); @@ -658,7 +661,7 @@ // No CoE configuration to be applied? if (list_empty(&slave->config->sdo_configs)) { // skip SDO configuration - ec_fsm_slave_config_enter_soe_conf(fsm); + ec_fsm_slave_config_enter_soe_conf_preop(fsm); return; } @@ -706,47 +709,50 @@ } // All SDOs are now configured. - ec_fsm_slave_config_enter_soe_conf(fsm); + ec_fsm_slave_config_enter_soe_conf_preop(fsm); } /*****************************************************************************/ /** Check for SoE configurations to be applied. */ -void ec_fsm_slave_config_enter_soe_conf( +void ec_fsm_slave_config_enter_soe_conf_preop( 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; + ec_soe_request_t *req; 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 - ec_master_queue_external_datagram(slave->master, fsm_soe->datagram); + list_for_each_entry(req, &slave->config->soe_configs, list) { + if (req->al_state == EC_AL_STATE_PREOP) { + // start SoE configuration + fsm->state = ec_fsm_slave_config_state_soe_conf_preop; + fsm->soe_request = req; + 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 + ec_master_queue_external_datagram(slave->master, + fsm_soe->datagram); + return; + } + } + + // No SoE configuration to be applied in PREOP + ec_fsm_slave_config_enter_pdo_conf(fsm); } /*****************************************************************************/ /** Slave configuration state: SOE_CONF. */ -void ec_fsm_slave_config_state_soe_conf( +void ec_fsm_slave_config_state_soe_conf_preop( ec_fsm_slave_config_t *fsm /**< slave state machine */ ) { @@ -770,19 +776,22 @@ return; } - // Another IDN to configure? - if (fsm->soe_request->list.next != &fsm->slave->config->soe_configs) { + // Another IDN to configure in PREOP? + while (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 - ec_master_queue_external_datagram(slave->master, fsm_soe->datagram); - return; - } - - // All SDOs are now configured. + if (fsm->soe_request->al_state == EC_AL_STATE_PREOP) { + 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 + ec_master_queue_external_datagram(slave->master, + fsm_soe->datagram); + return; + } + } + + // All PREOP IDNs are now configured. ec_fsm_slave_config_enter_pdo_conf(fsm); } @@ -1416,9 +1425,103 @@ return; } + ec_fsm_slave_config_enter_soe_conf_safeop(fsm); +} + +/*****************************************************************************/ + +/** Check for SoE configurations to be applied in SAFEOP. + */ +void ec_fsm_slave_config_enter_soe_conf_safeop( + 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; + ec_soe_request_t *req; + + if (!slave->config) { + ec_fsm_slave_config_enter_op(fsm); + return; + } + + list_for_each_entry(req, &slave->config->soe_configs, list) { + if (req->al_state == EC_AL_STATE_SAFEOP) { + // start SoE configuration + fsm->state = ec_fsm_slave_config_state_soe_conf_safeop; + fsm->soe_request = req; + 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 + ec_master_queue_external_datagram(slave->master, + fsm_soe->datagram); + return; + } + } + + // No SoE configuration to be applied in SAFEOP + ec_fsm_slave_config_enter_op(fsm); +} + +/*****************************************************************************/ + +/** Slave configuration state: SOE_CONF. + */ +void ec_fsm_slave_config_state_soe_conf_safeop( + 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)) { + ec_master_queue_external_datagram(slave->master, fsm_soe->datagram); + return; + } + + if (!ec_fsm_soe_success(fsm_soe)) { + EC_SLAVE_ERR(slave, "SoE configuration failed.\n"); + 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 in SAFEOP? + while (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); + if (fsm->soe_request->al_state == EC_AL_STATE_SAFEOP) { + 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 + ec_master_queue_external_datagram(slave->master, + fsm_soe->datagram); + return; + } + } + + // All SAFEOP IDNs are now configured. + ec_fsm_slave_config_enter_op(fsm); +} + +/*****************************************************************************/ + +/** Bring slave to OP. + */ +void ec_fsm_slave_config_enter_op( + ec_fsm_slave_config_t *fsm /**< slave state machine */ + ) +{ // set state to OP fsm->state = ec_fsm_slave_config_state_op; - ec_fsm_change_start(fsm->fsm_change, slave, EC_SLAVE_STATE_OP); + ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_OP); ec_fsm_change_exec(fsm->fsm_change); // execute immediately } diff -r 7e9deec7742d -r 73896ef6d077 master/ioctl.h --- a/master/ioctl.h Sun Jun 06 21:32:50 2010 +0200 +++ b/master/ioctl.h Sun Jun 06 22:40:22 2010 +0200 @@ -583,6 +583,7 @@ // inputs uint32_t config_index; uint16_t idn; + ec_al_state_t state; const uint8_t *data; size_t size; } ec_ioctl_sc_idn_t; diff -r 7e9deec7742d -r 73896ef6d077 master/slave_config.c --- a/master/slave_config.c Sun Jun 06 21:32:50 2010 +0200 +++ b/master/slave_config.c Sun Jun 06 22:40:22 2010 +0200 @@ -939,14 +939,21 @@ /*****************************************************************************/ int ecrt_slave_config_idn(ec_slave_config_t *sc, uint16_t idn, - const uint8_t *data, size_t size) + ec_al_state_t state, const uint8_t *data, size_t size) { ec_slave_t *slave = sc->slave; ec_soe_request_t *req; int ret; - EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, idn = 0x%04X, " - "data = 0x%p, size = %zu)\n", __func__, sc, idn, data, size); + EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, idn = 0x%04X, state = %u, " + "data = 0x%p, size = %zu)\n", + __func__, sc, idn, state, data, size); + + if (state != EC_AL_STATE_PREOP && state != EC_AL_STATE_SAFEOP) { + EC_CONFIG_ERR(sc, "AL state for IDN config" + " must be PREOP or SAFEOP!\n"); + return -EINVAL; + } if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { EC_CONFIG_WARN(sc, "Attached slave does not support SoE!\n"); @@ -961,6 +968,7 @@ ec_soe_request_init(req); ec_soe_request_set_idn(req, idn); + req->al_state = state; ret = ec_soe_request_copy_data(req, data, size); if (ret < 0) { diff -r 7e9deec7742d -r 73896ef6d077 master/soe_request.c --- a/master/soe_request.c Sun Jun 06 21:32:50 2010 +0200 +++ b/master/soe_request.c Sun Jun 06 22:40:22 2010 +0200 @@ -56,6 +56,7 @@ ec_soe_request_t *req /**< SoE request. */ ) { + req->al_state = EC_AL_STATE_INIT; req->data = NULL; req->mem_size = 0; req->data_size = 0; @@ -86,6 +87,7 @@ ) { req->idn = other->idn; + req->al_state = other->al_state; return ec_soe_request_copy_data(req, other->data, other->data_size); } diff -r 7e9deec7742d -r 73896ef6d077 master/soe_request.h --- a/master/soe_request.h Sun Jun 06 21:32:50 2010 +0200 +++ b/master/soe_request.h Sun Jun 06 22:40:22 2010 +0200 @@ -48,6 +48,7 @@ typedef struct { struct list_head list; /**< List item. */ uint16_t idn; /**< Sercos ID-Number. */ + ec_al_state_t al_state; /**< AL state (only valid for IDN config). */ uint8_t *data; /**< Pointer to SDO data. */ size_t mem_size; /**< Size of SDO data memory. */ size_t data_size; /**< Size of SDO data. */