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