# HG changeset patch
# User Florian Pose <fp@igh-essen.com>
# Date 1316097193 -7200
# Node ID 755e6ce823f0e6aa383fbd691972d7b4170253e8
# Parent  6d3620f2328ffe3fb6af99d75e32b06824ab3d32
Implemented ecrt_master_sdo_upload() and ecrt_master_sdo_download() also for
kernel space.

diff -r 6d3620f2328f -r 755e6ce823f0 include/ecrt.h
--- a/include/ecrt.h	Thu Sep 15 16:31:43 2011 +0200
+++ b/include/ecrt.h	Thu Sep 15 16:33:13 2011 +0200
@@ -635,14 +635,16 @@
         ec_pdo_entry_info_t *entry /**< Pointer to output structure. */
         );
 
-/** Executes an SDO write request to download data.
- *
- * This function operates aside of the normal way to request SDOs. Before the
- * activation of the master, these requests are processed by the master state
- * machine itself. After activation the user has to ensure cyclic processing.
+#endif /* #ifndef __KERNEL__ */
+
+/** Executes an SDO download request to write data to a slave.
+ *
+ * This request is processed by the master state machine. This method blocks,
+ * until the request has been processed and may not be called in realtime
+ * context.
  *
  * \retval  0 Success.
- * \retval -1 An error occured.
+ * \retval <0 Error code.
  */
 int ecrt_master_sdo_download(
         ec_master_t *master, /**< EtherCAT master. */
@@ -654,15 +656,14 @@
         uint32_t *abort_code /**< Abort code of the SDO download. */
         );
 
-/** Executes a SDO read request to upload data.
- *
- * This function operates aside of the normal way to request SDOs. Before the
- * activation of the master, these requests are processed by the master state
- * machine itself. After activation the user have to ensure cyclic
- * processing.
+/** Executes an SDO upload request to read data from a slave.
+ *
+ * This request is processed by the master state machine. This method blocks,
+ * until the request has been processed and may not be called in realtime
+ * context.
  *
  * \retval  0 Success.
- * \retval -1 Error occurred.
+ * \retval <0 Error code.
  */
 int ecrt_master_sdo_upload(
         ec_master_t *master, /**< EtherCAT master. */
@@ -675,8 +676,6 @@
         uint32_t *abort_code /**< Abort code of the SDO upload. */
         );
 
-#endif /* #ifndef __KERNEL__ */
-
 /** Executes an SoE write request.
  *
  * Starts writing an IDN and blocks until the request was processed, or an
diff -r 6d3620f2328f -r 755e6ce823f0 master/cdev.c
--- a/master/cdev.c	Thu Sep 15 16:31:43 2011 +0200
+++ b/master/cdev.c	Thu Sep 15 16:33:13 2011 +0200
@@ -838,184 +838,78 @@
         )
 {
     ec_ioctl_slave_sdo_upload_t data;
-    ec_master_sdo_request_t request;
-    int retval;
+    uint8_t *target;
+    int ret;
 
     if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
         return -EFAULT;
     }
 
-    ec_sdo_request_init(&request.req);
-    ec_sdo_request_address(&request.req,
-            data.sdo_index, data.sdo_entry_subindex);
-    ecrt_sdo_request_read(&request.req);
-
-    if (down_interruptible(&master->master_sem))
-        return -EINTR;
-
-    if (!(request.slave = ec_master_find_slave(
-                    master, 0, data.slave_position))) {
-        up(&master->master_sem);
-        ec_sdo_request_clear(&request.req);
-        EC_MASTER_ERR(master, "Slave %u does not exist!\n",
-                data.slave_position);
-        return -EINVAL;
-    }
-
-    EC_SLAVE_DBG(request.slave, 1, "Schedule SDO upload request.\n");
-
-    // schedule request.
-    list_add_tail(&request.list, &request.slave->slave_sdo_requests);
-
-    up(&master->master_sem);
-
-    // wait for processing through FSM
-    if (wait_event_interruptible(request.slave->sdo_queue,
-                request.req.state != EC_INT_REQUEST_QUEUED)) {
-        // interrupted by signal
-        down(&master->master_sem);
-        if (request.req.state == EC_INT_REQUEST_QUEUED) {
-            list_del(&request.list);
-            up(&master->master_sem);
-            ec_sdo_request_clear(&request.req);
-            return -EINTR;
-        }
-        // request already processing: interrupt not possible.
-        up(&master->master_sem);
-    }
-
-    // wait until master FSM has finished processing
-    wait_event(request.slave->sdo_queue,
-            request.req.state != EC_INT_REQUEST_BUSY);
-
-    EC_SLAVE_DBG(request.slave, 1, "Finished SDO upload request.\n");
-
-    data.abort_code = request.req.abort_code;
-
-    if (request.req.state != EC_INT_REQUEST_SUCCESS) {
-        data.data_size = 0;
-        if (request.req.errno) {
-            retval = -request.req.errno;
-        } else {
-            retval = -EIO;
-        }
-    } else {
-        if (request.req.data_size > data.target_size) {
-            EC_MASTER_ERR(master, "Buffer too small.\n");
-            ec_sdo_request_clear(&request.req);
-            return -EOVERFLOW;
-        }
-        data.data_size = request.req.data_size;
-
+    if (!(target = kmalloc(data.target_size, GFP_KERNEL))) {
+        EC_MASTER_ERR(master, "Failed to allocate %u bytes"
+                " for SDO upload.\n", data.target_size);
+        return -ENOMEM;
+    }
+
+    ret = ecrt_master_sdo_upload(master, data.slave_position,
+            data.sdo_index, data.sdo_entry_subindex, target,
+            data.target_size, &data.data_size, &data.abort_code);
+
+    if (!ret) {
         if (copy_to_user((void __user *) data.target,
-                    request.req.data, data.data_size)) {
-            ec_sdo_request_clear(&request.req);
+                    target, data.data_size)) {
+            kfree(target);
             return -EFAULT;
         }
-        retval = 0;
-    }
+    }
+
+    kfree(target);
+
+    if (__copy_to_user((void __user *) arg, &data, sizeof(data))) {
+        return -EFAULT;
+    }
+
+    return 0;
+}
+
+/*****************************************************************************/
+
+/** Download SDO.
+ */
+int ec_cdev_ioctl_slave_sdo_download(
+        ec_master_t *master, /**< EtherCAT master. */
+        unsigned long arg /**< ioctl() argument. */
+        )
+{
+    ec_ioctl_slave_sdo_download_t data;
+    uint8_t *sdo_data;
+    int retval;
+
+    if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+        return -EFAULT;
+    }
+
+    if (!(sdo_data = kmalloc(data.data_size, GFP_KERNEL))) {
+        EC_MASTER_ERR(master, "Failed to allocate %u bytes"
+                " for SDO download.\n", data.data_size);
+        return -ENOMEM;
+    }
+
+    if (copy_from_user(sdo_data, (void __user *) data.data, data.data_size)) {
+        kfree(sdo_data);
+        return -EFAULT;
+    }
+
+    retval = ecrt_master_sdo_download(master, data.slave_position,
+            data.sdo_index, data.sdo_entry_subindex, sdo_data, data.data_size,
+            &data.abort_code);
+
+    kfree(sdo_data);
 
     if (__copy_to_user((void __user *) arg, &data, sizeof(data))) {
         retval = -EFAULT;
     }
 
-    ec_sdo_request_clear(&request.req);
-    return retval;
-}
-
-/*****************************************************************************/
-
-/** Download SDO.
- */
-int ec_cdev_ioctl_slave_sdo_download(
-        ec_master_t *master, /**< EtherCAT master. */
-        unsigned long arg /**< ioctl() argument. */
-        )
-{
-    ec_ioctl_slave_sdo_download_t data;
-    ec_master_sdo_request_t request;
-    int retval;
-
-    if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
-        return -EFAULT;
-    }
-
-    // copy data to download
-    if (!data.data_size) {
-        EC_MASTER_ERR(master, "Zero data size!\n");
-        return -EINVAL;
-    }
-
-    ec_sdo_request_init(&request.req);
-    ec_sdo_request_address(&request.req,
-            data.sdo_index, data.sdo_entry_subindex);
-    if (ec_sdo_request_alloc(&request.req, data.data_size)) {
-        ec_sdo_request_clear(&request.req);
-        return -ENOMEM;
-    }
-    if (copy_from_user(request.req.data,
-                (void __user *) data.data, data.data_size)) {
-        ec_sdo_request_clear(&request.req);
-        return -EFAULT;
-    }
-    request.req.data_size = data.data_size;
-    ecrt_sdo_request_write(&request.req);
-
-    if (down_interruptible(&master->master_sem))
-        return -EINTR;
-
-    if (!(request.slave = ec_master_find_slave(
-                    master, 0, data.slave_position))) {
-        up(&master->master_sem);
-        EC_MASTER_ERR(master, "Slave %u does not exist!\n",
-                data.slave_position);
-        ec_sdo_request_clear(&request.req);
-        return -EINVAL;
-    }
-    
-    EC_SLAVE_DBG(request.slave, 1, "Schedule SDO download request.\n");
-
-    // schedule request.
-    list_add_tail(&request.list, &request.slave->slave_sdo_requests);
-
-    up(&master->master_sem);
-
-    // wait for processing through FSM
-    if (wait_event_interruptible(request.slave->sdo_queue,
-                request.req.state != EC_INT_REQUEST_QUEUED)) {
-        // interrupted by signal
-        down(&master->master_sem);
-        if (request.req.state == EC_INT_REQUEST_QUEUED) {
-            list_del(&request.list);
-            up(&master->master_sem);
-            ec_sdo_request_clear(&request.req);
-            return -EINTR;
-        }
-        // request already processing: interrupt not possible.
-        up(&master->master_sem);
-    }
-
-    // wait until master FSM has finished processing
-    wait_event(request.slave->sdo_queue,
-            request.req.state != EC_INT_REQUEST_BUSY);
-
-    EC_SLAVE_DBG(request.slave, 1, "Finished SDO download request.\n");
-
-    data.abort_code = request.req.abort_code;
-
-    if (request.req.state == EC_INT_REQUEST_SUCCESS) {
-        retval = 0;
-    } else if (request.req.errno) {
-        retval = -request.req.errno;
-    } else {
-        retval = -EIO;
-    }
-
-    if (__copy_to_user((void __user *) arg, &data, sizeof(data))) {
-        retval = -EFAULT;
-    }
-
-    ec_sdo_request_clear(&request.req);
     return retval;
 }
 
diff -r 6d3620f2328f -r 755e6ce823f0 master/master.c
--- a/master/master.c	Thu Sep 15 16:31:43 2011 +0200
+++ b/master/master.c	Thu Sep 15 16:33:13 2011 +0200
@@ -2366,6 +2366,167 @@
 
 /*****************************************************************************/
 
+int ecrt_master_sdo_download(ec_master_t *master, uint16_t slave_position,
+        uint16_t index, uint8_t subindex, uint8_t *data,
+        size_t data_size, uint32_t *abort_code)
+{
+    ec_master_sdo_request_t request;
+
+    EC_MASTER_DBG(master, 1, "%s(master = 0x%p,"
+            " slave_position = %u, index = 0x%04X, subindex = 0x%02X,"
+            " data = 0x%p, data_size = %zu, abort_code = 0x%p)\n",
+            __func__, master, slave_position, index, subindex,
+            data, data_size, abort_code);
+
+    if (!data_size) {
+        EC_MASTER_ERR(master, "Zero data size!\n");
+        return -EINVAL;
+    }
+
+    ec_sdo_request_init(&request.req);
+    ec_sdo_request_address(&request.req, index, subindex);
+    if (ec_sdo_request_alloc(&request.req, data_size)) {
+        ec_sdo_request_clear(&request.req);
+        return -ENOMEM;
+    }
+
+    memcpy(request.req.data, data, data_size);
+    request.req.data_size = data_size;
+    ecrt_sdo_request_write(&request.req);
+
+    if (down_interruptible(&master->master_sem))
+        return -EINTR;
+
+    if (!(request.slave = ec_master_find_slave(master, 0, slave_position))) {
+        up(&master->master_sem);
+        EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position);
+        ec_sdo_request_clear(&request.req);
+        return -EINVAL;
+    }
+
+    EC_SLAVE_DBG(request.slave, 1, "Schedule SDO download request.\n");
+
+    // schedule request.
+    list_add_tail(&request.list, &request.slave->slave_sdo_requests);
+
+    up(&master->master_sem);
+
+    // wait for processing through FSM
+    if (wait_event_interruptible(request.slave->sdo_queue,
+                request.req.state != EC_INT_REQUEST_QUEUED)) {
+        // interrupted by signal
+        down(&master->master_sem);
+        if (request.req.state == EC_INT_REQUEST_QUEUED) {
+            list_del(&request.list);
+            up(&master->master_sem);
+            ec_sdo_request_clear(&request.req);
+            return -EINTR;
+        }
+        // request already processing: interrupt not possible.
+        up(&master->master_sem);
+    }
+
+    // wait until master FSM has finished processing
+    wait_event(request.slave->sdo_queue,
+            request.req.state != EC_INT_REQUEST_BUSY);
+
+    EC_SLAVE_DBG(request.slave, 1, "Finished SDO download request.\n");
+
+    *abort_code = request.req.abort_code;
+
+    if (request.req.state == EC_INT_REQUEST_SUCCESS) {
+        return 0;
+    } else if (request.req.errno) {
+        return -request.req.errno;
+    } else {
+        return -EIO;
+    }
+}
+
+/*****************************************************************************/
+
+int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position,
+        uint16_t index, uint8_t subindex, uint8_t *target,
+        size_t target_size, size_t *result_size, uint32_t *abort_code)
+{
+    ec_master_sdo_request_t request;
+    int retval = 0;
+
+    EC_MASTER_DBG(master, 1, "%s(master = 0x%p,"
+            " slave_position = %u, index = 0x%04X, subindex = 0x%02X,"
+            " target = 0x%p, target_size = %zu, result_size = 0x%p,"
+            " abort_code = 0x%p)\n",
+            __func__, master, slave_position, index, subindex,
+            target, target_size, result_size, abort_code);
+
+    ec_sdo_request_init(&request.req);
+    ec_sdo_request_address(&request.req, index, subindex);
+    ecrt_sdo_request_read(&request.req);
+
+    if (down_interruptible(&master->master_sem)) {
+        return -EINTR;
+    }
+
+    if (!(request.slave = ec_master_find_slave(master, 0, slave_position))) {
+        up(&master->master_sem);
+        ec_sdo_request_clear(&request.req);
+        EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position);
+        return -EINVAL;
+    }
+
+    EC_SLAVE_DBG(request.slave, 1, "Schedule SDO upload request.\n");
+
+    // schedule request.
+    list_add_tail(&request.list, &request.slave->slave_sdo_requests);
+
+    up(&master->master_sem);
+
+    // wait for processing through FSM
+    if (wait_event_interruptible(request.slave->sdo_queue,
+                request.req.state != EC_INT_REQUEST_QUEUED)) {
+        // interrupted by signal
+        down(&master->master_sem);
+        if (request.req.state == EC_INT_REQUEST_QUEUED) {
+            list_del(&request.list);
+            up(&master->master_sem);
+            ec_sdo_request_clear(&request.req);
+            return -EINTR;
+        }
+        // request already processing: interrupt not possible.
+        up(&master->master_sem);
+    }
+
+    // wait until master FSM has finished processing
+    wait_event(request.slave->sdo_queue,
+            request.req.state != EC_INT_REQUEST_BUSY);
+
+    EC_SLAVE_DBG(request.slave, 1, "Finished SDO upload request.\n");
+
+    *abort_code = request.req.abort_code;
+
+    if (request.req.state != EC_INT_REQUEST_SUCCESS) {
+        *result_size = 0;
+        if (request.req.errno) {
+            retval = -request.req.errno;
+        } else {
+            retval = -EIO;
+        }
+    } else {
+        if (request.req.data_size > target_size) {
+            EC_MASTER_ERR(master, "Buffer too small.\n");
+            ec_sdo_request_clear(&request.req);
+            return -EOVERFLOW;
+        }
+        memcpy(target, request.req.data, request.req.data_size);
+        *result_size = request.req.data_size;
+    }
+
+    ec_sdo_request_clear(&request.req);
+    return retval;
+}
+
+/*****************************************************************************/
+
 int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
         uint8_t drive_no, uint16_t idn, uint8_t *data, size_t data_size,
         uint16_t *error_code)
@@ -2554,6 +2715,8 @@
 EXPORT_SYMBOL(ecrt_master_sync_slave_clocks);
 EXPORT_SYMBOL(ecrt_master_sync_monitor_queue);
 EXPORT_SYMBOL(ecrt_master_sync_monitor_process);
+EXPORT_SYMBOL(ecrt_master_sdo_download);
+EXPORT_SYMBOL(ecrt_master_sdo_upload);
 EXPORT_SYMBOL(ecrt_master_write_idn);
 EXPORT_SYMBOL(ecrt_master_read_idn);
 EXPORT_SYMBOL(ecrt_master_reset);