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