371 master->dc_ref_clock = NULL; |
373 master->dc_ref_clock = NULL; |
372 |
374 |
373 // external requests are obsolete, so we wake pending waiters and remove |
375 // external requests are obsolete, so we wake pending waiters and remove |
374 // them from the list |
376 // them from the list |
375 // |
377 // |
376 // SII requests |
378 // SII requests |
377 while (1) { |
379 while (1) { |
378 ec_sii_write_request_t *request; |
380 ec_sii_write_request_t *request; |
379 if (list_empty(&master->sii_requests)) |
381 if (list_empty(&master->sii_requests)) |
380 break; |
382 break; |
381 // get first request |
383 // get first request |
382 request = list_entry(master->sii_requests.next, |
384 request = list_entry(master->sii_requests.next, |
383 ec_sii_write_request_t, list); |
385 ec_sii_write_request_t, list); |
384 list_del_init(&request->list); // dequeue |
386 list_del_init(&request->list); // dequeue |
385 EC_INFO("Discarding SII request, slave %u does not exist anymore.\n", |
387 EC_INFO("Discarding SII request, slave %u does not exist anymore.\n", |
386 request->slave->ring_position); |
388 request->slave->ring_position); |
387 request->state = EC_INT_REQUEST_FAILURE; |
389 request->state = EC_INT_REQUEST_FAILURE; |
388 wake_up(&master->sii_queue); |
390 wake_up(&master->sii_queue); |
389 } |
391 } |
390 |
392 |
391 // Register requests |
393 // Register requests |
392 while (1) { |
394 while (1) { |
393 ec_reg_request_t *request; |
395 ec_reg_request_t *request; |
394 if (list_empty(&master->reg_requests)) |
396 if (list_empty(&master->reg_requests)) |
395 break; |
397 break; |
396 // get first request |
398 // get first request |
397 request = list_entry(master->reg_requests.next, |
399 request = list_entry(master->reg_requests.next, |
398 ec_reg_request_t, list); |
400 ec_reg_request_t, list); |
399 list_del_init(&request->list); // dequeue |
401 list_del_init(&request->list); // dequeue |
400 EC_INFO("Discarding Reg request, slave %u does not exist anymore.\n", |
402 EC_INFO("Discarding Reg request, slave %u does not exist anymore.\n", |
401 request->slave->ring_position); |
403 request->slave->ring_position); |
402 request->state = EC_INT_REQUEST_FAILURE; |
404 request->state = EC_INT_REQUEST_FAILURE; |
403 wake_up(&master->reg_queue); |
405 wake_up(&master->reg_queue); |
404 } |
406 } |
405 |
407 |
406 for (slave = master->slaves; |
408 for (slave = master->slaves; |
407 slave < master->slaves + master->slave_count; |
409 slave < master->slaves + master->slave_count; |
408 slave++) { |
410 slave++) { |
409 // SDO requests |
411 // SDO requests |
690 /*****************************************************************************/ |
692 /*****************************************************************************/ |
691 |
693 |
692 /** Injects external datagrams that fit into the datagram queue |
694 /** Injects external datagrams that fit into the datagram queue |
693 */ |
695 */ |
694 void ec_master_inject_external_datagrams( |
696 void ec_master_inject_external_datagrams( |
695 ec_master_t *master /**< EtherCAT master */ |
697 ec_master_t *master /**< EtherCAT master */ |
696 ) |
698 ) |
697 { |
699 { |
698 ec_datagram_t *datagram, *n; |
700 ec_datagram_t *datagram, *n; |
699 size_t queue_size = 0; |
701 size_t queue_size = 0; |
700 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
702 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
701 queue_size += datagram->data_size; |
703 queue_size += datagram->data_size; |
702 } |
704 } |
703 list_for_each_entry_safe(datagram, n, &master->external_datagram_queue, queue) { |
705 list_for_each_entry_safe(datagram, n, &master->external_datagram_queue, queue) { |
704 queue_size += datagram->data_size; |
706 queue_size += datagram->data_size; |
705 if (queue_size <= master->max_queue_size) { |
707 if (queue_size <= master->max_queue_size) { |
706 list_del_init(&datagram->queue); |
708 list_del_init(&datagram->queue); |
707 #if DEBUG_INJECT |
709 #if DEBUG_INJECT |
708 if (master->debug_level) { |
710 if (master->debug_level) { |
709 EC_DBG("Injecting external datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); |
711 EC_DBG("Injecting external datagram %08x size=%u," |
710 } |
712 " queue_size=%u\n", (unsigned int) datagram, |
|
713 datagram->data_size, queue_size); |
|
714 } |
711 #endif |
715 #endif |
712 #ifdef EC_HAVE_CYCLES |
716 #ifdef EC_HAVE_CYCLES |
713 datagram->cycles_sent = 0; |
717 datagram->cycles_sent = 0; |
714 #endif |
718 #endif |
715 datagram->jiffies_sent = 0; |
719 datagram->jiffies_sent = 0; |
716 ec_master_queue_datagram(master, datagram); |
720 ec_master_queue_datagram(master, datagram); |
717 } |
721 } |
718 else { |
722 else { |
719 if (datagram->data_size > master->max_queue_size) { |
723 if (datagram->data_size > master->max_queue_size) { |
720 list_del_init(&datagram->queue); |
724 list_del_init(&datagram->queue); |
721 datagram->state = EC_DATAGRAM_ERROR; |
725 datagram->state = EC_DATAGRAM_ERROR; |
722 EC_ERR("External datagram %08x is too large, size=%u, max_queue_size=%u\n",(unsigned int)datagram,datagram->data_size,master->max_queue_size); |
726 EC_ERR("External datagram %08x is too large, size=%u, max_queue_size=%u\n",(unsigned int)datagram,datagram->data_size,master->max_queue_size); |
723 } |
727 } |
724 else { |
728 else { |
725 #ifdef EC_HAVE_CYCLES |
729 #ifdef EC_HAVE_CYCLES |
726 cycles_t cycles_now = get_cycles(); |
730 cycles_t cycles_now = get_cycles(); |
727 if (cycles_now - datagram->cycles_sent |
731 if (cycles_now - datagram->cycles_sent |
728 > sdo_injection_timeout_cycles) { |
732 > sdo_injection_timeout_cycles) { |
729 #else |
733 #else |
730 if (jiffies - datagram->jiffies_sent |
734 if (jiffies - datagram->jiffies_sent |
731 > sdo_injection_timeout_jiffies) { |
735 > sdo_injection_timeout_jiffies) { |
732 #endif |
736 #endif |
733 unsigned int time_us; |
737 unsigned int time_us; |
734 list_del_init(&datagram->queue); |
738 list_del_init(&datagram->queue); |
735 datagram->state = EC_DATAGRAM_ERROR; |
739 datagram->state = EC_DATAGRAM_ERROR; |
736 #ifdef EC_HAVE_CYCLES |
740 #ifdef EC_HAVE_CYCLES |
737 time_us = (unsigned int) ((cycles_now - datagram->cycles_sent) * 1000LL) / cpu_khz; |
741 time_us = (unsigned int) ((cycles_now - datagram->cycles_sent) * 1000LL) / cpu_khz; |
738 #else |
742 #else |
739 time_us = (unsigned int) ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); |
743 time_us = (unsigned int) ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); |
740 #endif |
744 #endif |
741 EC_ERR("Timeout %u us: injecting external datagram %08x size=%u, max_queue_size=%u\n",time_us,(unsigned int)datagram,datagram->data_size,master->max_queue_size); |
745 EC_ERR("Timeout %u us: injecting external datagram %08x size=%u, max_queue_size=%u\n",time_us,(unsigned int)datagram,datagram->data_size,master->max_queue_size); |
742 } |
746 } |
743 else { |
747 else { |
744 #if DEBUG_INJECT |
748 #if DEBUG_INJECT |
745 if (master->debug_level) { |
749 if (master->debug_level) { |
746 EC_DBG("Deferred injecting of external datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); |
750 EC_DBG("Deferred injecting of external datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); |
747 } |
751 } |
748 #endif |
752 #endif |
749 } |
753 } |
750 } |
754 } |
751 } |
755 } |
752 } |
756 } |
753 } |
757 } |
754 |
758 |
755 /*****************************************************************************/ |
759 /*****************************************************************************/ |
756 |
760 |
757 /** sets the expected interval between calls to ecrt_master_send |
761 /** sets the expected interval between calls to ecrt_master_send |
758 and calculates the maximum amount of data to queue |
762 and calculates the maximum amount of data to queue |
759 */ |
763 */ |
760 void ec_master_set_send_interval( |
764 void ec_master_set_send_interval( |
761 ec_master_t *master, /**< EtherCAT master */ |
765 ec_master_t *master, /**< EtherCAT master */ |
762 size_t send_interval /**< send interval */ |
766 size_t send_interval /**< send interval */ |
763 ) |
767 ) |
764 { |
768 { |
765 master->send_interval = send_interval; |
769 master->send_interval = send_interval; |
766 master->max_queue_size = (send_interval * 1000) / EC_BYTE_TRANSMITION_TIME; |
770 master->max_queue_size = (send_interval * 1000) / EC_BYTE_TRANSMITION_TIME; |
767 master->max_queue_size -= master->max_queue_size / 10; |
771 master->max_queue_size -= master->max_queue_size / 10; |
768 } |
772 } |
769 |
773 |
770 |
774 |
771 /*****************************************************************************/ |
775 /*****************************************************************************/ |
772 |
776 |
1114 } |
1118 } |
1115 } |
1119 } |
1116 |
1120 |
1117 |
1121 |
1118 /*****************************************************************************/ |
1122 /*****************************************************************************/ |
|
1123 |
|
1124 #ifdef EC_USE_HRTIMER |
|
1125 |
1119 /* |
1126 /* |
1120 * Sleep related functions: |
1127 * Sleep related functions: |
1121 */ |
1128 */ |
1122 static enum hrtimer_restart ec_master_nanosleep_wakeup(struct hrtimer *timer) |
1129 static enum hrtimer_restart ec_master_nanosleep_wakeup(struct hrtimer *timer) |
1123 { |
1130 { |
1124 struct hrtimer_sleeper *t = |
1131 struct hrtimer_sleeper *t = |
1125 container_of(timer, struct hrtimer_sleeper, timer); |
1132 container_of(timer, struct hrtimer_sleeper, timer); |
1126 struct task_struct *task = t->task; |
1133 struct task_struct *task = t->task; |
1127 |
1134 |
1128 t->task = NULL; |
1135 t->task = NULL; |
1129 if (task) |
1136 if (task) |
1130 wake_up_process(task); |
1137 wake_up_process(task); |
1131 |
1138 |
1132 return HRTIMER_NORESTART; |
1139 return HRTIMER_NORESTART; |
1133 } |
1140 } |
|
1141 |
|
1142 /*****************************************************************************/ |
1134 |
1143 |
1135 #if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28) |
1144 #if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28) |
|
1145 |
1136 /* compatibility with new hrtimer interface */ |
1146 /* compatibility with new hrtimer interface */ |
1137 static inline ktime_t hrtimer_get_expires(const struct hrtimer *timer) |
1147 static inline ktime_t hrtimer_get_expires(const struct hrtimer *timer) |
1138 { |
1148 { |
1139 return timer->expires; |
1149 return timer->expires; |
1140 } |
1150 } |
|
1151 |
|
1152 /*****************************************************************************/ |
1141 |
1153 |
1142 static inline void hrtimer_set_expires(struct hrtimer *timer, ktime_t time) |
1154 static inline void hrtimer_set_expires(struct hrtimer *timer, ktime_t time) |
1143 { |
1155 { |
1144 timer->expires = time; |
1156 timer->expires = time; |
1145 } |
1157 } |
1146 #endif |
1158 |
1147 |
1159 #endif |
|
1160 |
|
1161 /*****************************************************************************/ |
1148 |
1162 |
1149 void ec_master_nanosleep(const unsigned long nsecs) |
1163 void ec_master_nanosleep(const unsigned long nsecs) |
1150 { |
1164 { |
1151 struct hrtimer_sleeper t; |
1165 struct hrtimer_sleeper t; |
1152 enum hrtimer_mode mode = HRTIMER_MODE_REL; |
1166 enum hrtimer_mode mode = HRTIMER_MODE_REL; |
1153 hrtimer_init(&t.timer, CLOCK_MONOTONIC,mode); |
1167 |
1154 t.timer.function = ec_master_nanosleep_wakeup; |
1168 hrtimer_init(&t.timer, CLOCK_MONOTONIC, mode); |
1155 t.task = current; |
1169 t.timer.function = ec_master_nanosleep_wakeup; |
|
1170 t.task = current; |
1156 #ifdef CONFIG_HIGH_RES_TIMERS |
1171 #ifdef CONFIG_HIGH_RES_TIMERS |
1157 #if LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 24) |
1172 #if LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 24) |
1158 t.timer.cb_mode = HRTIMER_CB_IRQSAFE_NO_RESTART; |
1173 t.timer.cb_mode = HRTIMER_CB_IRQSAFE_NO_RESTART; |
1159 #elif LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 26) |
1174 #elif LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 26) |
1160 t.timer.cb_mode = HRTIMER_CB_IRQSAFE_NO_SOFTIRQ; |
1175 t.timer.cb_mode = HRTIMER_CB_IRQSAFE_NO_SOFTIRQ; |
1161 #elif LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 28) |
1176 #elif LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 28) |
1162 t.timer.cb_mode = HRTIMER_CB_IRQSAFE_UNLOCKED; |
1177 t.timer.cb_mode = HRTIMER_CB_IRQSAFE_UNLOCKED; |
1163 #endif |
1178 #endif |
1164 #endif |
1179 #endif |
1165 hrtimer_set_expires(&t.timer, ktime_set(0,nsecs)); |
1180 hrtimer_set_expires(&t.timer, ktime_set(0, nsecs)); |
1166 do { |
1181 |
1167 set_current_state(TASK_INTERRUPTIBLE); |
1182 do { |
1168 hrtimer_start(&t.timer, hrtimer_get_expires(&t.timer), mode); |
1183 set_current_state(TASK_INTERRUPTIBLE); |
1169 |
1184 hrtimer_start(&t.timer, hrtimer_get_expires(&t.timer), mode); |
1170 if (likely(t.task)) |
1185 |
1171 schedule(); |
1186 if (likely(t.task)) |
1172 |
1187 schedule(); |
1173 hrtimer_cancel(&t.timer); |
1188 |
1174 mode = HRTIMER_MODE_ABS; |
1189 hrtimer_cancel(&t.timer); |
1175 |
1190 mode = HRTIMER_MODE_ABS; |
1176 } while (t.task && !signal_pending(current)); |
1191 |
1177 } |
1192 } while (t.task && !signal_pending(current)); |
1178 |
1193 } |
|
1194 |
|
1195 #endif // EC_USE_HRTIMER |
1179 |
1196 |
1180 /*****************************************************************************/ |
1197 /*****************************************************************************/ |
1181 |
1198 |
1182 /** Master kernel thread function for IDLE phase. |
1199 /** Master kernel thread function for IDLE phase. |
1183 */ |
1200 */ |
1239 static int ec_master_operation_thread(void *priv_data) |
1266 static int ec_master_operation_thread(void *priv_data) |
1240 { |
1267 { |
1241 ec_master_t *master = (ec_master_t *) priv_data; |
1268 ec_master_t *master = (ec_master_t *) priv_data; |
1242 ec_slave_t *slave = NULL; |
1269 ec_slave_t *slave = NULL; |
1243 int fsm_exec; |
1270 int fsm_exec; |
|
1271 |
1244 if (master->debug_level) |
1272 if (master->debug_level) |
1245 EC_DBG("Operation thread running with fsm interval = %d us, max data size=%d\n",master->send_interval,master->max_queue_size); |
1273 EC_DBG("Operation thread running with fsm interval = %d us," |
|
1274 " max data size=%d\n", |
|
1275 master->send_interval, |
|
1276 master->max_queue_size); |
1246 |
1277 |
1247 while (!kthread_should_stop()) { |
1278 while (!kthread_should_stop()) { |
1248 ec_datagram_output_stats(&master->fsm_datagram); |
1279 ec_datagram_output_stats(&master->fsm_datagram); |
|
1280 |
1249 if (master->injection_seq_rt == master->injection_seq_fsm) { |
1281 if (master->injection_seq_rt == master->injection_seq_fsm) { |
1250 // output statistics |
1282 // output statistics |
1251 ec_master_output_stats(master); |
1283 ec_master_output_stats(master); |
1252 |
1284 |
1253 fsm_exec = 0; |
1285 fsm_exec = 0; |