--- a/master/fsm_soe.c	Thu Feb 25 15:23:42 2016 +0100
+++ b/master/fsm_soe.c	Fri May 13 17:43:00 2016 +0200
@@ -58,6 +58,10 @@
  */
 #define EC_SOE_SIZE 0x04
 
+/** SoE header size.
+ */
+#define EC_SOE_HEADER_SIZE (EC_MBOX_HEADER_SIZE + EC_SOE_SIZE)
+
 /** SoE response timeout [ms].
  */
 #define EC_SOE_RESPONSE_TIMEOUT 1000
@@ -237,7 +241,7 @@
     EC_WRITE_U16(data + 2, request->idn);
 
     if (master->debug_level) {
-        EC_SLAVE_DBG(slave, 0, "SCC read request:\n");
+        EC_SLAVE_DBG(slave, 0, "SSC read request:\n");
         ec_print_data(data, EC_SOE_SIZE);
     }
 
@@ -434,7 +438,7 @@
     }
 
     if (master->debug_level) {
-        EC_SLAVE_DBG(slave, 0, "SCC read response:\n");
+        EC_SLAVE_DBG(slave, 0, "SSC read response:\n");
         ec_print_data(data, rec_size);
     }
 
@@ -528,20 +532,11 @@
     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;
+    size_t max_fragment_size, remaining_size;
     uint16_t fragments_left;
 
-    header_size = EC_MBOX_HEADER_SIZE + EC_SOE_SIZE;
-    if (slave->configured_rx_mailbox_size <= header_size) {
-        EC_SLAVE_ERR(slave, "Mailbox size (%u) too small for SoE write.\n",
-                slave->configured_rx_mailbox_size);
-        fsm->state = ec_fsm_soe_error;
-        ec_fsm_soe_print_error(fsm);
-        return;
-    }
-
     remaining_size = req->data_size - fsm->offset;
-    max_fragment_size = slave->configured_rx_mailbox_size - header_size;
+    max_fragment_size = slave->configured_rx_mailbox_size - EC_SOE_HEADER_SIZE;
     incomplete = remaining_size > max_fragment_size;
     fsm->fragment_size = incomplete ? max_fragment_size : remaining_size;
     fragments_left = remaining_size / fsm->fragment_size - 1;
@@ -561,10 +556,10 @@
             (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, fsm->fragment_size);
+    memcpy(data + EC_SOE_SIZE, req->data + fsm->offset, fsm->fragment_size);
 
     if (master->debug_level) {
-        EC_SLAVE_DBG(slave, 0, "SCC write request:\n");
+        EC_SLAVE_DBG(slave, 0, "SSC write request:\n");
         ec_print_data(data, EC_SOE_SIZE + fsm->fragment_size);
     }
 
@@ -594,6 +589,14 @@
         return;
     }
 
+    if (slave->configured_rx_mailbox_size <= EC_SOE_HEADER_SIZE) {
+        EC_SLAVE_ERR(slave, "Mailbox size (%u) too small for SoE write.\n",
+                slave->configured_rx_mailbox_size);
+        fsm->state = ec_fsm_soe_error;
+        ec_fsm_soe_print_error(fsm);
+        return;
+    }
+
     fsm->offset = 0;
     fsm->retries = EC_FSM_RETRIES;
     ec_fsm_soe_write_next_fragment(fsm, datagram);
@@ -642,11 +645,20 @@
         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_write_check;
+    // fragment successfully sent
+    fsm->offset += fsm->fragment_size;
+
+    if (fsm->offset < fsm->request->data_size) {
+        // next fragment
+        fsm->retries = EC_FSM_RETRIES;
+        ec_fsm_soe_write_next_fragment(fsm, datagram);
+    } else {
+        // all fragments sent; query response
+        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_write_check;
+    }
 }
 
 /*****************************************************************************/
@@ -749,7 +761,7 @@
     }
 
     if (master->debug_level) {
-        EC_SLAVE_DBG(slave, 0, "SCC write response:\n");
+        EC_SLAVE_DBG(slave, 0, "SSC write response:\n");
         ec_print_data(data, rec_size);
     }
 
@@ -804,17 +816,8 @@
         ec_print_data(data, rec_size);
         ec_fsm_soe_print_error(fsm);
         fsm->state = ec_fsm_soe_error;
-        return;
     } else {
         req->error_code = 0x0000;
-    }
-
-    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
     }
 }