diff -r 1a7067207637 -r 7bc131b92039 master/fsm_slave.c --- a/master/fsm_slave.c Fri Aug 10 15:08:44 2007 +0000 +++ b/master/fsm_slave.c Fri Aug 10 15:27:08 2007 +0000 @@ -42,6 +42,7 @@ #include "master.h" #include "mailbox.h" #include "fsm_slave.h" +#include "fsm_mapping.h" /*****************************************************************************/ @@ -56,19 +57,21 @@ void ec_fsm_slave_conf_state_start(ec_fsm_slave_t *); void ec_fsm_slave_conf_state_init(ec_fsm_slave_t *); void ec_fsm_slave_conf_state_clear_fmmus(ec_fsm_slave_t *); -void ec_fsm_slave_conf_state_sync(ec_fsm_slave_t *); +void ec_fsm_slave_conf_state_mbox_sync(ec_fsm_slave_t *); void ec_fsm_slave_conf_state_preop(ec_fsm_slave_t *); -void ec_fsm_slave_conf_state_sync2(ec_fsm_slave_t *); +void ec_fsm_slave_conf_state_pdo_sync(ec_fsm_slave_t *); void ec_fsm_slave_conf_state_fmmu(ec_fsm_slave_t *); void ec_fsm_slave_conf_state_sdoconf(ec_fsm_slave_t *); +void ec_fsm_slave_conf_state_mapconf(ec_fsm_slave_t *); void ec_fsm_slave_conf_state_saveop(ec_fsm_slave_t *); void ec_fsm_slave_conf_state_op(ec_fsm_slave_t *); -void ec_fsm_slave_conf_enter_sync(ec_fsm_slave_t *); +void ec_fsm_slave_conf_enter_mbox_sync(ec_fsm_slave_t *); void ec_fsm_slave_conf_enter_preop(ec_fsm_slave_t *); -void ec_fsm_slave_conf_enter_sync2(ec_fsm_slave_t *); +void ec_fsm_slave_conf_enter_pdo_sync(ec_fsm_slave_t *); void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *); void ec_fsm_slave_conf_enter_sdoconf(ec_fsm_slave_t *); +void ec_fsm_slave_conf_enter_mapconf(ec_fsm_slave_t *); void ec_fsm_slave_conf_enter_saveop(ec_fsm_slave_t *); void ec_fsm_slave_state_end(ec_fsm_slave_t *); @@ -90,6 +93,7 @@ ec_fsm_sii_init(&fsm->fsm_sii, fsm->datagram); ec_fsm_change_init(&fsm->fsm_change, fsm->datagram); ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); + ec_fsm_mapping_init(&fsm->fsm_map, &fsm->fsm_coe); } /*****************************************************************************/ @@ -104,6 +108,7 @@ ec_fsm_sii_clear(&fsm->fsm_sii); ec_fsm_change_clear(&fsm->fsm_change); ec_fsm_coe_clear(&fsm->fsm_coe); + ec_fsm_mapping_clear(&fsm->fsm_map); } /*****************************************************************************/ @@ -193,7 +198,6 @@ // write station address ec_datagram_apwr(fsm->datagram, fsm->slave->ring_position, 0x0010, 2); EC_WRITE_U16(fsm->datagram->data, fsm->slave->station_address); - ec_master_queue_datagram(fsm->slave->master, fsm->datagram); fsm->retries = EC_FSM_RETRIES; fsm->state = ec_fsm_slave_scan_state_address; } @@ -208,15 +212,14 @@ { ec_datagram_t *datagram = fsm->datagram; - if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { - ec_master_queue_datagram(fsm->slave->master, fsm->datagram); - return; - } + if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) + return; if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_state_error; - EC_ERR("Failed to receive station address datagram for slave %i.\n", - fsm->slave->ring_position); + EC_ERR("Failed to receive station address datagram for slave %i" + " (datagram state %i)\n", + fsm->slave->ring_position, datagram->state); return; } @@ -230,7 +233,6 @@ // Read AL state ec_datagram_nprd(datagram, fsm->slave->station_address, 0x0130, 2); - ec_master_queue_datagram(fsm->slave->master, datagram); fsm->retries = EC_FSM_RETRIES; fsm->state = ec_fsm_slave_scan_state_state; } @@ -246,15 +248,14 @@ ec_datagram_t *datagram = fsm->datagram; ec_slave_t *slave = fsm->slave; - if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { - ec_master_queue_datagram(fsm->slave->master, datagram); - return; - } + if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) + return; if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_state_error; - EC_ERR("Failed to receive AL state datagram from slave %i.\n", - fsm->slave->ring_position); + EC_ERR("Failed to receive AL state datagram from slave %i" + " (datagram state %i).\n", + fsm->slave->ring_position, datagram->state); return; } @@ -276,7 +277,6 @@ // read base data ec_datagram_nprd(datagram, fsm->slave->station_address, 0x0000, 6); - ec_master_queue_datagram(fsm->slave->master, datagram); fsm->retries = EC_FSM_RETRIES; fsm->state = ec_fsm_slave_scan_state_base; } @@ -292,15 +292,14 @@ ec_datagram_t *datagram = fsm->datagram; ec_slave_t *slave = fsm->slave; - if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { - ec_master_queue_datagram(fsm->slave->master, datagram); - return; - } + if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) + return; if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_state_error; - EC_ERR("Failed to receive base data datagram for slave %i.\n", - slave->ring_position); + EC_ERR("Failed to receive base data datagram for slave %i" + " (datagram state %i).\n", + slave->ring_position, datagram->state); return; } @@ -316,14 +315,12 @@ slave->base_revision = EC_READ_U8 (datagram->data + 1); slave->base_build = EC_READ_U16(datagram->data + 2); slave->base_fmmu_count = EC_READ_U8 (datagram->data + 4); - slave->base_sync_count = EC_READ_U8 (datagram->data + 5); if (slave->base_fmmu_count > EC_MAX_FMMUS) slave->base_fmmu_count = EC_MAX_FMMUS; // read data link status ec_datagram_nprd(datagram, slave->station_address, 0x0110, 2); - ec_master_queue_datagram(slave->master, datagram); fsm->retries = EC_FSM_RETRIES; fsm->state = ec_fsm_slave_scan_state_datalink; } @@ -341,15 +338,14 @@ uint16_t dl_status; unsigned int i; - if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { - ec_master_queue_datagram(fsm->slave->master, datagram); - return; - } + if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) + return; if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_state_error; - EC_ERR("Failed to receive DL status datagram from slave %i.\n", - slave->ring_position); + EC_ERR("Failed to receive DL status datagram from slave %i" + " (datagram state %i).\n", + slave->ring_position, datagram->state); return; } @@ -370,7 +366,7 @@ // Start fetching EEPROM size - fsm->sii_offset = 0x0040; // first category header + fsm->sii_offset = EC_FIRST_EEPROM_CATEGORY_OFFSET; // first category header ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset, EC_FSM_SII_NODE); fsm->state = ec_fsm_slave_scan_state_eeprom_size; fsm->state(fsm); // execute state immediately @@ -402,6 +398,13 @@ if (cat_type != 0xFFFF) { // not the last category fsm->sii_offset += cat_size + 2; + if (fsm->sii_offset >= EC_MAX_EEPROM_SIZE) { + EC_WARN("EEPROM size of slave %i exceeds" + " %i words (0xffff limiter missing?).\n", + slave->ring_position, EC_MAX_EEPROM_SIZE); + slave->eeprom_size = EC_FIRST_EEPROM_CATEGORY_OFFSET * 2; + goto alloc_eeprom; + } ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset, EC_FSM_SII_NODE); ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately @@ -410,6 +413,7 @@ slave->eeprom_size = (fsm->sii_offset + 1) * 2; +alloc_eeprom: if (slave->eeprom_data) { EC_INFO("Freeing old EEPROM data on slave %i...\n", slave->ring_position); @@ -498,34 +502,38 @@ EC_READ_U16(slave->eeprom_data + 2 * 0x001C); // evaluate category data - cat_word = (uint16_t *) slave->eeprom_data + 0x0040; + cat_word = (uint16_t *) slave->eeprom_data + EC_FIRST_EEPROM_CATEGORY_OFFSET; while (EC_READ_U16(cat_word) != 0xFFFF) { cat_type = EC_READ_U16(cat_word) & 0x7FFF; cat_size = EC_READ_U16(cat_word + 1); switch (cat_type) { case 0x000A: - if (ec_slave_fetch_strings(slave, (uint8_t *) (cat_word + 2))) + if (ec_slave_fetch_sii_strings( + slave, (uint8_t *) (cat_word + 2))) goto end; break; case 0x001E: - ec_slave_fetch_general(slave, (uint8_t *) (cat_word + 2)); + ec_slave_fetch_sii_general( + slave, (uint8_t *) (cat_word + 2)); break; case 0x0028: break; case 0x0029: - if (ec_slave_fetch_sync(slave, (uint8_t *) (cat_word + 2), - cat_size)) + if (ec_slave_fetch_sii_syncs( + slave, (uint8_t *) (cat_word + 2), cat_size)) goto end; break; case 0x0032: - if (ec_slave_fetch_pdo(slave, (uint8_t *) (cat_word + 2), - cat_size, EC_TX_PDO)) + if (ec_slave_fetch_sii_pdos( + slave, (uint8_t *) (cat_word + 2), + cat_size, EC_TX_PDO)) goto end; break; case 0x0033: - if (ec_slave_fetch_pdo(slave, (uint8_t *) (cat_word + 2), - cat_size, EC_RX_PDO)) + if (ec_slave_fetch_sii_pdos( + slave, (uint8_t *) (cat_word + 2), + cat_size, EC_RX_PDO)) goto end; break; default: @@ -596,7 +604,7 @@ // TODO: Implement state machine for CRC checking. if (!slave->base_fmmu_count) { // skip FMMU configuration - ec_fsm_slave_conf_enter_sync(fsm); + ec_fsm_slave_conf_enter_mbox_sync(fsm); return; } @@ -608,7 +616,6 @@ ec_datagram_npwr(datagram, slave->station_address, 0x0600, EC_FMMU_SIZE * slave->base_fmmu_count); memset(datagram->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count); - ec_master_queue_datagram(master, datagram); fsm->retries = EC_FSM_RETRIES; fsm->state = ec_fsm_slave_conf_state_clear_fmmus; } @@ -624,10 +631,8 @@ { ec_datagram_t *datagram = fsm->datagram; - if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { - ec_master_queue_datagram(fsm->slave->master, datagram); - return; - } + if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) + return; if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_state_error; @@ -644,21 +649,22 @@ return; } - ec_fsm_slave_conf_enter_sync(fsm); -} - -/*****************************************************************************/ - -/** -*/ - -void ec_fsm_slave_conf_enter_sync(ec_fsm_slave_t *fsm /**< slave state machine */) + ec_fsm_slave_conf_enter_mbox_sync(fsm); +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_slave_conf_enter_mbox_sync( + ec_fsm_slave_t *fsm /**< slave state machine */ + ) { ec_master_t *master = fsm->slave->master; ec_slave_t *slave = fsm->slave; ec_datagram_t *datagram = fsm->datagram; - const ec_sii_sync_t *sync; - ec_sii_sync_t mbox_sync; + unsigned int i; // slave is now in INIT if (slave->current_state == slave->requested_state) { @@ -670,56 +676,29 @@ return; } - if (!slave->base_sync_count) { // no sync managers + if (!slave->sii_mailbox_protocols || slave->sii_sync_count < 2) { + // no mailbox sync managers to be configured ec_fsm_slave_conf_enter_preop(fsm); return; } if (master->debug_level) { - EC_DBG("Configuring sync managers of slave %i.\n", + EC_DBG("Configuring mailbox sync managers of slave %i.\n", slave->ring_position); } // configure sync managers ec_datagram_npwr(datagram, slave->station_address, 0x0800, - EC_SYNC_SIZE * slave->base_sync_count); - memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); - - if (list_empty(&slave->sii_syncs)) { - if (slave->sii_rx_mailbox_offset && slave->sii_tx_mailbox_offset) { - if (slave->master->debug_level) - EC_DBG("Guessing sync manager settings for slave %i.\n", - slave->ring_position); - mbox_sync.index = 0; - mbox_sync.physical_start_address = slave->sii_tx_mailbox_offset; - mbox_sync.length = slave->sii_tx_mailbox_size; - mbox_sync.control_register = 0x26; - mbox_sync.enable = 0x01; - mbox_sync.est_length = 0; - ec_slave_sync_config(slave, &mbox_sync, - datagram->data + EC_SYNC_SIZE * mbox_sync.index); - mbox_sync.index = 1; - mbox_sync.physical_start_address = slave->sii_rx_mailbox_offset; - mbox_sync.length = slave->sii_rx_mailbox_size; - mbox_sync.control_register = 0x22; - mbox_sync.enable = 0x01; - mbox_sync.est_length = 0; - ec_slave_sync_config(slave, &mbox_sync, - datagram->data + EC_SYNC_SIZE * mbox_sync.index); - } - } - else if (slave->sii_mailbox_protocols) { // mailboxes present - list_for_each_entry(sync, &slave->sii_syncs, list) { - // only configure mailbox sync-managers - if (sync->index != 0 && sync->index != 1) continue; - ec_slave_sync_config(slave, sync, - datagram->data + EC_SYNC_SIZE * sync->index); - } - } - - ec_master_queue_datagram(fsm->slave->master, datagram); + EC_SYNC_SIZE * slave->sii_sync_count); + memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->sii_sync_count); + + for (i = 0; i < 2; i++) { + ec_sync_config(&slave->sii_syncs[i], + datagram->data + EC_SYNC_SIZE * i); + } + fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_slave_conf_state_sync; + fsm->state = ec_fsm_slave_conf_state_mbox_sync; } /*****************************************************************************/ @@ -728,20 +707,19 @@ Slave configuration state: SYNC. */ -void ec_fsm_slave_conf_state_sync(ec_fsm_slave_t *fsm /**< slave state machine */) +void ec_fsm_slave_conf_state_mbox_sync(ec_fsm_slave_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--) { - ec_master_queue_datagram(fsm->slave->master, datagram); - return; - } + if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) + return; if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_state_error; EC_ERR("Failed to receive sync manager configuration datagram for" - " slave %i.\n", slave->ring_position); + " slave %i (datagram state %i).\n", + slave->ring_position, datagram->state); return; } @@ -803,157 +781,21 @@ return; } - ec_fsm_slave_conf_enter_sync2(fsm); -} - -/*****************************************************************************/ - -/** -*/ - -void ec_fsm_slave_conf_enter_sync2(ec_fsm_slave_t *fsm /**< slave state machine */) -{ - ec_slave_t *slave = fsm->slave; - ec_datagram_t *datagram = fsm->datagram; - ec_sii_sync_t *sync; - - if (list_empty(&slave->sii_syncs)) { - ec_fsm_slave_conf_enter_fmmu(fsm); - return; - } - - // configure sync managers for process data - ec_datagram_npwr(datagram, slave->station_address, 0x0800, - EC_SYNC_SIZE * slave->base_sync_count); - memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); - - list_for_each_entry(sync, &slave->sii_syncs, list) { - ec_slave_sync_config(slave, sync, - datagram->data + EC_SYNC_SIZE * sync->index); - } - - ec_master_queue_datagram(fsm->slave->master, datagram); - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_slave_conf_state_sync2; -} - -/*****************************************************************************/ - -/** - Slave configuration state: SYNC2. -*/ - -void ec_fsm_slave_conf_state_sync2(ec_fsm_slave_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--) { - ec_master_queue_datagram(fsm->slave->master, datagram); - return; - } - - if (datagram->state != EC_DATAGRAM_RECEIVED) { - fsm->state = ec_fsm_slave_state_error; - EC_ERR("Failed to receive process data sync manager configuration" - " datagram for slave %i.\n", - slave->ring_position); - return; - } - - if (datagram->working_counter != 1) { - slave->error_flag = 1; - fsm->state = ec_fsm_slave_state_error; - EC_ERR("Failed to set process data sync managers - slave %i did not" - " respond.\n", slave->ring_position); - return; - } - - ec_fsm_slave_conf_enter_fmmu(fsm); -} - -/*****************************************************************************/ - -/** -*/ - -void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *fsm /**< slave state machine */) -{ - ec_slave_t *slave = fsm->slave; - ec_master_t *master = slave->master; - ec_datagram_t *datagram = fsm->datagram; - unsigned int j; - - if (!slave->base_fmmu_count) { // skip FMMU configuration - ec_fsm_slave_conf_enter_sdoconf(fsm); - return; - } - - // configure FMMUs - ec_datagram_npwr(datagram, slave->station_address, - 0x0600, EC_FMMU_SIZE * slave->base_fmmu_count); - memset(datagram->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count); - for (j = 0; j < slave->fmmu_count; j++) { - ec_slave_fmmu_config(slave, &slave->fmmus[j], - datagram->data + EC_FMMU_SIZE * j); - } - - ec_master_queue_datagram(master, datagram); - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_slave_conf_state_fmmu; -} - -/*****************************************************************************/ - -/** - Slave configuration state: FMMU. -*/ - -void ec_fsm_slave_conf_state_fmmu(ec_fsm_slave_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--) { - ec_master_queue_datagram(fsm->slave->master, datagram); - return; - } - - if (datagram->state != EC_DATAGRAM_RECEIVED) { - fsm->state = ec_fsm_slave_state_error; - EC_ERR("Failed to receive FMMUs datagram for slave %i.\n", - fsm->slave->ring_position); - return; - } - - if (datagram->working_counter != 1) { - fsm->slave->error_flag = 1; - fsm->state = ec_fsm_slave_state_error; - EC_ERR("Failed to set FMMUs - slave %i did not respond.\n", - fsm->slave->ring_position); - return; - } - - // No CoE configuration to be applied? Jump to SAVEOP state. + ec_fsm_slave_conf_enter_sdoconf(fsm); +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_slave_conf_enter_sdoconf(ec_fsm_slave_t *fsm /**< slave state machine */) +{ + ec_slave_t *slave = fsm->slave; + + // No CoE configuration to be applied? if (list_empty(&slave->sdo_confs)) { // skip SDO configuration - ec_fsm_slave_conf_enter_saveop(fsm); - return; - } - - ec_fsm_slave_conf_enter_sdoconf(fsm); -} - -/*****************************************************************************/ - -/** - */ - -void ec_fsm_slave_conf_enter_sdoconf(ec_fsm_slave_t *fsm /**< slave state machine */) -{ - ec_slave_t *slave = fsm->slave; - - if (list_empty(&slave->sdo_confs)) { // skip SDO configuration - ec_fsm_slave_conf_enter_saveop(fsm); + ec_fsm_slave_conf_enter_mapconf(fsm); return; } @@ -970,11 +812,15 @@ Slave configuration state: SDOCONF. */ -void ec_fsm_slave_conf_state_sdoconf(ec_fsm_slave_t *fsm /**< slave state machine */) +void ec_fsm_slave_conf_state_sdoconf( + ec_fsm_slave_t *fsm /**< slave state machine */ + ) { if (ec_fsm_coe_exec(&fsm->fsm_coe)) return; if (!ec_fsm_coe_success(&fsm->fsm_coe)) { + EC_ERR("SDO configuration failed for slave %u.\n", + fsm->slave->ring_position); fsm->slave->error_flag = 1; fsm->state = ec_fsm_slave_state_error; return; @@ -990,8 +836,177 @@ } // All SDOs are now configured. - - // set state to SAVEOP + ec_fsm_slave_conf_enter_mapconf(fsm); +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_slave_conf_enter_mapconf( + ec_fsm_slave_t *fsm /**< slave state machine */ + ) +{ + ec_slave_t *slave = fsm->slave; + + if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)) { + // Slave does not support CoE: no configuration of PDO mapping. + ec_fsm_slave_conf_enter_pdo_sync(fsm); + return; + } + + // start configuring PDO mapping + fsm->state = ec_fsm_slave_conf_state_mapconf; + ec_fsm_mapping_start(&fsm->fsm_map, fsm->slave); + ec_fsm_mapping_exec(&fsm->fsm_map); // execute immediately +} + +/*****************************************************************************/ + +/** + Slave configuration state: MAPCONF. +*/ + +void ec_fsm_slave_conf_state_mapconf( + ec_fsm_slave_t *fsm /**< slave state machine */ + ) +{ + if (ec_fsm_mapping_exec(&fsm->fsm_map)) return; + + if (!ec_fsm_mapping_success(&fsm->fsm_map)) { + EC_ERR("PDO mapping configuration failed for slave %u.\n", + fsm->slave->ring_position); + fsm->slave->error_flag = 1; + fsm->state = ec_fsm_slave_state_error; + return; + } + + ec_fsm_slave_conf_enter_pdo_sync(fsm); +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_slave_conf_enter_pdo_sync( + ec_fsm_slave_t *fsm /**< slave state machine */ + ) +{ + ec_slave_t *slave = fsm->slave; + ec_datagram_t *datagram = fsm->datagram; + unsigned int i; + + if (!slave->sii_sync_count) { + ec_fsm_slave_conf_enter_fmmu(fsm); + return; + } + + // configure sync managers for process data + ec_datagram_npwr(datagram, slave->station_address, 0x0800, + EC_SYNC_SIZE * slave->sii_sync_count); + memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->sii_sync_count); + + for (i = 0; i < slave->sii_sync_count; i++) { + ec_sync_config(&slave->sii_syncs[i], + datagram->data + EC_SYNC_SIZE * i); + } + + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_slave_conf_state_pdo_sync; +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_slave_conf_state_pdo_sync(ec_fsm_slave_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_state_error; + EC_ERR("Failed to receive process data sync manager configuration" + " datagram for slave %i (datagram state %i).\n", + slave->ring_position, datagram->state); + return; + } + + if (datagram->working_counter != 1) { + slave->error_flag = 1; + fsm->state = ec_fsm_slave_state_error; + EC_ERR("Failed to set process data sync managers - slave %i did not" + " respond.\n", slave->ring_position); + return; + } + + ec_fsm_slave_conf_enter_fmmu(fsm); +} + +/*****************************************************************************/ + +/** +*/ + +void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *fsm /**< slave state machine */) +{ + ec_slave_t *slave = fsm->slave; + ec_datagram_t *datagram = fsm->datagram; + unsigned int j; + + if (!slave->base_fmmu_count) { // skip FMMU configuration + ec_fsm_slave_conf_enter_saveop(fsm); + return; + } + + // configure FMMUs + ec_datagram_npwr(datagram, slave->station_address, + 0x0600, EC_FMMU_SIZE * slave->base_fmmu_count); + memset(datagram->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count); + for (j = 0; j < slave->fmmu_count; j++) { + ec_fmmu_config(&slave->fmmus[j], datagram->data + EC_FMMU_SIZE * j); + } + + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_slave_conf_state_fmmu; +} + +/*****************************************************************************/ + +/** + Slave configuration state: FMMU. +*/ + +void ec_fsm_slave_conf_state_fmmu(ec_fsm_slave_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_state_error; + EC_ERR("Failed to receive FMMUs datagram for slave %i" + " (datagram state %i).\n", + slave->ring_position, datagram->state); + return; + } + + if (datagram->working_counter != 1) { + slave->error_flag = 1; + fsm->state = ec_fsm_slave_state_error; + EC_ERR("Failed to set FMMUs - slave %i did not respond.\n", + slave->ring_position); + return; + } + ec_fsm_slave_conf_enter_saveop(fsm); }