214 ec_datagram_init(&master->fsm_datagram); |
214 ec_datagram_init(&master->fsm_datagram); |
215 snprintf(master->fsm_datagram.name, EC_DATAGRAM_NAME_SIZE, "master-fsm"); |
215 snprintf(master->fsm_datagram.name, EC_DATAGRAM_NAME_SIZE, "master-fsm"); |
216 ret = ec_datagram_prealloc(&master->fsm_datagram, EC_MAX_DATA_SIZE); |
216 ret = ec_datagram_prealloc(&master->fsm_datagram, EC_MAX_DATA_SIZE); |
217 if (ret < 0) { |
217 if (ret < 0) { |
218 ec_datagram_clear(&master->fsm_datagram); |
218 ec_datagram_clear(&master->fsm_datagram); |
219 EC_ERR("Failed to allocate FSM datagram.\n"); |
219 EC_MASTER_ERR(master, "Failed to allocate FSM datagram.\n"); |
220 goto out_clear_backup; |
220 goto out_clear_backup; |
221 } |
221 } |
222 |
222 |
223 // create state machine object |
223 // create state machine object |
224 ec_fsm_master_init(&master->fsm, master, &master->fsm_datagram); |
224 ec_fsm_master_init(&master->fsm, master, &master->fsm_datagram); |
227 ec_datagram_init(&master->ref_sync_datagram); |
227 ec_datagram_init(&master->ref_sync_datagram); |
228 snprintf(master->ref_sync_datagram.name, EC_DATAGRAM_NAME_SIZE, "refsync"); |
228 snprintf(master->ref_sync_datagram.name, EC_DATAGRAM_NAME_SIZE, "refsync"); |
229 ret = ec_datagram_apwr(&master->ref_sync_datagram, 0, 0x0910, 8); |
229 ret = ec_datagram_apwr(&master->ref_sync_datagram, 0, 0x0910, 8); |
230 if (ret < 0) { |
230 if (ret < 0) { |
231 ec_datagram_clear(&master->ref_sync_datagram); |
231 ec_datagram_clear(&master->ref_sync_datagram); |
232 EC_ERR("Failed to allocate reference synchronisation datagram.\n"); |
232 EC_MASTER_ERR(master, "Failed to allocate reference" |
|
233 " synchronisation datagram.\n"); |
233 goto out_clear_fsm; |
234 goto out_clear_fsm; |
234 } |
235 } |
235 |
236 |
236 // init sync datagram |
237 // init sync datagram |
237 ec_datagram_init(&master->sync_datagram); |
238 ec_datagram_init(&master->sync_datagram); |
238 snprintf(master->sync_datagram.name, EC_DATAGRAM_NAME_SIZE, "sync"); |
239 snprintf(master->sync_datagram.name, EC_DATAGRAM_NAME_SIZE, "sync"); |
239 ret = ec_datagram_prealloc(&master->sync_datagram, 4); |
240 ret = ec_datagram_prealloc(&master->sync_datagram, 4); |
240 if (ret < 0) { |
241 if (ret < 0) { |
241 ec_datagram_clear(&master->sync_datagram); |
242 ec_datagram_clear(&master->sync_datagram); |
242 EC_ERR("Failed to allocate synchronisation datagram.\n"); |
243 EC_MASTER_ERR(master, "Failed to allocate" |
|
244 " synchronisation datagram.\n"); |
243 goto out_clear_ref_sync; |
245 goto out_clear_ref_sync; |
244 } |
246 } |
245 |
247 |
246 // init sync monitor datagram |
248 // init sync monitor datagram |
247 ec_datagram_init(&master->sync_mon_datagram); |
249 ec_datagram_init(&master->sync_mon_datagram); |
248 snprintf(master->sync_mon_datagram.name, EC_DATAGRAM_NAME_SIZE, "syncmon"); |
250 snprintf(master->sync_mon_datagram.name, EC_DATAGRAM_NAME_SIZE, "syncmon"); |
249 ret = ec_datagram_brd(&master->sync_mon_datagram, 0x092c, 4); |
251 ret = ec_datagram_brd(&master->sync_mon_datagram, 0x092c, 4); |
250 if (ret < 0) { |
252 if (ret < 0) { |
251 ec_datagram_clear(&master->sync_mon_datagram); |
253 ec_datagram_clear(&master->sync_mon_datagram); |
252 EC_ERR("Failed to allocate sync monitoring datagram.\n"); |
254 EC_MASTER_ERR(master, "Failed to allocate sync" |
|
255 " monitoring datagram.\n"); |
253 goto out_clear_sync; |
256 goto out_clear_sync; |
254 } |
257 } |
255 |
258 |
256 ec_master_find_dc_ref_clock(master); |
259 ec_master_find_dc_ref_clock(master); |
257 |
260 |
380 |
383 |
381 master->dc_ref_clock = NULL; |
384 master->dc_ref_clock = NULL; |
382 |
385 |
383 // external requests are obsolete, so we wake pending waiters and remove |
386 // external requests are obsolete, so we wake pending waiters and remove |
384 // them from the list |
387 // them from the list |
385 // |
388 |
386 // SII requests |
389 while (!list_empty(&master->sii_requests)) { |
387 while (1) { |
390 ec_sii_write_request_t *request = |
388 ec_sii_write_request_t *request; |
391 list_entry(master->sii_requests.next, |
389 if (list_empty(&master->sii_requests)) |
392 ec_sii_write_request_t, list); |
390 break; |
|
391 // get first request |
|
392 request = list_entry(master->sii_requests.next, |
|
393 ec_sii_write_request_t, list); |
|
394 list_del_init(&request->list); // dequeue |
393 list_del_init(&request->list); // dequeue |
395 EC_INFO("Discarding SII request, slave %u does not exist anymore.\n", |
394 EC_MASTER_WARN(master, "Discarding SII request, slave %u about" |
396 request->slave->ring_position); |
395 " to be deleted.\n", request->slave->ring_position); |
397 request->state = EC_INT_REQUEST_FAILURE; |
396 request->state = EC_INT_REQUEST_FAILURE; |
398 wake_up(&master->sii_queue); |
397 wake_up(&master->sii_queue); |
399 } |
398 } |
400 |
399 |
401 // Register requests |
400 while (!list_empty(&master->reg_requests)) { |
402 while (1) { |
401 ec_reg_request_t *request = |
403 ec_reg_request_t *request; |
402 list_entry(master->reg_requests.next, ec_reg_request_t, list); |
404 if (list_empty(&master->reg_requests)) |
|
405 break; |
|
406 // get first request |
|
407 request = list_entry(master->reg_requests.next, |
|
408 ec_reg_request_t, list); |
|
409 list_del_init(&request->list); // dequeue |
403 list_del_init(&request->list); // dequeue |
410 EC_INFO("Discarding Reg request, slave %u does not exist anymore.\n", |
404 EC_MASTER_WARN(master, "Discarding register request, slave %u" |
411 request->slave->ring_position); |
405 " about to be deleted.\n", request->slave->ring_position); |
412 request->state = EC_INT_REQUEST_FAILURE; |
406 request->state = EC_INT_REQUEST_FAILURE; |
413 wake_up(&master->reg_queue); |
407 wake_up(&master->reg_queue); |
414 } |
408 } |
415 |
409 |
416 for (slave = master->slaves; |
410 for (slave = master->slaves; |
417 slave < master->slaves + master->slave_count; |
411 slave < master->slaves + master->slave_count; |
418 slave++) { |
412 slave++) { |
419 // SDO requests |
|
420 while (1) { |
|
421 ec_master_sdo_request_t *request; |
|
422 if (list_empty(&slave->slave_sdo_requests)) |
|
423 break; |
|
424 // get first request |
|
425 request = list_entry(slave->slave_sdo_requests.next, |
|
426 ec_master_sdo_request_t, list); |
|
427 list_del_init(&request->list); // dequeue |
|
428 EC_INFO("Discarding SDO request," |
|
429 " slave %u does not exist anymore.\n", |
|
430 slave->ring_position); |
|
431 request->req.state = EC_INT_REQUEST_FAILURE; |
|
432 wake_up(&slave->sdo_queue); |
|
433 } |
|
434 // FoE requests |
|
435 while (1) { |
|
436 ec_master_foe_request_t *request; |
|
437 if (list_empty(&slave->foe_requests)) |
|
438 break; |
|
439 // get first request |
|
440 request = list_entry(slave->foe_requests.next, |
|
441 ec_master_foe_request_t, list); |
|
442 list_del_init(&request->list); // dequeue |
|
443 EC_INFO("Discarding FoE request," |
|
444 " slave %u does not exist anymore.\n", |
|
445 slave->ring_position); |
|
446 request->req.state = EC_INT_REQUEST_FAILURE; |
|
447 wake_up(&slave->foe_queue); |
|
448 } |
|
449 // SoE requests |
|
450 while (1) { |
|
451 ec_master_soe_request_t *request; |
|
452 if (list_empty(&slave->soe_requests)) |
|
453 break; |
|
454 // get first request |
|
455 request = list_entry(slave->soe_requests.next, |
|
456 ec_master_soe_request_t, list); |
|
457 list_del_init(&request->list); // dequeue |
|
458 EC_INFO("Discarding SoE request," |
|
459 " slave %u does not exist anymore.\n", |
|
460 slave->ring_position); |
|
461 request->req.state = EC_INT_REQUEST_FAILURE; |
|
462 wake_up(&slave->soe_queue); |
|
463 } |
|
464 ec_slave_clear(slave); |
413 ec_slave_clear(slave); |
465 } |
414 } |
466 |
415 |
467 if (master->slaves) { |
416 if (master->slaves) { |
468 kfree(master->slaves); |
417 kfree(master->slaves); |
526 ec_master_t *master, /**< EtherCAT master */ |
475 ec_master_t *master, /**< EtherCAT master */ |
527 int (*thread_func)(void *), /**< thread function to start */ |
476 int (*thread_func)(void *), /**< thread function to start */ |
528 const char *name /**< Thread name. */ |
477 const char *name /**< Thread name. */ |
529 ) |
478 ) |
530 { |
479 { |
531 EC_INFO("Starting %s thread.\n", name); |
480 EC_MASTER_INFO(master, "Starting %s thread.\n", name); |
532 master->thread = kthread_run(thread_func, master, name); |
481 master->thread = kthread_run(thread_func, master, name); |
533 if (IS_ERR(master->thread)) { |
482 if (IS_ERR(master->thread)) { |
534 int err = (int) PTR_ERR(master->thread); |
483 int err = (int) PTR_ERR(master->thread); |
535 EC_ERR("Failed to start master thread (error %i)!\n", err); |
484 EC_MASTER_ERR(master, "Failed to start master thread (error %i)!\n", |
|
485 err); |
536 master->thread = NULL; |
486 master->thread = NULL; |
537 return err; |
487 return err; |
538 } |
488 } |
539 |
489 |
540 return 0; |
490 return 0; |
549 ) |
499 ) |
550 { |
500 { |
551 unsigned long sleep_jiffies; |
501 unsigned long sleep_jiffies; |
552 |
502 |
553 if (!master->thread) { |
503 if (!master->thread) { |
554 EC_WARN("ec_master_thread_stop(): Already finished!\n"); |
504 EC_MASTER_WARN(master, "%s(): Already finished!\n", __func__); |
555 return; |
505 return; |
556 } |
506 } |
557 |
507 |
558 if (master->debug_level) |
508 EC_MASTER_DBG(master, 1, "Stopping master thread.\n"); |
559 EC_DBG("Stopping master thread.\n"); |
|
560 |
509 |
561 kthread_stop(master->thread); |
510 kthread_stop(master->thread); |
562 master->thread = NULL; |
511 master->thread = NULL; |
563 EC_INFO("Master thread exited.\n"); |
512 EC_MASTER_INFO(master, "Master thread exited.\n"); |
564 |
513 |
565 if (master->fsm_datagram.state != EC_DATAGRAM_SENT) |
514 if (master->fsm_datagram.state != EC_DATAGRAM_SENT) |
566 return; |
515 return; |
567 |
516 |
568 // wait for FSM datagram |
517 // wait for FSM datagram |
627 ec_slave_t *slave; |
574 ec_slave_t *slave; |
628 #ifdef EC_EOE |
575 #ifdef EC_EOE |
629 ec_eoe_t *eoe; |
576 ec_eoe_t *eoe; |
630 #endif |
577 #endif |
631 |
578 |
632 if (master->debug_level) |
579 EC_MASTER_DBG(master, 1, "IDLE -> OPERATION.\n"); |
633 EC_DBG("IDLE -> OPERATION.\n"); |
|
634 |
580 |
635 down(&master->config_sem); |
581 down(&master->config_sem); |
636 master->allow_config = 0; // temporarily disable slave configuration |
582 master->allow_config = 0; // temporarily disable slave configuration |
637 if (master->config_busy) { |
583 if (master->config_busy) { |
638 up(&master->config_sem); |
584 up(&master->config_sem); |
639 |
585 |
640 // wait for slave configuration to complete |
586 // wait for slave configuration to complete |
641 ret = wait_event_interruptible(master->config_queue, |
587 ret = wait_event_interruptible(master->config_queue, |
642 !master->config_busy); |
588 !master->config_busy); |
643 if (ret) { |
589 if (ret) { |
644 EC_INFO("Finishing slave configuration interrupted by signal.\n"); |
590 EC_MASTER_INFO(master, "Finishing slave configuration" |
|
591 " interrupted by signal.\n"); |
645 goto out_allow; |
592 goto out_allow; |
646 } |
593 } |
647 |
594 |
648 if (master->debug_level) |
595 EC_MASTER_DBG(master, 1, "Waiting for pending slave" |
649 EC_DBG("Waiting for pending slave configuration returned.\n"); |
596 " configuration returned.\n"); |
650 } else { |
597 } else { |
651 up(&master->config_sem); |
598 up(&master->config_sem); |
652 } |
599 } |
653 |
600 |
654 down(&master->scan_sem); |
601 down(&master->scan_sem); |
659 up(&master->scan_sem); |
606 up(&master->scan_sem); |
660 |
607 |
661 // wait for slave scan to complete |
608 // wait for slave scan to complete |
662 ret = wait_event_interruptible(master->scan_queue, !master->scan_busy); |
609 ret = wait_event_interruptible(master->scan_queue, !master->scan_busy); |
663 if (ret) { |
610 if (ret) { |
664 EC_INFO("Waiting for slave scan interrupted by signal.\n"); |
611 EC_MASTER_INFO(master, "Waiting for slave scan" |
|
612 " interrupted by signal.\n"); |
665 goto out_allow; |
613 goto out_allow; |
666 } |
614 } |
667 |
615 |
668 if (master->debug_level) |
616 EC_MASTER_DBG(master, 1, "Waiting for pending" |
669 EC_DBG("Waiting for pending slave scan returned.\n"); |
617 " slave scan returned.\n"); |
670 } |
618 } |
671 |
619 |
672 // set states for all slaves |
620 // set states for all slaves |
673 for (slave = master->slaves; |
621 for (slave = master->slaves; |
674 slave < master->slaves + master->slave_count; |
622 slave < master->slaves + master->slave_count; |
733 queue) { |
680 queue) { |
734 queue_size += datagram->data_size; |
681 queue_size += datagram->data_size; |
735 if (queue_size <= master->max_queue_size) { |
682 if (queue_size <= master->max_queue_size) { |
736 list_del_init(&datagram->queue); |
683 list_del_init(&datagram->queue); |
737 #if DEBUG_INJECT |
684 #if DEBUG_INJECT |
738 if (master->debug_level) { |
685 EC_MASTER_DBG(master, 0, "Injecting external datagram %08x" |
739 EC_DBG("Injecting external datagram %08x size=%u," |
686 " size=%u, queue_size=%u\n", (unsigned int) datagram, |
740 " queue_size=%u\n", (unsigned int) datagram, |
687 datagram->data_size, queue_size); |
741 datagram->data_size, queue_size); |
|
742 } |
|
743 #endif |
688 #endif |
744 #ifdef EC_HAVE_CYCLES |
689 #ifdef EC_HAVE_CYCLES |
745 datagram->cycles_sent = 0; |
690 datagram->cycles_sent = 0; |
746 #endif |
691 #endif |
747 datagram->jiffies_sent = 0; |
692 datagram->jiffies_sent = 0; |
749 } |
694 } |
750 else { |
695 else { |
751 if (datagram->data_size > master->max_queue_size) { |
696 if (datagram->data_size > master->max_queue_size) { |
752 list_del_init(&datagram->queue); |
697 list_del_init(&datagram->queue); |
753 datagram->state = EC_DATAGRAM_ERROR; |
698 datagram->state = EC_DATAGRAM_ERROR; |
754 EC_ERR("External datagram %p is too large," |
699 EC_MASTER_ERR(master, "External datagram %p is too large," |
755 " size=%u, max_queue_size=%u\n", |
700 " size=%u, max_queue_size=%u\n", |
756 datagram, datagram->data_size, |
701 datagram, datagram->data_size, |
757 master->max_queue_size); |
702 master->max_queue_size); |
758 } else { |
703 } else { |
759 #ifdef EC_HAVE_CYCLES |
704 #ifdef EC_HAVE_CYCLES |
776 / cpu_khz; |
721 / cpu_khz; |
777 #else |
722 #else |
778 time_us = (unsigned int) |
723 time_us = (unsigned int) |
779 ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); |
724 ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); |
780 #endif |
725 #endif |
781 EC_ERR("Timeout %u us: injecting external datagram %p" |
726 EC_MASTER_ERR(master, "Timeout %u us: Injecting" |
782 " size=%u, max_queue_size=%u\n", |
727 " external datagram %p size=%u," |
783 time_us, datagram, |
728 " max_queue_size=%u\n", time_us, datagram, |
784 datagram->data_size, master->max_queue_size); |
729 datagram->data_size, master->max_queue_size); |
785 } |
730 } |
786 #if DEBUG_INJECT |
731 #if DEBUG_INJECT |
787 else if (master->debug_level) { |
732 else { |
788 EC_DBG("Deferred injecting of external datagram %p" |
733 EC_MASTER_DBG(master, 0, "Deferred injecting" |
|
734 " of external datagram %p" |
789 " size=%u, queue_size=%u\n", |
735 " size=%u, queue_size=%u\n", |
790 datagram, datagram->data_size, queue_size); |
736 datagram, datagram->data_size, queue_size); |
791 } |
737 } |
792 #endif |
738 #endif |
793 } |
739 } |
1034 const uint8_t *cur_data; |
974 const uint8_t *cur_data; |
1035 ec_datagram_t *datagram; |
975 ec_datagram_t *datagram; |
1036 |
976 |
1037 if (unlikely(size < EC_FRAME_HEADER_SIZE)) { |
977 if (unlikely(size < EC_FRAME_HEADER_SIZE)) { |
1038 if (master->debug_level) { |
978 if (master->debug_level) { |
1039 EC_DBG("Corrupted frame received (size %zu < %u byte):\n", |
979 EC_MASTER_DBG(master, 0, "Corrupted frame received" |
|
980 " (size %zu < %u byte):\n", |
1040 size, EC_FRAME_HEADER_SIZE); |
981 size, EC_FRAME_HEADER_SIZE); |
1041 ec_print_data(frame_data, size); |
982 ec_print_data(frame_data, size); |
1042 } |
983 } |
1043 master->stats.corrupted++; |
984 master->stats.corrupted++; |
1044 ec_master_output_stats(master); |
985 ec_master_output_stats(master); |
1051 frame_size = EC_READ_U16(cur_data) & 0x07FF; |
992 frame_size = EC_READ_U16(cur_data) & 0x07FF; |
1052 cur_data += EC_FRAME_HEADER_SIZE; |
993 cur_data += EC_FRAME_HEADER_SIZE; |
1053 |
994 |
1054 if (unlikely(frame_size > size)) { |
995 if (unlikely(frame_size > size)) { |
1055 if (master->debug_level) { |
996 if (master->debug_level) { |
1056 EC_DBG("Corrupted frame received (invalid frame size %zu for " |
997 EC_MASTER_DBG(master, 0, "Corrupted frame received" |
|
998 " (invalid frame size %zu for " |
1057 "received size %zu):\n", frame_size, size); |
999 "received size %zu):\n", frame_size, size); |
1058 ec_print_data(frame_data, size); |
1000 ec_print_data(frame_data, size); |
1059 } |
1001 } |
1060 master->stats.corrupted++; |
1002 master->stats.corrupted++; |
1061 ec_master_output_stats(master); |
1003 ec_master_output_stats(master); |
1072 cur_data += EC_DATAGRAM_HEADER_SIZE; |
1014 cur_data += EC_DATAGRAM_HEADER_SIZE; |
1073 |
1015 |
1074 if (unlikely(cur_data - frame_data |
1016 if (unlikely(cur_data - frame_data |
1075 + data_size + EC_DATAGRAM_FOOTER_SIZE > size)) { |
1017 + data_size + EC_DATAGRAM_FOOTER_SIZE > size)) { |
1076 if (master->debug_level) { |
1018 if (master->debug_level) { |
1077 EC_DBG("Corrupted frame received (invalid data size %zu):\n", |
1019 EC_MASTER_DBG(master, 0, "Corrupted frame received" |
1078 data_size); |
1020 " (invalid data size %zu):\n", data_size); |
1079 ec_print_data(frame_data, size); |
1021 ec_print_data(frame_data, size); |
1080 } |
1022 } |
1081 master->stats.corrupted++; |
1023 master->stats.corrupted++; |
1082 ec_master_output_stats(master); |
1024 ec_master_output_stats(master); |
1083 return; |
1025 return; |
1099 if (!matched) { |
1041 if (!matched) { |
1100 master->stats.unmatched++; |
1042 master->stats.unmatched++; |
1101 ec_master_output_stats(master); |
1043 ec_master_output_stats(master); |
1102 |
1044 |
1103 if (unlikely(master->debug_level > 0)) { |
1045 if (unlikely(master->debug_level > 0)) { |
1104 EC_DBG("UNMATCHED datagram:\n"); |
1046 EC_MASTER_DBG(master, 0, "UNMATCHED datagram:\n"); |
1105 ec_print_data(cur_data - EC_DATAGRAM_HEADER_SIZE, |
1047 ec_print_data(cur_data - EC_DATAGRAM_HEADER_SIZE, |
1106 EC_DATAGRAM_HEADER_SIZE + data_size |
1048 EC_DATAGRAM_HEADER_SIZE + data_size |
1107 + EC_DATAGRAM_FOOTER_SIZE); |
1049 + EC_DATAGRAM_FOOTER_SIZE); |
1108 #ifdef EC_DEBUG_RING |
1050 #ifdef EC_DEBUG_RING |
1109 ec_device_debug_ring_print(&master->main_device); |
1051 ec_device_debug_ring_print(&master->main_device); |
1143 { |
1085 { |
1144 if (unlikely(jiffies - master->stats.output_jiffies >= HZ)) { |
1086 if (unlikely(jiffies - master->stats.output_jiffies >= HZ)) { |
1145 master->stats.output_jiffies = jiffies; |
1087 master->stats.output_jiffies = jiffies; |
1146 |
1088 |
1147 if (master->stats.timeouts) { |
1089 if (master->stats.timeouts) { |
1148 EC_WARN("%u datagram%s TIMED OUT!\n", master->stats.timeouts, |
1090 EC_MASTER_WARN(master, "%u datagram%s TIMED OUT!\n", |
|
1091 master->stats.timeouts, |
1149 master->stats.timeouts == 1 ? "" : "s"); |
1092 master->stats.timeouts == 1 ? "" : "s"); |
1150 master->stats.timeouts = 0; |
1093 master->stats.timeouts = 0; |
1151 } |
1094 } |
1152 if (master->stats.corrupted) { |
1095 if (master->stats.corrupted) { |
1153 EC_WARN("%u frame%s CORRUPTED!\n", master->stats.corrupted, |
1096 EC_MASTER_WARN(master, "%u frame%s CORRUPTED!\n", |
|
1097 master->stats.corrupted, |
1154 master->stats.corrupted == 1 ? "" : "s"); |
1098 master->stats.corrupted == 1 ? "" : "s"); |
1155 master->stats.corrupted = 0; |
1099 master->stats.corrupted = 0; |
1156 } |
1100 } |
1157 if (master->stats.unmatched) { |
1101 if (master->stats.unmatched) { |
1158 EC_WARN("%u datagram%s UNMATCHED!\n", master->stats.unmatched, |
1102 EC_MASTER_WARN(master, "%u datagram%s UNMATCHED!\n", |
|
1103 master->stats.unmatched, |
1159 master->stats.unmatched == 1 ? "" : "s"); |
1104 master->stats.unmatched == 1 ? "" : "s"); |
1160 master->stats.unmatched = 0; |
1105 master->stats.unmatched = 0; |
1161 } |
1106 } |
1162 } |
1107 } |
1163 } |
1108 } |
1318 { |
1261 { |
1319 ec_master_t *master = (ec_master_t *) priv_data; |
1262 ec_master_t *master = (ec_master_t *) priv_data; |
1320 ec_slave_t *slave = NULL; |
1263 ec_slave_t *slave = NULL; |
1321 int fsm_exec; |
1264 int fsm_exec; |
1322 |
1265 |
1323 if (master->debug_level) |
1266 EC_MASTER_DBG(master, 1, "Operation thread running" |
1324 EC_DBG("Operation thread running with fsm interval = %d us," |
1267 " with fsm interval = %d us, max data size=%d\n", |
1325 " max data size=%d\n", |
1268 master->send_interval, master->max_queue_size); |
1326 master->send_interval, |
|
1327 master->max_queue_size); |
|
1328 |
1269 |
1329 while (!kthread_should_stop()) { |
1270 while (!kthread_should_stop()) { |
1330 ec_datagram_output_stats(&master->fsm_datagram); |
1271 ec_datagram_output_stats(&master->fsm_datagram); |
1331 |
1272 |
1332 if (master->injection_seq_rt == master->injection_seq_fsm) { |
1273 if (master->injection_seq_rt == master->injection_seq_fsm) { |
1377 void ec_master_eoe_start(ec_master_t *master /**< EtherCAT master */) |
1317 void ec_master_eoe_start(ec_master_t *master /**< EtherCAT master */) |
1378 { |
1318 { |
1379 struct sched_param param = { .sched_priority = 0 }; |
1319 struct sched_param param = { .sched_priority = 0 }; |
1380 |
1320 |
1381 if (master->eoe_thread) { |
1321 if (master->eoe_thread) { |
1382 EC_WARN("EoE already running!\n"); |
1322 EC_MASTER_WARN(master, "EoE already running!\n"); |
1383 return; |
1323 return; |
1384 } |
1324 } |
1385 |
1325 |
1386 if (list_empty(&master->eoe_handlers)) |
1326 if (list_empty(&master->eoe_handlers)) |
1387 return; |
1327 return; |
1388 |
1328 |
1389 if (!master->send_cb || !master->receive_cb) { |
1329 if (!master->send_cb || !master->receive_cb) { |
1390 EC_WARN("No EoE processing because of missing callbacks!\n"); |
1330 EC_MASTER_WARN(master, "No EoE processing" |
|
1331 " because of missing callbacks!\n"); |
1391 return; |
1332 return; |
1392 } |
1333 } |
1393 |
1334 |
1394 EC_INFO("Starting EoE thread.\n"); |
1335 EC_MASTER_INFO(master, "Starting EoE thread.\n"); |
1395 master->eoe_thread = kthread_run(ec_master_eoe_thread, master, |
1336 master->eoe_thread = kthread_run(ec_master_eoe_thread, master, |
1396 "EtherCAT-EoE"); |
1337 "EtherCAT-EoE"); |
1397 if (IS_ERR(master->eoe_thread)) { |
1338 if (IS_ERR(master->eoe_thread)) { |
1398 int err = (int) PTR_ERR(master->eoe_thread); |
1339 int err = (int) PTR_ERR(master->eoe_thread); |
1399 EC_ERR("Failed to start EoE thread (error %i)!\n", err); |
1340 EC_MASTER_ERR(master, "Failed to start EoE thread (error %i)!\n", |
|
1341 err); |
1400 master->eoe_thread = NULL; |
1342 master->eoe_thread = NULL; |
1401 return; |
1343 return; |
1402 } |
1344 } |
1403 |
1345 |
1404 sched_setscheduler(master->eoe_thread, SCHED_NORMAL, ¶m); |
1346 sched_setscheduler(master->eoe_thread, SCHED_NORMAL, ¶m); |
2180 { |
2119 { |
2181 ec_slave_config_t *sc; |
2120 ec_slave_config_t *sc; |
2182 unsigned int found = 0; |
2121 unsigned int found = 0; |
2183 |
2122 |
2184 |
2123 |
2185 if (master->debug_level) |
2124 EC_MASTER_DBG(master, 1, "ecrt_master_slave_config(master = 0x%p," |
2186 EC_DBG("ecrt_master_slave_config(master = 0x%p, alias = %u, " |
2125 " alias = %u, position = %u, vendor_id = 0x%08x," |
2187 "position = %u, vendor_id = 0x%08x, product_code = 0x%08x)\n", |
2126 " product_code = 0x%08x)\n", |
2188 master, alias, position, vendor_id, product_code); |
2127 master, alias, position, vendor_id, product_code); |
2189 |
2128 |
2190 list_for_each_entry(sc, &master->configs, list) { |
2129 list_for_each_entry(sc, &master->configs, list) { |
2191 if (sc->alias == alias && sc->position == position) { |
2130 if (sc->alias == alias && sc->position == position) { |
2192 found = 1; |
2131 found = 1; |
2193 break; |
2132 break; |
2194 } |
2133 } |
2195 } |
2134 } |
2196 |
2135 |
2197 if (found) { // config with same alias/position already existing |
2136 if (found) { // config with same alias/position already existing |
2198 if (sc->vendor_id != vendor_id || sc->product_code != product_code) { |
2137 if (sc->vendor_id != vendor_id || sc->product_code != product_code) { |
2199 EC_ERR("Slave type mismatch. Slave was configured as" |
2138 EC_MASTER_ERR(master, "Slave type mismatch. Slave was" |
2200 " 0x%08X/0x%08X before. Now configuring with" |
2139 " configured as 0x%08X/0x%08X before. Now configuring" |
2201 " 0x%08X/0x%08X.\n", sc->vendor_id, sc->product_code, |
2140 " with 0x%08X/0x%08X.\n", sc->vendor_id, sc->product_code, |
2202 vendor_id, product_code); |
2141 vendor_id, product_code); |
2203 return ERR_PTR(-ENOENT); |
2142 return ERR_PTR(-ENOENT); |
2204 } |
2143 } |
2205 } else { |
2144 } else { |
2206 if (master->debug_level) |
2145 EC_MASTER_DBG(master, 1, "Creating slave configuration for %u:%u," |
2207 EC_DBG("Creating slave configuration for %u:%u, 0x%08X/0x%08X.\n", |
2146 " 0x%08X/0x%08X.\n", |
2208 alias, position, vendor_id, product_code); |
2147 alias, position, vendor_id, product_code); |
2209 |
2148 |
2210 if (!(sc = (ec_slave_config_t *) kmalloc(sizeof(ec_slave_config_t), |
2149 if (!(sc = (ec_slave_config_t *) kmalloc(sizeof(ec_slave_config_t), |
2211 GFP_KERNEL))) { |
2150 GFP_KERNEL))) { |
2212 EC_ERR("Failed to allocate memory for slave configuration.\n"); |
2151 EC_MASTER_ERR(master, "Failed to allocate memory" |
|
2152 " for slave configuration.\n"); |
2213 return ERR_PTR(-ENOMEM); |
2153 return ERR_PTR(-ENOMEM); |
2214 } |
2154 } |
2215 |
2155 |
2216 ec_slave_config_init(sc, master, |
2156 ec_slave_config_init(sc, master, |
2217 alias, position, vendor_id, product_code); |
2157 alias, position, vendor_id, product_code); |
2242 |
2182 |
2243 /*****************************************************************************/ |
2183 /*****************************************************************************/ |
2244 |
2184 |
2245 int ecrt_master(ec_master_t *master, ec_master_info_t *master_info) |
2185 int ecrt_master(ec_master_t *master, ec_master_info_t *master_info) |
2246 { |
2186 { |
2247 if (master->debug_level) |
2187 EC_MASTER_DBG(master, 1, "ecrt_master(master = 0x%p," |
2248 EC_DBG("ecrt_master(master = 0x%p, master_info = 0x%p)\n", |
2188 " master_info = 0x%p)\n", master, master_info); |
2249 master, master_info); |
|
2250 |
2189 |
2251 master_info->slave_count = master->slave_count; |
2190 master_info->slave_count = master->slave_count; |
2252 master_info->link_up = master->main_device.link_state; |
2191 master_info->link_up = master->main_device.link_state; |
2253 master_info->scan_busy = master->scan_busy; |
2192 master_info->scan_busy = master->scan_busy; |
2254 master_info->app_time = master->app_time; |
2193 master_info->app_time = master->app_time; |
2293 /*****************************************************************************/ |
2232 /*****************************************************************************/ |
2294 |
2233 |
2295 void ecrt_master_callbacks(ec_master_t *master, |
2234 void ecrt_master_callbacks(ec_master_t *master, |
2296 void (*send_cb)(void *), void (*receive_cb)(void *), void *cb_data) |
2235 void (*send_cb)(void *), void (*receive_cb)(void *), void *cb_data) |
2297 { |
2236 { |
2298 if (master->debug_level) |
2237 EC_MASTER_DBG(master, 1, "ecrt_master_callbacks(master = 0x%p," |
2299 EC_DBG("ecrt_master_callbacks(master = 0x%p, send_cb = 0x%p, " |
2238 " send_cb = 0x%p, receive_cb = 0x%p, cb_data = 0x%p)\n", |
2300 " receive_cb = 0x%p, cb_data = 0x%p)\n", master, |
2239 master, send_cb, receive_cb, cb_data); |
2301 send_cb, receive_cb, cb_data); |
|
2302 |
2240 |
2303 master->app_send_cb = send_cb; |
2241 master->app_send_cb = send_cb; |
2304 master->app_receive_cb = receive_cb; |
2242 master->app_receive_cb = receive_cb; |
2305 master->app_cb_data = cb_data; |
2243 master->app_cb_data = cb_data; |
2306 } |
2244 } |