552 int ec_master_enter_idle_phase( |
552 int ec_master_enter_idle_phase( |
553 ec_master_t *master /**< EtherCAT master */ |
553 ec_master_t *master /**< EtherCAT master */ |
554 ) |
554 ) |
555 { |
555 { |
556 int ret; |
556 int ret; |
|
557 ec_device_index_t dev_idx; |
557 |
558 |
558 EC_MASTER_DBG(master, 1, "ORPHANED -> IDLE.\n"); |
559 EC_MASTER_DBG(master, 1, "ORPHANED -> IDLE.\n"); |
559 |
560 |
560 master->send_cb = ec_master_internal_send_cb; |
561 master->send_cb = ec_master_internal_send_cb; |
561 master->receive_cb = ec_master_internal_receive_cb; |
562 master->receive_cb = ec_master_internal_receive_cb; |
562 master->cb_data = master; |
563 master->cb_data = master; |
563 |
564 |
564 master->phase = EC_IDLE; |
565 master->phase = EC_IDLE; |
565 |
566 |
566 // reset number of responding slaves to trigger scanning |
567 // reset number of responding slaves to trigger scanning |
567 master->fsm.slaves_responding = 0; |
568 for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_NUM_DEVICES; dev_idx++) { |
|
569 master->fsm.slaves_responding[dev_idx] = 0; |
|
570 } |
568 |
571 |
569 ret = ec_master_thread_start(master, ec_master_idle_thread, |
572 ret = ec_master_thread_start(master, ec_master_idle_thread, |
570 "EtherCAT-IDLE"); |
573 "EtherCAT-IDLE"); |
571 if (ret) |
574 if (ret) |
572 master->phase = EC_ORPHANED; |
575 master->phase = EC_ORPHANED; |
725 #endif |
728 #endif |
726 #ifdef EC_HAVE_CYCLES |
729 #ifdef EC_HAVE_CYCLES |
727 datagram->cycles_sent = 0; |
730 datagram->cycles_sent = 0; |
728 #endif |
731 #endif |
729 datagram->jiffies_sent = 0; |
732 datagram->jiffies_sent = 0; |
730 ec_master_queue_datagram(master, datagram, |
733 ec_master_queue_datagram(master, datagram); |
731 datagram->device_index); |
|
732 } else { |
734 } else { |
733 if (datagram->data_size > master->max_queue_size) { |
735 if (datagram->data_size > master->max_queue_size) { |
734 list_del_init(&datagram->queue); |
736 list_del_init(&datagram->queue); |
735 datagram->state = EC_DATAGRAM_ERROR; |
737 datagram->state = EC_DATAGRAM_ERROR; |
736 EC_MASTER_ERR(master, "External datagram %p is too large," |
738 EC_MASTER_ERR(master, "External datagram %p is too large," |
798 |
800 |
799 /** Places an external datagram in the sdo datagram queue. |
801 /** Places an external datagram in the sdo datagram queue. |
800 */ |
802 */ |
801 void ec_master_queue_external_datagram( |
803 void ec_master_queue_external_datagram( |
802 ec_master_t *master, /**< EtherCAT master */ |
804 ec_master_t *master, /**< EtherCAT master */ |
803 ec_datagram_t *datagram, /**< datagram */ |
805 ec_datagram_t *datagram /**< datagram */ |
804 ec_device_index_t device_index /**< Device index. */ |
|
805 ) |
806 ) |
806 { |
807 { |
807 ec_datagram_t *queued_datagram; |
808 ec_datagram_t *queued_datagram; |
808 |
809 |
809 down(&master->io_sem); |
810 down(&master->io_sem); |
822 datagram, datagram->data_size); |
823 datagram, datagram->data_size); |
823 #endif |
824 #endif |
824 |
825 |
825 list_add_tail(&datagram->queue, &master->external_datagram_queue); |
826 list_add_tail(&datagram->queue, &master->external_datagram_queue); |
826 datagram->state = EC_DATAGRAM_QUEUED; |
827 datagram->state = EC_DATAGRAM_QUEUED; |
827 datagram->device_index = device_index; |
|
828 #ifdef EC_HAVE_CYCLES |
828 #ifdef EC_HAVE_CYCLES |
829 datagram->cycles_sent = get_cycles(); |
829 datagram->cycles_sent = get_cycles(); |
830 #endif |
830 #endif |
831 datagram->jiffies_sent = jiffies; |
831 datagram->jiffies_sent = jiffies; |
832 |
832 |
838 |
838 |
839 /** Places a datagram in the datagram queue. |
839 /** Places a datagram in the datagram queue. |
840 */ |
840 */ |
841 void ec_master_queue_datagram( |
841 void ec_master_queue_datagram( |
842 ec_master_t *master, /**< EtherCAT master */ |
842 ec_master_t *master, /**< EtherCAT master */ |
843 ec_datagram_t *datagram, /**< datagram */ |
843 ec_datagram_t *datagram /**< datagram */ |
844 ec_device_index_t device_index /**< Device index. */ |
|
845 ) |
844 ) |
846 { |
845 { |
847 ec_datagram_t *queued_datagram; |
846 ec_datagram_t *queued_datagram; |
848 |
847 |
849 switch (datagram->state) { |
848 switch (datagram->state) { |
879 } |
878 } |
880 } |
879 } |
881 |
880 |
882 list_add_tail(&datagram->queue, &master->datagram_queue); |
881 list_add_tail(&datagram->queue, &master->datagram_queue); |
883 datagram->state = EC_DATAGRAM_QUEUED; |
882 datagram->state = EC_DATAGRAM_QUEUED; |
884 datagram->device_index = device_index; |
|
885 } |
883 } |
886 |
884 |
887 /*****************************************************************************/ |
885 /*****************************************************************************/ |
888 |
886 |
889 /** Places a datagram in the non-application datagram queue. |
887 /** Places a datagram in the non-application datagram queue. |
1386 up(&master->master_sem); |
1384 up(&master->master_sem); |
1387 |
1385 |
1388 // queue and send |
1386 // queue and send |
1389 down(&master->io_sem); |
1387 down(&master->io_sem); |
1390 if (fsm_exec) { |
1388 if (fsm_exec) { |
1391 ec_master_queue_datagram(master, &master->fsm_datagram, |
1389 ec_master_queue_datagram(master, &master->fsm_datagram); |
1392 EC_DEVICE_MAIN); |
|
1393 } |
1390 } |
1394 ecrt_master_send(master); |
1391 ecrt_master_send(master); |
1395 #ifdef EC_USE_HRTIMER |
1392 #ifdef EC_USE_HRTIMER |
1396 sent_bytes = master->devices[EC_DEVICE_MAIN].tx_skb[ |
1393 sent_bytes = master->devices[EC_DEVICE_MAIN].tx_skb[ |
1397 master->devices[EC_DEVICE_MAIN].tx_ring_index]->len; |
1394 master->devices[EC_DEVICE_MAIN].tx_ring_index]->len; |
2229 /*****************************************************************************/ |
2226 /*****************************************************************************/ |
2230 |
2227 |
2231 void ecrt_master_send(ec_master_t *master) |
2228 void ecrt_master_send(ec_master_t *master) |
2232 { |
2229 { |
2233 ec_datagram_t *datagram, *n; |
2230 ec_datagram_t *datagram, *n; |
2234 unsigned int i; |
2231 ec_device_index_t dev_idx; |
2235 |
2232 |
2236 if (master->injection_seq_rt != master->injection_seq_fsm) { |
2233 if (master->injection_seq_rt != master->injection_seq_fsm) { |
2237 // inject datagrams produced by master & slave FSMs |
2234 // inject datagrams produced by master and slave FSMs |
2238 ec_master_queue_datagram(master, &master->fsm_datagram, |
2235 ec_master_queue_datagram(master, &master->fsm_datagram); |
2239 EC_DEVICE_MAIN); |
|
2240 master->injection_seq_rt = master->injection_seq_fsm; |
2236 master->injection_seq_rt = master->injection_seq_fsm; |
2241 } |
2237 } |
2242 |
2238 |
2243 ec_master_inject_external_datagrams(master); |
2239 ec_master_inject_external_datagrams(master); |
2244 |
2240 |
2245 for (i = 0; i < EC_NUM_DEVICES; i++) { |
2241 for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_NUM_DEVICES; dev_idx++) { |
2246 if (unlikely(!master->devices[i].link_state)) { |
2242 if (unlikely(!master->devices[dev_idx].link_state)) { |
2247 // link is down, no datagram can be sent |
2243 // link is down, no datagram can be sent |
2248 list_for_each_entry_safe(datagram, n, |
2244 list_for_each_entry_safe(datagram, n, |
2249 &master->datagram_queue, queue) { |
2245 &master->datagram_queue, queue) { |
2250 if (datagram->device_index == i) { |
2246 if (datagram->device_index == dev_idx) { |
2251 datagram->state = EC_DATAGRAM_ERROR; |
2247 datagram->state = EC_DATAGRAM_ERROR; |
2252 list_del_init(&datagram->queue); |
2248 list_del_init(&datagram->queue); |
2253 } |
2249 } |
2254 } |
2250 } |
2255 |
2251 |
2256 if (!master->devices[i].dev) { |
2252 if (!master->devices[dev_idx].dev) { |
2257 continue; |
2253 continue; |
2258 } |
2254 } |
2259 |
2255 |
2260 // query link state |
2256 // query link state |
2261 ec_device_poll(&master->devices[i]); |
2257 ec_device_poll(&master->devices[dev_idx]); |
2262 |
2258 |
2263 // clear frame statistics |
2259 // clear frame statistics |
2264 ec_device_clear_stats(&master->devices[i]); |
2260 ec_device_clear_stats(&master->devices[dev_idx]); |
2265 continue; |
2261 continue; |
2266 } |
2262 } |
2267 |
2263 |
2268 // send frames |
2264 // send frames |
2269 ec_master_send_datagrams(master, i); |
2265 ec_master_send_datagrams(master, dev_idx); |
2270 } |
2266 } |
2271 } |
2267 } |
2272 |
2268 |
2273 /*****************************************************************************/ |
2269 /*****************************************************************************/ |
2274 |
2270 |
2325 ec_datagram_t *datagram, *next; |
2321 ec_datagram_t *datagram, *next; |
2326 |
2322 |
2327 list_for_each_entry_safe(datagram, next, &master->ext_datagram_queue, |
2323 list_for_each_entry_safe(datagram, next, &master->ext_datagram_queue, |
2328 queue) { |
2324 queue) { |
2329 list_del(&datagram->queue); |
2325 list_del(&datagram->queue); |
2330 ec_master_queue_datagram(master, datagram, EC_DEVICE_MAIN); |
2326 ec_master_queue_datagram(master, datagram); |
2331 } |
2327 } |
2332 |
2328 |
2333 ecrt_master_send(master); |
2329 ecrt_master_send(master); |
2334 } |
2330 } |
2335 |
2331 |
2489 |
2485 |
2490 /*****************************************************************************/ |
2486 /*****************************************************************************/ |
2491 |
2487 |
2492 void ecrt_master_state(const ec_master_t *master, ec_master_state_t *state) |
2488 void ecrt_master_state(const ec_master_t *master, ec_master_state_t *state) |
2493 { |
2489 { |
2494 state->slaves_responding = master->fsm.slaves_responding; |
2490 ec_device_index_t dev_idx; |
2495 state->al_states = master->fsm.slave_states; |
2491 |
2496 state->link_up = master->devices[EC_DEVICE_MAIN].link_state; |
2492 state->slaves_responding = 0U; |
|
2493 state->al_states = 0; |
|
2494 state->link_up = 0U; |
|
2495 |
|
2496 for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_NUM_DEVICES; dev_idx++) { |
|
2497 /* Announce sum of responding slaves on all links. */ |
|
2498 state->slaves_responding += master->fsm.slaves_responding[dev_idx]; |
|
2499 |
|
2500 /* Binary-or slave states of all links. */ |
|
2501 state->al_states |= master->fsm.slave_states[dev_idx]; |
|
2502 |
|
2503 /* Signal link up if at least one device has link. */ |
|
2504 state->link_up |= master->devices[dev_idx].link_state; |
|
2505 } |
2497 } |
2506 } |
2498 |
2507 |
2499 /*****************************************************************************/ |
2508 /*****************************************************************************/ |
2500 |
2509 |
2501 void ecrt_master_application_time(ec_master_t *master, uint64_t app_time) |
2510 void ecrt_master_application_time(ec_master_t *master, uint64_t app_time) |
2511 /*****************************************************************************/ |
2520 /*****************************************************************************/ |
2512 |
2521 |
2513 void ecrt_master_sync_reference_clock(ec_master_t *master) |
2522 void ecrt_master_sync_reference_clock(ec_master_t *master) |
2514 { |
2523 { |
2515 EC_WRITE_U32(master->ref_sync_datagram.data, master->app_time); |
2524 EC_WRITE_U32(master->ref_sync_datagram.data, master->app_time); |
2516 ec_master_queue_datagram(master, &master->ref_sync_datagram, |
2525 ec_master_queue_datagram(master, &master->ref_sync_datagram); |
2517 EC_DEVICE_MAIN); |
|
2518 } |
2526 } |
2519 |
2527 |
2520 /*****************************************************************************/ |
2528 /*****************************************************************************/ |
2521 |
2529 |
2522 void ecrt_master_sync_slave_clocks(ec_master_t *master) |
2530 void ecrt_master_sync_slave_clocks(ec_master_t *master) |
2523 { |
2531 { |
2524 ec_datagram_zero(&master->sync_datagram); |
2532 ec_datagram_zero(&master->sync_datagram); |
2525 ec_master_queue_datagram(master, &master->sync_datagram, |
2533 ec_master_queue_datagram(master, &master->sync_datagram); |
2526 EC_DEVICE_MAIN); |
|
2527 } |
2534 } |
2528 |
2535 |
2529 /*****************************************************************************/ |
2536 /*****************************************************************************/ |
2530 |
2537 |
2531 void ecrt_master_sync_monitor_queue(ec_master_t *master) |
2538 void ecrt_master_sync_monitor_queue(ec_master_t *master) |
2532 { |
2539 { |
2533 ec_datagram_zero(&master->sync_mon_datagram); |
2540 ec_datagram_zero(&master->sync_mon_datagram); |
2534 ec_master_queue_datagram(master, &master->sync_mon_datagram, |
2541 ec_master_queue_datagram(master, &master->sync_mon_datagram); |
2535 EC_DEVICE_MAIN); |
|
2536 } |
2542 } |
2537 |
2543 |
2538 /*****************************************************************************/ |
2544 /*****************************************************************************/ |
2539 |
2545 |
2540 uint32_t ecrt_master_sync_monitor_process(ec_master_t *master) |
2546 uint32_t ecrt_master_sync_monitor_process(ec_master_t *master) |