# HG changeset patch # User Florian Pose # Date 1278421797 -7200 # Node ID 024a3c6aa3f76d5e942d0e24a19e2c8a3f9b1a69 # Parent 55a6e3294dabd457c358ba498377981a35600bd2 Implemented ecrt_master_read_idn() and ecrt_master_write_idn() in kernel space. diff -r 55a6e3294dab -r 024a3c6aa3f7 include/ecrt.h --- 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 diff -r 55a6e3294dab -r 024a3c6aa3f7 lib/master.c --- 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; diff -r 55a6e3294dab -r 024a3c6aa3f7 master/cdev.c --- 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; } diff -r 55a6e3294dab -r 024a3c6aa3f7 master/master.c --- 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 */