diff -r 2b5b6ccc00a0 -r 8686c5b3d14b master/fsm_slave_config.c --- a/master/fsm_slave_config.c Mon Apr 20 10:28:09 2009 +0000 +++ b/master/fsm_slave_config.c Mon Apr 20 11:53:17 2009 +0000 @@ -46,6 +46,7 @@ void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *); +void ec_fsm_slave_config_state_clear_dc_assign(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 *); @@ -62,6 +63,7 @@ void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *); void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *); +void ec_fsm_slave_config_enter_clear_dc_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 *); @@ -286,7 +288,7 @@ if (!slave->sii.sync_count) { // FIXME use base_sync_count? // no sync managers - ec_fsm_slave_config_enter_mbox_sync(fsm); + ec_fsm_slave_config_enter_clear_dc_assign(fsm); return; } @@ -332,6 +334,64 @@ return; } + ec_fsm_slave_config_enter_clear_dc_assign(fsm); +} + +/*****************************************************************************/ + +/** Clear the DC assignment. + */ +void ec_fsm_slave_config_enter_clear_dc_assign( + ec_fsm_slave_config_t *fsm /**< slave state machine */ + ) +{ + ec_slave_t *slave = fsm->slave; + ec_datagram_t *datagram = fsm->datagram; + + if (!slave->base_dc_supported) { + ec_fsm_slave_config_enter_mbox_sync(fsm); + return; + } + + if (slave->master->debug_level) + EC_DBG("Clearing DC assignment of slave %u...\n", + slave->ring_position); + + ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2); + ec_datagram_zero(datagram); + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_slave_config_state_clear_dc_assign; +} + +/*****************************************************************************/ + +/** Slave configuration state: CLEAR DC ASSIGN. + */ +void ec_fsm_slave_config_state_clear_dc_assign( + ec_fsm_slave_config_t *fsm /**< slave state machine */ + ) +{ + ec_datagram_t *datagram = fsm->datagram; + + if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) + return; + + if (datagram->state != EC_DATAGRAM_RECEIVED) { + fsm->state = ec_fsm_slave_config_state_error; + EC_ERR("Failed receive DC assignment clearing datagram" + " for slave %u.\n", fsm->slave->ring_position); + return; + } + + if (datagram->working_counter != 1) { + fsm->slave->error_flag = 1; + fsm->state = ec_fsm_slave_config_state_error; + EC_ERR("Failed to clear DC assignment of slave %u: ", + fsm->slave->ring_position); + ec_datagram_print_wc_error(datagram); + return; + } + ec_fsm_slave_config_enter_mbox_sync(fsm); }