Implemented ecrt_master_read_idn() and ecrt_master_write_idn() in kernel space.
--- a/include/ecrt.h Sun Jun 06 23:21:09 2010 +0200
+++ b/include/ecrt.h Tue Jul 06 15:09:57 2010 +0200
@@ -674,6 +674,8 @@
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
@@ -688,7 +690,7 @@
uint16_t idn, /**< SoE IDN (see ecrt_slave_config_idn()). */
uint8_t *data, /**< Pointer to data to write. */
size_t data_size, /**< Size of data to write. */
- uint32_t *error_code /**< Pointer to variable, where an SoE error code
+ uint16_t *error_code /**< Pointer to variable, where an SoE error code
can be stored. */
);
@@ -708,12 +710,10 @@
stored. */
size_t target_size, /**< Size of the memory \a target points to. */
size_t *result_size, /**< Actual size of the received data. */
- uint32_t *error_code /**< Pointer to variable, where an SoE error code
+ uint16_t *error_code /**< Pointer to variable, where an SoE error code
can be stored. */
);
-#endif /* #ifndef __KERNEL__ */
-
/** Finishes the configuration phase and prepares for cyclic operation.
*
* This function tells the master that the configuration phase is finished and
--- a/lib/master.c Sun Jun 06 23:21:09 2010 +0200
+++ b/lib/master.c Tue Jul 06 15:09:57 2010 +0200
@@ -305,7 +305,7 @@
/*****************************************************************************/
int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
- uint16_t idn, uint8_t *data, size_t data_size, uint32_t *error_code)
+ uint16_t idn, uint8_t *data, size_t data_size, uint16_t *error_code)
{
ec_ioctl_slave_soe_write_t io;
@@ -329,7 +329,7 @@
int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position,
uint16_t idn, uint8_t *target, size_t target_size,
- size_t *result_size, uint32_t *error_code)
+ size_t *result_size, uint16_t *error_code)
{
ec_ioctl_slave_soe_read_t io;
--- a/master/cdev.c Sun Jun 06 23:21:09 2010 +0200
+++ b/master/cdev.c Tue Jul 06 15:09:57 2010 +0200
@@ -3319,86 +3319,40 @@
)
{
ec_ioctl_slave_soe_read_t ioctl;
- ec_master_soe_request_t request;
+ u8 *data;
int retval;
if (copy_from_user(&ioctl, (void __user *) arg, sizeof(ioctl))) {
return -EFAULT;
}
- ec_soe_request_init(&request.req);
- ec_soe_request_set_idn(&request.req, ioctl.idn);
- ec_soe_request_read(&request.req);
-
- if (down_interruptible(&master->master_sem))
- return -EINTR;
-
- if (!(request.slave = ec_master_find_slave(
- master, 0, ioctl.slave_position))) {
- up(&master->master_sem);
- ec_soe_request_clear(&request.req);
- EC_MASTER_ERR(master, "Slave %u does not exist!\n",
- ioctl.slave_position);
- return -EINVAL;
- }
-
- // schedule request.
- list_add_tail(&request.list, &request.slave->soe_requests);
-
- up(&master->master_sem);
-
- EC_SLAVE_DBG(request.slave, 1, "Scheduled SoE read request.\n");
-
- // wait for processing through FSM
- if (wait_event_interruptible(request.slave->soe_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_soe_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->soe_queue,
- request.req.state != EC_INT_REQUEST_BUSY);
-
- ioctl.error_code = request.req.error_code;
-
- EC_SLAVE_DBG(request.slave, 1, "Read %zd bytes via SoE.\n",
- request.req.data_size);
-
- if (request.req.state != EC_INT_REQUEST_SUCCESS) {
- ioctl.data_size = 0;
- retval = -EIO;
- } else {
- if (request.req.data_size > ioctl.mem_size) {
- EC_MASTER_ERR(master, "Buffer too small.\n");
- ec_soe_request_clear(&request.req);
- return -EOVERFLOW;
- }
- ioctl.data_size = request.req.data_size;
- if (copy_to_user((void __user *) ioctl.data,
- request.req.data, ioctl.data_size)) {
- ec_soe_request_clear(&request.req);
- return -EFAULT;
- }
- retval = 0;
- }
+ data = kmalloc(ioctl.mem_size, GFP_KERNEL);
+ if (!data) {
+ EC_MASTER_ERR(master, "Failed to allocate %u bytes of IDN data.\n",
+ ioctl.mem_size);
+ return -ENOMEM;
+ }
+
+ retval = ecrt_master_read_idn(master, ioctl.slave_position,
+ ioctl.idn, data, ioctl.mem_size, &ioctl.data_size,
+ &ioctl.error_code);
+ if (retval) {
+ kfree(data);
+ return retval;
+ }
+
+ if (copy_to_user((void __user *) ioctl.data,
+ data, ioctl.data_size)) {
+ kfree(data);
+ return -EFAULT;
+ }
+ kfree(data);
if (__copy_to_user((void __user *) arg, &ioctl, sizeof(ioctl))) {
retval = -EFAULT;
}
- EC_SLAVE_DBG(request.slave, 1, "Finished SoE read request.\n");
-
- ec_soe_request_clear(&request.req);
-
+ EC_MASTER_DBG(master, 1, "Finished SoE read request.\n");
return retval;
}
@@ -3412,79 +3366,36 @@
)
{
ec_ioctl_slave_soe_write_t ioctl;
- ec_master_soe_request_t request;
+ u8 *data;
int retval;
if (copy_from_user(&ioctl, (void __user *) arg, sizeof(ioctl))) {
return -EFAULT;
}
- INIT_LIST_HEAD(&request.list);
-
- ec_soe_request_init(&request.req);
- ec_soe_request_set_idn(&request.req, ioctl.idn);
-
- if (ec_soe_request_alloc(&request.req, ioctl.data_size)) {
- ec_soe_request_clear(&request.req);
+ data = kmalloc(ioctl.data_size, GFP_KERNEL);
+ if (!data) {
+ EC_MASTER_ERR(master, "Failed to allocate %u bytes of IDN data.\n",
+ ioctl.data_size);
return -ENOMEM;
}
- if (copy_from_user(request.req.data,
- (void __user *) ioctl.data, ioctl.data_size)) {
- ec_soe_request_clear(&request.req);
- return -EFAULT;
- }
- request.req.data_size = ioctl.data_size;
- ec_soe_request_write(&request.req);
-
- if (down_interruptible(&master->master_sem))
- return -EINTR;
-
- if (!(request.slave = ec_master_find_slave(
- master, 0, ioctl.slave_position))) {
- up(&master->master_sem);
- EC_MASTER_ERR(master, "Slave %u does not exist!\n",
- ioctl.slave_position);
- ec_soe_request_clear(&request.req);
- return -EINVAL;
- }
-
- EC_SLAVE_DBG(request.slave, 1, "Scheduling SoE write request.\n");
-
- // schedule SoE write request.
- list_add_tail(&request.list, &request.slave->soe_requests);
-
- up(&master->master_sem);
-
- // wait for processing through FSM
- if (wait_event_interruptible(request.slave->soe_queue,
- request.req.state != EC_INT_REQUEST_QUEUED)) {
- // interrupted by signal
- down(&master->master_sem);
- if (request.req.state == EC_INT_REQUEST_QUEUED) {
- // abort request
- list_del(&request.list);
- up(&master->master_sem);
- ec_soe_request_clear(&request.req);
- return -EINTR;
- }
- up(&master->master_sem);
- }
-
- // wait until master FSM has finished processing
- wait_event(request.slave->soe_queue,
- request.req.state != EC_INT_REQUEST_BUSY);
-
- ioctl.error_code = request.req.error_code;
- retval = request.req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
+ if (copy_from_user(data, (void __user *) ioctl.data, ioctl.data_size)) {
+ kfree(data);
+ return -EFAULT;
+ }
+
+ retval = ecrt_master_write_idn(master, ioctl.slave_position,
+ ioctl.idn, data, ioctl.data_size, &ioctl.error_code);
+ kfree(data);
+ if (retval) {
+ return retval;
+ }
if (__copy_to_user((void __user *) arg, &ioctl, sizeof(ioctl))) {
retval = -EFAULT;
}
- ec_soe_request_clear(&request.req);
-
- EC_SLAVE_DBG(request.slave, 1, "Finished SoE write request.\n");
-
+ EC_MASTER_DBG(master, 1, "Finished SoE write request.\n");
return retval;
}
--- a/master/master.c Sun Jun 06 23:21:09 2010 +0200
+++ b/master/master.c Tue Jul 06 15:09:57 2010 +0200
@@ -2344,6 +2344,150 @@
/*****************************************************************************/
+int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
+ uint16_t idn, uint8_t *data, size_t data_size, uint16_t *error_code)
+{
+ ec_master_soe_request_t request;
+ int retval;
+
+ INIT_LIST_HEAD(&request.list);
+ ec_soe_request_init(&request.req);
+ ec_soe_request_set_idn(&request.req, idn);
+
+ if (ec_soe_request_alloc(&request.req, data_size)) {
+ ec_soe_request_clear(&request.req);
+ return -ENOMEM;
+ }
+
+ memcpy(request.req.data, data, data_size);
+ request.req.data_size = data_size;
+ ec_soe_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_soe_request_clear(&request.req);
+ return -EINVAL;
+ }
+
+ EC_SLAVE_DBG(request.slave, 1, "Scheduling SoE write request.\n");
+
+ // schedule SoE write request.
+ list_add_tail(&request.list, &request.slave->soe_requests);
+
+ up(&master->master_sem);
+
+ // wait for processing through FSM
+ if (wait_event_interruptible(request.slave->soe_queue,
+ request.req.state != EC_INT_REQUEST_QUEUED)) {
+ // interrupted by signal
+ down(&master->master_sem);
+ if (request.req.state == EC_INT_REQUEST_QUEUED) {
+ // abort request
+ list_del(&request.list);
+ up(&master->master_sem);
+ ec_soe_request_clear(&request.req);
+ return -EINTR;
+ }
+ up(&master->master_sem);
+ }
+
+ // wait until master FSM has finished processing
+ wait_event(request.slave->soe_queue,
+ request.req.state != EC_INT_REQUEST_BUSY);
+
+ if (error_code) {
+ *error_code = request.req.error_code;
+ }
+ retval = request.req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
+ ec_soe_request_clear(&request.req);
+
+ return retval;
+}
+
+/*****************************************************************************/
+
+int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position,
+ uint16_t idn, uint8_t *target, size_t target_size,
+ size_t *result_size, uint16_t *error_code)
+{
+ ec_master_soe_request_t request;
+
+ INIT_LIST_HEAD(&request.list);
+ ec_soe_request_init(&request.req);
+ ec_soe_request_set_idn(&request.req, idn);
+ ec_soe_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_soe_request_clear(&request.req);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position);
+ return -EINVAL;
+ }
+
+ // schedule request.
+ list_add_tail(&request.list, &request.slave->soe_requests);
+
+ up(&master->master_sem);
+
+ EC_SLAVE_DBG(request.slave, 1, "Scheduled SoE read request.\n");
+
+ // wait for processing through FSM
+ if (wait_event_interruptible(request.slave->soe_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_soe_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->soe_queue,
+ request.req.state != EC_INT_REQUEST_BUSY);
+
+ if (error_code) {
+ *error_code = request.req.error_code;
+ }
+
+ EC_SLAVE_DBG(request.slave, 1, "Read %zd bytes via SoE.\n",
+ request.req.data_size);
+
+ if (request.req.state != EC_INT_REQUEST_SUCCESS) {
+ if (result_size) {
+ *result_size = 0;
+ }
+ ec_soe_request_clear(&request.req);
+ return -EIO;
+ } else {
+ if (request.req.data_size > target_size) {
+ EC_MASTER_ERR(master, "Buffer too small.\n");
+ ec_soe_request_clear(&request.req);
+ return -EOVERFLOW;
+ }
+ if (result_size) {
+ *result_size = request.req.data_size;
+ }
+ memcpy(target, request.req.data, request.req.data_size);
+ return 0;
+ }
+}
+
+/*****************************************************************************/
+
/** \cond */
EXPORT_SYMBOL(ecrt_master_create_domain);
@@ -2362,6 +2506,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_write_idn);
+EXPORT_SYMBOL(ecrt_master_read_idn);
/** \endcond */