170 master->datagram_index = 0; |
170 master->datagram_index = 0; |
171 |
171 |
172 INIT_LIST_HEAD(&master->ext_datagram_queue); |
172 INIT_LIST_HEAD(&master->ext_datagram_queue); |
173 sema_init(&master->ext_queue_sem, 1); |
173 sema_init(&master->ext_queue_sem, 1); |
174 |
174 |
175 INIT_LIST_HEAD(&master->external_datagram_queue); |
175 INIT_LIST_HEAD(&master->fsm_datagram_queue); |
176 |
176 |
177 // send interval in IDLE phase |
177 // send interval in IDLE phase |
178 ec_master_set_send_interval(master, 1000000 / HZ); |
178 ec_master_set_send_interval(master, 1000000 / HZ); |
179 |
179 |
180 INIT_LIST_HEAD(&master->domains); |
180 INIT_LIST_HEAD(&master->domains); |
682 master->phase = EC_IDLE; |
682 master->phase = EC_IDLE; |
683 } |
683 } |
684 |
684 |
685 /*****************************************************************************/ |
685 /*****************************************************************************/ |
686 |
686 |
687 /** Injects external datagrams that fit into the datagram queue. |
687 /** Injects fsm datagrams that fit into the datagram queue. |
688 */ |
688 */ |
689 void ec_master_inject_external_datagrams( |
689 void ec_master_inject_fsm_datagrams( |
690 ec_master_t *master /**< EtherCAT master */ |
690 ec_master_t *master /**< EtherCAT master */ |
691 ) |
691 ) |
692 { |
692 { |
693 ec_datagram_t *datagram, *n; |
693 ec_datagram_t *datagram, *n; |
694 size_t queue_size = 0; |
694 size_t queue_size = 0; |
695 |
695 |
|
696 if (list_empty(&master->fsm_datagram_queue)) { |
|
697 return; |
|
698 } |
696 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
699 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
697 queue_size += datagram->data_size; |
700 queue_size += datagram->data_size; |
698 } |
701 } |
699 |
702 |
700 list_for_each_entry_safe(datagram, n, &master->external_datagram_queue, |
703 list_for_each_entry_safe(datagram, n, &master->fsm_datagram_queue, |
701 queue) { |
704 queue) { |
702 queue_size += datagram->data_size; |
705 queue_size += datagram->data_size; |
703 if (queue_size <= master->max_queue_size) { |
706 if (queue_size <= master->max_queue_size) { |
704 list_del_init(&datagram->queue); |
707 list_del_init(&datagram->queue); |
705 #if DEBUG_INJECT |
708 #if DEBUG_INJECT |
706 EC_MASTER_DBG(master, 0, "Injecting external datagram %08x" |
709 EC_MASTER_DBG(master, 0, "Injecting fsm datagram %p" |
707 " size=%u, queue_size=%u\n", (unsigned int) datagram, |
710 " size=%zu, queue_size=%zu\n", datagram, |
708 datagram->data_size, queue_size); |
711 datagram->data_size, queue_size); |
709 #endif |
712 #endif |
710 #ifdef EC_HAVE_CYCLES |
713 #ifdef EC_HAVE_CYCLES |
711 datagram->cycles_sent = 0; |
714 datagram->cycles_sent = 0; |
712 #endif |
715 #endif |
715 } |
718 } |
716 else { |
719 else { |
717 if (datagram->data_size > master->max_queue_size) { |
720 if (datagram->data_size > master->max_queue_size) { |
718 list_del_init(&datagram->queue); |
721 list_del_init(&datagram->queue); |
719 datagram->state = EC_DATAGRAM_ERROR; |
722 datagram->state = EC_DATAGRAM_ERROR; |
720 EC_MASTER_ERR(master, "External datagram %p is too large," |
723 EC_MASTER_ERR(master, "Fsm datagram %p is too large," |
721 " size=%zu, max_queue_size=%zu\n", |
724 " size=%zu, max_queue_size=%zu\n", |
722 datagram, datagram->data_size, |
725 datagram, datagram->data_size, |
723 master->max_queue_size); |
726 master->max_queue_size); |
724 } else { |
727 } else { |
725 #ifdef EC_HAVE_CYCLES |
728 #ifdef EC_HAVE_CYCLES |
743 #else |
746 #else |
744 time_us = (unsigned int) |
747 time_us = (unsigned int) |
745 ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); |
748 ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); |
746 #endif |
749 #endif |
747 EC_MASTER_ERR(master, "Timeout %u us: Injecting" |
750 EC_MASTER_ERR(master, "Timeout %u us: Injecting" |
748 " external datagram %p size=%zu," |
751 " fsm datagram %p size=%zu," |
749 " max_queue_size=%zu\n", time_us, datagram, |
752 " max_queue_size=%zu\n", time_us, datagram, |
750 datagram->data_size, master->max_queue_size); |
753 datagram->data_size, master->max_queue_size); |
751 } |
754 } |
752 #if DEBUG_INJECT |
755 #if DEBUG_INJECT |
753 else { |
756 else { |
754 EC_MASTER_DBG(master, 0, "Deferred injecting" |
757 EC_MASTER_DBG(master, 0, "Deferred injecting" |
755 " of external datagram %p" |
758 " of fsm datagram %p" |
756 " size=%u, queue_size=%u\n", |
759 " size=%zu, queue_size=%zu\n", |
757 datagram, datagram->data_size, queue_size); |
760 datagram, datagram->data_size, queue_size); |
758 } |
761 } |
759 #endif |
762 #endif |
760 } |
763 } |
761 } |
764 } |
778 master->max_queue_size -= master->max_queue_size / 10; |
781 master->max_queue_size -= master->max_queue_size / 10; |
779 } |
782 } |
780 |
783 |
781 /*****************************************************************************/ |
784 /*****************************************************************************/ |
782 |
785 |
783 /** Places an external datagram in the sdo datagram queue. |
786 /** Places an request (SDO/FoE/SoE/EoE) fsm datagram in the sdo datagram queue. |
784 */ |
787 */ |
785 void ec_master_queue_external_datagram( |
788 void ec_master_queue_request_fsm_datagram( |
786 ec_master_t *master, /**< EtherCAT master */ |
789 ec_master_t *master, /**< EtherCAT master */ |
787 ec_datagram_t *datagram /**< datagram */ |
790 ec_datagram_t *datagram /**< datagram */ |
788 ) |
791 ) |
789 { |
792 { |
|
793 ec_master_queue_fsm_datagram(master,datagram); |
|
794 master->fsm.idle = 0; // pump the bus as fast as possible |
|
795 } |
|
796 |
|
797 /*****************************************************************************/ |
|
798 |
|
799 /** Places an fsm datagram in the sdo datagram queue. |
|
800 */ |
|
801 void ec_master_queue_fsm_datagram( |
|
802 ec_master_t *master, /**< EtherCAT master */ |
|
803 ec_datagram_t *datagram /**< datagram */ |
|
804 ) |
|
805 { |
790 ec_datagram_t *queued_datagram; |
806 ec_datagram_t *queued_datagram; |
791 |
807 |
792 down(&master->io_sem); |
808 down(&master->io_sem); |
793 |
809 |
794 // check, if the datagram is already queued |
810 // check, if the datagram is already queued |
795 list_for_each_entry(queued_datagram, &master->external_datagram_queue, |
811 list_for_each_entry(queued_datagram, &master->fsm_datagram_queue, |
796 queue) { |
812 queue) { |
797 if (queued_datagram == datagram) { |
813 if (queued_datagram == datagram) { |
798 datagram->state = EC_DATAGRAM_QUEUED; |
814 datagram->state = EC_DATAGRAM_QUEUED; |
799 up(&master->io_sem); |
815 up(&master->io_sem); |
800 return; |
816 return; |
801 } |
817 } |
802 } |
818 } |
803 |
819 |
804 #if DEBUG_INJECT |
820 #if DEBUG_INJECT |
805 EC_MASTER_DBG(master, 0, "Requesting external datagram %p size=%u\n", |
821 EC_MASTER_DBG(master, 0, "Requesting fsm datagram %p size=%zu\n", |
806 datagram, datagram->data_size); |
822 datagram, datagram->data_size); |
807 #endif |
823 #endif |
808 |
824 |
809 list_add_tail(&datagram->queue, &master->external_datagram_queue); |
825 list_add_tail(&datagram->queue, &master->fsm_datagram_queue); |
810 datagram->state = EC_DATAGRAM_QUEUED; |
826 datagram->state = EC_DATAGRAM_QUEUED; |
811 #ifdef EC_HAVE_CYCLES |
827 #ifdef EC_HAVE_CYCLES |
812 datagram->cycles_sent = get_cycles(); |
828 datagram->cycles_sent = get_cycles(); |
813 #endif |
829 #endif |
814 datagram->jiffies_sent = jiffies; |
830 datagram->jiffies_sent = jiffies; |
815 |
831 |
816 master->fsm.idle = 0; |
|
817 up(&master->io_sem); |
832 up(&master->io_sem); |
818 } |
833 } |
819 |
834 |
820 /*****************************************************************************/ |
835 /*****************************************************************************/ |
821 |
836 |
1255 static int ec_master_idle_thread(void *priv_data) |
1270 static int ec_master_idle_thread(void *priv_data) |
1256 { |
1271 { |
1257 ec_master_t *master = (ec_master_t *) priv_data; |
1272 ec_master_t *master = (ec_master_t *) priv_data; |
1258 ec_slave_t *slave = NULL; |
1273 ec_slave_t *slave = NULL; |
1259 size_t sent_bytes; |
1274 size_t sent_bytes; |
1260 |
|
1261 // send interval in IDLE phase |
1275 // send interval in IDLE phase |
1262 ec_master_set_send_interval(master, 1000000 / HZ); |
1276 ec_master_set_send_interval(master, 1000000 / HZ); |
1263 |
1277 |
1264 EC_MASTER_DBG(master, 1, "Idle thread running with send interval = %u us," |
1278 EC_MASTER_DBG(master, 1, "Idle thread running with send interval = %u us," |
1265 " max data size=%zu\n", master->send_interval, |
1279 " max data size=%zu\n", master->send_interval, |
1271 // receive |
1285 // receive |
1272 down(&master->io_sem); |
1286 down(&master->io_sem); |
1273 ecrt_master_receive(master); |
1287 ecrt_master_receive(master); |
1274 up(&master->io_sem); |
1288 up(&master->io_sem); |
1275 |
1289 |
1276 if (master->injection_seq_rt == master->injection_seq_fsm) { |
1290 // execute master & slave state machines |
1277 // execute master & slave state machines |
1291 if (down_interruptible(&master->master_sem)) |
1278 if (down_interruptible(&master->master_sem)) |
1292 break; |
1279 break; |
1293 if (ec_fsm_master_exec(&master->fsm)) { |
1280 if (ec_fsm_master_exec(&master->fsm)) |
1294 ec_master_queue_fsm_datagram(master, &master->fsm_datagram); |
1281 master->injection_seq_fsm++; |
1295 } |
1282 for (slave = master->slaves; |
1296 for (slave = master->slaves; |
1283 slave < master->slaves + master->slave_count; |
1297 slave < master->slaves + master->slave_count; |
1284 slave++) { |
1298 slave++) { |
1285 if (ec_fsm_slave_exec(&slave->fsm)) |
1299 ec_fsm_slave_exec(&slave->fsm); // may queue datagram in external queue |
1286 master->injection_seq_fsm++; |
1300 } |
1287 } |
1301 up(&master->master_sem); |
1288 up(&master->master_sem); |
1302 |
1289 } |
|
1290 // queue and send |
1303 // queue and send |
1291 down(&master->io_sem); |
1304 down(&master->io_sem); |
1292 ecrt_master_send(master); |
1305 ecrt_master_send(master); |
1293 sent_bytes = master->main_device.tx_skb[ |
1306 sent_bytes = master->main_device.tx_skb[ |
1294 master->main_device.tx_ring_index]->len; |
1307 master->main_device.tx_ring_index]->len; |
1329 master->send_interval, master->max_queue_size); |
1342 master->send_interval, master->max_queue_size); |
1330 |
1343 |
1331 while (!kthread_should_stop()) { |
1344 while (!kthread_should_stop()) { |
1332 ec_datagram_output_stats(&master->fsm_datagram); |
1345 ec_datagram_output_stats(&master->fsm_datagram); |
1333 |
1346 |
1334 if (master->injection_seq_rt == master->injection_seq_fsm) { |
1347 // output statistics |
1335 // output statistics |
1348 ec_master_output_stats(master); |
1336 ec_master_output_stats(master); |
1349 |
1337 |
1350 // execute master & slave state machines |
1338 // execute master & slave state machines |
1351 if (down_interruptible(&master->master_sem)) |
1339 if (down_interruptible(&master->master_sem)) |
1352 break; |
1340 break; |
1353 if (ec_fsm_master_exec(&master->fsm)) |
1341 if (ec_fsm_master_exec(&master->fsm)) |
1354 ec_master_queue_fsm_datagram(master, &master->fsm_datagram); |
1342 master->injection_seq_fsm++; |
1355 for (slave = master->slaves; |
1343 for (slave = master->slaves; |
1356 slave < master->slaves + master->slave_count; |
1344 slave < master->slaves + master->slave_count; |
1357 slave++) { |
1345 slave++) { |
1358 ec_fsm_slave_exec(&slave->fsm); // may queue datagram in external queue |
1346 if (ec_fsm_slave_exec(&slave->fsm)) |
1359 } |
1347 master->injection_seq_fsm++; |
1360 up(&master->master_sem); |
1348 } |
|
1349 up(&master->master_sem); |
|
1350 } |
|
1351 |
1361 |
1352 #ifdef EC_USE_HRTIMER |
1362 #ifdef EC_USE_HRTIMER |
1353 // the op thread should not work faster than the sending RT thread |
1363 // the op thread should not work faster than the sending RT thread |
1354 ec_master_nanosleep(master->send_interval * 1000); |
1364 ec_master_nanosleep(master->send_interval * 1000); |
1355 #else |
1365 #else |
2117 |
2127 |
2118 void ecrt_master_send(ec_master_t *master) |
2128 void ecrt_master_send(ec_master_t *master) |
2119 { |
2129 { |
2120 ec_datagram_t *datagram, *n; |
2130 ec_datagram_t *datagram, *n; |
2121 |
2131 |
2122 if (master->injection_seq_rt != master->injection_seq_fsm) { |
2132 ec_master_inject_fsm_datagrams(master); |
2123 // inject datagrams produced by master & slave FSMs |
|
2124 ec_master_queue_datagram(master, &master->fsm_datagram); |
|
2125 master->injection_seq_rt = master->injection_seq_fsm; |
|
2126 } |
|
2127 ec_master_inject_external_datagrams(master); |
|
2128 |
2133 |
2129 if (unlikely(!master->main_device.link_state)) { |
2134 if (unlikely(!master->main_device.link_state)) { |
2130 // link is down, no datagram can be sent |
2135 // link is down, no datagram can be sent |
2131 list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) { |
2136 list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) { |
2132 datagram->state = EC_DATAGRAM_ERROR; |
2137 datagram->state = EC_DATAGRAM_ERROR; |