119 master->main_mac = main_mac; |
119 master->main_mac = main_mac; |
120 master->backup_mac = backup_mac; |
120 master->backup_mac = backup_mac; |
121 init_MUTEX(&master->device_sem); |
121 init_MUTEX(&master->device_sem); |
122 |
122 |
123 master->phase = EC_ORPHANED; |
123 master->phase = EC_ORPHANED; |
|
124 master->active = 0; |
124 master->injection_seq_fsm = 0; |
125 master->injection_seq_fsm = 0; |
125 master->injection_seq_rt = 0; |
126 master->injection_seq_rt = 0; |
126 |
127 |
127 master->slaves = NULL; |
128 master->slaves = NULL; |
128 master->slave_count = 0; |
129 master->slave_count = 0; |
224 if (ret < 0) { |
225 if (ret < 0) { |
225 ec_datagram_clear(&master->sync_datagram); |
226 ec_datagram_clear(&master->sync_datagram); |
226 EC_ERR("Failed to allocate synchronisation datagram.\n"); |
227 EC_ERR("Failed to allocate synchronisation datagram.\n"); |
227 goto out_clear_ref_sync; |
228 goto out_clear_ref_sync; |
228 } |
229 } |
|
230 |
|
231 // init sync monitor datagram |
|
232 ec_datagram_init(&master->sync_mon_datagram); |
|
233 snprintf(master->sync_mon_datagram.name, EC_DATAGRAM_NAME_SIZE, "syncmon"); |
|
234 ret = ec_datagram_brd(&master->sync_mon_datagram, 0x092c, 4); |
|
235 if (ret < 0) { |
|
236 ec_datagram_clear(&master->sync_mon_datagram); |
|
237 EC_ERR("Failed to allocate sync monitoring datagram.\n"); |
|
238 goto out_clear_sync; |
|
239 } |
|
240 |
229 ec_master_find_dc_ref_clock(master); |
241 ec_master_find_dc_ref_clock(master); |
230 |
242 |
231 // init character device |
243 // init character device |
232 ret = ec_cdev_init(&master->cdev, master, device_number); |
244 ret = ec_cdev_init(&master->cdev, master, device_number); |
233 if (ret) |
245 if (ret) |
234 goto out_clear_sync; |
246 goto out_clear_sync_mon; |
235 |
247 |
236 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27) |
248 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27) |
237 master->class_device = device_create(class, NULL, |
249 master->class_device = device_create(class, NULL, |
238 MKDEV(MAJOR(device_number), master->index), NULL, |
250 MKDEV(MAJOR(device_number), master->index), NULL, |
239 "EtherCAT%u", master->index); |
251 "EtherCAT%u", master->index); |
296 #endif |
310 #endif |
297 ec_master_clear_domains(master); |
311 ec_master_clear_domains(master); |
298 ec_master_clear_slave_configs(master); |
312 ec_master_clear_slave_configs(master); |
299 ec_master_clear_slaves(master); |
313 ec_master_clear_slaves(master); |
300 |
314 |
|
315 ec_datagram_clear(&master->sync_mon_datagram); |
301 ec_datagram_clear(&master->sync_datagram); |
316 ec_datagram_clear(&master->sync_datagram); |
302 ec_datagram_clear(&master->ref_sync_datagram); |
317 ec_datagram_clear(&master->ref_sync_datagram); |
303 ec_fsm_master_clear(&master->fsm); |
318 ec_fsm_master_clear(&master->fsm); |
304 ec_datagram_clear(&master->fsm_datagram); |
319 ec_datagram_clear(&master->fsm_datagram); |
305 ec_device_clear(&master->backup_device); |
320 ec_device_clear(&master->backup_device); |
347 void ec_master_clear_slaves(ec_master_t *master) |
362 void ec_master_clear_slaves(ec_master_t *master) |
348 { |
363 { |
349 ec_slave_t *slave; |
364 ec_slave_t *slave; |
350 |
365 |
351 master->dc_ref_clock = NULL; |
366 master->dc_ref_clock = NULL; |
|
367 |
|
368 // external requests are obsolete, so we wake pending waiters and remove |
|
369 // them from the list |
|
370 // |
|
371 // SII requests |
|
372 while (1) { |
|
373 ec_sii_write_request_t *request; |
|
374 if (list_empty(&master->sii_requests)) |
|
375 break; |
|
376 // get first request |
|
377 request = list_entry(master->sii_requests.next, |
|
378 ec_sii_write_request_t, list); |
|
379 list_del_init(&request->list); // dequeue |
|
380 EC_INFO("Discarding SII request, slave %u does not exist anymore.\n", |
|
381 request->slave->ring_position); |
|
382 request->state = EC_INT_REQUEST_FAILURE; |
|
383 wake_up(&master->sii_queue); |
|
384 } |
|
385 |
|
386 // Register requests |
|
387 while (1) { |
|
388 ec_reg_request_t *request; |
|
389 if (list_empty(&master->reg_requests)) |
|
390 break; |
|
391 // get first request |
|
392 request = list_entry(master->reg_requests.next, |
|
393 ec_reg_request_t, list); |
|
394 list_del_init(&request->list); // dequeue |
|
395 EC_INFO("Discarding Reg request, slave %u does not exist anymore.\n", |
|
396 request->slave->ring_position); |
|
397 request->state = EC_INT_REQUEST_FAILURE; |
|
398 wake_up(&master->reg_queue); |
|
399 } |
|
400 |
|
401 // SDO requests |
|
402 while (1) { |
|
403 ec_master_sdo_request_t *request; |
|
404 if (list_empty(&master->slave_sdo_requests)) |
|
405 break; |
|
406 // get first request |
|
407 request = list_entry(master->slave_sdo_requests.next, |
|
408 ec_master_sdo_request_t, list); |
|
409 list_del_init(&request->list); // dequeue |
|
410 EC_INFO("Discarding SDO request, slave %u does not exist anymore.\n", |
|
411 request->slave->ring_position); |
|
412 request->req.state = EC_INT_REQUEST_FAILURE; |
|
413 wake_up(&master->sdo_queue); |
|
414 } |
|
415 |
|
416 // FoE requests |
|
417 while (1) { |
|
418 ec_master_foe_request_t *request; |
|
419 if (list_empty(&master->foe_requests)) |
|
420 break; |
|
421 // get first request |
|
422 request = list_entry(master->foe_requests.next, |
|
423 ec_master_foe_request_t, list); |
|
424 list_del_init(&request->list); // dequeue |
|
425 EC_INFO("Discarding FOE request, slave %u does not exist anymore.\n", |
|
426 request->slave->ring_position); |
|
427 request->req.state = EC_INT_REQUEST_FAILURE; |
|
428 wake_up(&master->foe_queue); |
|
429 } |
352 |
430 |
353 for (slave = master->slaves; |
431 for (slave = master->slaves; |
354 slave < master->slaves + master->slave_count; |
432 slave < master->slaves + master->slave_count; |
355 slave++) { |
433 slave++) { |
356 ec_slave_clear(slave); |
434 ec_slave_clear(slave); |
590 |
668 |
591 /*****************************************************************************/ |
669 /*****************************************************************************/ |
592 |
670 |
593 /** Transition function from OPERATION to IDLE phase. |
671 /** Transition function from OPERATION to IDLE phase. |
594 */ |
672 */ |
595 void ec_master_leave_operation_phase(ec_master_t *master |
673 void ec_master_leave_operation_phase( |
596 /**< EtherCAT master */) |
674 ec_master_t *master /**< EtherCAT master */ |
597 { |
675 ) |
598 ec_slave_t *slave; |
676 { |
599 #ifdef EC_EOE |
677 if (master->active) |
600 ec_eoe_t *eoe; |
678 ecrt_master_deactivate(master); |
601 #endif |
|
602 |
679 |
603 if (master->debug_level) |
680 if (master->debug_level) |
604 EC_DBG("OPERATION -> IDLE.\n"); |
681 EC_DBG("OPERATION -> IDLE.\n"); |
605 |
682 |
606 master->phase = EC_IDLE; |
683 master->phase = EC_IDLE; |
607 |
|
608 #ifdef EC_EOE |
|
609 ec_master_eoe_stop(master); |
|
610 #endif |
|
611 ec_master_thread_stop(master); |
|
612 |
|
613 master->send_cb = ec_master_internal_send_cb; |
|
614 master->receive_cb = ec_master_internal_receive_cb; |
|
615 master->cb_data = master; |
|
616 |
|
617 down(&master->master_sem); |
|
618 ec_master_clear_domains(master); |
|
619 ec_master_clear_slave_configs(master); |
|
620 up(&master->master_sem); |
|
621 |
|
622 for (slave = master->slaves; |
|
623 slave < master->slaves + master->slave_count; |
|
624 slave++) { |
|
625 |
|
626 // set states for all slaves |
|
627 ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP); |
|
628 |
|
629 // mark for reconfiguration, because the master could have no |
|
630 // possibility for a reconfiguration between two sequential operation |
|
631 // phases. |
|
632 slave->force_config = 1; |
|
633 } |
|
634 |
|
635 #ifdef EC_EOE |
|
636 // ... but leave EoE slaves in OP |
|
637 list_for_each_entry(eoe, &master->eoe_handlers, list) { |
|
638 if (ec_eoe_is_open(eoe)) |
|
639 ec_slave_request_state(eoe->slave, EC_SLAVE_STATE_OP); |
|
640 } |
|
641 #endif |
|
642 |
|
643 master->app_time = 0ULL; |
|
644 master->app_start_time = 0ULL; |
|
645 master->has_start_time = 0; |
|
646 |
|
647 if (ec_master_thread_start(master, ec_master_idle_thread, |
|
648 "EtherCAT-IDLE")) |
|
649 EC_WARN("Failed to restart master thread!\n"); |
|
650 #ifdef EC_EOE |
|
651 ec_master_eoe_start(master); |
|
652 #endif |
|
653 |
|
654 master->allow_scan = 1; |
|
655 master->allow_config = 1; |
|
656 } |
684 } |
657 |
685 |
658 /*****************************************************************************/ |
686 /*****************************************************************************/ |
659 |
687 |
660 /** Places a datagram in the datagram queue. |
688 /** Places a datagram in the datagram queue. |
1633 int ret; |
1661 int ret; |
1634 |
1662 |
1635 if (master->debug_level) |
1663 if (master->debug_level) |
1636 EC_DBG("ecrt_master_activate(master = 0x%p)\n", master); |
1664 EC_DBG("ecrt_master_activate(master = 0x%p)\n", master); |
1637 |
1665 |
|
1666 if (master->active) { |
|
1667 EC_WARN("%s: Master already active!\n", __func__); |
|
1668 return 0; |
|
1669 } |
|
1670 |
1638 down(&master->master_sem); |
1671 down(&master->master_sem); |
1639 |
1672 |
1640 // finish all domains |
1673 // finish all domains |
1641 domain_offset = 0; |
1674 domain_offset = 0; |
1642 list_for_each_entry(domain, &master->domains, list) { |
1675 list_for_each_entry(domain, &master->domains, list) { |
1682 ec_master_eoe_start(master); |
1715 ec_master_eoe_start(master); |
1683 #endif |
1716 #endif |
1684 |
1717 |
1685 master->allow_config = 1; // request the current configuration |
1718 master->allow_config = 1; // request the current configuration |
1686 master->allow_scan = 1; // allow re-scanning on topology change |
1719 master->allow_scan = 1; // allow re-scanning on topology change |
|
1720 master->active = 1; |
1687 return 0; |
1721 return 0; |
|
1722 } |
|
1723 |
|
1724 /*****************************************************************************/ |
|
1725 |
|
1726 void ecrt_master_deactivate(ec_master_t *master) |
|
1727 { |
|
1728 ec_slave_t *slave; |
|
1729 #ifdef EC_EOE |
|
1730 ec_eoe_t *eoe; |
|
1731 #endif |
|
1732 |
|
1733 if (master->debug_level) |
|
1734 EC_DBG("ecrt_master_deactivate(master = 0x%x)\n", (u32) master); |
|
1735 |
|
1736 if (!master->active) { |
|
1737 EC_WARN("%s: Master not active.\n", __func__); |
|
1738 return; |
|
1739 } |
|
1740 |
|
1741 #ifdef EC_EOE |
|
1742 ec_master_eoe_stop(master); |
|
1743 #endif |
|
1744 ec_master_thread_stop(master); |
|
1745 |
|
1746 master->send_cb = ec_master_internal_send_cb; |
|
1747 master->receive_cb = ec_master_internal_receive_cb; |
|
1748 master->cb_data = master; |
|
1749 |
|
1750 down(&master->master_sem); |
|
1751 ec_master_clear_domains(master); |
|
1752 ec_master_clear_slave_configs(master); |
|
1753 up(&master->master_sem); |
|
1754 |
|
1755 for (slave = master->slaves; |
|
1756 slave < master->slaves + master->slave_count; |
|
1757 slave++) { |
|
1758 |
|
1759 // set states for all slaves |
|
1760 ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP); |
|
1761 |
|
1762 // mark for reconfiguration, because the master could have no |
|
1763 // possibility for a reconfiguration between two sequential operation |
|
1764 // phases. |
|
1765 slave->force_config = 1; |
|
1766 } |
|
1767 |
|
1768 #ifdef EC_EOE |
|
1769 // ... but leave EoE slaves in OP |
|
1770 list_for_each_entry(eoe, &master->eoe_handlers, list) { |
|
1771 if (ec_eoe_is_open(eoe)) |
|
1772 ec_slave_request_state(eoe->slave, EC_SLAVE_STATE_OP); |
|
1773 } |
|
1774 #endif |
|
1775 |
|
1776 master->app_time = 0ULL; |
|
1777 master->app_start_time = 0ULL; |
|
1778 master->has_start_time = 0; |
|
1779 |
|
1780 if (ec_master_thread_start(master, ec_master_idle_thread, |
|
1781 "EtherCAT-IDLE")) |
|
1782 EC_WARN("Failed to restart master thread!\n"); |
|
1783 #ifdef EC_EOE |
|
1784 ec_master_eoe_start(master); |
|
1785 #endif |
|
1786 |
|
1787 master->allow_scan = 1; |
|
1788 master->allow_config = 1; |
|
1789 master->active = 0; |
1688 } |
1790 } |
1689 |
1791 |
1690 /*****************************************************************************/ |
1792 /*****************************************************************************/ |
1691 |
1793 |
1692 void ecrt_master_send(ec_master_t *master) |
1794 void ecrt_master_send(ec_master_t *master) |
1897 ec_master_queue_datagram(master, &master->sync_datagram); |
1999 ec_master_queue_datagram(master, &master->sync_datagram); |
1898 } |
2000 } |
1899 |
2001 |
1900 /*****************************************************************************/ |
2002 /*****************************************************************************/ |
1901 |
2003 |
|
2004 void ecrt_master_sync_monitor_queue(ec_master_t *master) |
|
2005 { |
|
2006 ec_datagram_zero(&master->sync_mon_datagram); |
|
2007 ec_master_queue_datagram(master, &master->sync_mon_datagram); |
|
2008 } |
|
2009 |
|
2010 /*****************************************************************************/ |
|
2011 |
|
2012 uint32_t ecrt_master_sync_monitor_process(ec_master_t *master) |
|
2013 { |
|
2014 if (master->sync_mon_datagram.state == EC_DATAGRAM_RECEIVED) { |
|
2015 return EC_READ_U32(master->sync_mon_datagram.data) & 0x7fffffff; |
|
2016 } else { |
|
2017 return 0xffffffff; |
|
2018 } |
|
2019 } |
|
2020 |
|
2021 /*****************************************************************************/ |
|
2022 |
1902 /** \cond */ |
2023 /** \cond */ |
1903 |
2024 |
1904 EXPORT_SYMBOL(ecrt_master_create_domain); |
2025 EXPORT_SYMBOL(ecrt_master_create_domain); |
1905 EXPORT_SYMBOL(ecrt_master_activate); |
2026 EXPORT_SYMBOL(ecrt_master_activate); |
|
2027 EXPORT_SYMBOL(ecrt_master_deactivate); |
1906 EXPORT_SYMBOL(ecrt_master_send); |
2028 EXPORT_SYMBOL(ecrt_master_send); |
1907 EXPORT_SYMBOL(ecrt_master_send_ext); |
2029 EXPORT_SYMBOL(ecrt_master_send_ext); |
1908 EXPORT_SYMBOL(ecrt_master_receive); |
2030 EXPORT_SYMBOL(ecrt_master_receive); |
1909 EXPORT_SYMBOL(ecrt_master_callbacks); |
2031 EXPORT_SYMBOL(ecrt_master_callbacks); |
1910 EXPORT_SYMBOL(ecrt_master_slave_config); |
2032 EXPORT_SYMBOL(ecrt_master_slave_config); |
1911 EXPORT_SYMBOL(ecrt_master_state); |
2033 EXPORT_SYMBOL(ecrt_master_state); |
1912 EXPORT_SYMBOL(ecrt_master_application_time); |
2034 EXPORT_SYMBOL(ecrt_master_application_time); |
1913 EXPORT_SYMBOL(ecrt_master_sync_reference_clock); |
2035 EXPORT_SYMBOL(ecrt_master_sync_reference_clock); |
1914 EXPORT_SYMBOL(ecrt_master_sync_slave_clocks); |
2036 EXPORT_SYMBOL(ecrt_master_sync_slave_clocks); |
|
2037 EXPORT_SYMBOL(ecrt_master_sync_monitor_queue); |
|
2038 EXPORT_SYMBOL(ecrt_master_sync_monitor_process); |
1915 |
2039 |
1916 /** \endcond */ |
2040 /** \endcond */ |
1917 |
2041 |
1918 /*****************************************************************************/ |
2042 /*****************************************************************************/ |