diff -r 8686c5b3d14b -r e3974f72d31e master/fsm_slave_config.c --- a/master/fsm_slave_config.c Mon Apr 20 11:53:17 2009 +0000 +++ b/master/fsm_slave_config.c Mon Apr 20 13:43:57 2009 +0000 @@ -46,15 +46,15 @@ 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_dc_clear_assign(ec_fsm_slave_config_t *); +void ec_fsm_slave_config_state_dc_read_offset(ec_fsm_slave_config_t *); +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_pdo_sync(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *); -void ec_fsm_slave_config_state_dc_read(ec_fsm_slave_config_t *); -void ec_fsm_slave_config_state_dc_offset(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *); 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 *); @@ -63,14 +63,14 @@ 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_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_pdo_conf(ec_fsm_slave_config_t *); void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *); void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *); -void ec_fsm_slave_config_enter_dc_read(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_state_end(ec_fsm_slave_config_t *); @@ -288,7 +288,7 @@ if (!slave->sii.sync_count) { // FIXME use base_sync_count? // no sync managers - ec_fsm_slave_config_enter_clear_dc_assign(fsm); + ec_fsm_slave_config_enter_dc_clear_assign(fsm); return; } @@ -334,14 +334,14 @@ return; } - ec_fsm_slave_config_enter_clear_dc_assign(fsm); + ec_fsm_slave_config_enter_dc_clear_assign(fsm); } /*****************************************************************************/ /** Clear the DC assignment. */ -void ec_fsm_slave_config_enter_clear_dc_assign( +void ec_fsm_slave_config_enter_dc_clear_assign( ec_fsm_slave_config_t *fsm /**< slave state machine */ ) { @@ -360,14 +360,14 @@ 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; + fsm->state = ec_fsm_slave_config_state_dc_clear_assign; } /*****************************************************************************/ /** Slave configuration state: CLEAR DC ASSIGN. */ -void ec_fsm_slave_config_state_clear_dc_assign( +void ec_fsm_slave_config_state_dc_clear_assign( ec_fsm_slave_config_t *fsm /**< slave state machine */ ) { @@ -383,11 +383,95 @@ return; } + if (fsm->slave->master->debug_level && datagram->working_counter != 1) { + // clearing the DC assignment does not succeed on simple slaves + EC_DBG("Failed to clear DC assignment of slave %u: ", + fsm->slave->ring_position); + ec_datagram_print_wc_error(datagram); + } + + // read DC system time and system time offset + ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, 0x0910, 24); + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_slave_config_state_dc_read_offset; +} + +/*****************************************************************************/ + +/** Slave configuration state: DC READ OFFSET. + */ +void ec_fsm_slave_config_state_dc_read_offset( + ec_fsm_slave_config_t *fsm /**< slave state machine */ + ) +{ + ec_datagram_t *datagram = fsm->datagram; + ec_slave_t *slave = fsm->slave; + u64 system_time, old_offset, new_offset; + + 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 to receive DC times datagram for slave %u" + " (datagram state %u).\n", + slave->ring_position, datagram->state); + 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); + slave->error_flag = 1; + fsm->state = ec_fsm_slave_config_state_error; + EC_ERR("Failed to get DC times of slave %u: ", + slave->ring_position); + ec_datagram_print_wc_error(datagram); + return; + } + + system_time = EC_READ_U64(datagram->data); + old_offset = EC_READ_U64(datagram->data + 16); + new_offset = slave->master->app_time - system_time + old_offset; + + if (slave->master->debug_level) + EC_DBG("Slave %u: DC system_time=%llu old_offset=%llu, " + "app_time=%llu, new_offset=%llu\n", + slave->ring_position, system_time, old_offset, + slave->master->app_time, new_offset); + + // set DC system time offset + ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 8); + EC_WRITE_U64(datagram->data, new_offset); + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_slave_config_state_dc_write_offset; +} + +/*****************************************************************************/ + +/** Slave configuration state: DC WRITE OFFSET. + */ +void ec_fsm_slave_config_state_dc_write_offset( + ec_fsm_slave_config_t *fsm /**< slave state machine */ + ) +{ + ec_datagram_t *datagram = fsm->datagram; + ec_slave_t *slave = fsm->slave; + + 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 to receive DC system time offset datagram for slave %u" + " (datagram state %u).\n", + slave->ring_position, datagram->state); + return; + } + + if (datagram->working_counter != 1) { + slave->error_flag = 1; + fsm->state = ec_fsm_slave_config_state_error; + EC_ERR("Failed to set DC system time offset of slave %u: ", + slave->ring_position); ec_datagram_print_wc_error(datagram); return; } @@ -744,6 +828,8 @@ ec_fsm_slave_config_t *fsm /**< slave state machine */ ) { + // TODO check for config here + if (ec_fsm_pdo_exec(fsm->fsm_pdo)) return; @@ -840,11 +926,6 @@ return; } - if (!fsm->slave->config) { // config removed in the meantime - ec_fsm_slave_config_reconfigure(fsm); - return; - } - ec_fsm_slave_config_enter_fmmu(fsm); } @@ -862,6 +943,11 @@ const ec_fmmu_config_t *fmmu; const ec_sync_t *sync; + if (!slave->config) { // config removed in the meantime + ec_fsm_slave_config_reconfigure(fsm); + return; + } + if (slave->base_fmmu_count < slave->config->used_fmmus) { slave->error_flag = 1; fsm->state = ec_fsm_slave_config_state_error; @@ -872,7 +958,7 @@ } if (!slave->base_fmmu_count) { // skip FMMU configuration - ec_fsm_slave_config_enter_dc_read(fsm); + ec_fsm_slave_config_enter_dc_cycle(fsm); return; } @@ -928,111 +1014,32 @@ return; } - ec_fsm_slave_config_enter_dc_read(fsm); -} - -/*****************************************************************************/ - -/** Check for DCs to be configured. - */ -void ec_fsm_slave_config_enter_dc_read( - ec_fsm_slave_config_t *fsm /**< slave state machine */ - ) -{ - ec_slave_t *slave = fsm->slave; - - if (slave->base_dc_supported) { - // read DC system time and system time offset - ec_datagram_fprd(fsm->datagram, slave->station_address, 0x0910, 24); - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_slave_config_state_dc_read; - } else { - ec_fsm_slave_config_enter_safeop(fsm); - } -} - -/*****************************************************************************/ - -/** Slave configuration state: DC READ. - */ -void ec_fsm_slave_config_state_dc_read( - ec_fsm_slave_config_t *fsm /**< slave state machine */ - ) -{ - ec_datagram_t *datagram = fsm->datagram; - ec_slave_t *slave = fsm->slave; - u64 system_time, old_offset, new_offset; - - 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 to receive DC times datagram for slave %u" - " (datagram state %u).\n", - slave->ring_position, datagram->state); - return; - } - - if (datagram->working_counter != 1) { - slave->error_flag = 1; - fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to get DC times of slave %u: ", - slave->ring_position); - ec_datagram_print_wc_error(datagram); - return; - } - - system_time = EC_READ_U64(datagram->data); - old_offset = EC_READ_U64(datagram->data + 16); - new_offset = slave->master->app_time - system_time + old_offset; - - if (slave->master->debug_level) - EC_DBG("Slave %u: DC system_time=%llu old_offset=%llu, " - "app_time=%llu, new_offset=%llu\n", - slave->ring_position, system_time, old_offset, - slave->master->app_time, new_offset); - - // set DC system time offset - ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 8); - EC_WRITE_U64(datagram->data, new_offset); - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_slave_config_state_dc_offset; -} - -/*****************************************************************************/ - -/** Slave configuration state: DC OFFSET. - */ -void ec_fsm_slave_config_state_dc_offset( - ec_fsm_slave_config_t *fsm /**< slave state machine */ - ) -{ - ec_datagram_t *datagram = fsm->datagram; - ec_slave_t *slave = fsm->slave; - ec_slave_config_t *config = slave->config; // FIXME - - 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 to receive DC system time offset datagram for slave %u" - " (datagram state %u).\n", - slave->ring_position, datagram->state); - return; - } - - if (datagram->working_counter != 1) { - slave->error_flag = 1; - fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to set DC system time offset of slave %u: ", - slave->ring_position); - ec_datagram_print_wc_error(datagram); + ec_fsm_slave_config_enter_dc_cycle(fsm); +} + +/*****************************************************************************/ + +/** Check for DC to be configured. + */ +void ec_fsm_slave_config_enter_dc_cycle( + ec_fsm_slave_config_t *fsm /**< slave state machine */ + ) +{ + ec_datagram_t *datagram = fsm->datagram; + ec_slave_t *slave = fsm->slave; + ec_slave_config_t *config = slave->config; + + if (!config) { // config removed in the meantime + ec_fsm_slave_config_reconfigure(fsm); return; } if (config->dc_assign_activate) { + if (!slave->base_dc_supported) { + EC_WARN("Slave %u seems not to support distributed clocks!\n", + slave->ring_position); + } + // set DC cycle times ec_datagram_fpwr(datagram, slave->station_address, 0x09A0, 8); EC_WRITE_U32(datagram->data, config->dc_sync_cycle_times[0]); @@ -1078,7 +1085,7 @@ } // set DC start time - start_time = slave->master->app_time + 1000000000; // now plus 1 s + start_time = slave->master->app_time + 10000000ULL; // now + 100 ms if (slave->master->debug_level) EC_DBG("Slave %u: Setting DC cyclic operation start time to %llu.\n", slave->ring_position, start_time); @@ -1099,7 +1106,12 @@ { ec_datagram_t *datagram = fsm->datagram; ec_slave_t *slave = fsm->slave; - ec_slave_config_t *config = slave->config; // FIXME + ec_slave_config_t *config = slave->config; + + if (!config) { // config removed in the meantime + ec_fsm_slave_config_reconfigure(fsm); + return; + } if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) return; @@ -1153,7 +1165,7 @@ if (datagram->working_counter != 1) { slave->error_flag = 1; fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to set DC cyclia operation state of slave %u: ", + EC_ERR("Failed to set DC cyclic operation state of slave %u: ", slave->ring_position); ec_datagram_print_wc_error(datagram); return;