From 75992b9aa4dae856fdb275111378e928c6bbf04b Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Mon, 26 Jan 2009 14:13:50 +0000
Subject: [PATCH] merge -c1622 branches/1.4-foe: Fixed seg_size parameter when
 processing an CoE Upload Segment Response.

---
 master/fsm_coe.c | 47 ++++++++++++++++++++++++++++++-----------------
 master/fsm_coe.h |  1 +
 2 files changed, 31 insertions(+), 17 deletions(-)

diff --git a/master/fsm_coe.c b/master/fsm_coe.c
index 91b86adc..5242ed9a 100644
--- a/master/fsm_coe.c
+++ b/master/fsm_coe.c
@@ -1493,7 +1493,6 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
     uint8_t *data, mbox_prot;
     size_t rec_size, data_size;
     ec_sdo_request_t *request = fsm->request;
-    uint32_t complete_size;
     unsigned int expedited, size_specified;
 
     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
@@ -1591,12 +1590,12 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
 
         size_specified = EC_READ_U8(data + 2) & 0x01;
         if (size_specified) {
-            complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2);
+            fsm->complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2);
         } else {
-            complete_size = 4;
+            fsm->complete_size = 4;
         }
 
-        if (rec_size < 6 + complete_size) {
+        if (rec_size < 6 + fsm->complete_size) {
             fsm->state = ec_fsm_coe_error;
             EC_ERR("Received currupted SDO expedited upload"
                     " response (only %u bytes)!\n", rec_size);
@@ -1604,7 +1603,7 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
             return;
         }
 
-        if (ec_sdo_request_copy_data(request, data + 6, complete_size)) {
+        if (ec_sdo_request_copy_data(request, data + 6, fsm->complete_size)) {
             fsm->state = ec_fsm_coe_error;
             return;
         }
@@ -1634,16 +1633,16 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
         }
 
         data_size = rec_size - 10;
-        complete_size = EC_READ_U32(data + 6);
+        fsm->complete_size = EC_READ_U32(data + 6);
 
-        if (!complete_size) {
+        if (!fsm->complete_size) {
             fsm->state = ec_fsm_coe_error;
             EC_ERR("No complete size supplied!\n");
             ec_print_data(data, rec_size);
             return;
         }
 
-        if (ec_sdo_request_alloc(request, complete_size)) {
+        if (ec_sdo_request_alloc(request, fsm->complete_size)) {
             fsm->state = ec_fsm_coe_error;
             return;
         }
@@ -1655,9 +1654,10 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
 
         fsm->toggle = 0;
 
-        if (data_size < complete_size) {
-            EC_WARN("SDO data incomplete (%u / %u).\n",
-                    data_size, complete_size);
+        if (data_size < fsm->complete_size) {
+            if (master->debug_level)
+                EC_DBG("SDO data incomplete (%u / %u). Segmenting...\n",
+                        data_size, fsm->complete_size);
 
             data = ec_slave_mbox_prepare_send(slave, datagram,
                     0x03, 3);
@@ -1783,7 +1783,6 @@ void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)
 /**
    CoE state: UP RESPONSE.
    \todo Timeout behavior
-   \todo Check for \a data_size exceeding \a complete_size.
 */
 
 void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */)
@@ -1874,12 +1873,19 @@ void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */)
 
     last_segment = EC_READ_U8(data + 2) & 0x01;
     seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1;
-    data_size = rec_size - 10;
+    if (rec_size > 10) {
+        data_size = rec_size - 10;
+    } else { // == 10
+        /* seg_size contains the number of trailing bytes to ignore. */
+        data_size = rec_size - seg_size;
+    }
 
-    if (data_size != seg_size) {
-        EC_WARN("SDO segment data invalid (%u / %u)"
-                " - Fragmenting not implemented.\n",
-                data_size, seg_size);
+    if (request->data_size + data_size > fsm->complete_size) {
+        EC_ERR("SDO upload 0x%04X:%02X failed on slave %u: Fragment"
+                " exceeding complete size!\n",
+               request->index, request->subindex, slave->ring_position);
+        fsm->state = ec_fsm_coe_error;
+        return;
     }
 
     memcpy(request->data + request->data_size, data + 10, data_size);
@@ -1908,6 +1914,13 @@ void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */)
         return;
     }
 
+    if (request->data_size != fsm->complete_size) {
+        EC_WARN("SDO upload 0x%04X:%02X on slave %u: Assembled data"
+                " size (%u) does not match complete size (%u)!\n",
+                request->index, request->subindex, slave->ring_position,
+                request->data_size, fsm->complete_size);
+    }
+
     if (master->debug_level) {
         EC_DBG("Uploaded data:\n");
         ec_print_data(request->data, request->data_size);
diff --git a/master/fsm_coe.h b/master/fsm_coe.h
index 710f0fbf..b217956f 100644
--- a/master/fsm_coe.h
+++ b/master/fsm_coe.h
@@ -56,6 +56,7 @@ struct ec_fsm_coe {
     ec_sdo_t *sdo; /**< current SDO */
     uint8_t subindex; /**< current subindex */
     ec_sdo_request_t *request; /**< SDO request */
+    uint32_t complete_size; /**< Used when segmenting. */
     uint8_t toggle; /**< toggle bit for segment commands */
 };