718 #endif |
718 #endif |
719 #ifdef EC_HAVE_CYCLES |
719 #ifdef EC_HAVE_CYCLES |
720 datagram->cycles_sent = 0; |
720 datagram->cycles_sent = 0; |
721 #endif |
721 #endif |
722 datagram->jiffies_sent = 0; |
722 datagram->jiffies_sent = 0; |
723 ec_master_queue_datagram(master, datagram); |
723 ec_master_queue_datagram(master, datagram, |
724 } |
724 datagram->device_index); |
725 else { |
725 } else { |
726 if (datagram->data_size > master->max_queue_size) { |
726 if (datagram->data_size > master->max_queue_size) { |
727 list_del_init(&datagram->queue); |
727 list_del_init(&datagram->queue); |
728 datagram->state = EC_DATAGRAM_ERROR; |
728 datagram->state = EC_DATAGRAM_ERROR; |
729 EC_MASTER_ERR(master, "External datagram %p is too large," |
729 EC_MASTER_ERR(master, "External datagram %p is too large," |
730 " size=%zu, max_queue_size=%zu\n", |
730 " size=%zu, max_queue_size=%zu\n", |
791 |
791 |
792 /** Places an external datagram in the sdo datagram queue. |
792 /** Places an external datagram in the sdo datagram queue. |
793 */ |
793 */ |
794 void ec_master_queue_external_datagram( |
794 void ec_master_queue_external_datagram( |
795 ec_master_t *master, /**< EtherCAT master */ |
795 ec_master_t *master, /**< EtherCAT master */ |
796 ec_datagram_t *datagram /**< datagram */ |
796 ec_datagram_t *datagram, /**< datagram */ |
|
797 ec_device_index_t device_index /**< Device index. */ |
797 ) |
798 ) |
798 { |
799 { |
799 ec_datagram_t *queued_datagram; |
800 ec_datagram_t *queued_datagram; |
800 |
801 |
801 down(&master->io_sem); |
802 down(&master->io_sem); |
814 datagram, datagram->data_size); |
815 datagram, datagram->data_size); |
815 #endif |
816 #endif |
816 |
817 |
817 list_add_tail(&datagram->queue, &master->external_datagram_queue); |
818 list_add_tail(&datagram->queue, &master->external_datagram_queue); |
818 datagram->state = EC_DATAGRAM_QUEUED; |
819 datagram->state = EC_DATAGRAM_QUEUED; |
|
820 datagram->device_index = device_index; |
819 #ifdef EC_HAVE_CYCLES |
821 #ifdef EC_HAVE_CYCLES |
820 datagram->cycles_sent = get_cycles(); |
822 datagram->cycles_sent = get_cycles(); |
821 #endif |
823 #endif |
822 datagram->jiffies_sent = jiffies; |
824 datagram->jiffies_sent = jiffies; |
823 |
825 |
829 |
831 |
830 /** Places a datagram in the datagram queue. |
832 /** Places a datagram in the datagram queue. |
831 */ |
833 */ |
832 void ec_master_queue_datagram( |
834 void ec_master_queue_datagram( |
833 ec_master_t *master, /**< EtherCAT master */ |
835 ec_master_t *master, /**< EtherCAT master */ |
834 ec_datagram_t *datagram /**< datagram */ |
836 ec_datagram_t *datagram, /**< datagram */ |
|
837 ec_device_index_t device_index /**< Device index. */ |
835 ) |
838 ) |
836 { |
839 { |
837 ec_datagram_t *queued_datagram; |
840 ec_datagram_t *queued_datagram; |
838 |
841 |
839 switch (datagram->state) { |
842 switch (datagram->state) { |
869 } |
872 } |
870 } |
873 } |
871 |
874 |
872 list_add_tail(&datagram->queue, &master->datagram_queue); |
875 list_add_tail(&datagram->queue, &master->datagram_queue); |
873 datagram->state = EC_DATAGRAM_QUEUED; |
876 datagram->state = EC_DATAGRAM_QUEUED; |
|
877 datagram->device_index = device_index; |
874 } |
878 } |
875 |
879 |
876 /*****************************************************************************/ |
880 /*****************************************************************************/ |
877 |
881 |
878 /** Places a datagram in the non-application datagram queue. |
882 /** Places a datagram in the non-application datagram queue. |
887 up(&master->ext_queue_sem); |
891 up(&master->ext_queue_sem); |
888 } |
892 } |
889 |
893 |
890 /*****************************************************************************/ |
894 /*****************************************************************************/ |
891 |
895 |
892 /** Sends the datagrams in the queue. |
896 /** Sends the datagrams in the queue for a certain device. |
893 * |
897 * |
894 */ |
898 */ |
895 void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */) |
899 void ec_master_send_datagrams( |
|
900 ec_master_t *master, /**< EtherCAT master */ |
|
901 ec_device_index_t device_index /**< Device index. */ |
|
902 ) |
896 { |
903 { |
897 ec_datagram_t *datagram, *next; |
904 ec_datagram_t *datagram, *next; |
898 size_t datagram_size; |
905 size_t datagram_size; |
899 uint8_t *frame_data, *cur_data; |
906 uint8_t *frame_data, *cur_data = NULL; |
900 void *follows_word; |
907 void *follows_word; |
901 #ifdef EC_HAVE_CYCLES |
908 #ifdef EC_HAVE_CYCLES |
902 cycles_t cycles_start, cycles_sent, cycles_end; |
909 cycles_t cycles_start, cycles_sent, cycles_end; |
903 #endif |
910 #endif |
904 unsigned long jiffies_sent; |
911 unsigned long jiffies_sent; |
909 cycles_start = get_cycles(); |
916 cycles_start = get_cycles(); |
910 #endif |
917 #endif |
911 frame_count = 0; |
918 frame_count = 0; |
912 INIT_LIST_HEAD(&sent_datagrams); |
919 INIT_LIST_HEAD(&sent_datagrams); |
913 |
920 |
914 EC_MASTER_DBG(master, 2, "ec_master_send_datagrams\n"); |
921 EC_MASTER_DBG(master, 2, "%s(device_index = %u)\n", |
|
922 __func__, device_index); |
915 |
923 |
916 do { |
924 do { |
917 // fetch pointer to transmit socket buffer |
925 frame_data = NULL; |
918 frame_data = ec_device_tx_data(&master->devices[EC_DEVICE_MAIN]); |
|
919 cur_data = frame_data + EC_FRAME_HEADER_SIZE; |
|
920 follows_word = NULL; |
926 follows_word = NULL; |
921 more_datagrams_waiting = 0; |
927 more_datagrams_waiting = 0; |
922 |
928 |
923 // fill current frame with datagrams |
929 // fill current frame with datagrams |
924 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
930 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
925 if (datagram->state != EC_DATAGRAM_QUEUED) continue; |
931 if (datagram->state != EC_DATAGRAM_QUEUED || |
|
932 datagram->device_index != device_index) { |
|
933 continue; |
|
934 } |
|
935 |
|
936 if (!frame_data) { |
|
937 // fetch pointer to transmit socket buffer |
|
938 frame_data = |
|
939 ec_device_tx_data(&master->devices[device_index]); |
|
940 cur_data = frame_data + EC_FRAME_HEADER_SIZE; |
|
941 } |
926 |
942 |
927 // does the current datagram fit in the frame? |
943 // does the current datagram fit in the frame? |
928 datagram_size = EC_DATAGRAM_HEADER_SIZE + datagram->data_size |
944 datagram_size = EC_DATAGRAM_HEADER_SIZE + datagram->data_size |
929 + EC_DATAGRAM_FOOTER_SIZE; |
945 + EC_DATAGRAM_FOOTER_SIZE; |
930 if (cur_data - frame_data + datagram_size > ETH_DATA_LEN) { |
946 if (cur_data - frame_data + datagram_size > ETH_DATA_LEN) { |
933 } |
949 } |
934 |
950 |
935 list_add_tail(&datagram->sent, &sent_datagrams); |
951 list_add_tail(&datagram->sent, &sent_datagrams); |
936 datagram->index = master->datagram_index++; |
952 datagram->index = master->datagram_index++; |
937 |
953 |
938 EC_MASTER_DBG(master, 2, "adding datagram 0x%02X\n", |
954 EC_MASTER_DBG(master, 2, "Adding datagram 0x%02X\n", |
939 datagram->index); |
955 datagram->index); |
940 |
956 |
941 // set "datagram following" flag in previous frame |
957 // set "datagram following" flag in previous datagram |
942 if (follows_word) |
958 if (follows_word) { |
943 EC_WRITE_U16(follows_word, |
959 EC_WRITE_U16(follows_word, |
944 EC_READ_U16(follows_word) | 0x8000); |
960 EC_READ_U16(follows_word) | 0x8000); |
|
961 } |
945 |
962 |
946 // EtherCAT datagram header |
963 // EtherCAT datagram header |
947 EC_WRITE_U8 (cur_data, datagram->type); |
964 EC_WRITE_U8 (cur_data, datagram->type); |
948 EC_WRITE_U8 (cur_data + 1, datagram->index); |
965 EC_WRITE_U8 (cur_data + 1, datagram->index); |
949 memcpy(cur_data + 2, datagram->address, EC_ADDR_LEN); |
966 memcpy(cur_data + 2, datagram->address, EC_ADDR_LEN); |
950 EC_WRITE_U16(cur_data + 6, datagram->data_size & 0x7FF); |
967 EC_WRITE_U16(cur_data + 6, datagram->data_size & 0x7FF); |
951 EC_WRITE_U16(cur_data + 8, 0x0000); |
968 EC_WRITE_U16(cur_data + 8, 0x0000); |
952 follows_word = cur_data + 6; |
969 follows_word = cur_data + 6; |
975 EC_WRITE_U8(cur_data++, 0x00); |
992 EC_WRITE_U8(cur_data++, 0x00); |
976 |
993 |
977 EC_MASTER_DBG(master, 2, "frame size: %zu\n", cur_data - frame_data); |
994 EC_MASTER_DBG(master, 2, "frame size: %zu\n", cur_data - frame_data); |
978 |
995 |
979 // send frame |
996 // send frame |
980 ec_device_send(&master->devices[EC_DEVICE_MAIN], |
997 ec_device_send(&master->devices[device_index], |
981 cur_data - frame_data); |
998 cur_data - frame_data); |
982 #ifdef EC_HAVE_CYCLES |
999 #ifdef EC_HAVE_CYCLES |
983 cycles_sent = get_cycles(); |
1000 cycles_sent = get_cycles(); |
984 #endif |
1001 #endif |
985 jiffies_sent = jiffies; |
1002 jiffies_sent = jiffies; |
999 while (more_datagrams_waiting); |
1016 while (more_datagrams_waiting); |
1000 |
1017 |
1001 #ifdef EC_HAVE_CYCLES |
1018 #ifdef EC_HAVE_CYCLES |
1002 if (unlikely(master->debug_level > 1)) { |
1019 if (unlikely(master->debug_level > 1)) { |
1003 cycles_end = get_cycles(); |
1020 cycles_end = get_cycles(); |
1004 EC_MASTER_DBG(master, 0, "ec_master_send_datagrams" |
1021 EC_MASTER_DBG(master, 0, "%s()" |
1005 " sent %u frames in %uus.\n", frame_count, |
1022 " sent %u frames in %uus.\n", __func__, frame_count, |
1006 (unsigned int) (cycles_end - cycles_start) * 1000 / cpu_khz); |
1023 (unsigned int) (cycles_end - cycles_start) * 1000 / cpu_khz); |
1007 } |
1024 } |
1008 #endif |
1025 #endif |
1009 } |
1026 } |
1010 |
1027 |
1361 up(&master->master_sem); |
1378 up(&master->master_sem); |
1362 |
1379 |
1363 // queue and send |
1380 // queue and send |
1364 down(&master->io_sem); |
1381 down(&master->io_sem); |
1365 if (fsm_exec) { |
1382 if (fsm_exec) { |
1366 ec_master_queue_datagram(master, &master->fsm_datagram); |
1383 ec_master_queue_datagram(master, &master->fsm_datagram, |
|
1384 EC_DEVICE_MAIN); |
1367 } |
1385 } |
1368 ec_master_inject_external_datagrams(master); |
1386 ec_master_inject_external_datagrams(master); |
1369 ecrt_master_send(master); |
1387 ecrt_master_send(master); |
1370 sent_bytes = master->devices[EC_DEVICE_MAIN].tx_skb[ |
1388 sent_bytes = master->devices[EC_DEVICE_MAIN].tx_skb[ |
1371 master->devices[EC_DEVICE_MAIN].tx_ring_index]->len; |
1389 master->devices[EC_DEVICE_MAIN].tx_ring_index]->len; |
2188 /*****************************************************************************/ |
2206 /*****************************************************************************/ |
2189 |
2207 |
2190 void ecrt_master_send(ec_master_t *master) |
2208 void ecrt_master_send(ec_master_t *master) |
2191 { |
2209 { |
2192 ec_datagram_t *datagram, *n; |
2210 ec_datagram_t *datagram, *n; |
|
2211 unsigned int i; |
2193 |
2212 |
2194 if (master->injection_seq_rt != master->injection_seq_fsm) { |
2213 if (master->injection_seq_rt != master->injection_seq_fsm) { |
2195 // inject datagrams produced by master & slave FSMs |
2214 // inject datagrams produced by master & slave FSMs |
2196 ec_master_queue_datagram(master, &master->fsm_datagram); |
2215 ec_master_queue_datagram(master, &master->fsm_datagram, |
|
2216 EC_DEVICE_MAIN); |
2197 master->injection_seq_rt = master->injection_seq_fsm; |
2217 master->injection_seq_rt = master->injection_seq_fsm; |
2198 } |
2218 } |
2199 ec_master_inject_external_datagrams(master); |
2219 ec_master_inject_external_datagrams(master); |
2200 |
2220 |
2201 if (unlikely(!master->devices[EC_DEVICE_MAIN].link_state)) { |
2221 if (unlikely(!master->devices[EC_DEVICE_MAIN].link_state)) { |
2213 ec_device_clear_stats(&master->devices[EC_DEVICE_MAIN]); |
2233 ec_device_clear_stats(&master->devices[EC_DEVICE_MAIN]); |
2214 return; |
2234 return; |
2215 } |
2235 } |
2216 |
2236 |
2217 // send frames |
2237 // send frames |
2218 ec_master_send_datagrams(master); |
2238 for (i = 0; i < EC_NUM_DEVICES; i++) { |
|
2239 if (master->devices[i].dev) { |
|
2240 ec_master_send_datagrams(master, i); |
|
2241 } |
|
2242 } |
2219 } |
2243 } |
2220 |
2244 |
2221 /*****************************************************************************/ |
2245 /*****************************************************************************/ |
2222 |
2246 |
2223 void ecrt_master_receive(ec_master_t *master) |
2247 void ecrt_master_receive(ec_master_t *master) |
2273 ec_datagram_t *datagram, *next; |
2297 ec_datagram_t *datagram, *next; |
2274 |
2298 |
2275 list_for_each_entry_safe(datagram, next, &master->ext_datagram_queue, |
2299 list_for_each_entry_safe(datagram, next, &master->ext_datagram_queue, |
2276 queue) { |
2300 queue) { |
2277 list_del(&datagram->queue); |
2301 list_del(&datagram->queue); |
2278 ec_master_queue_datagram(master, datagram); |
2302 ec_master_queue_datagram(master, datagram, EC_DEVICE_MAIN); |
2279 } |
2303 } |
2280 |
2304 |
2281 ecrt_master_send(master); |
2305 ecrt_master_send(master); |
2282 } |
2306 } |
2283 |
2307 |
2439 /*****************************************************************************/ |
2463 /*****************************************************************************/ |
2440 |
2464 |
2441 void ecrt_master_sync_reference_clock(ec_master_t *master) |
2465 void ecrt_master_sync_reference_clock(ec_master_t *master) |
2442 { |
2466 { |
2443 EC_WRITE_U32(master->ref_sync_datagram.data, master->app_time); |
2467 EC_WRITE_U32(master->ref_sync_datagram.data, master->app_time); |
2444 ec_master_queue_datagram(master, &master->ref_sync_datagram); |
2468 ec_master_queue_datagram(master, &master->ref_sync_datagram, |
|
2469 EC_DEVICE_MAIN); |
2445 } |
2470 } |
2446 |
2471 |
2447 /*****************************************************************************/ |
2472 /*****************************************************************************/ |
2448 |
2473 |
2449 void ecrt_master_sync_slave_clocks(ec_master_t *master) |
2474 void ecrt_master_sync_slave_clocks(ec_master_t *master) |
2450 { |
2475 { |
2451 ec_datagram_zero(&master->sync_datagram); |
2476 ec_datagram_zero(&master->sync_datagram); |
2452 ec_master_queue_datagram(master, &master->sync_datagram); |
2477 ec_master_queue_datagram(master, &master->sync_datagram, |
|
2478 EC_DEVICE_MAIN); |
2453 } |
2479 } |
2454 |
2480 |
2455 /*****************************************************************************/ |
2481 /*****************************************************************************/ |
2456 |
2482 |
2457 void ecrt_master_sync_monitor_queue(ec_master_t *master) |
2483 void ecrt_master_sync_monitor_queue(ec_master_t *master) |
2458 { |
2484 { |
2459 ec_datagram_zero(&master->sync_mon_datagram); |
2485 ec_datagram_zero(&master->sync_mon_datagram); |
2460 ec_master_queue_datagram(master, &master->sync_mon_datagram); |
2486 ec_master_queue_datagram(master, &master->sync_mon_datagram, |
|
2487 EC_DEVICE_MAIN); |
2461 } |
2488 } |
2462 |
2489 |
2463 /*****************************************************************************/ |
2490 /*****************************************************************************/ |
2464 |
2491 |
2465 uint32_t ecrt_master_sync_monitor_process(ec_master_t *master) |
2492 uint32_t ecrt_master_sync_monitor_process(ec_master_t *master) |