184 ) |
184 ) |
185 { |
185 { |
186 ec_ioctl_master_t data; |
186 ec_ioctl_master_t data; |
187 unsigned int i; |
187 unsigned int i; |
188 |
188 |
189 if (down_interruptible(&master->master_sem)) |
189 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
190 return -EINTR; |
190 return -EINTR; |
191 data.slave_count = master->slave_count; |
191 data.slave_count = master->slave_count; |
192 data.config_count = ec_master_config_count(master); |
192 data.config_count = ec_master_config_count(master); |
193 data.domain_count = ec_master_domain_count(master); |
193 data.domain_count = ec_master_domain_count(master); |
194 #ifdef EC_EOE |
194 #ifdef EC_EOE |
195 data.eoe_handler_count = ec_master_eoe_handler_count(master); |
195 data.eoe_handler_count = ec_master_eoe_handler_count(master); |
196 #endif |
196 #endif |
197 data.phase = (uint8_t) master->phase; |
197 data.phase = (uint8_t) master->phase; |
198 data.active = (uint8_t) master->active; |
198 data.active = (uint8_t) master->active; |
199 data.scan_busy = master->scan_busy; |
199 data.scan_busy = master->scan_busy; |
200 up(&master->master_sem); |
200 ec_mutex_unlock(&master->master_mutex); |
201 |
201 |
202 if (down_interruptible(&master->device_sem)) |
202 if (ec_mutex_lock_interruptible(&master->device_mutex)) |
203 return -EINTR; |
203 return -EINTR; |
204 |
204 |
205 if (master->main_device.dev) { |
205 if (master->main_device.dev) { |
206 memcpy(data.devices[0].address, |
206 memcpy(data.devices[0].address, |
207 master->main_device.dev->dev_addr, ETH_ALEN); |
207 master->main_device.dev->dev_addr, ETH_ALEN); |
353 |
353 |
354 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
354 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
355 return -EFAULT; |
355 return -EFAULT; |
356 } |
356 } |
357 |
357 |
358 if (down_interruptible(&master->master_sem)) |
358 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
359 return -EINTR; |
359 return -EINTR; |
360 |
360 |
361 if (!(slave = ec_master_find_slave_const( |
361 if (!(slave = ec_master_find_slave_const( |
362 master, 0, data.slave_position))) { |
362 master, 0, data.slave_position))) { |
363 up(&master->master_sem); |
363 ec_mutex_unlock(&master->master_mutex); |
364 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
364 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
365 data.slave_position); |
365 data.slave_position); |
366 return -EINVAL; |
366 return -EINVAL; |
367 } |
367 } |
368 |
368 |
369 if (data.sync_index >= slave->sii.sync_count) { |
369 if (data.sync_index >= slave->sii.sync_count) { |
370 up(&master->master_sem); |
370 ec_mutex_unlock(&master->master_mutex); |
371 EC_SLAVE_ERR(slave, "Sync manager %u does not exist!\n", |
371 EC_SLAVE_ERR(slave, "Sync manager %u does not exist!\n", |
372 data.sync_index); |
372 data.sync_index); |
373 return -EINVAL; |
373 return -EINVAL; |
374 } |
374 } |
375 |
375 |
405 |
405 |
406 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
406 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
407 return -EFAULT; |
407 return -EFAULT; |
408 } |
408 } |
409 |
409 |
410 if (down_interruptible(&master->master_sem)) |
410 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
411 return -EINTR; |
411 return -EINTR; |
412 |
412 |
413 if (!(slave = ec_master_find_slave_const( |
413 if (!(slave = ec_master_find_slave_const( |
414 master, 0, data.slave_position))) { |
414 master, 0, data.slave_position))) { |
415 up(&master->master_sem); |
415 ec_mutex_unlock(&master->master_mutex); |
416 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
416 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
417 data.slave_position); |
417 data.slave_position); |
418 return -EINVAL; |
418 return -EINVAL; |
419 } |
419 } |
420 |
420 |
421 if (data.sync_index >= slave->sii.sync_count) { |
421 if (data.sync_index >= slave->sii.sync_count) { |
422 up(&master->master_sem); |
422 ec_mutex_unlock(&master->master_mutex); |
423 EC_SLAVE_ERR(slave, "Sync manager %u does not exist!\n", |
423 EC_SLAVE_ERR(slave, "Sync manager %u does not exist!\n", |
424 data.sync_index); |
424 data.sync_index); |
425 return -EINVAL; |
425 return -EINVAL; |
426 } |
426 } |
427 |
427 |
428 sync = &slave->sii.syncs[data.sync_index]; |
428 sync = &slave->sii.syncs[data.sync_index]; |
429 if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( |
429 if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( |
430 &sync->pdos, data.pdo_pos))) { |
430 &sync->pdos, data.pdo_pos))) { |
431 up(&master->master_sem); |
431 ec_mutex_unlock(&master->master_mutex); |
432 EC_SLAVE_ERR(slave, "Sync manager %u does not contain a PDO with " |
432 EC_SLAVE_ERR(slave, "Sync manager %u does not contain a PDO with " |
433 "position %u!\n", data.sync_index, data.pdo_pos); |
433 "position %u!\n", data.sync_index, data.pdo_pos); |
434 return -EINVAL; |
434 return -EINVAL; |
435 } |
435 } |
436 |
436 |
437 data.index = pdo->index; |
437 data.index = pdo->index; |
438 data.entry_count = ec_pdo_entry_count(pdo); |
438 data.entry_count = ec_pdo_entry_count(pdo); |
439 ec_cdev_strcpy(data.name, pdo->name); |
439 ec_cdev_strcpy(data.name, pdo->name); |
440 |
440 |
441 up(&master->master_sem); |
441 ec_mutex_unlock(&master->master_mutex); |
442 |
442 |
443 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
443 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
444 return -EFAULT; |
444 return -EFAULT; |
445 |
445 |
446 return 0; |
446 return 0; |
463 |
463 |
464 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
464 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
465 return -EFAULT; |
465 return -EFAULT; |
466 } |
466 } |
467 |
467 |
468 if (down_interruptible(&master->master_sem)) |
468 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
469 return -EINTR; |
469 return -EINTR; |
470 |
470 |
471 if (!(slave = ec_master_find_slave_const( |
471 if (!(slave = ec_master_find_slave_const( |
472 master, 0, data.slave_position))) { |
472 master, 0, data.slave_position))) { |
473 up(&master->master_sem); |
473 ec_mutex_unlock(&master->master_mutex); |
474 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
474 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
475 data.slave_position); |
475 data.slave_position); |
476 return -EINVAL; |
476 return -EINVAL; |
477 } |
477 } |
478 |
478 |
479 if (data.sync_index >= slave->sii.sync_count) { |
479 if (data.sync_index >= slave->sii.sync_count) { |
480 up(&master->master_sem); |
480 ec_mutex_unlock(&master->master_mutex); |
481 EC_SLAVE_ERR(slave, "Sync manager %u does not exist!\n", |
481 EC_SLAVE_ERR(slave, "Sync manager %u does not exist!\n", |
482 data.sync_index); |
482 data.sync_index); |
483 return -EINVAL; |
483 return -EINVAL; |
484 } |
484 } |
485 |
485 |
486 sync = &slave->sii.syncs[data.sync_index]; |
486 sync = &slave->sii.syncs[data.sync_index]; |
487 if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( |
487 if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( |
488 &sync->pdos, data.pdo_pos))) { |
488 &sync->pdos, data.pdo_pos))) { |
489 up(&master->master_sem); |
489 ec_mutex_unlock(&master->master_mutex); |
490 EC_SLAVE_ERR(slave, "Sync manager %u does not contain a PDO with " |
490 EC_SLAVE_ERR(slave, "Sync manager %u does not contain a PDO with " |
491 "position %u!\n", data.sync_index, data.pdo_pos); |
491 "position %u!\n", data.sync_index, data.pdo_pos); |
492 return -EINVAL; |
492 return -EINVAL; |
493 } |
493 } |
494 |
494 |
495 if (!(entry = ec_pdo_find_entry_by_pos_const( |
495 if (!(entry = ec_pdo_find_entry_by_pos_const( |
496 pdo, data.entry_pos))) { |
496 pdo, data.entry_pos))) { |
497 up(&master->master_sem); |
497 ec_mutex_unlock(&master->master_mutex); |
498 EC_SLAVE_ERR(slave, "PDO 0x%04X does not contain an entry with " |
498 EC_SLAVE_ERR(slave, "PDO 0x%04X does not contain an entry with " |
499 "position %u!\n", data.pdo_pos, data.entry_pos); |
499 "position %u!\n", data.pdo_pos, data.entry_pos); |
500 return -EINVAL; |
500 return -EINVAL; |
501 } |
501 } |
502 |
502 |
503 data.index = entry->index; |
503 data.index = entry->index; |
504 data.subindex = entry->subindex; |
504 data.subindex = entry->subindex; |
505 data.bit_length = entry->bit_length; |
505 data.bit_length = entry->bit_length; |
506 ec_cdev_strcpy(data.name, entry->name); |
506 ec_cdev_strcpy(data.name, entry->name); |
507 |
507 |
508 up(&master->master_sem); |
508 ec_mutex_unlock(&master->master_mutex); |
509 |
509 |
510 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
510 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
511 return -EFAULT; |
511 return -EFAULT; |
512 |
512 |
513 return 0; |
513 return 0; |
527 |
527 |
528 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
528 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
529 return -EFAULT; |
529 return -EFAULT; |
530 } |
530 } |
531 |
531 |
532 if (down_interruptible(&master->master_sem)) |
532 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
533 return -EINTR; |
533 return -EINTR; |
534 |
534 |
535 if (!(domain = ec_master_find_domain_const(master, data.index))) { |
535 if (!(domain = ec_master_find_domain_const(master, data.index))) { |
536 up(&master->master_sem); |
536 ec_mutex_unlock(&master->master_mutex); |
537 EC_MASTER_ERR(master, "Domain %u does not exist!\n", data.index); |
537 EC_MASTER_ERR(master, "Domain %u does not exist!\n", data.index); |
538 return -EINVAL; |
538 return -EINVAL; |
539 } |
539 } |
540 |
540 |
541 data.data_size = domain->data_size; |
541 data.data_size = domain->data_size; |
|
542 data.tx_size = domain->tx_size; |
542 data.logical_base_address = domain->logical_base_address; |
543 data.logical_base_address = domain->logical_base_address; |
543 data.working_counter = domain->working_counter; |
544 data.working_counter = domain->working_counter; |
544 data.expected_working_counter = domain->expected_working_counter; |
545 data.expected_working_counter = domain->expected_working_counter; |
545 data.fmmu_count = ec_domain_fmmu_count(domain); |
546 data.fmmu_count = ec_domain_fmmu_count(domain); |
546 |
547 |
547 up(&master->master_sem); |
548 ec_mutex_unlock(&master->master_mutex); |
548 |
549 |
549 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
550 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
550 return -EFAULT; |
551 return -EFAULT; |
551 |
552 |
552 return 0; |
553 return 0; |
567 |
568 |
568 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
569 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
569 return -EFAULT; |
570 return -EFAULT; |
570 } |
571 } |
571 |
572 |
572 if (down_interruptible(&master->master_sem)) |
573 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
573 return -EINTR; |
574 return -EINTR; |
574 |
575 |
575 if (!(domain = ec_master_find_domain_const(master, data.domain_index))) { |
576 if (!(domain = ec_master_find_domain_const(master, data.domain_index))) { |
576 up(&master->master_sem); |
577 ec_mutex_unlock(&master->master_mutex); |
577 EC_MASTER_ERR(master, "Domain %u does not exist!\n", |
578 EC_MASTER_ERR(master, "Domain %u does not exist!\n", |
578 data.domain_index); |
579 data.domain_index); |
579 return -EINVAL; |
580 return -EINVAL; |
580 } |
581 } |
581 |
582 |
582 if (!(fmmu = ec_domain_find_fmmu(domain, data.fmmu_index))) { |
583 if (!(fmmu = ec_domain_find_fmmu(domain, data.fmmu_index))) { |
583 up(&master->master_sem); |
584 ec_mutex_unlock(&master->master_mutex); |
584 EC_MASTER_ERR(master, "Domain %u has less than %u" |
585 EC_MASTER_ERR(master, "Domain %u has less than %u" |
585 " fmmu configurations.\n", |
586 " fmmu configurations.\n", |
586 data.domain_index, data.fmmu_index + 1); |
587 data.domain_index, data.fmmu_index + 1); |
587 return -EINVAL; |
588 return -EINVAL; |
588 } |
589 } |
616 |
618 |
617 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
619 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
618 return -EFAULT; |
620 return -EFAULT; |
619 } |
621 } |
620 |
622 |
621 if (down_interruptible(&master->master_sem)) |
623 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
622 return -EINTR; |
624 return -EINTR; |
623 |
625 |
624 if (!(domain = ec_master_find_domain_const(master, data.domain_index))) { |
626 if (!(domain = ec_master_find_domain_const(master, data.domain_index))) { |
625 up(&master->master_sem); |
627 ec_mutex_unlock(&master->master_mutex); |
626 EC_MASTER_ERR(master, "Domain %u does not exist!\n", |
628 EC_MASTER_ERR(master, "Domain %u does not exist!\n", |
627 data.domain_index); |
629 data.domain_index); |
628 return -EINVAL; |
630 return -EINVAL; |
629 } |
631 } |
630 |
632 |
631 if (domain->data_size != data.data_size) { |
633 if (domain->data_size != data.data_size) { |
632 up(&master->master_sem); |
634 ec_mutex_unlock(&master->master_mutex); |
633 EC_MASTER_ERR(master, "Data size mismatch %u/%zu!\n", |
635 EC_MASTER_ERR(master, "Data size mismatch %u/%zu!\n", |
634 data.data_size, domain->data_size); |
636 data.data_size, domain->data_size); |
635 return -EFAULT; |
637 return -EFAULT; |
636 } |
638 } |
637 |
639 |
638 if (copy_to_user((void __user *) data.target, domain->data, |
640 if (copy_to_user((void __user *) data.target, domain->data, |
639 domain->data_size)) { |
641 domain->data_size)) { |
640 up(&master->master_sem); |
642 ec_mutex_unlock(&master->master_mutex); |
641 return -EFAULT; |
643 return -EFAULT; |
642 } |
644 } |
643 |
645 |
644 up(&master->master_sem); |
646 ec_mutex_unlock(&master->master_mutex); |
645 return 0; |
647 return 0; |
646 } |
648 } |
647 |
649 |
648 /*****************************************************************************/ |
650 /*****************************************************************************/ |
649 |
651 |
684 |
686 |
685 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
687 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
686 return -EFAULT; |
688 return -EFAULT; |
687 } |
689 } |
688 |
690 |
689 if (down_interruptible(&master->master_sem)) |
691 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
690 return -EINTR; |
692 return -EINTR; |
691 |
693 |
692 if (!(slave = ec_master_find_slave( |
694 if (!(slave = ec_master_find_slave( |
693 master, 0, data.slave_position))) { |
695 master, 0, data.slave_position))) { |
694 up(&master->master_sem); |
696 ec_mutex_unlock(&master->master_mutex); |
695 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
697 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
696 data.slave_position); |
698 data.slave_position); |
697 return -EINVAL; |
699 return -EINVAL; |
698 } |
700 } |
699 |
701 |
700 ec_slave_request_state(slave, data.al_state); |
702 ec_slave_request_state(slave, data.al_state); |
701 |
703 |
702 up(&master->master_sem); |
704 ec_mutex_unlock(&master->master_mutex); |
703 return 0; |
705 return 0; |
704 } |
706 } |
705 |
707 |
706 /*****************************************************************************/ |
708 /*****************************************************************************/ |
707 |
709 |
718 |
720 |
719 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
721 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
720 return -EFAULT; |
722 return -EFAULT; |
721 } |
723 } |
722 |
724 |
723 if (down_interruptible(&master->master_sem)) |
725 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
724 return -EINTR; |
726 return -EINTR; |
725 |
727 |
726 if (!(slave = ec_master_find_slave_const( |
728 if (!(slave = ec_master_find_slave_const( |
727 master, 0, data.slave_position))) { |
729 master, 0, data.slave_position))) { |
728 up(&master->master_sem); |
730 ec_mutex_unlock(&master->master_mutex); |
729 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
731 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
730 data.slave_position); |
732 data.slave_position); |
731 return -EINVAL; |
733 return -EINVAL; |
732 } |
734 } |
733 |
735 |
734 if (!(sdo = ec_slave_get_sdo_by_pos_const( |
736 if (!(sdo = ec_slave_get_sdo_by_pos_const( |
735 slave, data.sdo_position))) { |
737 slave, data.sdo_position))) { |
736 up(&master->master_sem); |
738 ec_mutex_unlock(&master->master_mutex); |
737 EC_SLAVE_ERR(slave, "SDO %u does not exist!\n", data.sdo_position); |
739 EC_SLAVE_ERR(slave, "SDO %u does not exist!\n", data.sdo_position); |
738 return -EINVAL; |
740 return -EINVAL; |
739 } |
741 } |
740 |
742 |
741 data.sdo_index = sdo->index; |
743 data.sdo_index = sdo->index; |
742 data.max_subindex = sdo->max_subindex; |
744 data.max_subindex = sdo->max_subindex; |
743 ec_cdev_strcpy(data.name, sdo->name); |
745 ec_cdev_strcpy(data.name, sdo->name); |
744 |
746 |
745 up(&master->master_sem); |
747 ec_mutex_unlock(&master->master_mutex); |
746 |
748 |
747 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
749 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
748 return -EFAULT; |
750 return -EFAULT; |
749 |
751 |
750 return 0; |
752 return 0; |
766 |
768 |
767 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
769 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
768 return -EFAULT; |
770 return -EFAULT; |
769 } |
771 } |
770 |
772 |
771 if (down_interruptible(&master->master_sem)) |
773 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
772 return -EINTR; |
774 return -EINTR; |
773 |
775 |
774 if (!(slave = ec_master_find_slave_const( |
776 if (!(slave = ec_master_find_slave_const( |
775 master, 0, data.slave_position))) { |
777 master, 0, data.slave_position))) { |
776 up(&master->master_sem); |
778 ec_mutex_unlock(&master->master_mutex); |
777 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
779 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
778 data.slave_position); |
780 data.slave_position); |
779 return -EINVAL; |
781 return -EINVAL; |
780 } |
782 } |
781 |
783 |
782 if (data.sdo_spec <= 0) { |
784 if (data.sdo_spec <= 0) { |
783 if (!(sdo = ec_slave_get_sdo_by_pos_const( |
785 if (!(sdo = ec_slave_get_sdo_by_pos_const( |
784 slave, -data.sdo_spec))) { |
786 slave, -data.sdo_spec))) { |
785 up(&master->master_sem); |
787 ec_mutex_unlock(&master->master_mutex); |
786 EC_SLAVE_ERR(slave, "SDO %u does not exist!\n", -data.sdo_spec); |
788 EC_SLAVE_ERR(slave, "SDO %u does not exist!\n", -data.sdo_spec); |
787 return -EINVAL; |
789 return -EINVAL; |
788 } |
790 } |
789 } else { |
791 } else { |
790 if (!(sdo = ec_slave_get_sdo_const( |
792 if (!(sdo = ec_slave_get_sdo_const( |
791 slave, data.sdo_spec))) { |
793 slave, data.sdo_spec))) { |
792 up(&master->master_sem); |
794 ec_mutex_unlock(&master->master_mutex); |
793 EC_SLAVE_ERR(slave, "SDO 0x%04X does not exist!\n", |
795 EC_SLAVE_ERR(slave, "SDO 0x%04X does not exist!\n", |
794 data.sdo_spec); |
796 data.sdo_spec); |
795 return -EINVAL; |
797 return -EINVAL; |
796 } |
798 } |
797 } |
799 } |
798 |
800 |
799 if (!(entry = ec_sdo_get_entry_const( |
801 if (!(entry = ec_sdo_get_entry_const( |
800 sdo, data.sdo_entry_subindex))) { |
802 sdo, data.sdo_entry_subindex))) { |
801 up(&master->master_sem); |
803 ec_mutex_unlock(&master->master_mutex); |
802 EC_SLAVE_ERR(slave, "SDO entry 0x%04X:%02X does not exist!\n", |
804 EC_SLAVE_ERR(slave, "SDO entry 0x%04X:%02X does not exist!\n", |
803 sdo->index, data.sdo_entry_subindex); |
805 sdo->index, data.sdo_entry_subindex); |
804 return -EINVAL; |
806 return -EINVAL; |
805 } |
807 } |
806 |
808 |
836 ec_master_t *master, /**< EtherCAT master. */ |
838 ec_master_t *master, /**< EtherCAT master. */ |
837 unsigned long arg /**< ioctl() argument. */ |
839 unsigned long arg /**< ioctl() argument. */ |
838 ) |
840 ) |
839 { |
841 { |
840 ec_ioctl_slave_sdo_upload_t data; |
842 ec_ioctl_slave_sdo_upload_t data; |
841 ec_master_sdo_request_t request; |
843 ec_master_sdo_request_t* request; |
842 int retval; |
844 int retval; |
843 |
845 |
844 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
846 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
845 return -EFAULT; |
847 return -EFAULT; |
846 } |
848 } |
847 |
849 |
848 ec_sdo_request_init(&request.req); |
850 request = kmalloc(sizeof(*request), GFP_KERNEL); |
849 ec_sdo_request_address(&request.req, |
851 if (!request) |
850 data.sdo_index, data.sdo_entry_subindex); |
852 return -ENOMEM; |
851 ecrt_sdo_request_read(&request.req); |
853 kref_init(&request->refcount); |
852 |
854 |
853 if (down_interruptible(&master->master_sem)) |
855 ec_sdo_request_init(&request->req); |
854 return -EINTR; |
856 ec_sdo_request_address(&request->req, |
855 |
857 data.sdo_index, data.sdo_entry_subindex); |
856 if (!(request.slave = ec_master_find_slave( |
858 ecrt_sdo_request_read(&request->req); |
|
859 |
|
860 if (ec_mutex_lock_interruptible(&master->master_mutex)) { |
|
861 kref_put(&request->refcount,ec_master_sdo_request_release); |
|
862 return -EINTR; |
|
863 } |
|
864 if (!(request->slave = ec_master_find_slave( |
857 master, 0, data.slave_position))) { |
865 master, 0, data.slave_position))) { |
858 up(&master->master_sem); |
866 ec_mutex_unlock(&master->master_mutex); |
859 ec_sdo_request_clear(&request.req); |
|
860 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
867 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
861 data.slave_position); |
868 data.slave_position); |
862 return -EINVAL; |
869 kref_put(&request->refcount,ec_master_sdo_request_release); |
863 } |
870 return -EINVAL; |
864 |
871 } |
865 EC_SLAVE_DBG(request.slave, 1, "Schedule SDO upload request.\n"); |
872 |
|
873 EC_SLAVE_DBG(request->slave, 1, "Schedule SDO upload request %p.\n",request); |
866 |
874 |
867 // schedule request. |
875 // schedule request. |
868 list_add_tail(&request.list, &request.slave->slave_sdo_requests); |
876 kref_get(&request->refcount); |
869 |
877 list_add_tail(&request->list, &request->slave->slave_sdo_requests); |
870 up(&master->master_sem); |
878 |
|
879 ec_mutex_unlock(&master->master_mutex); |
871 |
880 |
872 // wait for processing through FSM |
881 // wait for processing through FSM |
873 if (wait_event_interruptible(request.slave->sdo_queue, |
882 if (wait_event_interruptible(request->slave->sdo_queue, |
874 request.req.state != EC_INT_REQUEST_QUEUED)) { |
883 ((request->req.state == EC_INT_REQUEST_SUCCESS) || (request->req.state == EC_INT_REQUEST_FAILURE)))) { |
875 // interrupted by signal |
884 // interrupted by signal |
876 down(&master->master_sem); |
885 kref_put(&request->refcount,ec_master_sdo_request_release); |
877 if (request.req.state == EC_INT_REQUEST_QUEUED) { |
886 return -EINTR; |
878 list_del(&request.list); |
887 } |
879 up(&master->master_sem); |
888 |
880 ec_sdo_request_clear(&request.req); |
889 EC_SLAVE_DBG(request->slave, 1, "Finished SDO upload request %p.\n",request); |
881 return -EINTR; |
890 |
882 } |
891 data.abort_code = request->req.abort_code; |
883 // request already processing: interrupt not possible. |
892 |
884 up(&master->master_sem); |
893 if (request->req.state != EC_INT_REQUEST_SUCCESS) { |
885 } |
|
886 |
|
887 // wait until master FSM has finished processing |
|
888 wait_event(request.slave->sdo_queue, |
|
889 request.req.state != EC_INT_REQUEST_BUSY); |
|
890 |
|
891 EC_SLAVE_DBG(request.slave, 1, "Finished SDO upload request.\n"); |
|
892 |
|
893 data.abort_code = request.req.abort_code; |
|
894 |
|
895 if (request.req.state != EC_INT_REQUEST_SUCCESS) { |
|
896 data.data_size = 0; |
894 data.data_size = 0; |
897 if (request.req.errno) { |
895 if (request->req.errno) { |
898 retval = -request.req.errno; |
896 retval = -request->req.errno; |
899 } else { |
897 } else { |
900 retval = -EIO; |
898 retval = -EIO; |
901 } |
899 } |
902 } else { |
900 } else { |
903 if (request.req.data_size > data.target_size) { |
901 if (request->req.data_size > data.target_size) { |
904 EC_MASTER_ERR(master, "Buffer too small.\n"); |
902 EC_MASTER_ERR(master, "Buffer too small.\n"); |
905 ec_sdo_request_clear(&request.req); |
903 kref_put(&request->refcount,ec_master_sdo_request_release); |
906 return -EOVERFLOW; |
904 return -EOVERFLOW; |
907 } |
905 } |
908 data.data_size = request.req.data_size; |
906 data.data_size = request->req.data_size; |
909 |
907 |
910 if (copy_to_user((void __user *) data.target, |
908 if (copy_to_user((void __user *) data.target, |
911 request.req.data, data.data_size)) { |
909 request->req.data, data.data_size)) { |
912 ec_sdo_request_clear(&request.req); |
910 kref_put(&request->refcount,ec_master_sdo_request_release); |
913 return -EFAULT; |
911 return -EFAULT; |
914 } |
912 } |
915 retval = 0; |
913 retval = 0; |
916 } |
914 } |
917 |
915 |
918 if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { |
916 if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { |
919 retval = -EFAULT; |
917 retval = -EFAULT; |
920 } |
918 } |
921 |
919 |
922 ec_sdo_request_clear(&request.req); |
920 kref_put(&request->refcount,ec_master_sdo_request_release); |
923 return retval; |
921 return retval; |
924 } |
922 } |
925 |
923 |
926 /*****************************************************************************/ |
924 /*****************************************************************************/ |
927 |
925 |
944 if (!data.data_size) { |
942 if (!data.data_size) { |
945 EC_MASTER_ERR(master, "Zero data size!\n"); |
943 EC_MASTER_ERR(master, "Zero data size!\n"); |
946 return -EINVAL; |
944 return -EINVAL; |
947 } |
945 } |
948 |
946 |
949 ec_sdo_request_init(&request.req); |
947 request = kmalloc(sizeof(*request), GFP_KERNEL); |
950 ec_sdo_request_address(&request.req, |
948 if (!request) |
|
949 return -ENOMEM; |
|
950 kref_init(&request->refcount); |
|
951 |
|
952 ec_sdo_request_init(&request->req); |
|
953 ec_sdo_request_address(&request->req, |
951 data.sdo_index, data.sdo_entry_subindex); |
954 data.sdo_index, data.sdo_entry_subindex); |
952 if (ec_sdo_request_alloc(&request.req, data.data_size)) { |
955 if (ec_sdo_request_alloc(&request->req, data.data_size)) { |
953 ec_sdo_request_clear(&request.req); |
956 kref_put(&request->refcount,ec_master_sdo_request_release); |
954 return -ENOMEM; |
957 return -ENOMEM; |
955 } |
958 } |
956 if (copy_from_user(request.req.data, |
959 if (copy_from_user(request->req.data, |
957 (void __user *) data.data, data.data_size)) { |
960 (void __user *) data.data, data.data_size)) { |
958 ec_sdo_request_clear(&request.req); |
961 kref_put(&request->refcount,ec_master_sdo_request_release); |
959 return -EFAULT; |
962 return -EFAULT; |
960 } |
963 } |
961 request.req.data_size = data.data_size; |
964 request->req.data_size = data.data_size; |
962 ecrt_sdo_request_write(&request.req); |
965 ecrt_sdo_request_write(&request->req); |
963 |
966 |
964 if (down_interruptible(&master->master_sem)) |
967 if (ec_mutex_lock_interruptible(&master->master_mutex)) { |
965 return -EINTR; |
968 kref_put(&request->refcount,ec_master_sdo_request_release); |
966 |
969 return -EINTR; |
967 if (!(request.slave = ec_master_find_slave( |
970 } |
|
971 if (!(request->slave = ec_master_find_slave( |
968 master, 0, data.slave_position))) { |
972 master, 0, data.slave_position))) { |
969 up(&master->master_sem); |
973 ec_mutex_unlock(&master->master_mutex); |
970 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
974 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
971 data.slave_position); |
975 data.slave_position); |
972 ec_sdo_request_clear(&request.req); |
976 kref_put(&request->refcount,ec_master_sdo_request_release); |
973 return -EINVAL; |
977 return -EINVAL; |
974 } |
978 } |
975 |
979 |
976 EC_SLAVE_DBG(request.slave, 1, "Schedule SDO download request.\n"); |
980 EC_SLAVE_DBG(request->slave, 1, "Schedule SDO download request %p.\n",request); |
977 |
981 |
978 // schedule request. |
982 // schedule request. |
979 list_add_tail(&request.list, &request.slave->slave_sdo_requests); |
983 kref_get(&request->refcount); |
980 |
984 list_add_tail(&request->list, &request->slave->slave_sdo_requests); |
981 up(&master->master_sem); |
985 |
|
986 ec_mutex_unlock(&master->master_mutex); |
982 |
987 |
983 // wait for processing through FSM |
988 // wait for processing through FSM |
984 if (wait_event_interruptible(request.slave->sdo_queue, |
989 if (wait_event_interruptible(request->slave->sdo_queue, |
985 request.req.state != EC_INT_REQUEST_QUEUED)) { |
990 ((request->req.state == EC_INT_REQUEST_SUCCESS) || (request->req.state == EC_INT_REQUEST_FAILURE)))) { |
986 // interrupted by signal |
991 // interrupted by signal |
987 down(&master->master_sem); |
992 kref_put(&request->refcount,ec_master_sdo_request_release); |
988 if (request.req.state == EC_INT_REQUEST_QUEUED) { |
993 return -EINTR; |
989 list_del(&request.list); |
994 } |
990 up(&master->master_sem); |
995 |
991 ec_sdo_request_clear(&request.req); |
996 EC_SLAVE_DBG(request->slave, 1, "Finished SDO download request %p.\n",request); |
992 return -EINTR; |
997 |
993 } |
998 data.abort_code = request->req.abort_code; |
994 // request already processing: interrupt not possible. |
999 |
995 up(&master->master_sem); |
1000 if (request->req.state == EC_INT_REQUEST_SUCCESS) { |
996 } |
|
997 |
|
998 // wait until master FSM has finished processing |
|
999 wait_event(request.slave->sdo_queue, |
|
1000 request.req.state != EC_INT_REQUEST_BUSY); |
|
1001 |
|
1002 EC_SLAVE_DBG(request.slave, 1, "Finished SDO download request.\n"); |
|
1003 |
|
1004 data.abort_code = request.req.abort_code; |
|
1005 |
|
1006 if (request.req.state == EC_INT_REQUEST_SUCCESS) { |
|
1007 retval = 0; |
1001 retval = 0; |
1008 } else if (request.req.errno) { |
1002 } else if (request->req.errno) { |
1009 retval = -request.req.errno; |
1003 retval = -request->req.errno; |
1010 } else { |
1004 } else { |
1011 retval = -EIO; |
1005 retval = -EIO; |
1012 } |
1006 } |
1013 |
1007 |
1014 if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { |
1008 if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { |
1015 retval = -EFAULT; |
1009 retval = -EFAULT; |
1016 } |
1010 } |
1017 |
1011 |
1018 ec_sdo_request_clear(&request.req); |
1012 kref_put(&request->refcount,ec_master_sdo_request_release); |
1019 return retval; |
1013 return retval; |
1020 } |
1014 } |
1021 |
1015 |
1022 /*****************************************************************************/ |
1016 /*****************************************************************************/ |
1023 |
1017 |
1034 |
1028 |
1035 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
1029 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
1036 return -EFAULT; |
1030 return -EFAULT; |
1037 } |
1031 } |
1038 |
1032 |
1039 if (down_interruptible(&master->master_sem)) |
1033 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
1040 return -EINTR; |
1034 return -EINTR; |
1041 |
1035 |
1042 if (!(slave = ec_master_find_slave_const( |
1036 if (!(slave = ec_master_find_slave_const( |
1043 master, 0, data.slave_position))) { |
1037 master, 0, data.slave_position))) { |
1044 up(&master->master_sem); |
1038 ec_mutex_unlock(&master->master_mutex); |
1045 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
1039 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
1046 data.slave_position); |
1040 data.slave_position); |
1047 return -EINVAL; |
1041 return -EINVAL; |
1048 } |
1042 } |
1049 |
1043 |
1050 if (!data.nwords |
1044 if (!data.nwords |
1051 || data.offset + data.nwords > slave->sii_nwords) { |
1045 || data.offset + data.nwords > slave->sii_nwords) { |
1052 up(&master->master_sem); |
1046 ec_mutex_unlock(&master->master_mutex); |
1053 EC_SLAVE_ERR(slave, "Invalid SII read offset/size %u/%u for slave SII" |
1047 EC_SLAVE_ERR(slave, "Invalid SII read offset/size %u/%u for slave SII" |
1054 " size %zu!\n", data.offset, data.nwords, slave->sii_nwords); |
1048 " size %zu!\n", data.offset, data.nwords, slave->sii_nwords); |
1055 return -EINVAL; |
1049 return -EINVAL; |
1056 } |
1050 } |
1057 |
1051 |
1092 EC_MASTER_ERR(master, "Failed to allocate %u bytes" |
1087 EC_MASTER_ERR(master, "Failed to allocate %u bytes" |
1093 " for SII contents.\n", byte_size); |
1088 " for SII contents.\n", byte_size); |
1094 return -ENOMEM; |
1089 return -ENOMEM; |
1095 } |
1090 } |
1096 |
1091 |
|
1092 request = kmalloc(sizeof(*request), GFP_KERNEL); |
|
1093 if (!request) |
|
1094 return -ENOMEM; |
|
1095 kref_init(&request->refcount); |
|
1096 // init SII write request |
|
1097 INIT_LIST_HEAD(&request->list); |
|
1098 request->words = words; // now "owned" by request, see ec_master_sii_write_request_release |
|
1099 request->offset = data.offset; |
|
1100 request->nwords = data.nwords; |
|
1101 |
1097 if (copy_from_user(words, |
1102 if (copy_from_user(words, |
1098 (void __user *) data.words, byte_size)) { |
1103 (void __user *) data.words, byte_size)) { |
1099 kfree(words); |
1104 kref_put(&request->refcount,ec_master_sii_write_request_release); |
1100 return -EFAULT; |
1105 return -EFAULT; |
1101 } |
1106 } |
1102 |
1107 |
1103 if (down_interruptible(&master->master_sem)) |
1108 if (ec_mutex_lock_interruptible(&master->master_mutex)) { |
1104 return -EINTR; |
1109 kref_put(&request->refcount,ec_master_sii_write_request_release); |
1105 |
1110 return -EINTR; |
|
1111 } |
1106 if (!(slave = ec_master_find_slave( |
1112 if (!(slave = ec_master_find_slave( |
1107 master, 0, data.slave_position))) { |
1113 master, 0, data.slave_position))) { |
1108 up(&master->master_sem); |
1114 ec_mutex_unlock(&master->master_mutex); |
1109 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
1115 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
1110 data.slave_position); |
1116 data.slave_position); |
1111 kfree(words); |
1117 kref_put(&request->refcount,ec_master_sii_write_request_release); |
1112 return -EINVAL; |
1118 return -EINVAL; |
1113 } |
1119 } |
1114 |
1120 |
1115 // init SII write request |
1121 request->slave = slave; |
1116 INIT_LIST_HEAD(&request.list); |
1122 request->state = EC_INT_REQUEST_QUEUED; |
1117 request.slave = slave; |
|
1118 request.words = words; |
|
1119 request.offset = data.offset; |
|
1120 request.nwords = data.nwords; |
|
1121 request.state = EC_INT_REQUEST_QUEUED; |
|
1122 |
1123 |
1123 // schedule SII write request. |
1124 // schedule SII write request. |
1124 list_add_tail(&request.list, &master->sii_requests); |
1125 list_add_tail(&request->list, &master->sii_requests); |
1125 |
1126 kref_get(&request->refcount); |
1126 up(&master->master_sem); |
1127 |
|
1128 ec_mutex_unlock(&master->master_mutex); |
1127 |
1129 |
1128 // wait for processing through FSM |
1130 // wait for processing through FSM |
1129 if (wait_event_interruptible(master->sii_queue, |
1131 if (wait_event_interruptible(master->sii_queue, |
1130 request.state != EC_INT_REQUEST_QUEUED)) { |
1132 ((request->state == EC_INT_REQUEST_SUCCESS) || (request->state == EC_INT_REQUEST_FAILURE)))) { |
1131 // interrupted by signal |
1133 // interrupted by signal |
1132 down(&master->master_sem); |
1134 kref_put(&request->refcount,ec_master_sii_write_request_release); |
1133 if (request.state == EC_INT_REQUEST_QUEUED) { |
1135 return -EINTR; |
1134 // abort request |
1136 } |
1135 list_del(&request.list); |
1137 |
1136 up(&master->master_sem); |
1138 |
1137 kfree(words); |
1139 retval = request->state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO; |
1138 return -EINTR; |
1140 kref_put(&request->refcount,ec_master_sii_write_request_release); |
1139 } |
1141 |
1140 up(&master->master_sem); |
1142 return retval; |
1141 } |
|
1142 |
|
1143 // wait until master FSM has finished processing |
|
1144 wait_event(master->sii_queue, request.state != EC_INT_REQUEST_BUSY); |
|
1145 |
|
1146 kfree(words); |
|
1147 |
|
1148 return request.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO; |
|
1149 } |
1143 } |
1150 |
1144 |
1151 /*****************************************************************************/ |
1145 /*****************************************************************************/ |
1152 |
1146 |
1153 /** Read a slave's registers. |
1147 /** Read a slave's registers. |
1173 EC_MASTER_ERR(master, "Failed to allocate %u bytes" |
1168 EC_MASTER_ERR(master, "Failed to allocate %u bytes" |
1174 " for register data.\n", data.length); |
1169 " for register data.\n", data.length); |
1175 return -ENOMEM; |
1170 return -ENOMEM; |
1176 } |
1171 } |
1177 |
1172 |
1178 if (down_interruptible(&master->master_sem)) |
1173 request = kmalloc(sizeof(*request), GFP_KERNEL); |
1179 return -EINTR; |
1174 if (!request) |
1180 |
1175 return -ENOMEM; |
|
1176 kref_init(&request->refcount); |
|
1177 |
|
1178 // init register request |
|
1179 INIT_LIST_HEAD(&request->list); |
|
1180 request->dir = EC_DIR_INPUT; |
|
1181 request->data = contents; // now "owned" by request, see ec_master_reg_request_release |
|
1182 request->offset = data.offset; |
|
1183 request->length = data.length; |
|
1184 |
|
1185 if (ec_mutex_lock_interruptible(&master->master_mutex)) { |
|
1186 kref_put(&request->refcount,ec_master_reg_request_release); |
|
1187 return -EINTR; |
|
1188 } |
1181 if (!(slave = ec_master_find_slave( |
1189 if (!(slave = ec_master_find_slave( |
1182 master, 0, data.slave_position))) { |
1190 master, 0, data.slave_position))) { |
1183 up(&master->master_sem); |
1191 ec_mutex_unlock(&master->master_mutex); |
1184 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
1192 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
1185 data.slave_position); |
1193 data.slave_position); |
1186 return -EINVAL; |
1194 kref_put(&request->refcount,ec_master_reg_request_release); |
1187 } |
1195 return -EINVAL; |
1188 |
1196 } |
1189 // init register request |
1197 |
1190 INIT_LIST_HEAD(&request.list); |
1198 request->slave = slave; |
1191 request.slave = slave; |
1199 request->state = EC_INT_REQUEST_QUEUED; |
1192 request.dir = EC_DIR_INPUT; |
|
1193 request.data = contents; |
|
1194 request.offset = data.offset; |
|
1195 request.length = data.length; |
|
1196 request.state = EC_INT_REQUEST_QUEUED; |
|
1197 |
1200 |
1198 // schedule request. |
1201 // schedule request. |
1199 list_add_tail(&request.list, &master->reg_requests); |
1202 list_add_tail(&request->list, &master->reg_requests); |
1200 |
1203 kref_get(&request->refcount); |
1201 up(&master->master_sem); |
1204 |
|
1205 ec_mutex_unlock(&master->master_mutex); |
1202 |
1206 |
1203 // wait for processing through FSM |
1207 // wait for processing through FSM |
1204 if (wait_event_interruptible(master->reg_queue, |
1208 if (wait_event_interruptible(master->reg_queue, |
1205 request.state != EC_INT_REQUEST_QUEUED)) { |
1209 ((request->state == EC_INT_REQUEST_SUCCESS) || (request->state == EC_INT_REQUEST_FAILURE)))) { |
1206 // interrupted by signal |
1210 // interrupted by signal |
1207 down(&master->master_sem); |
1211 kref_put(&request->refcount,ec_master_reg_request_release); |
1208 if (request.state == EC_INT_REQUEST_QUEUED) { |
1212 return -EINTR; |
1209 // abort request |
1213 } |
1210 list_del(&request.list); |
1214 |
1211 up(&master->master_sem); |
1215 if (request->state == EC_INT_REQUEST_SUCCESS) { |
1212 kfree(contents); |
1216 if (copy_to_user((void __user *) data.data, request->data, data.length)) { |
1213 return -EINTR; |
1217 kref_put(&request->refcount,ec_master_reg_request_release); |
|
1218 return -EFAULT; |
1214 } |
1219 } |
1215 up(&master->master_sem); |
1220 } |
1216 } |
1221 retval = request->state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO; |
1217 |
1222 |
1218 // wait until master FSM has finished processing |
1223 kref_put(&request->refcount,ec_master_reg_request_release); |
1219 wait_event(master->reg_queue, request.state != EC_INT_REQUEST_BUSY); |
1224 return retval; |
1220 |
|
1221 if (request.state == EC_INT_REQUEST_SUCCESS) { |
|
1222 if (copy_to_user((void __user *) data.data, contents, data.length)) |
|
1223 return -EFAULT; |
|
1224 } |
|
1225 kfree(contents); |
|
1226 |
|
1227 return request.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO; |
|
1228 } |
1225 } |
1229 |
1226 |
1230 /*****************************************************************************/ |
1227 /*****************************************************************************/ |
1231 |
1228 |
1232 /** Write a slave's registers. |
1229 /** Write a slave's registers. |
1257 if (copy_from_user(contents, (void __user *) data.data, data.length)) { |
1255 if (copy_from_user(contents, (void __user *) data.data, data.length)) { |
1258 kfree(contents); |
1256 kfree(contents); |
1259 return -EFAULT; |
1257 return -EFAULT; |
1260 } |
1258 } |
1261 |
1259 |
1262 if (down_interruptible(&master->master_sem)) |
1260 request = kmalloc(sizeof(*request), GFP_KERNEL); |
1263 return -EINTR; |
1261 if (!request) |
|
1262 return -ENOMEM; |
|
1263 kref_init(&request->refcount); |
|
1264 // init register request |
|
1265 INIT_LIST_HEAD(&request->list); |
|
1266 request->dir = EC_DIR_OUTPUT; |
|
1267 request->data = contents; // now "owned" by request, see ec_master_reg_request_release |
|
1268 request->offset = data.offset; |
|
1269 request->length = data.length; |
|
1270 |
|
1271 if (ec_mutex_lock_interruptible(&master->master_mutex)) { |
|
1272 kref_put(&request->refcount,ec_master_reg_request_release); |
|
1273 return -EINTR; |
|
1274 } |
1264 |
1275 |
1265 if (!(slave = ec_master_find_slave( |
1276 if (!(slave = ec_master_find_slave( |
1266 master, 0, data.slave_position))) { |
1277 master, 0, data.slave_position))) { |
1267 up(&master->master_sem); |
1278 ec_mutex_unlock(&master->master_mutex); |
1268 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
1279 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
1269 data.slave_position); |
1280 data.slave_position); |
1270 kfree(contents); |
1281 kref_put(&request->refcount,ec_master_reg_request_release); |
1271 return -EINVAL; |
1282 return -EINVAL; |
1272 } |
1283 } |
1273 |
1284 |
1274 // init register request |
1285 request->slave = slave; |
1275 INIT_LIST_HEAD(&request.list); |
1286 request->state = EC_INT_REQUEST_QUEUED; |
1276 request.slave = slave; |
|
1277 request.dir = EC_DIR_OUTPUT; |
|
1278 request.data = contents; |
|
1279 request.offset = data.offset; |
|
1280 request.length = data.length; |
|
1281 request.state = EC_INT_REQUEST_QUEUED; |
|
1282 |
1287 |
1283 // schedule request. |
1288 // schedule request. |
1284 list_add_tail(&request.list, &master->reg_requests); |
1289 list_add_tail(&request->list, &master->reg_requests); |
1285 |
1290 kref_get(&request->refcount); |
1286 up(&master->master_sem); |
1291 |
|
1292 ec_mutex_unlock(&master->master_mutex); |
1287 |
1293 |
1288 // wait for processing through FSM |
1294 // wait for processing through FSM |
1289 if (wait_event_interruptible(master->reg_queue, |
1295 if (wait_event_interruptible(master->reg_queue, |
1290 request.state != EC_INT_REQUEST_QUEUED)) { |
1296 ((request->state == EC_INT_REQUEST_SUCCESS) || (request->state == EC_INT_REQUEST_FAILURE)))) { |
1291 // interrupted by signal |
1297 // interrupted by signal |
1292 down(&master->master_sem); |
1298 kref_put(&request->refcount,ec_master_reg_request_release); |
1293 if (request.state == EC_INT_REQUEST_QUEUED) { |
1299 return -EINTR; |
1294 // abort request |
1300 } |
1295 list_del(&request.list); |
1301 |
1296 up(&master->master_sem); |
1302 retval = request->state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO; |
1297 kfree(contents); |
1303 kref_put(&request->refcount,ec_master_reg_request_release); |
1298 return -EINTR; |
1304 return retval; |
1299 } |
1305 |
1300 up(&master->master_sem); |
|
1301 } |
|
1302 |
|
1303 // wait until master FSM has finished processing |
|
1304 wait_event(master->reg_queue, request.state != EC_INT_REQUEST_BUSY); |
|
1305 |
|
1306 kfree(contents); |
|
1307 |
|
1308 return request.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO; |
|
1309 } |
1306 } |
1310 |
1307 |
1311 /*****************************************************************************/ |
1308 /*****************************************************************************/ |
1312 |
1309 |
1313 /** Get slave configuration information. |
1310 /** Get slave configuration information. |
1385 EC_MASTER_ERR(master, "Invalid sync manager index %u!\n", |
1382 EC_MASTER_ERR(master, "Invalid sync manager index %u!\n", |
1386 data.sync_index); |
1383 data.sync_index); |
1387 return -EINVAL; |
1384 return -EINVAL; |
1388 } |
1385 } |
1389 |
1386 |
1390 if (down_interruptible(&master->master_sem)) |
1387 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
1391 return -EINTR; |
1388 return -EINTR; |
1392 |
1389 |
1393 if (!(sc = ec_master_get_config_const( |
1390 if (!(sc = ec_master_get_config_const( |
1394 master, data.config_index))) { |
1391 master, data.config_index))) { |
1395 up(&master->master_sem); |
1392 ec_mutex_unlock(&master->master_mutex); |
1396 EC_MASTER_ERR(master, "Slave config %u does not exist!\n", |
1393 EC_MASTER_ERR(master, "Slave config %u does not exist!\n", |
1397 data.config_index); |
1394 data.config_index); |
1398 return -EINVAL; |
1395 return -EINVAL; |
1399 } |
1396 } |
1400 |
1397 |
1401 if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( |
1398 if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( |
1402 &sc->sync_configs[data.sync_index].pdos, |
1399 &sc->sync_configs[data.sync_index].pdos, |
1403 data.pdo_pos))) { |
1400 data.pdo_pos))) { |
1404 up(&master->master_sem); |
1401 ec_mutex_unlock(&master->master_mutex); |
1405 EC_MASTER_ERR(master, "Invalid PDO position!\n"); |
1402 EC_MASTER_ERR(master, "Invalid PDO position!\n"); |
1406 return -EINVAL; |
1403 return -EINVAL; |
1407 } |
1404 } |
1408 |
1405 |
1409 data.index = pdo->index; |
1406 data.index = pdo->index; |
1410 data.entry_count = ec_pdo_entry_count(pdo); |
1407 data.entry_count = ec_pdo_entry_count(pdo); |
1411 ec_cdev_strcpy(data.name, pdo->name); |
1408 ec_cdev_strcpy(data.name, pdo->name); |
1412 |
1409 |
1413 up(&master->master_sem); |
1410 ec_mutex_unlock(&master->master_mutex); |
1414 |
1411 |
1415 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
1412 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
1416 return -EFAULT; |
1413 return -EFAULT; |
1417 |
1414 |
1418 return 0; |
1415 return 0; |
1440 EC_MASTER_ERR(master, "Invalid sync manager index %u!\n", |
1437 EC_MASTER_ERR(master, "Invalid sync manager index %u!\n", |
1441 data.sync_index); |
1438 data.sync_index); |
1442 return -EINVAL; |
1439 return -EINVAL; |
1443 } |
1440 } |
1444 |
1441 |
1445 if (down_interruptible(&master->master_sem)) |
1442 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
1446 return -EINTR; |
1443 return -EINTR; |
1447 |
1444 |
1448 if (!(sc = ec_master_get_config_const( |
1445 if (!(sc = ec_master_get_config_const( |
1449 master, data.config_index))) { |
1446 master, data.config_index))) { |
1450 up(&master->master_sem); |
1447 ec_mutex_unlock(&master->master_mutex); |
1451 EC_MASTER_ERR(master, "Slave config %u does not exist!\n", |
1448 EC_MASTER_ERR(master, "Slave config %u does not exist!\n", |
1452 data.config_index); |
1449 data.config_index); |
1453 return -EINVAL; |
1450 return -EINVAL; |
1454 } |
1451 } |
1455 |
1452 |
1456 if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( |
1453 if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( |
1457 &sc->sync_configs[data.sync_index].pdos, |
1454 &sc->sync_configs[data.sync_index].pdos, |
1458 data.pdo_pos))) { |
1455 data.pdo_pos))) { |
1459 up(&master->master_sem); |
1456 ec_mutex_unlock(&master->master_mutex); |
1460 EC_MASTER_ERR(master, "Invalid PDO position!\n"); |
1457 EC_MASTER_ERR(master, "Invalid PDO position!\n"); |
1461 return -EINVAL; |
1458 return -EINVAL; |
1462 } |
1459 } |
1463 |
1460 |
1464 if (!(entry = ec_pdo_find_entry_by_pos_const( |
1461 if (!(entry = ec_pdo_find_entry_by_pos_const( |
1465 pdo, data.entry_pos))) { |
1462 pdo, data.entry_pos))) { |
1466 up(&master->master_sem); |
1463 ec_mutex_unlock(&master->master_mutex); |
1467 EC_MASTER_ERR(master, "Entry not found!\n"); |
1464 EC_MASTER_ERR(master, "Entry not found!\n"); |
1468 return -EINVAL; |
1465 return -EINVAL; |
1469 } |
1466 } |
1470 |
1467 |
1471 data.index = entry->index; |
1468 data.index = entry->index; |
1472 data.subindex = entry->subindex; |
1469 data.subindex = entry->subindex; |
1473 data.bit_length = entry->bit_length; |
1470 data.bit_length = entry->bit_length; |
1474 ec_cdev_strcpy(data.name, entry->name); |
1471 ec_cdev_strcpy(data.name, entry->name); |
1475 |
1472 |
1476 up(&master->master_sem); |
1473 ec_mutex_unlock(&master->master_mutex); |
1477 |
1474 |
1478 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
1475 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
1479 return -EFAULT; |
1476 return -EFAULT; |
1480 |
1477 |
1481 return 0; |
1478 return 0; |
1496 |
1493 |
1497 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
1494 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
1498 return -EFAULT; |
1495 return -EFAULT; |
1499 } |
1496 } |
1500 |
1497 |
1501 if (down_interruptible(&master->master_sem)) |
1498 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
1502 return -EINTR; |
1499 return -EINTR; |
1503 |
1500 |
1504 if (!(sc = ec_master_get_config_const( |
1501 if (!(sc = ec_master_get_config_const( |
1505 master, data.config_index))) { |
1502 master, data.config_index))) { |
1506 up(&master->master_sem); |
1503 ec_mutex_unlock(&master->master_mutex); |
1507 EC_MASTER_ERR(master, "Slave config %u does not exist!\n", |
1504 EC_MASTER_ERR(master, "Slave config %u does not exist!\n", |
1508 data.config_index); |
1505 data.config_index); |
1509 return -EINVAL; |
1506 return -EINVAL; |
1510 } |
1507 } |
1511 |
1508 |
1512 if (!(req = ec_slave_config_get_sdo_by_pos_const( |
1509 if (!(req = ec_slave_config_get_sdo_by_pos_const( |
1513 sc, data.sdo_pos))) { |
1510 sc, data.sdo_pos))) { |
1514 up(&master->master_sem); |
1511 ec_mutex_unlock(&master->master_mutex); |
1515 EC_MASTER_ERR(master, "Invalid SDO position!\n"); |
1512 EC_MASTER_ERR(master, "Invalid SDO position!\n"); |
1516 return -EINVAL; |
1513 return -EINVAL; |
1517 } |
1514 } |
1518 |
1515 |
1519 data.index = req->index; |
1516 data.index = req->index; |
1520 data.subindex = req->subindex; |
1517 data.subindex = req->subindex; |
1521 data.size = req->data_size; |
1518 data.size = req->data_size; |
1522 memcpy(&data.data, req->data, |
1519 memcpy(&data.data, req->data, |
1523 min((u32) data.size, (u32) EC_MAX_SDO_DATA_SIZE)); |
1520 min((u32) data.size, (u32) EC_MAX_SDO_DATA_SIZE)); |
1524 |
1521 |
1525 up(&master->master_sem); |
1522 ec_mutex_unlock(&master->master_mutex); |
1526 |
1523 |
1527 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
1524 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
1528 return -EFAULT; |
1525 return -EFAULT; |
1529 |
1526 |
1530 return 0; |
1527 return 0; |
1545 |
1542 |
1546 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
1543 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
1547 return -EFAULT; |
1544 return -EFAULT; |
1548 } |
1545 } |
1549 |
1546 |
1550 if (down_interruptible(&master->master_sem)) |
1547 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
1551 return -EINTR; |
1548 return -EINTR; |
1552 |
1549 |
1553 if (!(sc = ec_master_get_config_const( |
1550 if (!(sc = ec_master_get_config_const( |
1554 master, data.config_index))) { |
1551 master, data.config_index))) { |
1555 up(&master->master_sem); |
1552 ec_mutex_unlock(&master->master_mutex); |
1556 EC_MASTER_ERR(master, "Slave config %u does not exist!\n", |
1553 EC_MASTER_ERR(master, "Slave config %u does not exist!\n", |
1557 data.config_index); |
1554 data.config_index); |
1558 return -EINVAL; |
1555 return -EINVAL; |
1559 } |
1556 } |
1560 |
1557 |
1561 if (!(req = ec_slave_config_get_idn_by_pos_const( |
1558 if (!(req = ec_slave_config_get_idn_by_pos_const( |
1562 sc, data.idn_pos))) { |
1559 sc, data.idn_pos))) { |
1563 up(&master->master_sem); |
1560 ec_mutex_unlock(&master->master_mutex); |
1564 EC_MASTER_ERR(master, "Invalid IDN position!\n"); |
1561 EC_MASTER_ERR(master, "Invalid IDN position!\n"); |
1565 return -EINVAL; |
1562 return -EINVAL; |
1566 } |
1563 } |
1567 |
1564 |
1568 data.drive_no = req->drive_no; |
1565 data.drive_no = req->drive_no; |
1613 } else { |
1610 } else { |
1614 data.slave_position = 0xffff; |
1611 data.slave_position = 0xffff; |
1615 } |
1612 } |
1616 snprintf(data.name, EC_DATAGRAM_NAME_SIZE, eoe->dev->name); |
1613 snprintf(data.name, EC_DATAGRAM_NAME_SIZE, eoe->dev->name); |
1617 data.open = eoe->opened; |
1614 data.open = eoe->opened; |
1618 data.rx_bytes = eoe->stats.tx_bytes; |
1615 data.tx_bytes = eoe->stats.tx_bytes; |
|
1616 data.tx_rate = eoe->tx_rate; |
|
1617 data.rx_bytes = eoe->stats.rx_bytes; |
1619 data.rx_rate = eoe->tx_rate; |
1618 data.rx_rate = eoe->tx_rate; |
1620 data.tx_bytes = eoe->stats.rx_bytes; |
|
1621 data.tx_rate = eoe->tx_rate; |
|
1622 data.tx_queued_frames = eoe->tx_queued_frames; |
1619 data.tx_queued_frames = eoe->tx_queued_frames; |
1623 data.tx_queue_size = eoe->tx_queue_size; |
1620 data.tx_queue_size = eoe->tx_queue_size; |
1624 |
1621 |
1625 up(&master->master_sem); |
1622 ec_mutex_unlock(&master->master_mutex); |
1626 |
1623 |
1627 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
1624 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
1628 return -EFAULT; |
1625 return -EFAULT; |
1629 |
1626 |
1630 return 0; |
1627 return 0; |
1885 return 0; |
1879 return 0; |
1886 } |
1880 } |
1887 |
1881 |
1888 /*****************************************************************************/ |
1882 /*****************************************************************************/ |
1889 |
1883 |
|
1884 /** Get the master state of all configured slaves. |
|
1885 */ |
|
1886 int ec_cdev_ioctl_master_sc_state( |
|
1887 ec_master_t *master, /**< EtherCAT master. */ |
|
1888 unsigned long arg, /**< ioctl() argument. */ |
|
1889 ec_cdev_priv_t *priv /**< Private data structure of file handle. */ |
|
1890 ) |
|
1891 { |
|
1892 ec_master_state_t data; |
|
1893 |
|
1894 if (unlikely(!priv->requested)) |
|
1895 return -EPERM; |
|
1896 |
|
1897 ecrt_master_configured_slaves_state(master, &data); |
|
1898 |
|
1899 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
|
1900 return -EFAULT; |
|
1901 |
|
1902 return 0; |
|
1903 } |
|
1904 |
|
1905 /*****************************************************************************/ |
|
1906 |
1890 /** Get the master state. |
1907 /** Get the master state. |
1891 */ |
1908 */ |
1892 int ec_cdev_ioctl_app_time( |
1909 int ec_cdev_ioctl_app_time( |
1893 ec_master_t *master, /**< EtherCAT master. */ |
1910 ec_master_t *master, /**< EtherCAT master. */ |
1894 unsigned long arg, /**< ioctl() argument. */ |
1911 unsigned long arg, /**< ioctl() argument. */ |
2096 |
2113 |
2097 ecrt_slave_config_watchdog(sc, |
2114 ecrt_slave_config_watchdog(sc, |
2098 data.watchdog_divider, data.watchdog_intervals); |
2115 data.watchdog_divider, data.watchdog_intervals); |
2099 |
2116 |
2100 out_up: |
2117 out_up: |
2101 up(&master->master_sem); |
2118 ec_mutex_unlock(&master->master_mutex); |
2102 out_return: |
2119 out_return: |
2103 return ret; |
2120 return ret; |
2104 } |
2121 } |
2105 |
2122 |
|
2123 |
|
2124 /*****************************************************************************/ |
|
2125 |
|
2126 /** Configure wether a slave allows overlapping PDOs. |
|
2127 */ |
|
2128 int ec_cdev_ioctl_sc_allow_overlapping_pdos( |
|
2129 ec_master_t *master, /**< EtherCAT master. */ |
|
2130 unsigned long arg, /**< ioctl() argument. */ |
|
2131 ec_cdev_priv_t *priv /**< Private data structure of file handle. */ |
|
2132 ) |
|
2133 { |
|
2134 ec_ioctl_config_t data; |
|
2135 ec_slave_config_t *sc; |
|
2136 int ret = 0; |
|
2137 |
|
2138 if (unlikely(!priv->requested)) { |
|
2139 ret = -EPERM; |
|
2140 goto out_return; |
|
2141 } |
|
2142 |
|
2143 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
|
2144 ret = -EFAULT; |
|
2145 goto out_return; |
|
2146 } |
|
2147 |
|
2148 if (ec_mutex_lock_interruptible(&master->master_mutex)) { |
|
2149 ret = -EINTR; |
|
2150 goto out_return; |
|
2151 } |
|
2152 |
|
2153 if (!(sc = ec_master_get_config(master, data.config_index))) { |
|
2154 ret = -ENOENT; |
|
2155 goto out_up; |
|
2156 } |
|
2157 |
|
2158 ecrt_slave_config_overlapping_pdos(sc, |
|
2159 data.allow_overlapping_pdos); |
|
2160 |
|
2161 out_up: |
|
2162 ec_mutex_unlock(&master->master_mutex); |
|
2163 out_return: |
|
2164 return ret; |
|
2165 } |
|
2166 |
2106 /*****************************************************************************/ |
2167 /*****************************************************************************/ |
2107 |
2168 |
2108 /** Add a PDO to the assignment. |
2169 /** Add a PDO to the assignment. |
2109 */ |
2170 */ |
2110 int ec_cdev_ioctl_sc_add_pdo( |
2171 int ec_cdev_ioctl_sc_add_pdo( |
2253 return -EPERM; |
2314 return -EPERM; |
2254 |
2315 |
2255 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) |
2316 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) |
2256 return -EFAULT; |
2317 return -EFAULT; |
2257 |
2318 |
2258 if (down_interruptible(&master->master_sem)) |
2319 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
2259 return -EINTR; |
2320 return -EINTR; |
2260 |
2321 |
2261 if (!(sc = ec_master_get_config(master, data.config_index))) { |
2322 if (!(sc = ec_master_get_config(master, data.config_index))) { |
2262 up(&master->master_sem); |
2323 ec_mutex_unlock(&master->master_mutex); |
2263 return -ENOENT; |
2324 return -ENOENT; |
2264 } |
2325 } |
2265 |
2326 |
2266 if (!(domain = ec_master_find_domain(master, data.domain_index))) { |
2327 if (!(domain = ec_master_find_domain(master, data.domain_index))) { |
2267 up(&master->master_sem); |
2328 ec_mutex_unlock(&master->master_mutex); |
2268 return -ENOENT; |
2329 return -ENOENT; |
2269 } |
2330 } |
2270 |
2331 |
2271 up(&master->master_sem); // FIXME |
2332 ec_mutex_unlock(&master->master_mutex); // FIXME |
2272 |
2333 |
2273 ret = ecrt_slave_config_reg_pdo_entry(sc, data.entry_index, |
2334 ret = ecrt_slave_config_reg_pdo_entry(sc, data.entry_index, |
2274 data.entry_subindex, domain, &data.bit_position); |
2335 data.entry_subindex, domain, &data.bit_position); |
2275 |
2336 |
2276 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
2337 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
2296 return -EPERM; |
2357 return -EPERM; |
2297 |
2358 |
2298 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) |
2359 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) |
2299 return -EFAULT; |
2360 return -EFAULT; |
2300 |
2361 |
2301 if (down_interruptible(&master->master_sem)) |
2362 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
2302 return -EINTR; |
2363 return -EINTR; |
2303 |
2364 |
2304 if (!(sc = ec_master_get_config(master, data.config_index))) { |
2365 if (!(sc = ec_master_get_config(master, data.config_index))) { |
2305 up(&master->master_sem); |
2366 ec_mutex_unlock(&master->master_mutex); |
2306 return -ENOENT; |
2367 return -ENOENT; |
2307 } |
2368 } |
2308 |
2369 |
2309 ecrt_slave_config_dc(sc, data.dc_assign_activate, |
2370 ecrt_slave_config_dc(sc, data.dc_assign_activate, |
2310 data.dc_sync[0].cycle_time, |
2371 data.dc_sync[0].cycle_time, |
2311 data.dc_sync[0].shift_time, |
2372 data.dc_sync[0].shift_time, |
2312 data.dc_sync[1].cycle_time, |
2373 data.dc_sync[1].cycle_time, |
2313 data.dc_sync[1].shift_time); |
2374 data.dc_sync[1].shift_time); |
2314 |
2375 |
2315 up(&master->master_sem); |
2376 ec_mutex_unlock(&master->master_mutex); |
2316 |
2377 |
2317 return 0; |
2378 return 0; |
2318 } |
2379 } |
2319 |
2380 |
2320 /*****************************************************************************/ |
2381 /*****************************************************************************/ |
2746 return -EPERM; |
2807 return -EPERM; |
2747 |
2808 |
2748 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) |
2809 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) |
2749 return -EFAULT; |
2810 return -EFAULT; |
2750 |
2811 |
2751 if (down_interruptible(&master->master_sem)) |
2812 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
2752 return -EINTR; |
2813 return -EINTR; |
2753 |
2814 |
2754 if (!(sc = ec_master_get_config(master, data.config_index))) { |
2815 if (!(sc = ec_master_get_config(master, data.config_index))) { |
2755 up(&master->master_sem); |
2816 ec_mutex_unlock(&master->master_mutex); |
2756 return -ENOENT; |
2817 return -ENOENT; |
2757 } |
2818 } |
2758 |
2819 |
2759 if (!(req = ec_slave_config_find_sdo_request(sc, data.request_index))) { |
2820 if (!(req = ec_slave_config_find_sdo_request(sc, data.request_index))) { |
2760 up(&master->master_sem); |
2821 ec_mutex_unlock(&master->master_mutex); |
2761 return -ENOENT; |
2822 return -ENOENT; |
2762 } |
2823 } |
2763 |
2824 |
2764 data.state = ecrt_sdo_request_state(req); |
2825 data.state = ecrt_sdo_request_state(req); |
2765 if (data.state == EC_REQUEST_SUCCESS && req->dir == EC_DIR_INPUT) |
2826 if (data.state == EC_REQUEST_SUCCESS && req->dir == EC_DIR_INPUT) |
2766 data.size = ecrt_sdo_request_data_size(req); |
2827 data.size = ecrt_sdo_request_data_size(req); |
2767 else |
2828 else |
2768 data.size = 0; |
2829 data.size = 0; |
2769 |
2830 |
2770 up(&master->master_sem); |
2831 ec_mutex_unlock(&master->master_mutex); |
2771 |
2832 |
2772 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
2833 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
2773 return -EFAULT; |
2834 return -EFAULT; |
2774 |
2835 |
2775 return 0; |
2836 return 0; |
2976 return -EPERM; |
3037 return -EPERM; |
2977 |
3038 |
2978 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) |
3039 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) |
2979 return -EFAULT; |
3040 return -EFAULT; |
2980 |
3041 |
2981 if (down_interruptible(&master->master_sem)) |
3042 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
2982 return -EINTR; |
3043 return -EINTR; |
2983 |
3044 |
2984 if (!(sc = ec_master_get_config(master, data.config_index))) { |
3045 if (!(sc = ec_master_get_config(master, data.config_index))) { |
2985 up(&master->master_sem); |
3046 ec_mutex_unlock(&master->master_mutex); |
2986 return -ENOENT; |
3047 return -ENOENT; |
2987 } |
3048 } |
2988 |
3049 |
2989 if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) { |
3050 if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) { |
2990 up(&master->master_sem); |
3051 ec_mutex_unlock(&master->master_mutex); |
2991 return -ENOENT; |
3052 return -ENOENT; |
2992 } |
3053 } |
2993 |
3054 |
2994 ecrt_voe_handler_received_header(voe, &vendor_id, &vendor_type); |
3055 ecrt_voe_handler_received_header(voe, &vendor_id, &vendor_type); |
2995 |
3056 |
2996 up(&master->master_sem); |
3057 ec_mutex_unlock(&master->master_mutex); |
2997 |
3058 |
2998 if (likely(data.vendor_id)) |
3059 if (likely(data.vendor_id)) |
2999 if (put_user(vendor_id, data.vendor_id)) |
3060 if (put_user(vendor_id, data.vendor_id)) |
3000 return -EFAULT; |
3061 return -EFAULT; |
3001 |
3062 |
3229 ec_master_t *master, /**< EtherCAT master. */ |
3290 ec_master_t *master, /**< EtherCAT master. */ |
3230 unsigned long arg /**< ioctl() argument. */ |
3291 unsigned long arg /**< ioctl() argument. */ |
3231 ) |
3292 ) |
3232 { |
3293 { |
3233 ec_ioctl_slave_foe_t data; |
3294 ec_ioctl_slave_foe_t data; |
3234 ec_master_foe_request_t request; |
3295 ec_master_foe_request_t* request; |
3235 int retval; |
3296 int retval; |
3236 |
3297 |
3237 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
3298 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
3238 return -EFAULT; |
3299 return -EFAULT; |
3239 } |
3300 } |
3240 |
3301 |
3241 ec_foe_request_init(&request.req, data.file_name); |
3302 request = kmalloc(sizeof(*request), GFP_KERNEL); |
3242 ec_foe_request_read(&request.req); |
3303 if (!request) |
3243 ec_foe_request_alloc(&request.req, 10000); // FIXME |
3304 return -ENOMEM; |
3244 |
3305 kref_init(&request->refcount); |
3245 if (down_interruptible(&master->master_sem)) |
3306 |
3246 return -EINTR; |
3307 ec_foe_request_init(&request->req, data.file_name); |
3247 |
3308 ec_foe_request_read(&request->req); |
3248 if (!(request.slave = ec_master_find_slave( |
3309 ec_foe_request_alloc(&request->req, 10000); // FIXME |
|
3310 |
|
3311 if (ec_mutex_lock_interruptible(&master->master_mutex)) { |
|
3312 kref_put(&request->refcount,ec_master_foe_request_release); |
|
3313 return -EINTR; |
|
3314 } |
|
3315 if (!(request->slave = ec_master_find_slave( |
3249 master, 0, data.slave_position))) { |
3316 master, 0, data.slave_position))) { |
3250 up(&master->master_sem); |
3317 ec_mutex_unlock(&master->master_mutex); |
3251 ec_foe_request_clear(&request.req); |
3318 kref_put(&request->refcount,ec_master_foe_request_release); |
3252 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
3319 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
3253 data.slave_position); |
3320 data.slave_position); |
3254 return -EINVAL; |
3321 return -EINVAL; |
3255 } |
3322 } |
3256 |
3323 |
3257 // schedule request. |
3324 // schedule request. |
3258 list_add_tail(&request.list, &request.slave->foe_requests); |
3325 list_add_tail(&request->list, &request->slave->foe_requests); |
3259 |
3326 kref_get(&request->refcount); |
3260 up(&master->master_sem); |
3327 |
3261 |
3328 ec_mutex_unlock(&master->master_mutex); |
3262 EC_SLAVE_DBG(request.slave, 1, "Scheduled FoE read request.\n"); |
3329 |
|
3330 EC_SLAVE_DBG(request->slave, 1, "Scheduled FoE read request %p.\n",request); |
3263 |
3331 |
3264 // wait for processing through FSM |
3332 // wait for processing through FSM |
3265 if (wait_event_interruptible(request.slave->foe_queue, |
3333 if (wait_event_interruptible(request->slave->foe_queue, |
3266 request.req.state != EC_INT_REQUEST_QUEUED)) { |
3334 ((request->req.state == EC_INT_REQUEST_SUCCESS) || (request->req.state == EC_INT_REQUEST_FAILURE)))) { |
3267 // interrupted by signal |
3335 // interrupted by signal |
3268 down(&master->master_sem); |
3336 kref_put(&request->refcount,ec_master_foe_request_release); |
3269 if (request.req.state == EC_INT_REQUEST_QUEUED) { |
3337 return -EINTR; |
3270 list_del(&request.list); |
3338 } |
3271 up(&master->master_sem); |
3339 |
3272 ec_foe_request_clear(&request.req); |
3340 data.result = request->req.result; |
3273 return -EINTR; |
3341 data.error_code = request->req.error_code; |
3274 } |
3342 |
3275 // request already processing: interrupt not possible. |
3343 EC_SLAVE_DBG(request->slave, 1, "Read %zd bytes via FoE" |
3276 up(&master->master_sem); |
3344 " (result = 0x%x).\n", request->req.data_size, request->req.result); |
3277 } |
3345 |
3278 |
3346 if (request->req.state != EC_INT_REQUEST_SUCCESS) { |
3279 // wait until master FSM has finished processing |
|
3280 wait_event(request.slave->foe_queue, |
|
3281 request.req.state != EC_INT_REQUEST_BUSY); |
|
3282 |
|
3283 data.result = request.req.result; |
|
3284 data.error_code = request.req.error_code; |
|
3285 |
|
3286 EC_SLAVE_DBG(request.slave, 1, "Read %zd bytes via FoE" |
|
3287 " (result = 0x%x).\n", request.req.data_size, request.req.result); |
|
3288 |
|
3289 if (request.req.state != EC_INT_REQUEST_SUCCESS) { |
|
3290 data.data_size = 0; |
3347 data.data_size = 0; |
3291 retval = -EIO; |
3348 retval = -EIO; |
3292 } else { |
3349 } else { |
3293 if (request.req.data_size > data.buffer_size) { |
3350 if (request->req.data_size > data.buffer_size) { |
3294 EC_MASTER_ERR(master, "Buffer too small.\n"); |
3351 EC_MASTER_ERR(master, "Buffer too small.\n"); |
3295 ec_foe_request_clear(&request.req); |
3352 kref_put(&request->refcount,ec_master_foe_request_release); |
3296 return -EOVERFLOW; |
3353 return -EOVERFLOW; |
3297 } |
3354 } |
3298 data.data_size = request.req.data_size; |
3355 data.data_size = request->req.data_size; |
3299 if (copy_to_user((void __user *) data.buffer, |
3356 if (copy_to_user((void __user *) data.buffer, |
3300 request.req.buffer, data.data_size)) { |
3357 request->req.buffer, data.data_size)) { |
3301 ec_foe_request_clear(&request.req); |
3358 kref_put(&request->refcount,ec_master_foe_request_release); |
3302 return -EFAULT; |
3359 return -EFAULT; |
3303 } |
3360 } |
3304 retval = 0; |
3361 retval = 0; |
3305 } |
3362 } |
3306 |
3363 |
3307 if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { |
3364 if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { |
3308 retval = -EFAULT; |
3365 retval = -EFAULT; |
3309 } |
3366 } |
3310 |
3367 |
3311 EC_SLAVE_DBG(request.slave, 1, "Finished FoE read request.\n"); |
3368 EC_SLAVE_DBG(request->slave, 1, "Finished FoE read request %p.\n",request); |
3312 |
3369 kref_put(&request->refcount,ec_master_foe_request_release); |
3313 ec_foe_request_clear(&request.req); |
|
3314 |
3370 |
3315 return retval; |
3371 return retval; |
3316 } |
3372 } |
3317 |
3373 |
3318 /*****************************************************************************/ |
3374 /*****************************************************************************/ |
3323 ec_master_t *master, /**< EtherCAT master. */ |
3379 ec_master_t *master, /**< EtherCAT master. */ |
3324 unsigned long arg /**< ioctl() argument. */ |
3380 unsigned long arg /**< ioctl() argument. */ |
3325 ) |
3381 ) |
3326 { |
3382 { |
3327 ec_ioctl_slave_foe_t data; |
3383 ec_ioctl_slave_foe_t data; |
3328 ec_master_foe_request_t request; |
3384 ec_master_foe_request_t* request; |
3329 int retval; |
3385 int retval; |
3330 |
3386 |
3331 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
3387 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
3332 return -EFAULT; |
3388 return -EFAULT; |
3333 } |
3389 } |
3334 |
3390 |
3335 INIT_LIST_HEAD(&request.list); |
3391 request = kmalloc(sizeof(*request), GFP_KERNEL); |
3336 |
3392 if (!request) |
3337 ec_foe_request_init(&request.req, data.file_name); |
|
3338 |
|
3339 if (ec_foe_request_alloc(&request.req, data.buffer_size)) { |
|
3340 ec_foe_request_clear(&request.req); |
|
3341 return -ENOMEM; |
3393 return -ENOMEM; |
3342 } |
3394 kref_init(&request->refcount); |
3343 if (copy_from_user(request.req.buffer, |
3395 |
|
3396 INIT_LIST_HEAD(&request->list); |
|
3397 |
|
3398 ec_foe_request_init(&request->req, data.file_name); |
|
3399 |
|
3400 if (ec_foe_request_alloc(&request->req, data.buffer_size)) { |
|
3401 kref_put(&request->refcount,ec_master_foe_request_release); |
|
3402 return -ENOMEM; |
|
3403 } |
|
3404 if (copy_from_user(request->req.buffer, |
3344 (void __user *) data.buffer, data.buffer_size)) { |
3405 (void __user *) data.buffer, data.buffer_size)) { |
3345 ec_foe_request_clear(&request.req); |
3406 kref_put(&request->refcount,ec_master_foe_request_release); |
3346 return -EFAULT; |
3407 return -EFAULT; |
3347 } |
3408 } |
3348 request.req.data_size = data.buffer_size; |
3409 request->req.data_size = data.buffer_size; |
3349 ec_foe_request_write(&request.req); |
3410 ec_foe_request_write(&request->req); |
3350 |
3411 |
3351 if (down_interruptible(&master->master_sem)) |
3412 if (ec_mutex_lock_interruptible(&master->master_mutex)) |
3352 return -EINTR; |
3413 return -EINTR; |
3353 |
3414 |
3354 if (!(request.slave = ec_master_find_slave( |
3415 if (!(request->slave = ec_master_find_slave( |
3355 master, 0, data.slave_position))) { |
3416 master, 0, data.slave_position))) { |
3356 up(&master->master_sem); |
3417 ec_mutex_unlock(&master->master_mutex); |
3357 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
3418 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
3358 data.slave_position); |
3419 data.slave_position); |
3359 ec_foe_request_clear(&request.req); |
3420 kref_put(&request->refcount,ec_master_foe_request_release); |
3360 return -EINVAL; |
3421 return -EINVAL; |
3361 } |
3422 } |
3362 |
3423 |
3363 EC_SLAVE_DBG(request.slave, 1, "Scheduling FoE write request.\n"); |
3424 EC_SLAVE_DBG(request->slave, 1, "Scheduling FoE write request %p.\n",request); |
3364 |
3425 |
3365 // schedule FoE write request. |
3426 // schedule FoE write request. |
3366 list_add_tail(&request.list, &request.slave->foe_requests); |
3427 list_add_tail(&request->list, &request->slave->foe_requests); |
3367 |
3428 kref_get(&request->refcount); |
3368 up(&master->master_sem); |
3429 |
|
3430 ec_mutex_unlock(&master->master_mutex); |
3369 |
3431 |
3370 // wait for processing through FSM |
3432 // wait for processing through FSM |
3371 if (wait_event_interruptible(request.slave->foe_queue, |
3433 if (wait_event_interruptible(request->slave->foe_queue, |
3372 request.req.state != EC_INT_REQUEST_QUEUED)) { |
3434 ((request->req.state == EC_INT_REQUEST_SUCCESS) || (request->req.state == EC_INT_REQUEST_FAILURE)))) { |
3373 // interrupted by signal |
3435 // interrupted by signal |
3374 down(&master->master_sem); |
3436 kref_put(&request->refcount,ec_master_foe_request_release); |
3375 if (request.req.state == EC_INT_REQUEST_QUEUED) { |
3437 return -EINTR; |
3376 // abort request |
3438 } |
3377 list_del(&request.list); |
3439 |
3378 up(&master->master_sem); |
3440 data.result = request->req.result; |
3379 ec_foe_request_clear(&request.req); |
3441 data.error_code = request->req.error_code; |
3380 return -EINTR; |
3442 |
3381 } |
3443 retval = request->req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO; |
3382 up(&master->master_sem); |
|
3383 } |
|
3384 |
|
3385 // wait until master FSM has finished processing |
|
3386 wait_event(request.slave->foe_queue, |
|
3387 request.req.state != EC_INT_REQUEST_BUSY); |
|
3388 |
|
3389 data.result = request.req.result; |
|
3390 data.error_code = request.req.error_code; |
|
3391 |
|
3392 retval = request.req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO; |
|
3393 |
3444 |
3394 if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { |
3445 if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { |
3395 retval = -EFAULT; |
3446 retval = -EFAULT; |
3396 } |
3447 } |
3397 |
3448 |
3398 ec_foe_request_clear(&request.req); |
3449 EC_SLAVE_DBG(request->slave, 1, "Finished FoE write request %p.\n",request); |
3399 |
3450 kref_put(&request->refcount,ec_master_foe_request_release); |
3400 EC_SLAVE_DBG(request.slave, 1, "Finished FoE write request.\n"); |
|
3401 |
3451 |
3402 return retval; |
3452 return retval; |
3403 } |
3453 } |
3404 |
3454 |
3405 /*****************************************************************************/ |
3455 /*****************************************************************************/ |
3668 if (!(filp->f_mode & FMODE_WRITE)) |
3718 if (!(filp->f_mode & FMODE_WRITE)) |
3669 return -EPERM; |
3719 return -EPERM; |
3670 return ec_cdev_ioctl_receive(master, arg, priv); |
3720 return ec_cdev_ioctl_receive(master, arg, priv); |
3671 case EC_IOCTL_MASTER_STATE: |
3721 case EC_IOCTL_MASTER_STATE: |
3672 return ec_cdev_ioctl_master_state(master, arg, priv); |
3722 return ec_cdev_ioctl_master_state(master, arg, priv); |
|
3723 case EC_IOCTL_MASTER_SC_STATE: |
|
3724 return ec_cdev_ioctl_master_sc_state(master, arg, priv); |
3673 case EC_IOCTL_APP_TIME: |
3725 case EC_IOCTL_APP_TIME: |
3674 if (!(filp->f_mode & FMODE_WRITE)) |
3726 if (!(filp->f_mode & FMODE_WRITE)) |
3675 return -EPERM; |
3727 return -EPERM; |
3676 return ec_cdev_ioctl_app_time(master, arg, priv); |
3728 return ec_cdev_ioctl_app_time(master, arg, priv); |
3677 case EC_IOCTL_SYNC_REF: |
3729 case EC_IOCTL_SYNC_REF: |
3700 return ec_cdev_ioctl_sc_sync(master, arg, priv); |
3752 return ec_cdev_ioctl_sc_sync(master, arg, priv); |
3701 case EC_IOCTL_SC_WATCHDOG: |
3753 case EC_IOCTL_SC_WATCHDOG: |
3702 if (!(filp->f_mode & FMODE_WRITE)) |
3754 if (!(filp->f_mode & FMODE_WRITE)) |
3703 return -EPERM; |
3755 return -EPERM; |
3704 return ec_cdev_ioctl_sc_watchdog(master, arg, priv); |
3756 return ec_cdev_ioctl_sc_watchdog(master, arg, priv); |
|
3757 case EC_IOCTL_SC_OVERLAPPING_IO: |
|
3758 if (!(filp->f_mode & FMODE_WRITE)) |
|
3759 return -EPERM; |
|
3760 return ec_cdev_ioctl_sc_allow_overlapping_pdos(master,arg,priv); |
3705 case EC_IOCTL_SC_ADD_PDO: |
3761 case EC_IOCTL_SC_ADD_PDO: |
3706 if (!(filp->f_mode & FMODE_WRITE)) |
3762 if (!(filp->f_mode & FMODE_WRITE)) |
3707 return -EPERM; |
3763 return -EPERM; |
3708 return ec_cdev_ioctl_sc_add_pdo(master, arg, priv); |
3764 return ec_cdev_ioctl_sc_add_pdo(master, arg, priv); |
3709 case EC_IOCTL_SC_CLEAR_PDOS: |
3765 case EC_IOCTL_SC_CLEAR_PDOS: |