bkinnightskytw
/
ethercat
mirror of https://gitlab.com/bkinnightskytw/ethercat.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

State machines re-send datagrams on timeout.

This commit is contained in:
Florian Pose 2006-12-18 18:10:52 +00:00
parent 6fcaaff11b
commit 54b06ceb6f
9 changed files with 629 additions and 122 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

203
master/fsm.c
View File

@ -139,13 +139,20 @@ void ec_fsm_clear(ec_fsm_t *fsm /**< finite state machine */)
/**
Executes the current state of the state machine.
If the state machine's datagram is not sent or received yet, the execution
of the state machine is delayed to the next cycle.
\return false, if state machine has terminated
*/
int ec_fsm_exec(ec_fsm_t *fsm /**< finite state machine */)
{
fsm->master_state(fsm);
if (fsm->datagram.state == EC_DATAGRAM_SENT
|| fsm->datagram.state == EC_DATAGRAM_QUEUED) {
// datagram was not sent or received yet.
return ec_fsm_running(fsm);
}
fsm->master_state(fsm);
return ec_fsm_running(fsm);
}
@ -202,15 +209,17 @@ void ec_fsm_master_broadcast(ec_fsm_t *fsm /**< finite state machine */)
ec_slave_t *slave;
ec_master_t *master = fsm->master;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
if (!master->device->link_state) {
fsm->master_slaves_responding = 0;
list_for_each_entry(slave, &master->slaves, list) {
slave->online = 0;
}
}
else {
EC_ERR("Failed to receive broadcast datagram.\n");
if (datagram->state == EC_DATAGRAM_TIMED_OUT) {
// always retry
ec_master_queue_datagram(fsm->master, &fsm->datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) { // EC_DATAGRAM_ERROR
// link is down
fsm->master_slaves_responding = 0;
list_for_each_entry(slave, &master->slaves, list) {
slave->online = 0;
}
fsm->master_state = ec_fsm_master_error;
return;
@ -292,6 +301,7 @@ void ec_fsm_master_broadcast(ec_fsm_t *fsm /**< finite state machine */)
fsm->slave = list_entry(master->slaves.next, ec_slave_t, list);
ec_datagram_nprd(&fsm->datagram, fsm->slave->station_address, 0x0130, 2);
ec_master_queue_datagram(master, &fsm->datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->master_state = ec_fsm_master_read_states;
}
@ -436,6 +446,7 @@ void ec_fsm_master_action_next_slave_state(ec_fsm_t *fsm
ec_datagram_nprd(&fsm->datagram, fsm->slave->station_address,
0x0130, 2);
ec_master_queue_datagram(master, &fsm->datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->master_state = ec_fsm_master_read_states;
return;
}
@ -474,6 +485,11 @@ void ec_fsm_master_read_states(ec_fsm_t *fsm /**< finite state machine */)
ec_datagram_t *datagram = &fsm->datagram;
uint8_t new_state;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_master_queue_datagram(fsm->master, &fsm->datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
EC_ERR("Failed to receive AL state datagram for slave %i!\n",
slave->ring_position);
@ -613,6 +629,7 @@ void ec_fsm_master_action_addresses(ec_fsm_t *fsm /**< finite state machine */)
ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x0010, 2);
EC_WRITE_U16(datagram->data, fsm->slave->station_address);
ec_master_queue_datagram(fsm->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->master_state = ec_fsm_master_rewrite_addresses;
}
@ -674,10 +691,23 @@ void ec_fsm_master_rewrite_addresses(ec_fsm_t *fsm
ec_slave_t *slave = fsm->slave;
ec_datagram_t *datagram = &fsm->datagram;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
EC_ERR("Failed to write station address on slave %i.\n",
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_master_queue_datagram(fsm->master, &fsm->datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
EC_ERR("Failed to receive address datagram for slave %i.\n",
slave->ring_position);
fsm->master_state = ec_fsm_master_error;
return;
}
if (datagram->working_counter != 1) {
EC_ERR("Failed to write station address - slave %i did not respond.\n",
slave->ring_position);
fsm->master_state = ec_fsm_master_error;
return;
}
if (fsm->slave->list.next == &fsm->master->slaves) { // last slave?
@ -894,6 +924,7 @@ void ec_fsm_slavescan_start(ec_fsm_t *fsm /**< finite state machine */)
ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x0010, 2);
EC_WRITE_U16(datagram->data, fsm->slave->station_address);
ec_master_queue_datagram(fsm->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->slave_state = ec_fsm_slavescan_address;
}
@ -907,11 +938,22 @@ void ec_fsm_slavescan_address(ec_fsm_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = &fsm->datagram;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_master_queue_datagram(fsm->master, &fsm->datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to receive station address datagram for slave %i.\n",
fsm->slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to write station address of slave %i.\n",
EC_ERR("Failed to write station address - slave %i did not respond.\n",
fsm->slave->ring_position);
return;
}
@ -919,6 +961,7 @@ void ec_fsm_slavescan_address(ec_fsm_t *fsm /**< finite state machine */)
// Read AL state
ec_datagram_nprd(datagram, fsm->slave->station_address, 0x0130, 2);
ec_master_queue_datagram(fsm->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->slave_state = ec_fsm_slavescan_state;
}
@ -933,11 +976,22 @@ void ec_fsm_slavescan_state(ec_fsm_t *fsm /**< finite state machine */)
ec_datagram_t *datagram = &fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_master_queue_datagram(fsm->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to receive AL state datagram from slave %i.\n",
fsm->slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to read AL state of slave %i.\n",
EC_ERR("Failed to read AL state - slave %i did not respond.\n",
fsm->slave->ring_position);
return;
}
@ -953,6 +1007,7 @@ void ec_fsm_slavescan_state(ec_fsm_t *fsm /**< finite state machine */)
// read base data
ec_datagram_nprd(datagram, fsm->slave->station_address, 0x0000, 6);
ec_master_queue_datagram(fsm->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->slave_state = ec_fsm_slavescan_base;
}
@ -967,11 +1022,22 @@ void ec_fsm_slavescan_base(ec_fsm_t *fsm /**< finite state machine */)
ec_datagram_t *datagram = &fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_master_queue_datagram(fsm->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to receive base data datagram for slave %i.\n",
slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to read base data of slave %i.\n",
EC_ERR("Failed to read base data - slave %i did not respond.\n",
slave->ring_position);
return;
}
@ -988,6 +1054,7 @@ void ec_fsm_slavescan_base(ec_fsm_t *fsm /**< finite state machine */)
// 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->slave_state = ec_fsm_slavescan_datalink;
}
@ -1004,11 +1071,22 @@ void ec_fsm_slavescan_datalink(ec_fsm_t *fsm /**< finite state machine */)
uint16_t dl_status;
unsigned int i;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_master_queue_datagram(fsm->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to receive DL status datagram from slave %i.\n",
slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to read DL status of slave %i.\n",
EC_ERR("Failed to read DL status - slave %i did not respond.\n",
slave->ring_position);
return;
}
@ -1261,6 +1339,7 @@ void ec_fsm_slaveconf_state_init(ec_fsm_t *fsm /**< finite state machine */)
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->slave_state = ec_fsm_slaveconf_state_clear_fmmus;
}
@ -1275,11 +1354,22 @@ void ec_fsm_slaveconf_state_clear_fmmus(ec_fsm_t *fsm
{
ec_datagram_t *datagram = &fsm->datagram;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_master_queue_datagram(fsm->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed receive FMMU clearing datagram for slave %i.\n",
fsm->slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to clear FMMUs on slave %i.\n",
EC_ERR("Failed to clear FMMUs - slave %i did not respond.\n",
fsm->slave->ring_position);
return;
}
@ -1358,6 +1448,7 @@ void ec_fsm_slaveconf_enter_sync(ec_fsm_t *fsm /**< finite state machine */)
}
ec_master_queue_datagram(fsm->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->slave_state = ec_fsm_slaveconf_state_sync;
}
@ -1372,11 +1463,22 @@ void ec_fsm_slaveconf_state_sync(ec_fsm_t *fsm /**< finite state machine */)
ec_datagram_t *datagram = &fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_master_queue_datagram(fsm->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to receive sync manager configuration datagram for"
" slave %i.\n", slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to set sync managers on slave %i.\n",
EC_ERR("Failed to set sync managers - slave %i did not respond.\n",
slave->ring_position);
return;
}
@ -1461,6 +1563,7 @@ void ec_fsm_slaveconf_enter_sync2(ec_fsm_t *fsm /**< finite state machine */)
}
ec_master_queue_datagram(fsm->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->slave_state = ec_fsm_slaveconf_state_sync2;
}
@ -1475,12 +1578,24 @@ void ec_fsm_slaveconf_state_sync2(ec_fsm_t *fsm /**< finite state machine */)
ec_datagram_t *datagram = &fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_master_queue_datagram(fsm->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->slave_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->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to set process data sync managers on slave %i.\n",
slave->ring_position);
EC_ERR("Failed to set process data sync managers - slave %i did not"
" respond.\n", slave->ring_position);
return;
}
@ -1514,6 +1629,7 @@ void ec_fsm_slaveconf_enter_fmmu(ec_fsm_t *fsm /**< finite state machine */)
}
ec_master_queue_datagram(master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->slave_state = ec_fsm_slaveconf_state_fmmu;
}
@ -1528,11 +1644,22 @@ void ec_fsm_slaveconf_state_fmmu(ec_fsm_t *fsm /**< finite state machine */)
ec_datagram_t *datagram = &fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_master_queue_datagram(fsm->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->slave_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->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to set FMMUs on slave %i.\n",
EC_ERR("Failed to set FMMUs - slave %i did not respond.\n",
fsm->slave->ring_position);
return;
}

1
master/fsm.h
View File

@ -64,6 +64,7 @@ struct ec_fsm
ec_master_t *master; /**< master the FSM runs on */
ec_slave_t *slave; /**< slave the FSM runs on */
ec_datagram_t datagram; /**< datagram used in the state machine */
unsigned int retries; /**< retries on datagram timeout. */
void (*master_state)(ec_fsm_t *); /**< master state function */
unsigned int master_slaves_responding; /**< number of responding slaves */

81
master/fsm_change.c
View File

@ -159,6 +159,7 @@ void ec_fsm_change_state_start(ec_fsm_change_t *fsm
ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2);
EC_WRITE_U16(datagram->data, fsm->requested_state);
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_change_state_check;
}
@ -174,9 +175,14 @@ void ec_fsm_change_state_check(ec_fsm_change_t *fsm
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_change_state_error;
EC_ERR("Failed to send state datagram to slave %i!\n",
EC_ERR("Failed to receive state datagram from slave %i!\n",
fsm->slave->ring_position);
return;
}
@ -200,6 +206,7 @@ void ec_fsm_change_state_check(ec_fsm_change_t *fsm
ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2);
EC_WRITE_U16(datagram->data, fsm->requested_state);
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
return;
}
@ -208,6 +215,7 @@ void ec_fsm_change_state_check(ec_fsm_change_t *fsm
// read AL status from slave
ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2);
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_change_state_status;
}
@ -223,8 +231,19 @@ void ec_fsm_change_state_status(ec_fsm_change_t *fsm
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
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_change_state_error;
EC_ERR("Failed to receive state checking datagram from slave %i.\n",
slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
char req_state[EC_STATE_STRING_SIZE];
ec_state_string(fsm->requested_state, req_state);
fsm->state = ec_fsm_change_state_error;
@ -271,6 +290,7 @@ void ec_fsm_change_state_status(ec_fsm_change_t *fsm
// fetch AL status error code
ec_datagram_nprd(datagram, slave->station_address, 0x0134, 2);
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_change_state_code;
return;
}
@ -291,6 +311,7 @@ void ec_fsm_change_state_status(ec_fsm_change_t *fsm
// no timeout yet. check again
ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2);
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
}
/*****************************************************************************/
@ -346,8 +367,19 @@ void ec_fsm_change_state_code(ec_fsm_change_t *fsm
uint32_t code;
const ec_code_msg_t *al_msg;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
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_change_state_error;
EC_ERR("Failed to receive AL status code datagram from slave %i.\n",
fsm->slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
EC_WARN("Reception of AL status code datagram failed.\n");
}
else {
@ -382,6 +414,7 @@ void ec_fsm_change_state_start_ack(ec_fsm_change_t *fsm
ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2);
EC_WRITE_U16(datagram->data, slave->current_state);
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_change_state_ack;
}
@ -396,10 +429,22 @@ void ec_fsm_change_state_ack(ec_fsm_change_t *fsm /**< finite state machine */)
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
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_change_state_error;
EC_ERR("Reception of state ack datagram failed.\n");
EC_ERR("Failed to receive state ack datagram for slave %i.\n",
slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_change_state_error;
EC_ERR("Reception of state ack datagram failed - slave %i did not"
" respond.\n", slave->ring_position);
return;
}
@ -408,6 +453,7 @@ void ec_fsm_change_state_ack(ec_fsm_change_t *fsm /**< finite state machine */)
// read new AL status
ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2);
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_change_state_check_ack;
}
@ -423,10 +469,22 @@ void ec_fsm_change_state_check_ack(ec_fsm_change_t *fsm
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
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_change_state_error;
EC_ERR("Reception of state ack check datagram failed.\n");
EC_ERR("Failed to receive state ack check datagram from slave %i.\n",
slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_change_state_error;
EC_ERR("Reception of state ack check datagram failed - slave %i did"
" not respond.\n", slave->ring_position);
return;
}
@ -464,6 +522,7 @@ void ec_fsm_change_state_check_ack(ec_fsm_change_t *fsm
// reread new AL status
ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2);
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
}
/*****************************************************************************/

1
master/fsm_change.h
View File

@ -70,6 +70,7 @@ struct ec_fsm_change
{
ec_slave_t *slave; /**< slave the FSM runs on */
ec_datagram_t *datagram; /**< datagram used in the state machine */
unsigned int retries; /**< retries upon datagram timeout */
void (*state)(ec_fsm_change_t *); /**< slave state change state function */
ec_fsm_change_mode_t mode; /**< full state change, or ack only. */

382
master/fsm_coe.c
View File

@ -261,6 +261,7 @@ void ec_fsm_coe_dict_start(ec_fsm_coe_t *fsm /**< finite state machine */)
EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs!
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_request;
}
@ -268,6 +269,7 @@ void ec_fsm_coe_dict_start(ec_fsm_coe_t *fsm /**< finite state machine */)
/**
CoE state: DICT REQUEST.
\todo Timeout behavior
*/
void ec_fsm_coe_dict_request(ec_fsm_coe_t *fsm /**< finite state machine */)
@ -275,11 +277,23 @@ void ec_fsm_coe_dict_request(ec_fsm_coe_t *fsm /**< finite state machine */)
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
// FIXME: request again?
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE dictionary request failed on slave %i.\n",
slave->ring_position);
EC_ERR("Failed to receive CoE dictionary request datagram for"
" slave %i.\n", slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE dictionary request failed - slave %i did"
" not respond.\n", slave->ring_position);
return;
}
@ -287,6 +301,7 @@ void ec_fsm_coe_dict_request(ec_fsm_coe_t *fsm /**< finite state machine */)
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_check;
}
@ -301,10 +316,22 @@ void ec_fsm_coe_dict_check(ec_fsm_coe_t *fsm /**< finite state machine */)
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
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_coe_error;
EC_ERR("Reception of CoE mailbox check datagram failed on slave %i.\n",
EC_ERR("Failed to receive CoE mailbox check datagram for slave %i.\n",
slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE mailbox check datagram failed - slave %i did"
" not respond.\n",
slave->ring_position);
return;
}
@ -320,12 +347,14 @@ void ec_fsm_coe_dict_check(ec_fsm_coe_t *fsm /**< finite state machine */)
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_response;
}
@ -333,6 +362,7 @@ void ec_fsm_coe_dict_check(ec_fsm_coe_t *fsm /**< finite state machine */)
/**
CoE state: DICT RESPONSE.
\todo Timeout behavior
*/
void ec_fsm_coe_dict_response(ec_fsm_coe_t *fsm /**< finite state machine */)
@ -345,11 +375,23 @@ void ec_fsm_coe_dict_response(ec_fsm_coe_t *fsm /**< finite state machine */)
uint16_t sdo_index;
ec_sdo_t *sdo;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
// FIXME: request again?
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE dictionary response failed on slave %i.\n",
slave->ring_position);
EC_ERR("Failed to receive CoE dictionary response datagram for"
" slave %i.\n", slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE dictionary response failed - slave %i did"
" not respond.\n", slave->ring_position);
return;
}
@ -420,6 +462,7 @@ void ec_fsm_coe_dict_response(ec_fsm_coe_t *fsm /**< finite state machine */)
fsm->cycles_start = datagram->cycles_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_check;
return;
}
@ -445,6 +488,7 @@ void ec_fsm_coe_dict_response(ec_fsm_coe_t *fsm /**< finite state machine */)
EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_desc_request;
}
@ -452,6 +496,7 @@ void ec_fsm_coe_dict_response(ec_fsm_coe_t *fsm /**< finite state machine */)
/**
CoE state: DICT DESC REQUEST.
\todo Timeout behavior
*/
void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *fsm /**< finite state machine */)
@ -459,11 +504,23 @@ void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *fsm /**< finite state machine */
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
// FIXME: check for response first?
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE SDO description"
" request failed on slave %i.\n", slave->ring_position);
EC_ERR("Failed to receive CoE SDO description request datagram for"
" slave %i.\n", slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE SDO description request failed - slave %i did"
" not respond.\n", slave->ring_position);
return;
}
@ -471,6 +528,7 @@ void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *fsm /**< finite state machine */
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_desc_check;
}
@ -485,14 +543,25 @@ void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *fsm /**< finite state machine */)
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
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_coe_error;
EC_ERR("Reception of CoE mailbox check datagram failed on slave %i.\n",
EC_ERR("Failed to receive CoE mailbox check datagram from slave %i.\n",
slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE mailbox check datagram failed - slave %i did"
" not respond.\n", slave->ring_position);
return;
}
if (!ec_slave_mbox_check(datagram)) {
if (datagram->cycles_received
- fsm->cycles_start >= (cycles_t) 100 * cpu_khz) {
@ -504,12 +573,14 @@ void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *fsm /**< finite state machine */)
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_desc_response;
}
@ -517,6 +588,7 @@ void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *fsm /**< finite state machine */)
/**
CoE state: DICT DESC RESPONSE.
\todo Timeout behavior
*/
void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *fsm
@ -528,11 +600,23 @@ void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *fsm
uint8_t *data, mbox_prot;
size_t rec_size, name_size;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
// FIXME: request again?
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE SDO description"
" response failed on slave %i.\n", slave->ring_position);
EC_ERR("Failed to receive CoE SDO description response datagram from"
" slave %i.\n", slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE SDO description response failed - slave %i"
" did not respond.\n", slave->ring_position);
return;
}
@ -615,6 +699,7 @@ void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *fsm
EC_WRITE_U8 (data + 9, 0x00); // value info (no values)
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_entry_request;
}
@ -622,6 +707,7 @@ void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *fsm
/**
CoE state: DICT ENTRY REQUEST.
\todo Timeout behavior
*/
void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *fsm
@ -630,18 +716,31 @@ void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *fsm
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
// FIXME: check for response first?
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE SDO entry request failed on slave %i.\n",
slave->ring_position);
EC_ERR("Failed to receive CoE SDO entry request datagram for"
" slave %i.\n", slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE SDO entry request failed - slave %i did"
" not respond.\n", slave->ring_position);
return;
}
fsm->cycles_start = datagram->cycles_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_slave_mbox_prepare_check(slave, datagram); // can not fail
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_entry_check;
}
@ -657,14 +756,25 @@ void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *fsm
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
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_coe_error;
EC_ERR("Reception of CoE mailbox check datagram failed on slave %i.\n",
EC_ERR("Failed to receive CoE mailbox check datagram from slave %i.\n",
slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE mailbox check datagram failed - slave %i did"
" not respond.\n", slave->ring_position);
return;
}
if (!ec_slave_mbox_check(datagram)) {
if (datagram->cycles_received
- fsm->cycles_start >= (cycles_t) 100 * cpu_khz) {
@ -676,12 +786,14 @@ void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *fsm
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_entry_response;
}
@ -689,6 +801,7 @@ void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *fsm
/**
CoE state: DICT ENTRY RESPONSE.
\todo Timeout behavior
*/
void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *fsm
@ -701,11 +814,23 @@ void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *fsm
size_t rec_size, data_size;
ec_sdo_entry_t *entry;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
// FIXME: request again?
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE SDO description"
" response failed on slave %i.\n", slave->ring_position);
EC_ERR("Failed to receive CoE SDO description response datagram from"
" slave %i.\n", slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE SDO description response failed - slave %i"
" did not respond.\n", slave->ring_position);
return;
}
@ -799,6 +924,7 @@ void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *fsm
EC_WRITE_U8 (data + 9, 0x00); // value info (no values)
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_entry_request;
return;
}
@ -819,6 +945,7 @@ void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *fsm
EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_desc_request;
return;
}
@ -866,6 +993,7 @@ void ec_fsm_coe_down_start(ec_fsm_coe_t *fsm /**< finite state machine */)
memcpy(data + 10, sdodata->data, sdodata->size);
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_down_request;
}
@ -873,6 +1001,7 @@ void ec_fsm_coe_down_start(ec_fsm_coe_t *fsm /**< finite state machine */)
/**
CoE state: DOWN REQUEST.
\todo Timeout behavior
*/
void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */)
@ -880,10 +1009,23 @@ void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */)
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
// FIXME: check for response first?
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE download request failed.\n");
EC_ERR("Failed to receive CoE download request datagram for"
" slave %i.\n", slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE download request failed - slave %i did not"
" respond.\n", slave->ring_position);
return;
}
@ -891,6 +1033,7 @@ void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */)
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_down_check;
}
@ -905,10 +1048,22 @@ void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */)
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
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_coe_error;
EC_ERR("Reception of CoE mailbox check datagram failed.\n");
EC_ERR("Failed to receive CoE mailbox check datagram for slave %i.\n",
slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE mailbox check datagram failed - slave %i did"
" not respond.\n", slave->ring_position);
return;
}
@ -923,12 +1078,14 @@ void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */)
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_down_response;
}
@ -936,6 +1093,7 @@ void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */)
/**
CoE state: DOWN RESPONSE.
\todo Timeout behavior
*/
void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */)
@ -946,10 +1104,23 @@ void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */)
size_t rec_size;
ec_sdo_data_t *sdodata = fsm->sdodata;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
// FIXME: request again?
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE download response failed.\n");
EC_ERR("Failed to receive CoE download response datagram from"
" slave %i.\n", slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE download response failed - slave %i did not"
" respond.\n", slave->ring_position);
return;
}
@ -1040,6 +1211,7 @@ void ec_fsm_coe_up_start(ec_fsm_coe_t *fsm /**< finite state machine */)
}
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_request;
}
@ -1047,6 +1219,7 @@ void ec_fsm_coe_up_start(ec_fsm_coe_t *fsm /**< finite state machine */)
/**
CoE state: UP REQUEST.
\todo Timeout behavior
*/
void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */)
@ -1054,10 +1227,23 @@ void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */)
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
// FIXME: check for response first?
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE upload request failed.\n");
EC_ERR("Failed to receive CoE upload request for slave %i.\n",
slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE upload request failed - slave %i did not"
" respond.\n", slave->ring_position);
return;
}
@ -1065,6 +1251,7 @@ void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */)
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_check;
}
@ -1079,10 +1266,22 @@ void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */)
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
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_coe_error;
EC_ERR("Reception of CoE mailbox check datagram failed.\n");
EC_ERR("Failed to receive CoE mailbox check datagram from slave %i.\n",
slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE mailbox check datagram failed - slave %i did"
" not respond.\n", slave->ring_position);
return;
}
@ -1097,12 +1296,14 @@ void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */)
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_response;
}
@ -1110,6 +1311,7 @@ void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */)
/**
CoE state: UP RESPONSE.
\todo Timeout behavior
*/
void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
@ -1125,10 +1327,23 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
uint32_t complete_size;
unsigned int expedited, size_specified;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
// FIXME: request again?
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE upload response failed.\n");
EC_ERR("Failed to receive CoE upload response datagram for"
" slave %i.\n", slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE upload response failed - slave %i did not"
" respond.\n", slave->ring_position);
return;
}
@ -1239,6 +1454,7 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
}
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_seg_request;
return;
}
@ -1251,6 +1467,7 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
/**
CoE state: UP REQUEST.
\todo Timeout behavior
*/
void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *fsm /**< finite state machine */)
@ -1258,10 +1475,23 @@ void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *fsm /**< finite state machine */)
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
// FIXME: check for response first?
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE upload segment request failed.\n");
EC_ERR("Failed to receive CoE upload segment request datagram for"
" slave %i.\n", slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE upload segment request failed - slave %i did"
" not respond.\n", slave->ring_position);
return;
}
@ -1269,6 +1499,7 @@ void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *fsm /**< finite state machine */)
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_seg_check;
}
@ -1283,10 +1514,22 @@ void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
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_coe_error;
EC_ERR("Reception of CoE mailbox check datagram failed.\n");
EC_ERR("Failed to receive CoE mailbox check datagram for slave %i.\n",
slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE mailbox check datagram failed - slave %i did"
" not respond.\n", slave->ring_position);
return;
}
@ -1301,12 +1544,14 @@ void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_seg_response;
}
@ -1314,6 +1559,7 @@ void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)
/**
CoE state: UP RESPONSE.
\todo Timeout behavior
*/
void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */)
@ -1329,10 +1575,23 @@ void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */)
uint32_t seg_size;
unsigned int last_segment;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
// FIXME: request again?
ec_master_queue_datagram(fsm->slave->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE upload segment response failed.\n");
EC_ERR("Failed to receive CoE upload segment response datagram for"
" slave %i.\n", slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Reception of CoE upload segment response failed - slave %i"
" did not respond.\n", slave->ring_position);
return;
}
@ -1410,6 +1669,7 @@ void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */)
}
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_seg_request;
return;
}

1
master/fsm_coe.h
View File

@ -59,6 +59,7 @@ struct ec_fsm_coe
{
ec_slave_t *slave; /**< slave the FSM runs on */
ec_datagram_t *datagram; /**< datagram used in the state machine */
unsigned int retries; /**< retries upon datagram timeout */
void (*state)(ec_fsm_coe_t *); /**< CoE state function */
ec_sdo_data_t *sdodata; /**< input/output: SDO data object */

78
master/fsm_sii.c
View File

@ -169,6 +169,7 @@ void ec_fsm_sii_start_reading(ec_fsm_sii_t *fsm /**< finite state machine */)
EC_WRITE_U8 (datagram->data + 1, 0x01); // request read operation
EC_WRITE_U16(datagram->data + 2, fsm->offset);
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_sii_read_check;
}
@ -183,10 +184,22 @@ void ec_fsm_sii_read_check(ec_fsm_sii_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
EC_ERR("SII: Reception of read datagram failed.\n");
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_sii_error;
EC_ERR("Failed to receive SII read datagram from slave %i.\n",
fsm->slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_sii_error;
EC_ERR("Reception of SII read datagram failed - slave %i did not"
" respond.\n", fsm->slave->ring_position);
return;
}
@ -203,6 +216,7 @@ void ec_fsm_sii_read_check(ec_fsm_sii_t *fsm /**< finite state machine */)
break;
}
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_sii_read_fetch;
}
@ -217,10 +231,22 @@ void ec_fsm_sii_read_fetch(ec_fsm_sii_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
EC_ERR("SII: Reception of check/fetch datagram failed.\n");
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_sii_error;
EC_ERR("Failed to receive SII check/fetch datagram from slave %i.\n",
fsm->slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_sii_error;
EC_ERR("Reception of SII check/fetch datagram failed - slave %i did"
" not respond\n", fsm->slave->ring_position);
return;
}
@ -254,6 +280,7 @@ void ec_fsm_sii_read_fetch(ec_fsm_sii_t *fsm /**< finite state machine */)
break;
}
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
return;
}
@ -288,6 +315,7 @@ void ec_fsm_sii_start_writing(ec_fsm_sii_t *fsm /**< finite state machine */)
EC_WRITE_U32(datagram->data + 2, fsm->offset);
memcpy(datagram->data + 6, fsm->value, 2);
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_sii_write_check;
}
@ -301,10 +329,22 @@ void ec_fsm_sii_write_check(ec_fsm_sii_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
EC_ERR("SII: Reception of write datagram failed.\n");
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_sii_error;
EC_ERR("Failed to receive SII write datagram for slave %i.\n",
fsm->slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_sii_error;
EC_ERR("Reception of SII write datagram failed - slave %i did not"
" respond.\n", fsm->slave->ring_position);
return;
}
@ -314,6 +354,7 @@ void ec_fsm_sii_write_check(ec_fsm_sii_t *fsm /**< finite state machine */)
// issue check/fetch datagram
ec_datagram_nprd(datagram, fsm->slave->station_address, 0x502, 2);
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_sii_write_check2;
}
@ -327,10 +368,22 @@ void ec_fsm_sii_write_check2(ec_fsm_sii_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
EC_ERR("SII: Reception of write check datagram failed.\n");
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_sii_error;
EC_ERR("Failed to receive SII write check datagram from slave %i.\n",
fsm->slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_sii_error;
EC_ERR("Reception of SII write check datagram failed - slave %i did"
" not respond.\n", fsm->slave->ring_position);
return;
}
@ -348,6 +401,7 @@ void ec_fsm_sii_write_check2(ec_fsm_sii_t *fsm /**< finite state machine */)
// issue check/fetch datagram again
ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
return;
}

1
master/fsm_sii.h
View File

@ -67,6 +67,7 @@ struct ec_fsm_sii
{
ec_slave_t *slave; /**< slave the FSM runs on */
ec_datagram_t *datagram; /**< datagram used in the state machine */
unsigned int retries; /**< retries upon datagram timeout */
void (*state)(ec_fsm_sii_t *); /**< SII state function */
uint16_t offset; /**< input: offset in SII */

3
master/globals.h
View File

@ -61,6 +61,9 @@
/** datagram timeout in microseconds */
#define EC_IO_TIMEOUT 500
/** number of state machine retries on datagram timeout */
#define EC_FSM_RETRIES 3
/** Seconds to wait before fetching SDO dictionary
after slave entered PREOP state. */
#define EC_WAIT_SDO_DICT 3