diff -r fce58ba8a912 -r ef1266652c4d master/cdev.c --- a/master/cdev.c Wed Jul 02 09:22:55 2008 +0000 +++ b/master/cdev.c Wed Jul 02 11:26:51 2008 +0000 @@ -99,6 +99,1098 @@ cdev_del(&cdev->cdev); } +/*****************************************************************************/ + +/** Get master information. + */ +int ec_cdev_ioctl_master( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< Userspace address to store the results. */ + ) +{ + ec_ioctl_master_t data; + + down(&master->master_sem); + data.slave_count = master->slave_count; + data.config_count = ec_master_config_count(master); + data.domain_count = ec_master_domain_count(master); + data.phase = (uint8_t) master->phase; + up(&master->master_sem); + + down(&master->device_sem); + memcpy(data.devices[0].address, master->main_mac, ETH_ALEN); + data.devices[0].attached = master->main_device.dev ? 1 : 0; + data.devices[0].tx_count = master->main_device.tx_count; + data.devices[0].rx_count = master->main_device.rx_count; + memcpy(data.devices[1].address, master->backup_mac, ETH_ALEN); + data.devices[1].attached = master->backup_device.dev ? 1 : 0; + data.devices[1].tx_count = master->backup_device.tx_count; + data.devices[1].rx_count = master->backup_device.rx_count; + up(&master->device_sem); + + if (copy_to_user((void __user *) arg, &data, sizeof(data))) + return -EFAULT; + + return 0; +} + +/*****************************************************************************/ + +/** Get slave information. + */ +int ec_cdev_ioctl_slave( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< Userspace address to store the results. */ + ) +{ + ec_ioctl_slave_t data; + const ec_slave_t *slave; + + if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { + return -EFAULT; + } + + down(&master->master_sem); + + if (!(slave = ec_master_find_slave_const( + master, 0, data.position))) { + up(&master->master_sem); + EC_ERR("Slave %u does not exist!\n", data.position); + return -EINVAL; + } + + data.vendor_id = slave->sii.vendor_id; + data.product_code = slave->sii.product_code; + data.revision_number = slave->sii.revision_number; + data.serial_number = slave->sii.serial_number; + data.alias = slave->sii.alias; + data.rx_mailbox_offset = slave->sii.rx_mailbox_offset; + data.rx_mailbox_size = slave->sii.rx_mailbox_size; + data.tx_mailbox_offset = slave->sii.tx_mailbox_offset; + data.tx_mailbox_size = slave->sii.tx_mailbox_size; + data.mailbox_protocols = slave->sii.mailbox_protocols; + data.has_general_category = slave->sii.has_general; + data.coe_details = slave->sii.coe_details; + data.general_flags = slave->sii.general_flags; + data.current_on_ebus = slave->sii.current_on_ebus; + data.state = slave->current_state; + data.error_flag = slave->error_flag; + + data.sync_count = slave->sii.sync_count; + data.sdo_count = ec_slave_sdo_count(slave); + data.sii_nwords = slave->sii_nwords; + + if (slave->sii.name) { + strncpy(data.name, slave->sii.name, + EC_IOCTL_STRING_SIZE); + data.name[EC_IOCTL_STRING_SIZE - 1] = 0; + } else { + data.name[0] = 0; + } + + up(&master->master_sem); + + if (copy_to_user((void __user *) arg, &data, sizeof(data))) + return -EFAULT; + + return 0; +} + +/*****************************************************************************/ + +/** Get slave sync manager information. + */ +int ec_cdev_ioctl_slave_sync( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< Userspace address to store the results. */ + ) +{ + ec_ioctl_slave_sync_t data; + const ec_slave_t *slave; + const ec_sync_t *sync; + + if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { + return -EFAULT; + } + + down(&master->master_sem); + + if (!(slave = ec_master_find_slave_const( + master, 0, data.slave_position))) { + up(&master->master_sem); + EC_ERR("Slave %u does not exist!\n", data.slave_position); + return -EINVAL; + } + + if (data.sync_index >= slave->sii.sync_count) { + up(&master->master_sem); + EC_ERR("Sync manager %u does not exist in slave %u!\n", + data.sync_index, data.slave_position); + return -EINVAL; + } + + sync = &slave->sii.syncs[data.sync_index]; + + data.physical_start_address = sync->physical_start_address; + data.default_size = sync->default_length; + data.control_register = sync->control_register; + data.enable = sync->enable; + data.assign_source = sync->assign_source; + data.pdo_count = ec_pdo_list_count(&sync->pdos); + + up(&master->master_sem); + + if (copy_to_user((void __user *) arg, &data, sizeof(data))) + return -EFAULT; + + return 0; +} + +/*****************************************************************************/ + +/** Get slave sync manager Pdo information. + */ +int ec_cdev_ioctl_slave_sync_pdo( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< Userspace address to store the results. */ + ) +{ + ec_ioctl_slave_sync_pdo_t data; + const ec_slave_t *slave; + const ec_sync_t *sync; + const ec_pdo_t *pdo; + + if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { + return -EFAULT; + } + + down(&master->master_sem); + + if (!(slave = ec_master_find_slave_const( + master, 0, data.slave_position))) { + up(&master->master_sem); + EC_ERR("Slave %u does not exist!\n", data.slave_position); + return -EINVAL; + } + + if (data.sync_index >= slave->sii.sync_count) { + up(&master->master_sem); + EC_ERR("Sync manager %u does not exist in slave %u!\n", + data.sync_index, data.slave_position); + return -EINVAL; + } + + sync = &slave->sii.syncs[data.sync_index]; + if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( + &sync->pdos, data.pdo_pos))) { + up(&master->master_sem); + EC_ERR("Sync manager %u does not contain a Pdo with " + "position %u in slave %u!\n", data.sync_index, + data.pdo_pos, data.slave_position); + return -EINVAL; + } + + data.index = pdo->index; + data.entry_count = ec_pdo_entry_count(pdo); + + if (pdo->name) { + strncpy(data.name, pdo->name, EC_IOCTL_STRING_SIZE); + data.name[EC_IOCTL_STRING_SIZE - 1] = 0; + } else { + data.name[0] = 0; + } + + up(&master->master_sem); + + if (copy_to_user((void __user *) arg, &data, sizeof(data))) + return -EFAULT; + + return 0; +} + +/*****************************************************************************/ + +/** Get slave sync manager Pdo entry information. + */ +int ec_cdev_ioctl_slave_sync_pdo_entry( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< Userspace address to store the results. */ + ) +{ + ec_ioctl_slave_sync_pdo_entry_t data; + const ec_slave_t *slave; + const ec_sync_t *sync; + const ec_pdo_t *pdo; + const ec_pdo_entry_t *entry; + + if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { + return -EFAULT; + } + + down(&master->master_sem); + + if (!(slave = ec_master_find_slave_const( + master, 0, data.slave_position))) { + up(&master->master_sem); + EC_ERR("Slave %u does not exist!\n", data.slave_position); + return -EINVAL; + } + + if (data.sync_index >= slave->sii.sync_count) { + up(&master->master_sem); + EC_ERR("Sync manager %u does not exist in slave %u!\n", + data.sync_index, data.slave_position); + return -EINVAL; + } + + sync = &slave->sii.syncs[data.sync_index]; + if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( + &sync->pdos, data.pdo_pos))) { + up(&master->master_sem); + EC_ERR("Sync manager %u does not contain a Pdo with " + "position %u in slave %u!\n", data.sync_index, + data.pdo_pos, data.slave_position); + return -EINVAL; + } + + if (!(entry = ec_pdo_find_entry_by_pos_const( + pdo, data.entry_pos))) { + up(&master->master_sem); + EC_ERR("Pdo 0x%04X does not contain an entry with " + "position %u in slave %u!\n", data.pdo_pos, + data.entry_pos, data.slave_position); + return -EINVAL; + } + + data.index = entry->index; + data.subindex = entry->subindex; + data.bit_length = entry->bit_length; + if (entry->name) { + strncpy(data.name, entry->name, EC_IOCTL_STRING_SIZE); + data.name[EC_IOCTL_STRING_SIZE - 1] = 0; + } else { + data.name[0] = 0; + } + + up(&master->master_sem); + + if (copy_to_user((void __user *) arg, &data, sizeof(data))) + return -EFAULT; + + return 0; +} + +/*****************************************************************************/ + +/** Get domain information. + */ +int ec_cdev_ioctl_domain( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< Userspace address to store the results. */ + ) +{ + ec_ioctl_domain_t data; + const ec_domain_t *domain; + + if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { + return -EFAULT; + } + + down(&master->master_sem); + + if (!(domain = ec_master_find_domain_const(master, data.index))) { + up(&master->master_sem); + EC_ERR("Domain %u does not exist!\n", data.index); + return -EINVAL; + } + + data.data_size = domain->data_size; + data.logical_base_address = domain->logical_base_address; + data.working_counter = domain->working_counter; + data.expected_working_counter = domain->expected_working_counter; + data.fmmu_count = ec_domain_fmmu_count(domain); + + up(&master->master_sem); + + if (copy_to_user((void __user *) arg, &data, sizeof(data))) + return -EFAULT; + + return 0; +} + +/*****************************************************************************/ + +/** Get domain FMMU information. + */ +int ec_cdev_ioctl_domain_fmmu( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< Userspace address to store the results. */ + ) +{ + ec_ioctl_domain_fmmu_t data; + const ec_domain_t *domain; + const ec_fmmu_config_t *fmmu; + + if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { + return -EFAULT; + } + + down(&master->master_sem); + + if (!(domain = ec_master_find_domain_const(master, data.domain_index))) { + up(&master->master_sem); + EC_ERR("Domain %u does not exist!\n", data.domain_index); + return -EINVAL; + } + + if (!(fmmu = ec_domain_find_fmmu(domain, data.fmmu_index))) { + up(&master->master_sem); + EC_ERR("Domain %u has less than %u fmmu configurations.\n", + data.domain_index, data.fmmu_index + 1); + return -EINVAL; + } + + data.slave_config_alias = fmmu->sc->alias; + data.slave_config_position = fmmu->sc->position; + data.sync_index = fmmu->sync_index; + data.dir = fmmu->dir; + data.logical_address = fmmu->logical_start_address; + data.data_size = fmmu->data_size; + + up(&master->master_sem); + + if (copy_to_user((void __user *) arg, &data, sizeof(data))) + return -EFAULT; + + return 0; +} + +/*****************************************************************************/ + +/** Get domain data. + */ +int ec_cdev_ioctl_domain_data( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< Userspace address to store the results. */ + ) +{ + ec_ioctl_domain_data_t data; + const ec_domain_t *domain; + + if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { + return -EFAULT; + } + + down(&master->master_sem); + + if (!(domain = ec_master_find_domain_const(master, data.domain_index))) { + up(&master->master_sem); + EC_ERR("Domain %u does not exist!\n", data.domain_index); + return -EINVAL; + } + + if (domain->data_size != data.data_size) { + up(&master->master_sem); + EC_ERR("Data size mismatch %u/%u!\n", + data.data_size, domain->data_size); + return -EFAULT; + } + + if (copy_to_user((void __user *) data.target, domain->data, + domain->data_size)) + return -EFAULT; + + up(&master->master_sem); + return 0; +} + +/*****************************************************************************/ + +/** Set master debug level. + */ +int ec_cdev_ioctl_master_debug( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< ioctl() argument. */ + ) +{ + if (ec_master_debug_level(master, (unsigned int) arg)) + return -EINVAL; + + return 0; +} + +/*****************************************************************************/ + +/** Set slave state. + */ +int ec_cdev_ioctl_slave_state( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< ioctl() argument. */ + ) +{ + ec_ioctl_slave_state_t data; + ec_slave_t *slave; + + if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { + return -EFAULT; + } + + down(&master->master_sem); + + if (!(slave = ec_master_find_slave( + master, 0, data.slave_position))) { + EC_ERR("Slave %u does not exist!\n", data.slave_position); + return -EINVAL; + } + + ec_slave_request_state(slave, data.requested_state); + + up(&master->master_sem); + return 0; +} + +/*****************************************************************************/ + +/** Get slave Sdo information. + */ +int ec_cdev_ioctl_slave_sdo( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< ioctl() argument. */ + ) +{ + ec_ioctl_slave_sdo_t data; + const ec_slave_t *slave; + const ec_sdo_t *sdo; + + if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { + return -EFAULT; + } + + down(&master->master_sem); + + if (!(slave = ec_master_find_slave_const( + master, 0, data.slave_position))) { + up(&master->master_sem); + EC_ERR("Slave %u does not exist!\n", data.slave_position); + return -EINVAL; + } + + if (!(sdo = ec_slave_get_sdo_by_pos_const( + slave, data.sdo_position))) { + up(&master->master_sem); + EC_ERR("Sdo %u does not exist in slave %u!\n", + data.sdo_position, data.slave_position); + return -EINVAL; + } + + data.sdo_index = sdo->index; + data.max_subindex = sdo->max_subindex; + + if (sdo->name) { + strncpy(data.name, sdo->name, EC_IOCTL_STRING_SIZE); + data.name[EC_IOCTL_STRING_SIZE - 1] = 0; + } else { + data.name[0] = 0; + } + + up(&master->master_sem); + + if (copy_to_user((void __user *) arg, &data, sizeof(data))) + return -EFAULT; + + return 0; +} + +/*****************************************************************************/ + +/** Get slave Sdo entry information. + */ +int ec_cdev_ioctl_slave_sdo_entry( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< ioctl() argument. */ + ) +{ + ec_ioctl_slave_sdo_entry_t data; + const ec_slave_t *slave; + const ec_sdo_t *sdo; + const ec_sdo_entry_t *entry; + + if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { + return -EFAULT; + } + + down(&master->master_sem); + + if (!(slave = ec_master_find_slave_const( + master, 0, data.slave_position))) { + up(&master->master_sem); + EC_ERR("Slave %u does not exist!\n", data.slave_position); + return -EINVAL; + } + + if (data.sdo_spec <= 0) { + if (!(sdo = ec_slave_get_sdo_by_pos_const( + slave, -data.sdo_spec))) { + up(&master->master_sem); + EC_ERR("Sdo %u does not exist in slave %u!\n", + -data.sdo_spec, data.slave_position); + return -EINVAL; + } + } else { + if (!(sdo = ec_slave_get_sdo_const( + slave, data.sdo_spec))) { + up(&master->master_sem); + EC_ERR("Sdo 0x%04X does not exist in slave %u!\n", + data.sdo_spec, data.slave_position); + return -EINVAL; + } + } + + if (!(entry = ec_sdo_get_entry_const( + sdo, data.sdo_entry_subindex))) { + up(&master->master_sem); + EC_ERR("Sdo entry 0x%04X:%02X does not exist " + "in slave %u!\n", sdo->index, + data.sdo_entry_subindex, data.slave_position); + return -EINVAL; + } + + data.data_type = entry->data_type; + data.bit_length = entry->bit_length; + + if (entry->description) { + strncpy(data.description, entry->description, + EC_IOCTL_STRING_SIZE); + data.description[EC_IOCTL_STRING_SIZE - 1] + = 0; + } else { + data.description[0] = 0; + } + + up(&master->master_sem); + + if (copy_to_user((void __user *) arg, &data, sizeof(data))) + return -EFAULT; + + return 0; +} + +/*****************************************************************************/ + +/** Upload Sdo. + */ +int ec_cdev_ioctl_slave_sdo_upload( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< ioctl() argument. */ + ) +{ + ec_ioctl_slave_sdo_upload_t data; + ec_master_sdo_request_t request; + int retval; + + 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); + + down(&master->master_sem); + + if (!(request.slave = ec_master_find_slave( + master, 0, data.slave_position))) { + up(&master->master_sem); + ec_sdo_request_clear(&request.req); + EC_ERR("Slave %u does not exist!\n", data.slave_position); + return -EINVAL; + } + + // schedule request. + list_add_tail(&request.list, &master->slave_sdo_requests); + + up(&master->master_sem); + + // wait for processing through FSM + if (wait_event_interruptible(master->sdo_queue, + request.req.state != EC_REQUEST_QUEUED)) { + // interrupted by signal + down(&master->master_sem); + if (request.req.state == EC_REQUEST_QUEUED) { + list_del(&request.req.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(master->sdo_queue, request.req.state != EC_REQUEST_BUSY); + + data.abort_code = request.req.abort_code; + + if (request.req.state != EC_REQUEST_SUCCESS) { + data.data_size = 0; + retval = -EIO; + } else { + if (request.req.data_size > data.target_size) { + EC_ERR("Buffer too small.\n"); + ec_sdo_request_clear(&request.req); + return -EOVERFLOW; + } + data.data_size = request.req.data_size; + + if (copy_to_user((void __user *) data.target, + request.req.data, data.data_size)) { + ec_sdo_request_clear(&request.req); + return -EFAULT; + } + retval = 0; + } + + if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { + retval = -EFAULT; + } + + ec_sdo_request_clear(&request.req); + 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_ERR("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); + return -ENOMEM; + } + if (copy_from_user(request.req.data, + (void __user *) data.data, data.data_size)) { + ec_sdo_request_clear(&request.req); + return -EFAULT; + } + request.req.data_size = data.data_size; + ecrt_sdo_request_write(&request.req); + + + down(&master->master_sem); + + if (!(request.slave = ec_master_find_slave( + master, 0, data.slave_position))) { + up(&master->master_sem); + EC_ERR("Slave %u does not exist!\n", data.slave_position); + ec_sdo_request_clear(&request.req); + return -EINVAL; + } + + // schedule request. + list_add_tail(&request.list, &master->slave_sdo_requests); + + up(&master->master_sem); + + // wait for processing through FSM + if (wait_event_interruptible(master->sdo_queue, + request.req.state != EC_REQUEST_QUEUED)) { + // interrupted by signal + down(&master->master_sem); + if (request.req.state == EC_REQUEST_QUEUED) { + list_del(&request.req.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(master->sdo_queue, request.req.state != EC_REQUEST_BUSY); + + data.abort_code = request.req.abort_code; + + retval = request.req.state == EC_REQUEST_SUCCESS ? 0 : -EIO; + + if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { + retval = -EFAULT; + } + + ec_sdo_request_clear(&request.req); + return retval; +} + +/*****************************************************************************/ + +/** Read a slave's SII. + */ +int ec_cdev_ioctl_slave_sii_read( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< ioctl() argument. */ + ) +{ + ec_ioctl_slave_sii_t data; + const ec_slave_t *slave; + int retval; + + if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { + return -EFAULT; + } + + down(&master->master_sem); + + if (!(slave = ec_master_find_slave_const( + master, 0, data.slave_position))) { + up(&master->master_sem); + EC_ERR("Slave %u does not exist!\n", data.slave_position); + return -EINVAL; + } + + if (!data.nwords + || data.offset + data.nwords > slave->sii_nwords) { + up(&master->master_sem); + EC_ERR("Invalid SII read offset/size %u/%u for slave " + "SII size %u!\n", data.offset, + data.nwords, slave->sii_nwords); + return -EINVAL; + } + + if (copy_to_user((void __user *) data.words, + slave->sii_words + data.offset, data.nwords * 2)) + retval = -EFAULT; + else + retval = 0; + + up(&master->master_sem); + return retval; +} + +/*****************************************************************************/ + +/** Write a slave's SII. + */ +int ec_cdev_ioctl_slave_sii_write( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< ioctl() argument. */ + ) +{ + ec_ioctl_slave_sii_t data; + ec_slave_t *slave; + unsigned int byte_size; + uint16_t *words; + ec_sii_write_request_t request; + + if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { + return -EFAULT; + } + + if (!data.nwords) + return 0; + + byte_size = sizeof(uint16_t) * data.nwords; + if (!(words = kmalloc(byte_size, GFP_KERNEL))) { + EC_ERR("Failed to allocate %u bytes for SII contents.\n", + byte_size); + return -ENOMEM; + } + + if (copy_from_user(words, + (void __user *) data.words, byte_size)) { + kfree(words); + return -EFAULT; + } + + down(&master->master_sem); + + if (!(slave = ec_master_find_slave( + master, 0, data.slave_position))) { + up(&master->master_sem); + EC_ERR("Slave %u does not exist!\n", data.slave_position); + kfree(words); + 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_REQUEST_QUEUED; + + // schedule SII write request. + list_add_tail(&request.list, &master->sii_requests); + + up(&master->master_sem); + + // wait for processing through FSM + if (wait_event_interruptible(master->sii_queue, + request.state != EC_REQUEST_QUEUED)) { + // interrupted by signal + down(&master->master_sem); + if (request.state == EC_REQUEST_QUEUED) { + // abort request + list_del(&request.list); + up(&master->master_sem); + kfree(words); + return -EINTR; + } + up(&master->master_sem); + } + + // wait until master FSM has finished processing + wait_event(master->sii_queue, request.state != EC_REQUEST_BUSY); + + kfree(words); + + return request.state == EC_REQUEST_SUCCESS ? 0 : -EIO; +} + +/*****************************************************************************/ + +/** Get slave configuration information. + */ +int ec_cdev_ioctl_config( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< ioctl() argument. */ + ) +{ + ec_ioctl_config_t data; + const ec_slave_config_t *sc; + uint8_t i; + + if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { + return -EFAULT; + } + + down(&master->master_sem); + + if (!(sc = ec_master_get_config_const( + master, data.config_index))) { + up(&master->master_sem); + EC_ERR("Slave config %u does not exist!\n", + data.config_index); + return -EINVAL; + } + + data.alias = sc->alias; + data.position = sc->position; + data.vendor_id = sc->vendor_id; + data.product_code = sc->product_code; + for (i = 0; i < EC_MAX_SYNCS; i++) { + data.syncs[i].dir = sc->sync_configs[i].dir; + data.syncs[i].pdo_count = + ec_pdo_list_count(&sc->sync_configs[i].pdos); + } + data.sdo_count = ec_slave_config_sdo_count(sc); + data.attached = sc->slave != NULL; + data.operational = sc->slave && + sc->slave->current_state == EC_SLAVE_STATE_OP; + + up(&master->master_sem); + + if (copy_to_user((void __user *) arg, &data, sizeof(data))) + return -EFAULT; + + return 0; +} + +/*****************************************************************************/ + +/** Get slave configuration Pdo information. + */ +int ec_cdev_ioctl_config_pdo( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< ioctl() argument. */ + ) +{ + ec_ioctl_config_pdo_t data; + const ec_slave_config_t *sc; + const ec_pdo_t *pdo; + + if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { + return -EFAULT; + } + + if (data.sync_index >= EC_MAX_SYNCS) { + EC_ERR("Invalid sync manager index %u!\n", + data.sync_index); + return -EINVAL; + } + + down(&master->master_sem); + + if (!(sc = ec_master_get_config_const( + master, data.config_index))) { + up(&master->master_sem); + EC_ERR("Slave config %u does not exist!\n", + data.config_index); + return -EINVAL; + } + + if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( + &sc->sync_configs[data.sync_index].pdos, + data.pdo_pos))) { + up(&master->master_sem); + EC_ERR("Invalid Pdo position!\n"); + return -EINVAL; + } + + data.index = pdo->index; + data.entry_count = ec_pdo_entry_count(pdo); + + if (pdo->name) { + strncpy(data.name, pdo->name, EC_IOCTL_STRING_SIZE); + data.name[EC_IOCTL_STRING_SIZE - 1] = 0; + } else { + data.name[0] = 0; + } + + up(&master->master_sem); + + if (copy_to_user((void __user *) arg, &data, sizeof(data))) + return -EFAULT; + + return 0; +} + +/*****************************************************************************/ + +/** Get slave configuration Pdo entry information. + */ +int ec_cdev_ioctl_config_pdo_entry( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< ioctl() argument. */ + ) +{ + ec_ioctl_config_pdo_entry_t data; + const ec_slave_config_t *sc; + const ec_pdo_t *pdo; + const ec_pdo_entry_t *entry; + + if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { + return -EFAULT; + } + + if (data.sync_index >= EC_MAX_SYNCS) { + EC_ERR("Invalid sync manager index %u!\n", + data.sync_index); + return -EINVAL; + } + + down(&master->master_sem); + + if (!(sc = ec_master_get_config_const( + master, data.config_index))) { + up(&master->master_sem); + EC_ERR("Slave config %u does not exist!\n", + data.config_index); + return -EINVAL; + } + + if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( + &sc->sync_configs[data.sync_index].pdos, + data.pdo_pos))) { + up(&master->master_sem); + EC_ERR("Invalid Pdo position!\n"); + return -EINVAL; + } + + if (!(entry = ec_pdo_find_entry_by_pos_const( + pdo, data.entry_pos))) { + up(&master->master_sem); + EC_ERR("Entry not found!\n"); + return -EINVAL; + } + + data.index = entry->index; + data.subindex = entry->subindex; + data.bit_length = entry->bit_length; + if (entry->name) { + strncpy(data.name, entry->name, EC_IOCTL_STRING_SIZE); + data.name[EC_IOCTL_STRING_SIZE - 1] = 0; + } else { + data.name[0] = 0; + } + + up(&master->master_sem); + + if (copy_to_user((void __user *) arg, &data, sizeof(data))) + return -EFAULT; + + return 0; +} + +/*****************************************************************************/ + +/** Get slave configuration Sdo information. + */ +int ec_cdev_ioctl_config_sdo( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< ioctl() argument. */ + ) +{ + ec_ioctl_config_sdo_t data; + const ec_slave_config_t *sc; + const ec_sdo_request_t *req; + + if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { + return -EFAULT; + } + + down(&master->master_sem); + + if (!(sc = ec_master_get_config_const( + master, data.config_index))) { + up(&master->master_sem); + EC_ERR("Slave config %u does not exist!\n", + data.config_index); + return -EINVAL; + } + + if (!(req = ec_slave_config_get_sdo_by_pos_const( + sc, data.sdo_pos))) { + up(&master->master_sem); + EC_ERR("Invalid Sdo position!\n"); + return -EINVAL; + } + + data.index = req->index; + data.subindex = req->subindex; + data.size = req->data_size; + memcpy(&data.data, req->data, min((u32) data.size, (u32) 4)); + + up(&master->master_sem); + + if (copy_to_user((void __user *) arg, &data, sizeof(data))) + return -EFAULT; + + return 0; +} + /****************************************************************************** * File operations *****************************************************************************/ @@ -132,888 +1224,63 @@ { ec_cdev_t *cdev = (ec_cdev_t *) filp->private_data; ec_master_t *master = cdev->master; - long retval = 0; if (master->debug_level) EC_DBG("ioctl(filp = %x, cmd = %u (%u), arg = %x)\n", (u32) filp, (u32) cmd, (u32) _IOC_NR(cmd), (u32) arg); - down(&master->master_sem); - switch (cmd) { case EC_IOCTL_MASTER: - { - ec_ioctl_master_t data; - - data.slave_count = master->slave_count; - data.config_count = ec_master_config_count(master); - data.domain_count = ec_master_domain_count(master); - data.phase = (uint8_t) master->phase; - - memcpy(data.devices[0].address, master->main_mac, ETH_ALEN); - data.devices[0].attached = master->main_device.dev ? 1 : 0; - data.devices[0].tx_count = master->main_device.tx_count; - data.devices[0].rx_count = master->main_device.rx_count; - memcpy(data.devices[1].address, master->backup_mac, ETH_ALEN); - data.devices[1].attached = master->backup_device.dev ? 1 : 0; - data.devices[1].tx_count = master->backup_device.tx_count; - data.devices[1].rx_count = master->backup_device.rx_count; - - if (copy_to_user((void __user *) arg, &data, sizeof(data))) - retval = -EFAULT; - break; - } - + return ec_cdev_ioctl_master(master, arg); case EC_IOCTL_SLAVE: - { - ec_ioctl_slave_t data; - const ec_slave_t *slave; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - retval = -EFAULT; - break; - } - - if (!(slave = ec_master_find_slave( - master, 0, data.position))) { - EC_ERR("Slave %u does not exist!\n", data.position); - retval = -EINVAL; - break; - } - - data.vendor_id = slave->sii.vendor_id; - data.product_code = slave->sii.product_code; - data.revision_number = slave->sii.revision_number; - data.serial_number = slave->sii.serial_number; - data.alias = slave->sii.alias; - data.rx_mailbox_offset = slave->sii.rx_mailbox_offset; - data.rx_mailbox_size = slave->sii.rx_mailbox_size; - data.tx_mailbox_offset = slave->sii.tx_mailbox_offset; - data.tx_mailbox_size = slave->sii.tx_mailbox_size; - data.mailbox_protocols = slave->sii.mailbox_protocols; - data.has_general_category = slave->sii.has_general; - data.coe_details = slave->sii.coe_details; - data.general_flags = slave->sii.general_flags; - data.current_on_ebus = slave->sii.current_on_ebus; - data.state = slave->current_state; - data.error_flag = slave->error_flag; - - data.sync_count = slave->sii.sync_count; - data.sdo_count = ec_slave_sdo_count(slave); - data.sii_nwords = slave->sii_nwords; - - if (slave->sii.name) { - strncpy(data.name, slave->sii.name, - EC_IOCTL_STRING_SIZE); - data.name[EC_IOCTL_STRING_SIZE - 1] = 0; - } else { - data.name[0] = 0; - } - - if (copy_to_user((void __user *) arg, &data, sizeof(data))) - retval = -EFAULT; - break; - } - - case EC_IOCTL_SYNC: - { - ec_ioctl_sync_t data; - const ec_slave_t *slave; - const ec_sync_t *sync; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - retval = -EFAULT; - break; - } - - if (!(slave = ec_master_find_slave( - master, 0, data.slave_position))) { - EC_ERR("Slave %u does not exist!\n", data.slave_position); - retval = -EINVAL; - break; - } - - if (data.sync_index >= slave->sii.sync_count) { - EC_ERR("Sync manager %u does not exist in slave %u!\n", - data.sync_index, data.slave_position); - retval = -EINVAL; - break; - } - - sync = &slave->sii.syncs[data.sync_index]; - - data.physical_start_address = sync->physical_start_address; - data.default_size = sync->default_length; - data.control_register = sync->control_register; - data.enable = sync->enable; - data.assign_source = sync->assign_source; - data.pdo_count = ec_pdo_list_count(&sync->pdos); - - if (copy_to_user((void __user *) arg, &data, sizeof(data))) - retval = -EFAULT; - break; - } - - case EC_IOCTL_PDO: - { - ec_ioctl_pdo_t data; - const ec_slave_t *slave; - const ec_sync_t *sync; - const ec_pdo_t *pdo; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - retval = -EFAULT; - break; - } - - if (!(slave = ec_master_find_slave( - master, 0, data.slave_position))) { - EC_ERR("Slave %u does not exist!\n", data.slave_position); - retval = -EINVAL; - break; - } - - if (data.sync_index >= slave->sii.sync_count) { - EC_ERR("Sync manager %u does not exist in slave %u!\n", - data.sync_index, data.slave_position); - retval = -EINVAL; - break; - } - - sync = &slave->sii.syncs[data.sync_index]; - if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( - &sync->pdos, data.pdo_pos))) { - EC_ERR("Sync manager %u does not contain a Pdo with " - "position %u in slave %u!\n", data.sync_index, - data.pdo_pos, data.slave_position); - retval = -EINVAL; - break; - } - - data.index = pdo->index; - data.entry_count = ec_pdo_entry_count(pdo); - - if (pdo->name) { - strncpy(data.name, pdo->name, EC_IOCTL_STRING_SIZE); - data.name[EC_IOCTL_STRING_SIZE - 1] = 0; - } else { - data.name[0] = 0; - } - - if (copy_to_user((void __user *) arg, &data, sizeof(data))) - retval = -EFAULT; - break; - } - - case EC_IOCTL_PDO_ENTRY: - { - ec_ioctl_pdo_entry_t data; - const ec_slave_t *slave; - const ec_sync_t *sync; - const ec_pdo_t *pdo; - const ec_pdo_entry_t *entry; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - retval = -EFAULT; - break; - } - - if (!(slave = ec_master_find_slave( - master, 0, data.slave_position))) { - EC_ERR("Slave %u does not exist!\n", data.slave_position); - retval = -EINVAL; - break; - } - - if (data.sync_index >= slave->sii.sync_count) { - EC_ERR("Sync manager %u does not exist in slave %u!\n", - data.sync_index, data.slave_position); - retval = -EINVAL; - break; - } - - sync = &slave->sii.syncs[data.sync_index]; - if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( - &sync->pdos, data.pdo_pos))) { - EC_ERR("Sync manager %u does not contain a Pdo with " - "position %u in slave %u!\n", data.sync_index, - data.pdo_pos, data.slave_position); - retval = -EINVAL; - break; - } - - if (!(entry = ec_pdo_find_entry_by_pos_const( - pdo, data.entry_pos))) { - EC_ERR("Pdo 0x%04X does not contain an entry with " - "position %u in slave %u!\n", data.pdo_pos, - data.entry_pos, data.slave_position); - retval = -EINVAL; - break; - } - - data.index = entry->index; - data.subindex = entry->subindex; - data.bit_length = entry->bit_length; - if (entry->name) { - strncpy(data.name, entry->name, EC_IOCTL_STRING_SIZE); - data.name[EC_IOCTL_STRING_SIZE - 1] = 0; - } else { - data.name[0] = 0; - } - - if (copy_to_user((void __user *) arg, &data, sizeof(data))) - retval = -EFAULT; - break; - } - + return ec_cdev_ioctl_slave(master, arg); + case EC_IOCTL_SLAVE_SYNC: + return ec_cdev_ioctl_slave_sync(master, arg); + case EC_IOCTL_SLAVE_SYNC_PDO: + return ec_cdev_ioctl_slave_sync_pdo(master, arg); + case EC_IOCTL_SLAVE_SYNC_PDO_ENTRY: + return ec_cdev_ioctl_slave_sync_pdo_entry(master, arg); case EC_IOCTL_DOMAIN: - { - ec_ioctl_domain_t data; - const ec_domain_t *domain; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - retval = -EFAULT; - break; - } - - if (!(domain = ec_master_find_domain(master, data.index))) { - EC_ERR("Domain %u does not exist!\n", data.index); - retval = -EINVAL; - break; - } - - data.data_size = domain->data_size; - data.logical_base_address = domain->logical_base_address; - data.working_counter = domain->working_counter; - data.expected_working_counter = domain->expected_working_counter; - data.fmmu_count = ec_domain_fmmu_count(domain); - - if (copy_to_user((void __user *) arg, &data, sizeof(data))) - retval = -EFAULT; - break; - } - + return ec_cdev_ioctl_domain(master, arg); case EC_IOCTL_DOMAIN_FMMU: - { - ec_ioctl_domain_fmmu_t data; - const ec_domain_t *domain; - const ec_fmmu_config_t *fmmu; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - retval = -EFAULT; - break; - } - - if (!(domain = ec_master_find_domain(master, data.domain_index))) { - EC_ERR("Domain %u does not exist!\n", data.domain_index); - retval = -EINVAL; - break; - } - - if (!(fmmu = ec_domain_find_fmmu(domain, data.fmmu_index))) { - EC_ERR("Domain %u has less than %u fmmu configurations.\n", - data.domain_index, data.fmmu_index + 1); - retval = -EINVAL; - break; - } - - data.slave_config_alias = fmmu->sc->alias; - data.slave_config_position = fmmu->sc->position; - data.sync_index = fmmu->sync_index; - data.dir = fmmu->dir; - data.logical_address = fmmu->logical_start_address; - data.data_size = fmmu->data_size; - - if (copy_to_user((void __user *) arg, &data, sizeof(data))) - retval = -EFAULT; - break; - } - - case EC_IOCTL_DATA: - { - ec_ioctl_data_t data; - const ec_domain_t *domain; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - retval = -EFAULT; - break; - } - - if (!(domain = ec_master_find_domain(master, data.domain_index))) { - EC_ERR("Domain %u does not exist!\n", data.domain_index); - retval = -EINVAL; - break; - } - - if (domain->data_size != data.data_size) { - EC_ERR("Data size mismatch %u/%u!\n", - data.data_size, domain->data_size); - retval = -EFAULT; - break; - } - - if (copy_to_user((void __user *) data.target, domain->data, - domain->data_size)) - retval = -EFAULT; - break; - } - - case EC_IOCTL_SET_DEBUG: + return ec_cdev_ioctl_domain_fmmu(master, arg); + case EC_IOCTL_DOMAIN_DATA: + return ec_cdev_ioctl_domain_data(master, arg); + case EC_IOCTL_MASTER_DEBUG: if (!(filp->f_mode & FMODE_WRITE)) return -EPERM; - if (ec_master_debug_level(master, (unsigned int) arg)) - retval = -EINVAL; - break; - + return ec_cdev_ioctl_master_debug(master, arg); case EC_IOCTL_SLAVE_STATE: - { - ec_ioctl_slave_state_t data; - ec_slave_t *slave; - - if (!(filp->f_mode & FMODE_WRITE)) - return -EPERM; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - retval = -EFAULT; - break; - } - - if (!(slave = ec_master_find_slave( - master, 0, data.slave_position))) { - EC_ERR("Slave %u does not exist!\n", data.slave_position); - retval = -EINVAL; - break; - } - - ec_slave_request_state(slave, data.requested_state); - break; - } - - case EC_IOCTL_SDO: - { - ec_ioctl_sdo_t data; - const ec_slave_t *slave; - const ec_sdo_t *sdo; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - retval = -EFAULT; - break; - } - - if (!(slave = ec_master_find_slave( - master, 0, data.slave_position))) { - EC_ERR("Slave %u does not exist!\n", data.slave_position); - retval = -EINVAL; - break; - } - - if (!(sdo = ec_slave_get_sdo_by_pos_const( - slave, data.sdo_position))) { - EC_ERR("Sdo %u does not exist in slave %u!\n", - data.sdo_position, data.slave_position); - retval = -EINVAL; - break; - } - - data.sdo_index = sdo->index; - data.max_subindex = sdo->max_subindex; - - if (sdo->name) { - strncpy(data.name, sdo->name, EC_IOCTL_STRING_SIZE); - data.name[EC_IOCTL_STRING_SIZE - 1] = 0; - } else { - data.name[0] = 0; - } - - if (copy_to_user((void __user *) arg, &data, sizeof(data))) - retval = -EFAULT; - break; - } - - case EC_IOCTL_SDO_ENTRY: - { - ec_ioctl_sdo_entry_t data; - const ec_slave_t *slave; - const ec_sdo_t *sdo; - const ec_sdo_entry_t *entry; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - retval = -EFAULT; - break; - } - - if (!(slave = ec_master_find_slave( - master, 0, data.slave_position))) { - EC_ERR("Slave %u does not exist!\n", data.slave_position); - retval = -EINVAL; - break; - } - - if (data.sdo_spec <= 0) { - if (!(sdo = ec_slave_get_sdo_by_pos_const( - slave, -data.sdo_spec))) { - EC_ERR("Sdo %u does not exist in slave %u!\n", - -data.sdo_spec, data.slave_position); - retval = -EINVAL; - break; - } - } else { - if (!(sdo = ec_slave_get_sdo_const( - slave, data.sdo_spec))) { - EC_ERR("Sdo 0x%04X does not exist in slave %u!\n", - data.sdo_spec, data.slave_position); - retval = -EINVAL; - break; - } - } - - if (!(entry = ec_sdo_get_entry_const( - sdo, data.sdo_entry_subindex))) { - EC_ERR("Sdo entry 0x%04X:%02X does not exist " - "in slave %u!\n", sdo->index, - data.sdo_entry_subindex, data.slave_position); - retval = -EINVAL; - break; - } - - data.data_type = entry->data_type; - data.bit_length = entry->bit_length; - - if (entry->description) { - strncpy(data.description, entry->description, - EC_IOCTL_STRING_SIZE); - data.description[EC_IOCTL_STRING_SIZE - 1] - = 0; - } else { - data.description[0] = 0; - } - - if (copy_to_user((void __user *) arg, &data, sizeof(data))) - retval = -EFAULT; - break; - } - - case EC_IOCTL_SDO_UPLOAD: - { - ec_ioctl_sdo_upload_t data; - ec_master_sdo_request_t request; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - retval = -EFAULT; - break; - } - - if (!(request.slave = ec_master_find_slave( - master, 0, data.slave_position))) { - EC_ERR("Slave %u does not exist!\n", data.slave_position); - retval = -EINVAL; - break; - } - - 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); - - // schedule request. - down(&master->sdo_sem); - list_add_tail(&request.list, &master->slave_sdo_requests); - up(&master->sdo_sem); - - // wait for processing through FSM - if (wait_event_interruptible(master->sdo_queue, - request.req.state != EC_REQUEST_QUEUED)) { - // interrupted by signal - down(&master->sdo_sem); - if (request.req.state == EC_REQUEST_QUEUED) { - list_del(&request.req.list); - up(&master->sdo_sem); - ec_sdo_request_clear(&request.req); - retval = -EINTR; - break; - } - // request already processing: interrupt not possible. - up(&master->sdo_sem); - } - - // wait until master FSM has finished processing - wait_event(master->sdo_queue, request.req.state != EC_REQUEST_BUSY); - - data.abort_code = request.req.abort_code; - - if (request.req.state != EC_REQUEST_SUCCESS) { - data.data_size = 0; - retval = -EIO; - } else { - if (request.req.data_size > data.target_size) { - EC_ERR("Buffer too small.\n"); - ec_sdo_request_clear(&request.req); - retval = -EOVERFLOW; - break; - } - data.data_size = request.req.data_size; - - if (copy_to_user((void __user *) data.target, - request.req.data, data.data_size)) { - ec_sdo_request_clear(&request.req); - retval = -EFAULT; - break; - } - } - - if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { - retval = -EFAULT; - } - - ec_sdo_request_clear(&request.req); - break; - } - - case EC_IOCTL_SDO_DOWNLOAD: - { - ec_ioctl_sdo_download_t data; - ec_master_sdo_request_t request; - - if (!(filp->f_mode & FMODE_WRITE)) - return -EPERM; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - retval = -EFAULT; - break; - } - - if (!(request.slave = ec_master_find_slave( - master, 0, data.slave_position))) { - EC_ERR("Slave %u does not exist!\n", data.slave_position); - retval = -EINVAL; - break; - } - - // copy data to download - if (!data.data_size) { - EC_ERR("Zero data size!\n"); - retval = -EINVAL; - break; - } - - 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); - retval = -ENOMEM; - break; - } - if (copy_from_user(request.req.data, - (void __user *) data.data, data.data_size)) { - ec_sdo_request_clear(&request.req); - retval = -EFAULT; - break; - } - request.req.data_size = data.data_size; - ecrt_sdo_request_write(&request.req); - - // schedule request. - down(&master->sdo_sem); - list_add_tail(&request.list, &master->slave_sdo_requests); - up(&master->sdo_sem); - - // wait for processing through FSM - if (wait_event_interruptible(master->sdo_queue, - request.req.state != EC_REQUEST_QUEUED)) { - // interrupted by signal - down(&master->sdo_sem); - if (request.req.state == EC_REQUEST_QUEUED) { - list_del(&request.req.list); - up(&master->sdo_sem); - ec_sdo_request_clear(&request.req); - retval = -EINTR; - break; - } - // request already processing: interrupt not possible. - up(&master->sdo_sem); - } - - // wait until master FSM has finished processing - wait_event(master->sdo_queue, request.req.state != EC_REQUEST_BUSY); - - data.abort_code = request.req.abort_code; - - if (request.req.state != EC_REQUEST_SUCCESS) { - retval = -EIO; - } - - if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { - retval = -EFAULT; - } - - ec_sdo_request_clear(&request.req); - break; - } - - case EC_IOCTL_SII_READ: - { - ec_ioctl_sii_t data; - const ec_slave_t *slave; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - retval = -EFAULT; - break; - } - - if (!(slave = ec_master_find_slave( - master, 0, data.slave_position))) { - EC_ERR("Slave %u does not exist!\n", data.slave_position); - retval = -EINVAL; - break; - } - - if (!data.nwords - || data.offset + data.nwords > slave->sii_nwords) { - EC_ERR("Invalid SII read offset/size %u/%u for slave " - "SII size %u!\n", data.offset, - data.nwords, slave->sii_nwords); - retval = -EINVAL; - break; - } - - if (copy_to_user((void __user *) data.words, - slave->sii_words + data.offset, data.nwords * 2)) - retval = -EFAULT; - break; - } - - case EC_IOCTL_SII_WRITE: - { - ec_ioctl_sii_t data; - ec_slave_t *slave; - unsigned int byte_size; - uint16_t *words; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - retval = -EFAULT; - break; - } - - if (!(slave = ec_master_find_slave( - master, 0, data.slave_position))) { - EC_ERR("Slave %u does not exist!\n", data.slave_position); - retval = -EINVAL; - break; - } - - if (!data.nwords) - break; - - byte_size = sizeof(uint16_t) * data.nwords; - if (!(words = kmalloc(byte_size, GFP_KERNEL))) { - EC_ERR("Failed to allocate %u bytes for SII contents.\n", - byte_size); - retval = -ENOMEM; - break; - } - - if (copy_from_user(words, - (void __user *) data.words, byte_size)) { - retval = -EFAULT; - kfree(words); - break; - } - - if (ec_slave_write_sii(slave, - data.offset, data.nwords, words)) - retval = -EIO; - - kfree(words); - break; - } - + if (!(filp->f_mode & FMODE_WRITE)) + return -EPERM; + return ec_cdev_ioctl_slave_state(master, arg); + case EC_IOCTL_SLAVE_SDO: + return ec_cdev_ioctl_slave_sdo(master, arg); + case EC_IOCTL_SLAVE_SDO_ENTRY: + return ec_cdev_ioctl_slave_sdo_entry(master, arg); + case EC_IOCTL_SLAVE_SDO_UPLOAD: + return ec_cdev_ioctl_slave_sdo_upload(master, arg); + case EC_IOCTL_SLAVE_SDO_DOWNLOAD: + if (!(filp->f_mode & FMODE_WRITE)) + return -EPERM; + return ec_cdev_ioctl_slave_sdo_download(master, arg); + case EC_IOCTL_SLAVE_SII_READ: + return ec_cdev_ioctl_slave_sii_read(master, arg); + case EC_IOCTL_SLAVE_SII_WRITE: + if (!(filp->f_mode & FMODE_WRITE)) + return -EPERM; + return ec_cdev_ioctl_slave_sii_write(master, arg); case EC_IOCTL_CONFIG: - { - ec_ioctl_config_t data; - const ec_slave_config_t *sc; - uint8_t i; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - retval = -EFAULT; - break; - } - - if (!(sc = ec_master_get_config_const( - master, data.config_index))) { - EC_ERR("Slave config %u does not exist!\n", - data.config_index); - retval = -EINVAL; - break; - } - - data.alias = sc->alias; - data.position = sc->position; - data.vendor_id = sc->vendor_id; - data.product_code = sc->product_code; - for (i = 0; i < EC_MAX_SYNCS; i++) { - data.syncs[i].dir = sc->sync_configs[i].dir; - data.syncs[i].pdo_count = - ec_pdo_list_count(&sc->sync_configs[i].pdos); - } - data.sdo_count = ec_slave_config_sdo_count(sc); - data.attached = sc->slave != NULL; - data.operational = sc->slave && - sc->slave->current_state == EC_SLAVE_STATE_OP; - - if (copy_to_user((void __user *) arg, &data, sizeof(data))) - retval = -EFAULT; - break; - } - + return ec_cdev_ioctl_config(master, arg); case EC_IOCTL_CONFIG_PDO: - { - ec_ioctl_config_pdo_t data; - const ec_slave_config_t *sc; - const ec_pdo_t *pdo; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - retval = -EFAULT; - break; - } - - if (!(sc = ec_master_get_config_const( - master, data.config_index))) { - EC_ERR("Slave config %u does not exist!\n", - data.config_index); - retval = -EINVAL; - break; - } - - if (data.sync_index >= EC_MAX_SYNCS) { - EC_ERR("Invalid sync manager index %u!\n", - data.sync_index); - retval = -EINVAL; - break; - } - - if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( - &sc->sync_configs[data.sync_index].pdos, - data.pdo_pos))) { - EC_ERR("Invalid Pdo position!\n"); - retval = -EINVAL; - break; - } - - data.index = pdo->index; - data.entry_count = ec_pdo_entry_count(pdo); - - if (pdo->name) { - strncpy(data.name, pdo->name, EC_IOCTL_STRING_SIZE); - data.name[EC_IOCTL_STRING_SIZE - 1] = 0; - } else { - data.name[0] = 0; - } - - if (copy_to_user((void __user *) arg, &data, sizeof(data))) - retval = -EFAULT; - break; - } - + return ec_cdev_ioctl_config_pdo(master, arg); case EC_IOCTL_CONFIG_PDO_ENTRY: - { - ec_ioctl_config_pdo_entry_t data; - const ec_slave_config_t *sc; - const ec_pdo_t *pdo; - const ec_pdo_entry_t *entry; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - retval = -EFAULT; - break; - } - - if (!(sc = ec_master_get_config_const( - master, data.config_index))) { - EC_ERR("Slave config %u does not exist!\n", - data.config_index); - retval = -EINVAL; - break; - } - - if (data.sync_index >= EC_MAX_SYNCS) { - EC_ERR("Invalid sync manager index %u!\n", - data.sync_index); - retval = -EINVAL; - break; - } - - if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( - &sc->sync_configs[data.sync_index].pdos, - data.pdo_pos))) { - EC_ERR("Invalid Pdo position!\n"); - retval = -EINVAL; - break; - } - - if (!(entry = ec_pdo_find_entry_by_pos_const( - pdo, data.entry_pos))) { - EC_ERR("Entry not found!\n"); - retval = -EINVAL; - break; - } - - data.index = entry->index; - data.subindex = entry->subindex; - data.bit_length = entry->bit_length; - if (entry->name) { - strncpy(data.name, entry->name, EC_IOCTL_STRING_SIZE); - data.name[EC_IOCTL_STRING_SIZE - 1] = 0; - } else { - data.name[0] = 0; - } - - if (copy_to_user((void __user *) arg, &data, sizeof(data))) - retval = -EFAULT; - break; - } - + return ec_cdev_ioctl_config_pdo_entry(master, arg); case EC_IOCTL_CONFIG_SDO: - { - ec_ioctl_config_sdo_t data; - const ec_slave_config_t *sc; - const ec_sdo_request_t *req; - - if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { - retval = -EFAULT; - break; - } - - if (!(sc = ec_master_get_config_const( - master, data.config_index))) { - EC_ERR("Slave config %u does not exist!\n", - data.config_index); - retval = -EINVAL; - break; - } - - if (!(req = ec_slave_config_get_sdo_by_pos_const( - sc, data.sdo_pos))) { - EC_ERR("Invalid Sdo position!\n"); - retval = -EINVAL; - break; - } - - data.index = req->index; - data.subindex = req->subindex; - data.size = req->data_size; - memcpy(&data.data, req->data, min((u32) data.size, (u32) 4)); - - if (copy_to_user((void __user *) arg, &data, sizeof(data))) - retval = -EFAULT; - break; - } - + return ec_cdev_ioctl_config_sdo(master, arg); default: - retval = -ENOTTY; - } - - up(&master->master_sem); - return retval; -} - -/*****************************************************************************/ + return -ENOTTY; + } +} + +/*****************************************************************************/