# HG changeset patch
# User Florian Pose <fp@igh-essen.com>
# Date 1166465452 0
# Node ID bc443ca0077fee7ec7f96aad1a8a368eee3a307a
# Parent  63d11bdfe3df2baa8f65fade3396f753f6c6f329
State machines re-send datagrams on timeout.

diff -r 63d11bdfe3df -r bc443ca0077f master/fsm.c
--- 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;
     }
diff -r 63d11bdfe3df -r bc443ca0077f master/fsm.h
--- 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 */
diff -r 63d11bdfe3df -r bc443ca0077f master/fsm_change.c
--- 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;
 }
 
 /*****************************************************************************/
diff -r 63d11bdfe3df -r bc443ca0077f master/fsm_change.h
--- 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. */
diff -r 63d11bdfe3df -r bc443ca0077f master/fsm_coe.c
--- 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;
     }
diff -r 63d11bdfe3df -r bc443ca0077f master/fsm_coe.h
--- 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 */
diff -r 63d11bdfe3df -r bc443ca0077f master/fsm_sii.c
--- 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;
     }
 
diff -r 63d11bdfe3df -r bc443ca0077f master/fsm_sii.h
--- 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 */
diff -r 63d11bdfe3df -r bc443ca0077f master/globals.h
--- 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