135 ec_cdev_t *cdev = (ec_cdev_t *) filp->private_data; |
135 ec_cdev_t *cdev = (ec_cdev_t *) filp->private_data; |
136 ec_master_t *master = cdev->master; |
136 ec_master_t *master = cdev->master; |
137 long retval = 0; |
137 long retval = 0; |
138 |
138 |
139 if (master->debug_level) |
139 if (master->debug_level) |
140 EC_DBG("ioctl(filp = %x, cmd = %u, arg = %u)\n", |
140 EC_DBG("ioctl(filp = %x, cmd = %u, arg = %x)\n", |
141 (u32) filp, (u32) cmd, (u32) arg); |
141 (u32) filp, (u32) cmd, (u32) arg); |
142 |
142 |
143 // FIXME lock |
143 // FIXME lock |
144 |
144 |
145 switch (cmd) { |
145 switch (cmd) { |
560 EC_ERR("Slave %u does not exist!\n", data.slave_position); |
560 EC_ERR("Slave %u does not exist!\n", data.slave_position); |
561 retval = -EINVAL; |
561 retval = -EINVAL; |
562 break; |
562 break; |
563 } |
563 } |
564 |
564 |
565 if (!(sdo = ec_slave_get_sdo_by_pos_const( |
565 if (data.sdo_spec <= 0) { |
566 slave, data.sdo_position))) { |
566 if (!(sdo = ec_slave_get_sdo_by_pos_const( |
567 EC_ERR("Sdo %u does not exist in slave %u!\n", |
567 slave, -data.sdo_spec))) { |
568 data.sdo_position, data.slave_position); |
568 EC_ERR("Sdo %u does not exist in slave %u!\n", |
569 retval = -EINVAL; |
569 -data.sdo_spec, data.slave_position); |
570 break; |
570 retval = -EINVAL; |
|
571 break; |
|
572 } |
|
573 } else { |
|
574 if (!(sdo = ec_slave_get_sdo_const( |
|
575 slave, data.sdo_spec))) { |
|
576 EC_ERR("Sdo 0x%04X does not exist in slave %u!\n", |
|
577 data.sdo_spec, data.slave_position); |
|
578 retval = -EINVAL; |
|
579 break; |
|
580 } |
571 } |
581 } |
572 |
582 |
573 if (!(entry = ec_sdo_get_entry_const( |
583 if (!(entry = ec_sdo_get_entry_const( |
574 sdo, data.sdo_entry_subindex))) { |
584 sdo, data.sdo_entry_subindex))) { |
575 EC_ERR("Sdo entry %u does not exist in Sdo %u " |
585 EC_ERR("Sdo entry 0x%04X:%02X does not exist " |
576 "in slave %u!\n", data.sdo_entry_subindex, |
586 "in slave %u!\n", sdo->index, |
577 data.sdo_position, data.slave_position); |
587 data.sdo_entry_subindex, data.slave_position); |
578 retval = -EINVAL; |
588 retval = -EINVAL; |
579 break; |
589 break; |
580 } |
590 } |
581 |
591 |
582 data.data_type = entry->data_type; |
592 data.data_type = entry->data_type; |
596 break; |
606 break; |
597 } |
607 } |
598 break; |
608 break; |
599 } |
609 } |
600 |
610 |
|
611 case EC_IOCTL_SDO_UPLOAD: |
|
612 { |
|
613 ec_ioctl_sdo_upload_t data; |
|
614 ec_master_sdo_request_t request; |
|
615 |
|
616 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
|
617 retval = -EFAULT; |
|
618 break; |
|
619 } |
|
620 |
|
621 if (!(request.slave = ec_master_find_slave( |
|
622 master, 0, data.slave_position))) { |
|
623 EC_ERR("Slave %u does not exist!\n", data.slave_position); |
|
624 retval = -EINVAL; |
|
625 break; |
|
626 } |
|
627 |
|
628 ec_sdo_request_init(&request.req); |
|
629 ec_sdo_request_address(&request.req, |
|
630 data.sdo_index, data.sdo_entry_subindex); |
|
631 ecrt_sdo_request_read(&request.req); |
|
632 |
|
633 // schedule request. |
|
634 down(&master->sdo_sem); |
|
635 list_add_tail(&request.list, &master->slave_sdo_requests); |
|
636 up(&master->sdo_sem); |
|
637 |
|
638 // wait for processing through FSM |
|
639 if (wait_event_interruptible(master->sdo_queue, |
|
640 request.req.state != EC_REQUEST_QUEUED)) { |
|
641 // interrupted by signal |
|
642 down(&master->sdo_sem); |
|
643 if (request.req.state == EC_REQUEST_QUEUED) { |
|
644 list_del(&request.req.list); |
|
645 up(&master->sdo_sem); |
|
646 retval = -EINTR; |
|
647 break; |
|
648 } |
|
649 // request already processing: interrupt not possible. |
|
650 up(&master->sdo_sem); |
|
651 } |
|
652 |
|
653 // wait until master FSM has finished processing |
|
654 wait_event(master->sdo_queue, request.req.state != EC_REQUEST_BUSY); |
|
655 |
|
656 if (request.req.state != EC_REQUEST_SUCCESS) { |
|
657 retval = -EIO; |
|
658 break; |
|
659 } |
|
660 |
|
661 if (request.req.data_size > data.target_size) { |
|
662 EC_ERR("Buffer too small.\n"); |
|
663 retval = -EOVERFLOW; |
|
664 break; |
|
665 } |
|
666 data.data_size = request.req.data_size; |
|
667 |
|
668 if (copy_to_user((void __user *) data.target, |
|
669 request.req.data, data.data_size)) { |
|
670 retval = -EFAULT; |
|
671 break; |
|
672 } |
|
673 if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { |
|
674 retval = -EFAULT; |
|
675 break; |
|
676 } |
|
677 |
|
678 ec_sdo_request_clear(&request.req); |
|
679 break; |
|
680 } |
|
681 |
601 default: |
682 default: |
602 retval = -ENOIOCTLCMD; |
683 retval = -ENOIOCTLCMD; |
603 } |
684 } |
604 |
685 |
605 return retval; |
686 return retval; |