# HG changeset patch
# User Florian Pose <fp@igh-essen.com>
# Date 1197997012 0
# Node ID 2195c2ea37b4c1457be7287ac58c7b7822d2d8ea
# Parent  b709149d5bba2b7b5e2ac4225058072ab9d59477
Fixed normal/expedited bug at CoE upload; more robust behaviour, when
fragments_left is set.

diff -r b709149d5bba -r 2195c2ea37b4 master/fsm_coe.c
--- a/master/fsm_coe.c	Thu Nov 22 09:01:41 2007 +0000
+++ b/master/fsm_coe.c	Tue Dec 18 16:56:52 2007 +0000
@@ -366,7 +366,7 @@
     uint8_t *data, mbox_prot;
     size_t rec_size;
     unsigned int sdo_count, i;
-    uint16_t sdo_index;
+    uint16_t sdo_index, fragments_left;
     ec_sdo_t *sdo;
 
     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
@@ -397,7 +397,7 @@
     if (mbox_prot != 0x03) { // CoE
         EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
         fsm->state = ec_fsm_coe_error;
-	return;
+        return;
     }
 
     if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
@@ -406,7 +406,7 @@
                slave->ring_position);
         ec_canopen_abort_msg(EC_READ_U32(data + 6));
         fsm->state = ec_fsm_coe_error;
-	return;
+        return;
     }
 
     if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information
@@ -415,14 +415,14 @@
                slave->ring_position);
         ec_print_data(data, rec_size);
         fsm->state = ec_fsm_coe_error;
-	return;
+        return;
     }
 
     if (rec_size < 8) {
         EC_ERR("Invalid data size!\n");
         ec_print_data(data, rec_size);
         fsm->state = ec_fsm_coe_error;
-	return;
+        return;
     }
 
     sdo_count = (rec_size - 8) / 2;
@@ -451,7 +451,12 @@
         list_add_tail(&sdo->list, &slave->sdo_dictionary);
     }
 
-    if (EC_READ_U8(data + 2) & 0x80) { // more messages waiting. check again.
+    fragments_left = EC_READ_U16(data + 4);
+    if (slave->master->debug_level && fragments_left) {
+        EC_DBG("SDO list fragments left: %u\n", fragments_left);
+    }
+
+    if (EC_READ_U8(data + 2) & 0x80 || fragments_left) { // more messages waiting. check again.
         fsm->cycles_start = datagram->cycles_sent;
         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
         fsm->retries = EC_FSM_RETRIES;
@@ -1325,77 +1330,127 @@
     if (mbox_prot != 0x03) { // CoE
         EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
         fsm->state = ec_fsm_coe_error;
-	return;
-    }
-
-    if (rec_size < 10) {
-        EC_ERR("Received currupted SDO upload response!\n");
+        return;
+    }
+
+    if (rec_size < 3) {
+        EC_ERR("Received currupted SDO upload response (%u bytes)!\n", rec_size);
         ec_print_data(data, rec_size);
         fsm->state = ec_fsm_coe_error;
-	return;
+        return;
     }
 
     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
         EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n",
                entry->sdo->index, entry->subindex, slave->ring_position);
-        ec_canopen_abort_msg(EC_READ_U32(data + 6));
-        fsm->state = ec_fsm_coe_error;
-	return;
-    }
-
-    if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
-        EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response
-        EC_READ_U16(data + 3) != entry->sdo->index || // index
-        EC_READ_U8 (data + 5) != entry->subindex) { // subindex
-        EC_ERR("SDO upload 0x%04X:%X failed:\n", entry->sdo->index, entry->subindex);
-        EC_ERR("Invalid SDO upload response at slave %i!\n",
-               slave->ring_position);
-        ec_print_data(data, rec_size);
-        fsm->state = ec_fsm_coe_error;
-	return;
-    }
-
-    data_size = rec_size - 10;
+        if (rec_size >= 10)
+            ec_canopen_abort_msg(EC_READ_U32(data + 6));
+        else
+            EC_ERR("No abort message.\n");
+        fsm->state = ec_fsm_coe_error;
+        return;
+    }
+
+    if (request->data) {
+        kfree(request->data);
+        request->data = NULL;
+    }
+    request->size = 0;
+
+    // normal or expedited?
     expedited = EC_READ_U8(data + 2) & 0x02;
 
     if (expedited) {
+        if (rec_size < 7) {
+            EC_ERR("Received currupted SDO expedited upload"
+                    " response (only %u bytes)!\n", rec_size);
+            ec_print_data(data, rec_size);
+            fsm->state = ec_fsm_coe_error;
+            return;
+        }
+
+        if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
+                EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response
+                EC_READ_U16(data + 3) != entry->sdo->index || // index
+                EC_READ_U8 (data + 5) != entry->subindex) { // subindex
+            EC_ERR("SDO upload 0x%04X:%X failed:\n", entry->sdo->index, entry->subindex);
+            EC_ERR("Invalid SDO upload response at slave %i!\n",
+                    slave->ring_position);
+            ec_print_data(data, rec_size);
+            fsm->state = ec_fsm_coe_error;
+            return;
+        }
+
         size_specified = EC_READ_U8(data + 2) & 0x01;
         if (size_specified) {
             complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2);
-        }
-        else {
+        } else {
             complete_size = 4;
         }
-    }
-    else {
-        complete_size = EC_READ_U32(data + 6);
-    }
-
-    if (request->data) {
-        kfree(request->data);
-        request->data = NULL;
-    }
-    request->size = 0;
-
-    if (complete_size) {
+
+        if (rec_size < 6 + complete_size) {
+            EC_ERR("Received currupted SDO expedited upload"
+                    " response (only %u bytes)!\n", rec_size);
+            ec_print_data(data, rec_size);
+            fsm->state = ec_fsm_coe_error;
+            return;
+        }
+
         if (!(request->data = (uint8_t *)
-              kmalloc(complete_size + 1, GFP_ATOMIC))) {
+                    kmalloc(complete_size + 1, GFP_ATOMIC))) {
             EC_ERR("Failed to allocate %i bytes of SDO data!\n",
-                   complete_size);
+                    complete_size);
             fsm->state = ec_fsm_coe_error;
             return;
         }
         request->data[complete_size] = 0x00; // just to be sure...
-    }
-
-    if (expedited) {
+
         memcpy(request->data, data + 6, complete_size);
         request->size = complete_size;
-    }
-    else {
+
+    } else { // normal
+        if (rec_size < 10) {
+            EC_ERR("Received currupted SDO normal upload"
+                    " response (only %u bytes)!\n", rec_size);
+            ec_print_data(data, rec_size);
+            fsm->state = ec_fsm_coe_error;
+            return;
+        }
+
+        if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
+                EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response
+                EC_READ_U16(data + 3) != entry->sdo->index || // index
+                EC_READ_U8 (data + 5) != entry->subindex) { // subindex
+            EC_ERR("SDO upload 0x%04X:%X failed:\n", entry->sdo->index, entry->subindex);
+            EC_ERR("Invalid SDO upload response at slave %i!\n",
+                    slave->ring_position);
+            ec_print_data(data, rec_size);
+            fsm->state = ec_fsm_coe_error;
+            return;
+        }
+
+        data_size = rec_size - 10;
+        complete_size = EC_READ_U32(data + 6);
+
+        if (!complete_size) {
+            EC_ERR("No complete size supplied!\n");
+            ec_print_data(data, rec_size);
+            fsm->state = ec_fsm_coe_error;
+            return;
+        }
+
+        if (!(request->data = (uint8_t *)
+                    kmalloc(complete_size + 1, GFP_ATOMIC))) {
+            EC_ERR("Failed to allocate %i bytes of SDO data!\n",
+                    complete_size);
+            fsm->state = ec_fsm_coe_error;
+            return;
+        }
+        request->data[complete_size] = 0x00; // just to be sure...
+
         memcpy(request->data, data + 10, data_size);
-        request->size = data_size;
+        request->size = complete_size;
         fsm->toggle = 0;
 
         if (data_size < complete_size) {