diff -r 1a7067207637 -r 7bc131b92039 master/fsm_coe.c
--- a/master/fsm_coe.c	Fri Aug 10 15:08:44 2007 +0000
+++ b/master/fsm_coe.c	Fri Aug 10 15:27:08 2007 +0000
@@ -260,7 +260,6 @@
     EC_WRITE_U16(data + 4, 0x0000);
     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;
 }
@@ -277,16 +276,14 @@
     ec_datagram_t *datagram = fsm->datagram;
     ec_slave_t *slave = fsm->slave;
 
-    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_TIMED_OUT && fsm->retries--)
+        return; // FIXME: request again?
 
     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);
+               " slave %i (datagram state %i).\n",
+               slave->ring_position, datagram->state);
         return;
     }
 
@@ -300,7 +297,6 @@
     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;
 }
@@ -316,15 +312,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_coe_error;
-        EC_ERR("Failed to receive CoE mailbox check datagram for slave %i.\n",
-               slave->ring_position);
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+        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"
+                " (datagram state %i).\n",
+               slave->ring_position, datagram->state);
         return;
     }
 
@@ -346,14 +341,12 @@
         }
 
         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;
 }
@@ -375,16 +368,14 @@
     uint16_t sdo_index;
     ec_sdo_t *sdo;
 
-    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_TIMED_OUT && fsm->retries--)
+        return; // FIXME: request again?
 
     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);
+               " slave %i (datagram state %i).\n",
+               slave->ring_position, datagram->state);
         return;
     }
 
@@ -461,7 +452,6 @@
     if (EC_READ_U8(data + 2) & 0x80) { // more messages waiting. check again.
         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;
@@ -487,7 +477,6 @@
     EC_WRITE_U16(data + 4, 0x0000);
     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;
 }
@@ -504,16 +493,14 @@
     ec_datagram_t *datagram = fsm->datagram;
     ec_slave_t *slave = fsm->slave;
 
-    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_TIMED_OUT && fsm->retries--)
+        return; // FIXME: check for response first?
 
     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);
+               " slave %i (datagram state %i).\n",
+               slave->ring_position, datagram->state);
         return;
     }
 
@@ -527,7 +514,6 @@
     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_desc_check;
 }
@@ -543,15 +529,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_coe_error;
-        EC_ERR("Failed to receive CoE mailbox check datagram from slave %i.\n",
-               slave->ring_position);
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+        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"
+                " (datagram state %i).\n",
+                slave->ring_position, datagram->state);
         return;
     }
 
@@ -572,14 +557,12 @@
         }
 
         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;
 }
@@ -600,16 +583,14 @@
     uint8_t *data, mbox_prot;
     size_t rec_size, name_size;
 
-    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_TIMED_OUT && fsm->retries--)
+        return; // FIXME: request again?
 
     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);
+               " slave %i (datagram state %i).\n",
+               slave->ring_position, datagram->state);
         return;
     }
 
@@ -698,7 +679,6 @@
     EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex
     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;
 }
@@ -716,16 +696,14 @@
     ec_datagram_t *datagram = fsm->datagram;
     ec_slave_t *slave = fsm->slave;
 
-    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_TIMED_OUT && fsm->retries--)
+        return; // FIXME: check for response first?
 
     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);
+               " slave %i (datagram state %i).\n",
+               slave->ring_position, datagram->state);
         return;
     }
 
@@ -739,7 +717,6 @@
     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_entry_check;
 }
@@ -756,15 +733,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_coe_error;
-        EC_ERR("Failed to receive CoE mailbox check datagram from slave %i.\n",
-               slave->ring_position);
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+        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"
+                " (datagram state %i).\n",
+               slave->ring_position, datagram->state);
         return;
     }
 
@@ -785,14 +761,12 @@
         }
 
         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;
 }
@@ -814,16 +788,14 @@
     size_t rec_size, data_size;
     ec_sdo_entry_t *entry;
 
-    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_TIMED_OUT && fsm->retries--)
+        return; // FIXME: request again?
 
     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);
+               " slave %i (datagram state %i).\n",
+               slave->ring_position, datagram->state);
         return;
     }
 
@@ -923,7 +895,6 @@
         EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex
         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;
@@ -944,7 +915,6 @@
         EC_WRITE_U16(data + 4, 0x0000);
         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;
@@ -992,7 +962,6 @@
     EC_WRITE_U32(data + 6, sdodata->size);
     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;
 }
@@ -1009,16 +978,14 @@
     ec_datagram_t *datagram = fsm->datagram;
     ec_slave_t *slave = fsm->slave;
 
-    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_TIMED_OUT && fsm->retries--)
+        return; // FIXME: check for response first?
 
     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);
+               " slave %i (datagram state %i).\n",
+               slave->ring_position, datagram->state);
         return;
     }
 
@@ -1032,7 +999,6 @@
     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_down_check;
 }
@@ -1048,15 +1014,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_coe_error;
-        EC_ERR("Failed to receive CoE mailbox check datagram for slave %i.\n",
-               slave->ring_position);
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+        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"
+                " (datagram state %i).\n",
+               slave->ring_position, datagram->state);
         return;
     }
 
@@ -1077,14 +1042,12 @@
         }
 
         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;
 }
@@ -1104,16 +1067,14 @@
     size_t rec_size;
     ec_sdo_data_t *sdodata = fsm->sdodata;
 
-    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_TIMED_OUT && fsm->retries--)
+        return; // FIXME: request again?
 
     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);
+               " slave %i (datagram state %i).\n",
+               slave->ring_position, datagram->state);
         return;
     }
 
@@ -1210,7 +1171,6 @@
         ec_print_data(data, 10);
     }
 
-    ec_master_queue_datagram(fsm->slave->master, datagram);
     fsm->retries = EC_FSM_RETRIES;
     fsm->state = ec_fsm_coe_up_request;
 }
@@ -1227,16 +1187,14 @@
     ec_datagram_t *datagram = fsm->datagram;
     ec_slave_t *slave = fsm->slave;
 
-    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);
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+        return; // FIXME: check for response first?
+
+    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        fsm->state = ec_fsm_coe_error;
+        EC_ERR("Failed to receive CoE upload request for slave %i"
+                " (datagram state %i).\n",
+               slave->ring_position, datagram->state);
         return;
     }
 
@@ -1250,7 +1208,6 @@
     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_up_check;
 }
@@ -1266,15 +1223,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_coe_error;
-        EC_ERR("Failed to receive CoE mailbox check datagram from slave %i.\n",
-               slave->ring_position);
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+        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"
+                " (datagram state %i).\n",
+               slave->ring_position, datagram->state);
         return;
     }
 
@@ -1295,14 +1251,12 @@
         }
 
         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;
 }
@@ -1327,16 +1281,14 @@
     uint32_t complete_size;
     unsigned int expedited, size_specified;
 
-    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_TIMED_OUT && fsm->retries--)
+        return; // FIXME: request again?
 
     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);
+               " slave %i (datagram state %i).\n",
+               slave->ring_position, datagram->state);
         return;
     }
 
@@ -1453,7 +1405,6 @@
                 ec_print_data(data, 3);
             }
 
-            ec_master_queue_datagram(fsm->slave->master, datagram);
             fsm->retries = EC_FSM_RETRIES;
             fsm->state = ec_fsm_coe_up_seg_request;
             return;
@@ -1475,16 +1426,14 @@
     ec_datagram_t *datagram = fsm->datagram;
     ec_slave_t *slave = fsm->slave;
 
-    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_TIMED_OUT && fsm->retries--)
+        return; // FIXME: check for response first?
 
     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);
+               " slave %i (datagram state %i).\n",
+               slave->ring_position, datagram->state);
         return;
     }
 
@@ -1498,7 +1447,6 @@
     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_up_seg_check;
 }
@@ -1514,15 +1462,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_coe_error;
-        EC_ERR("Failed to receive CoE mailbox check datagram for slave %i.\n",
-               slave->ring_position);
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+        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"
+                " (datagram state %i).\n",
+               slave->ring_position, datagram->state);
         return;
     }
 
@@ -1543,14 +1490,12 @@
         }
 
         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;
 }
@@ -1575,16 +1520,14 @@
     uint32_t seg_size;
     unsigned int last_segment;
 
-    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_TIMED_OUT && fsm->retries--)
+        return; // FIXME: request again?
 
     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);
+               " slave %i (datagram state %i).\n",
+               slave->ring_position, datagram->state);
         return;
     }
 
@@ -1668,7 +1611,6 @@
             ec_print_data(data, 3);
         }
 
-        ec_master_queue_datagram(fsm->slave->master, datagram);
         fsm->retries = EC_FSM_RETRIES;
         fsm->state = ec_fsm_coe_up_seg_request;
         return;