Mercurial Mercurial > etherlabmaster / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
master/cdev.c
changeset 1831 1875b9fea0ba
parent 1826 ec6223c3b7ec
child 1837 32136215c1fa
equal deleted inserted replaced
1830:ef09f0ea0c4c 1831:1875b9fea0ba 
   282     data.current_on_ebus = slave->sii.current_on_ebus;
 
   282     data.current_on_ebus = slave->sii.current_on_ebus;
 
   283     for (i = 0; i < EC_MAX_PORTS; i++) {
 
   283     for (i = 0; i < EC_MAX_PORTS; i++) {
 
   284         data.ports[i].desc = slave->ports[i].desc;
 
   284         data.ports[i].desc = slave->ports[i].desc;
 
   285         data.ports[i].link.link_up = slave->ports[i].link.link_up;
 
   285         data.ports[i].link.link_up = slave->ports[i].link.link_up;
 
   286         data.ports[i].link.loop_closed = slave->ports[i].link.loop_closed;
 
   286         data.ports[i].link.loop_closed = slave->ports[i].link.loop_closed;
 
   287         data.ports[i].link.signal_detected = slave->ports[i].link.signal_detected;
 
   287         data.ports[i].link.signal_detected =
 
       
   288             slave->ports[i].link.signal_detected;
 
   288         data.ports[i].receive_time = slave->ports[i].receive_time;
 
   289         data.ports[i].receive_time = slave->ports[i].receive_time;
 
   289         if (slave->ports[i].next_slave) {
 
   290         if (slave->ports[i].next_slave) {
 
   290             data.ports[i].next_slave = slave->ports[i].next_slave->ring_position;
 
   291             data.ports[i].next_slave =
 
       
   292                 slave->ports[i].next_slave->ring_position;
 
   291         } else {
 
   293         } else {
 
   292             data.ports[i].next_slave = 0xffff;
 
   294             data.ports[i].next_slave = 0xffff;
 
   293         }
 
   295         }
 
   294         data.ports[i].delay_to_next_dc = slave->ports[i].delay_to_next_dc;
 
   296         data.ports[i].delay_to_next_dc = slave->ports[i].delay_to_next_dc;
 
   295     }
 
   297     }
 
   823         EC_ERR("Slave %u does not exist!\n", data.slave_position);
 
   825         EC_ERR("Slave %u does not exist!\n", data.slave_position);
 
   824         return -EINVAL;
 
   826         return -EINVAL;
 
   825     }
 
   827     }
 
   826 
   828 
   827     if (master->debug_level)
 
   829     if (master->debug_level)
 
   828         EC_DBG("Schedule SDO upload request for slave %u\n",request.slave->ring_position);
 
   830         EC_DBG("Schedule SDO upload request for slave %u\n",
 
       
   831                 request.slave->ring_position);
 
   829     // schedule request.
 
   832     // schedule request.
 
   830     list_add_tail(&request.list, &request.slave->slave_sdo_requests);
 
   833     list_add_tail(&request.list, &request.slave->slave_sdo_requests);
 
   831 
   834 
   832     up(&master->master_sem);
 
   835     up(&master->master_sem);
 
   833 
   836 
   845         // request already processing: interrupt not possible.
 
   848         // request already processing: interrupt not possible.
 
   846         up(&master->master_sem);
 
   849         up(&master->master_sem);
 
   847     }
 
   850     }
 
   848 
   851 
   849     // wait until master FSM has finished processing
 
   852     // wait until master FSM has finished processing
 
   850     wait_event(request.slave->sdo_queue, request.req.state != EC_INT_REQUEST_BUSY);
 
   853     wait_event(request.slave->sdo_queue,
 
       
   854             request.req.state != EC_INT_REQUEST_BUSY);
 
   851 
   855 
   852     if (master->debug_level)
 
   856     if (master->debug_level)
 
   853         EC_DBG("Scheduled SDO upload request for slave %u done\n",request.slave->ring_position);
 
   857         EC_DBG("Scheduled SDO upload request for slave %u done\n",
 
       
   858                 request.slave->ring_position);
 
   854 
   859 
   855     data.abort_code = request.req.abort_code;
 
   860     data.abort_code = request.req.abort_code;
 
   856 
   861 
   857     if (request.req.state != EC_INT_REQUEST_SUCCESS) {
 
   862     if (request.req.state != EC_INT_REQUEST_SUCCESS) {
 
   858         data.data_size = 0;
 
   863         data.data_size = 0;
 
   929         ec_sdo_request_clear(&request.req);
 
   934         ec_sdo_request_clear(&request.req);
 
   930         return -EINVAL;
 
   935         return -EINVAL;
 
   931     }
 
   936     }
 
   932     
   937     
   933     if (master->debug_level)
 
   938     if (master->debug_level)
 
   934         EC_DBG("Schedule SDO download request for slave %u\n",request.slave->ring_position);
 
   939         EC_DBG("Schedule SDO download request for slave %u\n",
 
       
   940                 request.slave->ring_position);
 
   935     // schedule request.
 
   941     // schedule request.
 
   936     list_add_tail(&request.list, &request.slave->slave_sdo_requests);
 
   942     list_add_tail(&request.list, &request.slave->slave_sdo_requests);
 
   937 
   943 
   938     up(&master->master_sem);
 
   944     up(&master->master_sem);
 
   939 
   945 
   951         // request already processing: interrupt not possible.
 
   957         // request already processing: interrupt not possible.
 
   952         up(&master->master_sem);
 
   958         up(&master->master_sem);
 
   953     }
 
   959     }
 
   954 
   960 
   955     // wait until master FSM has finished processing
 
   961     // wait until master FSM has finished processing
 
   956     wait_event(request.slave->sdo_queue, request.req.state != EC_INT_REQUEST_BUSY);
 
   962     wait_event(request.slave->sdo_queue,
 
       
   963             request.req.state != EC_INT_REQUEST_BUSY);
 
   957 
   964 
   958     if (master->debug_level)
 
   965     if (master->debug_level)
 
   959         EC_DBG("Scheduled SDO download request for slave %u done\n",request.slave->ring_position);
 
   966         EC_DBG("Scheduled SDO download request for slave %u done\n",
 
       
   967                 request.slave->ring_position);
 
   960 
   968 
   961     data.abort_code = request.req.abort_code;
 
   969     data.abort_code = request.req.abort_code;
 
   962 
   970 
   963     retval = request.req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
 
   971     retval = request.req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
 
   964 
   972 
  1118 
  1126 
  1119     if (!data.length)
 
  1127     if (!data.length)
 
  1120         return 0;
 
  1128         return 0;
 
  1121 
  1129 
  1122     if (!(contents = kmalloc(data.length, GFP_KERNEL))) {
 
  1130     if (!(contents = kmalloc(data.length, GFP_KERNEL))) {
 
  1123         EC_ERR("Failed to allocate %u bytes for register data.\n", data.length);
 
  1131         EC_ERR("Failed to allocate %u bytes for register data.\n",
 
       
  1132                 data.length);
 
  1124         return -ENOMEM;
 
  1133         return -ENOMEM;
 
  1125     }
 
  1134     }
 
  1126 
  1135 
  1127     if (down_interruptible(&master->master_sem))
 
  1136     if (down_interruptible(&master->master_sem))
 
  1128         return -EINTR;
 
  1137         return -EINTR;
 
  1195 
  1204 
  1196     if (!data.length)
 
  1205     if (!data.length)
 
  1197         return 0;
 
  1206         return 0;
 
  1198 
  1207 
  1199     if (!(contents = kmalloc(data.length, GFP_KERNEL))) {
 
  1208     if (!(contents = kmalloc(data.length, GFP_KERNEL))) {
 
  1200         EC_ERR("Failed to allocate %u bytes for register data.\n", data.length);
 
  1209         EC_ERR("Failed to allocate %u bytes for register data.\n",
 
       
  1210                 data.length);
 
  1201         return -ENOMEM;
 
  1211         return -ENOMEM;
 
  1202     }
 
  1212     }
 
  1203 
  1213 
  1204     if (copy_from_user(contents, (void __user *) data.data, data.length)) {
 
  1214     if (copy_from_user(contents, (void __user *) data.data, data.length)) {
 
  1205         kfree(contents);
 
  1215         kfree(contents);
 
  2238     }
 
  2248     }
 
  2239 
  2249 
  2240     up(&master->master_sem); // FIXME
 
  2250     up(&master->master_sem); // FIXME
 
  2241 
  2251 
  2242     if (data.complete_access) {
 
  2252     if (data.complete_access) {
 
  2243         ret = ecrt_slave_config_complete_sdo(sc, data.index, sdo_data, data.size);
 
  2253         ret = ecrt_slave_config_complete_sdo(sc,
 
       
  2254                 data.index, sdo_data, data.size);
 
  2244     } else {
 
  2255     } else {
 
  2245         ret = ecrt_slave_config_sdo(sc, data.index, data.subindex, sdo_data,
 
  2256         ret = ecrt_slave_config_sdo(sc, data.index, data.subindex, sdo_data,
 
  2246                 data.size);
 
  2257                 data.size);
 
  2247     }
 
  2258     }
 
  2248     kfree(sdo_data);
 
  2259     kfree(sdo_data);
 
  3100         // request already processing: interrupt not possible.
 
  3111         // request already processing: interrupt not possible.
 
  3101         up(&master->master_sem);
 
  3112         up(&master->master_sem);
 
  3102     }
 
  3113     }
 
  3103 
  3114 
  3104     // wait until master FSM has finished processing
 
  3115     // wait until master FSM has finished processing
 
  3105     wait_event(request.slave->foe_queue, request.req.state != EC_INT_REQUEST_BUSY);
 
  3116     wait_event(request.slave->foe_queue,
 
       
  3117             request.req.state != EC_INT_REQUEST_BUSY);
 
  3106 
  3118 
  3107     data.result = request.req.result;
 
  3119     data.result = request.req.result;
 
  3108     data.error_code = request.req.error_code;
 
  3120     data.error_code = request.req.error_code;
 
  3109 
  3121 
  3110     if (master->debug_level) {
 
  3122     if (master->debug_level) {
 
  3210         }
 
  3222         }
 
  3211         up(&master->master_sem);
 
  3223         up(&master->master_sem);
 
  3212     }
 
  3224     }
 
  3213 
  3225 
  3214     // wait until master FSM has finished processing
 
  3226     // wait until master FSM has finished processing
 
  3215     wait_event(request.slave->foe_queue, request.req.state != EC_INT_REQUEST_BUSY);
 
  3227     wait_event(request.slave->foe_queue,
 
       
  3228             request.req.state != EC_INT_REQUEST_BUSY);
 
  3216 
  3229 
  3217     data.result = request.req.result;
 
  3230     data.result = request.req.result;
 
  3218     data.error_code = request.req.error_code;
 
  3231     data.error_code = request.req.error_code;
 
  3219 
  3232 
  3220     retval = request.req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
 
  3233     retval = request.req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
 
  3224     }
 
  3237     }
 
  3225 
  3238 
  3226     ec_foe_request_clear(&request.req);
 
  3239     ec_foe_request_clear(&request.req);
 
  3227 
  3240 
  3228     if (master->debug_level) {
 
  3241     if (master->debug_level) {
 
  3229         printk("Finished FoE writing.\n");
 
  3242         EC_DBG("Finished FoE writing.\n");
 
       
  3243     }
 
       
  3244 
       
  3245     return retval;
 
       
  3246 }
 
       
  3247 
       
  3248 /*****************************************************************************/
 
       
  3249 
       
  3250 /** Read an SoE IDN.
 
       
  3251  */
 
       
  3252 int ec_cdev_ioctl_slave_soe_read(
 
       
  3253         ec_master_t *master, /**< EtherCAT master. */
 
       
  3254         unsigned long arg /**< ioctl() argument. */
 
       
  3255         )
 
       
  3256 {
 
       
  3257     ec_ioctl_slave_soe_t data;
 
       
  3258     ec_master_soe_request_t request;
 
       
  3259     int retval;
 
       
  3260 
       
  3261     if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
 
       
  3262         return -EFAULT;
 
       
  3263     }
 
       
  3264 
       
  3265     ec_soe_request_init(&request.req);
 
       
  3266     ec_soe_request_set_idn(&request.req, data.idn);
 
       
  3267     ec_soe_request_read(&request.req);
 
       
  3268 
       
  3269     if (down_interruptible(&master->master_sem))
 
       
  3270         return -EINTR;
 
       
  3271 
       
  3272     if (!(request.slave = ec_master_find_slave(
 
       
  3273                     master, 0, data.slave_position))) {
 
       
  3274         up(&master->master_sem);
 
       
  3275         ec_soe_request_clear(&request.req);
 
       
  3276         EC_ERR("Slave %u does not exist!\n", data.slave_position);
 
       
  3277         return -EINVAL;
 
       
  3278     }
 
       
  3279 
       
  3280     // schedule request.
 
       
  3281     list_add_tail(&request.list, &request.slave->soe_requests);
 
       
  3282 
       
  3283     up(&master->master_sem);
 
       
  3284 
       
  3285     if (master->debug_level) {
 
       
  3286         EC_DBG("Scheduled SoE read request on slave %u.\n",
 
       
  3287                 request.slave->ring_position);
 
       
  3288     }
 
       
  3289 
       
  3290     // wait for processing through FSM
 
       
  3291     if (wait_event_interruptible(request.slave->soe_queue,
 
       
  3292                 request.req.state != EC_INT_REQUEST_QUEUED)) {
 
       
  3293         // interrupted by signal
 
       
  3294         down(&master->master_sem);
 
       
  3295         if (request.req.state == EC_INT_REQUEST_QUEUED) {
 
       
  3296             list_del(&request.list);
 
       
  3297             up(&master->master_sem);
 
       
  3298             ec_soe_request_clear(&request.req);
 
       
  3299             return -EINTR;
 
       
  3300         }
 
       
  3301         // request already processing: interrupt not possible.
 
       
  3302         up(&master->master_sem);
 
       
  3303     }
 
       
  3304 
       
  3305     // wait until master FSM has finished processing
 
       
  3306     wait_event(request.slave->soe_queue,
 
       
  3307             request.req.state != EC_INT_REQUEST_BUSY);
 
       
  3308 
       
  3309     data.error_code = request.req.error_code;
 
       
  3310 
       
  3311     if (master->debug_level) {
 
       
  3312         EC_DBG("Read %zd bytes via SoE.\n", request.req.data_size);
 
       
  3313     }
 
       
  3314 
       
  3315     if (request.req.state != EC_INT_REQUEST_SUCCESS) {
 
       
  3316         data.data_size = 0;
 
       
  3317         retval = -EIO;
 
       
  3318     } else {
 
       
  3319         if (request.req.data_size > data.mem_size) {
 
       
  3320             EC_ERR("Buffer too small.\n");
 
       
  3321             ec_soe_request_clear(&request.req);
 
       
  3322             return -EOVERFLOW;
 
       
  3323         }
 
       
  3324         data.data_size = request.req.data_size;
 
       
  3325         if (copy_to_user((void __user *) data.data,
 
       
  3326                     request.req.data, data.data_size)) {
 
       
  3327             ec_soe_request_clear(&request.req);
 
       
  3328             return -EFAULT;
 
       
  3329         }
 
       
  3330         retval = 0;
 
       
  3331     }
 
       
  3332 
       
  3333     if (__copy_to_user((void __user *) arg, &data, sizeof(data))) {
 
       
  3334         retval = -EFAULT;
 
       
  3335     }
 
       
  3336 
       
  3337     if (master->debug_level)
 
       
  3338         EC_DBG("SoE read request finished on slave %u.\n",
 
       
  3339                 request.slave->ring_position);
 
       
  3340 
       
  3341     ec_soe_request_clear(&request.req);
 
       
  3342 
       
  3343     return retval;
 
       
  3344 }
 
       
  3345 
       
  3346 /*****************************************************************************/
 
       
  3347 
       
  3348 /** Write an IDN to a slave via SoE.
 
       
  3349  */
 
       
  3350 int ec_cdev_ioctl_slave_soe_write(
 
       
  3351         ec_master_t *master, /**< EtherCAT master. */
 
       
  3352         unsigned long arg /**< ioctl() argument. */
 
       
  3353         )
 
       
  3354 {
 
       
  3355     ec_ioctl_slave_soe_t data;
 
       
  3356     ec_master_soe_request_t request;
 
       
  3357     int retval;
 
       
  3358 
       
  3359     if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
 
       
  3360         return -EFAULT;
 
       
  3361     }
 
       
  3362 
       
  3363     INIT_LIST_HEAD(&request.list);
 
       
  3364 
       
  3365     ec_soe_request_init(&request.req);
 
       
  3366     ec_soe_request_set_idn(&request.req, data.idn);
 
       
  3367 
       
  3368     if (ec_soe_request_alloc(&request.req, data.mem_size)) {
 
       
  3369         ec_soe_request_clear(&request.req);
 
       
  3370         return -ENOMEM;
 
       
  3371     }
 
       
  3372     if (copy_from_user(request.req.data,
 
       
  3373                 (void __user *) data.data, data.mem_size)) {
 
       
  3374         ec_soe_request_clear(&request.req);
 
       
  3375         return -EFAULT;
 
       
  3376     }
 
       
  3377     request.req.data_size = data.mem_size;
 
       
  3378     ec_soe_request_write(&request.req);
 
       
  3379 
       
  3380     if (down_interruptible(&master->master_sem))
 
       
  3381         return -EINTR;
 
       
  3382 
       
  3383     if (!(request.slave = ec_master_find_slave(
 
       
  3384                     master, 0, data.slave_position))) {
 
       
  3385         up(&master->master_sem);
 
       
  3386         EC_ERR("Slave %u does not exist!\n", data.slave_position);
 
       
  3387         ec_soe_request_clear(&request.req);
 
       
  3388         return -EINVAL;
 
       
  3389     }
 
       
  3390 
       
  3391     if (master->debug_level) {
 
       
  3392         EC_DBG("Scheduling SoE write request.\n");
 
       
  3393     }
 
       
  3394 
       
  3395     // schedule SoE write request.
 
       
  3396     list_add_tail(&request.list, &request.slave->soe_requests);
 
       
  3397 
       
  3398     up(&master->master_sem);
 
       
  3399 
       
  3400     // wait for processing through FSM
 
       
  3401     if (wait_event_interruptible(request.slave->soe_queue,
 
       
  3402                 request.req.state != EC_INT_REQUEST_QUEUED)) {
 
       
  3403         // interrupted by signal
 
       
  3404         down(&master->master_sem);
 
       
  3405         if (request.req.state == EC_INT_REQUEST_QUEUED) {
 
       
  3406             // abort request
 
       
  3407             list_del(&request.list);
 
       
  3408             up(&master->master_sem);
 
       
  3409             ec_soe_request_clear(&request.req);
 
       
  3410             return -EINTR;
 
       
  3411         }
 
       
  3412         up(&master->master_sem);
 
       
  3413     }
 
       
  3414 
       
  3415     // wait until master FSM has finished processing
 
       
  3416     wait_event(request.slave->soe_queue,
 
       
  3417             request.req.state != EC_INT_REQUEST_BUSY);
 
       
  3418 
       
  3419     //data.result = request.req.result;
 
       
  3420 
       
  3421     retval = request.req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
 
       
  3422 
       
  3423     if (__copy_to_user((void __user *) arg, &data, sizeof(data))) {
 
       
  3424         retval = -EFAULT;
 
       
  3425     }
 
       
  3426 
       
  3427     ec_soe_request_clear(&request.req);
 
       
  3428 
       
  3429     if (master->debug_level) {
 
       
  3430         EC_DBG("Finished SoE writing.\n");
 
  3230     }
 
  3431     }
 
  3231 
  3432 
  3232     return retval;
 
  3433     return retval;
 
  3233 }
 
  3434 }
 
  3234 
  3435 
  3352             return ec_cdev_ioctl_slave_foe_read(master, arg);
 
  3553             return ec_cdev_ioctl_slave_foe_read(master, arg);
 
  3353         case EC_IOCTL_SLAVE_FOE_WRITE:
 
  3554         case EC_IOCTL_SLAVE_FOE_WRITE:
 
  3354             if (!(filp->f_mode & FMODE_WRITE))
 
  3555             if (!(filp->f_mode & FMODE_WRITE))
 
  3355                 return -EPERM;
 
  3556                 return -EPERM;
 
  3356             return ec_cdev_ioctl_slave_foe_write(master, arg);
 
  3557             return ec_cdev_ioctl_slave_foe_write(master, arg);
 
       
  3558         case EC_IOCTL_SLAVE_SOE_READ:
 
       
  3559             return ec_cdev_ioctl_slave_soe_read(master, arg);
 
       
  3560         case EC_IOCTL_SLAVE_SOE_WRITE:
 
       
  3561             if (!(filp->f_mode & FMODE_WRITE))
 
       
  3562                 return -EPERM;
 
       
  3563             return ec_cdev_ioctl_slave_soe_write(master, arg);
 
  3357         case EC_IOCTL_CONFIG:
 
  3564         case EC_IOCTL_CONFIG:
 
  3358             return ec_cdev_ioctl_config(master, arg);
 
  3565             return ec_cdev_ioctl_config(master, arg);
 
  3359         case EC_IOCTL_CONFIG_PDO:
 
  3566         case EC_IOCTL_CONFIG_PDO:
 
  3360             return ec_cdev_ioctl_config_pdo(master, arg);
 
  3567             return ec_cdev_ioctl_config_pdo(master, arg);
 
  3361         case EC_IOCTL_CONFIG_PDO_ENTRY:
 
  3568         case EC_IOCTL_CONFIG_PDO_ENTRY:
etherlabmaster