Implemented SoE fragmented reading.
authorFlorian Pose <fp@igh-essen.com>
Tue, 09 Mar 2010 13:37:15 +0100
changeset 1864 0a6b3aacc847
parent 1863 8ea4a79dfe84
child 1865 c6c8b457bb40
Implemented SoE fragmented reading.
master/fsm_soe.c
master/soe_request.c
master/soe_request.h
--- a/master/fsm_soe.c	Tue Mar 09 12:18:43 2010 +0100
+++ b/master/fsm_soe.c	Tue Mar 09 13:37:15 2010 +0100
@@ -110,6 +110,7 @@
     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;
 	}
 }
@@ -225,7 +226,6 @@
     }
 
     fsm->jiffies_start = datagram->jiffies_sent;
-
     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
     fsm->retries = EC_FSM_RETRIES;
     fsm->state = ec_fsm_soe_read_check;
@@ -290,7 +290,8 @@
     ec_datagram_t *datagram = fsm->datagram;
     ec_slave_t *slave = fsm->slave;
     ec_master_t *master = slave->master;
-    uint8_t *data, mbox_prot, opcode, error_flag, value_included;
+    uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag,
+            value_included;
     size_t rec_size, data_size;
     ec_soe_request_t *req = fsm->request;
 
@@ -338,7 +339,11 @@
         return;
     }
 
-	opcode = EC_READ_U8(data) & 0x7;
+    header = EC_READ_U8(data);
+	opcode = header & 0x7;
+    incomplete = (header >> 3) & 1;
+	error_flag = (header >> 4) & 1;
+
     if (opcode != EC_SOE_OPCODE_READ_RESPONSE) {
         EC_ERR("Received no read response (opcode %x).\n", opcode);
         ec_print_data(data, rec_size);
@@ -346,7 +351,6 @@
         return;
     }
 
-	error_flag = (EC_READ_U8(data) >> 4) & 1;
 	if (error_flag) {
 		req->error_code = EC_READ_U16(data + rec_size - 2);
 		EC_ERR("Received error response: 0x%04x.\n",
@@ -365,18 +369,29 @@
 	}
 
 	data_size = rec_size - EC_SOE_READ_RESPONSE_SIZE;
-	if (ec_soe_request_copy_data(req,
-				data + EC_SOE_READ_RESPONSE_SIZE, data_size)) {
+	if (ec_soe_request_append_data(req,
+                data + EC_SOE_READ_RESPONSE_SIZE, data_size)) {
 		fsm->state = ec_fsm_soe_error;
 		return;
 	}
 
-	if (master->debug_level) {
-		EC_DBG("IDN data:\n");
-		ec_print_data(req->data, req->data_size);
-	}
-
-    fsm->state = ec_fsm_soe_end; // success
+    if (incomplete) {
+        if (master->debug_level) {
+            EC_DBG("SoE data incomplete. Waiting for fragment"
+                    " at offset %zu.\n", req->data_size);
+        }
+        fsm->jiffies_start = datagram->jiffies_sent;
+        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+        fsm->retries = EC_FSM_RETRIES;
+        fsm->state = ec_fsm_soe_read_check;
+    } else {
+        if (master->debug_level) {
+            EC_DBG("IDN data:\n");
+            ec_print_data(req->data, req->data_size);
+        }
+
+        fsm->state = ec_fsm_soe_end; // success
+    }
 }
 
 /******************************************************************************
--- a/master/soe_request.c	Tue Mar 09 12:18:43 2010 +0100
+++ b/master/soe_request.c	Tue Mar 09 13:37:15 2010 +0100
@@ -172,6 +172,40 @@
 
 /*****************************************************************************/
 
+/** Copies SoE data from an external source.
+ *
+ * If the \a mem_size is to small, new memory is allocated.
+ *
+ * \retval  0 Success.
+ * \retval <0 Error code.
+ */
+int ec_soe_request_append_data(
+        ec_soe_request_t *req, /**< SoE request. */
+        const uint8_t *source, /**< Source data. */
+        size_t size /**< Number of bytes in \a source. */
+        )
+{
+    if (req->data_size + size > req->mem_size) {
+        size_t new_size = req->mem_size ? req->mem_size * 2 : size;
+        uint8_t *new_data = (uint8_t *) kmalloc(new_size, GFP_KERNEL);
+        if (!new_data) {
+            EC_ERR("Failed to allocate %zu bytes of SoE memory.\n",
+                    new_size);
+            return -ENOMEM;
+        }
+        memcpy(new_data, req->data, req->data_size);
+        kfree(req->data);
+        req->data = new_data;
+        req->mem_size = new_size;
+    }
+
+    memcpy(req->data + req->data_size, source, size);
+    req->data_size += size;
+    return 0;
+}
+
+/*****************************************************************************/
+
 void ec_soe_request_read(ec_soe_request_t *req)
 {
     req->dir = EC_DIR_INPUT;
--- a/master/soe_request.h	Tue Mar 09 12:18:43 2010 +0100
+++ b/master/soe_request.h	Tue Mar 09 13:37:15 2010 +0100
@@ -68,6 +68,7 @@
 void ec_soe_request_set_idn(ec_soe_request_t *, uint16_t);
 int ec_soe_request_alloc(ec_soe_request_t *, size_t);
 int ec_soe_request_copy_data(ec_soe_request_t *, const uint8_t *, size_t);
+int ec_soe_request_append_data(ec_soe_request_t *, const uint8_t *, size_t);
 void ec_soe_request_read(ec_soe_request_t *);
 void ec_soe_request_write(ec_soe_request_t *);