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. */
+        )
+{
+}
+
+/*****************************************************************************/