From a5cfa32f4b769972b6e6f6ab090473d52c2b4f87 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Fri, 13 May 2016 17:43:00 +0200
Subject: [PATCH] Fixed fragmented SoE write request.

---
 master/fsm_soe.c | 61 +++++++++++++++++++++++++-----------------------
 1 file changed, 32 insertions(+), 29 deletions(-)

diff --git a/master/fsm_soe.c b/master/fsm_soe.c
index 53b8ac6e..87e97a59 100644
--- a/master/fsm_soe.c
+++ b/master/fsm_soe.c
@@ -58,6 +58,10 @@ enum ec_soe_opcodes {
  */
 #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 @@ int ec_fsm_soe_prepare_read(
     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 @@ void ec_fsm_soe_read_response(
     }
 
     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 @@ void ec_fsm_soe_write_next_fragment(
     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 @@ void ec_fsm_soe_write_next_fragment(
             (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 @@ void ec_fsm_soe_write_start(
         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 @@ void ec_fsm_soe_write_request(
         return;
     }
 
-    fsm->jiffies_start = fsm->datagram->jiffies_sent;
+    // fragment successfully sent
+    fsm->offset += fsm->fragment_size;
 
-    ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
-    fsm->retries = EC_FSM_RETRIES;
-    fsm->state = ec_fsm_soe_write_check;
+    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 @@ void ec_fsm_soe_write_response(
     }
 
     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 @@ void ec_fsm_soe_write_response(
         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
     }
 }