From bbe96b32469398e6297d1a4612a84fdaadc91a98 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 15 Sep 2011 16:33:13 +0200 Subject: [PATCH] Implemented ecrt_master_sdo_upload() and ecrt_master_sdo_download() also for kernel space. --- include/ecrt.h | 25 ++++---- master/cdev.c | 160 ++++++++--------------------------------------- master/master.c | 163 ++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 202 insertions(+), 146 deletions(-) diff --git a/include/ecrt.h b/include/ecrt.h index 2cf1dbe3..418a26de 100644 --- a/include/ecrt.h +++ b/include/ecrt.h @@ -635,14 +635,16 @@ int ecrt_master_get_pdo_entry( ec_pdo_entry_info_t *entry /**< Pointer to output structure. */ ); -/** Executes an SDO write request to download data. +#endif /* #ifndef __KERNEL__ */ + +/** Executes an SDO download request to write data to a slave. * - * 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. + * 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 @@ int ecrt_master_sdo_download( uint32_t *abort_code /**< Abort code of the SDO download. */ ); -/** Executes a SDO read request to upload data. +/** Executes an SDO upload request to read data from a slave. * - * 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. + * 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 @@ int ecrt_master_sdo_upload( 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 --git a/master/cdev.c b/master/cdev.c index 76ebb7c5..3f848da0 100644 --- a/master/cdev.c +++ b/master/cdev.c @@ -838,89 +838,38 @@ int ec_cdev_ioctl_slave_sdo_upload( ) { 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; + 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; } - 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; + 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))) { - retval = -EFAULT; + return -EFAULT; } - ec_sdo_request_clear(&request.req); - return retval; + return 0; } /*****************************************************************************/ @@ -933,89 +882,34 @@ int ec_cdev_ioctl_slave_sdo_download( ) { ec_ioctl_slave_sdo_download_t data; - ec_master_sdo_request_t request; + uint8_t *sdo_data; 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); + 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(request.req.data, - (void __user *) data.data, data.data_size)) { - ec_sdo_request_clear(&request.req); + + if (copy_from_user(sdo_data, (void __user *) data.data, data.data_size)) { + kfree(sdo_data); return -EFAULT; } - request.req.data_size = data.data_size; - ecrt_sdo_request_write(&request.req); - if (down_interruptible(&master->master_sem)) - return -EINTR; + retval = ecrt_master_sdo_download(master, data.slave_position, + data.sdo_index, data.sdo_entry_subindex, sdo_data, data.data_size, + &data.abort_code); - 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; - } + kfree(sdo_data); if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { retval = -EFAULT; } - ec_sdo_request_clear(&request.req); return retval; } diff --git a/master/master.c b/master/master.c index 264eff9f..821e9717 100644 --- a/master/master.c +++ b/master/master.c @@ -2366,6 +2366,167 @@ uint32_t ecrt_master_sync_monitor_process(ec_master_t *master) /*****************************************************************************/ +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_reference_clock); 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);