58 |
58 |
59 /*****************************************************************************/ |
59 /*****************************************************************************/ |
60 |
60 |
61 void ec_master_clear_slave_configs(ec_master_t *); |
61 void ec_master_clear_slave_configs(ec_master_t *); |
62 void ec_master_clear_domains(ec_master_t *); |
62 void ec_master_clear_domains(ec_master_t *); |
63 static int ec_master_idle_thread(ec_master_t *); |
63 static int ec_master_idle_thread(void *); |
64 static int ec_master_operation_thread(ec_master_t *); |
64 static int ec_master_operation_thread(void *); |
65 #ifdef EC_EOE |
65 #ifdef EC_EOE |
66 void ec_master_eoe_run(unsigned long); |
66 void ec_master_eoe_run(unsigned long); |
67 #endif |
67 #endif |
68 |
68 |
69 /*****************************************************************************/ |
69 /*****************************************************************************/ |
118 master->stats.timeouts = 0; |
118 master->stats.timeouts = 0; |
119 master->stats.corrupted = 0; |
119 master->stats.corrupted = 0; |
120 master->stats.unmatched = 0; |
120 master->stats.unmatched = 0; |
121 master->stats.output_jiffies = 0; |
121 master->stats.output_jiffies = 0; |
122 master->frames_timed_out = 0; |
122 master->frames_timed_out = 0; |
|
123 |
|
124 master->thread = NULL; |
123 |
125 |
124 #ifdef EC_EOE |
126 #ifdef EC_EOE |
125 init_timer(&master->eoe_timer); |
127 init_timer(&master->eoe_timer); |
126 master->eoe_timer.function = ec_master_eoe_run; |
128 master->eoe_timer.function = ec_master_eoe_run; |
127 master->eoe_timer.data = (unsigned long) master; |
129 master->eoe_timer.data = (unsigned long) master; |
310 |
312 |
311 /** Starts the master thread. |
313 /** Starts the master thread. |
312 */ |
314 */ |
313 int ec_master_thread_start( |
315 int ec_master_thread_start( |
314 ec_master_t *master, /**< EtherCAT master */ |
316 ec_master_t *master, /**< EtherCAT master */ |
315 int (*thread_func)(ec_master_t *) /**< thread function to start */ |
317 int (*thread_func)(void *), /**< thread function to start */ |
|
318 const char *name /**< Thread name. */ |
316 ) |
319 ) |
317 { |
320 { |
318 init_completion(&master->thread_can_terminate); |
321 EC_INFO("Starting %s thread.\n", name); |
319 init_completion(&master->thread_exit); |
322 master->thread = kthread_run(thread_func, master, name); |
320 |
323 if (IS_ERR(master->thread)) { |
321 EC_INFO("Starting master thread.\n"); |
324 EC_ERR("Failed to start master thread (error %i)!\n", |
322 if (!(master->thread_id = kernel_thread((int (*)(void *)) thread_func, |
325 (int) PTR_ERR(master->thread)); |
323 master, CLONE_KERNEL))) |
326 master->thread = NULL; |
324 return -1; |
327 return -1; |
|
328 } |
325 |
329 |
326 return 0; |
330 return 0; |
327 } |
331 } |
328 |
332 |
329 /*****************************************************************************/ |
333 /*****************************************************************************/ |
334 ec_master_t *master /**< EtherCAT master */ |
338 ec_master_t *master /**< EtherCAT master */ |
335 ) |
339 ) |
336 { |
340 { |
337 unsigned long sleep_jiffies; |
341 unsigned long sleep_jiffies; |
338 |
342 |
339 if (!master->thread_id) { |
343 if (!master->thread) { |
340 EC_WARN("ec_master_thread_stop: Already finished!\n"); |
344 EC_WARN("ec_master_thread_stop(): Already finished!\n"); |
341 return; |
345 return; |
342 } |
346 } |
343 |
347 |
344 if (master->debug_level) |
348 if (master->debug_level) |
345 EC_DBG("Stopping master thread.\n"); |
349 EC_DBG("Stopping master thread.\n"); |
346 |
350 |
347 // wait until thread is ready to receive the SIGTERM |
351 kthread_stop(master->thread); |
348 wait_for_completion(&master->thread_can_terminate); |
352 master->thread = NULL; |
349 |
|
350 kill_proc(master->thread_id, SIGTERM, 1); |
|
351 wait_for_completion(&master->thread_exit); |
|
352 EC_INFO("Master thread exited.\n"); |
353 EC_INFO("Master thread exited.\n"); |
353 |
354 |
354 if (master->fsm_datagram.state != EC_DATAGRAM_SENT) |
355 if (master->fsm_datagram.state != EC_DATAGRAM_SENT) |
355 return; |
356 return; |
356 |
357 |
373 master->request_cb = ec_master_request_cb; |
374 master->request_cb = ec_master_request_cb; |
374 master->release_cb = ec_master_release_cb; |
375 master->release_cb = ec_master_release_cb; |
375 master->cb_data = master; |
376 master->cb_data = master; |
376 |
377 |
377 master->phase = EC_IDLE; |
378 master->phase = EC_IDLE; |
378 if (ec_master_thread_start(master, ec_master_idle_thread)) { |
379 if (ec_master_thread_start(master, ec_master_idle_thread, |
|
380 "EtherCAT-IDLE")) { |
379 master->phase = EC_ORPHANED; |
381 master->phase = EC_ORPHANED; |
380 return -1; |
382 return -1; |
381 } |
383 } |
382 |
384 |
383 return 0; |
385 return 0; |
529 if (ec_eoe_is_open(eoe)) |
531 if (ec_eoe_is_open(eoe)) |
530 ec_slave_request_state(eoe->slave, EC_SLAVE_STATE_OP); |
532 ec_slave_request_state(eoe->slave, EC_SLAVE_STATE_OP); |
531 } |
533 } |
532 #endif |
534 #endif |
533 |
535 |
534 if (ec_master_thread_start(master, ec_master_idle_thread)) |
536 if (ec_master_thread_start(master, ec_master_idle_thread, |
|
537 "EtherCAT-IDLE")) |
535 EC_WARN("Failed to restart master thread!\n"); |
538 EC_WARN("Failed to restart master thread!\n"); |
536 #ifdef EC_EOE |
539 #ifdef EC_EOE |
537 ec_master_eoe_start(master); |
540 ec_master_eoe_start(master); |
538 #endif |
541 #endif |
539 |
542 |
823 |
826 |
824 /*****************************************************************************/ |
827 /*****************************************************************************/ |
825 |
828 |
826 /** Master kernel thread function for IDLE phase. |
829 /** Master kernel thread function for IDLE phase. |
827 */ |
830 */ |
828 static int ec_master_idle_thread(ec_master_t *master) |
831 static int ec_master_idle_thread(void *priv_data) |
829 { |
832 { |
830 daemonize("EtherCAT-IDLE"); |
833 ec_master_t *master = (ec_master_t *) priv_data; |
831 allow_signal(SIGTERM); |
834 |
832 complete(&master->thread_can_terminate); |
835 if (master->debug_level) |
833 |
836 EC_DBG("Idle thread running.\n"); |
834 while (!signal_pending(current)) { |
837 |
|
838 while (!kthread_should_stop()) { |
835 ec_datagram_output_stats(&master->fsm_datagram); |
839 ec_datagram_output_stats(&master->fsm_datagram); |
836 |
840 |
837 // receive |
841 // receive |
838 spin_lock_bh(&master->internal_lock); |
842 spin_lock_bh(&master->internal_lock); |
839 ecrt_master_receive(master); |
843 ecrt_master_receive(master); |
841 |
845 |
842 if (master->fsm_datagram.state == EC_DATAGRAM_SENT) |
846 if (master->fsm_datagram.state == EC_DATAGRAM_SENT) |
843 goto schedule; |
847 goto schedule; |
844 |
848 |
845 // execute master state machine |
849 // execute master state machine |
846 down(&master->master_sem); |
850 if (down_interruptible(&master->master_sem)) |
|
851 break; |
847 ec_fsm_master_exec(&master->fsm); |
852 ec_fsm_master_exec(&master->fsm); |
848 up(&master->master_sem); |
853 up(&master->master_sem); |
849 |
854 |
850 // queue and send |
855 // queue and send |
851 spin_lock_bh(&master->internal_lock); |
856 spin_lock_bh(&master->internal_lock); |
861 else { |
866 else { |
862 schedule(); |
867 schedule(); |
863 } |
868 } |
864 } |
869 } |
865 |
870 |
866 master->thread_id = 0; |
|
867 if (master->debug_level) |
871 if (master->debug_level) |
868 EC_DBG("Master IDLE thread exiting...\n"); |
872 EC_DBG("Master IDLE thread exiting...\n"); |
869 complete_and_exit(&master->thread_exit, 0); |
873 return 0; |
870 } |
874 } |
871 |
875 |
872 /*****************************************************************************/ |
876 /*****************************************************************************/ |
873 |
877 |
874 /** Master kernel thread function for IDLE phase. |
878 /** Master kernel thread function for IDLE phase. |
875 */ |
879 */ |
876 static int ec_master_operation_thread(ec_master_t *master) |
880 static int ec_master_operation_thread(void *priv_data) |
877 { |
881 { |
878 daemonize("EtherCAT-OP"); |
882 ec_master_t *master = (ec_master_t *) priv_data; |
879 allow_signal(SIGTERM); |
883 |
880 complete(&master->thread_can_terminate); |
884 if (master->debug_level) |
881 |
885 EC_DBG("Operation thread running.\n"); |
882 while (!signal_pending(current)) { |
886 |
|
887 while (!kthread_should_stop()) { |
883 ec_datagram_output_stats(&master->fsm_datagram); |
888 ec_datagram_output_stats(&master->fsm_datagram); |
884 if (master->injection_seq_rt != master->injection_seq_fsm || |
889 if (master->injection_seq_rt != master->injection_seq_fsm || |
885 master->fsm_datagram.state == EC_DATAGRAM_SENT || |
890 master->fsm_datagram.state == EC_DATAGRAM_SENT || |
886 master->fsm_datagram.state == EC_DATAGRAM_QUEUED) |
891 master->fsm_datagram.state == EC_DATAGRAM_QUEUED) |
887 goto schedule; |
892 goto schedule; |
888 |
893 |
889 // output statistics |
894 // output statistics |
890 ec_master_output_stats(master); |
895 ec_master_output_stats(master); |
891 |
896 |
892 // execute master state machine |
897 // execute master state machine |
893 down(&master->master_sem); |
898 if (down_interruptible(&master->master_sem)) |
|
899 break; |
894 ec_fsm_master_exec(&master->fsm); |
900 ec_fsm_master_exec(&master->fsm); |
895 up(&master->master_sem); |
901 up(&master->master_sem); |
896 |
902 |
897 // inject datagram |
903 // inject datagram |
898 master->injection_seq_fsm++; |
904 master->injection_seq_fsm++; |
1294 master->injection_seq_rt = 0; |
1299 master->injection_seq_rt = 0; |
1295 master->request_cb = master->ext_request_cb; |
1300 master->request_cb = master->ext_request_cb; |
1296 master->release_cb = master->ext_release_cb; |
1301 master->release_cb = master->ext_release_cb; |
1297 master->cb_data = master->ext_cb_data; |
1302 master->cb_data = master->ext_cb_data; |
1298 |
1303 |
1299 if (ec_master_thread_start(master, ec_master_operation_thread)) { |
1304 if (ec_master_thread_start(master, ec_master_operation_thread, |
|
1305 "EtherCAT-OP")) { |
1300 EC_ERR("Failed to start master thread!\n"); |
1306 EC_ERR("Failed to start master thread!\n"); |
1301 return -1; |
1307 return -1; |
1302 } |
1308 } |
1303 #ifdef EC_EOE |
1309 #ifdef EC_EOE |
1304 ec_master_eoe_start(master); |
1310 ec_master_eoe_start(master); |