900 */ |
900 */ |
901 void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */) |
901 void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */) |
902 { |
902 { |
903 ec_datagram_t *datagram, *next; |
903 ec_datagram_t *datagram, *next; |
904 size_t datagram_size; |
904 size_t datagram_size; |
905 uint8_t *frame_data, *cur_data; |
905 uint8_t *frame_data, *cur_data, *frame_datagram_data; |
906 void *follows_word; |
906 void *follows_word; |
907 #ifdef EC_HAVE_CYCLES |
907 #ifdef EC_HAVE_CYCLES |
908 cycles_t cycles_start, cycles_sent, cycles_end; |
908 cycles_t cycles_start, cycles_sent, cycles_end; |
909 #endif |
909 #endif |
910 unsigned long jiffies_sent; |
910 unsigned long jiffies_sent; |
911 unsigned int frame_count, more_datagrams_waiting; |
911 unsigned int frame_count, more_datagrams_waiting; |
912 struct list_head sent_datagrams; |
912 struct list_head sent_datagrams; |
|
913 ec_fmmu_config_t* domain_fmmu; |
913 |
914 |
914 #ifdef EC_HAVE_CYCLES |
915 #ifdef EC_HAVE_CYCLES |
915 cycles_start = get_cycles(); |
916 cycles_start = get_cycles(); |
916 #endif |
917 #endif |
917 frame_count = 0; |
918 frame_count = 0; |
957 EC_WRITE_U16(cur_data + 8, 0x0000); |
958 EC_WRITE_U16(cur_data + 8, 0x0000); |
958 follows_word = cur_data + 6; |
959 follows_word = cur_data + 6; |
959 cur_data += EC_DATAGRAM_HEADER_SIZE; |
960 cur_data += EC_DATAGRAM_HEADER_SIZE; |
960 |
961 |
961 // EtherCAT datagram data |
962 // EtherCAT datagram data |
962 memcpy(cur_data, datagram->data, datagram->data_size); |
963 frame_datagram_data = cur_data; |
|
964 if (datagram->domain) { |
|
965 unsigned int datagram_address = EC_READ_U32(datagram->address); |
|
966 int i = 0; |
|
967 uint8_t *domain_data = datagram->data; |
|
968 list_for_each_entry(domain_fmmu, &datagram->domain->fmmu_configs, list) { |
|
969 if (domain_fmmu->dir == EC_DIR_OUTPUT ) { |
|
970 unsigned int frame_offset = domain_fmmu->logical_start_address-datagram_address; |
|
971 memcpy(frame_datagram_data+frame_offset, domain_data, domain_fmmu->data_size); |
|
972 if (unlikely(master->debug_level > 1)) { |
|
973 EC_DBG("sending dg 0x%02X fmmu %u fp=%u dp=%u size=%u\n", |
|
974 datagram->index, i,frame_offset,domain_data-datagram->data,domain_fmmu->data_size); |
|
975 ec_print_data(domain_data, domain_fmmu->data_size); |
|
976 } |
|
977 } |
|
978 domain_data += domain_fmmu->data_size; |
|
979 ++i; |
|
980 } |
|
981 } |
|
982 else { |
|
983 memcpy(frame_datagram_data, datagram->data, datagram->data_size); |
|
984 } |
963 cur_data += datagram->data_size; |
985 cur_data += datagram->data_size; |
964 |
986 |
965 // EtherCAT datagram footer |
987 // EtherCAT datagram footer |
966 EC_WRITE_U16(cur_data, 0x0000); // reset working counter |
988 EC_WRITE_U16(cur_data, 0x0000); // reset working counter |
967 cur_data += EC_DATAGRAM_FOOTER_SIZE; |
989 cur_data += EC_DATAGRAM_FOOTER_SIZE; |
1029 ) |
1051 ) |
1030 { |
1052 { |
1031 size_t frame_size, data_size; |
1053 size_t frame_size, data_size; |
1032 uint8_t datagram_type, datagram_index; |
1054 uint8_t datagram_type, datagram_index; |
1033 unsigned int cmd_follows, matched; |
1055 unsigned int cmd_follows, matched; |
1034 const uint8_t *cur_data; |
1056 const uint8_t *cur_data, *frame_datagram_data; |
1035 ec_datagram_t *datagram; |
1057 ec_datagram_t *datagram; |
|
1058 ec_fmmu_config_t* domain_fmmu; |
1036 |
1059 |
1037 if (unlikely(size < EC_FRAME_HEADER_SIZE)) { |
1060 if (unlikely(size < EC_FRAME_HEADER_SIZE)) { |
1038 if (master->debug_level) { |
1061 if (master->debug_level) { |
1039 EC_DBG("Corrupted frame received (size %zu < %u byte):\n", |
1062 EC_DBG("Corrupted frame received (size %zu < %u byte):\n", |
1040 size, EC_FRAME_HEADER_SIZE); |
1063 size, EC_FRAME_HEADER_SIZE); |
1111 } |
1134 } |
1112 |
1135 |
1113 cur_data += data_size + EC_DATAGRAM_FOOTER_SIZE; |
1136 cur_data += data_size + EC_DATAGRAM_FOOTER_SIZE; |
1114 continue; |
1137 continue; |
1115 } |
1138 } |
1116 |
1139 frame_datagram_data = cur_data; |
1117 // copy received data into the datagram memory |
1140 if (datagram->domain) { |
1118 memcpy(datagram->data, cur_data, data_size); |
1141 size_t datagram_address = EC_READ_U32(datagram->address); |
|
1142 int i = 0; |
|
1143 uint8_t *domain_data = datagram->data; |
|
1144 list_for_each_entry(domain_fmmu, &datagram->domain->fmmu_configs, list) { |
|
1145 if (domain_fmmu->dir == EC_DIR_INPUT ) { |
|
1146 unsigned int frame_offset = domain_fmmu->logical_start_address-datagram_address; |
|
1147 memcpy(domain_data, frame_datagram_data+frame_offset, domain_fmmu->data_size); |
|
1148 if (unlikely(master->debug_level > 1)) { |
|
1149 EC_DBG("receiving dg 0x%02X fmmu %u fp=%u dp=%u size=%u\n", |
|
1150 datagram->index, i,frame_offset,domain_data-datagram->data,domain_fmmu->data_size); |
|
1151 ec_print_data(domain_data, domain_fmmu->data_size); |
|
1152 } |
|
1153 } |
|
1154 domain_data += domain_fmmu->data_size; |
|
1155 ++i; |
|
1156 } |
|
1157 } |
|
1158 else { |
|
1159 // copy received data into the datagram memory |
|
1160 memcpy(datagram->data, frame_datagram_data, data_size); |
|
1161 } |
1119 cur_data += data_size; |
1162 cur_data += data_size; |
1120 |
1163 |
1121 // set the datagram's working counter |
1164 // set the datagram's working counter |
1122 datagram->working_counter = EC_READ_U16(cur_data); |
1165 datagram->working_counter = EC_READ_U16(cur_data); |
1123 cur_data += EC_DATAGRAM_FOOTER_SIZE; |
1166 cur_data += EC_DATAGRAM_FOOTER_SIZE; |