1393 int ec_cdev_ioctl_config_sdo( |
1393 int ec_cdev_ioctl_config_sdo( |
1394 ec_master_t *master, /**< EtherCAT master. */ |
1394 ec_master_t *master, /**< EtherCAT master. */ |
1395 unsigned long arg /**< ioctl() argument. */ |
1395 unsigned long arg /**< ioctl() argument. */ |
1396 ) |
1396 ) |
1397 { |
1397 { |
1398 ec_ioctl_config_sdo_t data; |
1398 ec_ioctl_config_sdo_t *ioctl; |
1399 const ec_slave_config_t *sc; |
1399 const ec_slave_config_t *sc; |
1400 const ec_sdo_request_t *req; |
1400 const ec_sdo_request_t *req; |
1401 |
1401 |
1402 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
1402 if (!(ioctl = kmalloc(sizeof(*ioctl), GFP_KERNEL))) { |
1403 return -EFAULT; |
1403 return -ENOMEM; |
1404 } |
1404 } |
1405 |
1405 |
1406 if (down_interruptible(&master->master_sem)) |
1406 if (copy_from_user(ioctl, (void __user *) arg, sizeof(*ioctl))) { |
|
1407 kfree(ioctl); |
|
1408 return -EFAULT; |
|
1409 } |
|
1410 |
|
1411 if (down_interruptible(&master->master_sem)) { |
|
1412 kfree(ioctl); |
1407 return -EINTR; |
1413 return -EINTR; |
|
1414 } |
1408 |
1415 |
1409 if (!(sc = ec_master_get_config_const( |
1416 if (!(sc = ec_master_get_config_const( |
1410 master, data.config_index))) { |
1417 master, ioctl->config_index))) { |
1411 up(&master->master_sem); |
1418 up(&master->master_sem); |
1412 EC_MASTER_ERR(master, "Slave config %u does not exist!\n", |
1419 EC_MASTER_ERR(master, "Slave config %u does not exist!\n", |
1413 data.config_index); |
1420 ioctl->config_index); |
|
1421 kfree(ioctl); |
1414 return -EINVAL; |
1422 return -EINVAL; |
1415 } |
1423 } |
1416 |
1424 |
1417 if (!(req = ec_slave_config_get_sdo_by_pos_const( |
1425 if (!(req = ec_slave_config_get_sdo_by_pos_const( |
1418 sc, data.sdo_pos))) { |
1426 sc, ioctl->sdo_pos))) { |
1419 up(&master->master_sem); |
1427 up(&master->master_sem); |
1420 EC_MASTER_ERR(master, "Invalid SDO position!\n"); |
1428 EC_MASTER_ERR(master, "Invalid SDO position!\n"); |
1421 return -EINVAL; |
1429 kfree(ioctl); |
1422 } |
1430 return -EINVAL; |
1423 |
1431 } |
1424 data.index = req->index; |
1432 |
1425 data.subindex = req->subindex; |
1433 ioctl->index = req->index; |
1426 data.size = req->data_size; |
1434 ioctl->subindex = req->subindex; |
1427 memcpy(&data.data, req->data, |
1435 ioctl->size = req->data_size; |
1428 min((u32) data.size, (u32) EC_MAX_SDO_DATA_SIZE)); |
1436 memcpy(ioctl->data, req->data, |
|
1437 min((u32) ioctl->size, (u32) EC_MAX_SDO_DATA_SIZE)); |
1429 |
1438 |
1430 up(&master->master_sem); |
1439 up(&master->master_sem); |
1431 |
1440 |
1432 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
1441 if (copy_to_user((void __user *) arg, ioctl, sizeof(*ioctl))) { |
1433 return -EFAULT; |
1442 kfree(ioctl); |
1434 |
1443 return -EFAULT; |
|
1444 } |
|
1445 |
|
1446 kfree(ioctl); |
1435 return 0; |
1447 return 0; |
1436 } |
1448 } |
1437 |
1449 |
1438 /*****************************************************************************/ |
1450 /*****************************************************************************/ |
1439 |
1451 |
1442 int ec_cdev_ioctl_config_idn( |
1454 int ec_cdev_ioctl_config_idn( |
1443 ec_master_t *master, /**< EtherCAT master. */ |
1455 ec_master_t *master, /**< EtherCAT master. */ |
1444 unsigned long arg /**< ioctl() argument. */ |
1456 unsigned long arg /**< ioctl() argument. */ |
1445 ) |
1457 ) |
1446 { |
1458 { |
1447 ec_ioctl_config_idn_t data; |
1459 ec_ioctl_config_idn_t *ioctl; |
1448 const ec_slave_config_t *sc; |
1460 const ec_slave_config_t *sc; |
1449 const ec_soe_request_t *req; |
1461 const ec_soe_request_t *req; |
1450 |
1462 |
1451 if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { |
1463 if (!(ioctl = kmalloc(sizeof(*ioctl), GFP_KERNEL))) { |
1452 return -EFAULT; |
1464 return -ENOMEM; |
1453 } |
1465 } |
1454 |
1466 |
1455 if (down_interruptible(&master->master_sem)) |
1467 if (copy_from_user(ioctl, (void __user *) arg, sizeof(*ioctl))) { |
|
1468 kfree(ioctl); |
|
1469 return -EFAULT; |
|
1470 } |
|
1471 |
|
1472 if (down_interruptible(&master->master_sem)) { |
|
1473 kfree(ioctl); |
1456 return -EINTR; |
1474 return -EINTR; |
|
1475 } |
1457 |
1476 |
1458 if (!(sc = ec_master_get_config_const( |
1477 if (!(sc = ec_master_get_config_const( |
1459 master, data.config_index))) { |
1478 master, ioctl->config_index))) { |
1460 up(&master->master_sem); |
1479 up(&master->master_sem); |
1461 EC_MASTER_ERR(master, "Slave config %u does not exist!\n", |
1480 EC_MASTER_ERR(master, "Slave config %u does not exist!\n", |
1462 data.config_index); |
1481 ioctl->config_index); |
|
1482 kfree(ioctl); |
1463 return -EINVAL; |
1483 return -EINVAL; |
1464 } |
1484 } |
1465 |
1485 |
1466 if (!(req = ec_slave_config_get_idn_by_pos_const( |
1486 if (!(req = ec_slave_config_get_idn_by_pos_const( |
1467 sc, data.idn_pos))) { |
1487 sc, ioctl->idn_pos))) { |
1468 up(&master->master_sem); |
1488 up(&master->master_sem); |
1469 EC_MASTER_ERR(master, "Invalid IDN position!\n"); |
1489 EC_MASTER_ERR(master, "Invalid IDN position!\n"); |
1470 return -EINVAL; |
1490 kfree(ioctl); |
1471 } |
1491 return -EINVAL; |
1472 |
1492 } |
1473 data.drive_no = req->drive_no; |
1493 |
1474 data.idn = req->idn; |
1494 ioctl->drive_no = req->drive_no; |
1475 data.state = req->state; |
1495 ioctl->idn = req->idn; |
1476 data.size = req->data_size; |
1496 ioctl->state = req->state; |
1477 memcpy(&data.data, req->data, |
1497 ioctl->size = req->data_size; |
1478 min((u32) data.size, (u32) EC_MAX_IDN_DATA_SIZE)); |
1498 memcpy(ioctl->data, req->data, |
|
1499 min((u32) ioctl->size, (u32) EC_MAX_IDN_DATA_SIZE)); |
1479 |
1500 |
1480 up(&master->master_sem); |
1501 up(&master->master_sem); |
1481 |
1502 |
1482 if (copy_to_user((void __user *) arg, &data, sizeof(data))) |
1503 if (copy_to_user((void __user *) arg, ioctl, sizeof(*ioctl))) { |
1483 return -EFAULT; |
1504 kfree(ioctl); |
1484 |
1505 return -EFAULT; |
|
1506 } |
|
1507 |
|
1508 kfree(ioctl); |
1485 return 0; |
1509 return 0; |
1486 } |
1510 } |
1487 |
1511 |
1488 /*****************************************************************************/ |
1512 /*****************************************************************************/ |
1489 |
1513 |