diff -r 86ebf18a029f -r 2c3ccdde3919 master/ioctl.c --- a/master/ioctl.c Wed Nov 14 22:08:32 2012 +0100 +++ b/master/ioctl.c Wed Nov 14 22:12:57 2012 +0100 @@ -977,51 +977,48 @@ void *arg /**< ioctl() argument. */ ) { - ec_ioctl_slave_reg_t data; + ec_ioctl_slave_reg_t io; ec_slave_t *slave; - uint8_t *contents; ec_reg_request_t request; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - return -EFAULT; - } - - if (!data.length) + int ret; + + if (copy_from_user(&io, (void __user *) arg, sizeof(io))) { + return -EFAULT; + } + + if (!io.size) { return 0; - - if (!(contents = kmalloc(data.length, GFP_KERNEL))) { - EC_MASTER_ERR(master, "Failed to allocate %u bytes" - " for register data.\n", data.length); - return -ENOMEM; - } - - if (down_interruptible(&master->master_sem)) + } + + // init register request + ret = ec_reg_request_init(&request, io.size); + if (ret) { + return ret; + } + + ecrt_reg_request_read(&request, io.address, io.size); + + if (down_interruptible(&master->master_sem)) { + ec_reg_request_clear(&request); return -EINTR; + } if (!(slave = ec_master_find_slave( - master, 0, data.slave_position))) { - up(&master->master_sem); + master, 0, io.slave_position))) { + up(&master->master_sem); + ec_reg_request_clear(&request); EC_MASTER_ERR(master, "Slave %u does not exist!\n", - data.slave_position); + io.slave_position); return -EINVAL; } - // init register request - INIT_LIST_HEAD(&request.list); - request.slave = slave; - request.dir = EC_DIR_INPUT; - request.data = contents; - request.offset = data.offset; - request.length = data.length; - request.state = EC_INT_REQUEST_QUEUED; - // schedule request. - list_add_tail(&request.list, &master->reg_requests); + list_add_tail(&request.list, &slave->reg_requests); up(&master->master_sem); // wait for processing through FSM - if (wait_event_interruptible(master->reg_queue, + if (wait_event_interruptible(slave->reg_queue, request.state != EC_INT_REQUEST_QUEUED)) { // interrupted by signal down(&master->master_sem); @@ -1029,20 +1026,21 @@ // abort request list_del(&request.list); up(&master->master_sem); - kfree(contents); + ec_reg_request_clear(&request); return -EINTR; } up(&master->master_sem); } // wait until master FSM has finished processing - wait_event(master->reg_queue, request.state != EC_INT_REQUEST_BUSY); + wait_event(slave->reg_queue, request.state != EC_INT_REQUEST_BUSY); if (request.state == EC_INT_REQUEST_SUCCESS) { - if (copy_to_user((void __user *) data.data, contents, data.length)) + if (copy_to_user((void __user *) io.data, request.data, io.size)) { return -EFAULT; - } - kfree(contents); + } + } + ec_reg_request_clear(&request); return request.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO; } @@ -1056,57 +1054,53 @@ void *arg /**< ioctl() argument. */ ) { - ec_ioctl_slave_reg_t data; + ec_ioctl_slave_reg_t io; ec_slave_t *slave; - uint8_t *contents; ec_reg_request_t request; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - return -EFAULT; - } - - if (!data.length) + int ret; + + if (copy_from_user(&io, (void __user *) arg, sizeof(io))) { + return -EFAULT; + } + + if (!io.size) { return 0; - - if (!(contents = kmalloc(data.length, GFP_KERNEL))) { - EC_MASTER_ERR(master, "Failed to allocate %u bytes" - " for register data.\n", data.length); - return -ENOMEM; - } - - if (copy_from_user(contents, (void __user *) data.data, data.length)) { - kfree(contents); - return -EFAULT; - } - - if (down_interruptible(&master->master_sem)) + } + + // init register request + ret = ec_reg_request_init(&request, io.size); + if (ret) { + return ret; + } + + if (copy_from_user(request.data, (void __user *) io.data, io.size)) { + ec_reg_request_clear(&request); + return -EFAULT; + } + + ecrt_reg_request_write(&request, io.address, io.size); + + if (down_interruptible(&master->master_sem)) { + ec_reg_request_clear(&request); return -EINTR; + } if (!(slave = ec_master_find_slave( - master, 0, data.slave_position))) { - up(&master->master_sem); + master, 0, io.slave_position))) { + up(&master->master_sem); + ec_reg_request_clear(&request); EC_MASTER_ERR(master, "Slave %u does not exist!\n", - data.slave_position); - kfree(contents); + io.slave_position); return -EINVAL; } - // init register request - INIT_LIST_HEAD(&request.list); - request.slave = slave; - request.dir = EC_DIR_OUTPUT; - request.data = contents; - request.offset = data.offset; - request.length = data.length; - request.state = EC_INT_REQUEST_QUEUED; - // schedule request. - list_add_tail(&request.list, &master->reg_requests); + list_add_tail(&request.list, &slave->reg_requests); up(&master->master_sem); // wait for processing through FSM - if (wait_event_interruptible(master->reg_queue, + if (wait_event_interruptible(slave->reg_queue, request.state != EC_INT_REQUEST_QUEUED)) { // interrupted by signal down(&master->master_sem); @@ -1114,16 +1108,16 @@ // abort request list_del(&request.list); up(&master->master_sem); - kfree(contents); + ec_reg_request_clear(&request); return -EINTR; } up(&master->master_sem); } // wait until master FSM has finished processing - wait_event(master->reg_queue, request.state != EC_INT_REQUEST_BUSY); - - kfree(contents); + wait_event(slave->reg_queue, request.state != EC_INT_REQUEST_BUSY); + + ec_reg_request_clear(&request); return request.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO; } @@ -1608,7 +1602,7 @@ /* Set the memory as external process data memory for the * domains. - */ + */ offset = 0; list_for_each_entry(domain, &master->domains, list) { ecrt_domain_external_memory(domain, @@ -1617,10 +1611,10 @@ } #ifdef EC_IOCTL_RTDM - /* RTDM uses a different approach for memory-mapping, which has to be - * initiated by the kernel. - */ - ret = ec_rtdm_mmap(ctx, &io.process_data); + /* RTDM uses a different approach for memory-mapping, which has to be + * initiated by the kernel. + */ + ret = ec_rtdm_mmap(ctx, &io.process_data); if (ret < 0) { EC_MASTER_ERR(master, "Failed to map process data" " memory to user space (code %i).\n", ret); @@ -2488,6 +2482,58 @@ /*****************************************************************************/ +/** Create a register request. + */ +static int ec_ioctl_sc_create_reg_request( + ec_master_t *master, /**< EtherCAT master. */ + void *arg, /**< ioctl() argument. */ + ec_ioctl_context_t *ctx /**< Private data structure of file handle. */ + ) +{ + ec_ioctl_reg_request_t io; + ec_slave_config_t *sc; + ec_reg_request_t *reg; + + if (unlikely(!ctx->requested)) { + return -EPERM; + } + + if (copy_from_user(&io, (void __user *) arg, sizeof(io))) { + return -EFAULT; + } + + io.request_index = 0; + + if (down_interruptible(&master->master_sem)) { + return -EINTR; + } + + sc = ec_master_get_config(master, io.config_index); + if (!sc) { + up(&master->master_sem); + return -ENOENT; + } + + list_for_each_entry(reg, &sc->reg_requests, list) { + io.request_index++; + } + + up(&master->master_sem); /** \fixme sc could be invalidated */ + + reg = ecrt_slave_config_create_reg_request_err(sc, io.mem_size); + if (IS_ERR(reg)) { + return PTR_ERR(reg); + } + + if (copy_to_user((void __user *) arg, &io, sizeof(io))) { + return -EFAULT; + } + + return 0; +} + +/*****************************************************************************/ + /** Create a VoE handler. */ static int ec_ioctl_sc_create_voe_handler( @@ -2979,6 +3025,181 @@ /*****************************************************************************/ +/** Read register data. + */ +static int ec_ioctl_reg_request_data( + ec_master_t *master, /**< EtherCAT master. */ + void *arg, /**< ioctl() argument. */ + ec_ioctl_context_t *ctx /**< Private data structure of file handle. */ + ) +{ + ec_ioctl_reg_request_t io; + ec_slave_config_t *sc; + ec_reg_request_t *reg; + + if (unlikely(!ctx->requested)) { + return -EPERM; + } + + if (copy_from_user(&io, (void __user *) arg, sizeof(io))) { + return -EFAULT; + } + + if (io.mem_size <= 0) { + return 0; + } + + /* no locking of master_sem needed, because neither sc nor reg will not be + * deleted in the meantime. */ + + if (!(sc = ec_master_get_config(master, io.config_index))) { + return -ENOENT; + } + + if (!(reg = ec_slave_config_find_reg_request(sc, io.request_index))) { + return -ENOENT; + } + + if (copy_to_user((void __user *) io.data, ecrt_reg_request_data(reg), + min(reg->mem_size, io.mem_size))) { + return -EFAULT; + } + + return 0; +} + +/*****************************************************************************/ + +/** Gets an register request's state. + */ +static int ec_ioctl_reg_request_state( + ec_master_t *master, /**< EtherCAT master. */ + void *arg, /**< ioctl() argument. */ + ec_ioctl_context_t *ctx /**< Private data structure of file handle. */ + ) +{ + ec_ioctl_reg_request_t io; + ec_slave_config_t *sc; + ec_reg_request_t *reg; + + if (unlikely(!ctx->requested)) { + return -EPERM; + } + + if (copy_from_user(&io, (void __user *) arg, sizeof(io))) { + return -EFAULT; + } + + /* no locking of master_sem needed, because neither sc nor reg will not be + * deleted in the meantime. */ + + if (!(sc = ec_master_get_config(master, io.config_index))) { + return -ENOENT; + } + + if (!(reg = ec_slave_config_find_reg_request(sc, io.request_index))) { + return -ENOENT; + } + + io.state = ecrt_reg_request_state(reg); + io.new_data = io.state == EC_REQUEST_SUCCESS && reg->dir == EC_DIR_INPUT; + + if (copy_to_user((void __user *) arg, &io, sizeof(io))) { + return -EFAULT; + } + + return 0; +} + +/*****************************************************************************/ + +/** Starts an register write operation. + */ +static int ec_ioctl_reg_request_write( + ec_master_t *master, /**< EtherCAT master. */ + void *arg, /**< ioctl() argument. */ + ec_ioctl_context_t *ctx /**< Private data structure of file handle. */ + ) +{ + ec_ioctl_reg_request_t io; + ec_slave_config_t *sc; + ec_reg_request_t *reg; + + if (unlikely(!ctx->requested)) { + return -EPERM; + } + + if (copy_from_user(&io, (void __user *) arg, sizeof(io))) { + return -EFAULT; + } + + /* no locking of master_sem needed, because neither sc nor reg will not be + * deleted in the meantime. */ + + if (!(sc = ec_master_get_config(master, io.config_index))) { + return -ENOENT; + } + + if (!(reg = ec_slave_config_find_reg_request(sc, io.request_index))) { + return -ENOENT; + } + + if (io.transfer_size > reg->mem_size) { + return -EOVERFLOW; + } + + if (copy_from_user(reg->data, (void __user *) io.data, + io.transfer_size)) { + return -EFAULT; + } + + ecrt_reg_request_write(reg, io.address, io.transfer_size); + return 0; +} + +/*****************************************************************************/ + +/** Starts an register read operation. + */ +static int ec_ioctl_reg_request_read( + ec_master_t *master, /**< EtherCAT master. */ + void *arg, /**< ioctl() argument. */ + ec_ioctl_context_t *ctx /**< Private data structure of file handle. */ + ) +{ + ec_ioctl_reg_request_t io; + ec_slave_config_t *sc; + ec_reg_request_t *reg; + + if (unlikely(!ctx->requested)) { + return -EPERM; + } + + if (copy_from_user(&io, (void __user *) arg, sizeof(io))) { + return -EFAULT; + } + + /* no locking of master_sem needed, because neither sc nor reg will not be + * deleted in the meantime. */ + + if (!(sc = ec_master_get_config(master, io.config_index))) { + return -ENOENT; + } + + if (!(reg = ec_slave_config_find_reg_request(sc, io.request_index))) { + return -ENOENT; + } + + if (io.transfer_size > reg->mem_size) { + return -EOVERFLOW; + } + + ecrt_reg_request_read(reg, io.address, io.transfer_size); + return 0; +} + +/*****************************************************************************/ + /** Sets the VoE send header. */ static int ec_ioctl_voe_send_header( @@ -3548,7 +3769,7 @@ /** Called when an ioctl() command is issued. */ long EC_IOCTL(ec_master_t *master, ec_ioctl_context_t *ctx, - unsigned int cmd, void *arg) + unsigned int cmd, void *arg) { #if DEBUG_LATENCY cycles_t a = get_cycles(), b; @@ -3876,6 +4097,13 @@ } ret = ec_ioctl_sc_create_sdo_request(master, arg, ctx); break; + case EC_IOCTL_SC_REG_REQUEST: + if (!ctx->writable) { + ret = -EPERM; + break; + } + ret = ec_ioctl_sc_create_reg_request(master, arg, ctx); + break; case EC_IOCTL_SC_VOE: if (!ctx->writable) { ret = -EPERM; @@ -3947,6 +4175,26 @@ case EC_IOCTL_SDO_REQUEST_DATA: ret = ec_ioctl_sdo_request_data(master, arg, ctx); break; + case EC_IOCTL_REG_REQUEST_DATA: + ret = ec_ioctl_reg_request_data(master, arg, ctx); + break; + case EC_IOCTL_REG_REQUEST_STATE: + ret = ec_ioctl_reg_request_state(master, arg, ctx); + break; + case EC_IOCTL_REG_REQUEST_WRITE: + if (!ctx->writable) { + ret = -EPERM; + break; + } + ret = ec_ioctl_reg_request_write(master, arg, ctx); + break; + case EC_IOCTL_REG_REQUEST_READ: + if (!ctx->writable) { + ret = -EPERM; + break; + } + ret = ec_ioctl_reg_request_read(master, arg, ctx); + break; case EC_IOCTL_VOE_SEND_HEADER: if (!ctx->writable) { ret = -EPERM;