--- a/master/fsm.c Mon Dec 18 17:53:29 2006 +0000
+++ b/master/fsm.c Mon Dec 18 18:10:52 2006 +0000
@@ -139,13 +139,20 @@
/**
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 */)
{
+ 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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
{
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 @@
// 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 @@
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 @@
// 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 @@
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 @@
// 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 @@
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 @@
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 @@
{
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 @@
}
ec_master_queue_datagram(fsm->master, datagram);
+ fsm->retries = EC_FSM_RETRIES;
fsm->slave_state = ec_fsm_slaveconf_state_sync;
}
@@ -1372,11 +1463,22 @@
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 @@
}
ec_master_queue_datagram(fsm->master, datagram);
+ fsm->retries = EC_FSM_RETRIES;
fsm->slave_state = ec_fsm_slaveconf_state_sync2;
}
@@ -1475,12 +1578,24 @@
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 @@
}
ec_master_queue_datagram(master, datagram);
+ fsm->retries = EC_FSM_RETRIES;
fsm->slave_state = ec_fsm_slaveconf_state_fmmu;
}
@@ -1528,11 +1644,22 @@
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;
}
--- a/master/fsm.h Mon Dec 18 17:53:29 2006 +0000
+++ b/master/fsm.h Mon Dec 18 18:10:52 2006 +0000
@@ -64,6 +64,7 @@
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 */
--- a/master/fsm_change.c Mon Dec 18 17:53:29 2006 +0000
+++ b/master/fsm_change.c Mon Dec 18 18:10:52 2006 +0000
@@ -159,6 +159,7 @@
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 @@
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 @@
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 @@
// 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 @@
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 @@
// 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 @@
// 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 @@
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 @@
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 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_change_state_error;
- EC_ERR("Reception of state ack 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_change_state_error;
+ 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 @@
// 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 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_change_state_error;
- EC_ERR("Reception of state ack 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_change_state_error;
+ 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 @@
// 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;
}
/*****************************************************************************/
--- a/master/fsm_change.h Mon Dec 18 17:53:29 2006 +0000
+++ b/master/fsm_change.h Mon Dec 18 18:10:52 2006 +0000
@@ -70,6 +70,7 @@
{
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. */
--- a/master/fsm_coe.c Mon Dec 18 17:53:29 2006 +0000
+++ b/master/fsm_coe.c Mon Dec 18 18:10:52 2006 +0000
@@ -261,6 +261,7 @@
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 @@
/**
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 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Reception of CoE dictionary request failed on slave %i.\n",
- slave->ring_position);
+ 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("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 @@
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 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Reception of CoE mailbox check datagram failed on slave %i.\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_coe_error;
+ 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 @@
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 @@
/**
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 @@
uint16_t sdo_index;
ec_sdo_t *sdo;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Reception of CoE dictionary response failed on slave %i.\n",
- slave->ring_position);
+ 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("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 @@
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 @@
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 @@
/**
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 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Reception of CoE SDO description"
- " request failed on slave %i.\n", slave->ring_position);
+ 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("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 @@
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 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Reception of CoE mailbox check datagram failed on slave %i.\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_coe_error;
+ 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 @@
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 @@
/**
CoE state: DICT DESC RESPONSE.
+ \todo Timeout behavior
*/
void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *fsm
@@ -528,11 +600,23 @@
uint8_t *data, mbox_prot;
size_t rec_size, name_size;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Reception of CoE SDO description"
- " response failed on slave %i.\n", slave->ring_position);
+ 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("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 @@
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 @@
/**
CoE state: DICT ENTRY REQUEST.
+ \todo Timeout behavior
*/
void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *fsm
@@ -630,18 +716,31 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Reception of CoE SDO entry request failed on slave %i.\n",
- slave->ring_position);
+ 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("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 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Reception of CoE mailbox check datagram failed on slave %i.\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_coe_error;
+ 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 @@
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 @@
/**
CoE state: DICT ENTRY RESPONSE.
+ \todo Timeout behavior
*/
void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *fsm
@@ -701,11 +814,23 @@
size_t rec_size, data_size;
ec_sdo_entry_t *entry;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Reception of CoE SDO description"
- " response failed on slave %i.\n", slave->ring_position);
+ 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("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 @@
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 @@
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 @@
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 @@
/**
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 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Reception of CoE download request failed.\n");
+ 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("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 @@
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 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Reception of CoE mailbox 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_coe_error;
+ 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 @@
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 @@
/**
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 @@
size_t rec_size;
ec_sdo_data_t *sdodata = fsm->sdodata;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Reception of CoE download response failed.\n");
+ 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("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 @@
}
ec_master_queue_datagram(fsm->slave->master, datagram);
+ fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_request;
}
@@ -1047,6 +1219,7 @@
/**
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 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Reception of CoE upload request failed.\n");
+ 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("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 @@
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 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Reception of CoE mailbox 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_coe_error;
+ 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 @@
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 @@
/**
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 @@
uint32_t complete_size;
unsigned int expedited, size_specified;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Reception of CoE upload response failed.\n");
+ 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("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 @@
}
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 @@
/**
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 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Reception of CoE upload segment request failed.\n");
+ 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("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 @@
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 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Reception of CoE mailbox 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_coe_error;
+ 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 @@
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 @@
/**
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 @@
uint32_t seg_size;
unsigned int last_segment;
- if (datagram->state != EC_DATAGRAM_RECEIVED
- || datagram->working_counter != 1) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Reception of CoE upload segment response failed.\n");
+ 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("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 @@
}
ec_master_queue_datagram(fsm->slave->master, datagram);
+ fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_seg_request;
return;
}
--- a/master/fsm_coe.h Mon Dec 18 17:53:29 2006 +0000
+++ b/master/fsm_coe.h Mon Dec 18 18:10:52 2006 +0000
@@ -59,6 +59,7 @@
{
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 */
--- a/master/fsm_sii.c Mon Dec 18 17:53:29 2006 +0000
+++ b/master/fsm_sii.c Mon Dec 18 18:10:52 2006 +0000
@@ -169,6 +169,7 @@
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 @@
{
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");
- fsm->state = ec_fsm_sii_error;
+ 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 @@
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 @@
{
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");
- fsm->state = ec_fsm_sii_error;
+ 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 @@
break;
}
ec_master_queue_datagram(fsm->slave->master, datagram);
+ fsm->retries = EC_FSM_RETRIES;
return;
}
@@ -288,6 +315,7 @@
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 @@
{
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");
- fsm->state = ec_fsm_sii_error;
+ 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 @@
// 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 @@
{
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");
- fsm->state = ec_fsm_sii_error;
+ 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 @@
// issue check/fetch datagram again
ec_master_queue_datagram(fsm->slave->master, datagram);
+ fsm->retries = EC_FSM_RETRIES;
return;
}
--- a/master/fsm_sii.h Mon Dec 18 17:53:29 2006 +0000
+++ b/master/fsm_sii.h Mon Dec 18 18:10:52 2006 +0000
@@ -67,6 +67,7 @@
{
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 */
--- a/master/globals.h Mon Dec 18 17:53:29 2006 +0000
+++ b/master/globals.h Mon Dec 18 18:10:52 2006 +0000
@@ -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