# HG changeset patch
# User Florian Pose <fp@igh-essen.com>
# Date 1268142025 -3600
# Node ID c6c8b457bb400bc1d9375b8bd699791ac50acf3f
# Parent  0a6b3aacc847dce9c4cdb70e246f3a2134f10bba
Implemented SoE write fragmenting.

diff -r 0a6b3aacc847 -r c6c8b457bb40 TODO
--- a/TODO	Tue Mar 09 13:37:15 2010 +0100
+++ b/TODO	Tue Mar 09 14:40:25 2010 +0100
@@ -23,7 +23,6 @@
 * Output warning on unmatched slave configuration.
 * Output warning when send_ext() is called in illegal context.
 * Add master index to log messages.
-* Implement SoE fragmenting.
 * Implement CompleteAccess for SDO uploads.
 * Check for Enable SDO Complete Access flag.
 * Remove allow_scanning flag.
diff -r 0a6b3aacc847 -r c6c8b457bb40 master/fsm_soe.c
--- a/master/fsm_soe.c	Tue Mar 09 13:37:15 2010 +0100
+++ b/master/fsm_soe.c	Tue Mar 09 14:40:25 2010 +0100
@@ -110,7 +110,6 @@
     if (request->dir == EC_DIR_OUTPUT) {
         fsm->state = ec_fsm_soe_write_start;
 	} else {
-        fsm->request->data_size = 0;
         fsm->state = ec_fsm_soe_read_start;
 	}
 }
@@ -181,6 +180,7 @@
         ec_print_data(data, EC_SOE_READ_REQUEST_SIZE);
     }
 
+    fsm->request->data_size = 0;
     fsm->request->jiffies_sent = jiffies;
     fsm->retries = EC_FSM_RETRIES;
     fsm->state = ec_fsm_soe_read_request;
@@ -398,19 +398,73 @@
  * SoE write state machine
  *****************************************************************************/
 
+/** Write next fragment.
+ */
+void ec_fsm_soe_write_next_fragment(
+        ec_fsm_soe_t *fsm /**< finite state machine */
+        )
+{
+    ec_datagram_t *datagram = fsm->datagram;
+    ec_slave_t *slave = fsm->slave;
+    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, fragment_size;
+    uint16_t fragments_left;
+
+    header_size = EC_MBOX_HEADER_SIZE + EC_SOE_WRITE_REQUEST_SIZE;
+    if (slave->configured_rx_mailbox_size <= header_size) {
+        EC_ERR("Mailbox size (%u) too small for SoE write.\n",
+                slave->configured_rx_mailbox_size);
+        fsm->state = ec_fsm_soe_error;
+        return;
+    }
+
+    remaining_size = req->data_size - fsm->offset;
+    max_fragment_size = slave->configured_rx_mailbox_size - header_size;
+    incomplete = remaining_size > max_fragment_size;
+    fragment_size = incomplete ? max_fragment_size : remaining_size;
+    fragments_left = remaining_size / fragment_size;
+    if (remaining_size % fragment_size) {
+        fragments_left++;
+    }
+
+    data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
+			EC_SOE_WRITE_REQUEST_SIZE + fragment_size);
+    if (IS_ERR(data)) {
+        fsm->state = ec_fsm_soe_error;
+        return;
+    }
+
+    EC_WRITE_U8(data, EC_SOE_OPCODE_WRITE_REQUEST | incomplete << 3);
+    EC_WRITE_U8(data + 1, 1 << 6); // only value included
+    EC_WRITE_U16(data + 2, fsm->offset ? fragments_left : req->idn);
+	memcpy(data + 4, req->data + fsm->offset, fragment_size);
+    fsm->offset += fragment_size;
+
+    if (master->debug_level) {
+        EC_DBG("SCC write request:\n");
+        ec_print_data(data, EC_SOE_WRITE_REQUEST_SIZE + fragment_size);
+    }
+
+    req->jiffies_sent = jiffies;
+    fsm->retries = EC_FSM_RETRIES;
+    fsm->state = ec_fsm_soe_write_request;
+}
+
+/*****************************************************************************/
+
 /** SoE state: WRITE START.
  */
 void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */)
 {
-    ec_datagram_t *datagram = fsm->datagram;
     ec_slave_t *slave = fsm->slave;
     ec_master_t *master = slave->master;
-    ec_soe_request_t *request = fsm->request;
-    uint8_t *data;
+    ec_soe_request_t *req = fsm->request;
 
     if (master->debug_level)
         EC_DBG("Writing IDN 0x%04X to slave %u.\n",
-               request->idn, slave->ring_position);
+               req->idn, slave->ring_position);
 
     if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
         EC_ERR("Slave %u does not support SoE!\n", slave->ring_position);
@@ -418,26 +472,8 @@
         return;
     }
 
-    data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
-			EC_SOE_WRITE_REQUEST_SIZE + request->data_size);
-    if (IS_ERR(data)) {
-        fsm->state = ec_fsm_soe_error;
-        return;
-    }
-
-    EC_WRITE_U8(data, EC_SOE_OPCODE_WRITE_REQUEST);
-    EC_WRITE_U8(data + 1, 1 << 6); // only value included
-    EC_WRITE_U16(data + 2, request->idn);
-	memcpy(data + 4, request->data, request->data_size);
-
-    if (master->debug_level) {
-        EC_DBG("SCC write request:\n");
-        ec_print_data(data, EC_SOE_WRITE_REQUEST_SIZE + request->data_size);
-    }
-
-    fsm->request->jiffies_sent = jiffies;
-    fsm->retries = EC_FSM_RETRIES;
-    fsm->state = ec_fsm_soe_write_request;
+    fsm->offset = 0;
+    ec_fsm_soe_write_next_fragment(fsm);
 }
 
 /*****************************************************************************/
@@ -627,7 +663,11 @@
 		req->error_code = 0x0000;
 	}
 
-    fsm->state = ec_fsm_soe_end; // success
+    if (fsm->offset < req->data_size) {
+        ec_fsm_soe_write_next_fragment(fsm);
+    } else {
+        fsm->state = ec_fsm_soe_end; // success
+    }
 }
 
 /*****************************************************************************/
diff -r 0a6b3aacc847 -r c6c8b457bb40 master/fsm_soe.h
--- a/master/fsm_soe.h	Tue Mar 09 13:37:15 2010 +0100
+++ b/master/fsm_soe.h	Tue Mar 09 14:40:25 2010 +0100
@@ -56,6 +56,7 @@
     void (*state)(ec_fsm_soe_t *); /**< CoE state function */
     unsigned long jiffies_start; /**< CoE timestamp. */
     ec_soe_request_t *request; /**< SoE request */
+    off_t offset; /**< IDN data offset during fragmented write. */
 };
 
 /*****************************************************************************/