diff -r c9308eb34c0e -r 58532f81a456 master/fsm_coe.c
--- a/master/fsm_coe.c	Fri Jul 10 10:07:48 2009 +0000
+++ b/master/fsm_coe.c	Fri Jul 10 10:37:14 2009 +0000
@@ -1494,6 +1494,31 @@
 
 /*****************************************************************************/
 
+/** Prepare an SDO upload segment request.
+ */
+void ec_fsm_coe_up_prepare_segment_request(
+        ec_fsm_coe_t *fsm /**< Finite state machine */
+        )
+{
+    uint8_t *data =
+        ec_slave_mbox_prepare_send(fsm->slave, fsm->datagram, 0x03, 10);
+    if (IS_ERR(data)) {
+        fsm->state = ec_fsm_coe_error;
+        return;
+    }
+
+    EC_WRITE_U16(data, 0x2 << 12); // SDO request
+    EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle
+                | 0x3 << 5)); // upload segment request
+
+    if (fsm->slave->master->debug_level) {
+        EC_DBG("Upload segment request:\n");
+        ec_print_data(data, 10);
+    }
+}
+
+/*****************************************************************************/
+
 /**
    CoE state: UP RESPONSE.
    \todo Timeout behavior
@@ -1661,22 +1686,7 @@
                 EC_DBG("SDO data incomplete (%u / %u). Segmenting...\n",
                         data_size, fsm->complete_size);
 
-            data = ec_slave_mbox_prepare_send(slave, datagram,
-                    0x03, 3);
-            if (IS_ERR(data)) {
-                fsm->state = ec_fsm_coe_error;
-                return;
-            }
-
-            EC_WRITE_U16(data, 0x2 << 12); // SDO request
-            EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle
-                                    | 0x3 << 5)); // upload segment request
-
-            if (master->debug_level) {
-                EC_DBG("Upload segment request:\n");
-                ec_print_data(data, 3);
-            }
-
+            ec_fsm_coe_up_prepare_segment_request(fsm);
             fsm->retries = EC_FSM_RETRIES;
             fsm->state = ec_fsm_coe_up_seg_request;
             return;
@@ -1876,7 +1886,8 @@
     last_segment = EC_READ_U8(data + 2) & 0x01;
     seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1;
     if (rec_size > 10) {
-        data_size = rec_size - 10;
+        data_size = rec_size - 3; /* Header of segment upload is smaller than
+                                     normal upload */
     } else { // == 10
         /* seg_size contains the number of trailing bytes to ignore. */
         data_size = rec_size - seg_size;
@@ -1890,27 +1901,12 @@
         return;
     }
 
-    memcpy(request->data + request->data_size, data + 10, data_size);
+    memcpy(request->data + request->data_size, data + 3, data_size);
     request->data_size += data_size;
 
     if (!last_segment) {
         fsm->toggle = !fsm->toggle;
-
-        data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 3);
-        if (IS_ERR(data)) {
-            fsm->state = ec_fsm_coe_error;
-            return;
-        }
-
-        EC_WRITE_U16(data, 0x2 << 12); // SDO request
-        EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle
-                                | 0x3 << 5)); // upload segment request
-
-        if (master->debug_level) {
-            EC_DBG("Upload segment request:\n");
-            ec_print_data(data, 3);
-        }
-
+        ec_fsm_coe_up_prepare_segment_request(fsm);
         fsm->retries = EC_FSM_RETRIES;
         fsm->state = ec_fsm_coe_up_seg_request;
         return;