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);