3317 ec_master_t *master, /**< EtherCAT master. */ |
3317 ec_master_t *master, /**< EtherCAT master. */ |
3318 unsigned long arg /**< ioctl() argument. */ |
3318 unsigned long arg /**< ioctl() argument. */ |
3319 ) |
3319 ) |
3320 { |
3320 { |
3321 ec_ioctl_slave_soe_read_t ioctl; |
3321 ec_ioctl_slave_soe_read_t ioctl; |
3322 ec_master_soe_request_t request; |
3322 u8 *data; |
3323 int retval; |
3323 int retval; |
3324 |
3324 |
3325 if (copy_from_user(&ioctl, (void __user *) arg, sizeof(ioctl))) { |
3325 if (copy_from_user(&ioctl, (void __user *) arg, sizeof(ioctl))) { |
3326 return -EFAULT; |
3326 return -EFAULT; |
3327 } |
3327 } |
3328 |
3328 |
3329 ec_soe_request_init(&request.req); |
3329 data = kmalloc(ioctl.mem_size, GFP_KERNEL); |
3330 ec_soe_request_set_idn(&request.req, ioctl.idn); |
3330 if (!data) { |
3331 ec_soe_request_read(&request.req); |
3331 EC_MASTER_ERR(master, "Failed to allocate %u bytes of IDN data.\n", |
3332 |
3332 ioctl.mem_size); |
3333 if (down_interruptible(&master->master_sem)) |
3333 return -ENOMEM; |
3334 return -EINTR; |
3334 } |
3335 |
3335 |
3336 if (!(request.slave = ec_master_find_slave( |
3336 retval = ecrt_master_read_idn(master, ioctl.slave_position, |
3337 master, 0, ioctl.slave_position))) { |
3337 ioctl.idn, data, ioctl.mem_size, &ioctl.data_size, |
3338 up(&master->master_sem); |
3338 &ioctl.error_code); |
3339 ec_soe_request_clear(&request.req); |
3339 if (retval) { |
3340 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
3340 kfree(data); |
3341 ioctl.slave_position); |
3341 return retval; |
3342 return -EINVAL; |
3342 } |
3343 } |
3343 |
3344 |
3344 if (copy_to_user((void __user *) ioctl.data, |
3345 // schedule request. |
3345 data, ioctl.data_size)) { |
3346 list_add_tail(&request.list, &request.slave->soe_requests); |
3346 kfree(data); |
3347 |
3347 return -EFAULT; |
3348 up(&master->master_sem); |
3348 } |
3349 |
3349 kfree(data); |
3350 EC_SLAVE_DBG(request.slave, 1, "Scheduled SoE read request.\n"); |
|
3351 |
|
3352 // wait for processing through FSM |
|
3353 if (wait_event_interruptible(request.slave->soe_queue, |
|
3354 request.req.state != EC_INT_REQUEST_QUEUED)) { |
|
3355 // interrupted by signal |
|
3356 down(&master->master_sem); |
|
3357 if (request.req.state == EC_INT_REQUEST_QUEUED) { |
|
3358 list_del(&request.list); |
|
3359 up(&master->master_sem); |
|
3360 ec_soe_request_clear(&request.req); |
|
3361 return -EINTR; |
|
3362 } |
|
3363 // request already processing: interrupt not possible. |
|
3364 up(&master->master_sem); |
|
3365 } |
|
3366 |
|
3367 // wait until master FSM has finished processing |
|
3368 wait_event(request.slave->soe_queue, |
|
3369 request.req.state != EC_INT_REQUEST_BUSY); |
|
3370 |
|
3371 ioctl.error_code = request.req.error_code; |
|
3372 |
|
3373 EC_SLAVE_DBG(request.slave, 1, "Read %zd bytes via SoE.\n", |
|
3374 request.req.data_size); |
|
3375 |
|
3376 if (request.req.state != EC_INT_REQUEST_SUCCESS) { |
|
3377 ioctl.data_size = 0; |
|
3378 retval = -EIO; |
|
3379 } else { |
|
3380 if (request.req.data_size > ioctl.mem_size) { |
|
3381 EC_MASTER_ERR(master, "Buffer too small.\n"); |
|
3382 ec_soe_request_clear(&request.req); |
|
3383 return -EOVERFLOW; |
|
3384 } |
|
3385 ioctl.data_size = request.req.data_size; |
|
3386 if (copy_to_user((void __user *) ioctl.data, |
|
3387 request.req.data, ioctl.data_size)) { |
|
3388 ec_soe_request_clear(&request.req); |
|
3389 return -EFAULT; |
|
3390 } |
|
3391 retval = 0; |
|
3392 } |
|
3393 |
3350 |
3394 if (__copy_to_user((void __user *) arg, &ioctl, sizeof(ioctl))) { |
3351 if (__copy_to_user((void __user *) arg, &ioctl, sizeof(ioctl))) { |
3395 retval = -EFAULT; |
3352 retval = -EFAULT; |
3396 } |
3353 } |
3397 |
3354 |
3398 EC_SLAVE_DBG(request.slave, 1, "Finished SoE read request.\n"); |
3355 EC_MASTER_DBG(master, 1, "Finished SoE read request.\n"); |
3399 |
|
3400 ec_soe_request_clear(&request.req); |
|
3401 |
|
3402 return retval; |
3356 return retval; |
3403 } |
3357 } |
3404 |
3358 |
3405 /*****************************************************************************/ |
3359 /*****************************************************************************/ |
3406 |
3360 |
3410 ec_master_t *master, /**< EtherCAT master. */ |
3364 ec_master_t *master, /**< EtherCAT master. */ |
3411 unsigned long arg /**< ioctl() argument. */ |
3365 unsigned long arg /**< ioctl() argument. */ |
3412 ) |
3366 ) |
3413 { |
3367 { |
3414 ec_ioctl_slave_soe_write_t ioctl; |
3368 ec_ioctl_slave_soe_write_t ioctl; |
3415 ec_master_soe_request_t request; |
3369 u8 *data; |
3416 int retval; |
3370 int retval; |
3417 |
3371 |
3418 if (copy_from_user(&ioctl, (void __user *) arg, sizeof(ioctl))) { |
3372 if (copy_from_user(&ioctl, (void __user *) arg, sizeof(ioctl))) { |
3419 return -EFAULT; |
3373 return -EFAULT; |
3420 } |
3374 } |
3421 |
3375 |
3422 INIT_LIST_HEAD(&request.list); |
3376 data = kmalloc(ioctl.data_size, GFP_KERNEL); |
3423 |
3377 if (!data) { |
3424 ec_soe_request_init(&request.req); |
3378 EC_MASTER_ERR(master, "Failed to allocate %u bytes of IDN data.\n", |
3425 ec_soe_request_set_idn(&request.req, ioctl.idn); |
3379 ioctl.data_size); |
3426 |
|
3427 if (ec_soe_request_alloc(&request.req, ioctl.data_size)) { |
|
3428 ec_soe_request_clear(&request.req); |
|
3429 return -ENOMEM; |
3380 return -ENOMEM; |
3430 } |
3381 } |
3431 if (copy_from_user(request.req.data, |
3382 if (copy_from_user(data, (void __user *) ioctl.data, ioctl.data_size)) { |
3432 (void __user *) ioctl.data, ioctl.data_size)) { |
3383 kfree(data); |
3433 ec_soe_request_clear(&request.req); |
3384 return -EFAULT; |
3434 return -EFAULT; |
3385 } |
3435 } |
3386 |
3436 request.req.data_size = ioctl.data_size; |
3387 retval = ecrt_master_write_idn(master, ioctl.slave_position, |
3437 ec_soe_request_write(&request.req); |
3388 ioctl.idn, data, ioctl.data_size, &ioctl.error_code); |
3438 |
3389 kfree(data); |
3439 if (down_interruptible(&master->master_sem)) |
3390 if (retval) { |
3440 return -EINTR; |
3391 return retval; |
3441 |
3392 } |
3442 if (!(request.slave = ec_master_find_slave( |
|
3443 master, 0, ioctl.slave_position))) { |
|
3444 up(&master->master_sem); |
|
3445 EC_MASTER_ERR(master, "Slave %u does not exist!\n", |
|
3446 ioctl.slave_position); |
|
3447 ec_soe_request_clear(&request.req); |
|
3448 return -EINVAL; |
|
3449 } |
|
3450 |
|
3451 EC_SLAVE_DBG(request.slave, 1, "Scheduling SoE write request.\n"); |
|
3452 |
|
3453 // schedule SoE write request. |
|
3454 list_add_tail(&request.list, &request.slave->soe_requests); |
|
3455 |
|
3456 up(&master->master_sem); |
|
3457 |
|
3458 // wait for processing through FSM |
|
3459 if (wait_event_interruptible(request.slave->soe_queue, |
|
3460 request.req.state != EC_INT_REQUEST_QUEUED)) { |
|
3461 // interrupted by signal |
|
3462 down(&master->master_sem); |
|
3463 if (request.req.state == EC_INT_REQUEST_QUEUED) { |
|
3464 // abort request |
|
3465 list_del(&request.list); |
|
3466 up(&master->master_sem); |
|
3467 ec_soe_request_clear(&request.req); |
|
3468 return -EINTR; |
|
3469 } |
|
3470 up(&master->master_sem); |
|
3471 } |
|
3472 |
|
3473 // wait until master FSM has finished processing |
|
3474 wait_event(request.slave->soe_queue, |
|
3475 request.req.state != EC_INT_REQUEST_BUSY); |
|
3476 |
|
3477 ioctl.error_code = request.req.error_code; |
|
3478 retval = request.req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO; |
|
3479 |
3393 |
3480 if (__copy_to_user((void __user *) arg, &ioctl, sizeof(ioctl))) { |
3394 if (__copy_to_user((void __user *) arg, &ioctl, sizeof(ioctl))) { |
3481 retval = -EFAULT; |
3395 retval = -EFAULT; |
3482 } |
3396 } |
3483 |
3397 |
3484 ec_soe_request_clear(&request.req); |
3398 EC_MASTER_DBG(master, 1, "Finished SoE write request.\n"); |
3485 |
|
3486 EC_SLAVE_DBG(request.slave, 1, "Finished SoE write request.\n"); |
|
3487 |
|
3488 return retval; |
3399 return retval; |
3489 } |
3400 } |
3490 |
3401 |
3491 /****************************************************************************** |
3402 /****************************************************************************** |
3492 * File operations |
3403 * File operations |