1382 cycles_end = get_cycles(); |
1382 cycles_end = get_cycles(); |
1383 if (cycles_end - cycles_start >= cycles_timeout) break; |
1383 if (cycles_end - cycles_start >= cycles_timeout) break; |
1384 } |
1384 } |
1385 } |
1385 } |
1386 |
1386 |
1387 /****************************************************************************** |
1387 /*****************************************************************************/ |
1388 * Realtime interface |
1388 |
1389 *****************************************************************************/ |
1389 /** |
1390 |
1390 * Translates an ASCII coded bus-address to a slave pointer. |
1391 /** |
1391 * These are the valid addressing schemes: |
1392 Creates a domain. |
1392 * - \a "X" = the X. slave on the bus (ring position), |
1393 \return pointer to new domain on success, else NULL |
1393 * - \a "X:Y" = the Y. slave after the X. branch (bus coupler), |
1394 \ingroup RealtimeInterface |
1394 * - \a "#X" = the slave with alias X, |
1395 */ |
1395 * - \a "#X:Y" = the Y. slave after the branch (bus coupler) with alias X. |
1396 |
1396 * X and Y are zero-based indices and may be provided in hexadecimal or octal |
1397 ec_domain_t *ecrt_master_create_domain(ec_master_t *master /**< master */) |
1397 * notation (with respective prefix). |
1398 { |
1398 * \return pointer to the slave on success, else NULL |
1399 ec_domain_t *domain, *last_domain; |
1399 */ |
1400 unsigned int index; |
1400 |
1401 |
1401 ec_slave_t *ec_master_parse_slave_address( |
1402 if (!(domain = (ec_domain_t *) kmalloc(sizeof(ec_domain_t), GFP_KERNEL))) { |
1402 const ec_master_t *master, /**< EtherCAT master */ |
1403 EC_ERR("Error allocating domain memory!\n"); |
1403 const char *address /**< address string */ |
1404 return NULL; |
1404 ) |
1405 } |
|
1406 |
|
1407 if (list_empty(&master->domains)) index = 0; |
|
1408 else { |
|
1409 last_domain = list_entry(master->domains.prev, ec_domain_t, list); |
|
1410 index = last_domain->index + 1; |
|
1411 } |
|
1412 |
|
1413 if (ec_domain_init(domain, master, index)) { |
|
1414 EC_ERR("Failed to init domain.\n"); |
|
1415 return NULL; |
|
1416 } |
|
1417 |
|
1418 list_add_tail(&domain->list, &master->domains); |
|
1419 |
|
1420 return domain; |
|
1421 } |
|
1422 |
|
1423 /*****************************************************************************/ |
|
1424 |
|
1425 /** |
|
1426 Configures all slaves and leads them to the OP state. |
|
1427 Does the complete configuration and activation for all slaves. Sets sync |
|
1428 managers and FMMUs, and does the appropriate transitions, until the slave |
|
1429 is operational. |
|
1430 \return 0 in case of success, else < 0 |
|
1431 \ingroup RealtimeInterface |
|
1432 */ |
|
1433 |
|
1434 int ecrt_master_activate(ec_master_t *master /**< EtherCAT master */) |
|
1435 { |
|
1436 uint32_t domain_offset; |
|
1437 ec_domain_t *domain; |
|
1438 ec_fsm_slave_t fsm_slave; |
|
1439 ec_slave_t *slave; |
|
1440 |
|
1441 // allocate all domains |
|
1442 domain_offset = 0; |
|
1443 list_for_each_entry(domain, &master->domains, list) { |
|
1444 if (ec_domain_alloc(domain, domain_offset)) { |
|
1445 EC_ERR("Failed to allocate domain %X!\n", (u32) domain); |
|
1446 return -1; |
|
1447 } |
|
1448 domain_offset += domain->data_size; |
|
1449 } |
|
1450 |
|
1451 ec_fsm_slave_init(&fsm_slave, &master->fsm_datagram); |
|
1452 |
|
1453 // configure all slaves |
|
1454 list_for_each_entry(slave, &master->slaves, list) { |
|
1455 ec_fsm_slave_start_conf(&fsm_slave, slave); |
|
1456 while (ec_fsm_slave_exec(&fsm_slave)) { |
|
1457 ec_master_queue_datagram(master, &master->fsm_datagram); |
|
1458 ec_master_sync_io(master); |
|
1459 } |
|
1460 |
|
1461 if (!ec_fsm_slave_success(&fsm_slave)) { |
|
1462 ec_fsm_slave_clear(&fsm_slave); |
|
1463 EC_ERR("Failed to configure slave %i!\n", slave->ring_position); |
|
1464 return -1; |
|
1465 } |
|
1466 } |
|
1467 |
|
1468 ec_fsm_slave_clear(&fsm_slave); |
|
1469 ec_master_prepare(master); // prepare asynchronous IO |
|
1470 |
|
1471 if (master->debug_level) |
|
1472 EC_DBG("FSM datagram is %x.\n", (unsigned int) &master->fsm_datagram); |
|
1473 |
|
1474 if (ec_master_thread_start(master)) { |
|
1475 EC_ERR("Failed to start master thread!\n"); |
|
1476 return -1; |
|
1477 } |
|
1478 |
|
1479 return 0; |
|
1480 } |
|
1481 |
|
1482 /*****************************************************************************/ |
|
1483 |
|
1484 /** |
|
1485 Sends queued datagrams and waits for their reception. |
|
1486 */ |
|
1487 |
|
1488 void ec_master_sync_io(ec_master_t *master /**< EtherCAT master */) |
|
1489 { |
|
1490 ec_datagram_t *datagram; |
|
1491 unsigned int datagrams_sent; |
|
1492 |
|
1493 // send all datagrams |
|
1494 ecrt_master_send(master); |
|
1495 |
|
1496 while (1) { // active waiting |
|
1497 schedule(); // schedule other processes while waiting. |
|
1498 ecrt_master_receive(master); // receive and dequeue datagrams |
|
1499 |
|
1500 // count number of datagrams still waiting for response |
|
1501 datagrams_sent = 0; |
|
1502 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
|
1503 // there may be another process that queued commands |
|
1504 // in the meantime. |
|
1505 if (datagram->state == EC_DATAGRAM_QUEUED) continue; |
|
1506 datagrams_sent++; |
|
1507 } |
|
1508 |
|
1509 // abort loop if there are no more datagrams marked as sent. |
|
1510 if (!datagrams_sent) break; |
|
1511 } |
|
1512 } |
|
1513 |
|
1514 /*****************************************************************************/ |
|
1515 |
|
1516 /** |
|
1517 Asynchronous sending of datagrams. |
|
1518 \ingroup RealtimeInterface |
|
1519 */ |
|
1520 |
|
1521 void ecrt_master_send(ec_master_t *master /**< EtherCAT master */) |
|
1522 { |
|
1523 ec_datagram_t *datagram, *n; |
|
1524 |
|
1525 if (master->injection_seq_rt != master->injection_seq_fsm) { |
|
1526 // inject datagram produced by master FSM |
|
1527 ec_master_queue_datagram(master, &master->fsm_datagram); |
|
1528 master->injection_seq_rt = master->injection_seq_fsm; |
|
1529 } |
|
1530 |
|
1531 if (unlikely(!master->main_device.link_state)) { |
|
1532 // link is down, no datagram can be sent |
|
1533 list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) { |
|
1534 datagram->state = EC_DATAGRAM_ERROR; |
|
1535 list_del_init(&datagram->queue); |
|
1536 } |
|
1537 |
|
1538 // query link state |
|
1539 ec_device_poll(&master->main_device); |
|
1540 return; |
|
1541 } |
|
1542 |
|
1543 // send frames |
|
1544 ec_master_send_datagrams(master); |
|
1545 } |
|
1546 |
|
1547 /*****************************************************************************/ |
|
1548 |
|
1549 /** |
|
1550 Asynchronous receiving of datagrams. |
|
1551 \ingroup RealtimeInterface |
|
1552 */ |
|
1553 |
|
1554 void ecrt_master_receive(ec_master_t *master /**< EtherCAT master */) |
|
1555 { |
|
1556 ec_datagram_t *datagram, *next; |
|
1557 cycles_t cycles_timeout; |
|
1558 |
|
1559 // receive datagrams |
|
1560 ec_device_poll(&master->main_device); |
|
1561 |
|
1562 cycles_timeout = (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000); |
|
1563 |
|
1564 // dequeue all datagrams that timed out |
|
1565 list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue) { |
|
1566 if (datagram->state != EC_DATAGRAM_SENT) continue; |
|
1567 |
|
1568 if (master->main_device.cycles_poll - datagram->cycles_sent |
|
1569 > cycles_timeout) { |
|
1570 list_del_init(&datagram->queue); |
|
1571 datagram->state = EC_DATAGRAM_TIMED_OUT; |
|
1572 master->stats.timeouts++; |
|
1573 ec_master_output_stats(master); |
|
1574 } |
|
1575 } |
|
1576 } |
|
1577 |
|
1578 /*****************************************************************************/ |
|
1579 |
|
1580 /** |
|
1581 Translates an ASCII coded bus-address to a slave pointer. |
|
1582 These are the valid addressing schemes: |
|
1583 - \a "X" = the X. slave on the bus, |
|
1584 - \a "X:Y" = the Y. slave after the X. branch (bus coupler), |
|
1585 - \a "#X" = the slave with alias X, |
|
1586 - \a "#X:Y" = the Y. slave after the branch (bus coupler) with alias X. |
|
1587 X and Y are zero-based indices and may be provided in hexadecimal or octal |
|
1588 notation (with respective prefix). |
|
1589 \return pointer to the slave on success, else NULL |
|
1590 \ingroup RealtimeInterface |
|
1591 */ |
|
1592 |
|
1593 ec_slave_t *ecrt_master_get_slave(const ec_master_t *master, /**< Master */ |
|
1594 const char *address /**< address string */ |
|
1595 ) |
|
1596 { |
1405 { |
1597 unsigned long first, second; |
1406 unsigned long first, second; |
1598 char *remainder, *remainder2; |
1407 char *remainder, *remainder2; |
1599 const char *original; |
1408 const char *original; |
1600 unsigned int alias_requested, alias_found; |
1409 unsigned int alias_requested, alias_found; |
1683 EC_ERR("Slave address \"%s\" - Invalid format!\n", original); |
1492 EC_ERR("Slave address \"%s\" - Invalid format!\n", original); |
1684 |
1493 |
1685 return NULL; |
1494 return NULL; |
1686 } |
1495 } |
1687 |
1496 |
|
1497 |
|
1498 /****************************************************************************** |
|
1499 * Realtime interface |
|
1500 *****************************************************************************/ |
|
1501 |
|
1502 /** |
|
1503 Creates a domain. |
|
1504 \return pointer to new domain on success, else NULL |
|
1505 \ingroup RealtimeInterface |
|
1506 */ |
|
1507 |
|
1508 ec_domain_t *ecrt_master_create_domain(ec_master_t *master /**< master */) |
|
1509 { |
|
1510 ec_domain_t *domain, *last_domain; |
|
1511 unsigned int index; |
|
1512 |
|
1513 if (!(domain = (ec_domain_t *) kmalloc(sizeof(ec_domain_t), GFP_KERNEL))) { |
|
1514 EC_ERR("Error allocating domain memory!\n"); |
|
1515 return NULL; |
|
1516 } |
|
1517 |
|
1518 if (list_empty(&master->domains)) index = 0; |
|
1519 else { |
|
1520 last_domain = list_entry(master->domains.prev, ec_domain_t, list); |
|
1521 index = last_domain->index + 1; |
|
1522 } |
|
1523 |
|
1524 if (ec_domain_init(domain, master, index)) { |
|
1525 EC_ERR("Failed to init domain.\n"); |
|
1526 return NULL; |
|
1527 } |
|
1528 |
|
1529 list_add_tail(&domain->list, &master->domains); |
|
1530 |
|
1531 return domain; |
|
1532 } |
|
1533 |
|
1534 /*****************************************************************************/ |
|
1535 |
|
1536 /** |
|
1537 Configures all slaves and leads them to the OP state. |
|
1538 Does the complete configuration and activation for all slaves. Sets sync |
|
1539 managers and FMMUs, and does the appropriate transitions, until the slave |
|
1540 is operational. |
|
1541 \return 0 in case of success, else < 0 |
|
1542 \ingroup RealtimeInterface |
|
1543 */ |
|
1544 |
|
1545 int ecrt_master_activate(ec_master_t *master /**< EtherCAT master */) |
|
1546 { |
|
1547 uint32_t domain_offset; |
|
1548 ec_domain_t *domain; |
|
1549 ec_fsm_slave_t fsm_slave; |
|
1550 ec_slave_t *slave; |
|
1551 |
|
1552 // allocate all domains |
|
1553 domain_offset = 0; |
|
1554 list_for_each_entry(domain, &master->domains, list) { |
|
1555 if (ec_domain_alloc(domain, domain_offset)) { |
|
1556 EC_ERR("Failed to allocate domain %X!\n", (u32) domain); |
|
1557 return -1; |
|
1558 } |
|
1559 domain_offset += domain->data_size; |
|
1560 } |
|
1561 |
|
1562 ec_fsm_slave_init(&fsm_slave, &master->fsm_datagram); |
|
1563 |
|
1564 // configure all slaves |
|
1565 list_for_each_entry(slave, &master->slaves, list) { |
|
1566 ec_fsm_slave_start_conf(&fsm_slave, slave); |
|
1567 while (ec_fsm_slave_exec(&fsm_slave)) { |
|
1568 ec_master_queue_datagram(master, &master->fsm_datagram); |
|
1569 ec_master_sync_io(master); |
|
1570 } |
|
1571 |
|
1572 if (!ec_fsm_slave_success(&fsm_slave)) { |
|
1573 ec_fsm_slave_clear(&fsm_slave); |
|
1574 EC_ERR("Failed to configure slave %i!\n", slave->ring_position); |
|
1575 return -1; |
|
1576 } |
|
1577 } |
|
1578 |
|
1579 ec_fsm_slave_clear(&fsm_slave); |
|
1580 ec_master_prepare(master); // prepare asynchronous IO |
|
1581 |
|
1582 if (master->debug_level) |
|
1583 EC_DBG("FSM datagram is %x.\n", (unsigned int) &master->fsm_datagram); |
|
1584 |
|
1585 if (ec_master_thread_start(master)) { |
|
1586 EC_ERR("Failed to start master thread!\n"); |
|
1587 return -1; |
|
1588 } |
|
1589 |
|
1590 return 0; |
|
1591 } |
|
1592 |
|
1593 /*****************************************************************************/ |
|
1594 |
|
1595 /** |
|
1596 Sends queued datagrams and waits for their reception. |
|
1597 */ |
|
1598 |
|
1599 void ec_master_sync_io(ec_master_t *master /**< EtherCAT master */) |
|
1600 { |
|
1601 ec_datagram_t *datagram; |
|
1602 unsigned int datagrams_sent; |
|
1603 |
|
1604 // send all datagrams |
|
1605 ecrt_master_send(master); |
|
1606 |
|
1607 while (1) { // active waiting |
|
1608 schedule(); // schedule other processes while waiting. |
|
1609 ecrt_master_receive(master); // receive and dequeue datagrams |
|
1610 |
|
1611 // count number of datagrams still waiting for response |
|
1612 datagrams_sent = 0; |
|
1613 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
|
1614 // there may be another process that queued commands |
|
1615 // in the meantime. |
|
1616 if (datagram->state == EC_DATAGRAM_QUEUED) continue; |
|
1617 datagrams_sent++; |
|
1618 } |
|
1619 |
|
1620 // abort loop if there are no more datagrams marked as sent. |
|
1621 if (!datagrams_sent) break; |
|
1622 } |
|
1623 } |
|
1624 |
|
1625 /*****************************************************************************/ |
|
1626 |
|
1627 /** |
|
1628 Asynchronous sending of datagrams. |
|
1629 \ingroup RealtimeInterface |
|
1630 */ |
|
1631 |
|
1632 void ecrt_master_send(ec_master_t *master /**< EtherCAT master */) |
|
1633 { |
|
1634 ec_datagram_t *datagram, *n; |
|
1635 |
|
1636 if (master->injection_seq_rt != master->injection_seq_fsm) { |
|
1637 // inject datagram produced by master FSM |
|
1638 ec_master_queue_datagram(master, &master->fsm_datagram); |
|
1639 master->injection_seq_rt = master->injection_seq_fsm; |
|
1640 } |
|
1641 |
|
1642 if (unlikely(!master->main_device.link_state)) { |
|
1643 // link is down, no datagram can be sent |
|
1644 list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) { |
|
1645 datagram->state = EC_DATAGRAM_ERROR; |
|
1646 list_del_init(&datagram->queue); |
|
1647 } |
|
1648 |
|
1649 // query link state |
|
1650 ec_device_poll(&master->main_device); |
|
1651 return; |
|
1652 } |
|
1653 |
|
1654 // send frames |
|
1655 ec_master_send_datagrams(master); |
|
1656 } |
|
1657 |
|
1658 /*****************************************************************************/ |
|
1659 |
|
1660 /** |
|
1661 Asynchronous receiving of datagrams. |
|
1662 \ingroup RealtimeInterface |
|
1663 */ |
|
1664 |
|
1665 void ecrt_master_receive(ec_master_t *master /**< EtherCAT master */) |
|
1666 { |
|
1667 ec_datagram_t *datagram, *next; |
|
1668 cycles_t cycles_timeout; |
|
1669 |
|
1670 // receive datagrams |
|
1671 ec_device_poll(&master->main_device); |
|
1672 |
|
1673 cycles_timeout = (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000); |
|
1674 |
|
1675 // dequeue all datagrams that timed out |
|
1676 list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue) { |
|
1677 if (datagram->state != EC_DATAGRAM_SENT) continue; |
|
1678 |
|
1679 if (master->main_device.cycles_poll - datagram->cycles_sent |
|
1680 > cycles_timeout) { |
|
1681 list_del_init(&datagram->queue); |
|
1682 datagram->state = EC_DATAGRAM_TIMED_OUT; |
|
1683 master->stats.timeouts++; |
|
1684 ec_master_output_stats(master); |
|
1685 } |
|
1686 } |
|
1687 } |
|
1688 |
|
1689 /*****************************************************************************/ |
|
1690 |
|
1691 /** |
|
1692 * Obtains a slave pointer by its bus address. |
|
1693 * A valid slave pointer is only returned, if vendor ID and product code are |
|
1694 * matching. |
|
1695 * \return pointer to the slave on success, else NULL |
|
1696 * \ingroup RealtimeInterface |
|
1697 */ |
|
1698 |
|
1699 ec_slave_t *ecrt_master_get_slave( |
|
1700 const ec_master_t *master, /**< EtherCAT master */ |
|
1701 const char *address, /**< address string |
|
1702 \see ec_master_parse_slave_address() */ |
|
1703 uint32_t vendor_id, /**< vendor ID */ |
|
1704 uint32_t product_code /**< product code */ |
|
1705 ) |
|
1706 { |
|
1707 ec_slave_t *slave = ec_master_parse_slave_address(master, address); |
|
1708 |
|
1709 if (!slave) |
|
1710 return NULL; |
|
1711 |
|
1712 return ec_slave_validate(slave, vendor_id, product_code) ? NULL : slave; |
|
1713 } |
|
1714 |
1688 /*****************************************************************************/ |
1715 /*****************************************************************************/ |
1689 |
1716 |
1690 /** |
1717 /** |
1691 Sets the locking callbacks. |
1718 Sets the locking callbacks. |
1692 The request_cb function must return zero, to allow another instance |
1719 The request_cb function must return zero, to allow another instance |