689 } |
689 } |
690 |
690 |
691 |
691 |
692 /*****************************************************************************/ |
692 /*****************************************************************************/ |
693 |
693 |
694 /** Injects external datagrams that fit into the datagram queue |
694 /** Injects external datagrams that fit into the datagram queue. |
695 */ |
695 */ |
696 void ec_master_inject_external_datagrams( |
696 void ec_master_inject_external_datagrams( |
697 ec_master_t *master /**< EtherCAT master */ |
697 ec_master_t *master /**< EtherCAT master */ |
698 ) |
698 ) |
699 { |
699 { |
700 ec_datagram_t *datagram, *n; |
700 ec_datagram_t *datagram, *n; |
701 size_t queue_size = 0; |
701 size_t queue_size = 0; |
|
702 |
702 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
703 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
703 queue_size += datagram->data_size; |
704 queue_size += datagram->data_size; |
704 } |
705 } |
705 list_for_each_entry_safe(datagram, n, &master->external_datagram_queue, queue) { |
706 |
|
707 list_for_each_entry_safe(datagram, n, &master->external_datagram_queue, |
|
708 queue) { |
706 queue_size += datagram->data_size; |
709 queue_size += datagram->data_size; |
707 if (queue_size <= master->max_queue_size) { |
710 if (queue_size <= master->max_queue_size) { |
708 list_del_init(&datagram->queue); |
711 list_del_init(&datagram->queue); |
709 #if DEBUG_INJECT |
712 #if DEBUG_INJECT |
710 if (master->debug_level) { |
713 if (master->debug_level) { |
721 } |
724 } |
722 else { |
725 else { |
723 if (datagram->data_size > master->max_queue_size) { |
726 if (datagram->data_size > master->max_queue_size) { |
724 list_del_init(&datagram->queue); |
727 list_del_init(&datagram->queue); |
725 datagram->state = EC_DATAGRAM_ERROR; |
728 datagram->state = EC_DATAGRAM_ERROR; |
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); |
729 EC_ERR("External datagram %p is too large," |
727 } |
730 " size=%u, max_queue_size=%u\n", |
728 else { |
731 datagram, datagram->data_size, |
|
732 master->max_queue_size); |
|
733 } else { |
729 #ifdef EC_HAVE_CYCLES |
734 #ifdef EC_HAVE_CYCLES |
730 cycles_t cycles_now = get_cycles(); |
735 cycles_t cycles_now = get_cycles(); |
|
736 |
731 if (cycles_now - datagram->cycles_sent |
737 if (cycles_now - datagram->cycles_sent |
732 > sdo_injection_timeout_cycles) { |
738 > sdo_injection_timeout_cycles) |
733 #else |
739 #else |
734 if (jiffies - datagram->jiffies_sent |
740 if (jiffies - datagram->jiffies_sent |
735 > sdo_injection_timeout_jiffies) { |
741 > sdo_injection_timeout_jiffies) |
736 #endif |
742 #endif |
737 unsigned int time_us; |
743 { |
738 list_del_init(&datagram->queue); |
744 unsigned int time_us; |
739 datagram->state = EC_DATAGRAM_ERROR; |
745 |
|
746 list_del_init(&datagram->queue); |
|
747 datagram->state = EC_DATAGRAM_ERROR; |
740 #ifdef EC_HAVE_CYCLES |
748 #ifdef EC_HAVE_CYCLES |
741 time_us = (unsigned int) ((cycles_now - datagram->cycles_sent) * 1000LL) / cpu_khz; |
749 time_us = (unsigned int) |
|
750 ((cycles_now - datagram->cycles_sent) * 1000LL) |
|
751 / cpu_khz; |
742 #else |
752 #else |
743 time_us = (unsigned int) ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); |
753 time_us = (unsigned int) |
744 #endif |
754 ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); |
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); |
755 #endif |
746 } |
756 EC_ERR("Timeout %u us: injecting external datagram %p" |
747 else { |
757 " size=%u, max_queue_size=%u\n", |
|
758 time_us, datagram, |
|
759 datagram->data_size, master->max_queue_size); |
|
760 } |
748 #if DEBUG_INJECT |
761 #if DEBUG_INJECT |
749 if (master->debug_level) { |
762 else if (master->debug_level) { |
750 EC_DBG("Deferred injecting of external datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); |
763 EC_DBG("Deferred injecting of external datagram %p" |
751 } |
764 " size=%u, queue_size=%u\n", |
752 #endif |
765 datagram, datagram->data_size, queue_size); |
753 } |
|
754 } |
766 } |
|
767 #endif |
755 } |
768 } |
756 } |
769 } |
757 } |
770 } |
758 |
771 } |
759 /*****************************************************************************/ |
772 |
760 |
773 /*****************************************************************************/ |
761 /** sets the expected interval between calls to ecrt_master_send |
774 |
762 and calculates the maximum amount of data to queue |
775 /** Sets the expected interval between calls to ecrt_master_send |
763 */ |
776 * and calculates the maximum amount of data to queue. |
764 void ec_master_set_send_interval( |
777 */ |
765 ec_master_t *master, /**< EtherCAT master */ |
778 void ec_master_set_send_interval( |
766 size_t send_interval /**< send interval */ |
779 ec_master_t *master, /**< EtherCAT master */ |
767 ) |
780 size_t send_interval /**< Send interval */ |
768 { |
781 ) |
769 master->send_interval = send_interval; |
782 { |
770 master->max_queue_size = (send_interval * 1000) / EC_BYTE_TRANSMITION_TIME; |
783 master->send_interval = send_interval; |
771 master->max_queue_size -= master->max_queue_size / 10; |
784 master->max_queue_size = |
772 } |
785 (send_interval * 1000) / EC_BYTE_TRANSMISSION_TIME_NS; |
773 |
786 master->max_queue_size -= master->max_queue_size / 10; |
|
787 } |
774 |
788 |
775 /*****************************************************************************/ |
789 /*****************************************************************************/ |
776 |
790 |
777 /** Places an external datagram in the sdo datagram queue. |
791 /** Places an external datagram in the sdo datagram queue. |
778 */ |
792 */ |
782 ) |
796 ) |
783 { |
797 { |
784 ec_datagram_t *queued_datagram; |
798 ec_datagram_t *queued_datagram; |
785 |
799 |
786 down(&master->io_sem); |
800 down(&master->io_sem); |
|
801 |
787 // check, if the datagram is already queued |
802 // check, if the datagram is already queued |
788 list_for_each_entry(queued_datagram, &master->external_datagram_queue, queue) { |
803 list_for_each_entry(queued_datagram, &master->external_datagram_queue, |
|
804 queue) { |
789 if (queued_datagram == datagram) { |
805 if (queued_datagram == datagram) { |
790 datagram->state = EC_DATAGRAM_QUEUED; |
806 datagram->state = EC_DATAGRAM_QUEUED; |
791 return; |
807 return; |
792 } |
808 } |
793 } |
809 } |
|
810 |
794 #if DEBUG_INJECT |
811 #if DEBUG_INJECT |
795 if (master->debug_level) { |
812 if (master->debug_level) { |
796 EC_DBG("Requesting external datagram %08x size=%u\n",(unsigned int)datagram,datagram->data_size); |
813 EC_DBG("Requesting external datagram %p size=%u\n", |
|
814 datagram, datagram->data_size); |
797 } |
815 } |
798 #endif |
816 #endif |
|
817 |
799 list_add_tail(&datagram->queue, &master->external_datagram_queue); |
818 list_add_tail(&datagram->queue, &master->external_datagram_queue); |
800 datagram->state = EC_DATAGRAM_QUEUED; |
819 datagram->state = EC_DATAGRAM_QUEUED; |
801 #ifdef EC_HAVE_CYCLES |
820 #ifdef EC_HAVE_CYCLES |
802 datagram->cycles_sent = get_cycles(); |
821 datagram->cycles_sent = get_cycles(); |
803 #endif |
822 #endif |
1202 { |
1221 { |
1203 ec_master_t *master = (ec_master_t *) priv_data; |
1222 ec_master_t *master = (ec_master_t *) priv_data; |
1204 ec_slave_t *slave = NULL; |
1223 ec_slave_t *slave = NULL; |
1205 int fsm_exec; |
1224 int fsm_exec; |
1206 size_t sent_bytes; |
1225 size_t sent_bytes; |
1207 ec_master_set_send_interval(master,1000000 / HZ); // send interval in IDLE phase |
1226 |
|
1227 // send interval in IDLE phase |
|
1228 ec_master_set_send_interval(master, 1000000 / HZ); |
|
1229 |
1208 if (master->debug_level) |
1230 if (master->debug_level) |
1209 EC_DBG("Idle thread running with send interval = %d us, max data size=%d\n",master->send_interval,master->max_queue_size); |
1231 EC_DBG("Idle thread running with send interval = %d us," |
|
1232 " max data size=%d\n", master->send_interval, |
|
1233 master->max_queue_size); |
1210 |
1234 |
1211 while (!kthread_should_stop()) { |
1235 while (!kthread_should_stop()) { |
1212 ec_datagram_output_stats(&master->fsm_datagram); |
1236 ec_datagram_output_stats(&master->fsm_datagram); |
1213 |
1237 |
1214 // receive |
1238 // receive |
1233 if (fsm_exec) { |
1257 if (fsm_exec) { |
1234 ec_master_queue_datagram(master, &master->fsm_datagram); |
1258 ec_master_queue_datagram(master, &master->fsm_datagram); |
1235 } |
1259 } |
1236 ec_master_inject_external_datagrams(master); |
1260 ec_master_inject_external_datagrams(master); |
1237 ecrt_master_send(master); |
1261 ecrt_master_send(master); |
1238 sent_bytes = master->main_device.tx_skb[master->main_device.tx_ring_index]->len; |
1262 sent_bytes = master->main_device.tx_skb[ |
|
1263 master->main_device.tx_ring_index]->len; |
1239 up(&master->io_sem); |
1264 up(&master->io_sem); |
1240 |
1265 |
1241 if (ec_fsm_master_idle(&master->fsm)) { |
1266 if (ec_fsm_master_idle(&master->fsm)) { |
1242 #ifdef EC_USE_HRTIMER |
1267 #ifdef EC_USE_HRTIMER |
1243 ec_master_nanosleep(master->send_interval * 1000); |
1268 ec_master_nanosleep(master->send_interval * 1000); |