2663 |
2663 |
2664 int ecrt_master_sdo_download(ec_master_t *master, uint16_t slave_position, |
2664 int ecrt_master_sdo_download(ec_master_t *master, uint16_t slave_position, |
2665 uint16_t index, uint8_t subindex, uint8_t *data, |
2665 uint16_t index, uint8_t subindex, uint8_t *data, |
2666 size_t data_size, uint32_t *abort_code) |
2666 size_t data_size, uint32_t *abort_code) |
2667 { |
2667 { |
2668 ec_master_sdo_request_t request; |
2668 ec_sdo_request_t request; |
|
2669 ec_slave_t *slave; |
2669 |
2670 |
2670 EC_MASTER_DBG(master, 1, "%s(master = 0x%p," |
2671 EC_MASTER_DBG(master, 1, "%s(master = 0x%p," |
2671 " slave_position = %u, index = 0x%04X, subindex = 0x%02X," |
2672 " slave_position = %u, index = 0x%04X, subindex = 0x%02X," |
2672 " data = 0x%p, data_size = %zu, abort_code = 0x%p)\n", |
2673 " data = 0x%p, data_size = %zu, abort_code = 0x%p)\n", |
2673 __func__, master, slave_position, index, subindex, |
2674 __func__, master, slave_position, index, subindex, |
2676 if (!data_size) { |
2677 if (!data_size) { |
2677 EC_MASTER_ERR(master, "Zero data size!\n"); |
2678 EC_MASTER_ERR(master, "Zero data size!\n"); |
2678 return -EINVAL; |
2679 return -EINVAL; |
2679 } |
2680 } |
2680 |
2681 |
2681 ec_sdo_request_init(&request.req); |
2682 ec_sdo_request_init(&request); |
2682 ecrt_sdo_request_index(&request.req, index, subindex); |
2683 ecrt_sdo_request_index(&request, index, subindex); |
2683 if (ec_sdo_request_alloc(&request.req, data_size)) { |
2684 if (ec_sdo_request_alloc(&request, data_size)) { |
2684 ec_sdo_request_clear(&request.req); |
2685 ec_sdo_request_clear(&request); |
2685 return -ENOMEM; |
2686 return -ENOMEM; |
2686 } |
2687 } |
2687 |
2688 |
2688 memcpy(request.req.data, data, data_size); |
2689 memcpy(request.data, data, data_size); |
2689 request.req.data_size = data_size; |
2690 request.data_size = data_size; |
2690 ecrt_sdo_request_write(&request.req); |
2691 ecrt_sdo_request_write(&request); |
2691 |
2692 |
2692 if (down_interruptible(&master->master_sem)) |
2693 if (down_interruptible(&master->master_sem)) { |
|
2694 ec_sdo_request_clear(&request); |
2693 return -EINTR; |
2695 return -EINTR; |
2694 |
2696 } |
2695 if (!(request.slave = ec_master_find_slave(master, 0, slave_position))) { |
2697 |
|
2698 if (!(slave = ec_master_find_slave(master, 0, slave_position))) { |
2696 up(&master->master_sem); |
2699 up(&master->master_sem); |
2697 EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position); |
2700 EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position); |
2698 ec_sdo_request_clear(&request.req); |
2701 ec_sdo_request_clear(&request); |
2699 return -EINVAL; |
2702 return -EINVAL; |
2700 } |
2703 } |
2701 |
2704 |
2702 EC_SLAVE_DBG(request.slave, 1, "Schedule SDO download request.\n"); |
2705 EC_SLAVE_DBG(slave, 1, "Schedule SDO download request.\n"); |
2703 |
2706 |
2704 // schedule request. |
2707 // schedule request. |
2705 list_add_tail(&request.list, &request.slave->sdo_requests); |
2708 list_add_tail(&request.list, &slave->sdo_requests); |
2706 |
2709 |
2707 up(&master->master_sem); |
2710 up(&master->master_sem); |
2708 |
2711 |
2709 // wait for processing through FSM |
2712 // wait for processing through FSM |
2710 if (wait_event_interruptible(request.slave->sdo_queue, |
2713 if (wait_event_interruptible(slave->sdo_queue, |
2711 request.req.state != EC_INT_REQUEST_QUEUED)) { |
2714 request.state != EC_INT_REQUEST_QUEUED)) { |
2712 // interrupted by signal |
2715 // interrupted by signal |
2713 down(&master->master_sem); |
2716 down(&master->master_sem); |
2714 if (request.req.state == EC_INT_REQUEST_QUEUED) { |
2717 if (request.state == EC_INT_REQUEST_QUEUED) { |
2715 list_del(&request.list); |
2718 list_del(&request.list); |
2716 up(&master->master_sem); |
2719 up(&master->master_sem); |
2717 ec_sdo_request_clear(&request.req); |
2720 ec_sdo_request_clear(&request); |
2718 return -EINTR; |
2721 return -EINTR; |
2719 } |
2722 } |
2720 // request already processing: interrupt not possible. |
2723 // request already processing: interrupt not possible. |
2721 up(&master->master_sem); |
2724 up(&master->master_sem); |
2722 } |
2725 } |
2723 |
2726 |
|
2727 // FIXME slave may become invalid! |
|
2728 |
2724 // wait until master FSM has finished processing |
2729 // wait until master FSM has finished processing |
2725 wait_event(request.slave->sdo_queue, |
2730 wait_event(slave->sdo_queue, request.state != EC_INT_REQUEST_BUSY); |
2726 request.req.state != EC_INT_REQUEST_BUSY); |
2731 |
2727 |
2732 EC_SLAVE_DBG(slave, 1, "Finished SDO download request.\n"); |
2728 EC_SLAVE_DBG(request.slave, 1, "Finished SDO download request.\n"); |
2733 |
2729 |
2734 *abort_code = request.abort_code; |
2730 *abort_code = request.req.abort_code; |
2735 |
2731 |
2736 if (request.state == EC_INT_REQUEST_SUCCESS) { |
2732 if (request.req.state == EC_INT_REQUEST_SUCCESS) { |
|
2733 return 0; |
2737 return 0; |
2734 } else if (request.req.errno) { |
2738 } else if (request.errno) { |
2735 return -request.req.errno; |
2739 return -request.errno; |
2736 } else { |
2740 } else { |
2737 return -EIO; |
2741 return -EIO; |
2738 } |
2742 } |
2739 } |
2743 } |
2740 |
2744 |
2742 |
2746 |
2743 int ecrt_master_sdo_download_complete(ec_master_t *master, |
2747 int ecrt_master_sdo_download_complete(ec_master_t *master, |
2744 uint16_t slave_position, uint16_t index, uint8_t *data, |
2748 uint16_t slave_position, uint16_t index, uint8_t *data, |
2745 size_t data_size, uint32_t *abort_code) |
2749 size_t data_size, uint32_t *abort_code) |
2746 { |
2750 { |
2747 ec_master_sdo_request_t request; |
2751 ec_sdo_request_t request; |
|
2752 ec_slave_t *slave; |
2748 |
2753 |
2749 EC_MASTER_DBG(master, 1, "%s(master = 0x%p," |
2754 EC_MASTER_DBG(master, 1, "%s(master = 0x%p," |
2750 " slave_position = %u, index = 0x%04X," |
2755 " slave_position = %u, index = 0x%04X," |
2751 " data = 0x%p, data_size = %zu, abort_code = 0x%p)\n", |
2756 " data = 0x%p, data_size = %zu, abort_code = 0x%p)\n", |
2752 __func__, master, slave_position, index, data, data_size, |
2757 __func__, master, slave_position, index, data, data_size, |
2755 if (!data_size) { |
2760 if (!data_size) { |
2756 EC_MASTER_ERR(master, "Zero data size!\n"); |
2761 EC_MASTER_ERR(master, "Zero data size!\n"); |
2757 return -EINVAL; |
2762 return -EINVAL; |
2758 } |
2763 } |
2759 |
2764 |
2760 ec_sdo_request_init(&request.req); |
2765 ec_sdo_request_init(&request); |
2761 ecrt_sdo_request_index(&request.req, index, 0); |
2766 ecrt_sdo_request_index(&request, index, 0); |
2762 if (ec_sdo_request_alloc(&request.req, data_size)) { |
2767 if (ec_sdo_request_alloc(&request, data_size)) { |
2763 ec_sdo_request_clear(&request.req); |
2768 ec_sdo_request_clear(&request); |
2764 return -ENOMEM; |
2769 return -ENOMEM; |
2765 } |
2770 } |
2766 |
2771 |
2767 request.req.complete_access = 1; |
2772 request.complete_access = 1; |
2768 memcpy(request.req.data, data, data_size); |
2773 memcpy(request.data, data, data_size); |
2769 request.req.data_size = data_size; |
2774 request.data_size = data_size; |
2770 ecrt_sdo_request_write(&request.req); |
2775 ecrt_sdo_request_write(&request); |
2771 |
2776 |
2772 if (down_interruptible(&master->master_sem)) |
2777 if (down_interruptible(&master->master_sem)) { |
|
2778 ec_sdo_request_clear(&request); |
2773 return -EINTR; |
2779 return -EINTR; |
2774 |
2780 } |
2775 if (!(request.slave = ec_master_find_slave(master, 0, slave_position))) { |
2781 |
|
2782 if (!(slave = ec_master_find_slave(master, 0, slave_position))) { |
2776 up(&master->master_sem); |
2783 up(&master->master_sem); |
2777 EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position); |
2784 EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position); |
2778 ec_sdo_request_clear(&request.req); |
2785 ec_sdo_request_clear(&request); |
2779 return -EINVAL; |
2786 return -EINVAL; |
2780 } |
2787 } |
2781 |
2788 |
2782 EC_SLAVE_DBG(request.slave, 1, "Schedule SDO download request" |
2789 EC_SLAVE_DBG(slave, 1, "Schedule SDO download request" |
2783 " (complete access).\n"); |
2790 " (complete access).\n"); |
2784 |
2791 |
2785 // schedule request. |
2792 // schedule request. |
2786 list_add_tail(&request.list, &request.slave->sdo_requests); |
2793 list_add_tail(&request.list, &slave->sdo_requests); |
2787 |
2794 |
2788 up(&master->master_sem); |
2795 up(&master->master_sem); |
2789 |
2796 |
2790 // wait for processing through FSM |
2797 // wait for processing through FSM |
2791 if (wait_event_interruptible(request.slave->sdo_queue, |
2798 if (wait_event_interruptible(slave->sdo_queue, |
2792 request.req.state != EC_INT_REQUEST_QUEUED)) { |
2799 request.state != EC_INT_REQUEST_QUEUED)) { |
2793 // interrupted by signal |
2800 // interrupted by signal |
2794 down(&master->master_sem); |
2801 down(&master->master_sem); |
2795 if (request.req.state == EC_INT_REQUEST_QUEUED) { |
2802 if (request.state == EC_INT_REQUEST_QUEUED) { |
2796 list_del(&request.list); |
2803 list_del(&request.list); |
2797 up(&master->master_sem); |
2804 up(&master->master_sem); |
2798 ec_sdo_request_clear(&request.req); |
2805 ec_sdo_request_clear(&request); |
2799 return -EINTR; |
2806 return -EINTR; |
2800 } |
2807 } |
2801 // request already processing: interrupt not possible. |
2808 // request already processing: interrupt not possible. |
2802 up(&master->master_sem); |
2809 up(&master->master_sem); |
2803 } |
2810 } |
2804 |
2811 |
|
2812 // FIXME slave may become invalid! |
|
2813 |
2805 // wait until master FSM has finished processing |
2814 // wait until master FSM has finished processing |
2806 wait_event(request.slave->sdo_queue, |
2815 wait_event(slave->sdo_queue, request.state != EC_INT_REQUEST_BUSY); |
2807 request.req.state != EC_INT_REQUEST_BUSY); |
2816 |
2808 |
2817 EC_SLAVE_DBG(slave, 1, "Finished SDO download request" |
2809 EC_SLAVE_DBG(request.slave, 1, "Finished SDO download request" |
|
2810 " (complete access).\n"); |
2818 " (complete access).\n"); |
2811 |
2819 |
2812 *abort_code = request.req.abort_code; |
2820 *abort_code = request.abort_code; |
2813 |
2821 |
2814 if (request.req.state == EC_INT_REQUEST_SUCCESS) { |
2822 if (request.state == EC_INT_REQUEST_SUCCESS) { |
2815 return 0; |
2823 return 0; |
2816 } else if (request.req.errno) { |
2824 } else if (request.errno) { |
2817 return -request.req.errno; |
2825 return -request.errno; |
2818 } else { |
2826 } else { |
2819 return -EIO; |
2827 return -EIO; |
2820 } |
2828 } |
2821 } |
2829 } |
2822 |
2830 |
2824 |
2832 |
2825 int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position, |
2833 int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position, |
2826 uint16_t index, uint8_t subindex, uint8_t *target, |
2834 uint16_t index, uint8_t subindex, uint8_t *target, |
2827 size_t target_size, size_t *result_size, uint32_t *abort_code) |
2835 size_t target_size, size_t *result_size, uint32_t *abort_code) |
2828 { |
2836 { |
2829 ec_master_sdo_request_t request; |
2837 ec_sdo_request_t request; |
|
2838 ec_slave_t *slave; |
2830 int retval = 0; |
2839 int retval = 0; |
2831 |
2840 |
2832 EC_MASTER_DBG(master, 1, "%s(master = 0x%p," |
2841 EC_MASTER_DBG(master, 1, "%s(master = 0x%p," |
2833 " slave_position = %u, index = 0x%04X, subindex = 0x%02X," |
2842 " slave_position = %u, index = 0x%04X, subindex = 0x%02X," |
2834 " target = 0x%p, target_size = %zu, result_size = 0x%p," |
2843 " target = 0x%p, target_size = %zu, result_size = 0x%p," |
2835 " abort_code = 0x%p)\n", |
2844 " abort_code = 0x%p)\n", |
2836 __func__, master, slave_position, index, subindex, |
2845 __func__, master, slave_position, index, subindex, |
2837 target, target_size, result_size, abort_code); |
2846 target, target_size, result_size, abort_code); |
2838 |
2847 |
2839 ec_sdo_request_init(&request.req); |
2848 ec_sdo_request_init(&request); |
2840 ecrt_sdo_request_index(&request.req, index, subindex); |
2849 ecrt_sdo_request_index(&request, index, subindex); |
2841 ecrt_sdo_request_read(&request.req); |
2850 ecrt_sdo_request_read(&request); |
2842 |
2851 |
2843 if (down_interruptible(&master->master_sem)) { |
2852 if (down_interruptible(&master->master_sem)) { |
|
2853 ec_sdo_request_clear(&request); |
2844 return -EINTR; |
2854 return -EINTR; |
2845 } |
2855 } |
2846 |
2856 |
2847 if (!(request.slave = ec_master_find_slave(master, 0, slave_position))) { |
2857 if (!(slave = ec_master_find_slave(master, 0, slave_position))) { |
2848 up(&master->master_sem); |
2858 up(&master->master_sem); |
2849 ec_sdo_request_clear(&request.req); |
2859 ec_sdo_request_clear(&request); |
2850 EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position); |
2860 EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position); |
2851 return -EINVAL; |
2861 return -EINVAL; |
2852 } |
2862 } |
2853 |
2863 |
2854 EC_SLAVE_DBG(request.slave, 1, "Schedule SDO upload request.\n"); |
2864 EC_SLAVE_DBG(slave, 1, "Schedule SDO upload request.\n"); |
2855 |
2865 |
2856 // schedule request. |
2866 // schedule request. |
2857 list_add_tail(&request.list, &request.slave->sdo_requests); |
2867 list_add_tail(&request.list, &slave->sdo_requests); |
2858 |
2868 |
2859 up(&master->master_sem); |
2869 up(&master->master_sem); |
2860 |
2870 |
2861 // wait for processing through FSM |
2871 // wait for processing through FSM |
2862 if (wait_event_interruptible(request.slave->sdo_queue, |
2872 if (wait_event_interruptible(slave->sdo_queue, |
2863 request.req.state != EC_INT_REQUEST_QUEUED)) { |
2873 request.state != EC_INT_REQUEST_QUEUED)) { |
2864 // interrupted by signal |
2874 // interrupted by signal |
2865 down(&master->master_sem); |
2875 down(&master->master_sem); |
2866 if (request.req.state == EC_INT_REQUEST_QUEUED) { |
2876 if (request.state == EC_INT_REQUEST_QUEUED) { |
2867 list_del(&request.list); |
2877 list_del(&request.list); |
2868 up(&master->master_sem); |
2878 up(&master->master_sem); |
2869 ec_sdo_request_clear(&request.req); |
2879 ec_sdo_request_clear(&request); |
2870 return -EINTR; |
2880 return -EINTR; |
2871 } |
2881 } |
2872 // request already processing: interrupt not possible. |
2882 // request already processing: interrupt not possible. |
2873 up(&master->master_sem); |
2883 up(&master->master_sem); |
2874 } |
2884 } |
2875 |
2885 |
|
2886 // FIXME slave may become invalid! |
|
2887 |
2876 // wait until master FSM has finished processing |
2888 // wait until master FSM has finished processing |
2877 wait_event(request.slave->sdo_queue, |
2889 wait_event(slave->sdo_queue, request.state != EC_INT_REQUEST_BUSY); |
2878 request.req.state != EC_INT_REQUEST_BUSY); |
2890 |
2879 |
2891 EC_SLAVE_DBG(slave, 1, "Finished SDO upload request.\n"); |
2880 EC_SLAVE_DBG(request.slave, 1, "Finished SDO upload request.\n"); |
2892 |
2881 |
2893 *abort_code = request.abort_code; |
2882 *abort_code = request.req.abort_code; |
2894 |
2883 |
2895 if (request.state != EC_INT_REQUEST_SUCCESS) { |
2884 if (request.req.state != EC_INT_REQUEST_SUCCESS) { |
|
2885 *result_size = 0; |
2896 *result_size = 0; |
2886 if (request.req.errno) { |
2897 if (request.errno) { |
2887 retval = -request.req.errno; |
2898 retval = -request.errno; |
2888 } else { |
2899 } else { |
2889 retval = -EIO; |
2900 retval = -EIO; |
2890 } |
2901 } |
2891 } else { |
2902 } else { |
2892 if (request.req.data_size > target_size) { |
2903 if (request.data_size > target_size) { |
2893 EC_MASTER_ERR(master, "Buffer too small.\n"); |
2904 EC_MASTER_ERR(master, "Buffer too small.\n"); |
2894 ec_sdo_request_clear(&request.req); |
2905 ec_sdo_request_clear(&request); |
2895 return -EOVERFLOW; |
2906 return -EOVERFLOW; |
2896 } |
2907 } |
2897 memcpy(target, request.req.data, request.req.data_size); |
2908 memcpy(target, request.data, request.data_size); |
2898 *result_size = request.req.data_size; |
2909 *result_size = request.data_size; |
2899 } |
2910 } |
2900 |
2911 |
2901 ec_sdo_request_clear(&request.req); |
2912 ec_sdo_request_clear(&request); |
2902 return retval; |
2913 return retval; |
2903 } |
2914 } |
2904 |
2915 |
2905 /*****************************************************************************/ |
2916 /*****************************************************************************/ |
2906 |
2917 |