85 */ |
86 */ |
86 void ec_master_init_static(void) |
87 void ec_master_init_static(void) |
87 { |
88 { |
88 #ifdef EC_HAVE_CYCLES |
89 #ifdef EC_HAVE_CYCLES |
89 timeout_cycles = (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000); |
90 timeout_cycles = (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000); |
|
91 sdo_injection_timeout_cycles = (cycles_t) EC_SDO_INJECTION_TIMEOUT /* us */ * (cpu_khz / 1000); |
90 #else |
92 #else |
91 // one jiffy may always elapse between time measurement |
93 // one jiffy may always elapse between time measurement |
92 timeout_jiffies = max(EC_IO_TIMEOUT * HZ / 1000000, 1); |
94 timeout_jiffies = max(EC_IO_TIMEOUT * HZ / 1000000, 1); |
|
95 sdo_injection_timeout_jiffies = max(EC_SDO_INJECTION_TIMEOUT * HZ / 1000000, 1); |
93 #endif |
96 #endif |
94 } |
97 } |
95 |
98 |
96 /*****************************************************************************/ |
99 /*****************************************************************************/ |
97 |
100 |
148 INIT_LIST_HEAD(&master->datagram_queue); |
151 INIT_LIST_HEAD(&master->datagram_queue); |
149 master->datagram_index = 0; |
152 master->datagram_index = 0; |
150 |
153 |
151 INIT_LIST_HEAD(&master->ext_datagram_queue); |
154 INIT_LIST_HEAD(&master->ext_datagram_queue); |
152 sema_init(&master->ext_queue_sem, 1); |
155 sema_init(&master->ext_queue_sem, 1); |
|
156 |
|
157 INIT_LIST_HEAD(&master->sdo_datagram_queue); |
|
158 master->max_queue_size = EC_MAX_DATA_SIZE; |
153 |
159 |
154 INIT_LIST_HEAD(&master->domains); |
160 INIT_LIST_HEAD(&master->domains); |
155 |
161 |
156 master->debug_level = debug_level; |
162 master->debug_level = debug_level; |
157 master->stats.timeouts = 0; |
163 master->stats.timeouts = 0; |
682 EC_DBG("OPERATION -> IDLE.\n"); |
688 EC_DBG("OPERATION -> IDLE.\n"); |
683 |
689 |
684 master->phase = EC_IDLE; |
690 master->phase = EC_IDLE; |
685 } |
691 } |
686 |
692 |
|
693 |
|
694 /*****************************************************************************/ |
|
695 |
|
696 /** Injects sdo datagrams that fit into the datagram queue |
|
697 */ |
|
698 void ec_master_inject_sdo_datagrams( |
|
699 ec_master_t *master /**< EtherCAT master */ |
|
700 ) |
|
701 { |
|
702 ec_datagram_t *datagram, *n; |
|
703 size_t queue_size = 0; |
|
704 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
|
705 queue_size += datagram->data_size; |
|
706 } |
|
707 list_for_each_entry_safe(datagram, n, &master->sdo_datagram_queue, queue) { |
|
708 queue_size += datagram->data_size; |
|
709 if (queue_size <= master->max_queue_size) { |
|
710 list_del_init(&datagram->queue); |
|
711 if (master->debug_level) { |
|
712 EC_DBG("Injecting SDO datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); |
|
713 } |
|
714 #ifdef EC_HAVE_CYCLES |
|
715 datagram->cycles_sent = 0; |
|
716 #endif |
|
717 datagram->jiffies_sent = 0; |
|
718 ec_master_queue_datagram(master, datagram); |
|
719 } |
|
720 else { |
|
721 if (datagram->data_size > master->max_queue_size) { |
|
722 list_del_init(&datagram->queue); |
|
723 datagram->state = EC_DATAGRAM_ERROR; |
|
724 EC_ERR("SDO datagram %08x is too large, size=%u, max_queue_size=%u\n",(unsigned int)datagram,datagram->data_size,master->max_queue_size); |
|
725 } |
|
726 else { |
|
727 #ifdef EC_HAVE_CYCLES |
|
728 cycles_t cycles_now = get_cycles(); |
|
729 if (cycles_now - datagram->cycles_sent |
|
730 > sdo_injection_timeout_cycles) { |
|
731 #else |
|
732 if (jiffies - datagram->jiffies_sent |
|
733 > sdo_injection_timeout_jiffies) { |
|
734 #endif |
|
735 unsigned int time_us; |
|
736 list_del_init(&datagram->queue); |
|
737 datagram->state = EC_DATAGRAM_ERROR; |
|
738 #ifdef EC_HAVE_CYCLES |
|
739 time_us = (unsigned int) ((cycles_now - datagram->cycles_sent) * 1000LL) / cpu_khz; |
|
740 #else |
|
741 time_us = (unsigned int) ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); |
|
742 #endif |
|
743 EC_ERR("Timeout %u us: injecting SDO datagram %08x size=%u, max_queue_size=%u\n",time_us,(unsigned int)datagram,datagram->data_size,master->max_queue_size); |
|
744 } |
|
745 else { |
|
746 if (master->debug_level) { |
|
747 EC_DBG("Deferred injecting of SDO datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); |
|
748 } |
|
749 } |
|
750 } |
|
751 } |
|
752 } |
|
753 } |
|
754 |
|
755 /*****************************************************************************/ |
|
756 |
|
757 /** Places a sdo datagram in the sdo datagram queue. |
|
758 */ |
|
759 void ec_master_queue_sdo_datagram( |
|
760 ec_master_t *master, /**< EtherCAT master */ |
|
761 ec_datagram_t *datagram /**< datagram */ |
|
762 ) |
|
763 { |
|
764 if (master->debug_level) { |
|
765 EC_DBG("Requesting SDO datagram %08x size=%u\n",(unsigned int)datagram,datagram->data_size); |
|
766 } |
|
767 datagram->state = EC_DATAGRAM_QUEUED; |
|
768 #ifdef EC_HAVE_CYCLES |
|
769 datagram->cycles_sent = get_cycles(); |
|
770 #endif |
|
771 datagram->jiffies_sent = jiffies; |
|
772 |
|
773 down(&master->io_sem); |
|
774 list_add_tail(&datagram->queue, &master->sdo_datagram_queue); |
|
775 up(&master->io_sem); |
|
776 } |
|
777 |
687 /*****************************************************************************/ |
778 /*****************************************************************************/ |
688 |
779 |
689 /** Places a datagram in the datagram queue. |
780 /** Places a datagram in the datagram queue. |
690 */ |
781 */ |
691 void ec_master_queue_datagram( |
782 void ec_master_queue_datagram( |
693 ec_datagram_t *datagram /**< datagram */ |
784 ec_datagram_t *datagram /**< datagram */ |
694 ) |
785 ) |
695 { |
786 { |
696 ec_datagram_t *queued_datagram; |
787 ec_datagram_t *queued_datagram; |
697 |
788 |
|
789 if (datagram->state == EC_DATAGRAM_SENT) |
|
790 return; |
698 // check, if the datagram is already queued |
791 // check, if the datagram is already queued |
699 list_for_each_entry(queued_datagram, &master->datagram_queue, queue) { |
792 list_for_each_entry(queued_datagram, &master->datagram_queue, queue) { |
700 if (queued_datagram == datagram) { |
793 if (queued_datagram == datagram) { |
701 datagram->skip_count++; |
794 datagram->skip_count++; |
702 if (master->debug_level) |
795 if (master->debug_level) |
1011 // receive |
1105 // receive |
1012 down(&master->io_sem); |
1106 down(&master->io_sem); |
1013 ecrt_master_receive(master); |
1107 ecrt_master_receive(master); |
1014 up(&master->io_sem); |
1108 up(&master->io_sem); |
1015 |
1109 |
1016 if (master->fsm_datagram.state == EC_DATAGRAM_SENT) |
1110 fsm_exec = 0; |
1017 goto schedule; |
1111 // execute master & slave state machines |
1018 |
|
1019 // execute master state machine |
|
1020 if (down_interruptible(&master->master_sem)) |
1112 if (down_interruptible(&master->master_sem)) |
1021 break; |
1113 break; |
1022 ec_fsm_master_exec(&master->fsm); |
1114 fsm_exec = ec_fsm_master_exec(&master->fsm); |
|
1115 for (slave = master->slaves; |
|
1116 slave < master->slaves + master->slave_count; |
|
1117 slave++) { |
|
1118 ec_fsm_slave_exec(&slave->fsm); |
|
1119 } |
1023 up(&master->master_sem); |
1120 up(&master->master_sem); |
1024 |
1121 |
1025 // queue and send |
1122 // queue and send |
1026 down(&master->io_sem); |
1123 down(&master->io_sem); |
1027 ec_master_queue_datagram(master, &master->fsm_datagram); |
1124 if (fsm_exec) { |
|
1125 ec_master_queue_datagram(master, &master->fsm_datagram); |
|
1126 } |
|
1127 ec_master_inject_sdo_datagrams(master); |
1028 ecrt_master_send(master); |
1128 ecrt_master_send(master); |
1029 up(&master->io_sem); |
1129 up(&master->io_sem); |
1030 |
1130 |
1031 schedule: |
|
1032 if (ec_fsm_master_idle(&master->fsm)) { |
1131 if (ec_fsm_master_idle(&master->fsm)) { |
1033 set_current_state(TASK_INTERRUPTIBLE); |
1132 set_current_state(TASK_INTERRUPTIBLE); |
1034 schedule_timeout(1); |
1133 schedule_timeout(1); |
1035 } |
1134 } |
1036 else { |
1135 else { |
1043 return 0; |
1142 return 0; |
1044 } |
1143 } |
1045 |
1144 |
1046 /*****************************************************************************/ |
1145 /*****************************************************************************/ |
1047 |
1146 |
1048 /** Master kernel thread function for IDLE phase. |
1147 /** Master kernel thread function for OPERATION phase. |
1049 */ |
1148 */ |
1050 static int ec_master_operation_thread(void *priv_data) |
1149 static int ec_master_operation_thread(void *priv_data) |
1051 { |
1150 { |
1052 ec_master_t *master = (ec_master_t *) priv_data; |
1151 ec_master_t *master = (ec_master_t *) priv_data; |
1053 |
1152 ec_slave_t *slave = NULL; |
|
1153 int fsm_exec; |
1054 if (master->debug_level) |
1154 if (master->debug_level) |
1055 EC_DBG("Operation thread running.\n"); |
1155 EC_DBG("Operation thread running.\n"); |
1056 |
1156 |
1057 while (!kthread_should_stop()) { |
1157 while (!kthread_should_stop()) { |
1058 ec_datagram_output_stats(&master->fsm_datagram); |
1158 ec_datagram_output_stats(&master->fsm_datagram); |
1059 if (master->injection_seq_rt != master->injection_seq_fsm || |
1159 if (master->injection_seq_rt == master->injection_seq_fsm) { |
1060 master->fsm_datagram.state == EC_DATAGRAM_SENT || |
1160 // output statistics |
1061 master->fsm_datagram.state == EC_DATAGRAM_QUEUED) |
1161 ec_master_output_stats(master); |
1062 goto schedule; |
1162 |
1063 |
1163 fsm_exec = 0; |
1064 // output statistics |
1164 // execute master & slave state machines |
1065 ec_master_output_stats(master); |
1165 if (down_interruptible(&master->master_sem)) |
1066 |
1166 break; |
1067 // execute master state machine |
1167 fsm_exec += ec_fsm_master_exec(&master->fsm); |
1068 if (down_interruptible(&master->master_sem)) |
1168 for (slave = master->slaves; |
1069 break; |
1169 slave < master->slaves + master->slave_count; |
1070 ec_fsm_master_exec(&master->fsm); |
1170 slave++) { |
1071 up(&master->master_sem); |
1171 ec_fsm_slave_exec(&slave->fsm); |
1072 |
1172 } |
1073 // inject datagram |
1173 up(&master->master_sem); |
1074 master->injection_seq_fsm++; |
1174 |
1075 |
1175 // inject datagrams (let the rt thread queue them, see ecrt_master_send) |
1076 schedule: |
1176 if (fsm_exec) |
|
1177 master->injection_seq_fsm++; |
|
1178 } |
1077 if (ec_fsm_master_idle(&master->fsm)) { |
1179 if (ec_fsm_master_idle(&master->fsm)) { |
1078 set_current_state(TASK_INTERRUPTIBLE); |
1180 set_current_state(TASK_INTERRUPTIBLE); |
1079 schedule_timeout(1); |
1181 schedule_timeout(1); |
1080 } |
1182 } |
1081 else { |
1183 else { |
1795 void ecrt_master_send(ec_master_t *master) |
1897 void ecrt_master_send(ec_master_t *master) |
1796 { |
1898 { |
1797 ec_datagram_t *datagram, *n; |
1899 ec_datagram_t *datagram, *n; |
1798 |
1900 |
1799 if (master->injection_seq_rt != master->injection_seq_fsm) { |
1901 if (master->injection_seq_rt != master->injection_seq_fsm) { |
1800 // inject datagram produced by master FSM |
1902 // inject datagrams produced by master & slave FSMs |
1801 ec_master_queue_datagram(master, &master->fsm_datagram); |
1903 ec_master_queue_datagram(master, &master->fsm_datagram); |
1802 master->injection_seq_rt = master->injection_seq_fsm; |
1904 master->injection_seq_rt = master->injection_seq_fsm; |
1803 } |
1905 } |
|
1906 ec_master_inject_sdo_datagrams(master); |
1804 |
1907 |
1805 if (unlikely(!master->main_device.link_state)) { |
1908 if (unlikely(!master->main_device.link_state)) { |
1806 // link is down, no datagram can be sent |
1909 // link is down, no datagram can be sent |
1807 list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) { |
1910 list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) { |
1808 datagram->state = EC_DATAGRAM_ERROR; |
1911 datagram->state = EC_DATAGRAM_ERROR; |