diff --git a/master/cdev.c b/master/cdev.c index e84288f3..8beee98c 100644 --- a/master/cdev.c +++ b/master/cdev.c @@ -1072,7 +1072,8 @@ int ec_cdev_ioctl_slave_sii_write( ec_slave_t *slave; unsigned int byte_size; uint16_t *words; - ec_sii_write_request_t request; + ec_sii_write_request_t* request; + int retval; if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { return -EFAULT; @@ -1088,58 +1089,57 @@ int ec_cdev_ioctl_slave_sii_write( return -ENOMEM; } + request = kmalloc(sizeof(*request), GFP_KERNEL); + if (!request) + return -ENOMEM; + kref_init(&request->refcount); + // init SII write request + INIT_LIST_HEAD(&request->list); + request->words = words; // now "owned" by request, see ec_master_sii_write_request_release + request->offset = data.offset; + request->nwords = data.nwords; + if (copy_from_user(words, (void __user *) data.words, byte_size)) { - kfree(words); + kref_put(&request->refcount,ec_master_sii_write_request_release); return -EFAULT; } - if (ec_mutex_lock_interruptible(&master->master_mutex)) + if (ec_mutex_lock_interruptible(&master->master_mutex)) { + kref_put(&request->refcount,ec_master_sii_write_request_release); return -EINTR; - + } if (!(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); - kfree(words); + kref_put(&request->refcount,ec_master_sii_write_request_release); return -EINVAL; } - // init SII write request - INIT_LIST_HEAD(&request.list); - request.slave = slave; - request.words = words; - request.offset = data.offset; - request.nwords = data.nwords; - request.state = EC_INT_REQUEST_QUEUED; + request->slave = slave; + request->state = EC_INT_REQUEST_QUEUED; // schedule SII write request. - list_add_tail(&request.list, &master->sii_requests); + list_add_tail(&request->list, &master->sii_requests); + kref_get(&request->refcount); ec_mutex_unlock(&master->master_mutex); // wait for processing through FSM if (wait_event_interruptible(master->sii_queue, - request.state != EC_INT_REQUEST_QUEUED)) { - // interrupted by signal - ec_mutex_lock(&master->master_mutex); - if (request.state == EC_INT_REQUEST_QUEUED) { - // abort request - list_del(&request.list); - ec_mutex_unlock(&master->master_mutex); - kfree(words); - return -EINTR; - } - ec_mutex_unlock(&master->master_mutex); + ((request->state == EC_INT_REQUEST_SUCCESS) || (request->state == EC_INT_REQUEST_FAILURE)))) { + // interrupted by signal + kref_put(&request->refcount,ec_master_sii_write_request_release); + return -EINTR; } - // wait until master FSM has finished processing - wait_event(master->sii_queue, request.state != EC_INT_REQUEST_BUSY); - kfree(words); + retval = request->state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO; + kref_put(&request->refcount,ec_master_sii_write_request_release); - return request.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO; + return retval; } /*****************************************************************************/ diff --git a/master/fsm_master.c b/master/fsm_master.c index 23082b43..584224f2 100644 --- a/master/fsm_master.c +++ b/master/fsm_master.c @@ -1068,6 +1068,7 @@ void ec_fsm_master_state_write_sii( if (!ec_fsm_sii_success(&fsm->fsm_sii)) { EC_SLAVE_ERR(slave, "Failed to write SII data.\n"); request->state = EC_INT_REQUEST_FAILURE; + kref_put(&request->refcount,ec_master_sii_write_request_release); wake_up(&master->sii_queue); ec_fsm_master_restart(fsm); return; @@ -1096,6 +1097,7 @@ void ec_fsm_master_state_write_sii( // TODO: Evaluate other SII contents! request->state = EC_INT_REQUEST_SUCCESS; + kref_put(&request->refcount,ec_master_sii_write_request_release); wake_up(&master->sii_queue); // check for another SII write request @@ -1228,6 +1230,19 @@ void ec_fsm_master_state_reg_request( /*****************************************************************************/ +/** called by kref_put if the SII write request's refcount becomes zero. + * + */ +void ec_master_sii_write_request_release(struct kref *ref) +{ + ec_sii_write_request_t *request = container_of(ref, ec_sii_write_request_t, refcount); + EC_SLAVE_DBG(request->slave, 1, "Releasing SII write request %p.\n",request); + kfree(request->words); + kfree(request); +} + +/*****************************************************************************/ + /** called by kref_put if the SDO request's refcount becomes zero. * */ diff --git a/master/fsm_master.h b/master/fsm_master.h index 02c8467d..85e0c839 100644 --- a/master/fsm_master.h +++ b/master/fsm_master.h @@ -57,8 +57,11 @@ typedef struct { size_t nwords; /**< Number of words. */ const uint16_t *words; /**< Pointer to the data words. */ ec_internal_request_state_t state; /**< State of the request. */ + struct kref refcount; } ec_sii_write_request_t; +void ec_master_sii_write_request_release(struct kref *); + /*****************************************************************************/ /** Register request. diff --git a/master/master.c b/master/master.c index a664ede2..9e709ea9 100644 --- a/master/master.c +++ b/master/master.c @@ -397,6 +397,7 @@ void ec_master_clear_slaves(ec_master_t *master) EC_MASTER_WARN(master, "Discarding SII request, slave %u about" " to be deleted.\n", request->slave->ring_position); request->state = EC_INT_REQUEST_FAILURE; + kref_put(&request->refcount,ec_master_sii_write_request_release); wake_up(&master->sii_queue); }