690 /*****************************************************************************/ |
690 /*****************************************************************************/ |
691 |
691 |
692 /** Injects external datagrams that fit into the datagram queue |
692 /** Injects external datagrams that fit into the datagram queue |
693 */ |
693 */ |
694 void ec_master_inject_external_datagrams( |
694 void ec_master_inject_external_datagrams( |
695 ec_master_t *master /**< EtherCAT master */ |
695 ec_master_t *master /**< EtherCAT master */ |
696 ) |
696 ) |
697 { |
697 { |
698 ec_datagram_t *datagram, *n; |
698 ec_datagram_t *datagram, *n; |
699 size_t queue_size = 0; |
699 size_t queue_size = 0; |
700 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
700 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
701 queue_size += datagram->data_size; |
701 queue_size += datagram->data_size; |
702 } |
702 } |
703 list_for_each_entry_safe(datagram, n, &master->external_datagram_queue, queue) { |
703 list_for_each_entry_safe(datagram, n, &master->external_datagram_queue, queue) { |
704 queue_size += datagram->data_size; |
704 queue_size += datagram->data_size; |
705 if (queue_size <= master->max_queue_size) { |
705 if (queue_size <= master->max_queue_size) { |
706 list_del_init(&datagram->queue); |
706 list_del_init(&datagram->queue); |
707 #if DEBUG_INJECT |
707 #if DEBUG_INJECT |
708 if (master->debug_level) { |
708 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); |
709 EC_DBG("Injecting external datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); |
710 } |
710 } |
711 #endif |
711 #endif |
712 #ifdef EC_HAVE_CYCLES |
712 #ifdef EC_HAVE_CYCLES |
713 datagram->cycles_sent = 0; |
713 datagram->cycles_sent = 0; |
714 #endif |
714 #endif |
715 datagram->jiffies_sent = 0; |
715 datagram->jiffies_sent = 0; |
716 ec_master_queue_datagram(master, datagram); |
716 ec_master_queue_datagram(master, datagram); |
717 } |
717 } |
718 else { |
718 else { |
719 if (datagram->data_size > master->max_queue_size) { |
719 if (datagram->data_size > master->max_queue_size) { |
720 list_del_init(&datagram->queue); |
720 list_del_init(&datagram->queue); |
721 datagram->state = EC_DATAGRAM_ERROR; |
721 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); |
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); |
723 } |
723 } |
724 else { |
724 else { |
725 #ifdef EC_HAVE_CYCLES |
725 #ifdef EC_HAVE_CYCLES |
726 cycles_t cycles_now = get_cycles(); |
726 cycles_t cycles_now = get_cycles(); |
727 if (cycles_now - datagram->cycles_sent |
727 if (cycles_now - datagram->cycles_sent |
728 > sdo_injection_timeout_cycles) { |
728 > sdo_injection_timeout_cycles) { |
729 #else |
729 #else |
730 if (jiffies - datagram->jiffies_sent |
730 if (jiffies - datagram->jiffies_sent |
731 > sdo_injection_timeout_jiffies) { |
731 > sdo_injection_timeout_jiffies) { |
732 #endif |
732 #endif |
733 unsigned int time_us; |
733 unsigned int time_us; |
734 list_del_init(&datagram->queue); |
734 list_del_init(&datagram->queue); |
735 datagram->state = EC_DATAGRAM_ERROR; |
735 datagram->state = EC_DATAGRAM_ERROR; |
736 #ifdef EC_HAVE_CYCLES |
736 #ifdef EC_HAVE_CYCLES |
737 time_us = (unsigned int) ((cycles_now - datagram->cycles_sent) * 1000LL) / cpu_khz; |
737 time_us = (unsigned int) ((cycles_now - datagram->cycles_sent) * 1000LL) / cpu_khz; |
738 #else |
738 #else |
739 time_us = (unsigned int) ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); |
739 time_us = (unsigned int) ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); |
740 #endif |
740 #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); |
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); |
742 } |
742 } |
743 else { |
743 else { |
744 #if DEBUG_INJECT |
744 #if DEBUG_INJECT |
745 if (master->debug_level) { |
745 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); |
746 EC_DBG("Deferred injecting of external datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); |
747 } |
747 } |
748 #endif |
748 #endif |
749 } |
749 } |
750 } |
750 } |
751 } |
751 } |
752 } |
752 } |
753 } |
753 } |
|
754 |
|
755 /*****************************************************************************/ |
|
756 |
|
757 /** sets the expected interval between calls to ecrt_master_send |
|
758 and calculates the maximum amount of data to queue |
|
759 */ |
|
760 void ec_master_set_send_interval( |
|
761 ec_master_t *master, /**< EtherCAT master */ |
|
762 size_t send_interval /**< send interval */ |
|
763 ) |
|
764 { |
|
765 master->send_interval = send_interval; |
|
766 master->max_queue_size = (send_interval * 1000) / EC_BYTE_TRANSMITION_TIME; |
|
767 master->max_queue_size -= master->max_queue_size / 10; |
|
768 } |
|
769 |
754 |
770 |
755 /*****************************************************************************/ |
771 /*****************************************************************************/ |
756 |
772 |
757 /** Places an external datagram in the sdo datagram queue. |
773 /** Places an external datagram in the sdo datagram queue. |
758 */ |
774 */ |
759 void ec_master_queue_external_datagram( |
775 void ec_master_queue_external_datagram( |
760 ec_master_t *master, /**< EtherCAT master */ |
776 ec_master_t *master, /**< EtherCAT master */ |
761 ec_datagram_t *datagram /**< datagram */ |
777 ec_datagram_t *datagram /**< datagram */ |
762 ) |
778 ) |
763 { |
779 { |
|
780 ec_datagram_t *queued_datagram; |
|
781 |
|
782 down(&master->io_sem); |
|
783 // check, if the datagram is already queued |
|
784 list_for_each_entry(queued_datagram, &master->external_datagram_queue, queue) { |
|
785 if (queued_datagram == datagram) { |
|
786 datagram->state = EC_DATAGRAM_QUEUED; |
|
787 return; |
|
788 } |
|
789 } |
764 #if DEBUG_INJECT |
790 #if DEBUG_INJECT |
765 if (master->debug_level) { |
791 if (master->debug_level) { |
766 EC_DBG("Requesting external datagram %08x size=%u\n",(unsigned int)datagram,datagram->data_size); |
792 EC_DBG("Requesting external datagram %08x size=%u\n",(unsigned int)datagram,datagram->data_size); |
767 } |
793 } |
768 #endif |
794 #endif |
769 datagram->state = EC_DATAGRAM_QUEUED; |
795 list_add_tail(&datagram->queue, &master->external_datagram_queue); |
|
796 datagram->state = EC_DATAGRAM_QUEUED; |
770 #ifdef EC_HAVE_CYCLES |
797 #ifdef EC_HAVE_CYCLES |
771 datagram->cycles_sent = get_cycles(); |
798 datagram->cycles_sent = get_cycles(); |
772 #endif |
799 #endif |
773 datagram->jiffies_sent = jiffies; |
800 datagram->jiffies_sent = jiffies; |
774 |
801 |
775 master->fsm.idle = 0; |
802 master->fsm.idle = 0; |
776 |
803 up(&master->io_sem); |
777 down(&master->io_sem); |
|
778 list_add_tail(&datagram->queue, &master->external_datagram_queue); |
|
779 up(&master->io_sem); |
|
780 } |
804 } |
781 |
805 |
782 /*****************************************************************************/ |
806 /*****************************************************************************/ |
783 |
807 |
784 /** Places a datagram in the datagram queue. |
808 /** Places a datagram in the datagram queue. |
1089 master->stats.unmatched = 0; |
1113 master->stats.unmatched = 0; |
1090 } |
1114 } |
1091 } |
1115 } |
1092 } |
1116 } |
1093 |
1117 |
|
1118 |
|
1119 /*****************************************************************************/ |
|
1120 /* |
|
1121 * Sleep related functions: |
|
1122 */ |
|
1123 static enum hrtimer_restart ec_master_nanosleep_wakeup(struct hrtimer *timer) |
|
1124 { |
|
1125 struct hrtimer_sleeper *t = |
|
1126 container_of(timer, struct hrtimer_sleeper, timer); |
|
1127 struct task_struct *task = t->task; |
|
1128 |
|
1129 t->task = NULL; |
|
1130 if (task) |
|
1131 wake_up_process(task); |
|
1132 |
|
1133 return HRTIMER_NORESTART; |
|
1134 } |
|
1135 |
|
1136 void ec_master_nanosleep(const unsigned long nsecs) |
|
1137 { |
|
1138 struct hrtimer_sleeper t; |
|
1139 enum hrtimer_mode mode = HRTIMER_MODE_REL; |
|
1140 hrtimer_init(&t.timer, CLOCK_MONOTONIC,mode); |
|
1141 t.timer.function = ec_master_nanosleep_wakeup; |
|
1142 t.task = current; |
|
1143 #ifdef CONFIG_HIGH_RES_TIMERS |
|
1144 t.timer.cb_mode = HRTIMER_CB_IRQSAFE_NO_RESTART; |
|
1145 #endif |
|
1146 t.timer.expires = ktime_set(0,nsecs); |
|
1147 do { |
|
1148 set_current_state(TASK_INTERRUPTIBLE); |
|
1149 hrtimer_start(&t.timer, t.timer.expires, mode); |
|
1150 |
|
1151 if (likely(t.task)) |
|
1152 schedule(); |
|
1153 |
|
1154 hrtimer_cancel(&t.timer); |
|
1155 mode = HRTIMER_MODE_ABS; |
|
1156 |
|
1157 } while (t.task && !signal_pending(current)); |
|
1158 } |
|
1159 |
|
1160 |
1094 /*****************************************************************************/ |
1161 /*****************************************************************************/ |
1095 |
1162 |
1096 /** Master kernel thread function for IDLE phase. |
1163 /** Master kernel thread function for IDLE phase. |
1097 */ |
1164 */ |
1098 static int ec_master_idle_thread(void *priv_data) |
1165 static int ec_master_idle_thread(void *priv_data) |
1099 { |
1166 { |
1100 ec_master_t *master = (ec_master_t *) priv_data; |
1167 ec_master_t *master = (ec_master_t *) priv_data; |
1101 ec_slave_t *slave = NULL; |
1168 ec_slave_t *slave = NULL; |
1102 int fsm_exec; |
1169 int fsm_exec; |
1103 if (master->debug_level) |
1170 ec_master_set_send_interval(master,1000000 / HZ); // send interval in IDLE phase |
1104 EC_DBG("Idle thread running.\n"); |
1171 if (master->debug_level) |
|
1172 EC_DBG("Idle thread running with send interval = %d us, max data size=%d\n",master->send_interval,master->max_queue_size); |
1105 |
1173 |
1106 while (!kthread_should_stop()) { |
1174 while (!kthread_should_stop()) { |
1107 ec_datagram_output_stats(&master->fsm_datagram); |
1175 ec_datagram_output_stats(&master->fsm_datagram); |
1108 |
1176 |
1109 // receive |
1177 // receive |