108 |
108 |
109 master->index = index; |
109 master->index = index; |
110 master->device = NULL; |
110 master->device = NULL; |
111 master->reserved = 0; |
111 master->reserved = 0; |
112 INIT_LIST_HEAD(&master->slaves); |
112 INIT_LIST_HEAD(&master->slaves); |
113 INIT_LIST_HEAD(&master->command_queue); |
113 INIT_LIST_HEAD(&master->datagram_queue); |
114 INIT_LIST_HEAD(&master->domains); |
114 INIT_LIST_HEAD(&master->domains); |
115 INIT_LIST_HEAD(&master->eoe_handlers); |
115 INIT_LIST_HEAD(&master->eoe_handlers); |
116 ec_command_init(&master->simple_command); |
116 ec_datagram_init(&master->simple_datagram); |
117 INIT_WORK(&master->freerun_work, ec_master_freerun, (void *) master); |
117 INIT_WORK(&master->freerun_work, ec_master_freerun, (void *) master); |
118 init_timer(&master->eoe_timer); |
118 init_timer(&master->eoe_timer); |
119 master->eoe_timer.function = ec_master_eoe_run; |
119 master->eoe_timer.function = ec_master_eoe_run; |
120 master->eoe_timer.data = (unsigned long) master; |
120 master->eoe_timer.data = (unsigned long) master; |
121 master->internal_lock = SPIN_LOCK_UNLOCKED; |
121 master->internal_lock = SPIN_LOCK_UNLOCKED; |
210 called, to free the slave list, domains etc. |
210 called, to free the slave list, domains etc. |
211 */ |
211 */ |
212 |
212 |
213 void ec_master_reset(ec_master_t *master /**< EtherCAT master */) |
213 void ec_master_reset(ec_master_t *master /**< EtherCAT master */) |
214 { |
214 { |
215 ec_command_t *command, *next_c; |
215 ec_datagram_t *datagram, *next_c; |
216 ec_domain_t *domain, *next_d; |
216 ec_domain_t *domain, *next_d; |
217 |
217 |
218 ec_master_eoe_stop(master); |
218 ec_master_eoe_stop(master); |
219 ec_master_freerun_stop(master); |
219 ec_master_freerun_stop(master); |
220 ec_master_clear_slaves(master); |
220 ec_master_clear_slaves(master); |
221 |
221 |
222 // empty command queue |
222 // empty datagram queue |
223 list_for_each_entry_safe(command, next_c, &master->command_queue, queue) { |
223 list_for_each_entry_safe(datagram, next_c, |
224 list_del_init(&command->queue); |
224 &master->datagram_queue, queue) { |
225 command->state = EC_CMD_ERROR; |
225 list_del_init(&datagram->queue); |
|
226 datagram->state = EC_CMD_ERROR; |
226 } |
227 } |
227 |
228 |
228 // clear domains |
229 // clear domains |
229 list_for_each_entry_safe(domain, next_d, &master->domains, list) { |
230 list_for_each_entry_safe(domain, next_d, &master->domains, list) { |
230 list_del(&domain->list); |
231 list_del(&domain->list); |
231 kobject_del(&domain->kobj); |
232 kobject_del(&domain->kobj); |
232 kobject_put(&domain->kobj); |
233 kobject_put(&domain->kobj); |
233 } |
234 } |
234 |
235 |
235 master->command_index = 0; |
236 master->datagram_index = 0; |
236 master->debug_level = 0; |
237 master->debug_level = 0; |
237 master->timeout = 500; // 500us |
238 master->timeout = 500; // 500us |
238 |
239 |
239 master->stats.timeouts = 0; |
240 master->stats.timeouts = 0; |
240 master->stats.delayed = 0; |
241 master->stats.delayed = 0; |
272 } |
273 } |
273 |
274 |
274 /*****************************************************************************/ |
275 /*****************************************************************************/ |
275 |
276 |
276 /** |
277 /** |
277 Places a command in the command queue. |
278 Places a datagram in the datagram queue. |
278 */ |
279 */ |
279 |
280 |
280 void ec_master_queue_command(ec_master_t *master, /**< EtherCAT master */ |
281 void ec_master_queue_datagram(ec_master_t *master, /**< EtherCAT master */ |
281 ec_command_t *command /**< command */ |
282 ec_datagram_t *datagram /**< datagram */ |
282 ) |
283 ) |
283 { |
284 { |
284 ec_command_t *queued_command; |
285 ec_datagram_t *queued_datagram; |
285 |
286 |
286 // check, if the command is already queued |
287 // check, if the datagram is already queued |
287 list_for_each_entry(queued_command, &master->command_queue, queue) { |
288 list_for_each_entry(queued_datagram, &master->datagram_queue, queue) { |
288 if (queued_command == command) { |
289 if (queued_datagram == datagram) { |
289 command->state = EC_CMD_QUEUED; |
290 datagram->state = EC_CMD_QUEUED; |
290 if (unlikely(master->debug_level)) |
291 if (unlikely(master->debug_level)) |
291 EC_WARN("command already queued.\n"); |
292 EC_WARN("datagram already queued.\n"); |
292 return; |
293 return; |
293 } |
294 } |
294 } |
295 } |
295 |
296 |
296 list_add_tail(&command->queue, &master->command_queue); |
297 list_add_tail(&datagram->queue, &master->datagram_queue); |
297 command->state = EC_CMD_QUEUED; |
298 datagram->state = EC_CMD_QUEUED; |
298 } |
299 } |
299 |
300 |
300 /*****************************************************************************/ |
301 /*****************************************************************************/ |
301 |
302 |
302 /** |
303 /** |
303 Sends the commands in the queue. |
304 Sends the datagrams in the queue. |
304 \return 0 in case of success, else < 0 |
305 \return 0 in case of success, else < 0 |
305 */ |
306 */ |
306 |
307 |
307 void ec_master_send_commands(ec_master_t *master /**< EtherCAT master */) |
308 void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */) |
308 { |
309 { |
309 ec_command_t *command; |
310 ec_datagram_t *datagram; |
310 size_t command_size; |
311 size_t datagram_size; |
311 uint8_t *frame_data, *cur_data; |
312 uint8_t *frame_data, *cur_data; |
312 void *follows_word; |
313 void *follows_word; |
313 cycles_t t_start, t_end; |
314 cycles_t t_start, t_end; |
314 unsigned int frame_count, more_commands_waiting; |
315 unsigned int frame_count, more_datagrams_waiting; |
315 |
316 |
316 frame_count = 0; |
317 frame_count = 0; |
317 t_start = get_cycles(); |
318 t_start = get_cycles(); |
318 |
319 |
319 if (unlikely(master->debug_level > 0)) |
320 if (unlikely(master->debug_level > 0)) |
320 EC_DBG("ec_master_send_commands\n"); |
321 EC_DBG("ec_master_send_datagrams\n"); |
321 |
322 |
322 do { |
323 do { |
323 // fetch pointer to transmit socket buffer |
324 // fetch pointer to transmit socket buffer |
324 frame_data = ec_device_tx_data(master->device); |
325 frame_data = ec_device_tx_data(master->device); |
325 cur_data = frame_data + EC_FRAME_HEADER_SIZE; |
326 cur_data = frame_data + EC_FRAME_HEADER_SIZE; |
326 follows_word = NULL; |
327 follows_word = NULL; |
327 more_commands_waiting = 0; |
328 more_datagrams_waiting = 0; |
328 |
329 |
329 // fill current frame with commands |
330 // fill current frame with datagrams |
330 list_for_each_entry(command, &master->command_queue, queue) { |
331 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
331 if (command->state != EC_CMD_QUEUED) continue; |
332 if (datagram->state != EC_CMD_QUEUED) continue; |
332 |
333 |
333 // does the current command fit in the frame? |
334 // does the current datagram fit in the frame? |
334 command_size = EC_COMMAND_HEADER_SIZE + command->data_size |
335 datagram_size = EC_DATAGRAM_HEADER_SIZE + datagram->data_size |
335 + EC_COMMAND_FOOTER_SIZE; |
336 + EC_DATAGRAM_FOOTER_SIZE; |
336 if (cur_data - frame_data + command_size > ETH_DATA_LEN) { |
337 if (cur_data - frame_data + datagram_size > ETH_DATA_LEN) { |
337 more_commands_waiting = 1; |
338 more_datagrams_waiting = 1; |
338 break; |
339 break; |
339 } |
340 } |
340 |
341 |
341 command->state = EC_CMD_SENT; |
342 datagram->state = EC_CMD_SENT; |
342 command->t_sent = t_start; |
343 datagram->t_sent = t_start; |
343 command->index = master->command_index++; |
344 datagram->index = master->datagram_index++; |
344 |
345 |
345 if (unlikely(master->debug_level > 0)) |
346 if (unlikely(master->debug_level > 0)) |
346 EC_DBG("adding command 0x%02X\n", command->index); |
347 EC_DBG("adding datagram 0x%02X\n", datagram->index); |
347 |
348 |
348 // set "command following" flag in previous frame |
349 // set "datagram following" flag in previous frame |
349 if (follows_word) |
350 if (follows_word) |
350 EC_WRITE_U16(follows_word, EC_READ_U16(follows_word) | 0x8000); |
351 EC_WRITE_U16(follows_word, EC_READ_U16(follows_word) | 0x8000); |
351 |
352 |
352 // EtherCAT command header |
353 // EtherCAT datagram header |
353 EC_WRITE_U8 (cur_data, command->type); |
354 EC_WRITE_U8 (cur_data, datagram->type); |
354 EC_WRITE_U8 (cur_data + 1, command->index); |
355 EC_WRITE_U8 (cur_data + 1, datagram->index); |
355 EC_WRITE_U32(cur_data + 2, command->address.logical); |
356 EC_WRITE_U32(cur_data + 2, datagram->address.logical); |
356 EC_WRITE_U16(cur_data + 6, command->data_size & 0x7FF); |
357 EC_WRITE_U16(cur_data + 6, datagram->data_size & 0x7FF); |
357 EC_WRITE_U16(cur_data + 8, 0x0000); |
358 EC_WRITE_U16(cur_data + 8, 0x0000); |
358 follows_word = cur_data + 6; |
359 follows_word = cur_data + 6; |
359 cur_data += EC_COMMAND_HEADER_SIZE; |
360 cur_data += EC_DATAGRAM_HEADER_SIZE; |
360 |
361 |
361 // EtherCAT command data |
362 // EtherCAT datagram data |
362 memcpy(cur_data, command->data, command->data_size); |
363 memcpy(cur_data, datagram->data, datagram->data_size); |
363 cur_data += command->data_size; |
364 cur_data += datagram->data_size; |
364 |
365 |
365 // EtherCAT command footer |
366 // EtherCAT datagram footer |
366 EC_WRITE_U16(cur_data, 0x0000); // reset working counter |
367 EC_WRITE_U16(cur_data, 0x0000); // reset working counter |
367 cur_data += EC_COMMAND_FOOTER_SIZE; |
368 cur_data += EC_DATAGRAM_FOOTER_SIZE; |
368 } |
369 } |
369 |
370 |
370 if (cur_data - frame_data == EC_FRAME_HEADER_SIZE) { |
371 if (cur_data - frame_data == EC_FRAME_HEADER_SIZE) { |
371 if (unlikely(master->debug_level > 0)) |
372 if (unlikely(master->debug_level > 0)) |
372 EC_DBG("nothing to send.\n"); |
373 EC_DBG("nothing to send.\n"); |
434 return; |
435 return; |
435 } |
436 } |
436 |
437 |
437 cmd_follows = 1; |
438 cmd_follows = 1; |
438 while (cmd_follows) { |
439 while (cmd_follows) { |
439 // process command header |
440 // process datagram header |
440 command_type = EC_READ_U8 (cur_data); |
441 datagram_type = EC_READ_U8 (cur_data); |
441 command_index = EC_READ_U8 (cur_data + 1); |
442 datagram_index = EC_READ_U8 (cur_data + 1); |
442 data_size = EC_READ_U16(cur_data + 6) & 0x07FF; |
443 data_size = EC_READ_U16(cur_data + 6) & 0x07FF; |
443 cmd_follows = EC_READ_U16(cur_data + 6) & 0x8000; |
444 cmd_follows = EC_READ_U16(cur_data + 6) & 0x8000; |
444 cur_data += EC_COMMAND_HEADER_SIZE; |
445 cur_data += EC_DATAGRAM_HEADER_SIZE; |
445 |
446 |
446 if (unlikely(cur_data - frame_data |
447 if (unlikely(cur_data - frame_data |
447 + data_size + EC_COMMAND_FOOTER_SIZE > size)) { |
448 + data_size + EC_DATAGRAM_FOOTER_SIZE > size)) { |
448 master->stats.corrupted++; |
449 master->stats.corrupted++; |
449 ec_master_output_stats(master); |
450 ec_master_output_stats(master); |
450 return; |
451 return; |
451 } |
452 } |
452 |
453 |
453 // search for matching command in the queue |
454 // search for matching datagram in the queue |
454 matched = 0; |
455 matched = 0; |
455 list_for_each_entry(command, &master->command_queue, queue) { |
456 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
456 if (command->state == EC_CMD_SENT |
457 if (datagram->state == EC_CMD_SENT |
457 && command->type == command_type |
458 && datagram->type == datagram_type |
458 && command->index == command_index |
459 && datagram->index == datagram_index |
459 && command->data_size == data_size) { |
460 && datagram->data_size == data_size) { |
460 matched = 1; |
461 matched = 1; |
461 break; |
462 break; |
462 } |
463 } |
463 } |
464 } |
464 |
465 |
465 // no matching command was found |
466 // no matching datagram was found |
466 if (!matched) { |
467 if (!matched) { |
467 master->stats.unmatched++; |
468 master->stats.unmatched++; |
468 ec_master_output_stats(master); |
469 ec_master_output_stats(master); |
469 cur_data += data_size + EC_COMMAND_FOOTER_SIZE; |
470 cur_data += data_size + EC_DATAGRAM_FOOTER_SIZE; |
470 continue; |
471 continue; |
471 } |
472 } |
472 |
473 |
473 // copy received data in the command memory |
474 // copy received data in the datagram memory |
474 memcpy(command->data, cur_data, data_size); |
475 memcpy(datagram->data, cur_data, data_size); |
475 cur_data += data_size; |
476 cur_data += data_size; |
476 |
477 |
477 // set the command's working counter |
478 // set the datagram's working counter |
478 command->working_counter = EC_READ_U16(cur_data); |
479 datagram->working_counter = EC_READ_U16(cur_data); |
479 cur_data += EC_COMMAND_FOOTER_SIZE; |
480 cur_data += EC_DATAGRAM_FOOTER_SIZE; |
480 |
481 |
481 // dequeue the received command |
482 // dequeue the received datagram |
482 command->state = EC_CMD_RECEIVED; |
483 datagram->state = EC_CMD_RECEIVED; |
483 list_del_init(&command->queue); |
484 list_del_init(&datagram->queue); |
484 } |
485 } |
485 } |
486 } |
486 |
487 |
487 /*****************************************************************************/ |
488 /*****************************************************************************/ |
488 |
489 |
489 /** |
490 /** |
490 Sends a single command and waits for its reception. |
491 Sends a single datagram and waits for its reception. |
491 If the slave doesn't respond, the command is sent again. |
492 If the slave doesn't respond, the datagram is sent again. |
492 \return 0 in case of success, else < 0 |
493 \return 0 in case of success, else < 0 |
493 */ |
494 */ |
494 |
495 |
495 int ec_master_simple_io(ec_master_t *master, /**< EtherCAT master */ |
496 int ec_master_simple_io(ec_master_t *master, /**< EtherCAT master */ |
496 ec_command_t *command /**< command */ |
497 ec_datagram_t *datagram /**< datagram */ |
497 ) |
498 ) |
498 { |
499 { |
499 unsigned int response_tries_left; |
500 unsigned int response_tries_left; |
500 |
501 |
501 response_tries_left = 10000; |
502 response_tries_left = 10000; |
502 |
503 |
503 while (1) |
504 while (1) |
504 { |
505 { |
505 ec_master_queue_command(master, command); |
506 ec_master_queue_datagram(master, datagram); |
506 ecrt_master_sync_io(master); |
507 ecrt_master_sync_io(master); |
507 |
508 |
508 if (command->state == EC_CMD_RECEIVED) { |
509 if (datagram->state == EC_CMD_RECEIVED) { |
509 if (likely(command->working_counter)) |
510 if (likely(datagram->working_counter)) |
510 return 0; |
511 return 0; |
511 } |
512 } |
512 else if (command->state == EC_CMD_TIMEOUT) { |
513 else if (datagram->state == EC_CMD_TIMEOUT) { |
513 EC_ERR("Simple-IO TIMEOUT!\n"); |
514 EC_ERR("Simple-IO TIMEOUT!\n"); |
514 return -1; |
515 return -1; |
515 } |
516 } |
516 else if (command->state == EC_CMD_ERROR) { |
517 else if (datagram->state == EC_CMD_ERROR) { |
517 EC_ERR("Simple-IO command error!\n"); |
518 EC_ERR("Simple-IO datagram error!\n"); |
518 return -1; |
519 return -1; |
519 } |
520 } |
520 |
521 |
521 // no direct response, wait a little bit... |
522 // no direct response, wait a little bit... |
522 udelay(100); |
523 udelay(100); |
538 |
539 |
539 int ec_master_bus_scan(ec_master_t *master /**< EtherCAT master */) |
540 int ec_master_bus_scan(ec_master_t *master /**< EtherCAT master */) |
540 { |
541 { |
541 ec_slave_t *slave; |
542 ec_slave_t *slave; |
542 ec_slave_ident_t *ident; |
543 ec_slave_ident_t *ident; |
543 ec_command_t *command; |
544 ec_datagram_t *datagram; |
544 unsigned int i; |
545 unsigned int i; |
545 uint16_t coupler_index, coupler_subindex; |
546 uint16_t coupler_index, coupler_subindex; |
546 uint16_t reverse_coupler_index, current_coupler_index; |
547 uint16_t reverse_coupler_index, current_coupler_index; |
547 |
548 |
548 if (!list_empty(&master->slaves)) { |
549 if (!list_empty(&master->slaves)) { |
549 EC_ERR("Slave scan already done!\n"); |
550 EC_ERR("Slave scan already done!\n"); |
550 return -1; |
551 return -1; |
551 } |
552 } |
552 |
553 |
553 command = &master->simple_command; |
554 datagram = &master->simple_datagram; |
554 |
555 |
555 // determine number of slaves on bus |
556 // determine number of slaves on bus |
556 if (ec_command_brd(command, 0x0000, 4)) return -1; |
557 if (ec_datagram_brd(datagram, 0x0000, 4)) return -1; |
557 if (unlikely(ec_master_simple_io(master, command))) return -1; |
558 if (unlikely(ec_master_simple_io(master, datagram))) return -1; |
558 master->slave_count = command->working_counter; |
559 master->slave_count = datagram->working_counter; |
559 EC_INFO("Found %i slaves on bus.\n", master->slave_count); |
560 EC_INFO("Found %i slaves on bus.\n", master->slave_count); |
560 |
561 |
561 if (!master->slave_count) return 0; |
562 if (!master->slave_count) return 0; |
562 |
563 |
563 // init slaves |
564 // init slaves |
1110 // check and reset CRC fault counters |
1111 // check and reset CRC fault counters |
1111 ec_slave_check_crc(slave); |
1112 ec_slave_check_crc(slave); |
1112 |
1113 |
1113 // reset FMMUs |
1114 // reset FMMUs |
1114 if (slave->base_fmmu_count) { |
1115 if (slave->base_fmmu_count) { |
1115 if (ec_command_npwr(command, slave->station_address, 0x0600, |
1116 if (ec_datagram_npwr(datagram, slave->station_address, 0x0600, |
1116 EC_FMMU_SIZE * slave->base_fmmu_count)) |
1117 EC_FMMU_SIZE * slave->base_fmmu_count)) |
1117 return -1; |
1118 return -1; |
1118 memset(command->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count); |
1119 memset(datagram->data, 0x00, |
1119 if (unlikely(ec_master_simple_io(master, command))) { |
1120 EC_FMMU_SIZE * slave->base_fmmu_count); |
|
1121 if (unlikely(ec_master_simple_io(master, datagram))) { |
1120 EC_ERR("Resetting FMMUs failed on slave %i!\n", |
1122 EC_ERR("Resetting FMMUs failed on slave %i!\n", |
1121 slave->ring_position); |
1123 slave->ring_position); |
1122 return -1; |
1124 return -1; |
1123 } |
1125 } |
1124 } |
1126 } |
1125 |
1127 |
1126 // reset sync managers |
1128 // reset sync managers |
1127 if (slave->base_sync_count) { |
1129 if (slave->base_sync_count) { |
1128 if (ec_command_npwr(command, slave->station_address, 0x0800, |
1130 if (ec_datagram_npwr(datagram, slave->station_address, 0x0800, |
1129 EC_SYNC_SIZE * slave->base_sync_count)) |
1131 EC_SYNC_SIZE * slave->base_sync_count)) |
1130 return -1; |
1132 return -1; |
1131 memset(command->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); |
1133 memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); |
1132 if (unlikely(ec_master_simple_io(master, command))) { |
1134 if (unlikely(ec_master_simple_io(master, datagram))) { |
1133 EC_ERR("Resetting sync managers failed on slave %i!\n", |
1135 EC_ERR("Resetting sync managers failed on slave %i!\n", |
1134 slave->ring_position); |
1136 slave->ring_position); |
1135 return -1; |
1137 return -1; |
1136 } |
1138 } |
1137 } |
1139 } |
1138 |
1140 |
1139 // configure sync managers |
1141 // configure sync managers |
1140 if (type) { // known slave type, take type's SM information |
1142 if (type) { // known slave type, take type's SM information |
1141 for (j = 0; type->sync_managers[j] && j < EC_MAX_SYNC; j++) { |
1143 for (j = 0; type->sync_managers[j] && j < EC_MAX_SYNC; j++) { |
1142 sync = type->sync_managers[j]; |
1144 sync = type->sync_managers[j]; |
1143 if (ec_command_npwr(command, slave->station_address, |
1145 if (ec_datagram_npwr(datagram, slave->station_address, |
1144 0x0800 + j * EC_SYNC_SIZE, EC_SYNC_SIZE)) |
1146 0x0800 + j * EC_SYNC_SIZE, EC_SYNC_SIZE)) |
1145 return -1; |
1147 return -1; |
1146 ec_sync_config(sync, slave, command->data); |
1148 ec_sync_config(sync, slave, datagram->data); |
1147 if (unlikely(ec_master_simple_io(master, command))) { |
1149 if (unlikely(ec_master_simple_io(master, datagram))) { |
1148 EC_ERR("Setting sync manager %i failed on slave %i!\n", |
1150 EC_ERR("Setting sync manager %i failed on slave %i!\n", |
1149 j, slave->ring_position); |
1151 j, slave->ring_position); |
1150 return -1; |
1152 return -1; |
1151 } |
1153 } |
1152 } |
1154 } |
1154 else if (slave->sii_mailbox_protocols) { // unknown type, but mailbox |
1156 else if (slave->sii_mailbox_protocols) { // unknown type, but mailbox |
1155 // does the device supply SM configurations in its EEPROM? |
1157 // does the device supply SM configurations in its EEPROM? |
1156 if (!list_empty(&slave->eeprom_syncs)) { |
1158 if (!list_empty(&slave->eeprom_syncs)) { |
1157 list_for_each_entry(eeprom_sync, &slave->eeprom_syncs, list) { |
1159 list_for_each_entry(eeprom_sync, &slave->eeprom_syncs, list) { |
1158 EC_INFO("Sync manager %i...\n", eeprom_sync->index); |
1160 EC_INFO("Sync manager %i...\n", eeprom_sync->index); |
1159 if (ec_command_npwr(command, slave->station_address, |
1161 if (ec_datagram_npwr(datagram, slave->station_address, |
1160 0x800 + eeprom_sync->index * |
1162 0x800 + eeprom_sync->index * |
1161 EC_SYNC_SIZE, |
1163 EC_SYNC_SIZE, |
1162 EC_SYNC_SIZE)) return -1; |
1164 EC_SYNC_SIZE)) return -1; |
1163 ec_eeprom_sync_config(eeprom_sync, command->data); |
1165 ec_eeprom_sync_config(eeprom_sync, datagram->data); |
1164 if (unlikely(ec_master_simple_io(master, command))) { |
1166 if (unlikely(ec_master_simple_io(master, datagram))) { |
1165 EC_ERR("Setting sync manager %i failed on slave %i!\n", |
1167 EC_ERR("Setting sync manager %i failed on slave %i!\n", |
1166 eeprom_sync->index, slave->ring_position); |
1168 eeprom_sync->index, slave->ring_position); |
1167 return -1; |
1169 return -1; |
1168 } |
1170 } |
1169 } |
1171 } |
1172 mbox_sync.physical_start_address = |
1174 mbox_sync.physical_start_address = |
1173 slave->sii_rx_mailbox_offset; |
1175 slave->sii_rx_mailbox_offset; |
1174 mbox_sync.length = slave->sii_rx_mailbox_size; |
1176 mbox_sync.length = slave->sii_rx_mailbox_size; |
1175 mbox_sync.control_register = 0x26; |
1177 mbox_sync.control_register = 0x26; |
1176 mbox_sync.enable = 1; |
1178 mbox_sync.enable = 1; |
1177 if (ec_command_npwr(command, slave->station_address, |
1179 if (ec_datagram_npwr(datagram, slave->station_address, |
1178 0x800,EC_SYNC_SIZE)) return -1; |
1180 0x800,EC_SYNC_SIZE)) return -1; |
1179 ec_eeprom_sync_config(&mbox_sync, command->data); |
1181 ec_eeprom_sync_config(&mbox_sync, datagram->data); |
1180 if (unlikely(ec_master_simple_io(master, command))) { |
1182 if (unlikely(ec_master_simple_io(master, datagram))) { |
1181 EC_ERR("Setting sync manager 0 failed on slave %i!\n", |
1183 EC_ERR("Setting sync manager 0 failed on slave %i!\n", |
1182 slave->ring_position); |
1184 slave->ring_position); |
1183 return -1; |
1185 return -1; |
1184 } |
1186 } |
1185 |
1187 |
1186 mbox_sync.physical_start_address = |
1188 mbox_sync.physical_start_address = |
1187 slave->sii_tx_mailbox_offset; |
1189 slave->sii_tx_mailbox_offset; |
1188 mbox_sync.length = slave->sii_tx_mailbox_size; |
1190 mbox_sync.length = slave->sii_tx_mailbox_size; |
1189 mbox_sync.control_register = 0x22; |
1191 mbox_sync.control_register = 0x22; |
1190 mbox_sync.enable = 1; |
1192 mbox_sync.enable = 1; |
1191 if (ec_command_npwr(command, slave->station_address, |
1193 if (ec_datagram_npwr(datagram, slave->station_address, |
1192 0x808, EC_SYNC_SIZE)) return -1; |
1194 0x808, EC_SYNC_SIZE)) return -1; |
1193 ec_eeprom_sync_config(&mbox_sync, command->data); |
1195 ec_eeprom_sync_config(&mbox_sync, datagram->data); |
1194 if (unlikely(ec_master_simple_io(master, command))) { |
1196 if (unlikely(ec_master_simple_io(master, datagram))) { |
1195 EC_ERR("Setting sync manager 1 failed on slave %i!\n", |
1197 EC_ERR("Setting sync manager 1 failed on slave %i!\n", |
1196 slave->ring_position); |
1198 slave->ring_position); |
1197 return -1; |
1199 return -1; |
1198 } |
1200 } |
1199 } |
1201 } |
1308 ec_device_call_isr(master->device); |
1310 ec_device_call_isr(master->device); |
1309 |
1311 |
1310 t_end = get_cycles(); // take current time |
1312 t_end = get_cycles(); // take current time |
1311 if (t_end - t_start >= t_timeout) break; // timeout! |
1313 if (t_end - t_start >= t_timeout) break; // timeout! |
1312 |
1314 |
1313 commands_sent = 0; |
1315 datagrams_sent = 0; |
1314 list_for_each_entry_safe(command, n, &master->command_queue, queue) { |
1316 list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) { |
1315 if (command->state == EC_CMD_RECEIVED) |
1317 if (datagram->state == EC_CMD_RECEIVED) |
1316 list_del_init(&command->queue); |
1318 list_del_init(&datagram->queue); |
1317 else if (command->state == EC_CMD_SENT) |
1319 else if (datagram->state == EC_CMD_SENT) |
1318 commands_sent++; |
1320 datagrams_sent++; |
1319 } |
1321 } |
1320 |
1322 |
1321 if (!commands_sent) break; |
1323 if (!datagrams_sent) break; |
1322 } |
1324 } |
1323 |
1325 |
1324 // timeout; dequeue all commands |
1326 // timeout; dequeue all datagrams |
1325 list_for_each_entry_safe(command, n, &master->command_queue, queue) { |
1327 list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) { |
1326 switch (command->state) { |
1328 switch (datagram->state) { |
1327 case EC_CMD_SENT: |
1329 case EC_CMD_SENT: |
1328 case EC_CMD_QUEUED: |
1330 case EC_CMD_QUEUED: |
1329 command->state = EC_CMD_TIMEOUT; |
1331 datagram->state = EC_CMD_TIMEOUT; |
1330 master->stats.timeouts++; |
1332 master->stats.timeouts++; |
1331 ec_master_output_stats(master); |
1333 ec_master_output_stats(master); |
1332 break; |
1334 break; |
1333 case EC_CMD_RECEIVED: |
1335 case EC_CMD_RECEIVED: |
1334 master->stats.delayed++; |
1336 master->stats.delayed++; |
1335 ec_master_output_stats(master); |
1337 ec_master_output_stats(master); |
1336 break; |
1338 break; |
1337 default: |
1339 default: |
1338 break; |
1340 break; |
1339 } |
1341 } |
1340 list_del_init(&command->queue); |
1342 list_del_init(&datagram->queue); |
1341 } |
1343 } |
1342 } |
1344 } |
1343 |
1345 |
1344 /*****************************************************************************/ |
1346 /*****************************************************************************/ |
1345 |
1347 |
1346 /** |
1348 /** |
1347 Asynchronous sending of commands. |
1349 Asynchronous sending of datagrams. |
1348 \ingroup RealtimeInterface |
1350 \ingroup RealtimeInterface |
1349 */ |
1351 */ |
1350 |
1352 |
1351 void ecrt_master_async_send(ec_master_t *master /**< EtherCAT master */) |
1353 void ecrt_master_async_send(ec_master_t *master /**< EtherCAT master */) |
1352 { |
1354 { |
1353 ec_command_t *command, *n; |
1355 ec_datagram_t *datagram, *n; |
1354 |
1356 |
1355 if (unlikely(!master->device->link_state)) { |
1357 if (unlikely(!master->device->link_state)) { |
1356 // link is down, no command can be sent |
1358 // link is down, no datagram can be sent |
1357 list_for_each_entry_safe(command, n, &master->command_queue, queue) { |
1359 list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) { |
1358 command->state = EC_CMD_ERROR; |
1360 datagram->state = EC_CMD_ERROR; |
1359 list_del_init(&command->queue); |
1361 list_del_init(&datagram->queue); |
1360 } |
1362 } |
1361 |
1363 |
1362 // query link state |
1364 // query link state |
1363 ec_device_call_isr(master->device); |
1365 ec_device_call_isr(master->device); |
1364 return; |
1366 return; |
1365 } |
1367 } |
1366 |
1368 |
1367 // send frames |
1369 // send frames |
1368 ec_master_send_commands(master); |
1370 ec_master_send_datagrams(master); |
1369 } |
1371 } |
1370 |
1372 |
1371 /*****************************************************************************/ |
1373 /*****************************************************************************/ |
1372 |
1374 |
1373 /** |
1375 /** |
1374 Asynchronous receiving of commands. |
1376 Asynchronous receiving of datagrams. |
1375 \ingroup RealtimeInterface |
1377 \ingroup RealtimeInterface |
1376 */ |
1378 */ |
1377 |
1379 |
1378 void ecrt_master_async_receive(ec_master_t *master /**< EtherCAT master */) |
1380 void ecrt_master_async_receive(ec_master_t *master /**< EtherCAT master */) |
1379 { |
1381 { |
1380 ec_command_t *command, *next; |
1382 ec_datagram_t *datagram, *next; |
1381 cycles_t t_received, t_timeout; |
1383 cycles_t t_received, t_timeout; |
1382 |
1384 |
1383 ec_device_call_isr(master->device); |
1385 ec_device_call_isr(master->device); |
1384 |
1386 |
1385 t_received = get_cycles(); |
1387 t_received = get_cycles(); |
1386 t_timeout = (cycles_t) master->timeout * (cpu_khz / 1000); |
1388 t_timeout = (cycles_t) master->timeout * (cpu_khz / 1000); |
1387 |
1389 |
1388 // dequeue all received commands |
1390 // dequeue all received datagrams |
1389 list_for_each_entry_safe(command, next, &master->command_queue, queue) |
1391 list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue) |
1390 if (command->state == EC_CMD_RECEIVED) list_del_init(&command->queue); |
1392 if (datagram->state == EC_CMD_RECEIVED) |
1391 |
1393 list_del_init(&datagram->queue); |
1392 // dequeue all commands that timed out |
1394 |
1393 list_for_each_entry_safe(command, next, &master->command_queue, queue) { |
1395 // dequeue all datagrams that timed out |
1394 switch (command->state) { |
1396 list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue) { |
|
1397 switch (datagram->state) { |
1395 case EC_CMD_SENT: |
1398 case EC_CMD_SENT: |
1396 case EC_CMD_QUEUED: |
1399 case EC_CMD_QUEUED: |
1397 if (t_received - command->t_sent > t_timeout) { |
1400 if (t_received - datagram->t_sent > t_timeout) { |
1398 list_del_init(&command->queue); |
1401 list_del_init(&datagram->queue); |
1399 command->state = EC_CMD_TIMEOUT; |
1402 datagram->state = EC_CMD_TIMEOUT; |
1400 master->stats.timeouts++; |
1403 master->stats.timeouts++; |
1401 ec_master_output_stats(master); |
1404 ec_master_output_stats(master); |
1402 } |
1405 } |
1403 break; |
1406 break; |
1404 default: |
1407 default: |