355 EC_INFO("Master thread exited.\n"); |
357 EC_INFO("Master thread exited.\n"); |
356 |
358 |
357 if (master->fsm_datagram.state != EC_DATAGRAM_SENT) return; |
359 if (master->fsm_datagram.state != EC_DATAGRAM_SENT) return; |
358 |
360 |
359 // wait for FSM datagram |
361 // wait for FSM datagram |
360 while (get_cycles() - master->fsm_datagram.cycles_sent |
362 sleep_jiffies = max(HZ / 100, 1); // 10 ms, at least 1 jiffy |
361 < (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000)) |
363 schedule_timeout(sleep_jiffies); |
362 schedule(); |
|
363 } |
364 } |
364 |
365 |
365 /*****************************************************************************/ |
366 /*****************************************************************************/ |
366 |
367 |
367 /** Transition function from ORPHANED to IDLE phase. |
368 /** Transition function from ORPHANED to IDLE phase. |
570 { |
571 { |
571 ec_datagram_t *datagram, *next; |
572 ec_datagram_t *datagram, *next; |
572 size_t datagram_size; |
573 size_t datagram_size; |
573 uint8_t *frame_data, *cur_data; |
574 uint8_t *frame_data, *cur_data; |
574 void *follows_word; |
575 void *follows_word; |
|
576 #ifdef EC_HAVE_CYCLES |
575 cycles_t cycles_start, cycles_sent, cycles_end; |
577 cycles_t cycles_start, cycles_sent, cycles_end; |
|
578 #endif |
576 unsigned long jiffies_sent; |
579 unsigned long jiffies_sent; |
577 unsigned int frame_count, more_datagrams_waiting; |
580 unsigned int frame_count, more_datagrams_waiting; |
578 struct list_head sent_datagrams; |
581 struct list_head sent_datagrams; |
579 |
582 |
|
583 #ifdef EC_HAVE_CYCLES |
580 cycles_start = get_cycles(); |
584 cycles_start = get_cycles(); |
|
585 #endif |
581 frame_count = 0; |
586 frame_count = 0; |
582 INIT_LIST_HEAD(&sent_datagrams); |
587 INIT_LIST_HEAD(&sent_datagrams); |
583 |
588 |
584 if (unlikely(master->debug_level > 1)) |
589 if (unlikely(master->debug_level > 1)) |
585 EC_DBG("ec_master_send_datagrams\n"); |
590 EC_DBG("ec_master_send_datagrams\n"); |
648 if (unlikely(master->debug_level > 1)) |
653 if (unlikely(master->debug_level > 1)) |
649 EC_DBG("frame size: %u\n", cur_data - frame_data); |
654 EC_DBG("frame size: %u\n", cur_data - frame_data); |
650 |
655 |
651 // send frame |
656 // send frame |
652 ec_device_send(&master->main_device, cur_data - frame_data); |
657 ec_device_send(&master->main_device, cur_data - frame_data); |
|
658 #ifdef EC_HAVE_CYCLES |
653 cycles_sent = get_cycles(); |
659 cycles_sent = get_cycles(); |
|
660 #endif |
654 jiffies_sent = jiffies; |
661 jiffies_sent = jiffies; |
655 |
662 |
656 // set datagram states and sending timestamps |
663 // set datagram states and sending timestamps |
657 list_for_each_entry_safe(datagram, next, &sent_datagrams, sent) { |
664 list_for_each_entry_safe(datagram, next, &sent_datagrams, sent) { |
658 datagram->state = EC_DATAGRAM_SENT; |
665 datagram->state = EC_DATAGRAM_SENT; |
|
666 #ifdef EC_HAVE_CYCLES |
659 datagram->cycles_sent = cycles_sent; |
667 datagram->cycles_sent = cycles_sent; |
|
668 #endif |
660 datagram->jiffies_sent = jiffies_sent; |
669 datagram->jiffies_sent = jiffies_sent; |
661 list_del_init(&datagram->sent); // empty list of sent datagrams |
670 list_del_init(&datagram->sent); // empty list of sent datagrams |
662 } |
671 } |
663 |
672 |
664 frame_count++; |
673 frame_count++; |
665 } |
674 } |
666 while (more_datagrams_waiting); |
675 while (more_datagrams_waiting); |
667 |
676 |
|
677 #ifdef EC_HAVE_CYCLES |
668 if (unlikely(master->debug_level > 1)) { |
678 if (unlikely(master->debug_level > 1)) { |
669 cycles_end = get_cycles(); |
679 cycles_end = get_cycles(); |
670 EC_DBG("ec_master_send_datagrams sent %u frames in %uus.\n", |
680 EC_DBG("ec_master_send_datagrams sent %u frames in %uus.\n", |
671 frame_count, |
681 frame_count, |
672 (unsigned int) (cycles_end - cycles_start) * 1000 / cpu_khz); |
682 (unsigned int) (cycles_end - cycles_start) * 1000 / cpu_khz); |
673 } |
683 } |
|
684 #endif |
674 } |
685 } |
675 |
686 |
676 /*****************************************************************************/ |
687 /*****************************************************************************/ |
677 |
688 |
678 /** Processes a received frame. |
689 /** Processes a received frame. |
765 datagram->working_counter = EC_READ_U16(cur_data); |
776 datagram->working_counter = EC_READ_U16(cur_data); |
766 cur_data += EC_DATAGRAM_FOOTER_SIZE; |
777 cur_data += EC_DATAGRAM_FOOTER_SIZE; |
767 |
778 |
768 // dequeue the received datagram |
779 // dequeue the received datagram |
769 datagram->state = EC_DATAGRAM_RECEIVED; |
780 datagram->state = EC_DATAGRAM_RECEIVED; |
|
781 #ifdef EC_HAVE_CYCLES |
770 datagram->cycles_received = master->main_device.cycles_poll; |
782 datagram->cycles_received = master->main_device.cycles_poll; |
|
783 #endif |
771 datagram->jiffies_received = master->main_device.jiffies_poll; |
784 datagram->jiffies_received = master->main_device.jiffies_poll; |
772 list_del_init(&datagram->queue); |
785 list_del_init(&datagram->queue); |
773 } |
786 } |
774 } |
787 } |
775 |
788 |
1258 /*****************************************************************************/ |
1271 /*****************************************************************************/ |
1259 |
1272 |
1260 void ecrt_master_receive(ec_master_t *master) |
1273 void ecrt_master_receive(ec_master_t *master) |
1261 { |
1274 { |
1262 ec_datagram_t *datagram, *next; |
1275 ec_datagram_t *datagram, *next; |
|
1276 #ifdef EC_HAVE_CYCLES |
1263 cycles_t cycles_timeout; |
1277 cycles_t cycles_timeout; |
|
1278 #else |
|
1279 unsigned long diff_ms, timeout_ms; |
|
1280 #endif |
1264 unsigned int frames_timed_out = 0; |
1281 unsigned int frames_timed_out = 0; |
1265 |
1282 |
1266 // receive datagrams |
1283 // receive datagrams |
1267 ec_device_poll(&master->main_device); |
1284 ec_device_poll(&master->main_device); |
1268 |
1285 |
|
1286 #ifdef EC_HAVE_CYCLES |
1269 cycles_timeout = (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000); |
1287 cycles_timeout = (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000); |
|
1288 #else |
|
1289 timeout_ms = max(EC_IO_TIMEOUT /* us */ / 1000, 2); |
|
1290 #endif |
1270 |
1291 |
1271 // dequeue all datagrams that timed out |
1292 // dequeue all datagrams that timed out |
1272 list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue) { |
1293 list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue) { |
1273 if (datagram->state != EC_DATAGRAM_SENT) continue; |
1294 if (datagram->state != EC_DATAGRAM_SENT) continue; |
1274 |
1295 |
|
1296 #ifdef EC_HAVE_CYCLES |
1275 if (master->main_device.cycles_poll - datagram->cycles_sent |
1297 if (master->main_device.cycles_poll - datagram->cycles_sent |
1276 > cycles_timeout) { |
1298 > cycles_timeout) { |
|
1299 #else |
|
1300 diff_ms = (master->main_device.jiffies_poll |
|
1301 - datagram->jiffies_sent) * 1000 / HZ; |
|
1302 if (diff_ms > timeout_ms) { |
|
1303 #endif |
1277 frames_timed_out = 1; |
1304 frames_timed_out = 1; |
1278 list_del_init(&datagram->queue); |
1305 list_del_init(&datagram->queue); |
1279 datagram->state = EC_DATAGRAM_TIMED_OUT; |
1306 datagram->state = EC_DATAGRAM_TIMED_OUT; |
1280 master->stats.timeouts++; |
1307 master->stats.timeouts++; |
1281 ec_master_output_stats(master); |
1308 ec_master_output_stats(master); |
1282 |
1309 |
1283 if (unlikely(master->debug_level > 0)) { |
1310 if (unlikely(master->debug_level > 0)) { |
1284 EC_DBG("TIMED OUT datagram %08x, index %02X waited %u us.\n", |
1311 EC_DBG("TIMED OUT datagram %08x, index %02X waited %u us.\n", |
1285 (unsigned int) datagram, datagram->index, |
1312 (unsigned int) datagram, datagram->index, |
|
1313 #ifdef EC_HAVE_CYCLES |
1286 (unsigned int) (master->main_device.cycles_poll |
1314 (unsigned int) (master->main_device.cycles_poll |
1287 - datagram->cycles_sent) * 1000 / cpu_khz); |
1315 - datagram->cycles_sent) * 1000 / cpu_khz |
|
1316 #else |
|
1317 (unsigned int) (diff_ms * 1000) |
|
1318 #endif |
|
1319 ); |
|
1320 |
1288 } |
1321 } |
1289 } |
1322 } |
1290 } |
1323 } |
1291 |
1324 |
1292 master->frames_timed_out = frames_timed_out; |
1325 master->frames_timed_out = frames_timed_out; |