57 #ifdef EC_HAVE_CYCLES |
57 #ifdef EC_HAVE_CYCLES |
58 |
58 |
59 /** Frame timeout in cycles. |
59 /** Frame timeout in cycles. |
60 */ |
60 */ |
61 static cycles_t timeout_cycles; |
61 static cycles_t timeout_cycles; |
62 |
62 static cycles_t sdo_injection_timeout_cycles; |
63 #else |
63 #else |
64 |
64 |
65 /** Frame timeout in jiffies. |
65 /** Frame timeout in jiffies. |
66 */ |
66 */ |
67 static unsigned long timeout_jiffies; |
67 static unsigned long timeout_jiffies; |
68 |
68 static unsigned long sdo_injection_timeout_jiffies; |
|
69 |
69 #endif |
70 #endif |
70 |
71 |
71 /*****************************************************************************/ |
72 /*****************************************************************************/ |
72 |
73 |
73 void ec_master_clear_slave_configs(ec_master_t *); |
74 void ec_master_clear_slave_configs(ec_master_t *); |
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 |
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); |
153 |
156 |
154 INIT_LIST_HEAD(&master->sdo_datagram_queue); |
157 INIT_LIST_HEAD(&master->sdo_datagram_queue); |
|
158 master->max_queue_size = EC_MAX_DATA_SIZE; |
155 |
159 |
156 INIT_LIST_HEAD(&master->domains); |
160 INIT_LIST_HEAD(&master->domains); |
157 |
161 |
158 master->debug_level = debug_level; |
162 master->debug_level = debug_level; |
159 master->stats.timeouts = 0; |
163 master->stats.timeouts = 0; |
693 */ |
697 */ |
694 void ec_master_inject_sdo_datagrams( |
698 void ec_master_inject_sdo_datagrams( |
695 ec_master_t *master /**< EtherCAT master */ |
699 ec_master_t *master /**< EtherCAT master */ |
696 ) |
700 ) |
697 { |
701 { |
698 ec_datagram_t *sdo_datagram, *n; |
702 ec_datagram_t *datagram, *n; |
699 list_for_each_entry_safe(sdo_datagram, n, &master->sdo_datagram_queue, queue) { |
703 size_t queue_size = 0; |
700 list_del_init(&sdo_datagram->queue); |
704 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
701 if (master->debug_level) |
705 queue_size += datagram->data_size; |
702 EC_DBG("queuing SDO datagram %08x\n",(unsigned int)sdo_datagram); |
706 } |
703 ec_master_queue_datagram(master, sdo_datagram); |
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 } |
704 } |
752 } |
705 } |
753 } |
706 |
754 |
707 /*****************************************************************************/ |
755 /*****************************************************************************/ |
708 |
756 |
711 void ec_master_queue_sdo_datagram( |
759 void ec_master_queue_sdo_datagram( |
712 ec_master_t *master, /**< EtherCAT master */ |
760 ec_master_t *master, /**< EtherCAT master */ |
713 ec_datagram_t *datagram /**< datagram */ |
761 ec_datagram_t *datagram /**< datagram */ |
714 ) |
762 ) |
715 { |
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 |
716 down(&master->io_sem); |
773 down(&master->io_sem); |
717 list_add_tail(&datagram->queue, &master->sdo_datagram_queue); |
774 list_add_tail(&datagram->queue, &master->sdo_datagram_queue); |
718 datagram->state = EC_DATAGRAM_QUEUED; |
|
719 up(&master->io_sem); |
775 up(&master->io_sem); |
720 } |
776 } |
721 |
777 |
722 /*****************************************************************************/ |
778 /*****************************************************************************/ |
723 |
779 |