1510 }

  1511 

  1512 /*****************************************************************************/

  1513 

  1514 /** Set the direction of a sync manager.

  1515  */

  1516 int ec_cdev_ioctl_sc_sync(

  1517         ec_master_t *master, /**< EtherCAT master. */

  1518         unsigned long arg, /**< ioctl() argument. */

  1519         ec_cdev_priv_t *priv /**< Private data structure of file handle. */

  1520         )

  1521 {

  1522     ec_ioctl_config_t data;

  1523     ec_slave_config_t *sc;

  1524     unsigned int i;

  1525     int ret = 0;

  1526 

  1527 	if (unlikely(!priv->requested)) {

  1528         ret = -EPERM;

  1529         goto out_return;

  1530     }

  1531 

  1532     if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {

  1533         ret = -EFAULT;

  1534         goto out_return;

  1535     }

  1536 

  1537     if (down_interruptible(&master->master_sem)) {

  1538         ret = -EINTR;

  1539         goto out_return;

  1540     }

  1541 

  1542     if (!(sc = ec_master_get_config(master, data.config_index))) {

  1543         ret = -ESRCH;

  1544         goto out_up;

  1545     }

  1546 

  1547     for (i = 0; i < EC_MAX_SYNC_MANAGERS; i++) {

  1548         ec_direction_t dir = data.syncs[i].dir;

  1549         if (dir == EC_DIR_INPUT || dir == EC_DIR_OUTPUT) {

  1550             if (ecrt_slave_config_sync_manager(sc, i, dir)) {

  1551                 ret = -EINVAL;

  1552                 goto out_up;

  1553             }

  1554         }

  1555     }

  1556 

  1557 out_up:

  1558     up(&master->master_sem);

  1559 out_return:

  1560     return ret;

  1561 }

  1562 

  1563 /*****************************************************************************/

  1564 

  1565 /** Add a Pdo to the assignment.

  1566  */

  1567 int ec_cdev_ioctl_sc_add_pdo(

  1568         ec_master_t *master, /**< EtherCAT master. */

  1569         unsigned long arg, /**< ioctl() argument. */

  1570         ec_cdev_priv_t *priv /**< Private data structure of file handle. */

  1571         )

  1572 {

  1573     ec_ioctl_config_pdo_t data;

  1574     ec_slave_config_t *sc;

  1575 

  1576 	if (unlikely(!priv->requested))

  1577         return -EPERM;

  1578 

  1579     if (copy_from_user(&data, (void __user *) arg, sizeof(data)))

  1580         return -EFAULT;

  1581 

  1582     if (down_interruptible(&master->master_sem))

  1583         return -EINTR;

  1584 

  1585     if (!(sc = ec_master_get_config(master, data.config_index))) {

  1586         up(&master->master_sem);

  1587         return -ESRCH;

  1588     }

  1589 

  1590     up(&master->master_sem); // FIXME

  1591 

  1592     return ecrt_slave_config_pdo_assign_add(sc, data.sync_index, data.index);

  1593 }

  1594 

  1595 /*****************************************************************************/

  1596 

  1597 /** Clears the Pdo assignment.

  1598  */

  1599 int ec_cdev_ioctl_sc_clear_pdos(

  1600         ec_master_t *master, /**< EtherCAT master. */

  1601         unsigned long arg, /**< ioctl() argument. */

  1602         ec_cdev_priv_t *priv /**< Private data structure of file handle. */

  1603         )

  1604 {

  1605     ec_ioctl_config_pdo_t data;

  1606     ec_slave_config_t *sc;

  1607 

  1608 	if (unlikely(!priv->requested))

  1609         return -EPERM;

  1610 

  1611     if (copy_from_user(&data, (void __user *) arg, sizeof(data)))

  1612         return -EFAULT;

  1613 

  1614     if (down_interruptible(&master->master_sem))

  1615         return -EINTR;

  1616 

  1617     if (!(sc = ec_master_get_config(master, data.config_index))) {

  1618         up(&master->master_sem);

  1619         return -ESRCH;

  1620     }

  1621 

  1622     up(&master->master_sem); // FIXME

  1623 

  1624     ecrt_slave_config_pdo_assign_clear(sc, data.sync_index);

  1625     return 0;

  1626 }

  1627 

  1628 /*****************************************************************************/

  1629 

  1630 /** Add an entry to a Pdo's mapping.

  1631  */

  1632 int ec_cdev_ioctl_sc_add_entry(

  1633         ec_master_t *master, /**< EtherCAT master. */

  1634         unsigned long arg, /**< ioctl() argument. */

  1635         ec_cdev_priv_t *priv /**< Private data structure of file handle. */

  1636         )

  1637 {

  1638     ec_ioctl_add_pdo_entry_t data;

  1639     ec_slave_config_t *sc;

  1640 

  1641 	if (unlikely(!priv->requested))

  1642         return -EPERM;

  1643 

  1644     if (copy_from_user(&data, (void __user *) arg, sizeof(data)))

  1645         return -EFAULT;

  1646 

  1647     if (down_interruptible(&master->master_sem))

  1648         return -EINTR;

  1649 

  1650     if (!(sc = ec_master_get_config(master, data.config_index))) {

  1651         up(&master->master_sem);

  1652         return -ESRCH;

  1653     }

  1654 

  1655     up(&master->master_sem); // FIXME

  1656 

  1657     return ecrt_slave_config_pdo_mapping_add(sc, data.pdo_index,

  1658             data.entry_index, data.entry_subindex, data.entry_bit_length);

  1659 }

  1660 

  1661 /*****************************************************************************/

  1662 

  1663 /** Clears the mapping of a Pdo.

  1664  */

  1665 int ec_cdev_ioctl_sc_clear_entries(

  1666         ec_master_t *master, /**< EtherCAT master. */

  1667         unsigned long arg, /**< ioctl() argument. */

  1668         ec_cdev_priv_t *priv /**< Private data structure of file handle. */

  1669         )

  1670 {

  1671     ec_ioctl_config_pdo_t data;

  1672     ec_slave_config_t *sc;

  1673 

  1674 	if (unlikely(!priv->requested))

  1675         return -EPERM;

  1676 

  1677     if (copy_from_user(&data, (void __user *) arg, sizeof(data)))

  1678         return -EFAULT;

  1679 

  1680     if (down_interruptible(&master->master_sem))

  1681         return -EINTR;

  1682 

  1683     if (!(sc = ec_master_get_config(master, data.config_index))) {

  1684         up(&master->master_sem);

  1685         return -ESRCH;

  1686     }

  1687 

  1688     up(&master->master_sem); // FIXME

  1689 

  1690     ecrt_slave_config_pdo_mapping_clear(sc, data.index);

  1691     return 0;

  1692 }

  1693 

  1694 /*****************************************************************************/

  1695 

  1696 /** Registers a Pdo entry.

  1697  */

  1698 int ec_cdev_ioctl_sc_reg_pdo_entry(

  1699         ec_master_t *master, /**< EtherCAT master. */

  1700         unsigned long arg, /**< ioctl() argument. */

  1701         ec_cdev_priv_t *priv /**< Private data structure of file handle. */

  1702         )

  1703 {

  1704     ec_ioctl_reg_pdo_entry_t data;

  1705     ec_slave_config_t *sc;

  1706     ec_domain_t *domain;

  1707     int ret;

  1708 

  1709 	if (unlikely(!priv->requested))

  1710         return -EPERM;

  1711 

  1712     if (copy_from_user(&data, (void __user *) arg, sizeof(data)))

  1713         return -EFAULT;

  1714 

  1715     if (down_interruptible(&master->master_sem))

  1716         return -EINTR;

  1717 

  1718     if (!(sc = ec_master_get_config(master, data.config_index))) {

  1719         up(&master->master_sem);

  1720         return -ESRCH;

  1721     }

  1722 

  1723     if (!(domain = ec_master_find_domain(master, data.domain_index))) {

  1724         up(&master->master_sem);

  1725         return -ESRCH;

  1726     }

  1727 

  1728     up(&master->master_sem); // FIXME

  1729 

  1730     ret = ecrt_slave_config_reg_pdo_entry(sc, data.entry_index,

  1731             data.entry_subindex, domain, &data.bit_position);

  1732 

  1733     if (copy_to_user((void __user *) arg, &data, sizeof(data)))

  1734         return -EFAULT;

  1735 

  1736     return ret;

  1737 }

  1738 

  1739 /*****************************************************************************/

  1740 

  1741 /** Configures an Sdo.

  1742  */

  1743 int ec_cdev_ioctl_sc_sdo(

  1744         ec_master_t *master, /**< EtherCAT master. */

  1745         unsigned long arg, /**< ioctl() argument. */

  1746         ec_cdev_priv_t *priv /**< Private data structure of file handle. */

  1747         )

  1748 {

  1749     ec_ioctl_sc_sdo_t data;

  1750     ec_slave_config_t *sc;

  1751     uint8_t *sdo_data = NULL;

  1752     int ret;

  1753 

  1754 	if (unlikely(!priv->requested))

  1755         return -EPERM;

  1756 

  1757     if (copy_from_user(&data, (void __user *) arg, sizeof(data)))

  1758         return -EFAULT;

  1759 

  1760     if (!data.size)

  1761         return -EINVAL;

  1762 

  1763     if (!(sdo_data = kmalloc(data.size, GFP_KERNEL))) {

  1764         return -ENOMEM;

  1765     }

  1766 

  1767     if (copy_from_user(sdo_data, (void __user *) data.data, data.size)) {

  1768         kfree(sdo_data);

  1769         return -EFAULT;

  1770     }

  1771 

  1772     if (down_interruptible(&master->master_sem)) {

  1773         kfree(sdo_data);

  1774         return -EINTR;

  1775     }

  1776 

  1777     if (!(sc = ec_master_get_config(master, data.config_index))) {

  1778         up(&master->master_sem);

  1779         kfree(sdo_data);

  1780         return -ESRCH;

  1781     }

  1782 

  1783     up(&master->master_sem); // FIXME

  1784 

  1785     ret = ecrt_slave_config_sdo(sc, data.index, data.subindex, sdo_data,

  1786             data.size);

  1787     kfree(sdo_data);

  1788     return ret;

  1928         case EC_IOCTL_SC_SYNC:

  1929             if (!(filp->f_mode & FMODE_WRITE))

  1930 				return -EPERM;

  1931 			return ec_cdev_ioctl_sc_sync(master, arg, priv);

  1932         case EC_IOCTL_SC_ADD_PDO:

  1933             if (!(filp->f_mode & FMODE_WRITE))

  1934 				return -EPERM;

  1935 			return ec_cdev_ioctl_sc_add_pdo(master, arg, priv);

  1936         case EC_IOCTL_SC_CLEAR_PDOS:

  1937             if (!(filp->f_mode & FMODE_WRITE))

  1938 				return -EPERM;

  1939 			return ec_cdev_ioctl_sc_clear_pdos(master, arg, priv);

  1940         case EC_IOCTL_SC_ADD_ENTRY:

  1941             if (!(filp->f_mode & FMODE_WRITE))

  1942 				return -EPERM;

  1943 			return ec_cdev_ioctl_sc_add_entry(master, arg, priv);

  1944         case EC_IOCTL_SC_CLEAR_ENTRIES:

  1945             if (!(filp->f_mode & FMODE_WRITE))

  1946 				return -EPERM;

  1947 			return ec_cdev_ioctl_sc_clear_entries(master, arg, priv);

  1948         case EC_IOCTL_SC_REG_PDO_ENTRY:

  1949             if (!(filp->f_mode & FMODE_WRITE))

  1950 				return -EPERM;

  1951 			return ec_cdev_ioctl_sc_reg_pdo_entry(master, arg, priv);

  1952         case EC_IOCTL_SC_SDO:

  1953             if (!(filp->f_mode & FMODE_WRITE))

  1954 				return -EPERM;

  1955 			return ec_cdev_ioctl_sc_sdo(master, arg, priv);