320 \return 0 in case of success, else < 0 |
321 \return 0 in case of success, else < 0 |
321 */ |
322 */ |
322 |
323 |
323 void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */) |
324 void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */) |
324 { |
325 { |
325 ec_datagram_t *datagram; |
326 ec_datagram_t *datagram, *next; |
326 size_t datagram_size; |
327 size_t datagram_size; |
327 uint8_t *frame_data, *cur_data; |
328 uint8_t *frame_data, *cur_data; |
328 void *follows_word; |
329 void *follows_word; |
329 cycles_t cycles_start, cycles_end; |
330 cycles_t cycles_start, cycles_sent, cycles_end; |
|
331 unsigned long jiffies_sent; |
330 unsigned int frame_count, more_datagrams_waiting; |
332 unsigned int frame_count, more_datagrams_waiting; |
331 |
333 struct list_head sent_datagrams; |
|
334 |
|
335 cycles_start = get_cycles(); |
332 frame_count = 0; |
336 frame_count = 0; |
333 cycles_start = get_cycles(); |
337 INIT_LIST_HEAD(&sent_datagrams); |
334 |
338 |
335 if (unlikely(master->debug_level > 1)) |
339 if (unlikely(master->debug_level > 1)) |
336 EC_DBG("ec_master_send_datagrams\n"); |
340 EC_DBG("ec_master_send_datagrams\n"); |
337 |
341 |
338 do { |
342 do { |
352 if (cur_data - frame_data + datagram_size > ETH_DATA_LEN) { |
356 if (cur_data - frame_data + datagram_size > ETH_DATA_LEN) { |
353 more_datagrams_waiting = 1; |
357 more_datagrams_waiting = 1; |
354 break; |
358 break; |
355 } |
359 } |
356 |
360 |
357 datagram->state = EC_DATAGRAM_SENT; |
361 list_add_tail(&datagram->sent, &sent_datagrams); |
358 datagram->cycles_sent = cycles_start; |
|
359 datagram->check_once_more = 1; |
|
360 datagram->index = master->datagram_index++; |
362 datagram->index = master->datagram_index++; |
361 |
363 |
362 if (unlikely(master->debug_level > 1)) |
364 if (unlikely(master->debug_level > 1)) |
363 EC_DBG("adding datagram 0x%02X\n", datagram->index); |
365 EC_DBG("adding datagram 0x%02X\n", datagram->index); |
364 |
366 |
382 // EtherCAT datagram footer |
384 // EtherCAT datagram footer |
383 EC_WRITE_U16(cur_data, 0x0000); // reset working counter |
385 EC_WRITE_U16(cur_data, 0x0000); // reset working counter |
384 cur_data += EC_DATAGRAM_FOOTER_SIZE; |
386 cur_data += EC_DATAGRAM_FOOTER_SIZE; |
385 } |
387 } |
386 |
388 |
387 if (cur_data - frame_data == EC_FRAME_HEADER_SIZE) { |
389 if (list_empty(&sent_datagrams)) { |
388 if (unlikely(master->debug_level > 1)) |
390 if (unlikely(master->debug_level > 1)) |
389 EC_DBG("nothing to send.\n"); |
391 EC_DBG("nothing to send.\n"); |
390 break; |
392 break; |
391 } |
393 } |
392 |
394 |
401 if (unlikely(master->debug_level > 1)) |
403 if (unlikely(master->debug_level > 1)) |
402 EC_DBG("frame size: %i\n", cur_data - frame_data); |
404 EC_DBG("frame size: %i\n", cur_data - frame_data); |
403 |
405 |
404 // send frame |
406 // send frame |
405 ec_device_send(master->device, cur_data - frame_data); |
407 ec_device_send(master->device, cur_data - frame_data); |
|
408 cycles_sent = get_cycles(); |
|
409 jiffies_sent = jiffies; |
|
410 |
|
411 // set datagram states and sending timestamps |
|
412 list_for_each_entry_safe(datagram, next, &sent_datagrams, sent) { |
|
413 datagram->state = EC_DATAGRAM_SENT; |
|
414 datagram->cycles_sent = cycles_sent; |
|
415 datagram->jiffies_sent = jiffies_sent; |
|
416 list_del_init(&datagram->sent); // empty list of sent datagrams |
|
417 } |
|
418 |
406 frame_count++; |
419 frame_count++; |
407 } |
420 } |
408 while (more_datagrams_waiting); |
421 while (more_datagrams_waiting); |
409 |
422 |
410 if (unlikely(master->debug_level > 1)) { |
423 if (unlikely(master->debug_level > 1)) { |
496 datagram->working_counter = EC_READ_U16(cur_data); |
509 datagram->working_counter = EC_READ_U16(cur_data); |
497 cur_data += EC_DATAGRAM_FOOTER_SIZE; |
510 cur_data += EC_DATAGRAM_FOOTER_SIZE; |
498 |
511 |
499 // dequeue the received datagram |
512 // dequeue the received datagram |
500 datagram->state = EC_DATAGRAM_RECEIVED; |
513 datagram->state = EC_DATAGRAM_RECEIVED; |
|
514 datagram->cycles_received = master->device->cycles_isr; |
|
515 datagram->jiffies_received = master->device->jiffies_isr; |
501 list_del_init(&datagram->queue); |
516 list_del_init(&datagram->queue); |
502 } |
517 } |
503 } |
518 } |
504 |
519 |
505 /*****************************************************************************/ |
520 /*****************************************************************************/ |
1061 */ |
1076 */ |
1062 |
1077 |
1063 int ec_master_measure_bus_time(ec_master_t *master) |
1078 int ec_master_measure_bus_time(ec_master_t *master) |
1064 { |
1079 { |
1065 ec_datagram_t datagram; |
1080 ec_datagram_t datagram; |
1066 cycles_t cycles_start, cycles_end, cycles_timeout; |
1081 uint32_t cur, sum, min, max, i; |
1067 uint32_t times[100], sum, min, max, i; |
|
1068 |
1082 |
1069 ec_datagram_init(&datagram); |
1083 ec_datagram_init(&datagram); |
1070 |
1084 |
1071 if (ec_datagram_brd(&datagram, 0x130, 2)) { |
1085 if (ec_datagram_brd(&datagram, 0x130, 2)) { |
1072 EC_ERR("Failed to allocate datagram for bus time measuring.\n"); |
1086 EC_ERR("Failed to allocate datagram for bus time measuring.\n"); |
1073 ec_datagram_clear(&datagram); |
1087 ec_datagram_clear(&datagram); |
1074 return -1; |
1088 return -1; |
1075 } |
1089 } |
1076 |
1090 |
1077 cycles_timeout = (cycles_t) EC_IO_TIMEOUT * (cpu_khz / 1000); |
1091 ecrt_master_receive(master); |
1078 |
1092 |
1079 sum = 0; |
1093 sum = 0; |
1080 min = 0xFFFFFFFF; |
1094 min = 0xFFFFFFFF; |
1081 max = 0; |
1095 max = 0; |
1082 |
1096 |
1083 for (i = 0; i < 100; i++) { |
1097 for (i = 0; i < 100; i++) { |
1084 ec_master_queue_datagram(master, &datagram); |
1098 ec_master_queue_datagram(master, &datagram); |
1085 ecrt_master_send(master); |
1099 ecrt_master_send(master); |
1086 cycles_start = get_cycles(); |
1100 |
1087 |
1101 while (1) { |
1088 while (1) { // active waiting |
1102 ecrt_master_receive(master); |
1089 ec_device_call_isr(master->device); |
|
1090 cycles_end = get_cycles(); // take current time |
|
1091 |
1103 |
1092 if (datagram.state == EC_DATAGRAM_RECEIVED) { |
1104 if (datagram.state == EC_DATAGRAM_RECEIVED) { |
1093 break; |
1105 break; |
1094 } |
1106 } |
1095 else if (datagram.state == EC_DATAGRAM_ERROR) { |
1107 else if (datagram.state == EC_DATAGRAM_ERROR) { |
1096 EC_WARN("Failed to measure bus time.\n"); |
1108 EC_WARN("Failed to measure bus time.\n"); |
1097 goto error; |
1109 goto error; |
1098 } |
1110 } |
1099 else if (cycles_end - cycles_start >= cycles_timeout) { |
1111 else if (datagram.state == EC_DATAGRAM_TIMED_OUT) { |
1100 EC_WARN("Timeout while measuring bus time.\n"); |
1112 EC_WARN("Timeout while measuring bus time.\n"); |
1101 goto error; |
1113 goto error; |
1102 } |
1114 } |
1103 } |
1115 } |
1104 |
1116 |
1105 times[i] = (unsigned int) (cycles_end - cycles_start) * 1000 / cpu_khz; |
1117 cur = (unsigned int) (datagram.cycles_received |
1106 sum += times[i]; |
1118 - datagram.cycles_sent) * 1000 / cpu_khz; |
1107 if (times[i] > max) max = times[i]; |
1119 sum += cur; |
1108 if (times[i] < min) min = times[i]; |
1120 if (cur > max) max = cur; |
1109 } |
1121 if (cur < min) min = cur; |
1110 |
1122 } |
1111 EC_INFO("Bus time is (min/avg/max) %u/%u.%u/%u us.\n", |
1123 |
|
1124 EC_INFO("Bus time is (min/avg/max) %u / %u.%u / %u us.\n", |
1112 min, sum / 100, sum % 100, max); |
1125 min, sum / 100, sum % 100, max); |
|
1126 ec_datagram_clear(&datagram); |
1113 return 0; |
1127 return 0; |
1114 |
1128 |
1115 error: |
1129 error: |
1116 // Dequeue and free datagram |
|
1117 list_del(&datagram.queue); |
|
1118 ec_datagram_clear(&datagram); |
1130 ec_datagram_clear(&datagram); |
1119 return -1; |
1131 return -1; |
1120 } |
1132 } |
1121 |
1133 |
1122 /****************************************************************************** |
1134 /****************************************************************************** |
1307 */ |
1319 */ |
1308 |
1320 |
1309 void ecrt_master_receive(ec_master_t *master /**< EtherCAT master */) |
1321 void ecrt_master_receive(ec_master_t *master /**< EtherCAT master */) |
1310 { |
1322 { |
1311 ec_datagram_t *datagram, *next; |
1323 ec_datagram_t *datagram, *next; |
1312 cycles_t cycles_received, cycles_timeout; |
1324 cycles_t cycles_timeout; |
1313 |
1325 |
|
1326 // receive datagrams |
1314 ec_device_call_isr(master->device); |
1327 ec_device_call_isr(master->device); |
1315 |
1328 |
1316 cycles_received = get_cycles(); |
1329 cycles_timeout = (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000); |
1317 cycles_timeout = EC_IO_TIMEOUT /* us */ * cpu_khz / 1000; |
|
1318 |
1330 |
1319 // dequeue all datagrams that timed out |
1331 // dequeue all datagrams that timed out |
1320 list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue) { |
1332 list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue) { |
1321 switch (datagram->state) { |
1333 switch (datagram->state) { |
|
1334 case EC_DATAGRAM_QUEUED: |
|
1335 if (master->device->cycles_isr |
|
1336 - datagram->cycles_queued > cycles_timeout) { |
|
1337 list_del_init(&datagram->queue); |
|
1338 datagram->state = EC_DATAGRAM_TIMED_OUT; |
|
1339 master->stats.timeouts++; |
|
1340 ec_master_output_stats(master); |
|
1341 } |
|
1342 break; |
1322 case EC_DATAGRAM_SENT: |
1343 case EC_DATAGRAM_SENT: |
1323 case EC_DATAGRAM_QUEUED: |
1344 if (master->device->cycles_isr |
1324 if (cycles_received - datagram->cycles_sent > cycles_timeout) { |
1345 - datagram->cycles_sent > cycles_timeout) { |
1325 if (datagram->state == EC_DATAGRAM_SENT |
|
1326 && datagram->check_once_more) { |
|
1327 datagram->check_once_more = 0; |
|
1328 break; |
|
1329 } |
|
1330 list_del_init(&datagram->queue); |
1346 list_del_init(&datagram->queue); |
1331 datagram->state = EC_DATAGRAM_TIMED_OUT; |
1347 datagram->state = EC_DATAGRAM_TIMED_OUT; |
1332 master->stats.timeouts++; |
1348 master->stats.timeouts++; |
1333 ec_master_output_stats(master); |
1349 ec_master_output_stats(master); |
1334 } |
1350 } |
1357 list_for_each_entry(domain, &master->domains, list) |
1373 list_for_each_entry(domain, &master->domains, list) |
1358 ec_domain_queue(domain); |
1374 ec_domain_queue(domain); |
1359 |
1375 |
1360 ecrt_master_send(master); |
1376 ecrt_master_send(master); |
1361 |
1377 |
1362 cycles_start = get_cycles(); // take sending time |
1378 cycles_start = get_cycles(); |
1363 cycles_timeout = (cycles_t) EC_IO_TIMEOUT * (cpu_khz / 1000); |
1379 cycles_timeout = (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000); |
1364 |
1380 |
1365 // active waiting |
1381 // active waiting |
1366 while (1) { |
1382 while (1) { |
|
1383 udelay(100); |
1367 cycles_end = get_cycles(); |
1384 cycles_end = get_cycles(); |
1368 if (cycles_end - cycles_start >= cycles_timeout) break; |
1385 if (cycles_end - cycles_start >= cycles_timeout) break; |
1369 } |
1386 } |
1370 } |
1387 } |
1371 |
1388 |