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