diff -r e6efa5e99590 master/fsm_coe.c
--- a/master/fsm_coe.c	Fri Jun 13 16:21:02 2014 +1200
+++ b/master/fsm_coe.c	Mon Jun 30 18:08:46 2014 +1200
@@ -227,7 +227,7 @@
 
 /** Executes the current state of the state machine.
  *
- * \return 1 if the datagram was used, else 0.
+ * \return 1 if the state machine is still in progress, else 0.
  */
 int ec_fsm_coe_exec(
         ec_fsm_coe_t *fsm, /**< Finite state machine. */
@@ -235,17 +235,18 @@
         )
 {
     ec_slave_t *slave = fsm->slave;
-    int datagram_used = 0;
 
     if (fsm->state == ec_fsm_coe_end || fsm->state == ec_fsm_coe_error)
-        return datagram_used;
+        return 0;
 
     if (fsm->datagram &&
             (fsm->datagram->state == EC_DATAGRAM_INIT ||
              fsm->datagram->state == EC_DATAGRAM_QUEUED ||
              fsm->datagram->state == EC_DATAGRAM_SENT)) {
         // datagram not received yet
-        return datagram_used;
+        if (datagram != fsm->datagram)
+            datagram->state = EC_DATAGRAM_INVALID;
+        return 1;
     }
 
     // Prevent concurrent access; somewhat fair scheduling (not very
@@ -264,21 +265,17 @@
 
     fsm->state(fsm, datagram);
 
-    datagram_used =
-        fsm->state != ec_fsm_coe_end && fsm->state != ec_fsm_coe_error;
-
-    if (datagram_used) {
-        fsm->datagram = datagram;
-    } else {
+    if (fsm->state == ec_fsm_coe_end || fsm->state == ec_fsm_coe_error) {
         fsm->datagram = NULL;
-        // Datagram is not used when state machine has finished, so release "lock"
         if (slave->coe_in_use_by == fsm) {
             slave->coe_in_use_by = slave->coe_waiting_for;
             slave->coe_waiting_for = NULL;
         }
+        return 0;
     }
 
-    return datagram_used;
+    fsm->datagram = datagram;
+    return 1;
 }
 
 /*****************************************************************************/