838 ec_master_t *master, /**< EtherCAT master. */ |
838 ec_master_t *master, /**< EtherCAT master. */ |
839 unsigned long arg /**< ioctl() argument. */ |
839 unsigned long arg /**< ioctl() argument. */ |
840 ) |
840 ) |
841 { |
841 { |
842 ec_ioctl_slave_sdo_upload_t data; |
842 ec_ioctl_slave_sdo_upload_t data; |
843 ec_master_sdo_request_t request; |
843 ec_master_sdo_request_t* request; |
844 int retval; |
844 int retval; |
845 |
845 |
846 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
846 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
847 return -EFAULT; |
847 return -EFAULT; |
848 } |
848 } |
849 |
849 |
850 ec_sdo_request_init(&request.req); |
850 request = kmalloc(sizeof(*request), GFP_KERNEL); |
851 ec_sdo_request_address(&request.req, |
851 if (!request) |
852 data.sdo_index, data.sdo_entry_subindex); |
852 return -ENOMEM; |
853 ecrt_sdo_request_read(&request.req); |
853 kref_init(&request->refcount); |
854 |
854 |
855 if (down_interruptible(&master->master_sem)) |
855 ec_sdo_request_init(&request->req); |
856 return -EINTR; |
856 ec_sdo_request_address(&request->req, |
857 |
857 data.sdo_index, data.sdo_entry_subindex); |
858 if (!(request.slave = ec_master_find_slave( |
858 ecrt_sdo_request_read(&request->req); |
|
859 |
|
860 if (down_interruptible(&master->master_sem)) { |
|
861 kref_put(&request->refcount,ec_master_sdo_request_release); |
|
862 return -EINTR; |
|
863 } |
|
864 if (!(request->slave = ec_master_find_slave( |
859 master, 0, data.slave_position))) { |
865 master, 0, data.slave_position))) { |
860 up(&master->master_sem); |
866 up(&master->master_sem); |
861 ec_sdo_request_clear(&request.req); |
867 kref_put(&request->refcount,ec_master_sdo_request_release); |
862 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
868 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
863 data.slave_position); |
869 data.slave_position); |
864 return -EINVAL; |
870 return -EINVAL; |
865 } |
871 } |
866 |
872 |
867 EC_SLAVE_DBG(request.slave, 1, "Schedule SDO upload request.\n"); |
873 EC_SLAVE_DBG(request->slave, 1, "Schedule SDO upload request %p.\n",request); |
868 |
874 |
869 // schedule request. |
875 // schedule request. |
870 list_add_tail(&request.list, &request.slave->slave_sdo_requests); |
876 kref_get(&request->refcount); |
|
877 list_add_tail(&request->list, &request->slave->slave_sdo_requests); |
871 |
878 |
872 up(&master->master_sem); |
879 up(&master->master_sem); |
873 |
880 |
874 // wait for processing through FSM |
881 // wait for processing through FSM |
875 if (wait_event_interruptible(request.slave->sdo_queue, |
882 if (wait_event_interruptible(request->slave->sdo_queue, |
876 request.req.state != EC_INT_REQUEST_QUEUED)) { |
883 ((request->req.state == EC_INT_REQUEST_SUCCESS) || (request->req.state == EC_INT_REQUEST_FAILURE)))) { |
877 // interrupted by signal |
884 // interrupted by signal |
878 down(&master->master_sem); |
885 kref_put(&request->refcount,ec_master_sdo_request_release); |
879 if (request.req.state == EC_INT_REQUEST_QUEUED) { |
886 return -EINTR; |
880 list_del(&request.list); |
887 } |
881 up(&master->master_sem); |
888 |
882 ec_sdo_request_clear(&request.req); |
889 EC_SLAVE_DBG(request->slave, 1, "Finished SDO upload request %p.\n",request); |
883 return -EINTR; |
890 |
884 } |
891 data.abort_code = request->req.abort_code; |
885 // request already processing: interrupt not possible. |
892 |
886 up(&master->master_sem); |
893 if (request->req.state != EC_INT_REQUEST_SUCCESS) { |
887 } |
|
888 |
|
889 // wait until master FSM has finished processing |
|
890 wait_event(request.slave->sdo_queue, |
|
891 request.req.state != EC_INT_REQUEST_BUSY); |
|
892 |
|
893 EC_SLAVE_DBG(request.slave, 1, "Finished SDO upload request.\n"); |
|
894 |
|
895 data.abort_code = request.req.abort_code; |
|
896 |
|
897 if (request.req.state != EC_INT_REQUEST_SUCCESS) { |
|
898 data.data_size = 0; |
894 data.data_size = 0; |
899 if (request.req.errno) { |
895 if (request->req.errno) { |
900 retval = -request.req.errno; |
896 retval = -request->req.errno; |
901 } else { |
897 } else { |
902 retval = -EIO; |
898 retval = -EIO; |
903 } |
899 } |
904 } else { |
900 } else { |
905 if (request.req.data_size > data.target_size) { |
901 if (request->req.data_size > data.target_size) { |
906 EC_MASTER_ERR(master, "Buffer too small.\n"); |
902 EC_MASTER_ERR(master, "Buffer too small.\n"); |
907 ec_sdo_request_clear(&request.req); |
903 kref_put(&request->refcount,ec_master_sdo_request_release); |
908 return -EOVERFLOW; |
904 return -EOVERFLOW; |
909 } |
905 } |
910 data.data_size = request.req.data_size; |
906 data.data_size = request->req.data_size; |
911 |
907 |
912 if (copy_to_user((void __user *) data.target, |
908 if (copy_to_user((void __user *) data.target, |
913 request.req.data, data.data_size)) { |
909 request->req.data, data.data_size)) { |
914 ec_sdo_request_clear(&request.req); |
910 kref_put(&request->refcount,ec_master_sdo_request_release); |
915 return -EFAULT; |
911 return -EFAULT; |
916 } |
912 } |
917 retval = 0; |
913 retval = 0; |
918 } |
914 } |
919 |
915 |
920 if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { |
916 if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { |
921 retval = -EFAULT; |
917 retval = -EFAULT; |
922 } |
918 } |
923 |
919 |
924 ec_sdo_request_clear(&request.req); |
920 kref_put(&request->refcount,ec_master_sdo_request_release); |
925 return retval; |
921 return retval; |
926 } |
922 } |
927 |
923 |
928 /*****************************************************************************/ |
924 /*****************************************************************************/ |
929 |
925 |
946 if (!data.data_size) { |
942 if (!data.data_size) { |
947 EC_MASTER_ERR(master, "Zero data size!\n"); |
943 EC_MASTER_ERR(master, "Zero data size!\n"); |
948 return -EINVAL; |
944 return -EINVAL; |
949 } |
945 } |
950 |
946 |
951 ec_sdo_request_init(&request.req); |
947 request = kmalloc(sizeof(*request), GFP_KERNEL); |
952 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, |
953 data.sdo_index, data.sdo_entry_subindex); |
954 data.sdo_index, data.sdo_entry_subindex); |
954 if (ec_sdo_request_alloc(&request.req, data.data_size)) { |
955 if (ec_sdo_request_alloc(&request->req, data.data_size)) { |
955 ec_sdo_request_clear(&request.req); |
956 kref_put(&request->refcount,ec_master_sdo_request_release); |
956 return -ENOMEM; |
957 return -ENOMEM; |
957 } |
958 } |
958 if (copy_from_user(request.req.data, |
959 if (copy_from_user(request->req.data, |
959 (void __user *) data.data, data.data_size)) { |
960 (void __user *) data.data, data.data_size)) { |
960 ec_sdo_request_clear(&request.req); |
961 kref_put(&request->refcount,ec_master_sdo_request_release); |
961 return -EFAULT; |
962 return -EFAULT; |
962 } |
963 } |
963 request.req.data_size = data.data_size; |
964 request->req.data_size = data.data_size; |
964 ecrt_sdo_request_write(&request.req); |
965 ecrt_sdo_request_write(&request->req); |
965 |
966 |
966 if (down_interruptible(&master->master_sem)) |
967 if (down_interruptible(&master->master_sem)) { |
967 return -EINTR; |
968 kref_put(&request->refcount,ec_master_sdo_request_release); |
968 |
969 return -EINTR; |
969 if (!(request.slave = ec_master_find_slave( |
970 } |
|
971 if (!(request->slave = ec_master_find_slave( |
970 master, 0, data.slave_position))) { |
972 master, 0, data.slave_position))) { |
971 up(&master->master_sem); |
973 up(&master->master_sem); |
972 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
974 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
973 data.slave_position); |
975 data.slave_position); |
974 ec_sdo_request_clear(&request.req); |
976 kref_put(&request->refcount,ec_master_sdo_request_release); |
975 return -EINVAL; |
977 return -EINVAL; |
976 } |
978 } |
977 |
979 |
978 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); |
979 |
981 |
980 // schedule request. |
982 // schedule request. |
981 list_add_tail(&request.list, &request.slave->slave_sdo_requests); |
983 kref_get(&request->refcount); |
|
984 list_add_tail(&request->list, &request->slave->slave_sdo_requests); |
982 |
985 |
983 up(&master->master_sem); |
986 up(&master->master_sem); |
984 |
987 |
985 // wait for processing through FSM |
988 // wait for processing through FSM |
986 if (wait_event_interruptible(request.slave->sdo_queue, |
989 if (wait_event_interruptible(request->slave->sdo_queue, |
987 request.req.state != EC_INT_REQUEST_QUEUED)) { |
990 ((request->req.state == EC_INT_REQUEST_SUCCESS) || (request->req.state == EC_INT_REQUEST_FAILURE)))) { |
988 // interrupted by signal |
991 // interrupted by signal |
989 down(&master->master_sem); |
992 kref_put(&request->refcount,ec_master_sdo_request_release); |
990 if (request.req.state == EC_INT_REQUEST_QUEUED) { |
993 return -EINTR; |
991 list_del(&request.list); |
994 } |
992 up(&master->master_sem); |
995 |
993 ec_sdo_request_clear(&request.req); |
996 EC_SLAVE_DBG(request->slave, 1, "Finished SDO download request %p.\n",request); |
994 return -EINTR; |
997 |
995 } |
998 data.abort_code = request->req.abort_code; |
996 // request already processing: interrupt not possible. |
999 |
997 up(&master->master_sem); |
1000 if (request->req.state == EC_INT_REQUEST_SUCCESS) { |
998 } |
|
999 |
|
1000 // wait until master FSM has finished processing |
|
1001 wait_event(request.slave->sdo_queue, |
|
1002 request.req.state != EC_INT_REQUEST_BUSY); |
|
1003 |
|
1004 EC_SLAVE_DBG(request.slave, 1, "Finished SDO download request.\n"); |
|
1005 |
|
1006 data.abort_code = request.req.abort_code; |
|
1007 |
|
1008 if (request.req.state == EC_INT_REQUEST_SUCCESS) { |
|
1009 retval = 0; |
1001 retval = 0; |
1010 } else if (request.req.errno) { |
1002 } else if (request->req.errno) { |
1011 retval = -request.req.errno; |
1003 retval = -request->req.errno; |
1012 } else { |
1004 } else { |
1013 retval = -EIO; |
1005 retval = -EIO; |
1014 } |
1006 } |
1015 |
1007 |
1016 if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { |
1008 if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { |
1017 retval = -EFAULT; |
1009 retval = -EFAULT; |
1018 } |
1010 } |
1019 |
1011 |
1020 ec_sdo_request_clear(&request.req); |
1012 kref_put(&request->refcount,ec_master_sdo_request_release); |
1021 return retval; |
1013 return retval; |
1022 } |
1014 } |
1023 |
1015 |
1024 /*****************************************************************************/ |
1016 /*****************************************************************************/ |
1025 |
1017 |