# HG changeset patch # User Florian Pose # Date 1316095138 -7200 # Node ID ebda087981e12e7547033d016e740e38d6fdef78 # Parent 505871ced7676ca4cf99e9206263babac59ba500 Implemented ecrt_master_sdo_upload() and ecrt_master_sdo_download() for kernel space. diff -r 505871ced767 -r ebda087981e1 include/ecrt.h --- a/include/ecrt.h Thu Sep 15 15:56:49 2011 +0200 +++ b/include/ecrt.h Thu Sep 15 15:58:58 2011 +0200 @@ -701,14 +701,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. */ @@ -720,15 +722,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. */ @@ -741,8 +742,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 505871ced767 -r ebda087981e1 master/cdev.c --- a/master/cdev.c Thu Sep 15 15:56:49 2011 +0200 +++ b/master/cdev.c Thu Sep 15 15:58:58 2011 +0200 @@ -840,182 +840,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; } - request = kmalloc(sizeof(*request), GFP_KERNEL); - if (!request) + 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; - kref_init(&request->refcount); - - 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 (ec_mutex_lock_interruptible(&master->master_mutex)) { - kref_put(&request->refcount, ec_master_sdo_request_release); - return -EINTR; - } - if (!(request->slave = ec_master_find_slave( - master, 0, data.slave_position))) { - ec_mutex_unlock(&master->master_mutex); - EC_MASTER_ERR(master, "Slave %u does not exist!\n", - data.slave_position); - kref_put(&request->refcount, ec_master_sdo_request_release); - return -EINVAL; - } - - EC_SLAVE_DBG(request->slave, 1, "Schedule SDO upload request %p.\n", - request); - - // schedule request. - kref_get(&request->refcount); - list_add_tail(&request->list, &request->slave->slave_sdo_requests); - - ec_mutex_unlock(&master->master_mutex); - - // wait for processing through FSM - if (wait_event_interruptible(request->slave->sdo_queue, - ((request->req.state == EC_INT_REQUEST_SUCCESS) || - (request->req.state == EC_INT_REQUEST_FAILURE)))) { - // interrupted by signal - kref_put(&request->refcount, ec_master_sdo_request_release); - return -EINTR; - } - - EC_SLAVE_DBG(request->slave, 1, "Finished SDO upload request %p.\n", - request); - - 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"); - kref_put(&request->refcount, ec_master_sdo_request_release); - 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)) { - kref_put(&request->refcount, ec_master_sdo_request_release); + 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; } - kref_put(&request->refcount, ec_master_sdo_request_release); - 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; - } - - request = kmalloc(sizeof(*request), GFP_KERNEL); - if (!request) - return -ENOMEM; - kref_init(&request->refcount); - - 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)) { - kref_put(&request->refcount, ec_master_sdo_request_release); - return -ENOMEM; - } - if (copy_from_user(request->req.data, - (void __user *) data.data, data.data_size)) { - kref_put(&request->refcount, ec_master_sdo_request_release); - return -EFAULT; - } - request->req.data_size = data.data_size; - ecrt_sdo_request_write(&request->req); - - if (ec_mutex_lock_interruptible(&master->master_mutex)) { - kref_put(&request->refcount, ec_master_sdo_request_release); - return -EINTR; - } - if (!(request->slave = ec_master_find_slave( - master, 0, data.slave_position))) { - ec_mutex_unlock(&master->master_mutex); - EC_MASTER_ERR(master, "Slave %u does not exist!\n", - data.slave_position); - kref_put(&request->refcount, ec_master_sdo_request_release); - return -EINVAL; - } - - EC_SLAVE_DBG(request->slave, 1, "Schedule SDO download request %p.\n", - request); - - // schedule request. - kref_get(&request->refcount); - list_add_tail(&request->list, &request->slave->slave_sdo_requests); - - ec_mutex_unlock(&master->master_mutex); - - // wait for processing through FSM - if (wait_event_interruptible(request->slave->sdo_queue, - ((request->req.state == EC_INT_REQUEST_SUCCESS) || - (request->req.state == EC_INT_REQUEST_FAILURE)))) { - // interrupted by signal - kref_put(&request->refcount, ec_master_sdo_request_release); - return -EINTR; - } - - EC_SLAVE_DBG(request->slave, 1, "Finished SDO download request %p.\n", - request); - - 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; - } - - kref_put(&request->refcount, ec_master_sdo_request_release); return retval; } diff -r 505871ced767 -r ebda087981e1 master/master.c --- a/master/master.c Thu Sep 15 15:56:49 2011 +0200 +++ b/master/master.c Thu Sep 15 15:58:58 2011 +0200 @@ -418,7 +418,8 @@ wake_up(&master->reg_queue); } - // we must lock the io_mutex here because the slave's fsm_datagram will be unqueued + // we must lock the io_mutex here because the slave's fsm_datagram + // will be unqueued ec_mutex_lock(&master->io_mutex); for (slave = master->slaves; slave < master->slaves + master->slave_count; @@ -443,7 +444,8 @@ { ec_domain_t *domain, *next; - // we must lock the io_mutex here because the domains's datagram will be unqueued + // we must lock the io_mutex here because the domains's datagram + // will be unqueued ec_mutex_lock(&master->io_mutex); list_for_each_entry_safe(domain, next, &master->domains, list) { list_del(&domain->list); @@ -568,7 +570,9 @@ /** Transition function from IDLE to OPERATION phase. */ -int ec_master_enter_operation_phase(ec_master_t *master /**< EtherCAT master */) +int ec_master_enter_operation_phase(i + ec_master_t *master /**< EtherCAT master */ + ) { int ret = 0; ec_slave_t *slave; @@ -1940,7 +1944,8 @@ EC_MASTER_DBG(master, 1, "ecrt_master_create_domain(master = 0x%p)\n", master); - if (!(domain = (ec_domain_t *) kmalloc(sizeof(ec_domain_t), GFP_KERNEL))) { + if (!(domain = + (ec_domain_t *) kmalloc(sizeof(ec_domain_t), GFP_KERNEL))) { EC_MASTER_ERR(master, "Error allocating domain memory!\n"); return ERR_PTR(-ENOMEM); } @@ -2075,8 +2080,8 @@ if (!is_eoe_slave) { ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP); // mark for reconfiguration, because the master could have no - // possibility for a reconfiguration between two sequential operation - // phases. + // possibility for a reconfiguration between two sequential + // operation phases. slave->force_config = 1; } #else @@ -2320,7 +2325,10 @@ /*****************************************************************************/ -void ecrt_master_configured_slaves_state(const ec_master_t *master, ec_master_state_t *state) +void ecrt_master_configured_slaves_state( + const ec_master_t *master, + ec_master_state_t *state + ) { const ec_slave_config_t *sc; ec_slave_config_state_t sc_state; @@ -2344,12 +2352,13 @@ master->app_time = app_time; if (unlikely(!master->has_app_time)) { - EC_MASTER_DBG(master, 1, "set application start time = %llu\n",app_time); - master->app_start_time = app_time; + EC_MASTER_DBG(master, 1, "Set application start time = %llu\n", + app_time); + master->app_start_time = app_time; #ifdef EC_HAVE_CYCLES - master->dc_cycles_app_start_time = get_cycles(); -#endif - master->dc_jiffies_app_start_time = jiffies; + master->dc_cycles_app_start_time = get_cycles(); +#endif + master->dc_jiffies_app_start_time = jiffies; master->has_app_time = 1; } } @@ -2391,6 +2400,160 @@ /*****************************************************************************/ +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; + int retval; + + if (!data_size) { + EC_MASTER_ERR(master, "Zero data size!\n"); + return -EINVAL; + } + + request = kmalloc(sizeof(*request), GFP_KERNEL); + if (!request) { + return -ENOMEM; + } + kref_init(&request->refcount); + + ec_sdo_request_init(&request->req); + ec_sdo_request_address(&request->req, index, subindex); + if (ec_sdo_request_alloc(&request->req, data_size)) { + kref_put(&request->refcount, ec_master_sdo_request_release); + return -ENOMEM; + } + memcpy(request->req.data, data, data_size); + request->req.data_size = data_size; + ecrt_sdo_request_write(&request->req); + + if (ec_mutex_lock_interruptible(&master->master_mutex)) { + kref_put(&request->refcount, ec_master_sdo_request_release); + return -EINTR; + } + + if (!(request->slave = ec_master_find_slave( + master, 0, slave_position))) { + ec_mutex_unlock(&master->master_mutex); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position); + kref_put(&request->refcount, ec_master_sdo_request_release); + return -EINVAL; + } + + EC_SLAVE_DBG(request->slave, 1, "Schedule SDO download request %p.\n", + request); + + // schedule request + kref_get(&request->refcount); + list_add_tail(&request->list, &request->slave->slave_sdo_requests); + + ec_mutex_unlock(&master->master_mutex); + + // wait for processing through FSM + if (wait_event_interruptible(request->slave->sdo_queue, + ((request->req.state == EC_INT_REQUEST_SUCCESS) || + (request->req.state == EC_INT_REQUEST_FAILURE)))) { + // interrupted by signal + kref_put(&request->refcount, ec_master_sdo_request_release); + return -EINTR; + } + + EC_SLAVE_DBG(request->slave, 1, "Finished SDO download request %p.\n", + request); + + *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; + } + + kref_put(&request->refcount, ec_master_sdo_request_release); + return retval; +} + +/*****************************************************************************/ + +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; + + request = kmalloc(sizeof(*request), GFP_KERNEL); + if (!request) + return -ENOMEM; + kref_init(&request->refcount); + + ec_sdo_request_init(&request->req); + ec_sdo_request_address(&request->req, index, subindex); + ecrt_sdo_request_read(&request->req); + + if (ec_mutex_lock_interruptible(&master->master_mutex)) { + kref_put(&request->refcount, ec_master_sdo_request_release); + return -EINTR; + } + + if (!(request->slave = ec_master_find_slave( + master, 0, slave_position))) { + ec_mutex_unlock(&master->master_mutex); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position); + kref_put(&request->refcount, ec_master_sdo_request_release); + return -EINVAL; + } + + EC_SLAVE_DBG(request->slave, 1, "Schedule SDO upload request %p.\n", + request); + + // schedule request + kref_get(&request->refcount); + list_add_tail(&request->list, &request->slave->slave_sdo_requests); + + ec_mutex_unlock(&master->master_mutex); + + // wait for processing through FSM + if (wait_event_interruptible(request->slave->sdo_queue, + ((request->req.state == EC_INT_REQUEST_SUCCESS) || + (request->req.state == EC_INT_REQUEST_FAILURE)))) { + // interrupted by signal + kref_put(&request->refcount, ec_master_sdo_request_release); + return -EINTR; + } + + EC_SLAVE_DBG(request->slave, 1, "Finished SDO upload request %p.\n", + request); + + *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"); + kref_put(&request->refcount, ec_master_sdo_request_release); + return -EOVERFLOW; + } + memcpy(target, request->req.data, request->req.data_size); + *result_size = request->req.data_size; + retval = 0; + } + + kref_put(&request->refcount, ec_master_sdo_request_release); + 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) @@ -2437,7 +2600,8 @@ return -EINVAL; } - EC_SLAVE_DBG(request->slave, 1, "Scheduled SoE write request %p.\n",request); + EC_SLAVE_DBG(request->slave, 1, "Scheduled SoE write request %p.\n", + request); // schedule SoE write request. list_add_tail(&request->list, &request->slave->soe_requests); @@ -2447,7 +2611,8 @@ // wait for processing through FSM if (wait_event_interruptible(request->slave->soe_queue, - ((request->req.state == EC_INT_REQUEST_SUCCESS) || (request->req.state == EC_INT_REQUEST_FAILURE)))) { + ((request->req.state == EC_INT_REQUEST_SUCCESS) || + (request->req.state == EC_INT_REQUEST_FAILURE)))) { // interrupted by signal kref_put(&request->refcount,ec_master_soe_request_release); return -EINTR; @@ -2504,11 +2669,13 @@ ec_mutex_unlock(&master->master_mutex); - EC_SLAVE_DBG(request->slave, 1, "Scheduled SoE read request %p.\n",request); + EC_SLAVE_DBG(request->slave, 1, "Scheduled SoE read request %p.\n", + request); // wait for processing through FSM if (wait_event_interruptible(request->slave->soe_queue, - ((request->req.state == EC_INT_REQUEST_SUCCESS) || (request->req.state == EC_INT_REQUEST_FAILURE)))) { + ((request->req.state == EC_INT_REQUEST_SUCCESS) || + (request->req.state == EC_INT_REQUEST_FAILURE)))) { // interrupted by signal kref_put(&request->refcount,ec_master_soe_request_release); return -EINTR; @@ -2518,8 +2685,8 @@ *error_code = request->req.error_code; } - EC_SLAVE_DBG(request->slave, 1, "SoE request %p read %zd bytes via SoE.\n", - request,request->req.data_size); + EC_SLAVE_DBG(request->slave, 1, "SoE request %p read %zd bytes" + " via SoE.\n", request, request->req.data_size); if (request->req.state != EC_INT_REQUEST_SUCCESS) { if (result_size) { @@ -2574,6 +2741,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);