# HG changeset patch
# User Florian Pose <fp@igh-essen.com>
# Date 1357835801 -3600
# Node ID 9cdd7669dc0bd89a7fd89369a3e3d0ccc3302c49
# Parent  505cf41488a4fb49906a752c84a6ab57ec569eeb
Avoided io_sem locking from userspace/rtdm library.

diff -r 505cf41488a4 -r 9cdd7669dc0b NEWS
--- a/NEWS	Thu Jan 10 12:34:58 2013 +0100
+++ b/NEWS	Thu Jan 10 17:36:41 2013 +0100
@@ -17,6 +17,7 @@
 * Added fix for ESC port order (DC delay calculation).
 * Added e1000 driver for kernels 2.6.35, 3.0, 3.4.
 * Added 8139too driver for kernel 3.4.
+* Avoided slave FSM datagram queue; implemented datagram ring instead.
 
 Changes in 1.5.0:
 
diff -r 505cf41488a4 -r 9cdd7669dc0b master/fsm_coe.c
--- a/master/fsm_coe.c	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_coe.c	Thu Jan 10 17:36:41 2013 +0100
@@ -27,10 +27,8 @@
  *
  *****************************************************************************/
 
-/**
-   \file
-   EtherCAT CoE state machines.
-*/
+/** \file EtherCAT CoE state machines.
+ */
 
 /*****************************************************************************/
 
@@ -68,44 +66,43 @@
 
 /*****************************************************************************/
 
-void ec_fsm_coe_dict_start(ec_fsm_coe_t *);
-void ec_fsm_coe_dict_request(ec_fsm_coe_t *);
-void ec_fsm_coe_dict_check(ec_fsm_coe_t *);
-void ec_fsm_coe_dict_response(ec_fsm_coe_t *);
-void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *);
-void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *);
-void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *);
-void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *);
-void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *);
-void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *);
-
-void ec_fsm_coe_down_start(ec_fsm_coe_t *);
-void ec_fsm_coe_down_request(ec_fsm_coe_t *);
-void ec_fsm_coe_down_check(ec_fsm_coe_t *);
-void ec_fsm_coe_down_response(ec_fsm_coe_t *);
-void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *);
-void ec_fsm_coe_down_seg_response(ec_fsm_coe_t *);
-
-void ec_fsm_coe_up_start(ec_fsm_coe_t *);
-void ec_fsm_coe_up_request(ec_fsm_coe_t *);
-void ec_fsm_coe_up_check(ec_fsm_coe_t *);
-void ec_fsm_coe_up_response(ec_fsm_coe_t *);
-void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *);
-void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *);
-void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *);
-
-void ec_fsm_coe_end(ec_fsm_coe_t *);
-void ec_fsm_coe_error(ec_fsm_coe_t *);
-
-/*****************************************************************************/
-
-/**
-   SDO abort messages.
-   The "abort SDO transfer request" supplies an abort code,
-   which can be translated to clear text. This table does
-   the mapping of the codes and messages.
-*/
-
+void ec_fsm_coe_dict_start(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_request(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_response(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *, ec_datagram_t *);
+
+void ec_fsm_coe_down_start(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_down_request(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_down_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_down_response(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_down_seg_response(ec_fsm_coe_t *, ec_datagram_t *);
+
+void ec_fsm_coe_up_start(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_up_request(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_up_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_up_response(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *, ec_datagram_t *);
+
+void ec_fsm_coe_end(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_error(ec_fsm_coe_t *, ec_datagram_t *);
+
+/*****************************************************************************/
+
+/** SDO abort messages.
+ *
+ * The "abort SDO transfer request" supplies an abort code, which can be
+ * translated to clear text. This table does the mapping of the codes and
+ * messages.
+ */
 const ec_code_msg_t sdo_abort_messages[] = {
     {0x05030000, "Toggle bit not changed"},
     {0x05040000, "SDO protocol timeout"},
@@ -147,7 +144,10 @@
 
 /** Outputs an SDO abort message.
  */
-void ec_canopen_abort_msg(const ec_slave_t *slave, uint32_t abort_code)
+void ec_canopen_abort_msg(
+        const ec_slave_t *slave, /**< Slave. */
+        uint32_t abort_code /**< Abort code to search for. */
+        )
 {
     const ec_code_msg_t *abort_msg;
 
@@ -164,37 +164,34 @@
 
 /*****************************************************************************/
 
-/**
-   Constructor.
-*/
-
-void ec_fsm_coe_init(ec_fsm_coe_t *fsm, /**< finite state machine */
-                     ec_datagram_t *datagram /**< datagram */
-                     )
+/** Constructor.
+ */
+void ec_fsm_coe_init(
+        ec_fsm_coe_t *fsm /**< Finite state machine */
+        )
 {
     fsm->state = NULL;
-    fsm->datagram = datagram;
-}
-
-/*****************************************************************************/
-
-/**
-   Destructor.
-*/
-
-void ec_fsm_coe_clear(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-}
-
-/*****************************************************************************/
-
-/**
-   Starts reading a slaves' SDO dictionary.
-*/
-
-void ec_fsm_coe_dictionary(ec_fsm_coe_t *fsm, /**< finite state machine */
-                           ec_slave_t *slave /**< EtherCAT slave */
-                           )
+    fsm->datagram = NULL;
+}
+
+/*****************************************************************************/
+
+/** Destructor.
+ */
+void ec_fsm_coe_clear(
+        ec_fsm_coe_t *fsm /**< Finite state machine */
+        )
+{
+}
+
+/*****************************************************************************/
+
+/** Starts reading a slaves' SDO dictionary.
+ */
+void ec_fsm_coe_dictionary(
+        ec_fsm_coe_t *fsm, /**< Finite state machine */
+        ec_slave_t *slave /**< EtherCAT slave */
+        )
 {
     fsm->slave = slave;
     fsm->state = ec_fsm_coe_dict_start;
@@ -202,10 +199,8 @@
 
 /*****************************************************************************/
 
-/**
-   Starts to transfer an SDO to/from a slave.
-*/
-
+/** Starts to transfer an SDO to/from a slave.
+ */
 void ec_fsm_coe_transfer(
         ec_fsm_coe_t *fsm, /**< State machine. */
         ec_slave_t *slave, /**< EtherCAT slave. */
@@ -214,34 +209,58 @@
 {
     fsm->slave = slave;
     fsm->request = request;
-    if (request->dir == EC_DIR_OUTPUT)
+
+    if (request->dir == EC_DIR_OUTPUT) {
         fsm->state = ec_fsm_coe_down_start;
-    else
+    }
+    else {
         fsm->state = ec_fsm_coe_up_start;
-}
-
-/*****************************************************************************/
-
-/**
-   Executes the current state of the state machine.
-   \return false, if state machine has terminated
-*/
-
-int ec_fsm_coe_exec(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-    fsm->state(fsm);
-
-    return fsm->state != ec_fsm_coe_end && fsm->state != ec_fsm_coe_error;
-}
-
-/*****************************************************************************/
-
-/**
-   Returns, if the state machine terminated with success.
-   \return non-zero if successful.
-*/
-
-int ec_fsm_coe_success(ec_fsm_coe_t *fsm /**< Finite state machine */)
+    }
+}
+
+/*****************************************************************************/
+
+/** Executes the current state of the state machine.
+ *
+ * \return 1 if the datagram was used, else 0.
+ */
+int ec_fsm_coe_exec(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    int datagram_used = 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;
+    }
+
+    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 {
+        fsm->datagram = NULL;
+    }
+
+    return datagram_used;
+}
+
+/*****************************************************************************/
+
+/** Returns, if the state machine terminated with success.
+ * \return non-zero if successful.
+ */
+int ec_fsm_coe_success(
+        const ec_fsm_coe_t *fsm /**< Finite state machine */
+        )
 {
     return fsm->state == ec_fsm_coe_end;
 }
@@ -288,33 +307,15 @@
  *  CoE dictionary state machine
  *****************************************************************************/
 
-/**
-   CoE state: DICT START.
-*/
-
-void ec_fsm_coe_dict_start(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
-    ec_slave_t *slave = fsm->slave;
-    uint8_t *data;
-
-    if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
-        EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");
-        fsm->state = ec_fsm_coe_error;
-        return;
-    }
-
-    if (slave->sii.has_general && !slave->sii.coe_details.enable_sdo_info) {
-        EC_SLAVE_ERR(slave, "Slave does not support"
-                " SDO information service!\n");
-        fsm->state = ec_fsm_coe_error;
-        return;
-    }
-
-    data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8);
+int ec_fsm_coe_prepare_dict(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    uint8_t *data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8);
     if (IS_ERR(data)) {
-        fsm->state = ec_fsm_coe_error;
-        return;
+        return PTR_ERR(data);
     }
 
     EC_WRITE_U16(data, 0x8 << 12); // SDO information
@@ -323,41 +324,76 @@
     EC_WRITE_U16(data + 4, 0x0000);
     EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs!
 
+    fsm->state = ec_fsm_coe_dict_request;
+    return 0;
+}
+
+/*****************************************************************************/
+
+/** CoE state: DICT START.
+ */
+void ec_fsm_coe_dict_start(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+
+    if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
+        EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");
+        fsm->state = ec_fsm_coe_error;
+        return;
+    }
+
+    if (slave->sii.has_general && !slave->sii.coe_details.enable_sdo_info) {
+        EC_SLAVE_ERR(slave, "Slave does not support"
+                " SDO information service!\n");
+        fsm->state = ec_fsm_coe_error;
+        return;
+    }
+
     fsm->retries = EC_FSM_RETRIES;
-    fsm->state = ec_fsm_coe_dict_request;
-}
-
-/*****************************************************************************/
-
-/**
-   CoE state: DICT REQUEST.
-   \todo Timeout behavior
-*/
-
-void ec_fsm_coe_dict_request(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
-    ec_slave_t *slave = fsm->slave;
-
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return; // FIXME: request again?
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+
+    if (ec_fsm_coe_prepare_dict(fsm, datagram)) {
+        fsm->state = ec_fsm_coe_error;
+    }
+}
+
+/*****************************************************************************/
+
+/** CoE state: DICT REQUEST.
+ * \todo Timeout behavior
+ */
+void ec_fsm_coe_dict_request(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        if (ec_fsm_coe_prepare_dict(fsm, datagram)) {
+            fsm->state = ec_fsm_coe_error;
+        }
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary"
                 " request datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE dictionary request failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    fsm->jiffies_start = datagram->jiffies_sent;
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    fsm->jiffies_start = fsm->datagram->jiffies_sent;
 
     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
     fsm->retries = EC_FSM_RETRIES;
@@ -366,36 +402,39 @@
 
 /*****************************************************************************/
 
-/**
-   CoE state: DICT CHECK.
-*/
-
-void ec_fsm_coe_dict_check(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
-    ec_slave_t *slave = fsm->slave;
-
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return;
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+/** CoE state: DICT CHECK.
+ */
+void ec_fsm_coe_dict_check(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave,"Reception of CoE mailbox check"
                 " datagram failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    if (!ec_slave_mbox_check(datagram)) {
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    if (!ec_slave_mbox_check(fsm->datagram)) {
         unsigned long diff_ms =
-            (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+            (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+            1000 / HZ;
         if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {
             fsm->state = ec_fsm_coe_error;
             EC_SLAVE_ERR(slave, "Timeout while waiting for"
@@ -416,14 +455,39 @@
 
 /*****************************************************************************/
 
+int ec_fsm_coe_dict_prepare_desc(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    u8 *data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8);
+    if (IS_ERR(data)) {
+        return PTR_ERR(data);
+    }
+
+    EC_WRITE_U16(data, 0x8 << 12); // SDO information
+    EC_WRITE_U8 (data + 2, 0x03); // Get object description request
+    EC_WRITE_U8 (data + 3, 0x00);
+    EC_WRITE_U16(data + 4, 0x0000);
+    EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index
+
+    fsm->state = ec_fsm_coe_dict_desc_request;
+    return 0;
+}
+
+/*****************************************************************************/
+
 /**
    CoE state: DICT RESPONSE.
    \todo Timeout behavior
 */
 
-void ec_fsm_coe_dict_response(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_coe_dict_response(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
     uint8_t *data, mbox_prot;
     size_t rec_size;
@@ -433,25 +497,27 @@
     bool first_segment;
     size_t index_list_offset;
 
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return; // FIXME: request again?
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary"
                 " response datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE dictionary response failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
     if (IS_ERR(data)) {
         fsm->state = ec_fsm_coe_error;
         return;
@@ -543,7 +609,7 @@
 
     if (EC_READ_U8(data + 2) & 0x80 || fragments_left) {
         // more messages waiting. check again.
-        fsm->jiffies_start = datagram->jiffies_sent;
+        fsm->jiffies_start = fsm->datagram->jiffies_sent;
         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
         fsm->retries = EC_FSM_RETRIES;
         fsm->state = ec_fsm_coe_dict_check;
@@ -559,20 +625,10 @@
     // fetch SDO descriptions
     fsm->sdo = list_entry(slave->sdo_dictionary.next, ec_sdo_t, list);
 
-    data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8);
-    if (IS_ERR(data)) {
-        fsm->state = ec_fsm_coe_error;
-        return;
-    }
-
-    EC_WRITE_U16(data, 0x8 << 12); // SDO information
-    EC_WRITE_U8 (data + 2, 0x03); // Get object description request
-    EC_WRITE_U8 (data + 3, 0x00);
-    EC_WRITE_U16(data + 4, 0x0000);
-    EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index
-
     fsm->retries = EC_FSM_RETRIES;
-    fsm->state = ec_fsm_coe_dict_desc_request;
+    if (ec_fsm_coe_dict_prepare_desc(fsm, datagram)) {
+        fsm->state = ec_fsm_coe_error;
+    }
 }
 
 /*****************************************************************************/
@@ -582,31 +638,37 @@
    \todo Timeout behavior
 */
 
-void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
-    ec_slave_t *slave = fsm->slave;
-
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return; // FIXME: check for response first?
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_coe_dict_desc_request(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        if (ec_fsm_coe_dict_prepare_desc(fsm, datagram)) {
+            fsm->state = ec_fsm_coe_error;
+        }
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE SDO"
                 " description request datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE SDO description"
                 " request failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    fsm->jiffies_start = datagram->jiffies_sent;
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    fsm->jiffies_start = fsm->datagram->jiffies_sent;
 
     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
     fsm->retries = EC_FSM_RETRIES;
@@ -619,32 +681,37 @@
    CoE state: DICT DESC CHECK.
 */
 
-void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
-    ec_slave_t *slave = fsm->slave;
-
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return;
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_coe_dict_desc_check(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
                 " datagram failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    if (!ec_slave_mbox_check(datagram)) {
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    if (!ec_slave_mbox_check(fsm->datagram)) {
         unsigned long diff_ms =
-            (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+            (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+            1000 / HZ;
         if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {
             fsm->state = ec_fsm_coe_error;
             EC_SLAVE_ERR(slave, "Timeout while waiting for"
@@ -666,40 +733,68 @@
 
 /*****************************************************************************/
 
+int ec_fsm_coe_dict_prepare_entry(
+        ec_fsm_coe_t *fsm, /**< Finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    u8 *data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10);
+    if (IS_ERR(data)) {
+        return PTR_ERR(data);
+    }
+
+    EC_WRITE_U16(data, 0x8 << 12); // SDO information
+    EC_WRITE_U8 (data + 2, 0x05); // Get entry description request
+    EC_WRITE_U8 (data + 3, 0x00);
+    EC_WRITE_U16(data + 4, 0x0000);
+    EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index
+    EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex
+    EC_WRITE_U8 (data + 9, 0x01); // value info (access rights only)
+
+    fsm->state = ec_fsm_coe_dict_entry_request;
+    return 0;
+}
+
+/*****************************************************************************/
+
 /**
    CoE state: DICT DESC RESPONSE.
    \todo Timeout behavior
 */
 
-void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *fsm
-                                   /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_coe_dict_desc_response(
+        ec_fsm_coe_t *fsm, /**< Finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
     ec_sdo_t *sdo = fsm->sdo;
     uint8_t *data, mbox_prot;
     size_t rec_size, name_size;
 
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return; // FIXME: request again?
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE SDO description"
                 " response datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE SDO description"
                 " response failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
     if (IS_ERR(data)) {
         fsm->state = ec_fsm_coe_error;
         return;
@@ -789,23 +884,11 @@
     // start fetching entries
 
     fsm->subindex = 0;
-
-    data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10);
-    if (IS_ERR(data)) {
-        fsm->state = ec_fsm_coe_error;
-        return;
-    }
-
-    EC_WRITE_U16(data, 0x8 << 12); // SDO information
-    EC_WRITE_U8 (data + 2, 0x05); // Get entry description request
-    EC_WRITE_U8 (data + 3, 0x00);
-    EC_WRITE_U16(data + 4, 0x0000);
-    EC_WRITE_U16(data + 6, sdo->index); // SDO index
-    EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex
-    EC_WRITE_U8 (data + 9, 0x01); // value info (access rights only)
-
     fsm->retries = EC_FSM_RETRIES;
-    fsm->state = ec_fsm_coe_dict_entry_request;
+
+    if (ec_fsm_coe_dict_prepare_entry(fsm, datagram)) {
+        fsm->state = ec_fsm_coe_error;
+    }
 }
 
 /*****************************************************************************/
@@ -815,31 +898,36 @@
    \todo Timeout behavior
 */
 
-void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *fsm
-                                   /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
-    ec_slave_t *slave = fsm->slave;
-
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return; // FIXME: check for response first?
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_coe_dict_entry_request(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        if (ec_fsm_coe_dict_prepare_entry(fsm, datagram)) {
+            fsm->state = ec_fsm_coe_error;
+        }
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE SDO entry"
                 " request datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE SDO entry request failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    fsm->jiffies_start = datagram->jiffies_sent;
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    fsm->jiffies_start = fsm->datagram->jiffies_sent;
 
     ec_slave_mbox_prepare_check(slave, datagram); // can not fail
     fsm->retries = EC_FSM_RETRIES;
@@ -852,33 +940,37 @@
    CoE state: DICT ENTRY CHECK.
 */
 
-void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *fsm
-                                 /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
-    ec_slave_t *slave = fsm->slave;
-
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return;
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_coe_dict_entry_check(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_slave_mbox_prepare_check(slave, datagram); // can not fail
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
                 " datagram failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    if (!ec_slave_mbox_check(datagram)) {
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    if (!ec_slave_mbox_check(fsm->datagram)) {
         unsigned long diff_ms =
-            (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+            (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+            1000 / HZ;
         if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {
             fsm->state = ec_fsm_coe_error;
             EC_SLAVE_ERR(slave, "Timeout while waiting for"
@@ -905,10 +997,11 @@
    \todo Timeout behavior
 */
 
-void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *fsm
-                                    /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_coe_dict_entry_response(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
     ec_sdo_t *sdo = fsm->sdo;
     uint8_t *data, mbox_prot;
@@ -916,26 +1009,28 @@
     ec_sdo_entry_t *entry;
     u16 word;
 
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return; // FIXME: request again?
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE SDO"
                 " description response datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE SDO description"
                 " response failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
     if (IS_ERR(data)) {
         fsm->state = ec_fsm_coe_error;
         return;
@@ -1046,45 +1141,27 @@
     }
 
     if (fsm->subindex < sdo->max_subindex) {
+
         fsm->subindex++;
-
-        data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10);
-        if (IS_ERR(data)) {
+        fsm->retries = EC_FSM_RETRIES;
+
+        if (ec_fsm_coe_dict_prepare_entry(fsm, datagram)) {
             fsm->state = ec_fsm_coe_error;
-            return;
-        }
-
-        EC_WRITE_U16(data, 0x8 << 12); // SDO information
-        EC_WRITE_U8 (data + 2, 0x05); // Get entry description request
-        EC_WRITE_U8 (data + 3, 0x00);
-        EC_WRITE_U16(data + 4, 0x0000);
-        EC_WRITE_U16(data + 6, sdo->index); // SDO index
-        EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex
-        EC_WRITE_U8 (data + 9, 0x00); // value info (no values)
-
-        fsm->retries = EC_FSM_RETRIES;
-        fsm->state = ec_fsm_coe_dict_entry_request;
+        }
+
         return;
     }
 
     // another SDO description to fetch?
     if (fsm->sdo->list.next != &slave->sdo_dictionary) {
+
         fsm->sdo = list_entry(fsm->sdo->list.next, ec_sdo_t, list);
-
-        data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8);
-        if (IS_ERR(data)) {
+        fsm->retries = EC_FSM_RETRIES;
+
+        if (ec_fsm_coe_dict_prepare_desc(fsm, datagram)) {
             fsm->state = ec_fsm_coe_error;
-            return;
-        }
-
-        EC_WRITE_U16(data, 0x8 << 12); // SDO information
-        EC_WRITE_U8 (data + 2, 0x03); // Get object description request
-        EC_WRITE_U8 (data + 3, 0x00);
-        EC_WRITE_U16(data + 4, 0x0000);
-        EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index
-
-        fsm->retries = EC_FSM_RETRIES;
-        fsm->state = ec_fsm_coe_dict_desc_request;
+        }
+
         return;
     }
 
@@ -1095,52 +1172,22 @@
  *  CoE state machine
  *****************************************************************************/
 
-/** CoE state: DOWN START.
- */
-void ec_fsm_coe_down_start(
-        ec_fsm_coe_t *fsm /**< finite state machine */
-        )
-{
-    ec_datagram_t *datagram = fsm->datagram;
+int ec_fsm_coe_prepare_down_start(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    u8 *data;
     ec_slave_t *slave = fsm->slave;
     ec_sdo_request_t *request = fsm->request;
-    uint8_t *data;
     uint8_t data_set_size;
 
-    if (fsm->slave->master->debug_level) {
-        char subidxstr[10];
-        if (request->complete_access) {
-            subidxstr[0] = 0x00;
-        } else {
-            sprintf(subidxstr, ":%02X", request->subindex);
-        }
-        EC_SLAVE_DBG(slave, 1, "Downloading SDO 0x%04X%s.\n",
-                request->index, subidxstr);
-        ec_print_data(request->data, request->data_size);
-    }
-
-    if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
-        EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");
-        request->errno = EPROTONOSUPPORT;
-        fsm->state = ec_fsm_coe_error;
-        return;
-    }
-
-    if (slave->configured_rx_mailbox_size <
-            EC_MBOX_HEADER_SIZE + EC_COE_DOWN_REQ_HEADER_SIZE) {
-        EC_SLAVE_ERR(slave, "Mailbox too small!\n");
-        request->errno = EOVERFLOW;
-        fsm->state = ec_fsm_coe_error;
-        return;
-    }
-
     if (request->data_size <= 4) { // use expedited transfer type
         data = ec_slave_mbox_prepare_send(slave, datagram, 0x03,
                 EC_COE_DOWN_REQ_HEADER_SIZE);
         if (IS_ERR(data)) {
             request->errno = PTR_ERR(data);
-            fsm->state = ec_fsm_coe_error;
-            return;
+            return PTR_ERR(data);
         }
 
         fsm->remaining = 0;
@@ -1181,8 +1228,7 @@
                 data_size);
         if (IS_ERR(data)) {
             request->errno = PTR_ERR(data);
-            fsm->state = ec_fsm_coe_error;
-            return;
+            return PTR_ERR(data);
         }
 
         fsm->offset = 0;
@@ -1212,9 +1258,56 @@
         }
     }
 
+    fsm->state = ec_fsm_coe_down_request;
+    return 0;
+}
+
+/****************************************************************************/
+
+/** CoE state: DOWN START.
+ */
+void ec_fsm_coe_down_start(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    ec_sdo_request_t *request = fsm->request;
+
+    if (fsm->slave->master->debug_level) {
+        char subidxstr[10];
+        if (request->complete_access) {
+            subidxstr[0] = 0x00;
+        } else {
+            sprintf(subidxstr, ":%02X", request->subindex);
+        }
+        EC_SLAVE_DBG(slave, 1, "Downloading SDO 0x%04X%s.\n",
+                request->index, subidxstr);
+        ec_print_data(request->data, request->data_size);
+    }
+
+    if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
+        EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");
+        request->errno = EPROTONOSUPPORT;
+        fsm->state = ec_fsm_coe_error;
+        return;
+    }
+
+    if (slave->configured_rx_mailbox_size <
+            EC_MBOX_HEADER_SIZE + EC_COE_DOWN_REQ_HEADER_SIZE) {
+        EC_SLAVE_ERR(slave, "Mailbox too small!\n");
+        request->errno = EOVERFLOW;
+        fsm->state = ec_fsm_coe_error;
+        return;
+    }
+
+
     fsm->request->jiffies_sent = jiffies;
     fsm->retries = EC_FSM_RETRIES;
-    fsm->state = ec_fsm_coe_down_request;
+
+    if (ec_fsm_coe_prepare_down_start(fsm, datagram)) {
+        fsm->state = ec_fsm_coe_error;
+    }
 }
 
 /*****************************************************************************/
@@ -1224,28 +1317,34 @@
    \todo Timeout behavior
 */
 
-void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_coe_down_request(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
     unsigned long diff_ms;
 
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return; // FIXME: check for response first?
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        if (ec_fsm_coe_prepare_down_start(fsm, datagram)) {
+            fsm->state = ec_fsm_coe_error;
+        }
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE download"
                 " request datagram: ");
-        ec_datagram_print_state(datagram);
+        ec_datagram_print_state(fsm->datagram);
         return;
     }
 
     diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
 
-    if (datagram->working_counter != 1) {
-        if (!datagram->working_counter) {
+    if (fsm->datagram->working_counter != 1) {
+        if (!fsm->datagram->working_counter) {
             if (diff_ms < fsm->request->response_timeout) {
 #if DEBUG_RETRIES
                 EC_SLAVE_DBG(slave, 1, "Slave did not respond to SDO"
@@ -1253,6 +1352,9 @@
                         diff_ms);
 #endif
                 // no response; send request datagram again
+                if (ec_fsm_coe_prepare_down_start(fsm, datagram)) {
+                    fsm->state = ec_fsm_coe_error;
+                }
                 return;
             }
         }
@@ -1261,7 +1363,7 @@
         EC_SLAVE_ERR(slave, "Reception of CoE download request"
                 " for SDO 0x%04x:%x failed with timeout after %lu ms: ",
                 fsm->request->index, fsm->request->subindex, diff_ms);
-        ec_datagram_print_wc_error(datagram);
+        ec_datagram_print_wc_error(fsm->datagram);
         return;
     }
 
@@ -1272,7 +1374,7 @@
     }
 #endif
 
-    fsm->jiffies_start = datagram->jiffies_sent;
+    fsm->jiffies_start = fsm->datagram->jiffies_sent;
 
     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
     fsm->retries = EC_FSM_RETRIES;
@@ -1283,35 +1385,40 @@
 
 /** CoE state: DOWN CHECK.
  */
-void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
-    ec_slave_t *slave = fsm->slave;
-
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return;
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_coe_down_check(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check"
                 " datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
                 " datagram failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    if (!ec_slave_mbox_check(datagram)) {
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    if (!ec_slave_mbox_check(fsm->datagram)) {
         unsigned long diff_ms =
-            (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+            (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+            1000 / HZ;
         if (diff_ms >= fsm->request->response_timeout) {
             fsm->request->errno = EIO;
             fsm->state = ec_fsm_coe_error;
@@ -1337,32 +1444,32 @@
 /** Prepare a download segment request.
  */
 void ec_fsm_coe_down_prepare_segment_request(
-        ec_fsm_coe_t *fsm /**< finite state machine */
-        )
-{
-    ec_datagram_t *datagram = fsm->datagram;
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
     ec_sdo_request_t *request = fsm->request;
     size_t max_segment_size =
         slave->configured_rx_mailbox_size
         - EC_MBOX_HEADER_SIZE
         - EC_COE_DOWN_SEG_REQ_HEADER_SIZE;
-    size_t segment_size, data_size;
+    size_t data_size;
     uint8_t last_segment, seg_data_size, *data;
 
     if (fsm->remaining > max_segment_size) {
-        segment_size = max_segment_size;
+        fsm->segment_size = max_segment_size;
         last_segment = 0;
     } else {
-        segment_size = fsm->remaining;
+        fsm->segment_size = fsm->remaining;
         last_segment = 1;
     }
 
-    if (segment_size > EC_COE_DOWN_SEG_MIN_DATA_SIZE) {
+    if (fsm->segment_size > EC_COE_DOWN_SEG_MIN_DATA_SIZE) {
         seg_data_size = 0x00;
-        data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE + segment_size;
+        data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE + fsm->segment_size;
     } else {
-        seg_data_size = EC_COE_DOWN_SEG_MIN_DATA_SIZE - segment_size;
+        seg_data_size = EC_COE_DOWN_SEG_MIN_DATA_SIZE - fsm->segment_size;
         data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE
             + EC_COE_DOWN_SEG_MIN_DATA_SIZE;
     }
@@ -1381,14 +1488,11 @@
             | (fsm->toggle << 4)
             | (0x00 << 5)); // Download segment request
     memcpy(data + EC_COE_DOWN_SEG_REQ_HEADER_SIZE,
-            request->data + fsm->offset, segment_size);
-    if (segment_size < EC_COE_DOWN_SEG_MIN_DATA_SIZE) {
-        memset(data + EC_COE_DOWN_SEG_REQ_HEADER_SIZE + segment_size, 0x00,
-                EC_COE_DOWN_SEG_MIN_DATA_SIZE - segment_size);
-    }
-
-    fsm->offset += segment_size;
-    fsm->remaining -= segment_size;
+            request->data + fsm->offset, fsm->segment_size);
+    if (fsm->segment_size < EC_COE_DOWN_SEG_MIN_DATA_SIZE) {
+        memset(data + EC_COE_DOWN_SEG_REQ_HEADER_SIZE + fsm->segment_size,
+                0x00, EC_COE_DOWN_SEG_MIN_DATA_SIZE - fsm->segment_size);
+    }
 
     if (slave->master->debug_level) {
         EC_SLAVE_DBG(slave, 1, "Download segment request:\n");
@@ -1405,35 +1509,39 @@
    \todo Timeout behavior
 */
 
-void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_coe_down_response(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
     uint8_t *data, mbox_prot;
     size_t rec_size;
     ec_sdo_request_t *request = fsm->request;
 
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return; // FIXME: request again?
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE download"
                 " response datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE download response failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
     if (IS_ERR(data)) {
         request->errno = PTR_ERR(data);
         fsm->state = ec_fsm_coe_error;
@@ -1510,7 +1618,7 @@
 
     if (fsm->remaining) { // more segments to download
         fsm->toggle = 0;
-        ec_fsm_coe_down_prepare_segment_request(fsm);
+        ec_fsm_coe_down_prepare_segment_request(fsm, datagram);
     } else {
         fsm->state = ec_fsm_coe_end; // success
     }
@@ -1522,34 +1630,37 @@
    CoE state: DOWN SEG CHECK.
 */
 
-void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
-    ec_slave_t *slave = fsm->slave;
-
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return;
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_coe_down_seg_check(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+        return;
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE mailbox segment check"
                 " datagram failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    if (!ec_slave_mbox_check(datagram)) {
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    if (!ec_slave_mbox_check(fsm->datagram)) {
         unsigned long diff_ms =
-            (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+            (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+            1000 / HZ;
         if (diff_ms >= fsm->request->response_timeout) {
             fsm->request->errno = EIO;
             fsm->state = ec_fsm_coe_error;
@@ -1577,36 +1688,38 @@
 */
 
 void ec_fsm_coe_down_seg_response(
-        ec_fsm_coe_t *fsm /**< finite state machine */
-        )
-{
-    ec_datagram_t *datagram = fsm->datagram;
+        ec_fsm_coe_t *fsm, /**< Finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
     uint8_t *data, mbox_prot;
     size_t rec_size;
     ec_sdo_request_t *request = fsm->request;
 
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return; // FIXME: request again?
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE download response"
                 " datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE download response failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
     if (IS_ERR(data)) {
         request->errno = PTR_ERR(data);
         fsm->state = ec_fsm_coe_error;
@@ -1688,9 +1801,12 @@
         return;
     }
 
+    fsm->offset += fsm->segment_size;
+    fsm->remaining -= fsm->segment_size;
+
     if (fsm->remaining) { // more segments to download
         fsm->toggle = !fsm->toggle;
-        ec_fsm_coe_down_prepare_segment_request(fsm);
+        ec_fsm_coe_down_prepare_segment_request(fsm, datagram);
     } else {
         fsm->state = ec_fsm_coe_end; // success
     }
@@ -1698,33 +1814,19 @@
 
 /*****************************************************************************/
 
-/**
-   CoE state: UP START.
-*/
-
-void ec_fsm_coe_up_start(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
-    ec_slave_t *slave = fsm->slave;
+int ec_fsm_coe_prepare_up(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    ec_sdo_request_t *request = fsm->request;
     ec_master_t *master = slave->master;
-    ec_sdo_request_t *request = fsm->request;
-    uint8_t *data;
-
-    EC_SLAVE_DBG(slave, 1, "Uploading SDO 0x%04X:%02X.\n",
-            request->index, request->subindex);
-
-    if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
-        EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");
-        request->errno = EPROTONOSUPPORT;
-        fsm->state = ec_fsm_coe_error;
-        return;
-    }
-
-    data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10);
+
+    u8 *data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10);
     if (IS_ERR(data)) {
         request->errno = PTR_ERR(data);
-        fsm->state = ec_fsm_coe_error;
-        return;
+        return PTR_ERR(data);
     }
 
     EC_WRITE_U16(data, 0x2 << 12); // SDO request
@@ -1738,39 +1840,75 @@
         ec_print_data(data, 10);
     }
 
+    fsm->state = ec_fsm_coe_up_request;
+    return 0;
+}
+
+/*****************************************************************************/
+
+/**
+   CoE state: UP START.
+*/
+
+void ec_fsm_coe_up_start(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    ec_sdo_request_t *request = fsm->request;
+
+    EC_SLAVE_DBG(slave, 1, "Uploading SDO 0x%04X:%02X.\n",
+            request->index, request->subindex);
+
+    if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
+        EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");
+        request->errno = EPROTONOSUPPORT;
+        fsm->state = ec_fsm_coe_error;
+        return;
+    }
+
+    fsm->retries = EC_FSM_RETRIES;
     fsm->request->jiffies_sent = jiffies;
-    fsm->retries = EC_FSM_RETRIES;
-    fsm->state = ec_fsm_coe_up_request;
-}
-
-/*****************************************************************************/
-
+
+    if (ec_fsm_coe_prepare_up(fsm, datagram)) {
+        fsm->state = ec_fsm_coe_error;
+    }
+}
+
+/*****************************************************************************/
 /**
    CoE state: UP REQUEST.
    \todo Timeout behavior
 */
 
-void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_coe_up_request(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
     unsigned long diff_ms;
 
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return; // FIXME: check for response first?
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        if (ec_fsm_coe_prepare_up(fsm, datagram)) {
+            fsm->state = ec_fsm_coe_error;
+        }
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE upload request: ");
-        ec_datagram_print_state(datagram);
+        ec_datagram_print_state(fsm->datagram);
         return;
     }
 
     diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
 
-    if (datagram->working_counter != 1) {
-        if (!datagram->working_counter) {
+    if (fsm->datagram->working_counter != 1) {
+        if (!fsm->datagram->working_counter) {
             if (diff_ms < fsm->request->response_timeout) {
 #if DEBUG_RETRIES
                 EC_SLAVE_DBG(slave, 1, "Slave did not respond to"
@@ -1778,6 +1916,9 @@
                         diff_ms);
 #endif
                 // no response; send request datagram again
+                if (ec_fsm_coe_prepare_up(fsm, datagram)) {
+                    fsm->state = ec_fsm_coe_error;
+                }
                 return;
             }
         }
@@ -1786,7 +1927,7 @@
         EC_SLAVE_ERR(slave, "Reception of CoE upload request for"
                 " SDO 0x%04x:%x failed with timeout after %lu ms: ",
                 fsm->request->index, fsm->request->subindex, diff_ms);
-        ec_datagram_print_wc_error(datagram);
+        ec_datagram_print_wc_error(fsm->datagram);
         return;
     }
 
@@ -1797,7 +1938,7 @@
     }
 #endif
 
-    fsm->jiffies_start = datagram->jiffies_sent;
+    fsm->jiffies_start = fsm->datagram->jiffies_sent;
 
     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
     fsm->retries = EC_FSM_RETRIES;
@@ -1810,34 +1951,39 @@
    CoE state: UP CHECK.
 */
 
-void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
-    ec_slave_t *slave = fsm->slave;
-
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return;
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_coe_up_check(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
                 " datagram failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    if (!ec_slave_mbox_check(datagram)) {
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    if (!ec_slave_mbox_check(fsm->datagram)) {
         unsigned long diff_ms =
-            (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+            (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+            1000 / HZ;
         if (diff_ms >= fsm->request->response_timeout) {
             fsm->request->errno = EIO;
             fsm->state = ec_fsm_coe_error;
@@ -1863,11 +2009,12 @@
 /** Prepare an SDO upload segment request.
  */
 void ec_fsm_coe_up_prepare_segment_request(
-        ec_fsm_coe_t *fsm /**< Finite state machine */
+        ec_fsm_coe_t *fsm, /**< Finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     uint8_t *data =
-        ec_slave_mbox_prepare_send(fsm->slave, fsm->datagram, 0x03, 10);
+        ec_slave_mbox_prepare_send(fsm->slave, datagram, 0x03, 10);
     if (IS_ERR(data)) {
         fsm->request->errno = PTR_ERR(data);
         fsm->state = ec_fsm_coe_error;
@@ -1892,9 +2039,11 @@
    \todo Timeout behavior
 */
 
-void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_coe_up_response(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
     ec_master_t *master = slave->master;
     uint16_t rec_index;
@@ -1904,27 +2053,29 @@
     unsigned int expedited, size_specified;
     int ret;
 
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return; // FIXME: request again?
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE upload response"
                 " datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE upload response failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
     if (IS_ERR(data)) {
         request->errno = PTR_ERR(data);
         fsm->state = ec_fsm_coe_error;
@@ -2069,7 +2220,7 @@
         if (data_size < fsm->complete_size) {
             EC_SLAVE_DBG(slave, 1, "SDO data incomplete (%zu / %u)."
                     " Segmenting...\n", data_size, fsm->complete_size);
-            ec_fsm_coe_up_prepare_segment_request(fsm);
+            ec_fsm_coe_up_prepare_segment_request(fsm, datagram);
             fsm->retries = EC_FSM_RETRIES;
             fsm->state = ec_fsm_coe_up_seg_request;
             return;
@@ -2091,33 +2242,37 @@
    \todo Timeout behavior
 */
 
-void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
-    ec_slave_t *slave = fsm->slave;
-
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return; // FIXME: check for response first?
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_coe_up_seg_request(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_fsm_coe_up_prepare_segment_request(fsm, datagram);
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment"
                 " request datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE upload segment"
                 " request failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    fsm->jiffies_start = datagram->jiffies_sent;
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    fsm->jiffies_start = fsm->datagram->jiffies_sent;
 
     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
     fsm->retries = EC_FSM_RETRIES;
@@ -2130,35 +2285,40 @@
    CoE state: UP CHECK.
 */
 
-void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
-    ec_slave_t *slave = fsm->slave;
-
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return;
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_coe_up_seg_check(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check"
                 " datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE mailbox check datagram"
                 " failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    if (!ec_slave_mbox_check(datagram)) {
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    if (!ec_slave_mbox_check(fsm->datagram)) {
         unsigned long diff_ms =
-            (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+            (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+            1000 / HZ;
         if (diff_ms >= fsm->request->response_timeout) {
             fsm->request->errno = EIO;
             fsm->state = ec_fsm_coe_error;
@@ -2185,9 +2345,11 @@
    \todo Timeout behavior
 */
 
-void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_coe_up_seg_response(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
     ec_master_t *master = slave->master;
     uint8_t *data, mbox_prot;
@@ -2195,28 +2357,30 @@
     ec_sdo_request_t *request = fsm->request;
     unsigned int last_segment;
 
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return; // FIXME: request again?
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment"
                 " response datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE upload segment"
                 " response failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
     if (IS_ERR(data)) {
         request->errno = PTR_ERR(data);
         fsm->state = ec_fsm_coe_error;
@@ -2299,7 +2463,7 @@
     last_segment = EC_READ_U8(data + 2) & 0x01;
     if (!last_segment) {
         fsm->toggle = !fsm->toggle;
-        ec_fsm_coe_up_prepare_segment_request(fsm);
+        ec_fsm_coe_up_prepare_segment_request(fsm, datagram);
         fsm->retries = EC_FSM_RETRIES;
         fsm->state = ec_fsm_coe_up_seg_request;
         return;
@@ -2326,7 +2490,10 @@
    State: ERROR.
 */
 
-void ec_fsm_coe_error(ec_fsm_coe_t *fsm /**< finite state machine */)
+void ec_fsm_coe_error(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
 {
 }
 
@@ -2336,8 +2503,11 @@
    State: END.
 */
 
-void ec_fsm_coe_end(ec_fsm_coe_t *fsm /**< finite state machine */)
-{
-}
-
-/*****************************************************************************/
+void ec_fsm_coe_end(
+        ec_fsm_coe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+}
+
+/*****************************************************************************/
diff -r 505cf41488a4 -r 9cdd7669dc0b master/fsm_coe.h
--- a/master/fsm_coe.h	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_coe.h	Thu Jan 10 17:36:41 2013 +0100
@@ -51,10 +51,10 @@
  */
 struct ec_fsm_coe {
     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 */
+    void (*state)(ec_fsm_coe_t *, ec_datagram_t *); /**< CoE state function */
+    ec_datagram_t *datagram; /**< Datagram used in last step. */
     unsigned long jiffies_start; /**< CoE timestamp. */
     ec_sdo_t *sdo; /**< current SDO */
     uint8_t subindex; /**< current subindex */
@@ -63,18 +63,19 @@
     uint8_t toggle; /**< toggle bit for segment commands */
     uint32_t offset; /**< Data offset during segmented download. */
     uint32_t remaining; /**< Remaining bytes during segmented download. */
+    size_t segment_size; /**< Current segment size. */
 };
 
 /*****************************************************************************/
 
-void ec_fsm_coe_init(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_init(ec_fsm_coe_t *);
 void ec_fsm_coe_clear(ec_fsm_coe_t *);
 
 void ec_fsm_coe_dictionary(ec_fsm_coe_t *, ec_slave_t *);
 void ec_fsm_coe_transfer(ec_fsm_coe_t *, ec_slave_t *, ec_sdo_request_t *);
 
-int ec_fsm_coe_exec(ec_fsm_coe_t *);
-int ec_fsm_coe_success(ec_fsm_coe_t *);
+int ec_fsm_coe_exec(ec_fsm_coe_t *, ec_datagram_t *);
+int ec_fsm_coe_success(const ec_fsm_coe_t *);
 
 /*****************************************************************************/
 
diff -r 505cf41488a4 -r 9cdd7669dc0b master/fsm_foe.c
--- a/master/fsm_foe.c	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_foe.c	Thu Jan 10 17:36:41 2013 +0100
@@ -74,44 +74,42 @@
 
 /*****************************************************************************/
 
-int ec_foe_prepare_data_send(ec_fsm_foe_t *);
-int ec_foe_prepare_wrq_send(ec_fsm_foe_t *);
-int ec_foe_prepare_rrq_send(ec_fsm_foe_t *);
-int ec_foe_prepare_send_ack(ec_fsm_foe_t *);
+int ec_foe_prepare_data_send(ec_fsm_foe_t *, ec_datagram_t *);
+int ec_foe_prepare_wrq_send(ec_fsm_foe_t *, ec_datagram_t *);
+int ec_foe_prepare_rrq_send(ec_fsm_foe_t *, ec_datagram_t *);
+int ec_foe_prepare_send_ack(ec_fsm_foe_t *, ec_datagram_t *);
 
 void ec_foe_set_tx_error(ec_fsm_foe_t *, uint32_t);
 void ec_foe_set_rx_error(ec_fsm_foe_t *, uint32_t);
 
-void ec_fsm_foe_write(ec_fsm_foe_t *);
-void ec_fsm_foe_read(ec_fsm_foe_t *);
-void ec_fsm_foe_end(ec_fsm_foe_t *);
-void ec_fsm_foe_error(ec_fsm_foe_t *);
-
-void ec_fsm_foe_state_wrq_sent(ec_fsm_foe_t *);
-void ec_fsm_foe_state_rrq_sent(ec_fsm_foe_t *);
-
-void ec_fsm_foe_state_ack_check(ec_fsm_foe_t *);
-void ec_fsm_foe_state_ack_read(ec_fsm_foe_t *);
-
-void ec_fsm_foe_state_data_sent(ec_fsm_foe_t *);
-
-void ec_fsm_foe_state_data_check(ec_fsm_foe_t *);
-void ec_fsm_foe_state_data_read(ec_fsm_foe_t *);
-void ec_fsm_foe_state_sent_ack(ec_fsm_foe_t *);
-
-void ec_fsm_foe_write_start(ec_fsm_foe_t *);
-void ec_fsm_foe_read_start(ec_fsm_foe_t *);
+void ec_fsm_foe_end(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_error(ec_fsm_foe_t *, ec_datagram_t *);
+
+void ec_fsm_foe_state_wrq_sent(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_state_rrq_sent(ec_fsm_foe_t *, ec_datagram_t *);
+
+void ec_fsm_foe_state_ack_check(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_state_ack_read(ec_fsm_foe_t *, ec_datagram_t *);
+
+void ec_fsm_foe_state_data_sent(ec_fsm_foe_t *, ec_datagram_t *);
+
+void ec_fsm_foe_state_data_check(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_state_data_read(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_state_sent_ack(ec_fsm_foe_t *, ec_datagram_t *);
+
+void ec_fsm_foe_write_start(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_read_start(ec_fsm_foe_t *, ec_datagram_t *);
 
 /*****************************************************************************/
 
 /** Constructor.
  */
-void ec_fsm_foe_init(ec_fsm_foe_t *fsm, /**< finite state machine */
-                     ec_datagram_t *datagram /**< datagram */
-                     )
-{
-    fsm->state     = NULL;
-    fsm->datagram  = datagram;
+void ec_fsm_foe_init(
+        ec_fsm_foe_t *fsm /**< finite state machine */
+        )
+{
+    fsm->state = NULL;
+    fsm->datagram = NULL;
 }
 
 /*****************************************************************************/
@@ -125,13 +123,36 @@
 /*****************************************************************************/
 
 /** Executes the current state of the state machine.
- * \return false, if state machine has terminated
- */
-int ec_fsm_foe_exec(ec_fsm_foe_t *fsm /**< finite state machine */)
-{
-    fsm->state(fsm);
-
-    return fsm->state != ec_fsm_foe_end && fsm->state != ec_fsm_foe_error;
+ *
+ * \return 1, if the datagram was used, else 0.
+ */
+int ec_fsm_foe_exec(
+        ec_fsm_foe_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    int datagram_used = 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;
+    }
+
+    fsm->state(fsm, datagram);
+
+    datagram_used =
+        fsm->state != ec_fsm_foe_end && fsm->state != ec_fsm_foe_error;
+
+    if (datagram_used) {
+        fsm->datagram = datagram;
+    } else {
+        fsm->datagram = NULL;
+    }
+
+    return datagram_used;
 }
 
 /*****************************************************************************/
@@ -139,7 +160,7 @@
 /** Returns, if the state machine terminated with success.
  * \return non-zero if successful.
  */
-int ec_fsm_foe_success(ec_fsm_foe_t *fsm /**< Finite state machine */)
+int ec_fsm_foe_success(const ec_fsm_foe_t *fsm /**< Finite state machine */)
 {
     return fsm->state == ec_fsm_foe_end;
 }
@@ -156,11 +177,25 @@
 {
     fsm->slave = slave;
     fsm->request = request;
+
     if (request->dir == EC_DIR_OUTPUT) {
-        fsm->state = ec_fsm_foe_write;
+        fsm->tx_buffer = fsm->request->buffer;
+        fsm->tx_buffer_size = fsm->request->data_size;
+        fsm->tx_buffer_offset = 0;
+
+        fsm->tx_filename = fsm->request->file_name;
+        fsm->tx_filename_len = strlen(fsm->tx_filename);
+
+        fsm->state = ec_fsm_foe_write_start;
     }
     else {
-        fsm->state = ec_fsm_foe_read;
+        fsm->rx_buffer = fsm->request->buffer;
+        fsm->rx_buffer_size = fsm->request->buffer_size;
+
+        fsm->rx_filename = fsm->request->file_name;
+        fsm->rx_filename_len = strlen(fsm->rx_filename);
+
+        fsm->state = ec_fsm_foe_read_start;
     }
 }
 
@@ -168,7 +203,10 @@
 
 /** State: ERROR.
  */
-void ec_fsm_foe_error(ec_fsm_foe_t *fsm /**< finite state machine */)
+void ec_fsm_foe_error(
+        ec_fsm_foe_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
 {
 #ifdef DEBUG_FOE
     printk("ec_fsm_foe_error()\n");
@@ -179,7 +217,10 @@
 
 /** State: END.
  */
-void ec_fsm_foe_end(ec_fsm_foe_t *fsm /**< finite state machine */)
+void ec_fsm_foe_end(
+        ec_fsm_foe_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
 {
 #ifdef DEBUG_FOE
     printk("ec_fsm_foe_end\n");
@@ -190,7 +231,10 @@
 
 /** Sends a file or the next fragment.
  */
-int ec_foe_prepare_data_send(ec_fsm_foe_t *fsm)
+int ec_foe_prepare_data_send(
+        ec_fsm_foe_t *fsm,
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
 {
     size_t remaining_size, current_size;
     uint8_t *data;
@@ -207,13 +251,14 @@
     }
 
     data = ec_slave_mbox_prepare_send(fsm->slave,
-            fsm->datagram, EC_MBOX_TYPE_FILEACCESS,
+            datagram, EC_MBOX_TYPE_FILEACCESS,
             current_size + EC_FOE_HEADER_SIZE);
-    if (IS_ERR(data))
+    if (IS_ERR(data)) {
         return -1;
-
-    EC_WRITE_U8 ( data, EC_FOE_OPCODE_DATA );    // OpCode = DataBlock req.
-    EC_WRITE_U32( data + 2, fsm->tx_packet_no ); // PacketNo, Password
+    }
+
+    EC_WRITE_U8 (data, EC_FOE_OPCODE_DATA);    // OpCode = DataBlock req.
+    EC_WRITE_U32(data + 2, fsm->tx_packet_no); // PacketNo, Password
 
     memcpy(data + EC_FOE_HEADER_SIZE,
             fsm->tx_buffer + fsm->tx_buffer_offset, current_size);
@@ -226,7 +271,10 @@
 
 /** Prepare a write request (WRQ) with filename
  */
-int ec_foe_prepare_wrq_send(ec_fsm_foe_t *fsm)
+int ec_foe_prepare_wrq_send(
+        ec_fsm_foe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
 {
     size_t current_size;
     uint8_t *data;
@@ -238,10 +286,11 @@
 
     current_size = fsm->tx_filename_len;
 
-    data = ec_slave_mbox_prepare_send(fsm->slave, fsm->datagram,
+    data = ec_slave_mbox_prepare_send(fsm->slave, datagram,
             EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE);
-    if (IS_ERR(data))
+    if (IS_ERR(data)) {
         return -1;
+    }
 
     EC_WRITE_U16( data, EC_FOE_OPCODE_WRQ); // fsm write request
     EC_WRITE_U32( data + 2, fsm->tx_packet_no );
@@ -253,27 +302,12 @@
 
 /*****************************************************************************/
 
-/** Start a write operation.
- */
-void ec_fsm_foe_write(
-        ec_fsm_foe_t *fsm /**< FoE statemachine. */
-        )
-{
-    fsm->tx_buffer = fsm->request->buffer;
-    fsm->tx_buffer_size = fsm->request->data_size;
-    fsm->tx_buffer_offset = 0;
-
-    fsm->tx_filename = fsm->request->file_name;
-    fsm->tx_filename_len = strlen(fsm->tx_filename);
-
-    fsm->state = ec_fsm_foe_write_start;
-}
-
-/*****************************************************************************/
-
 /** Initializes the FoE write state machine.
  */
-void ec_fsm_foe_write_start(ec_fsm_foe_t *fsm /**< finite state machine */)
+void ec_fsm_foe_write_start(
+        ec_fsm_foe_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
 {
     ec_slave_t *slave = fsm->slave;
 
@@ -292,7 +326,7 @@
         return;
     }
 
-    if (ec_foe_prepare_wrq_send(fsm)) {
+    if (ec_foe_prepare_wrq_send(fsm, datagram)) {
         ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
         return;
     }
@@ -305,33 +339,33 @@
 /** Check for acknowledge.
  */
 void ec_fsm_foe_state_ack_check(
-        ec_fsm_foe_t *fsm /**< FoE statemachine. */
-        )
-{
-    ec_datagram_t *datagram = fsm->datagram;
+        ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
 
 #ifdef DEBUG_FOE
     printk("ec_fsm_foe_ack_check()\n");
 #endif
 
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
         EC_SLAVE_ERR(slave, "Failed to receive FoE mailbox check datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
-        // slave did not put anything in the mailbox yet
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
         EC_SLAVE_ERR(slave, "Reception of FoE mailbox check datagram"
                 " failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    if (!ec_slave_mbox_check(datagram)) {
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    if (!ec_slave_mbox_check(fsm->datagram)) {
+        // slave did not put anything in the mailbox yet
         unsigned long diff_ms =
             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
         if (diff_ms >= EC_FSM_FOE_TIMEOUT) {
@@ -357,10 +391,10 @@
 /** Acknowledge a read operation.
  */
 void ec_fsm_foe_state_ack_read(
-        ec_fsm_foe_t *fsm /**< FoE statemachine. */
-        )
-{
-    ec_datagram_t *datagram = fsm->datagram;
+        ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
     uint8_t *data, mbox_prot;
     uint8_t opCode;
@@ -370,22 +404,22 @@
     printk("ec_fsm_foe_ack_read()\n");
 #endif
 
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
         EC_SLAVE_ERR(slave, "Failed to receive FoE ack response datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
         EC_SLAVE_ERR(slave, "Reception of FoE ack response failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    if (!(data = ec_slave_mbox_fetch(fsm->slave, datagram,
-                    &mbox_prot, &rec_size))) {
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    data = ec_slave_mbox_fetch(fsm->slave, datagram, &mbox_prot, &rec_size);
+    if (IS_ERR(data)) {
         ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
         return;
     }
@@ -401,7 +435,7 @@
 
     if (opCode == EC_FOE_OPCODE_BUSY) {
         // slave not ready
-        if (ec_foe_prepare_data_send(fsm)) {
+        if (ec_foe_prepare_data_send(fsm, datagram)) {
             ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
             EC_SLAVE_ERR(slave, "Slave is busy.\n");
             return;
@@ -419,7 +453,7 @@
             return;
         }
 
-        if (ec_foe_prepare_data_send(fsm)) {
+        if (ec_foe_prepare_data_send(fsm, datagram)) {
             ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
             return;
         }
@@ -437,32 +471,32 @@
  * fragment, if necessary.
  */
 void ec_fsm_foe_state_wrq_sent(
-        ec_fsm_foe_t *fsm /**< FoE statemachine. */
-        )
-{
-    ec_datagram_t *datagram = fsm->datagram;
+        ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
 
 #ifdef DEBUG_FOE
     printk("ec_foe_state_sent_wrq()\n");
 #endif
 
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
         EC_SLAVE_ERR(slave, "Failed to send FoE WRQ: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         // slave did not put anything in the mailbox yet
         ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
         EC_SLAVE_ERR(slave, "Reception of FoE WRQ failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    fsm->jiffies_start = datagram->jiffies_sent;
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    fsm->jiffies_start = fsm->datagram->jiffies_sent;
 
     ec_slave_mbox_prepare_check(fsm->slave, datagram); // can not fail.
 
@@ -478,10 +512,10 @@
  * fragment, if necessary.
  */
 void ec_fsm_foe_state_data_sent(
-        ec_fsm_foe_t *fsm /**< Foe statemachine. */
-        )
-{
-    ec_datagram_t *datagram = fsm->datagram;
+        ec_fsm_foe_t *fsm, /**< Foe statemachine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
 
 #ifdef DEBUG_FOE
@@ -491,18 +525,18 @@
     if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         ec_foe_set_tx_error(fsm, FOE_RECEIVE_ERROR);
         EC_SLAVE_ERR(slave, "Failed to receive FoE ack response datagram: ");
-        ec_datagram_print_state(datagram);
+        ec_datagram_print_state(fsm->datagram);
         return;
     }
 
     if (fsm->datagram->working_counter != 1) {
         ec_foe_set_tx_error(fsm, FOE_WC_ERROR);
         EC_SLAVE_ERR(slave, "Reception of FoE data send failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    ec_slave_mbox_prepare_check(fsm->slave, fsm->datagram);
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    ec_slave_mbox_prepare_check(fsm->slave, datagram);
     fsm->jiffies_start = jiffies;
     fsm->retries = EC_FSM_RETRIES;
     fsm->state = ec_fsm_foe_state_ack_check;
@@ -512,17 +546,21 @@
 
 /** Prepare a read request (RRQ) with filename
  */
-int ec_foe_prepare_rrq_send(ec_fsm_foe_t *fsm)
+int ec_foe_prepare_rrq_send(
+        ec_fsm_foe_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
 {
     size_t current_size;
     uint8_t *data;
 
     current_size = fsm->rx_filename_len;
 
-    data = ec_slave_mbox_prepare_send(fsm->slave, fsm->datagram,
+    data = ec_slave_mbox_prepare_send(fsm->slave, datagram,
             EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE);
-    if (IS_ERR(data))
+    if (IS_ERR(data)) {
         return -1;
+    }
 
     EC_WRITE_U16(data, EC_FOE_OPCODE_RRQ); // fsm read request
     EC_WRITE_U32(data + 2, 0x00000000); // no passwd
@@ -541,18 +579,20 @@
 /** Prepare to send an acknowledge.
  */
 int ec_foe_prepare_send_ack(
-        ec_fsm_foe_t *foe /**< FoE statemachine. */
+        ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     uint8_t *data;
 
-    data = ec_slave_mbox_prepare_send(foe->slave, foe->datagram,
+    data = ec_slave_mbox_prepare_send(fsm->slave, datagram,
             EC_MBOX_TYPE_FILEACCESS, EC_FOE_HEADER_SIZE);
-    if (IS_ERR(data))
+    if (IS_ERR(data)) {
         return -1;
+    }
 
     EC_WRITE_U16(data, EC_FOE_OPCODE_ACK);
-    EC_WRITE_U32(data + 2, foe->rx_expected_packet_no);
+    EC_WRITE_U32(data + 2, fsm->rx_expected_packet_no);
 
     return 0;
 }
@@ -561,36 +601,36 @@
 
 /** State: RRQ SENT.
  *
- * Checks is the previous transmit datagram succeded and sends the next
+ * Checks is the previous transmit datagram succeeded and sends the next
  * fragment, if necessary.
  */
 void ec_fsm_foe_state_rrq_sent(
-        ec_fsm_foe_t *fsm /**< FoE statemachine. */
-        )
-{
-    ec_datagram_t *datagram = fsm->datagram;
+        ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
 
 #ifdef DEBUG_FOE
     printk("ec_foe_state_rrq_sent()\n");
 #endif
 
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
         EC_SLAVE_ERR(slave, "Failed to send FoE RRQ: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         // slave did not put anything in the mailbox yet
         ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
         EC_SLAVE_ERR(slave, "Reception of FoE RRQ failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    fsm->jiffies_start = datagram->jiffies_sent;
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    fsm->jiffies_start = fsm->datagram->jiffies_sent;
 
     ec_slave_mbox_prepare_check(fsm->slave, datagram); // can not fail.
 
@@ -600,26 +640,11 @@
 
 /*****************************************************************************/
 
-/** Start a read operation.
- */
-void ec_fsm_foe_read(
-        ec_fsm_foe_t *fsm /**< FoE state machine. */
-        )
-{
-    fsm->state = ec_fsm_foe_read_start;
-    fsm->rx_filename = fsm->request->file_name;
-    fsm->rx_filename_len = strlen(fsm->rx_filename);
-
-    fsm->rx_buffer = fsm->request->buffer;
-    fsm->rx_buffer_size = fsm->request->buffer_size;
-}
-
-/*****************************************************************************/
-
 /** Starting state for read operations.
  */
 void ec_fsm_foe_read_start(
-        ec_fsm_foe_t *fsm /**< FoE statemachine. */
+        ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     ec_slave_t *slave = fsm->slave;
@@ -638,7 +663,7 @@
         return;
     }
 
-    if (ec_foe_prepare_rrq_send(fsm)) {
+    if (ec_foe_prepare_rrq_send(fsm, datagram)) {
         ec_foe_set_rx_error(fsm, FOE_PROT_ERROR);
         return;
     }
@@ -651,33 +676,34 @@
 /** Check for data.
  */
 void ec_fsm_foe_state_data_check(
-        ec_fsm_foe_t *fsm /**< FoE statemachine. */
-        )
-{
-    ec_datagram_t *datagram = fsm->datagram;
+        ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
 
 #ifdef DEBUG_FOE
     printk("ec_fsm_foe_state_data_check()\n");
 #endif
 
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
         EC_SLAVE_ERR(slave, "Failed to send FoE DATA READ: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
         EC_SLAVE_ERR(slave, "Reception of FoE DATA READ: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    if (!ec_slave_mbox_check(datagram)) {
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    if (!ec_slave_mbox_check(fsm->datagram)) {
         unsigned long diff_ms =
-            (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+            (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+            1000 / HZ;
         if (diff_ms >= EC_FSM_FOE_TIMEOUT) {
             ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);
             EC_SLAVE_ERR(slave, "Timeout while waiting for ack response.\n");
@@ -702,35 +728,35 @@
 /** Start reading data.
  */
 void ec_fsm_foe_state_data_read(
-        ec_fsm_foe_t *fsm /**< FoE statemachine. */
+        ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     size_t rec_size;
     uint8_t *data, opCode, packet_no, mbox_prot;
 
-    ec_datagram_t *datagram = fsm->datagram;
     ec_slave_t *slave = fsm->slave;
 
 #ifdef DEBUG_FOE
     printk("ec_fsm_foe_state_data_read()\n");
 #endif
 
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
         EC_SLAVE_ERR(slave, "Failed to receive FoE DATA READ datagram: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
         EC_SLAVE_ERR(slave, "Reception of FoE DATA READ failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    if (!(data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot,
-                    &rec_size))) {
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+    if (IS_ERR(data)) {
         ec_foe_set_rx_error(fsm, FOE_MBOX_FETCH_ERROR);
         return;
     }
@@ -745,7 +771,7 @@
     opCode = EC_READ_U8(data);
 
     if (opCode == EC_FOE_OPCODE_BUSY) {
-        if (ec_foe_prepare_send_ack(fsm)) {
+        if (ec_foe_prepare_send_ack(fsm, datagram)) {
             ec_foe_set_rx_error(fsm, FOE_PROT_ERROR);
         }
         return;
@@ -800,7 +826,7 @@
 #ifdef DEBUG_FOE
         printk ("last_packet=true\n");
 #endif
-        if (ec_foe_prepare_send_ack(fsm)) {
+        if (ec_foe_prepare_send_ack(fsm, datagram)) {
             ec_foe_set_rx_error(fsm, FOE_RX_DATA_ACK_ERROR);
             return;
         }
@@ -826,32 +852,32 @@
 /** Sent an acknowledge.
  */
 void ec_fsm_foe_state_sent_ack(
-        ec_fsm_foe_t *fsm /**< FoE statemachine. */
-        )
-{
-    ec_datagram_t *datagram = fsm->datagram;
+        ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
 
 #ifdef DEBUG_FOE
     printk("ec_foe_state_sent_ack()\n");
 #endif
 
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
         EC_SLAVE_ERR(slave, "Failed to send FoE ACK: ");
-        ec_datagram_print_state(datagram);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         // slave did not put anything into the mailbox yet
         ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
         EC_SLAVE_ERR(slave, "Reception of FoE ACK failed: ");
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    fsm->jiffies_start = datagram->jiffies_sent;
+        ec_datagram_print_wc_error(fsm->datagram);
+        return;
+    }
+
+    fsm->jiffies_start = fsm->datagram->jiffies_sent;
 
     ec_slave_mbox_prepare_check(fsm->slave, datagram); // can not fail.
 
diff -r 505cf41488a4 -r 9cdd7669dc0b master/fsm_foe.h
--- a/master/fsm_foe.h	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_foe.h	Thu Jan 10 17:36:41 2013 +0100
@@ -2,7 +2,8 @@
  *
  *  $Id$
  *
- *  Copyright (C) 2008  Olav Zarges, imc Messsysteme GmbH
+ *  Copyright (C) 2008       Olav Zarges, imc Messsysteme GmbH
+ *                2009-2012  Florian Pose <fp@igh-essen.com>
  *
  *  This file is part of the IgH EtherCAT Master.
  *
@@ -50,15 +51,15 @@
 /** Finite state machines for the CANopen-over-EtherCAT protocol.
  */
 struct ec_fsm_foe {
-    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 */
+    ec_slave_t *slave; /**< Slave the FSM runs on. */
+    unsigned int retries; /**< Retries upon datagram timeout */
 
-    void (*state)(ec_fsm_foe_t *); /**< FoE state function */
+    void (*state)(ec_fsm_foe_t *, ec_datagram_t *); /**< FoE state function. */
+    ec_datagram_t *datagram; /**< Datagram used in previous step. */
     unsigned long jiffies_start; /**< FoE timestamp. */
-    uint8_t subindex; /**< current subindex */
-    ec_foe_request_t *request; /**< FoE request */
-    uint8_t toggle; /**< toggle bit for segment commands */
+    uint8_t subindex; /**< Current subindex. */
+    ec_foe_request_t *request; /**< FoE request. */
+    uint8_t toggle; /**< Toggle bit for segment commands. */
 
     uint8_t *tx_buffer; /**< Buffer with data to transmit. */
     uint32_t tx_buffer_size; /**< Size of data to transmit. */
@@ -80,11 +81,11 @@
 
 /*****************************************************************************/
 
-void ec_fsm_foe_init(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_init(ec_fsm_foe_t *);
 void ec_fsm_foe_clear(ec_fsm_foe_t *);
 
-int ec_fsm_foe_exec(ec_fsm_foe_t *);
-int ec_fsm_foe_success(ec_fsm_foe_t *);
+int ec_fsm_foe_exec(ec_fsm_foe_t *, ec_datagram_t *);
+int ec_fsm_foe_success(const ec_fsm_foe_t *);
 
 void ec_fsm_foe_transfer(ec_fsm_foe_t *, ec_slave_t *, ec_foe_request_t *);
 
diff -r 505cf41488a4 -r 9cdd7669dc0b master/fsm_master.c
--- a/master/fsm_master.c	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_master.c	Thu Jan 10 17:36:41 2013 +0100
@@ -85,8 +85,8 @@
     ec_fsm_master_reset(fsm);
 
     // init sub-state-machines
-    ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);
-    ec_fsm_soe_init(&fsm->fsm_soe, fsm->datagram);
+    ec_fsm_coe_init(&fsm->fsm_coe);
+    ec_fsm_soe_init(&fsm->fsm_soe);
     ec_fsm_pdo_init(&fsm->fsm_pdo, &fsm->fsm_coe);
     ec_fsm_change_init(&fsm->fsm_change, fsm->datagram);
     ec_fsm_slave_config_init(&fsm->fsm_slave_config, fsm->datagram,
@@ -342,6 +342,7 @@
                 }
             }
             master->slave_count = count;
+            master->fsm_slave = master->slaves;
 
             /* start with first device with slaves responding; at least one
              * has responding slaves, otherwise count would be zero. */
@@ -465,7 +466,7 @@
                 fsm->slave = slave;
                 fsm->state = ec_fsm_master_state_sdo_request;
                 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, req);
-                ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
+                ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram);
                 return 1;
             }
         }
@@ -495,7 +496,7 @@
     for (slave = master->slaves;
             slave < master->slaves + master->slave_count;
             slave++) {
-        ec_fsm_slave_ready(&slave->fsm);
+        ec_fsm_slave_set_ready(&slave->fsm);
     }
 
     // check, if slaves have an SDO dictionary to read out.
@@ -520,7 +521,7 @@
         fsm->slave = slave;
         fsm->state = ec_fsm_master_state_sdo_dictionary;
         ec_fsm_coe_dictionary(&fsm->fsm_coe, slave);
-        ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
+        ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram); // execute immediately
         fsm->datagram->device_index = fsm->slave->device_index;
         return;
     }
@@ -1138,7 +1139,7 @@
     if (!ec_fsm_sii_success(&fsm->fsm_sii)) {
         EC_SLAVE_ERR(slave, "Failed to write SII data.\n");
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&master->request_queue);
+        wake_up_all(&master->request_queue);
         ec_fsm_master_restart(fsm);
         return;
     }
@@ -1166,7 +1167,7 @@
     // TODO: Evaluate other SII contents!
 
     request->state = EC_INT_REQUEST_SUCCESS;
-    wake_up(&master->request_queue);
+    wake_up_all(&master->request_queue);
 
     // check for another SII write request
     if (ec_fsm_master_action_process_sii(fsm))
@@ -1186,7 +1187,7 @@
     ec_slave_t *slave = fsm->slave;
     ec_master_t *master = fsm->master;
 
-    if (ec_fsm_coe_exec(&fsm->fsm_coe)) {
+    if (ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram)) {
         return;
     }
 
@@ -1226,7 +1227,7 @@
         return;
     }
 
-    if (ec_fsm_coe_exec(&fsm->fsm_coe)) {
+    if (ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram)) {
         return;
     }
 
@@ -1234,14 +1235,14 @@
         EC_SLAVE_DBG(fsm->slave, 1,
                 "Failed to process internal SDO request.\n");
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&fsm->master->request_queue);
+        wake_up_all(&fsm->master->request_queue);
         ec_fsm_master_restart(fsm);
         return;
     }
 
     // SDO request finished
     request->state = EC_INT_REQUEST_SUCCESS;
-    wake_up(&fsm->master->request_queue);
+    wake_up_all(&fsm->master->request_queue);
 
     EC_SLAVE_DBG(fsm->slave, 1, "Finished internal SDO request.\n");
 
diff -r 505cf41488a4 -r 9cdd7669dc0b master/fsm_pdo.c
--- a/master/fsm_pdo.c	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_pdo.c	Thu Jan 10 17:36:41 2013 +0100
@@ -27,8 +27,7 @@
  *
  *****************************************************************************/
 
-/** \file
- * EtherCAT PDO configuration state machine.
+/** \file EtherCAT PDO configuration state machine.
  */
 
 /*****************************************************************************/
@@ -42,30 +41,30 @@
 
 /*****************************************************************************/
 
-void ec_fsm_pdo_read_state_start(ec_fsm_pdo_t *);
-void ec_fsm_pdo_read_state_pdo_count(ec_fsm_pdo_t *);
-void ec_fsm_pdo_read_state_pdo(ec_fsm_pdo_t *);
-void ec_fsm_pdo_read_state_pdo_entries(ec_fsm_pdo_t *);
-
-void ec_fsm_pdo_read_action_next_sync(ec_fsm_pdo_t *);
-void ec_fsm_pdo_read_action_next_pdo(ec_fsm_pdo_t *);
-
-void ec_fsm_pdo_conf_state_start(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_state_read_mapping(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_state_mapping(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_state_zero_pdo_count(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_state_assign_pdo(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_state_set_pdo_count(ec_fsm_pdo_t *);
-
-void ec_fsm_pdo_conf_action_next_sync(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_action_pdo_mapping(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_action_check_mapping(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_action_next_pdo_mapping(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_action_check_assignment(ec_fsm_pdo_t *);
-void ec_fsm_pdo_conf_action_assign_pdo(ec_fsm_pdo_t *);
-
-void ec_fsm_pdo_state_end(ec_fsm_pdo_t *);
-void ec_fsm_pdo_state_error(ec_fsm_pdo_t *);
+void ec_fsm_pdo_read_state_start(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_read_state_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_read_state_pdo(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_read_state_pdo_entries(ec_fsm_pdo_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_read_action_next_sync(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_read_action_next_pdo(ec_fsm_pdo_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_conf_state_start(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_state_read_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_state_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_state_zero_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_state_assign_pdo(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_state_set_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_conf_action_next_sync(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_action_pdo_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_action_check_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_action_next_pdo_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_action_check_assignment(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_action_assign_pdo(ec_fsm_pdo_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_state_end(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_state_error(ec_fsm_pdo_t *, ec_datagram_t *);
 
 /*****************************************************************************/
 
@@ -162,10 +161,12 @@
  * \return false, if state machine has terminated
  */
 int ec_fsm_pdo_exec(
-        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
-        )
-{
-    fsm->state(fsm);
+        ec_fsm_pdo_t *fsm, /**< PDO configuration state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    fsm->state(fsm, datagram);
+
     return ec_fsm_pdo_running(fsm);
 }
 
@@ -189,12 +190,13 @@
 /** Start reading PDO assignment.
  */
 void ec_fsm_pdo_read_state_start(
-        ec_fsm_pdo_t *fsm /**< finite state machine */
+        ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     // read PDO assignment for first sync manager not reserved for mailbox
     fsm->sync_index = 1; // next is 2
-    ec_fsm_pdo_read_action_next_sync(fsm);
+    ec_fsm_pdo_read_action_next_sync(fsm, datagram);
 }
 
 /*****************************************************************************/
@@ -202,7 +204,8 @@
 /** Read PDO assignment of next sync manager.
  */
 void ec_fsm_pdo_read_action_next_sync(
-        ec_fsm_pdo_t *fsm /**< Finite state machine */
+        ec_fsm_pdo_t *fsm, /**< finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     ec_slave_t *slave = fsm->slave;
@@ -222,7 +225,7 @@
         ecrt_sdo_request_read(&fsm->request);
         fsm->state = ec_fsm_pdo_read_state_pdo_count;
         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
-        ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+        ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
         return;
     }
 
@@ -237,15 +240,18 @@
 /** Count assigned PDOs.
  */
 void ec_fsm_pdo_read_state_pdo_count(
-        ec_fsm_pdo_t *fsm /**< finite state machine */
-        )
-{
-    if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
+        ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+        return;
+    }
 
     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
         EC_SLAVE_ERR(fsm->slave, "Failed to read number of assigned PDOs"
                 " for SM%u.\n", fsm->sync_index);
-        ec_fsm_pdo_read_action_next_sync(fsm);
+        ec_fsm_pdo_read_action_next_sync(fsm, datagram);
         return;
     }
 
@@ -253,7 +259,7 @@
         EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu returned"
                 " when uploading SDO 0x%04X:%02X.\n", fsm->request.data_size,
                 fsm->request.index, fsm->request.subindex);
-        ec_fsm_pdo_read_action_next_sync(fsm);
+        ec_fsm_pdo_read_action_next_sync(fsm, datagram);
         return;
     }
     fsm->pdo_count = EC_READ_U8(fsm->request.data);
@@ -262,7 +268,7 @@
 
     // read first PDO
     fsm->pdo_pos = 1;
-    ec_fsm_pdo_read_action_next_pdo(fsm);
+    ec_fsm_pdo_read_action_next_pdo(fsm, datagram);
 }
 
 /*****************************************************************************/
@@ -270,7 +276,8 @@
 /** Read next PDO.
  */
 void ec_fsm_pdo_read_action_next_pdo(
-        ec_fsm_pdo_t *fsm /**< finite state machine */
+        ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     if (fsm->pdo_pos <= fsm->pdo_count) {
@@ -279,7 +286,7 @@
         ecrt_sdo_request_read(&fsm->request);
         fsm->state = ec_fsm_pdo_read_state_pdo;
         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
-        ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+        ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
         return;
     }
 
@@ -289,7 +296,7 @@
     ec_pdo_list_clear_pdos(&fsm->pdos);
 
     // next sync manager
-    ec_fsm_pdo_read_action_next_sync(fsm);
+    ec_fsm_pdo_read_action_next_sync(fsm, datagram);
 }
 
 /*****************************************************************************/
@@ -297,16 +304,19 @@
 /** Fetch PDO information.
  */
 void ec_fsm_pdo_read_state_pdo(
-        ec_fsm_pdo_t *fsm /**< finite state machine */
-        )
-{
-    if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
+        ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+        return;
+    }
 
     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
         EC_SLAVE_ERR(fsm->slave, "Failed to read index of"
                 " assigned PDO %u from SM%u.\n",
                 fsm->pdo_pos, fsm->sync_index);
-        ec_fsm_pdo_read_action_next_sync(fsm);
+        ec_fsm_pdo_read_action_next_sync(fsm, datagram);
         return;
     }
 
@@ -314,14 +324,14 @@
         EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu returned"
                 " when uploading SDO 0x%04X:%02X.\n", fsm->request.data_size,
                 fsm->request.index, fsm->request.subindex);
-        ec_fsm_pdo_read_action_next_sync(fsm);
+        ec_fsm_pdo_read_action_next_sync(fsm, datagram);
         return;
     }
 
     if (!(fsm->pdo = (ec_pdo_t *)
                 kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
         EC_SLAVE_ERR(fsm->slave, "Failed to allocate PDO.\n");
-        ec_fsm_pdo_read_action_next_sync(fsm);
+        ec_fsm_pdo_read_action_next_sync(fsm, datagram);
         return;
     }
 
@@ -335,7 +345,7 @@
 
     fsm->state = ec_fsm_pdo_read_state_pdo_entries;
     ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave, fsm->pdo);
-    fsm->state(fsm); // execute immediately
+    fsm->state(fsm, datagram); // execute immediately
 }
 
 /*****************************************************************************/
@@ -343,22 +353,24 @@
 /** Fetch PDO information.
  */
 void ec_fsm_pdo_read_state_pdo_entries(
-        ec_fsm_pdo_t *fsm /**< finite state machine */
-        )
-{
-    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))
-        return;
+        ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry, datagram)) {
+        return;
+    }
 
     if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) {
         EC_SLAVE_ERR(fsm->slave, "Failed to read mapped PDO entries"
                 " for PDO 0x%04X.\n", fsm->pdo->index);
-        ec_fsm_pdo_read_action_next_sync(fsm);
+        ec_fsm_pdo_read_action_next_sync(fsm, datagram);
         return;
     }
 
     // next PDO
     fsm->pdo_pos++;
-    ec_fsm_pdo_read_action_next_pdo(fsm);
+    ec_fsm_pdo_read_action_next_pdo(fsm, datagram);
 }
 
 /******************************************************************************
@@ -368,7 +380,8 @@
 /** Start PDO configuration.
  */
 void ec_fsm_pdo_conf_state_start(
-        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+        ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     if (!fsm->slave->config) {
@@ -377,7 +390,7 @@
     }
 
     fsm->sync_index = 1; // next is 2
-    ec_fsm_pdo_conf_action_next_sync(fsm);
+    ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
 }
 
 /*****************************************************************************/
@@ -400,7 +413,8 @@
 /** Get the next sync manager for a pdo configuration.
  */
 void ec_fsm_pdo_conf_action_next_sync(
-        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+        ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     fsm->sync_index++;
@@ -432,11 +446,11 @@
         if (!(fsm->pdo =
                     ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {
             // no pdos configured
-            ec_fsm_pdo_conf_action_check_assignment(fsm);
+            ec_fsm_pdo_conf_action_check_assignment(fsm, datagram);
             return;
         }
 
-        ec_fsm_pdo_conf_action_pdo_mapping(fsm);
+        ec_fsm_pdo_conf_action_pdo_mapping(fsm, datagram);
         return;
     }
 
@@ -448,7 +462,8 @@
 /** Check if the mapping has to be read, otherwise start to configure it.
  */
 void ec_fsm_pdo_conf_action_pdo_mapping(
-        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+        ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     const ec_pdo_t *assigned_pdo;
@@ -469,12 +484,12 @@
         ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave,
                 &fsm->slave_pdo);
         fsm->state = ec_fsm_pdo_conf_state_read_mapping;
-        fsm->state(fsm); // execute immediately
+        fsm->state(fsm, datagram); // execute immediately
         return;
     }
 
     // pdo mapping is known, check if it most be re-configured
-    ec_fsm_pdo_conf_action_check_mapping(fsm);
+    ec_fsm_pdo_conf_action_check_mapping(fsm, datagram);
 }
 
 /*****************************************************************************/
@@ -482,11 +497,13 @@
 /** Execute the PDO entry state machine to read the current PDO's mapping.
  */
 void ec_fsm_pdo_conf_state_read_mapping(
-        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
-        )
-{
-    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))
-        return;
+        ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry, datagram)) {
+        return;
+    }
 
     if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry))
         EC_SLAVE_WARN(fsm->slave,
@@ -494,7 +511,7 @@
                 fsm->pdo->index);
 
     // check if the mapping must be re-configured
-    ec_fsm_pdo_conf_action_check_mapping(fsm);
+    ec_fsm_pdo_conf_action_check_mapping(fsm, datagram);
 }
 
 /*****************************************************************************/
@@ -504,7 +521,8 @@
  * \todo Display mapping differences.
  */
 void ec_fsm_pdo_conf_action_check_mapping(
-        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+        ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     // check, if slave supports PDO configuration
@@ -516,7 +534,7 @@
         ec_fsm_pdo_entry_start_configuration(&fsm->fsm_pdo_entry, fsm->slave,
                 fsm->pdo, &fsm->slave_pdo);
         fsm->state = ec_fsm_pdo_conf_state_mapping;
-        fsm->state(fsm); // execure immediately
+        fsm->state(fsm, datagram); // execure immediately
         return;
     }
     else if (!ec_pdo_equal_entries(fsm->pdo, &fsm->slave_pdo)) {
@@ -530,7 +548,7 @@
         printk("\n");
     }
 
-    ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);
+    ec_fsm_pdo_conf_action_next_pdo_mapping(fsm, datagram);
 }
 
 /*****************************************************************************/
@@ -538,18 +556,20 @@
 /** Let the PDO entry state machine configure the current PDO's mapping.
  */
 void ec_fsm_pdo_conf_state_mapping(
-        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
-        )
-{
-    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))
-        return;
+        ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry, datagram)) {
+        return;
+    }
 
     if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry))
         EC_SLAVE_WARN(fsm->slave,
                 "Failed to configure mapping of PDO 0x%04X.\n",
                 fsm->pdo->index);
 
-    ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);
+    ec_fsm_pdo_conf_action_next_pdo_mapping(fsm, datagram);
 }
 
 /*****************************************************************************/
@@ -557,17 +577,18 @@
 /** Check mapping of next PDO, otherwise configure assignment.
  */
 void ec_fsm_pdo_conf_action_next_pdo_mapping(
-        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+        ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     // get next configured PDO
     if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdo->list))) {
         // no more configured pdos
-        ec_fsm_pdo_conf_action_check_assignment(fsm);
-        return;
-    }
-
-    ec_fsm_pdo_conf_action_pdo_mapping(fsm);
+        ec_fsm_pdo_conf_action_check_assignment(fsm, datagram);
+        return;
+    }
+
+    ec_fsm_pdo_conf_action_pdo_mapping(fsm, datagram);
 }
 
 /*****************************************************************************/
@@ -575,7 +596,8 @@
 /** Check if the PDO assignment of the current SM has to be re-configured.
  */
 void ec_fsm_pdo_conf_action_check_assignment(
-        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+        ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     if ((fsm->slave->sii.mailbox_protocols & EC_MBOX_COE)
@@ -605,7 +627,7 @@
 
         fsm->state = ec_fsm_pdo_conf_state_zero_pdo_count;
         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
-        ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+        ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
         return;
     }
     else if (!ec_pdo_list_equal(&fsm->sync->pdos, &fsm->pdos)) {
@@ -613,7 +635,7 @@
         EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm);
     }
 
-    ec_fsm_pdo_conf_action_next_sync(fsm);
+    ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
 }
 
 /*****************************************************************************/
@@ -621,17 +643,20 @@
 /** Set the number of assigned PDOs to zero.
  */
 void ec_fsm_pdo_conf_state_zero_pdo_count(
-        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
-        )
-{
-    if (ec_fsm_coe_exec(fsm->fsm_coe))
-        return;
+        ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+        return;
+    }
 
     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
         EC_SLAVE_WARN(fsm->slave, "Failed to clear PDO assignment of SM%u.\n",
                 fsm->sync_index);
-        EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm);
-        ec_fsm_pdo_conf_action_next_sync(fsm);
+        EC_SLAVE_WARN(fsm->slave, "");
+        ec_fsm_pdo_print(fsm);
+        ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
         return;
     }
 
@@ -642,15 +667,14 @@
 
     // find first PDO
     if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {
-
         // check for mapping to be altered
-        ec_fsm_pdo_conf_action_next_sync(fsm);
+        ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
         return;
     }
 
     // assign first PDO
     fsm->pdo_pos = 1;
-    ec_fsm_pdo_conf_action_assign_pdo(fsm);
+    ec_fsm_pdo_conf_action_assign_pdo(fsm, datagram);
 }
 
 /*****************************************************************************/
@@ -658,7 +682,8 @@
 /** Assign a PDO.
  */
 void ec_fsm_pdo_conf_action_assign_pdo(
-        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+        ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     EC_WRITE_U16(fsm->request.data, fsm->pdo->index);
@@ -672,7 +697,7 @@
 
     fsm->state = ec_fsm_pdo_conf_state_assign_pdo;
     ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
-    ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+    ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
 }
 
 /*****************************************************************************/
@@ -680,10 +705,13 @@
 /** Add a PDO to the sync managers PDO assignment.
  */
 void ec_fsm_pdo_conf_state_assign_pdo(
-        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
-        )
-{
-    if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
+        ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+        return;
+    }
 
     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
         EC_SLAVE_WARN(fsm->slave, "Failed to assign PDO 0x%04X at position %u"
@@ -696,7 +724,6 @@
 
     // find next PDO
     if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdo->list))) {
-
         // no more PDOs to assign, set PDO count
         EC_WRITE_U8(fsm->request.data, fsm->pdo_pos);
         fsm->request.data_size = 1;
@@ -709,13 +736,13 @@
 
         fsm->state = ec_fsm_pdo_conf_state_set_pdo_count;
         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
-        ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+        ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
         return;
     }
 
     // add next PDO to assignment
     fsm->pdo_pos++;
-    ec_fsm_pdo_conf_action_assign_pdo(fsm);
+    ec_fsm_pdo_conf_action_assign_pdo(fsm, datagram);
 }
 
 /*****************************************************************************/
@@ -723,10 +750,13 @@
 /** Set the number of assigned PDOs.
  */
 void ec_fsm_pdo_conf_state_set_pdo_count(
-        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
-        )
-{
-    if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
+        ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+        return;
+    }
 
     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
         EC_SLAVE_WARN(fsm->slave, "Failed to set number of"
@@ -743,7 +773,7 @@
             " PDO assignment of SM%u.\n", fsm->sync_index);
 
     // check if PDO mapping has to be altered
-    ec_fsm_pdo_conf_action_next_sync(fsm);
+    ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
 }
 
 /******************************************************************************
@@ -753,7 +783,8 @@
 /** State: ERROR.
  */
 void ec_fsm_pdo_state_error(
-        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+        ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
 }
@@ -763,9 +794,10 @@
 /** State: END.
  */
 void ec_fsm_pdo_state_end(
-        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
-        )
-{
-}
-
-/*****************************************************************************/
+        ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+}
+
+/*****************************************************************************/
diff -r 505cf41488a4 -r 9cdd7669dc0b master/fsm_pdo.h
--- a/master/fsm_pdo.h	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_pdo.h	Thu Jan 10 17:36:41 2013 +0100
@@ -53,7 +53,7 @@
  */
 struct ec_fsm_pdo
 {
-    void (*state)(ec_fsm_pdo_t *); /**< State function. */
+    void (*state)(ec_fsm_pdo_t *, ec_datagram_t *); /**< State function. */
     ec_fsm_coe_t *fsm_coe; /**< CoE state machine to use. */
     ec_fsm_pdo_entry_t fsm_pdo_entry; /**< PDO entry state machine. */
     ec_pdo_list_t pdos; /**< PDO configuration. */
@@ -76,7 +76,7 @@
 void ec_fsm_pdo_start_reading(ec_fsm_pdo_t *, ec_slave_t *);
 void ec_fsm_pdo_start_configuration(ec_fsm_pdo_t *, ec_slave_t *);
 
-int ec_fsm_pdo_exec(ec_fsm_pdo_t *);
+int ec_fsm_pdo_exec(ec_fsm_pdo_t *, ec_datagram_t *);
 int ec_fsm_pdo_success(const ec_fsm_pdo_t *);
 
 /*****************************************************************************/
diff -r 505cf41488a4 -r 9cdd7669dc0b master/fsm_pdo_entry.c
--- a/master/fsm_pdo_entry.c	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_pdo_entry.c	Thu Jan 10 17:36:41 2013 +0100
@@ -42,21 +42,24 @@
 
 /*****************************************************************************/
 
-void ec_fsm_pdo_entry_read_state_start(ec_fsm_pdo_entry_t *);
-void ec_fsm_pdo_entry_read_state_count(ec_fsm_pdo_entry_t *);
-void ec_fsm_pdo_entry_read_state_entry(ec_fsm_pdo_entry_t *);
-
-void ec_fsm_pdo_entry_read_action_next(ec_fsm_pdo_entry_t *);
-
-void ec_fsm_pdo_entry_conf_state_start(ec_fsm_pdo_entry_t *);
-void ec_fsm_pdo_entry_conf_state_zero_entry_count(ec_fsm_pdo_entry_t *);
-void ec_fsm_pdo_entry_conf_state_map_entry(ec_fsm_pdo_entry_t *);
-void ec_fsm_pdo_entry_conf_state_set_entry_count(ec_fsm_pdo_entry_t *);
-
-void ec_fsm_pdo_entry_conf_action_map(ec_fsm_pdo_entry_t *);
-
-void ec_fsm_pdo_entry_state_end(ec_fsm_pdo_entry_t *);
-void ec_fsm_pdo_entry_state_error(ec_fsm_pdo_entry_t *);
+void ec_fsm_pdo_entry_read_state_start(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+void ec_fsm_pdo_entry_read_state_count(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+void ec_fsm_pdo_entry_read_state_entry(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_entry_read_action_next(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_entry_conf_state_start(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+void ec_fsm_pdo_entry_conf_state_zero_entry_count(ec_fsm_pdo_entry_t *,
+        ec_datagram_t *);
+void ec_fsm_pdo_entry_conf_state_map_entry(ec_fsm_pdo_entry_t *,
+        ec_datagram_t *);
+void ec_fsm_pdo_entry_conf_state_set_entry_count(ec_fsm_pdo_entry_t *,
+        ec_datagram_t *);
+
+void ec_fsm_pdo_entry_conf_action_map(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_entry_state_end(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+void ec_fsm_pdo_entry_state_error(ec_fsm_pdo_entry_t *, ec_datagram_t *);
 
 /*****************************************************************************/
 
@@ -87,7 +90,7 @@
 /** Print the current and desired PDO mapping.
  */
 void ec_fsm_pdo_entry_print(
-        ec_fsm_pdo_entry_t *fsm /**< PDO configuration state machine. */
+        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
         )
 {
     printk("Currently mapped PDO entries: ");
@@ -103,7 +106,7 @@
  */
 void ec_fsm_pdo_entry_start_reading(
         ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
-        ec_slave_t *slave, /**< slave to configure */
+        ec_slave_t *slave, /**< Slave to configure. */
         ec_pdo_t *pdo /**< PDO to read entries for. */
         )
 {
@@ -121,7 +124,7 @@
  */
 void ec_fsm_pdo_entry_start_configuration(
         ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
-        ec_slave_t *slave, /**< slave to configure */
+        ec_slave_t *slave, /**< Slave to configure. */
         const ec_pdo_t *pdo, /**< PDO with the desired entries. */
         const ec_pdo_t *cur_pdo /**< Current PDO mapping. */
         )
@@ -160,10 +163,12 @@
  * \return false, if state machine has terminated
  */
 int ec_fsm_pdo_entry_exec(
-        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
-        )
-{
-    fsm->state(fsm);
+        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    fsm->state(fsm, datagram);
+
     return ec_fsm_pdo_entry_running(fsm);
 }
 
@@ -187,7 +192,8 @@
 /** Request reading the number of mapped PDO entries.
  */
 void ec_fsm_pdo_entry_read_state_start(
-        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
+        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     ecrt_sdo_request_index(&fsm->request, fsm->target_pdo->index, 0);
@@ -195,7 +201,7 @@
 
     fsm->state = ec_fsm_pdo_entry_read_state_count;
     ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
-    ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+    ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
 }
 
 /*****************************************************************************/
@@ -203,11 +209,13 @@
 /** Read number of mapped PDO entries.
  */
 void ec_fsm_pdo_entry_read_state_count(
-        ec_fsm_pdo_entry_t *fsm /**< finite state machine */
-        )
-{
-    if (ec_fsm_coe_exec(fsm->fsm_coe))
-        return;
+        ec_fsm_pdo_entry_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+        return;
+    }
 
     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
         EC_SLAVE_ERR(fsm->slave,
@@ -231,7 +239,7 @@
 
     // read first PDO entry
     fsm->entry_pos = 1;
-    ec_fsm_pdo_entry_read_action_next(fsm);
+    ec_fsm_pdo_entry_read_action_next(fsm, datagram);
 }
 
 /*****************************************************************************/
@@ -239,7 +247,8 @@
 /** Read next PDO entry.
  */
 void ec_fsm_pdo_entry_read_action_next(
-        ec_fsm_pdo_entry_t *fsm /**< finite state machine */
+        ec_fsm_pdo_entry_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     if (fsm->entry_pos <= fsm->entry_count) {
@@ -248,7 +257,7 @@
         ecrt_sdo_request_read(&fsm->request);
         fsm->state = ec_fsm_pdo_entry_read_state_entry;
         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
-        ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+        ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
         return;
     }
 
@@ -261,10 +270,13 @@
 /** Read PDO entry information.
  */
 void ec_fsm_pdo_entry_read_state_entry(
-        ec_fsm_pdo_entry_t *fsm /**< finite state machine */
-        )
-{
-    if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
+        ec_fsm_pdo_entry_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+        return;
+    }
 
     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
         EC_SLAVE_ERR(fsm->slave, "Failed to read mapped PDO entry.\n");
@@ -315,7 +327,7 @@
 
         // next PDO entry
         fsm->entry_pos++;
-        ec_fsm_pdo_entry_read_action_next(fsm);
+        ec_fsm_pdo_entry_read_action_next(fsm, datagram);
     }
 }
 
@@ -326,7 +338,8 @@
 /** Start PDO mapping.
  */
 void ec_fsm_pdo_entry_conf_state_start(
-        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
+        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     if (ec_sdo_request_alloc(&fsm->request, 4)) {
@@ -344,7 +357,7 @@
 
     fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count;
     ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
-    ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+    ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
 }
 
 /*****************************************************************************/
@@ -367,11 +380,13 @@
 /** Set the number of mapped entries to zero.
  */
 void ec_fsm_pdo_entry_conf_state_zero_entry_count(
-        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
-        )
-{
-    if (ec_fsm_coe_exec(fsm->fsm_coe))
-        return;
+        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+        return;
+    }
 
     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
         EC_SLAVE_WARN(fsm->slave, "Failed to clear PDO mapping.\n");
@@ -392,7 +407,7 @@
 
     // add first entry
     fsm->entry_pos = 1;
-    ec_fsm_pdo_entry_conf_action_map(fsm);
+    ec_fsm_pdo_entry_conf_action_map(fsm, datagram);
 }
 
 /*****************************************************************************/
@@ -400,7 +415,8 @@
 /** Starts to add a PDO entry.
  */
 void ec_fsm_pdo_entry_conf_action_map(
-        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
+        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     uint32_t value;
@@ -420,7 +436,7 @@
 
     fsm->state = ec_fsm_pdo_entry_conf_state_map_entry;
     ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
-    ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+    ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
 }
 
 /*****************************************************************************/
@@ -428,10 +444,13 @@
 /** Add a PDO entry.
  */
 void ec_fsm_pdo_entry_conf_state_map_entry(
-        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
-        )
-{
-    if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
+        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+        return;
+    }
 
     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
         EC_SLAVE_WARN(fsm->slave, "Failed to map PDO entry"
@@ -458,13 +477,13 @@
 
         fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count;
         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
-        ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+        ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
         return;
     }
 
     // add next entry
     fsm->entry_pos++;
-    ec_fsm_pdo_entry_conf_action_map(fsm);
+    ec_fsm_pdo_entry_conf_action_map(fsm, datagram);
 }
 
 /*****************************************************************************/
@@ -472,10 +491,13 @@
 /** Set the number of entries.
  */
 void ec_fsm_pdo_entry_conf_state_set_entry_count(
-        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
-        )
-{
-    if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
+        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+        return;
+    }
 
     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
         EC_SLAVE_WARN(fsm->slave, "Failed to set number of entries.\n");
@@ -497,7 +519,8 @@
 /** State: ERROR.
  */
 void ec_fsm_pdo_entry_state_error(
-        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
+        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
 }
@@ -507,9 +530,10 @@
 /** State: END.
  */
 void ec_fsm_pdo_entry_state_end(
-        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
-        )
-{
-}
-
-/*****************************************************************************/
+        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+}
+
+/*****************************************************************************/
diff -r 505cf41488a4 -r 9cdd7669dc0b master/fsm_pdo_entry.h
--- a/master/fsm_pdo_entry.h	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_pdo_entry.h	Thu Jan 10 17:36:41 2013 +0100
@@ -51,7 +51,8 @@
  */
 struct ec_fsm_pdo_entry
 {
-    void (*state)(ec_fsm_pdo_entry_t *); /**< state function */
+    void (*state)(ec_fsm_pdo_entry_t *, ec_datagram_t *); /**< state function
+                                                           */
     ec_fsm_coe_t *fsm_coe; /**< CoE state machine to use */
     ec_sdo_request_t request; /**< SDO request. */
 
@@ -74,7 +75,7 @@
 void ec_fsm_pdo_entry_start_configuration(ec_fsm_pdo_entry_t *, ec_slave_t *,
         const ec_pdo_t *, const ec_pdo_t *);
 
-int ec_fsm_pdo_entry_exec(ec_fsm_pdo_entry_t *);
+int ec_fsm_pdo_entry_exec(ec_fsm_pdo_entry_t *, ec_datagram_t *);
 int ec_fsm_pdo_entry_success(const ec_fsm_pdo_entry_t *);
 
 /*****************************************************************************/
diff -r 505cf41488a4 -r 9cdd7669dc0b master/fsm_slave.c
--- a/master/fsm_slave.c	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_slave.c	Thu Jan 10 17:36:41 2013 +0100
@@ -42,16 +42,16 @@
 
 /*****************************************************************************/
 
-void ec_fsm_slave_state_idle(ec_fsm_slave_t *);
-void ec_fsm_slave_state_ready(ec_fsm_slave_t *);
-int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *);
-void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *);
-int ec_fsm_slave_action_process_reg(ec_fsm_slave_t *);
-void ec_fsm_slave_state_reg_request(ec_fsm_slave_t *);
-int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *);
-void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *);
-int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *);
-void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *);
+void ec_fsm_slave_state_idle(ec_fsm_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_state_ready(ec_fsm_slave_t *, ec_datagram_t *);
+int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *, ec_datagram_t *);
+int ec_fsm_slave_action_process_reg(ec_fsm_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_state_reg_request(ec_fsm_slave_t *, ec_datagram_t *);
+int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *, ec_datagram_t *);
+int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *, ec_datagram_t *);
 
 /*****************************************************************************/
 
@@ -59,22 +59,23 @@
  */
 void ec_fsm_slave_init(
         ec_fsm_slave_t *fsm, /**< Slave state machine. */
-        ec_slave_t *slave, /**< EtherCAT slave. */
-        ec_datagram_t *datagram /**< Datagram object to use. */
+        ec_slave_t *slave /**< EtherCAT slave. */
         )
 {
     fsm->slave = slave;
-    fsm->datagram = datagram;
-    fsm->datagram->data_size = 0;
-
-    EC_SLAVE_DBG(slave, 1, "Init FSM.\n");
+    INIT_LIST_HEAD(&fsm->list); // mark as unlisted
 
     fsm->state = ec_fsm_slave_state_idle;
-
-    // init sub-state-machines
-    ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);
-    ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram);
-    ec_fsm_soe_init(&fsm->fsm_soe, fsm->datagram);
+    fsm->datagram = NULL;
+    fsm->sdo_request = NULL;
+    fsm->reg_request = NULL;
+    fsm->foe_request = NULL;
+    fsm->soe_request = NULL;
+
+    // Init sub-state-machines
+    ec_fsm_coe_init(&fsm->fsm_coe);
+    ec_fsm_foe_init(&fsm->fsm_foe);
+    ec_fsm_soe_init(&fsm->fsm_soe);
 }
 
 /*****************************************************************************/
@@ -85,6 +86,28 @@
         ec_fsm_slave_t *fsm /**< Master state machine. */
         )
 {
+    // signal requests that are currently in operation
+
+    if (fsm->sdo_request) {
+        fsm->sdo_request->state = EC_INT_REQUEST_FAILURE;
+        wake_up_all(&fsm->slave->master->request_queue);
+    }
+
+    if (fsm->reg_request) {
+        fsm->reg_request->state = EC_INT_REQUEST_FAILURE;
+        wake_up_all(&fsm->slave->master->request_queue);
+    }
+
+    if (fsm->foe_request) {
+        fsm->foe_request->state = EC_INT_REQUEST_FAILURE;
+        wake_up_all(&fsm->slave->master->request_queue);
+    }
+
+    if (fsm->soe_request) {
+        fsm->soe_request->state = EC_INT_REQUEST_FAILURE;
+        wake_up_all(&fsm->slave->master->request_queue);
+    }
+
     // clear sub-state machines
     ec_fsm_coe_clear(&fsm->fsm_coe);
     ec_fsm_foe_clear(&fsm->fsm_foe);
@@ -95,29 +118,34 @@
 
 /** 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.
- */
-void ec_fsm_slave_exec(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
-        )
-{
-    if (fsm->datagram->state == EC_DATAGRAM_SENT
-        || fsm->datagram->state == EC_DATAGRAM_QUEUED) {
-        // datagram was not sent or received yet.
-        return;
-    }
-
-    fsm->state(fsm);
-
-    ec_datagram_output_stats(fsm->datagram);
+ * \return 1 if \a datagram was used, else 0.
+ */
+int ec_fsm_slave_exec(
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< New datagram to use. */
+        )
+{
+    int datagram_used;
+
+    fsm->state(fsm, datagram);
+
+    datagram_used = fsm->state != ec_fsm_slave_state_idle &&
+        fsm->state != ec_fsm_slave_state_ready;
+
+    if (datagram_used) {
+        fsm->datagram = datagram;
+    } else {
+        fsm->datagram = NULL;
+    }
+
+    return datagram_used;
 }
 
 /*****************************************************************************/
 
 /** Sets the current state of the state machine to READY
  */
-void ec_fsm_slave_ready(
+void ec_fsm_slave_set_ready(
         ec_fsm_slave_t *fsm /**< Slave state machine. */
         )
 {
@@ -127,6 +155,17 @@
     }
 }
 
+/*****************************************************************************/
+
+/** Returns, if the FSM is currently not busy and ready to execute.
+ */
+int ec_fsm_slave_is_ready(
+        const ec_fsm_slave_t *fsm /**< Slave state machine. */
+        )
+{
+    return fsm->state == ec_fsm_slave_state_ready;
+}
+
 /******************************************************************************
  * Slave state machine
  *****************************************************************************/
@@ -134,7 +173,8 @@
 /** Slave state: IDLE.
  */
 void ec_fsm_slave_state_idle(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     // do nothing
@@ -145,26 +185,27 @@
 /** Slave state: READY.
  */
 void ec_fsm_slave_state_ready(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     // Check for pending external SDO requests
-    if (ec_fsm_slave_action_process_sdo(fsm)) {
+    if (ec_fsm_slave_action_process_sdo(fsm, datagram)) {
         return;
     }
 
     // Check for pending external register requests
-    if (ec_fsm_slave_action_process_reg(fsm)) {
+    if (ec_fsm_slave_action_process_reg(fsm, datagram)) {
         return;
     }
 
     // Check for pending FoE requests
-    if (ec_fsm_slave_action_process_foe(fsm)) {
+    if (ec_fsm_slave_action_process_foe(fsm, datagram)) {
         return;
     }
 
     // Check for pending SoE requests
-    if (ec_fsm_slave_action_process_soe(fsm)) {
+    if (ec_fsm_slave_action_process_soe(fsm, datagram)) {
         return;
     }
 }
@@ -176,7 +217,8 @@
  * \return non-zero, if an SDO request is processed.
  */
 int ec_fsm_slave_action_process_sdo(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     ec_slave_t *slave = fsm->slave;
@@ -194,7 +236,7 @@
         EC_SLAVE_WARN(slave, "Aborting SDO request,"
                 " slave has error flag set.\n");
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
+        wake_up_all(&slave->master->request_queue);
         fsm->state = ec_fsm_slave_state_idle;
         return 1;
     }
@@ -202,22 +244,21 @@
     if (slave->current_state == EC_SLAVE_STATE_INIT) {
         EC_SLAVE_WARN(slave, "Aborting SDO request, slave is in INIT.\n");
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
+        wake_up_all(&slave->master->request_queue);
         fsm->state = ec_fsm_slave_state_idle;
         return 1;
     }
 
+    fsm->sdo_request = request;
     request->state = EC_INT_REQUEST_BUSY;
 
     // Found pending SDO request. Execute it!
     EC_SLAVE_DBG(slave, 1, "Processing SDO request...\n");
 
     // Start SDO transfer
-    fsm->sdo_request = request;
     fsm->state = ec_fsm_slave_state_sdo_request;
     ec_fsm_coe_transfer(&fsm->fsm_coe, slave, request);
-    ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
-    ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+    ec_fsm_coe_exec(&fsm->fsm_coe, datagram); // execute immediately
     return 1;
 }
 
@@ -226,21 +267,21 @@
 /** Slave state: SDO_REQUEST.
  */
 void ec_fsm_slave_state_sdo_request(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     ec_slave_t *slave = fsm->slave;
     ec_sdo_request_t *request = fsm->sdo_request;
 
-    if (ec_fsm_coe_exec(&fsm->fsm_coe)) {
-        ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+    if (ec_fsm_coe_exec(&fsm->fsm_coe, datagram)) {
         return;
     }
 
     if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
         EC_SLAVE_ERR(slave, "Failed to process SDO request.\n");
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
+        wake_up_all(&slave->master->request_queue);
         fsm->sdo_request = NULL;
         fsm->state = ec_fsm_slave_state_ready;
         return;
@@ -250,7 +291,7 @@
 
     // SDO request finished
     request->state = EC_INT_REQUEST_SUCCESS;
-    wake_up(&slave->master->request_queue);
+    wake_up_all(&slave->master->request_queue);
     fsm->sdo_request = NULL;
     fsm->state = ec_fsm_slave_state_ready;
 }
@@ -262,7 +303,8 @@
  * \return non-zero, if a register request is processed.
  */
 int ec_fsm_slave_action_process_reg(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     ec_slave_t *slave = fsm->slave;
@@ -295,7 +337,7 @@
         EC_SLAVE_WARN(slave, "Aborting register request,"
                 " slave has error flag set.\n");
         fsm->reg_request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
+        wake_up_all(&slave->master->request_queue);
         fsm->reg_request = NULL;
         fsm->state = ec_fsm_slave_state_idle;
         return 1;
@@ -308,17 +350,16 @@
 
     // Start register access
     if (fsm->reg_request->dir == EC_DIR_INPUT) {
-        ec_datagram_fprd(fsm->datagram, slave->station_address,
+        ec_datagram_fprd(datagram, slave->station_address,
                 fsm->reg_request->address, fsm->reg_request->transfer_size);
-        ec_datagram_zero(fsm->datagram);
+        ec_datagram_zero(datagram);
     } else {
-        ec_datagram_fpwr(fsm->datagram, slave->station_address,
+        ec_datagram_fpwr(datagram, slave->station_address,
                 fsm->reg_request->address, fsm->reg_request->transfer_size);
-        memcpy(fsm->datagram->data, fsm->reg_request->data,
+        memcpy(datagram->data, fsm->reg_request->data,
                 fsm->reg_request->transfer_size);
     }
-    fsm->datagram->device_index = slave->device_index;
-    ec_master_queue_external_datagram(slave->master, fsm->datagram);
+    datagram->device_index = slave->device_index;
     fsm->state = ec_fsm_slave_state_reg_request;
     return 1;
 }
@@ -328,7 +369,8 @@
 /** Slave state: Register request.
  */
 void ec_fsm_slave_state_reg_request(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     ec_slave_t *slave = fsm->slave;
@@ -337,6 +379,7 @@
     if (!reg) {
         // configuration was cleared in the meantime
         fsm->state = ec_fsm_slave_state_ready;
+        fsm->reg_request = NULL;
         return;
     }
 
@@ -345,7 +388,7 @@
                 " request datagram: ");
         ec_datagram_print_state(fsm->datagram);
         reg->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
+        wake_up_all(&slave->master->request_queue);
         fsm->reg_request = NULL;
         fsm->state = ec_fsm_slave_state_ready;
         return;
@@ -366,7 +409,7 @@
                 fsm->datagram->working_counter);
     }
 
-    wake_up(&slave->master->request_queue);
+    wake_up_all(&slave->master->request_queue);
     fsm->reg_request = NULL;
     fsm->state = ec_fsm_slave_state_ready;
 }
@@ -378,7 +421,8 @@
  * \return non-zero, if an FoE request is processed.
  */
 int ec_fsm_slave_action_process_foe(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     ec_slave_t *slave = fsm->slave;
@@ -396,20 +440,19 @@
         EC_SLAVE_WARN(slave, "Aborting FoE request,"
                 " slave has error flag set.\n");
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
+        wake_up_all(&slave->master->request_queue);
         fsm->state = ec_fsm_slave_state_idle;
         return 1;
     }
 
     request->state = EC_INT_REQUEST_BUSY;
+    fsm->foe_request = request;
 
     EC_SLAVE_DBG(slave, 1, "Processing FoE request.\n");
 
-    fsm->foe_request = request;
     fsm->state = ec_fsm_slave_state_foe_request;
     ec_fsm_foe_transfer(&fsm->fsm_foe, slave, request);
-    ec_fsm_foe_exec(&fsm->fsm_foe);
-    ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+    ec_fsm_foe_exec(&fsm->fsm_foe, datagram);
     return 1;
 }
 
@@ -418,21 +461,21 @@
 /** Slave state: FOE REQUEST.
  */
 void ec_fsm_slave_state_foe_request(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     ec_slave_t *slave = fsm->slave;
     ec_foe_request_t *request = fsm->foe_request;
 
-    if (ec_fsm_foe_exec(&fsm->fsm_foe)) {
-        ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+    if (ec_fsm_foe_exec(&fsm->fsm_foe, datagram)) {
         return;
     }
 
     if (!ec_fsm_foe_success(&fsm->fsm_foe)) {
         EC_SLAVE_ERR(slave, "Failed to handle FoE request.\n");
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
+        wake_up_all(&slave->master->request_queue);
         fsm->foe_request = NULL;
         fsm->state = ec_fsm_slave_state_ready;
         return;
@@ -443,7 +486,7 @@
             " data.\n", request->data_size);
 
     request->state = EC_INT_REQUEST_SUCCESS;
-    wake_up(&slave->master->request_queue);
+    wake_up_all(&slave->master->request_queue);
     fsm->foe_request = NULL;
     fsm->state = ec_fsm_slave_state_ready;
 }
@@ -455,7 +498,8 @@
  * \return non-zero, if a request is processed.
  */
 int ec_fsm_slave_action_process_soe(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     ec_slave_t *slave = fsm->slave;
@@ -473,7 +517,7 @@
         EC_SLAVE_WARN(slave, "Aborting SoE request,"
                 " slave has error flag set.\n");
         req->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
+        wake_up_all(&slave->master->request_queue);
         fsm->state = ec_fsm_slave_state_idle;
         return 1;
     }
@@ -481,22 +525,21 @@
     if (slave->current_state == EC_SLAVE_STATE_INIT) {
         EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n");
         req->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
+        wake_up_all(&slave->master->request_queue);
         fsm->state = ec_fsm_slave_state_idle;
         return 0;
     }
 
+    fsm->soe_request = req;
     req->state = EC_INT_REQUEST_BUSY;
 
     // Found pending request. Execute it!
     EC_SLAVE_DBG(slave, 1, "Processing SoE request...\n");
 
     // Start SoE transfer
-    fsm->soe_request = req;
     fsm->state = ec_fsm_slave_state_soe_request;
     ec_fsm_soe_transfer(&fsm->fsm_soe, slave, req);
-    ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately
-    ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+    ec_fsm_soe_exec(&fsm->fsm_soe, datagram); // execute immediately
     return 1;
 }
 
@@ -505,21 +548,21 @@
 /** Slave state: SOE_REQUEST.
  */
 void ec_fsm_slave_state_soe_request(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     ec_slave_t *slave = fsm->slave;
     ec_soe_request_t *request = fsm->soe_request;
 
-    if (ec_fsm_soe_exec(&fsm->fsm_soe)) {
-        ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+    if (ec_fsm_soe_exec(&fsm->fsm_soe, datagram)) {
         return;
     }
 
     if (!ec_fsm_soe_success(&fsm->fsm_soe)) {
         EC_SLAVE_ERR(slave, "Failed to process SoE request.\n");
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
+        wake_up_all(&slave->master->request_queue);
         fsm->soe_request = NULL;
         fsm->state = ec_fsm_slave_state_ready;
         return;
@@ -529,7 +572,7 @@
 
     // SoE request finished
     request->state = EC_INT_REQUEST_SUCCESS;
-    wake_up(&slave->master->request_queue);
+    wake_up_all(&slave->master->request_queue);
     fsm->soe_request = NULL;
     fsm->state = ec_fsm_slave_state_ready;
 }
diff -r 505cf41488a4 -r 9cdd7669dc0b master/fsm_slave.h
--- a/master/fsm_slave.h	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_slave.h	Thu Jan 10 17:36:41 2013 +0100
@@ -53,27 +53,29 @@
  */
 struct ec_fsm_slave {
     ec_slave_t *slave; /**< slave the FSM runs on */
-    ec_datagram_t *datagram; /**< datagram used in the state machine */
+    struct list_head list; /**< Used for execution list. */
 
-    void (*state)(ec_fsm_slave_t *); /**< master state function */
+    void (*state)(ec_fsm_slave_t *, ec_datagram_t *); /**< State function. */
+    ec_datagram_t *datagram; /**< Previous state datagram. */
     ec_sdo_request_t *sdo_request; /**< SDO request to process. */
     ec_reg_request_t *reg_request; /**< Register request to process. */
     ec_foe_request_t *foe_request; /**< FoE request to process. */
-    off_t foe_index; /**< index to FoE write request data */
+    off_t foe_index; /**< Index to FoE write request data. */
     ec_soe_request_t *soe_request; /**< SoE request to process. */
 
-    ec_fsm_coe_t fsm_coe; /**< CoE state machine */
-    ec_fsm_foe_t fsm_foe; /**< FoE state machine */
-    ec_fsm_soe_t fsm_soe; /**< SoE state machine */
+    ec_fsm_coe_t fsm_coe; /**< CoE state machine. */
+    ec_fsm_foe_t fsm_foe; /**< FoE state machine. */
+    ec_fsm_soe_t fsm_soe; /**< SoE state machine. */
 };
 
 /*****************************************************************************/
 
-void ec_fsm_slave_init(ec_fsm_slave_t *, ec_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_init(ec_fsm_slave_t *, ec_slave_t *);
 void ec_fsm_slave_clear(ec_fsm_slave_t *);
 
-void ec_fsm_slave_exec(ec_fsm_slave_t *);
-void ec_fsm_slave_ready(ec_fsm_slave_t *);
+int ec_fsm_slave_exec(ec_fsm_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_set_ready(ec_fsm_slave_t *);
+int ec_fsm_slave_is_ready(const ec_fsm_slave_t *);
 
 /*****************************************************************************/
 
diff -r 505cf41488a4 -r 9cdd7669dc0b master/fsm_slave_config.c
--- a/master/fsm_slave_config.c	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_slave_config.c	Thu Jan 10 17:36:41 2013 +0100
@@ -679,7 +679,7 @@
     ec_sdo_request_copy(&fsm->request_copy, fsm->request);
     ecrt_sdo_request_write(&fsm->request_copy);
     ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request_copy);
-    ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+    ec_fsm_coe_exec(fsm->fsm_coe, fsm->datagram); // execute immediately
 }
 
 /*****************************************************************************/
@@ -690,7 +690,7 @@
         ec_fsm_slave_config_t *fsm /**< slave state machine */
         )
 {
-    if (ec_fsm_coe_exec(fsm->fsm_coe)) {
+    if (ec_fsm_coe_exec(fsm->fsm_coe, fsm->datagram)) {
         return;
     }
 
@@ -713,7 +713,7 @@
         ec_sdo_request_copy(&fsm->request_copy, fsm->request);
         ecrt_sdo_request_write(&fsm->request_copy);
         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request_copy);
-        ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+        ec_fsm_coe_exec(fsm->fsm_coe, fsm->datagram); // execute immediately
         return;
     }
 
@@ -746,7 +746,7 @@
             ec_soe_request_write(&fsm->soe_request_copy);
             ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave,
                     &fsm->soe_request_copy);
-            ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately
+            ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram);
             return;
         }
     }
@@ -765,7 +765,7 @@
 {
     ec_slave_t *slave = fsm->slave;
 
-    if (ec_fsm_soe_exec(fsm->fsm_soe)) {
+    if (ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram)) {
         return;
     }
 
@@ -790,7 +790,7 @@
             ec_soe_request_write(&fsm->soe_request_copy);
             ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave,
                     &fsm->soe_request_copy);
-            ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately
+            ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram);
             return;
         }
     }
@@ -823,8 +823,9 @@
 {
     // TODO check for config here
 
-    if (ec_fsm_pdo_exec(fsm->fsm_pdo))
-        return;
+    if (ec_fsm_pdo_exec(fsm->fsm_pdo, fsm->datagram)) {
+        return;
+    }
 
     if (!fsm->slave->config) { // config removed in the meantime
         ec_fsm_slave_config_reconfigure(fsm);
@@ -1470,7 +1471,7 @@
             ec_soe_request_write(&fsm->soe_request_copy);
             ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave,
                     &fsm->soe_request_copy);
-            ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately
+            ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram);
             return;
         }
     }
@@ -1489,7 +1490,7 @@
 {
     ec_slave_t *slave = fsm->slave;
 
-    if (ec_fsm_soe_exec(fsm->fsm_soe)) {
+    if (ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram)) {
         return;
     }
 
@@ -1514,7 +1515,7 @@
             ec_soe_request_write(&fsm->soe_request_copy);
             ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave,
                     &fsm->soe_request_copy);
-            ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately
+            ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram);
             return;
         }
     }
diff -r 505cf41488a4 -r 9cdd7669dc0b master/fsm_slave_scan.c
--- a/master/fsm_slave_scan.c	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_slave_scan.c	Thu Jan 10 17:36:41 2013 +0100
@@ -894,7 +894,7 @@
     EC_SLAVE_DBG(slave, 1, "Scanning PDO assignment and mapping.\n");
     fsm->state = ec_fsm_slave_scan_state_pdos;
     ec_fsm_pdo_start_reading(fsm->fsm_pdo, slave);
-    ec_fsm_pdo_exec(fsm->fsm_pdo); // execute immediately
+    ec_fsm_pdo_exec(fsm->fsm_pdo, fsm->datagram); // execute immediately
 }
 
 /*****************************************************************************/
@@ -905,8 +905,9 @@
         ec_fsm_slave_scan_t *fsm /**< slave state machine */
         )
 {
-    if (ec_fsm_pdo_exec(fsm->fsm_pdo))
-        return;
+    if (ec_fsm_pdo_exec(fsm->fsm_pdo, fsm->datagram)) {
+        return;
+    }
 
     if (!ec_fsm_pdo_success(fsm->fsm_pdo)) {
         fsm->state = ec_fsm_slave_scan_state_error;
diff -r 505cf41488a4 -r 9cdd7669dc0b master/fsm_soe.c
--- a/master/fsm_soe.c	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_soe.c	Thu Jan 10 17:36:41 2013 +0100
@@ -64,18 +64,18 @@
 
 /*****************************************************************************/
 
-void ec_fsm_soe_read_start(ec_fsm_soe_t *);
-void ec_fsm_soe_read_request(ec_fsm_soe_t *);
-void ec_fsm_soe_read_check(ec_fsm_soe_t *);
-void ec_fsm_soe_read_response(ec_fsm_soe_t *);
-
-void ec_fsm_soe_write_start(ec_fsm_soe_t *);
-void ec_fsm_soe_write_request(ec_fsm_soe_t *);
-void ec_fsm_soe_write_check(ec_fsm_soe_t *);
-void ec_fsm_soe_write_response(ec_fsm_soe_t *);
-
-void ec_fsm_soe_end(ec_fsm_soe_t *);
-void ec_fsm_soe_error(ec_fsm_soe_t *);
+void ec_fsm_soe_read_start(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_read_request(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_read_check(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_read_response(ec_fsm_soe_t *, ec_datagram_t *);
+
+void ec_fsm_soe_write_start(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_write_request(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_write_check(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_write_response(ec_fsm_soe_t *, ec_datagram_t *);
+
+void ec_fsm_soe_end(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_error(ec_fsm_soe_t *, ec_datagram_t *);
 
 /*****************************************************************************/
 
@@ -105,12 +105,12 @@
 /** Constructor.
  */
 void ec_fsm_soe_init(
-        ec_fsm_soe_t *fsm, /**< finite state machine */
-        ec_datagram_t *datagram /**< datagram */
+        ec_fsm_soe_t *fsm /**< finite state machine */
         )
 {
     fsm->state = NULL;
-    fsm->datagram = datagram;
+    fsm->datagram = NULL;
+    fsm->fragment_size = 0;
 }
 
 /*****************************************************************************/
@@ -135,6 +135,7 @@
 {
     fsm->slave = slave;
     fsm->request = request;
+
     if (request->dir == EC_DIR_OUTPUT) {
         fsm->state = ec_fsm_soe_write_start;
     } else {
@@ -144,26 +145,46 @@
 
 /*****************************************************************************/
 
-/**
-   Executes the current state of the state machine.
-   \return false, if state machine has terminated
-*/
-
-int ec_fsm_soe_exec(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
-    fsm->state(fsm);
-
-    return fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error;
-}
-
-/*****************************************************************************/
-
-/**
-   Returns, if the state machine terminated with success.
-   \return non-zero if successful.
-*/
-
-int ec_fsm_soe_success(ec_fsm_soe_t *fsm /**< Finite state machine */)
+/** Executes the current state of the state machine.
+ *
+ * \return 1 if the datagram was used, else 0.
+ */
+int ec_fsm_soe_exec(
+        ec_fsm_soe_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    int datagram_used = 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;
+    }
+
+    fsm->state(fsm, datagram);
+
+    datagram_used =
+        fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error;
+
+    if (datagram_used) {
+        fsm->datagram = datagram;
+    } else {
+        fsm->datagram = NULL;
+    }
+
+    return datagram_used;
+}
+
+/*****************************************************************************/
+
+/** Returns, if the state machine terminated with success.
+ *
+ * \return non-zero if successful.
+ */
+int ec_fsm_soe_success(const ec_fsm_soe_t *fsm /**< Finite state machine */)
 {
     return fsm->state == ec_fsm_soe_end;
 }
@@ -191,32 +212,24 @@
  * SoE read state machine
  *****************************************************************************/
 
-/** SoE state: READ START.
- */
-void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
+/** Prepare a read operation.
+ *
+ * \return 0 on success, otherwise a negative error code.
+ */
+int ec_fsm_soe_prepare_read(
+        ec_fsm_soe_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    uint8_t *data;
     ec_slave_t *slave = fsm->slave;
     ec_master_t *master = slave->master;
     ec_soe_request_t *request = fsm->request;
-    uint8_t *data;
-
-    EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X of drive %u.\n", request->idn,
-            request->drive_no);
-
-    if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
-        EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
-        fsm->state = ec_fsm_soe_error;
-        ec_fsm_soe_print_error(fsm);
-        return;
-    }
 
     data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
             EC_SOE_SIZE);
     if (IS_ERR(data)) {
-        fsm->state = ec_fsm_soe_error;
-        ec_fsm_soe_print_error(fsm);
-        return;
+        return PTR_ERR(data);
     }
 
     EC_WRITE_U8(data, OPCODE_READ_REQUEST | (request->drive_no & 0x07) << 5);
@@ -228,51 +241,93 @@
         ec_print_data(data, EC_SOE_SIZE);
     }
 
-    fsm->request->data_size = 0;
     fsm->request->jiffies_sent = jiffies;
+    fsm->state = ec_fsm_soe_read_request;
+
+    return 0;
+}
+
+/*****************************************************************************/
+
+/** SoE state: READ START.
+ */
+void ec_fsm_soe_read_start(
+        ec_fsm_soe_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    ec_soe_request_t *request = fsm->request;
+
+    EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X of drive %u.\n", request->idn,
+            request->drive_no);
+
+    if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
+        EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
+        fsm->state = ec_fsm_soe_error;
+        ec_fsm_soe_print_error(fsm);
+        return;
+    }
+
+    request->data_size = 0;
     fsm->retries = EC_FSM_RETRIES;
-    fsm->state = ec_fsm_soe_read_request;
+
+    if (ec_fsm_soe_prepare_read(fsm, datagram)) {
+        fsm->state = ec_fsm_soe_error;
+        ec_fsm_soe_print_error(fsm);
+    }
 }
 
 /*****************************************************************************/
 
 /** SoE state: READ REQUEST.
  */
-void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_soe_read_request(
+        ec_fsm_soe_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
     unsigned long diff_ms;
 
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return; // FIXME: check for response first?
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        if (ec_fsm_soe_prepare_read(fsm, datagram)) {
+            fsm->state = ec_fsm_soe_error;
+            ec_fsm_soe_print_error(fsm);
+        }
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_soe_error;
         EC_SLAVE_ERR(slave, "Failed to receive SoE read request: ");
-        ec_datagram_print_state(datagram);
+        ec_datagram_print_state(fsm->datagram);
         ec_fsm_soe_print_error(fsm);
         return;
     }
 
     diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
 
-    if (datagram->working_counter != 1) {
-        if (!datagram->working_counter) {
+    if (fsm->datagram->working_counter != 1) {
+        if (!fsm->datagram->working_counter) {
             if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {
                 // no response; send request datagram again
+                if (ec_fsm_soe_prepare_read(fsm, datagram)) {
+                    fsm->state = ec_fsm_soe_error;
+                    ec_fsm_soe_print_error(fsm);
+                }
                 return;
             }
         }
         fsm->state = ec_fsm_soe_error;
         EC_SLAVE_ERR(slave, "Reception of SoE read request"
                 " failed after %lu ms: ", diff_ms);
-        ec_datagram_print_wc_error(datagram);
-        ec_fsm_soe_print_error(fsm);
-        return;
-    }
-
-    fsm->jiffies_start = datagram->jiffies_sent;
+        ec_datagram_print_wc_error(fsm->datagram);
+        ec_fsm_soe_print_error(fsm);
+        return;
+    }
+
+    fsm->jiffies_start = fsm->datagram->jiffies_sent;
     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
     fsm->retries = EC_FSM_RETRIES;
     fsm->state = ec_fsm_soe_read_check;
@@ -282,34 +337,39 @@
 
 /** CoE state: READ CHECK.
  */
-void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
-    ec_slave_t *slave = fsm->slave;
-
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return;
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_soe_read_check(
+        ec_fsm_soe_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_soe_error;
         EC_SLAVE_ERR(slave, "Failed to receive SoE mailbox check datagram: ");
-        ec_datagram_print_state(datagram);
-        ec_fsm_soe_print_error(fsm);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        ec_fsm_soe_print_error(fsm);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         fsm->state = ec_fsm_soe_error;
         EC_SLAVE_ERR(slave, "Reception of SoE mailbox check"
                 " datagram failed: ");
-        ec_datagram_print_wc_error(datagram);
-        ec_fsm_soe_print_error(fsm);
-        return;
-    }
-
-    if (!ec_slave_mbox_check(datagram)) {
+        ec_datagram_print_wc_error(fsm->datagram);
+        ec_fsm_soe_print_error(fsm);
+        return;
+    }
+
+    if (!ec_slave_mbox_check(fsm->datagram)) {
         unsigned long diff_ms =
-            (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+            (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+            1000 / HZ;
         if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
             fsm->state = ec_fsm_soe_error;
             EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
@@ -333,9 +393,11 @@
 
 /** SoE state: READ RESPONSE.
  */
-void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_soe_read_response(
+        ec_fsm_soe_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
     ec_master_t *master = slave->master;
     uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag,
@@ -343,26 +405,28 @@
     size_t rec_size, data_size;
     ec_soe_request_t *req = fsm->request;
 
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return; // FIXME: request again?
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_soe_error;
         EC_SLAVE_ERR(slave, "Failed to receive SoE read response datagram: ");
-        ec_datagram_print_state(datagram);
-        ec_fsm_soe_print_error(fsm);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        ec_fsm_soe_print_error(fsm);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         fsm->state = ec_fsm_soe_error;
         EC_SLAVE_ERR(slave, "Reception of SoE read response failed: ");
-        ec_datagram_print_wc_error(datagram);
-        ec_fsm_soe_print_error(fsm);
-        return;
-    }
-
-    data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+        ec_datagram_print_wc_error(fsm->datagram);
+        ec_fsm_soe_print_error(fsm);
+        return;
+    }
+
+    data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
     if (IS_ERR(data)) {
         fsm->state = ec_fsm_soe_error;
         ec_fsm_soe_print_error(fsm);
@@ -435,7 +499,7 @@
     if (incomplete) {
         EC_SLAVE_DBG(slave, 1, "SoE data incomplete. Waiting for fragment"
                 " at offset %zu.\n", req->data_size);
-        fsm->jiffies_start = datagram->jiffies_sent;
+        fsm->jiffies_start = fsm->datagram->jiffies_sent;
         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
         fsm->retries = EC_FSM_RETRIES;
         fsm->state = ec_fsm_soe_read_check;
@@ -456,15 +520,15 @@
 /** Write next fragment.
  */
 void ec_fsm_soe_write_next_fragment(
-        ec_fsm_soe_t *fsm /**< finite state machine */
-        )
-{
-    ec_datagram_t *datagram = fsm->datagram;
+        ec_fsm_soe_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
     ec_master_t *master = slave->master;
     ec_soe_request_t *req = fsm->request;
     uint8_t incomplete, *data;
-    size_t header_size, max_fragment_size, remaining_size, fragment_size;
+    size_t header_size, max_fragment_size, remaining_size;
     uint16_t fragments_left;
 
     header_size = EC_MBOX_HEADER_SIZE + EC_SOE_SIZE;
@@ -479,14 +543,14 @@
     remaining_size = req->data_size - fsm->offset;
     max_fragment_size = slave->configured_rx_mailbox_size - header_size;
     incomplete = remaining_size > max_fragment_size;
-    fragment_size = incomplete ? max_fragment_size : remaining_size;
-    fragments_left = remaining_size / fragment_size - 1;
-    if (remaining_size % fragment_size) {
+    fsm->fragment_size = incomplete ? max_fragment_size : remaining_size;
+    fragments_left = remaining_size / fsm->fragment_size - 1;
+    if (remaining_size % fsm->fragment_size) {
         fragments_left++;
     }
 
     data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
-            EC_SOE_SIZE + fragment_size);
+            EC_SOE_SIZE + fsm->fragment_size);
     if (IS_ERR(data)) {
         fsm->state = ec_fsm_soe_error;
         ec_fsm_soe_print_error(fsm);
@@ -497,16 +561,14 @@
             (req->drive_no & 0x07) << 5);
     EC_WRITE_U8(data + 1, 1 << 6); // only value included
     EC_WRITE_U16(data + 2, incomplete ? fragments_left : req->idn);
-    memcpy(data + 4, req->data + fsm->offset, fragment_size);
-    fsm->offset += fragment_size;
+    memcpy(data + 4, req->data + fsm->offset, fsm->fragment_size);
 
     if (master->debug_level) {
         EC_SLAVE_DBG(slave, 0, "SCC write request:\n");
-        ec_print_data(data, EC_SOE_SIZE + fragment_size);
+        ec_print_data(data, EC_SOE_SIZE + fsm->fragment_size);
     }
 
     req->jiffies_sent = jiffies;
-    fsm->retries = EC_FSM_RETRIES;
     fsm->state = ec_fsm_soe_write_request;
 }
 
@@ -514,7 +576,10 @@
 
 /** SoE state: WRITE START.
  */
-void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */)
+void ec_fsm_soe_write_start(
+        ec_fsm_soe_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
 {
     ec_slave_t *slave = fsm->slave;
     ec_soe_request_t *req = fsm->request;
@@ -530,48 +595,54 @@
     }
 
     fsm->offset = 0;
-    ec_fsm_soe_write_next_fragment(fsm);
+    fsm->retries = EC_FSM_RETRIES;
+    ec_fsm_soe_write_next_fragment(fsm, datagram);
 }
 
 /*****************************************************************************/
 
 /** SoE state: WRITE REQUEST.
  */
-void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_soe_write_request(
+        ec_fsm_soe_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
     unsigned long diff_ms;
 
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return; // FIXME: check for response first?
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_fsm_soe_write_next_fragment(fsm, datagram);
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_soe_error;
         EC_SLAVE_ERR(slave, "Failed to receive SoE write request: ");
-        ec_datagram_print_state(datagram);
+        ec_datagram_print_state(fsm->datagram);
         ec_fsm_soe_print_error(fsm);
         return;
     }
 
     diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
 
-    if (datagram->working_counter != 1) {
-        if (!datagram->working_counter) {
+    if (fsm->datagram->working_counter != 1) {
+        if (!fsm->datagram->working_counter) {
             if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {
                 // no response; send request datagram again
+                ec_fsm_soe_write_next_fragment(fsm, datagram);
                 return;
             }
         }
         fsm->state = ec_fsm_soe_error;
         EC_SLAVE_ERR(slave, "Reception of SoE write request"
                 " failed after %lu ms: ", diff_ms);
-        ec_datagram_print_wc_error(datagram);
-        ec_fsm_soe_print_error(fsm);
-        return;
-    }
-
-    fsm->jiffies_start = datagram->jiffies_sent;
+        ec_datagram_print_wc_error(fsm->datagram);
+        ec_fsm_soe_print_error(fsm);
+        return;
+    }
+
+    fsm->jiffies_start = fsm->datagram->jiffies_sent;
 
     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
     fsm->retries = EC_FSM_RETRIES;
@@ -582,64 +653,65 @@
 
 /** CoE state: WRITE CHECK.
  */
-void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
-    ec_slave_t *slave = fsm->slave;
-    ec_soe_request_t *req = fsm->request;
-
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return;
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+void ec_fsm_soe_write_check(
+        ec_fsm_soe_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_soe_error;
         EC_SLAVE_ERR(slave, "Failed to receive SoE write request datagram: ");
-        ec_datagram_print_state(datagram);
-        ec_fsm_soe_print_error(fsm);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        ec_fsm_soe_print_error(fsm);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         fsm->state = ec_fsm_soe_error;
         EC_SLAVE_ERR(slave, "Reception of SoE write request datagram: ");
-        ec_datagram_print_wc_error(datagram);
-        ec_fsm_soe_print_error(fsm);
-        return;
-    }
-
-    if (fsm->offset < req->data_size) {
-        ec_fsm_soe_write_next_fragment(fsm);
-    } else {
-        if (!ec_slave_mbox_check(datagram)) {
-            unsigned long diff_ms =
-                (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
-            if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
-                fsm->state = ec_fsm_soe_error;
-                EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting"
-                        " for write response.\n", diff_ms);
-                ec_fsm_soe_print_error(fsm);
-                return;
-            }
-
-            ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
-            fsm->retries = EC_FSM_RETRIES;
+        ec_datagram_print_wc_error(fsm->datagram);
+        ec_fsm_soe_print_error(fsm);
+        return;
+    }
+
+    if (!ec_slave_mbox_check(fsm->datagram)) {
+        unsigned long diff_ms =
+            (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+        if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
+            fsm->state = ec_fsm_soe_error;
+            EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting"
+                    " for write response.\n", diff_ms);
+            ec_fsm_soe_print_error(fsm);
             return;
         }
 
-        // Fetch response
-        ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
         fsm->retries = EC_FSM_RETRIES;
-        fsm->state = ec_fsm_soe_write_response;
-    }
+        return;
+    }
+
+    // Fetch response
+    ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+    fsm->retries = EC_FSM_RETRIES;
+    fsm->state = ec_fsm_soe_write_response;
 }
 
 /*****************************************************************************/
 
 /** SoE state: WRITE RESPONSE.
  */
-void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
-    ec_datagram_t *datagram = fsm->datagram;
+void ec_fsm_soe_write_response(
+        ec_fsm_soe_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
     ec_slave_t *slave = fsm->slave;
     ec_master_t *master = slave->master;
     ec_soe_request_t *req = fsm->request;
@@ -647,27 +719,29 @@
     uint16_t idn;
     size_t rec_size;
 
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
         return; // FIXME: request again?
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_soe_error;
         EC_SLAVE_ERR(slave, "Failed to receive SoE write"
                 " response datagram: ");
-        ec_datagram_print_state(datagram);
-        ec_fsm_soe_print_error(fsm);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
+        ec_datagram_print_state(fsm->datagram);
+        ec_fsm_soe_print_error(fsm);
+        return;
+    }
+
+    if (fsm->datagram->working_counter != 1) {
         fsm->state = ec_fsm_soe_error;
         EC_SLAVE_ERR(slave, "Reception of SoE write response failed: ");
-        ec_datagram_print_wc_error(datagram);
-        ec_fsm_soe_print_error(fsm);
-        return;
-    }
-
-    data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+        ec_datagram_print_wc_error(fsm->datagram);
+        ec_fsm_soe_print_error(fsm);
+        return;
+    }
+
+    data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
     if (IS_ERR(data)) {
         fsm->state = ec_fsm_soe_error;
         ec_fsm_soe_print_error(fsm);
@@ -689,7 +763,7 @@
 
     if (rec_size < EC_SOE_SIZE) {
         fsm->state = ec_fsm_soe_error;
-        EC_SLAVE_ERR(slave, "Received currupted SoE write response"
+        EC_SLAVE_ERR(slave, "Received corrupted SoE write response"
                 " (%zu bytes)!\n", rec_size);
         ec_print_data(data, rec_size);
         ec_fsm_soe_print_error(fsm);
@@ -735,14 +809,24 @@
         req->error_code = 0x0000;
     }
 
-    fsm->state = ec_fsm_soe_end; // success
+    fsm->offset += fsm->fragment_size;
+
+    if (fsm->offset < req->data_size) {
+        fsm->retries = EC_FSM_RETRIES;
+        ec_fsm_soe_write_next_fragment(fsm, datagram);
+    } else {
+        fsm->state = ec_fsm_soe_end; // success
+    }
 }
 
 /*****************************************************************************/
 
 /** State: ERROR.
  */
-void ec_fsm_soe_error(ec_fsm_soe_t *fsm /**< finite state machine */)
+void ec_fsm_soe_error(
+        ec_fsm_soe_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
 {
 }
 
@@ -750,8 +834,11 @@
 
 /** State: END.
  */
-void ec_fsm_soe_end(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
-}
-
-/*****************************************************************************/
+void ec_fsm_soe_end(
+        ec_fsm_soe_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+}
+
+/*****************************************************************************/
diff -r 505cf41488a4 -r 9cdd7669dc0b master/fsm_soe.h
--- a/master/fsm_soe.h	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_soe.h	Thu Jan 10 17:36:41 2013 +0100
@@ -50,24 +50,25 @@
  */
 struct ec_fsm_soe {
     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_soe_t *); /**< CoE state function */
-    unsigned long jiffies_start; /**< CoE timestamp. */
+    void (*state)(ec_fsm_soe_t *, ec_datagram_t *); /**< CoE state function */
+    ec_datagram_t *datagram; /**< Datagram used in the previous step. */
+    unsigned long jiffies_start; /**< Timestamp. */
     ec_soe_request_t *request; /**< SoE request */
     off_t offset; /**< IDN data offset during fragmented write. */
+    size_t fragment_size; /**< Size of the current fragment. */
 };
 
 /*****************************************************************************/
 
-void ec_fsm_soe_init(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_init(ec_fsm_soe_t *);
 void ec_fsm_soe_clear(ec_fsm_soe_t *);
 
 void ec_fsm_soe_transfer(ec_fsm_soe_t *, ec_slave_t *, ec_soe_request_t *);
 
-int ec_fsm_soe_exec(ec_fsm_soe_t *);
-int ec_fsm_soe_success(ec_fsm_soe_t *);
+int ec_fsm_soe_exec(ec_fsm_soe_t *, ec_datagram_t *);
+int ec_fsm_soe_success(const ec_fsm_soe_t *);
 
 /*****************************************************************************/
 
diff -r 505cf41488a4 -r 9cdd7669dc0b master/ioctl.c
--- a/master/ioctl.c	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/ioctl.c	Thu Jan 10 17:36:41 2013 +0100
@@ -1753,12 +1753,11 @@
         ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
         )
 {
-    if (unlikely(!ctx->requested))
-        return -EPERM;
-
-    down(&master->io_sem);
+    if (unlikely(!ctx->requested)) {
+        return -EPERM;
+    }
+
     ecrt_master_send(master);
-    up(&master->io_sem);
     return 0;
 }
 
@@ -1772,12 +1771,11 @@
         ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
         )
 {
-    if (unlikely(!ctx->requested))
-        return -EPERM;
-
-    down(&master->io_sem);
+    if (unlikely(!ctx->requested)) {
+        return -EPERM;
+    }
+
     ecrt_master_receive(master);
-    up(&master->io_sem);
     return 0;
 }
 
@@ -1864,12 +1862,11 @@
         ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
         )
 {
-    if (unlikely(!ctx->requested))
-        return -EPERM;
-
-    down(&master->io_sem);
+    if (unlikely(!ctx->requested)) {
+        return -EPERM;
+    }
+
     ecrt_master_sync_reference_clock(master);
-    up(&master->io_sem);
     return 0;
 }
 
@@ -1883,12 +1880,11 @@
         ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
         )
 {
-    if (unlikely(!ctx->requested))
-        return -EPERM;
-
-    down(&master->io_sem);
+    if (unlikely(!ctx->requested)) {
+        return -EPERM;
+    }
+
     ecrt_master_sync_slave_clocks(master);
-    up(&master->io_sem);
     return 0;
 }
 
@@ -1931,12 +1927,11 @@
         ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
         )
 {
-    if (unlikely(!ctx->requested))
-        return -EPERM;
-
-    down(&master->io_sem);
+    if (unlikely(!ctx->requested)) {
+        return -EPERM;
+    }
+
     ecrt_master_sync_monitor_queue(master);
-    up(&master->io_sem);
     return 0;
 }
 
diff -r 505cf41488a4 -r 9cdd7669dc0b master/master.c
--- a/master/master.c	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/master.c	Thu Jan 10 17:36:41 2013 +0100
@@ -136,7 +136,7 @@
         )
 {
     int ret;
-    unsigned int dev_idx;
+    unsigned int dev_idx, i;
 
     master->index = index;
     master->reserved = 0;
@@ -193,11 +193,23 @@
     INIT_LIST_HEAD(&master->ext_datagram_queue);
     sema_init(&master->ext_queue_sem, 1);
 
-    INIT_LIST_HEAD(&master->external_datagram_queue);
+    master->ext_ring_idx_rt = 0;
+    master->ext_ring_idx_fsm = 0;
+
+    // init external datagram ring
+    for (i = 0; i < EC_EXT_RING_SIZE; i++) {
+        ec_datagram_t *datagram = &master->ext_datagram_ring[i];
+        ec_datagram_init(datagram);
+        snprintf(datagram->name, EC_DATAGRAM_NAME_SIZE, "ext-%u", i);
+    }
 
     // send interval in IDLE phase
     ec_master_set_send_interval(master, 1000000 / HZ);
 
+    master->fsm_slave = NULL;
+    INIT_LIST_HEAD(&master->fsm_exec_list);
+    master->fsm_exec_count = 0U;
+
     master->debug_level = debug_level;
     master->stats.timeouts = 0;
     master->stats.corrupted = 0;
@@ -245,6 +257,17 @@
     // create state machine object
     ec_fsm_master_init(&master->fsm, master, &master->fsm_datagram);
 
+    // alloc external datagram ring
+    for (i = 0; i < EC_EXT_RING_SIZE; i++) {
+        ec_datagram_t *datagram = &master->ext_datagram_ring[i];
+        ret = ec_datagram_prealloc(datagram, EC_MAX_DATA_SIZE);
+        if (ret) {
+            EC_MASTER_ERR(master, "Failed to allocate external"
+                    " datagram %u.\n", i);
+            goto out_clear_ext_datagrams;
+        }
+    }
+
     // init reference sync datagram
     ec_datagram_init(&master->ref_sync_datagram);
     snprintf(master->ref_sync_datagram.name, EC_DATAGRAM_NAME_SIZE,
@@ -254,7 +277,7 @@
         ec_datagram_clear(&master->ref_sync_datagram);
         EC_MASTER_ERR(master, "Failed to allocate reference"
                 " synchronisation datagram.\n");
-        goto out_clear_fsm;
+        goto out_clear_ext_datagrams;
     }
 
     // init sync datagram
@@ -337,7 +360,10 @@
     ec_datagram_clear(&master->sync_datagram);
 out_clear_ref_sync:
     ec_datagram_clear(&master->ref_sync_datagram);
-out_clear_fsm:
+out_clear_ext_datagrams:
+    for (i = 0; i < EC_EXT_RING_SIZE; i++) {
+        ec_datagram_clear(&master->ext_datagram_ring[i]);
+    }
     ec_fsm_master_clear(&master->fsm);
     ec_datagram_clear(&master->fsm_datagram);
 out_clear_devices:
@@ -355,7 +381,7 @@
         ec_master_t *master /**< EtherCAT master */
         )
 {
-    unsigned int dev_idx;
+    unsigned int dev_idx, i;
 
 #ifdef EC_RTDM
     ec_rtdm_dev_clear(&master->rtdm_dev);
@@ -379,6 +405,11 @@
     ec_datagram_clear(&master->sync_mon_datagram);
     ec_datagram_clear(&master->sync_datagram);
     ec_datagram_clear(&master->ref_sync_datagram);
+
+    for (i = 0; i < EC_EXT_RING_SIZE; i++) {
+        ec_datagram_clear(&master->ext_datagram_ring[i]);
+    }
+
     ec_fsm_master_clear(&master->fsm);
     ec_datagram_clear(&master->fsm_datagram);
 
@@ -446,8 +477,12 @@
         EC_MASTER_WARN(master, "Discarding SII request, slave %u about"
                 " to be deleted.\n", request->slave->ring_position);
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&master->request_queue);
-    }
+        wake_up_all(&master->request_queue);
+    }
+
+    master->fsm_slave = NULL;
+    INIT_LIST_HEAD(&master->fsm_exec_list);
+    master->fsm_exec_count = 0;
 
     for (slave = master->slaves;
             slave < master->slaves + master->slave_count;
@@ -743,75 +778,104 @@
         ec_master_t *master /**< EtherCAT master */
         )
 {
-    ec_datagram_t *datagram, *n;
-    size_t queue_size = 0;
+    ec_datagram_t *datagram;
+    size_t queue_size = 0, new_queue_size = 0;
+#if DEBUG_INJECT
+    unsigned int datagram_count = 0;
+#endif
+
+    if (master->ext_ring_idx_rt == master->ext_ring_idx_fsm) {
+        // nothing to inject
+        return;
+    }
 
     list_for_each_entry(datagram, &master->datagram_queue, queue) {
-        queue_size += datagram->data_size;
-    }
-
-    list_for_each_entry_safe(datagram, n, &master->external_datagram_queue,
-            queue) {
-        queue_size += datagram->data_size;
-        if (queue_size <= master->max_queue_size) {
-            list_del_init(&datagram->queue);
+        if (datagram->state == EC_DATAGRAM_QUEUED) {
+            queue_size += datagram->data_size;
+        }
+    }
+
 #if DEBUG_INJECT
-            EC_MASTER_DBG(master, 0, "Injecting external datagram %08x"
-                    " size=%u, queue_size=%u\n", (unsigned int) datagram,
-                    datagram->data_size, queue_size);
+    EC_MASTER_DBG(master, 1, "Injecting datagrams, queue_size=%zu\n",
+            queue_size);
+#endif
+
+    while (master->ext_ring_idx_rt != master->ext_ring_idx_fsm) {
+        datagram = &master->ext_datagram_ring[master->ext_ring_idx_rt];
+
+        if (datagram->state != EC_DATAGRAM_INIT) {
+            // skip datagram
+            master->ext_ring_idx_rt =
+                (master->ext_ring_idx_rt + 1) % EC_EXT_RING_SIZE;
+            continue;
+        }
+
+        new_queue_size = queue_size + datagram->data_size;
+        if (new_queue_size <= master->max_queue_size) {
+#if DEBUG_INJECT
+            EC_MASTER_DBG(master, 1, "Injecting datagram %s"
+                    " size=%zu, queue_size=%zu\n", datagram->name,
+                    datagram->data_size, new_queue_size);
+            datagram_count++;
 #endif
 #ifdef EC_HAVE_CYCLES
             datagram->cycles_sent = 0;
 #endif
             datagram->jiffies_sent = 0;
             ec_master_queue_datagram(master, datagram);
-        } else {
-            if (datagram->data_size > master->max_queue_size) {
-                list_del_init(&datagram->queue);
+            queue_size = new_queue_size;
+        }
+        else if (datagram->data_size > master->max_queue_size) {
+            datagram->state = EC_DATAGRAM_ERROR;
+            EC_MASTER_ERR(master, "External datagram %s is too large,"
+                    " size=%zu, max_queue_size=%zu\n",
+                    datagram->name, datagram->data_size,
+                    master->max_queue_size);
+        }
+        else { // datagram does not fit in the current cycle
+#ifdef EC_HAVE_CYCLES
+            cycles_t cycles_now = get_cycles();
+
+            if (cycles_now - datagram->cycles_sent
+                    > ext_injection_timeout_cycles)
+#else
+            if (jiffies - datagram->jiffies_sent
+                    > ext_injection_timeout_jiffies)
+#endif
+            {
+                unsigned int time_us;
+
                 datagram->state = EC_DATAGRAM_ERROR;
-                EC_MASTER_ERR(master, "External datagram %p is too large,"
-                        " size=%zu, max_queue_size=%zu\n",
-                        datagram, datagram->data_size,
-                        master->max_queue_size);
-            } else {
 #ifdef EC_HAVE_CYCLES
-                cycles_t cycles_now = get_cycles();
-
-                if (cycles_now - datagram->cycles_sent
-                        > ext_injection_timeout_cycles)
+                time_us = (unsigned int)
+                    ((cycles_now - datagram->cycles_sent) * 1000LL)
+                    / cpu_khz;
 #else
-                if (jiffies - datagram->jiffies_sent
-                        > ext_injection_timeout_jiffies)
-#endif
-                {
-                    unsigned int time_us;
-
-                    list_del_init(&datagram->queue);
-                    datagram->state = EC_DATAGRAM_ERROR;
-#ifdef EC_HAVE_CYCLES
-                    time_us = (unsigned int)
-                        ((cycles_now - datagram->cycles_sent) * 1000LL)
-                        / cpu_khz;
-#else
-                    time_us = (unsigned int)
-                        ((jiffies - datagram->jiffies_sent) * 1000000 / HZ);
-#endif
-                    EC_MASTER_ERR(master, "Timeout %u us: Injecting"
-                            " external datagram %p size=%zu,"
-                            " max_queue_size=%zu\n", time_us, datagram,
-                            datagram->data_size, master->max_queue_size);
-                }
+                time_us = (unsigned int)
+                    ((jiffies - datagram->jiffies_sent) * 1000000 / HZ);
+#endif
+                EC_MASTER_ERR(master, "Timeout %u us: Injecting"
+                        " external datagram %s size=%zu,"
+                        " max_queue_size=%zu\n", time_us, datagram->name,
+                        datagram->data_size, master->max_queue_size);
+            }
+            else {
 #if DEBUG_INJECT
-                else {
-                    EC_MASTER_DBG(master, 0, "Deferred injecting"
-                            " of external datagram %p"
-                            " size=%u, queue_size=%u\n",
-                            datagram, datagram->data_size, queue_size);
-                }
-#endif
+                EC_MASTER_DBG(master, 1, "Deferred injecting"
+                        " external datagram %s size=%u, queue_size=%u\n",
+                        datagram->name, datagram->data_size, queue_size);
+#endif
+                break;
             }
         }
-    }
+
+        master->ext_ring_idx_rt =
+            (master->ext_ring_idx_rt + 1) % EC_EXT_RING_SIZE;
+    }
+
+#if DEBUG_INJECT
+    EC_MASTER_DBG(master, 1, "Injected %u datagrams.\n", datagram_count);
+#endif
 }
 
 /*****************************************************************************/
@@ -832,42 +896,21 @@
 
 /*****************************************************************************/
 
-/** Places an external datagram in the sdo datagram queue.
- */
-void ec_master_queue_external_datagram(
-        ec_master_t *master, /**< EtherCAT master */
-        ec_datagram_t *datagram /**< datagram */
-        )
-{
-    ec_datagram_t *queued_datagram;
-
-    down(&master->io_sem);
-
-    // check, if the datagram is already queued
-    list_for_each_entry(queued_datagram, &master->external_datagram_queue,
-            queue) {
-        if (queued_datagram == datagram) {
-            up(&master->io_sem);
-            datagram->state = EC_DATAGRAM_QUEUED;
-            return;
-        }
-    }
-
-#if DEBUG_INJECT
-    EC_MASTER_DBG(master, 0, "Requesting external datagram %p size=%u\n",
-            datagram, datagram->data_size);
-#endif
-
-    list_add_tail(&datagram->queue, &master->external_datagram_queue);
-    datagram->state = EC_DATAGRAM_QUEUED;
-#ifdef EC_HAVE_CYCLES
-    datagram->cycles_sent = get_cycles();
-#endif
-    datagram->jiffies_sent = jiffies;
-
-    up(&master->io_sem);
-
-    master->fsm.idle = 0;
+/** Searches for a free datagram in the external datagram ring.
+ */
+ec_datagram_t *ec_master_get_external_datagram(
+        ec_master_t *master /**< EtherCAT master */
+        )
+{
+    if ((master->ext_ring_idx_fsm + 1) % EC_EXT_RING_SIZE !=
+            master->ext_ring_idx_rt) {
+        ec_datagram_t *datagram =
+            &master->ext_datagram_ring[master->ext_ring_idx_fsm];
+        return datagram;
+    }
+    else {
+        return NULL;
+    }
 }
 
 /*****************************************************************************/
@@ -1366,12 +1409,101 @@
 
 /*****************************************************************************/
 
+/** Execute slave FSMs.
+ */
+void ec_master_exec_slave_fsms(
+        ec_master_t *master /**< EtherCAT master. */
+        )
+{
+    ec_datagram_t *datagram;
+    ec_fsm_slave_t *fsm, *next;
+    unsigned int count = 0;
+
+    list_for_each_entry_safe(fsm, next, &master->fsm_exec_list, list) {
+        if (!fsm->datagram) {
+            EC_MASTER_WARN(master, "Slave %u FSM has zero datagram."
+                    "This is a bug!\n", fsm->slave->ring_position);
+            list_del_init(&fsm->list);
+            master->fsm_exec_count--;
+            return;
+        }
+
+        if (fsm->datagram->state == EC_DATAGRAM_INIT ||
+                fsm->datagram->state == EC_DATAGRAM_QUEUED ||
+                fsm->datagram->state == EC_DATAGRAM_SENT) {
+            // previous datagram was not sent or received yet.
+            // wait until next thread execution
+            return;
+        }
+
+        datagram = ec_master_get_external_datagram(master);
+        if (!datagram) {
+            // no free datagrams at the moment
+            EC_MASTER_WARN(master, "No free datagram during"
+                    " slave FSM execution. This is a bug!\n");
+            continue;
+        }
+
+#if DEBUG_INJECT
+        EC_MASTER_DBG(master, 1, "Executing slave %u FSM.\n",
+                fsm->slave->ring_position);
+#endif
+        if (ec_fsm_slave_exec(fsm, datagram)) {
+            // FSM consumed datagram
+#if DEBUG_INJECT
+            EC_MASTER_DBG(master, 1, "FSM consumed datagram %s\n",
+                    datagram->name);
+#endif
+            master->ext_ring_idx_fsm =
+                (master->ext_ring_idx_fsm + 1) % EC_EXT_RING_SIZE;
+        }
+        else {
+            // FSM finished
+            list_del_init(&fsm->list);
+            master->fsm_exec_count--;
+#if DEBUG_INJECT
+            EC_MASTER_DBG(master, 1, "FSM finished. %u remaining.\n",
+                    master->fsm_exec_count);
+#endif
+        }
+    }
+
+    while (master->fsm_exec_count < EC_EXT_RING_SIZE / 2
+            && count < master->slave_count) {
+
+        if (ec_fsm_slave_is_ready(&master->fsm_slave->fsm)) {
+            datagram = ec_master_get_external_datagram(master);
+
+            if (ec_fsm_slave_exec(&master->fsm_slave->fsm, datagram)) {
+                master->ext_ring_idx_fsm =
+                    (master->ext_ring_idx_fsm + 1) % EC_EXT_RING_SIZE;
+                list_add_tail(&master->fsm_slave->fsm.list,
+                        &master->fsm_exec_list);
+                master->fsm_exec_count++;
+#if DEBUG_INJECT
+                EC_MASTER_DBG(master, 1, "New slave %u FSM"
+                        " consumed datagram %s, now %u FSMs in list.\n",
+                        master->fsm_slave->ring_position, datagram->name,
+                        master->fsm_exec_count);
+#endif
+            }
+        }
+
+        master->fsm_slave++;
+        if (master->fsm_slave >= master->slaves + master->slave_count) {
+            master->fsm_slave = master->slaves;
+        }
+        count++;
+    }
+}
+
+/*****************************************************************************/
+
 /** Master kernel thread function for IDLE phase.
  */
 static int ec_master_idle_thread(void *priv_data)
 {
     ec_master_t *master = (ec_master_t *) priv_data;
-    ec_slave_t *slave = NULL;
     int fsm_exec;
 #ifdef EC_USE_HRTIMER
     size_t sent_bytes;
@@ -1392,8 +1524,6 @@
         ecrt_master_receive(master);
         up(&master->io_sem);
 
-        fsm_exec = 0;
-
         // execute master & slave state machines
         if (down_interruptible(&master->master_sem)) {
             break;
@@ -1401,11 +1531,7 @@
 
         fsm_exec = ec_fsm_master_exec(&master->fsm);
 
-        for (slave = master->slaves;
-                slave < master->slaves + master->slave_count;
-                slave++) {
-            ec_fsm_slave_exec(&slave->fsm);
-        }
+        ec_master_exec_slave_fsms(master);
 
         up(&master->master_sem);
 
@@ -1449,8 +1575,6 @@
 static int ec_master_operation_thread(void *priv_data)
 {
     ec_master_t *master = (ec_master_t *) priv_data;
-    ec_slave_t *slave = NULL;
-    int fsm_exec;
 
     EC_MASTER_DBG(master, 1, "Operation thread running"
             " with fsm interval = %u us, max data size=%zu\n",
@@ -1463,28 +1587,20 @@
             // output statistics
             ec_master_output_stats(master);
 
-            fsm_exec = 0;
-
             // execute master & slave state machines
             if (down_interruptible(&master->master_sem)) {
                 break;
             }
 
-            fsm_exec += ec_fsm_master_exec(&master->fsm);
-
-            for (slave = master->slaves;
-                    slave < master->slaves + master->slave_count;
-                    slave++) {
-                ec_fsm_slave_exec(&slave->fsm);
-            }
-
-            up(&master->master_sem);
-
-            // Inject datagrams (let the RT thread queue them, see
-            // ecrt_master_send())
-            if (fsm_exec) {
+            if (ec_fsm_master_exec(&master->fsm)) {
+                // Inject datagrams (let the RT thread queue them, see
+                // ecrt_master_send())
                 master->injection_seq_fsm++;
             }
+
+            ec_master_exec_slave_fsms(master);
+
+            up(&master->master_sem);
         }
 
 #ifdef EC_USE_HRTIMER
@@ -2282,7 +2398,7 @@
     ec_device_index_t dev_idx;
 
     if (master->injection_seq_rt != master->injection_seq_fsm) {
-        // inject datagrams produced by master FSM
+        // inject datagram produced by master FSM
         ec_master_queue_datagram(master, &master->fsm_datagram);
         master->injection_seq_rt = master->injection_seq_fsm;
     }
diff -r 505cf41488a4 -r 9cdd7669dc0b master/master.h
--- a/master/master.h	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/master.h	Thu Jan 10 17:36:41 2013 +0100
@@ -114,6 +114,13 @@
         } \
     } while (0)
 
+
+/** Size of the external datagram ring.
+ *
+ * The external datagram ring is used for slave FSMs.
+ */
+#define EC_EXT_RING_SIZE 32
+
 /*****************************************************************************/
 
 /** EtherCAT master phase.
@@ -260,11 +267,20 @@
     struct semaphore ext_queue_sem; /**< Semaphore protecting the \a
                                       ext_datagram_queue. */
 
-    struct list_head external_datagram_queue; /**< External Datagram queue. */
+    ec_datagram_t ext_datagram_ring[EC_EXT_RING_SIZE]; /**< External datagram
+                                                         ring. */
+    unsigned int ext_ring_idx_rt; /**< Index in external datagram ring for RT
+                                    side. */
+    unsigned int ext_ring_idx_fsm; /**< Index in external datagram ring for
+                                     FSM side. */
     unsigned int send_interval; /**< Interval between two calls to
                                   ecrt_master_send(). */
     size_t max_queue_size; /**< Maximum size of datagram queue */
 
+    ec_slave_t *fsm_slave; /**< Slave that is queried next for FSM exec. */
+    struct list_head fsm_exec_list; /**< Slave FSM execution list. */
+    unsigned int fsm_exec_count; /**< Number of entries in execution list. */
+
     unsigned int debug_level; /**< Master debug level. */
     ec_stats_t stats; /**< Cyclic statistics. */
 
@@ -324,8 +340,6 @@
 void ec_master_receive_datagrams(ec_master_t *, const uint8_t *, size_t);
 void ec_master_queue_datagram(ec_master_t *, ec_datagram_t *);
 void ec_master_queue_datagram_ext(ec_master_t *, ec_datagram_t *);
-void ec_master_queue_external_datagram(ec_master_t *, ec_datagram_t *);
-void ec_master_inject_external_datagrams(ec_master_t *);
 
 // misc.
 void ec_master_set_send_interval(ec_master_t *, unsigned int);
diff -r 505cf41488a4 -r 9cdd7669dc0b master/slave.c
--- a/master/slave.c	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/slave.c	Thu Jan 10 17:36:41 2013 +0100
@@ -68,7 +68,6 @@
         )
 {
     unsigned int i;
-    int ret;
 
     slave->master = master;
     slave->device_index = dev_idx;
@@ -157,19 +156,8 @@
     INIT_LIST_HEAD(&slave->foe_requests);
     INIT_LIST_HEAD(&slave->soe_requests);
 
-    // init state machine datagram
-    ec_datagram_init(&slave->fsm_datagram);
-    snprintf(slave->fsm_datagram.name, EC_DATAGRAM_NAME_SIZE,
-            "slave%u-fsm", slave->ring_position);
-    ret = ec_datagram_prealloc(&slave->fsm_datagram, EC_MAX_DATA_SIZE);
-    if (ret < 0) {
-        ec_datagram_clear(&slave->fsm_datagram);
-        EC_SLAVE_ERR(slave, "Failed to allocate FSM datagram.\n");
-        return;
-    }
-
     // create state machine object
-    ec_fsm_slave_init(&slave->fsm, slave, &slave->fsm_datagram);
+    ec_fsm_slave_init(&slave->fsm, slave);
 }
 
 /*****************************************************************************/
@@ -194,7 +182,6 @@
         EC_SLAVE_WARN(slave, "Discarding SDO request,"
                 " slave about to be deleted.\n");
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
     }
 
     while (!list_empty(&slave->reg_requests)) {
@@ -204,7 +191,6 @@
         EC_SLAVE_WARN(slave, "Discarding register request,"
                 " slave about to be deleted.\n");
         reg->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
     }
 
     while (!list_empty(&slave->foe_requests)) {
@@ -214,7 +200,6 @@
         EC_SLAVE_WARN(slave, "Discarding FoE request,"
                 " slave about to be deleted.\n");
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
     }
 
     while (!list_empty(&slave->soe_requests)) {
@@ -224,8 +209,9 @@
         EC_SLAVE_WARN(slave, "Discarding SoE request,"
                 " slave about to be deleted.\n");
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
-    }
+    }
+
+    wake_up_all(&slave->master->request_queue);
 
     if (slave->config) {
         ec_slave_config_detach(slave->config);
@@ -260,7 +246,6 @@
     }
 
     ec_fsm_slave_clear(&slave->fsm);
-    ec_datagram_clear(&slave->fsm_datagram);
 }
 
 /*****************************************************************************/
diff -r 505cf41488a4 -r 9cdd7669dc0b master/slave.h
--- a/master/slave.h	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/slave.h	Thu Jan 10 17:36:41 2013 +0100
@@ -230,7 +230,6 @@
     struct list_head soe_requests; /**< SoE write requests. */
 
     ec_fsm_slave_t fsm; /**< Slave state machine. */
-    ec_datagram_t fsm_datagram; /**< Datagram used for state machines. */
 };
 
 /*****************************************************************************/