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