--- a/master/fsm_coe.c	Tue Feb 13 13:36:31 2007 +0000
+++ b/master/fsm_coe.c	Tue Feb 13 13:42:37 2007 +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;
     }