179 // Write station address |
171 // Write station address |
180 |
172 |
181 data[0] = cur->station_address & 0x00FF; |
173 data[0] = cur->station_address & 0x00FF; |
182 data[1] = (cur->station_address & 0xFF00) >> 8; |
174 data[1] = (cur->station_address & 0xFF00) >> 8; |
183 |
175 |
184 if ((cmd = EtherCAT_position_write(master, cur->ring_position, 0x0010, 2, data)) == NULL) |
176 EtherCAT_command_position_write(&cmd, cur->ring_position, 0x0010, 2, data); |
185 { |
177 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
186 return -1; |
178 |
187 } |
179 if (cmd.working_counter != 1) |
188 |
|
189 if (EtherCAT_async_send_receive(master) != 0) |
|
190 { |
|
191 return -1; |
|
192 } |
|
193 |
|
194 if (cmd->working_counter != 1) |
|
195 { |
180 { |
196 EC_DBG(KERN_ERR "EtherCAT: Slave %i did not repond while writing station address!\n", i); |
181 EC_DBG(KERN_ERR "EtherCAT: Slave %i did not repond while writing station address!\n", i); |
197 return -1; |
182 return -1; |
198 } |
183 } |
199 |
184 |
200 EtherCAT_remove_command(master, cmd); |
|
201 |
|
202 // Read base data |
185 // Read base data |
203 |
186 |
204 if ((cmd = EtherCAT_read(master, cur->station_address, 0x0000, 4)) == NULL) |
187 EtherCAT_command_read(&cmd, cur->station_address, 0x0000, 4); |
205 { |
188 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
206 EC_DBG(KERN_ERR "EtherCAT: Could not create command!\n"); |
189 |
207 return -1; |
190 if (cmd.working_counter != 1) |
208 } |
191 { |
209 |
|
210 if (EtherCAT_async_send_receive(master) != 0) |
|
211 { |
|
212 EtherCAT_remove_command(master, cmd); |
|
213 |
|
214 EC_DBG(KERN_ERR "EtherCAT: Could not send command!\n"); |
|
215 return -1; |
|
216 } |
|
217 |
|
218 if (cmd->working_counter != 1) |
|
219 { |
|
220 EtherCAT_remove_command(master, cmd); |
|
221 |
|
222 EC_DBG(KERN_ERR "EtherCAT: Slave %i did not respond while reading base data!\n", i); |
192 EC_DBG(KERN_ERR "EtherCAT: Slave %i did not respond while reading base data!\n", i); |
223 return -1; |
193 return -1; |
224 } |
194 } |
225 |
195 |
226 // Get base data |
196 // Get base data |
227 cur->type = cmd->data[0]; |
197 cur->type = cmd.data[0]; |
228 cur->revision = cmd->data[1]; |
198 cur->revision = cmd.data[1]; |
229 cur->build = cmd->data[2] | (cmd->data[3] << 8); |
199 cur->build = cmd.data[2] | (cmd.data[3] << 8); |
230 |
|
231 EtherCAT_remove_command(master, cmd); |
|
232 |
200 |
233 // Read identification from "Slave Information Interface" (SII) |
201 // Read identification from "Slave Information Interface" (SII) |
234 |
202 |
235 if (EtherCAT_read_slave_information(master, cur->station_address, |
203 if (EtherCAT_read_slave_information(master, cur->station_address, |
236 0x0008, &cur->vendor_id) != 0) |
204 0x0008, &cur->vendor_id) != 0) |
374 data[2] = offset & 0xFF; |
342 data[2] = offset & 0xFF; |
375 data[3] = (offset & 0xFF00) >> 8; |
343 data[3] = (offset & 0xFF00) >> 8; |
376 data[4] = 0x00; |
344 data[4] = 0x00; |
377 data[5] = 0x00; |
345 data[5] = 0x00; |
378 |
346 |
379 if ((cmd = EtherCAT_write(master, node_address, 0x502, 6, data)) == NULL) |
347 EtherCAT_command_write(&cmd, node_address, 0x502, 6, data); |
380 return -2; |
348 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -3; |
381 |
349 |
382 if (EtherCAT_async_send_receive(master) != 0) |
350 if (cmd.working_counter != 1) |
383 { |
351 { |
384 EtherCAT_remove_command(master, cmd); |
|
385 return -3; |
|
386 } |
|
387 |
|
388 if (cmd->working_counter != 1) |
|
389 { |
|
390 EtherCAT_remove_command(master, cmd); |
|
391 EC_DBG(KERN_ERR "EtherCAT: SII-read - Slave %04X did not respond!\n", |
352 EC_DBG(KERN_ERR "EtherCAT: SII-read - Slave %04X did not respond!\n", |
392 node_address); |
353 node_address); |
393 return -4; |
354 return -4; |
394 } |
355 } |
395 |
356 |
396 EtherCAT_remove_command(master, cmd); |
|
397 |
|
398 // Get status of read operation |
|
399 |
|
400 // ?? FIXME warum hier tries ?? Hm |
|
401 |
|
402 // Der Slave legt die Informationen des Slave-Information-Interface |
357 // Der Slave legt die Informationen des Slave-Information-Interface |
403 // in das Datenregister und löscht daraufhin ein Busy-Bit. Solange |
358 // in das Datenregister und löscht daraufhin ein Busy-Bit. Solange |
404 // den Status auslesen, bis das Bit weg ist. fp |
359 // den Status auslesen, bis das Bit weg ist. |
405 |
360 |
406 tries_left = 1000; |
361 tries_left = 100; |
407 while (tries_left) |
362 while (tries_left) |
408 { |
363 { |
409 udelay(10); |
364 udelay(10); |
410 |
365 |
411 if ((cmd = EtherCAT_read(master, node_address, 0x502, 10)) == NULL) |
366 EtherCAT_command_read(&cmd, node_address, 0x502, 10); |
412 return -2; |
367 if (EtherCAT_simple_send_receive(master, &cmd) != 0) return -3; |
413 |
368 |
414 if (EtherCAT_async_send_receive(master) != 0) |
369 if (cmd.working_counter != 1) |
415 { |
370 { |
416 EtherCAT_remove_command(master, cmd); |
|
417 return -3; |
|
418 } |
|
419 |
|
420 if (cmd->working_counter != 1) |
|
421 { |
|
422 EtherCAT_remove_command(master, cmd); |
|
423 EC_DBG(KERN_ERR "EtherCAT: SII-read status - Slave %04X did not respond!\n", |
371 EC_DBG(KERN_ERR "EtherCAT: SII-read status - Slave %04X did not respond!\n", |
424 node_address); |
372 node_address); |
425 return -4; |
373 return -4; |
426 } |
374 } |
427 |
375 |
428 if ((cmd->data[1] & 0x81) == 0) |
376 if ((cmd.data[1] & 0x81) == 0) |
429 { |
377 { |
430 memcpy(target, cmd->data + 6, 4); |
378 memcpy(target, cmd.data + 6, 4); |
431 EtherCAT_remove_command(master, cmd); |
|
432 break; |
379 break; |
433 } |
380 } |
434 |
|
435 EtherCAT_remove_command(master, cmd); |
|
436 |
381 |
437 tries_left--; |
382 tries_left--; |
438 } |
383 } |
439 |
384 |
440 if (!tries_left) |
385 if (!tries_left) |
516 } |
462 } |
517 |
463 |
518 /***************************************************************/ |
464 /***************************************************************/ |
519 |
465 |
520 /** |
466 /** |
|
467 Sendet alle wartenden Kommandos. |
|
468 |
|
469 Errechnet erst die benötigte Gesamt-Rahmenlänge, und erstellt |
|
470 dann alle Kommando-Bytefolgen im statischen Sendespeicher. |
|
471 Danach wird die Sendefunktion des EtherCAT-Gerätes aufgerufen. |
|
472 |
|
473 @param master EtherCAT-Master |
|
474 |
|
475 @return 0 bei Erfolg, sonst < 0 |
|
476 */ |
|
477 |
|
478 int EtherCAT_send(EtherCAT_master_t *master) |
|
479 { |
|
480 unsigned int i, length, framelength, pos; |
|
481 EtherCAT_command_t *cmd; |
|
482 int cmdcnt = 0; |
|
483 |
|
484 if (master->debug_level > 0) |
|
485 { |
|
486 EC_DBG(KERN_DEBUG "EtherCAT_send, first_command = %X\n", (int) master->first_command); |
|
487 } |
|
488 |
|
489 length = 0; |
|
490 for (cmd = master->first_command; cmd != NULL; cmd = cmd->next) |
|
491 { |
|
492 if (cmd->state != ECAT_CS_READY) continue; |
|
493 length += cmd->data_length + 12; |
|
494 cmdcnt++; |
|
495 } |
|
496 |
|
497 if (master->debug_level > 0) |
|
498 { |
|
499 EC_DBG(KERN_DEBUG "%i commands to send.\n", cmdcnt); |
|
500 } |
|
501 |
|
502 if (length == 0) |
|
503 { |
|
504 EC_DBG(KERN_WARNING "EtherCAT: Nothing to send...\n"); |
|
505 return 0; |
|
506 } |
|
507 |
|
508 framelength = length + 2; |
|
509 if (framelength < 46) framelength = 46; |
|
510 |
|
511 if (master->debug_level > 0) |
|
512 { |
|
513 EC_DBG(KERN_DEBUG "framelength: %i\n", framelength); |
|
514 } |
|
515 |
|
516 master->tx_data[0] = length & 0xFF; |
|
517 master->tx_data[1] = ((length & 0x700) >> 8) | 0x10; |
|
518 pos = 2; |
|
519 |
|
520 for (cmd = master->first_command; cmd != NULL; cmd = cmd->next) |
|
521 { |
|
522 if (cmd->state != ECAT_CS_READY) continue; |
|
523 |
|
524 cmd->index = master->command_index; |
|
525 master->command_index = (master->command_index + 1) % 0x0100; |
|
526 |
|
527 if (master->debug_level > 0) |
|
528 { |
|
529 EC_DBG(KERN_DEBUG "Sending command index %i\n", cmd->index); |
|
530 } |
|
531 |
|
532 cmd->state = ECAT_CS_SENT; |
|
533 |
|
534 master->tx_data[pos + 0] = cmd->type; |
|
535 master->tx_data[pos + 1] = cmd->index; |
|
536 |
|
537 master->tx_data[pos + 2] = cmd->address.raw[0]; |
|
538 master->tx_data[pos + 3] = cmd->address.raw[1]; |
|
539 master->tx_data[pos + 4] = cmd->address.raw[2]; |
|
540 master->tx_data[pos + 5] = cmd->address.raw[3]; |
|
541 |
|
542 master->tx_data[pos + 6] = cmd->data_length & 0xFF; |
|
543 master->tx_data[pos + 7] = (cmd->data_length & 0x700) >> 8; |
|
544 |
|
545 if (cmd->next) master->tx_data[pos + 7] |= 0x80; |
|
546 |
|
547 master->tx_data[pos + 8] = 0x00; |
|
548 master->tx_data[pos + 9] = 0x00; |
|
549 |
|
550 if (cmd->type == ECAT_CMD_APWR |
|
551 || cmd->type == ECAT_CMD_NPWR |
|
552 || cmd->type == ECAT_CMD_BWR |
|
553 || cmd->type == ECAT_CMD_LRW) // Write |
|
554 { |
|
555 for (i = 0; i < cmd->data_length; i++) master->tx_data[pos + 10 + i] = cmd->data[i]; |
|
556 } |
|
557 else // Read |
|
558 { |
|
559 for (i = 0; i < cmd->data_length; i++) master->tx_data[pos + 10 + i] = 0x00; |
|
560 } |
|
561 |
|
562 master->tx_data[pos + 10 + cmd->data_length] = 0x00; |
|
563 master->tx_data[pos + 11 + cmd->data_length] = 0x00; |
|
564 |
|
565 pos += 12 + cmd->data_length; |
|
566 } |
|
567 |
|
568 // Pad with zeros |
|
569 while (pos < 46) master->tx_data[pos++] = 0x00; |
|
570 |
|
571 master->tx_data_length = framelength; |
|
572 |
|
573 #ifdef DEBUG_SEND_RECEIVE |
|
574 EC_DBG(KERN_DEBUG "\n"); |
|
575 EC_DBG(KERN_DEBUG ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"); |
|
576 EC_DBG(KERN_DEBUG); |
|
577 for (i = 0; i < framelength; i++) |
|
578 { |
|
579 EC_DBG("%02X ", master->tx_data[i]); |
|
580 |
|
581 if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG); |
|
582 } |
|
583 EC_DBG("\n"); |
|
584 EC_DBG(KERN_DEBUG "-----------------------------------------------\n"); |
|
585 #endif |
|
586 |
|
587 if (master->debug_level > 0) |
|
588 { |
|
589 EC_DBG(KERN_DEBUG "device send...\n"); |
|
590 } |
|
591 |
|
592 // Send frame |
|
593 if (EtherCAT_device_send(master->dev, master->tx_data, framelength) != 0) |
|
594 { |
|
595 EC_DBG(KERN_ERR "EtherCAT: Could not send!\n"); |
|
596 EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); |
|
597 output_debug_data(master->tx_data, framelength); |
|
598 return -1; |
|
599 } |
|
600 |
|
601 if (master->debug_level > 0) |
|
602 { |
|
603 EC_DBG(KERN_DEBUG "EtherCAT_send done.\n"); |
|
604 } |
|
605 |
|
606 return 0; |
|
607 } |
|
608 |
|
609 /***************************************************************/ |
|
610 |
|
611 /** |
|
612 Holt alle empfangenen Kommandos vom EtherCAT-Gerät. |
|
613 |
|
614 Kopiert einen empfangenen Rahmen vom EtherCAT-Gerät |
|
615 in den Statischen Empfangsspeicher und ordnet dann |
|
616 allen gesendeten Kommandos ihre Antworten zu. |
|
617 |
|
618 @param master EtherCAT-Master |
|
619 |
|
620 @return 0 bei Erfolg, sonst < 0 |
|
621 */ |
|
622 |
|
623 int EtherCAT_receive(EtherCAT_master_t *master) |
|
624 { |
|
625 EtherCAT_command_t *cmd; |
|
626 unsigned int length, pos, found, rx_data_length; |
|
627 unsigned int command_follows, command_type, command_index; |
|
628 |
|
629 // Copy received data into master buffer (with locking) |
|
630 rx_data_length = EtherCAT_device_receive(master->dev, master->rx_data, |
|
631 ECAT_FRAME_BUFFER_SIZE); |
|
632 |
|
633 #ifdef DEBUG_SEND_RECEIVE |
|
634 for (i = 0; i < rx_data_length; i++) |
|
635 { |
|
636 if (master->rx_data[i] == master->tx_data[i]) EC_DBG(" "); |
|
637 else EC_DBG("%02X ", master->rx_data[i]); |
|
638 if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG); |
|
639 } |
|
640 EC_DBG("\n"); |
|
641 EC_DBG(KERN_DEBUG "<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n"); |
|
642 EC_DBG(KERN_DEBUG "\n"); |
|
643 #endif |
|
644 |
|
645 if (rx_data_length < 2) |
|
646 { |
|
647 EC_DBG(KERN_ERR "EtherCAT: Received corrupted frame (illegal length)!\n"); |
|
648 EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); |
|
649 output_debug_data(master->tx_data, master->tx_data_length); |
|
650 EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n"); |
|
651 output_debug_data(master->rx_data, rx_data_length); |
|
652 return -1; |
|
653 } |
|
654 |
|
655 // Länge des gesamten Frames prüfen |
|
656 length = ((master->rx_data[1] & 0x07) << 8) | (master->rx_data[0] & 0xFF); |
|
657 |
|
658 if (length > rx_data_length) |
|
659 { |
|
660 EC_DBG(KERN_ERR "EtherCAT: Received corrupted frame (length does not match)!\n"); |
|
661 EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); |
|
662 output_debug_data(master->tx_data, master->tx_data_length); |
|
663 EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n"); |
|
664 output_debug_data(master->rx_data, rx_data_length); |
|
665 return -1; |
|
666 } |
|
667 |
|
668 pos = 2; // LibPCAP: 16 |
|
669 command_follows = 1; |
|
670 while (command_follows) |
|
671 { |
|
672 if (pos + 10 > rx_data_length) |
|
673 { |
|
674 EC_DBG(KERN_ERR "EtherCAT: Received frame with incomplete command header!\n"); |
|
675 EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); |
|
676 output_debug_data(master->tx_data, master->tx_data_length); |
|
677 EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n"); |
|
678 output_debug_data(master->rx_data, rx_data_length); |
|
679 return -1; |
|
680 } |
|
681 |
|
682 command_type = master->rx_data[pos]; |
|
683 command_index = master->rx_data[pos + 1]; |
|
684 length = (master->rx_data[pos + 6] & 0xFF) |
|
685 | ((master->rx_data[pos + 7] & 0x07) << 8); |
|
686 command_follows = master->rx_data[pos + 7] & 0x80; |
|
687 |
|
688 if (pos + length + 12 > rx_data_length) |
|
689 { |
|
690 EC_DBG(KERN_ERR "EtherCAT: Received frame with incomplete command data!\n"); |
|
691 EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); |
|
692 output_debug_data(master->tx_data, master->tx_data_length); |
|
693 EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n"); |
|
694 output_debug_data(master->rx_data, rx_data_length); |
|
695 return -1; |
|
696 } |
|
697 |
|
698 found = 0; |
|
699 |
|
700 for (cmd = master->first_command; cmd != NULL; cmd = cmd->next) |
|
701 { |
|
702 if (cmd->state == ECAT_CS_SENT |
|
703 && cmd->type == command_type |
|
704 && cmd->index == command_index |
|
705 && cmd->data_length == length) |
|
706 { |
|
707 found = 1; |
|
708 cmd->state = ECAT_CS_RECEIVED; |
|
709 |
|
710 // Empfangene Daten in Kommandodatenspeicher kopieren |
|
711 memcpy(cmd->data, master->rx_data + pos + 10, length); |
|
712 |
|
713 // Working-Counter setzen |
|
714 cmd->working_counter = (master->rx_data[pos + length + 10] & 0xFF) |
|
715 | ((master->rx_data[pos + length + 11] & 0xFF) << 8); |
|
716 } |
|
717 } |
|
718 |
|
719 if (!found) |
|
720 { |
|
721 EC_DBG(KERN_WARNING "EtherCAT: WARNING - Command not assigned!\n"); |
|
722 } |
|
723 |
|
724 pos += length + 12; |
|
725 } |
|
726 |
|
727 for (cmd = master->first_command; cmd != NULL; cmd = cmd->next) |
|
728 { |
|
729 if (cmd->state == ECAT_CS_SENT) |
|
730 { |
|
731 EC_DBG(KERN_WARNING "EtherCAT: WARNING - One command received no response!\n"); |
|
732 } |
|
733 } |
|
734 |
|
735 master->dev->state = ECAT_DS_READY; |
|
736 |
|
737 return 0; |
|
738 } |
|
739 #endif |
|
740 |
|
741 /***************************************************************/ |
|
742 |
|
743 /** |
521 Sendet ein einzelnes Kommando in einem Frame und |
744 Sendet ein einzelnes Kommando in einem Frame und |
522 wartet auf dessen Empfang. |
745 wartet auf dessen Empfang. |
523 |
746 |
524 @param master EtherCAT-Master |
747 @param master EtherCAT-Master |
525 @param cmd Kommando zum Senden/Empfangen |
748 @param cmd Kommando zum Senden/Empfangen |
526 |
749 |
527 @return 0 bei Erfolg, sonst < 0 |
750 @return 0 bei Erfolg, sonst < 0 |
528 */ |
751 */ |
529 |
752 |
530 int EtherCAT_send_receive_command(EtherCAT_master_t *master, |
753 int EtherCAT_simple_send_receive(EtherCAT_master_t *master, |
531 EtherCAT_command_t *cmd) |
754 EtherCAT_command_t *cmd) |
532 { |
755 { |
533 unsigned int length, framelength, i, tries_left, rx_data_length; |
756 unsigned int tries_left; |
534 unsigned char command_type, command_index; |
757 |
|
758 if (EtherCAT_simple_send(master, cmd) < 0) return -1; |
|
759 |
|
760 tries_left = 100; |
|
761 while (cmd->state == ECAT_CS_SENT && tries_left) |
|
762 { |
|
763 udelay(10); |
|
764 EtherCAT_device_call_isr(master->dev); |
|
765 tries_left--; |
|
766 } |
|
767 |
|
768 if (EtherCAT_simple_receive(master, cmd) < 0) return -1; |
|
769 |
|
770 return 0; |
|
771 } |
|
772 |
|
773 /***************************************************************/ |
|
774 |
|
775 /** |
|
776 Sendet ein einzelnes Kommando in einem Frame. |
|
777 |
|
778 @param master EtherCAT-Master |
|
779 @param cmd Kommando zum Senden |
|
780 |
|
781 @return 0 bei Erfolg, sonst < 0 |
|
782 */ |
|
783 |
|
784 int EtherCAT_simple_send(EtherCAT_master_t *master, |
|
785 EtherCAT_command_t *cmd) |
|
786 { |
|
787 unsigned int length, framelength, i; |
535 |
788 |
536 if (master->debug_level > 0) |
789 if (master->debug_level > 0) |
537 { |
790 { |
538 EC_DBG(KERN_DEBUG "EtherCAT_send_receive_command\n"); |
791 EC_DBG(KERN_DEBUG "EtherCAT_send_receive_command\n"); |
539 } |
792 } |
676 | ((master->rx_data[length + 2 + 11] & 0xFF) << 8)); |
942 | ((master->rx_data[length + 2 + 11] & 0xFF) << 8)); |
677 } |
943 } |
678 else |
944 else |
679 { |
945 { |
680 EC_DBG(KERN_WARNING "EtherCAT: WARNING - Send/Receive anomaly!\n"); |
946 EC_DBG(KERN_WARNING "EtherCAT: WARNING - Send/Receive anomaly!\n"); |
681 } |
|
682 |
|
683 master->dev->state = ECAT_DS_READY; |
|
684 |
|
685 return 0; |
|
686 } |
|
687 |
|
688 /***************************************************************/ |
|
689 |
|
690 /** |
|
691 Sendet alle wartenden Kommandos. |
|
692 |
|
693 Errechnet erst die benötigte Gesamt-Rahmenlänge, und erstellt |
|
694 dann alle Kommando-Bytefolgen im statischen Sendespeicher. |
|
695 Danach wird die Sendefunktion des EtherCAT-Gerätes aufgerufen. |
|
696 |
|
697 @param master EtherCAT-Master |
|
698 |
|
699 @return 0 bei Erfolg, sonst < 0 |
|
700 */ |
|
701 |
|
702 int EtherCAT_send(EtherCAT_master_t *master) |
|
703 { |
|
704 unsigned int i, length, framelength, pos; |
|
705 EtherCAT_command_t *cmd; |
|
706 int cmdcnt = 0; |
|
707 |
|
708 if (master->debug_level > 0) |
|
709 { |
|
710 EC_DBG(KERN_DEBUG "EtherCAT_send, first_command = %X\n", (int) master->first_command); |
|
711 } |
|
712 |
|
713 length = 0; |
|
714 for (cmd = master->first_command; cmd != NULL; cmd = cmd->next) |
|
715 { |
|
716 if (cmd->state != ECAT_CS_READY) continue; |
|
717 length += cmd->data_length + 12; |
|
718 cmdcnt++; |
|
719 } |
|
720 |
|
721 if (master->debug_level > 0) |
|
722 { |
|
723 EC_DBG(KERN_DEBUG "%i commands to send.\n", cmdcnt); |
|
724 } |
|
725 |
|
726 if (length == 0) |
|
727 { |
|
728 EC_DBG(KERN_WARNING "EtherCAT: Nothing to send...\n"); |
|
729 return 0; |
|
730 } |
|
731 |
|
732 framelength = length + 2; |
|
733 if (framelength < 46) framelength = 46; |
|
734 |
|
735 if (master->debug_level > 0) |
|
736 { |
|
737 EC_DBG(KERN_DEBUG "framelength: %i\n", framelength); |
|
738 } |
|
739 |
|
740 master->tx_data[0] = length & 0xFF; |
|
741 master->tx_data[1] = ((length & 0x700) >> 8) | 0x10; |
|
742 pos = 2; |
|
743 |
|
744 for (cmd = master->first_command; cmd != NULL; cmd = cmd->next) |
|
745 { |
|
746 if (cmd->state != ECAT_CS_READY) continue; |
|
747 |
|
748 cmd->index = master->command_index; |
|
749 master->command_index = (master->command_index + 1) % 0x0100; |
|
750 |
|
751 if (master->debug_level > 0) |
|
752 { |
|
753 EC_DBG(KERN_DEBUG "Sending command index %i\n", cmd->index); |
|
754 } |
|
755 |
|
756 cmd->state = ECAT_CS_SENT; |
|
757 |
|
758 master->tx_data[pos + 0] = cmd->type; |
|
759 master->tx_data[pos + 1] = cmd->index; |
|
760 |
|
761 master->tx_data[pos + 2] = cmd->address.raw[0]; |
|
762 master->tx_data[pos + 3] = cmd->address.raw[1]; |
|
763 master->tx_data[pos + 4] = cmd->address.raw[2]; |
|
764 master->tx_data[pos + 5] = cmd->address.raw[3]; |
|
765 |
|
766 master->tx_data[pos + 6] = cmd->data_length & 0xFF; |
|
767 master->tx_data[pos + 7] = (cmd->data_length & 0x700) >> 8; |
|
768 |
|
769 if (cmd->next) master->tx_data[pos + 7] |= 0x80; |
|
770 |
|
771 master->tx_data[pos + 8] = 0x00; |
|
772 master->tx_data[pos + 9] = 0x00; |
|
773 |
|
774 if (cmd->type == ECAT_CMD_APWR |
|
775 || cmd->type == ECAT_CMD_NPWR |
|
776 || cmd->type == ECAT_CMD_BWR |
|
777 || cmd->type == ECAT_CMD_LRW) // Write |
|
778 { |
|
779 for (i = 0; i < cmd->data_length; i++) master->tx_data[pos + 10 + i] = cmd->data[i]; |
|
780 } |
|
781 else // Read |
|
782 { |
|
783 for (i = 0; i < cmd->data_length; i++) master->tx_data[pos + 10 + i] = 0x00; |
|
784 } |
|
785 |
|
786 master->tx_data[pos + 10 + cmd->data_length] = 0x00; |
|
787 master->tx_data[pos + 11 + cmd->data_length] = 0x00; |
|
788 |
|
789 pos += 12 + cmd->data_length; |
|
790 } |
|
791 |
|
792 // Pad with zeros |
|
793 while (pos < 46) master->tx_data[pos++] = 0x00; |
|
794 |
|
795 master->tx_data_length = framelength; |
|
796 |
|
797 #ifdef DEBUG_SEND_RECEIVE |
|
798 EC_DBG(KERN_DEBUG "\n"); |
|
799 EC_DBG(KERN_DEBUG ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"); |
|
800 EC_DBG(KERN_DEBUG); |
|
801 for (i = 0; i < framelength; i++) |
|
802 { |
|
803 EC_DBG("%02X ", master->tx_data[i]); |
|
804 |
|
805 if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG); |
|
806 } |
|
807 EC_DBG("\n"); |
|
808 EC_DBG(KERN_DEBUG "-----------------------------------------------\n"); |
|
809 #endif |
|
810 |
|
811 if (master->debug_level > 0) |
|
812 { |
|
813 EC_DBG(KERN_DEBUG "device send...\n"); |
|
814 } |
|
815 |
|
816 // Send frame |
|
817 if (EtherCAT_device_send(master->dev, master->tx_data, framelength) != 0) |
|
818 { |
|
819 EC_DBG(KERN_ERR "EtherCAT: Could not send!\n"); |
|
820 EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); |
|
821 output_debug_data(master->tx_data, framelength); |
|
822 return -1; |
|
823 } |
|
824 |
|
825 if (master->debug_level > 0) |
|
826 { |
|
827 EC_DBG(KERN_DEBUG "EtherCAT_send done.\n"); |
|
828 } |
|
829 |
|
830 return 0; |
|
831 } |
|
832 |
|
833 /***************************************************************/ |
|
834 |
|
835 /** |
|
836 Holt alle empfangenen Kommandos vom EtherCAT-Gerät. |
|
837 |
|
838 Kopiert einen empfangenen Rahmen vom EtherCAT-Gerät |
|
839 in den Statischen Empfangsspeicher und ordnet dann |
|
840 allen gesendeten Kommandos ihre Antworten zu. |
|
841 |
|
842 @param master EtherCAT-Master |
|
843 |
|
844 @return 0 bei Erfolg, sonst < 0 |
|
845 */ |
|
846 |
|
847 int EtherCAT_receive(EtherCAT_master_t *master) |
|
848 { |
|
849 EtherCAT_command_t *cmd; |
|
850 unsigned int length, pos, found, rx_data_length; |
|
851 unsigned int command_follows, command_type, command_index; |
|
852 |
|
853 // Copy received data into master buffer (with locking) |
|
854 rx_data_length = EtherCAT_device_receive(master->dev, master->rx_data, |
|
855 ECAT_FRAME_BUFFER_SIZE); |
|
856 |
|
857 #ifdef DEBUG_SEND_RECEIVE |
|
858 for (i = 0; i < rx_data_length; i++) |
|
859 { |
|
860 if (master->rx_data[i] == master->tx_data[i]) EC_DBG(" "); |
|
861 else EC_DBG("%02X ", master->rx_data[i]); |
|
862 if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG); |
|
863 } |
|
864 EC_DBG("\n"); |
|
865 EC_DBG(KERN_DEBUG "<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n"); |
|
866 EC_DBG(KERN_DEBUG "\n"); |
|
867 #endif |
|
868 |
|
869 if (rx_data_length < 2) |
|
870 { |
|
871 EC_DBG(KERN_ERR "EtherCAT: Received corrupted frame (illegal length)!\n"); |
|
872 EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); |
947 EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); |
873 output_debug_data(master->tx_data, master->tx_data_length); |
948 output_debug_data(master->tx_data, master->tx_data_length); |
874 EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n"); |
949 EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n"); |
875 output_debug_data(master->rx_data, rx_data_length); |
950 output_debug_data(master->rx_data, rx_data_length); |
876 return -1; |
|
877 } |
|
878 |
|
879 // Länge des gesamten Frames prüfen |
|
880 length = ((master->rx_data[1] & 0x07) << 8) | (master->rx_data[0] & 0xFF); |
|
881 |
|
882 if (length > rx_data_length) |
|
883 { |
|
884 EC_DBG(KERN_ERR "EtherCAT: Received corrupted frame (length does not match)!\n"); |
|
885 EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); |
|
886 output_debug_data(master->tx_data, master->tx_data_length); |
|
887 EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n"); |
|
888 output_debug_data(master->rx_data, rx_data_length); |
|
889 return -1; |
|
890 } |
|
891 |
|
892 pos = 2; // LibPCAP: 16 |
|
893 command_follows = 1; |
|
894 while (command_follows) |
|
895 { |
|
896 if (pos + 10 > rx_data_length) |
|
897 { |
|
898 EC_DBG(KERN_ERR "EtherCAT: Received frame with incomplete command header!\n"); |
|
899 EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); |
|
900 output_debug_data(master->tx_data, master->tx_data_length); |
|
901 EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n"); |
|
902 output_debug_data(master->rx_data, rx_data_length); |
|
903 return -1; |
|
904 } |
|
905 |
|
906 command_type = master->rx_data[pos]; |
|
907 command_index = master->rx_data[pos + 1]; |
|
908 length = (master->rx_data[pos + 6] & 0xFF) |
|
909 | ((master->rx_data[pos + 7] & 0x07) << 8); |
|
910 command_follows = master->rx_data[pos + 7] & 0x80; |
|
911 |
|
912 if (pos + length + 12 > rx_data_length) |
|
913 { |
|
914 EC_DBG(KERN_ERR "EtherCAT: Received frame with incomplete command data!\n"); |
|
915 EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); |
|
916 output_debug_data(master->tx_data, master->tx_data_length); |
|
917 EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n"); |
|
918 output_debug_data(master->rx_data, rx_data_length); |
|
919 return -1; |
|
920 } |
|
921 |
|
922 found = 0; |
|
923 |
|
924 for (cmd = master->first_command; cmd != NULL; cmd = cmd->next) |
|
925 { |
|
926 if (cmd->state == ECAT_CS_SENT |
|
927 && cmd->type == command_type |
|
928 && cmd->index == command_index |
|
929 && cmd->data_length == length) |
|
930 { |
|
931 found = 1; |
|
932 cmd->state = ECAT_CS_RECEIVED; |
|
933 |
|
934 // Empfangene Daten in Kommandodatenspeicher kopieren |
|
935 memcpy(cmd->data, master->rx_data + pos + 10, length); |
|
936 |
|
937 // Working-Counter setzen |
|
938 cmd->working_counter = (master->rx_data[pos + length + 10] & 0xFF) |
|
939 | ((master->rx_data[pos + length + 11] & 0xFF) << 8); |
|
940 } |
|
941 } |
|
942 |
|
943 if (!found) |
|
944 { |
|
945 EC_DBG(KERN_WARNING "EtherCAT: WARNING - Command not assigned!\n"); |
|
946 } |
|
947 |
|
948 pos += length + 12; |
|
949 } |
|
950 |
|
951 for (cmd = master->first_command; cmd != NULL; cmd = cmd->next) |
|
952 { |
|
953 if (cmd->state == ECAT_CS_SENT) |
|
954 { |
|
955 EC_DBG(KERN_WARNING "EtherCAT: WARNING - One command received no response!\n"); |
|
956 } |
|
957 } |
951 } |
958 |
952 |
959 master->dev->state = ECAT_DS_READY; |
953 master->dev->state = ECAT_DS_READY; |
960 |
954 |
961 return 0; |
955 return 0; |
962 } |
956 } |
963 |
957 |
964 /***************************************************************/ |
958 /***************************************************************/ |
|
959 |
|
960 #if 0 |
965 |
961 |
966 #define ECAT_FUNC_HEADER \ |
962 #define ECAT_FUNC_HEADER \ |
967 EtherCAT_command_t *cmd; \ |
963 EtherCAT_command_t *cmd; \ |
968 if ((cmd = alloc_cmd(master)) == NULL) \ |
964 if ((cmd = alloc_cmd(master)) == NULL) \ |
969 { \ |
965 { \ |
1350 |
1347 |
1351 int EtherCAT_state_change(EtherCAT_master_t *master, |
1348 int EtherCAT_state_change(EtherCAT_master_t *master, |
1352 EtherCAT_slave_t *slave, |
1349 EtherCAT_slave_t *slave, |
1353 unsigned char state_and_ack) |
1350 unsigned char state_and_ack) |
1354 { |
1351 { |
1355 EtherCAT_command_t *cmd; |
1352 EtherCAT_command_t cmd; |
1356 unsigned char data[2]; |
1353 unsigned char data[2]; |
1357 unsigned int tries_left; |
1354 unsigned int tries_left; |
1358 |
1355 |
1359 data[0] = state_and_ack; |
1356 data[0] = state_and_ack; |
1360 data[1] = 0x00; |
1357 data[1] = 0x00; |
1361 |
1358 |
1362 if ((cmd = EtherCAT_write(master, slave->station_address, |
1359 EtherCAT_command_write(&cmd, slave->station_address, 0x0120, 2, data); |
1363 0x0120, 2, data)) == NULL) |
1360 |
1364 { |
1361 if (EtherCAT_simple_send_receive(master, &cmd) != 0) |
1365 EC_DBG(KERN_ERR "EtherCAT: Could not set state %02X - Out of memory!\n", state_and_ack); |
1362 { |
1366 return -1; |
|
1367 } |
|
1368 |
|
1369 if (EtherCAT_async_send_receive(master) != 0) |
|
1370 { |
|
1371 EtherCAT_remove_command(master, cmd); |
|
1372 |
|
1373 EC_DBG(KERN_ERR "EtherCAT: Could not set state %02X - Unable to send!\n", state_and_ack); |
1363 EC_DBG(KERN_ERR "EtherCAT: Could not set state %02X - Unable to send!\n", state_and_ack); |
1374 return -2; |
1364 return -2; |
1375 } |
1365 } |
1376 |
1366 |
1377 if (cmd->working_counter != 1) |
1367 if (cmd.working_counter != 1) |
1378 { |
1368 { |
1379 EtherCAT_remove_command(master, cmd); |
|
1380 |
|
1381 EC_DBG(KERN_ERR "EtherCAT: Could not set state %02X - Device did not respond!\n", state_and_ack); |
1369 EC_DBG(KERN_ERR "EtherCAT: Could not set state %02X - Device did not respond!\n", state_and_ack); |
1382 return -3; |
1370 return -3; |
1383 } |
1371 } |
1384 |
1372 |
1385 EtherCAT_remove_command(master, cmd); |
|
1386 |
|
1387 slave->requested_state = state_and_ack & 0x0F; |
1373 slave->requested_state = state_and_ack & 0x0F; |
1388 |
1374 |
1389 tries_left = 1000; |
1375 tries_left = 100; |
1390 |
|
1391 while (tries_left) |
1376 while (tries_left) |
1392 { |
1377 { |
1393 udelay(10); |
1378 udelay(10); |
1394 |
1379 |
1395 if ((cmd = EtherCAT_read(master, slave->station_address, |
1380 EtherCAT_command_read(&cmd, slave->station_address, 0x0130, 2); |
1396 0x0130, 2)) == NULL) |
1381 |
1397 { |
1382 if (EtherCAT_simple_send_receive(master, &cmd) != 0) |
1398 EC_DBG(KERN_ERR "EtherCAT: Could not check state %02X - Out of memory!\n", state_and_ack); |
1383 { |
1399 return -1; |
|
1400 } |
|
1401 |
|
1402 if (EtherCAT_async_send_receive(master) != 0) |
|
1403 { |
|
1404 EtherCAT_remove_command(master, cmd); |
|
1405 |
|
1406 EC_DBG(KERN_ERR "EtherCAT: Could not check state %02X - Unable to send!\n", state_and_ack); |
1384 EC_DBG(KERN_ERR "EtherCAT: Could not check state %02X - Unable to send!\n", state_and_ack); |
1407 return -2; |
1385 return -2; |
1408 } |
1386 } |
1409 |
1387 |
1410 if (cmd->working_counter != 1) |
1388 if (cmd.working_counter != 1) |
1411 { |
1389 { |
1412 EtherCAT_remove_command(master, cmd); |
|
1413 |
|
1414 EC_DBG(KERN_ERR "EtherCAT: Could not check state %02X - Device did not respond!\n", state_and_ack); |
1390 EC_DBG(KERN_ERR "EtherCAT: Could not check state %02X - Device did not respond!\n", state_and_ack); |
1415 return -3; |
1391 return -3; |
1416 } |
1392 } |
1417 |
1393 |
1418 if (cmd->data[0] & 0x10) // State change error |
1394 if (cmd.data[0] & 0x10) // State change error |
1419 { |
1395 { |
1420 EtherCAT_remove_command(master, cmd); |
1396 EC_DBG(KERN_ERR "EtherCAT: Could not set state %02X - Device refused state change (code %02X)!\n", state_and_ack, cmd.data[0]); |
1421 |
|
1422 EC_DBG(KERN_ERR "EtherCAT: Could not set state %02X - Device refused state change (code %02X)!\n", state_and_ack, cmd->data[0]); |
|
1423 return -4; |
1397 return -4; |
1424 } |
1398 } |
1425 |
1399 |
1426 if (cmd->data[0] == (state_and_ack & 0x0F)) // State change successful |
1400 if (cmd.data[0] == (state_and_ack & 0x0F)) // State change successful |
1427 { |
1401 { |
1428 EtherCAT_remove_command(master, cmd); |
|
1429 |
|
1430 break; |
1402 break; |
1431 } |
1403 } |
1432 |
|
1433 EtherCAT_remove_command(master, cmd); |
|
1434 |
1404 |
1435 tries_left--; |
1405 tries_left--; |
1436 } |
1406 } |
1437 |
1407 |
1438 if (!tries_left) |
1408 if (!tries_left) |
1478 |
1448 |
1479 // Resetting FMMU's |
1449 // Resetting FMMU's |
1480 |
1450 |
1481 memset(data, 0x00, 256); |
1451 memset(data, 0x00, 256); |
1482 |
1452 |
1483 if ((cmd = EtherCAT_write(master, slave->station_address, 0x0600, 256, data)) == NULL) |
1453 EtherCAT_command_write(&cmd, slave->station_address, 0x0600, 256, data); |
1484 return -1; |
1454 |
1485 |
1455 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
1486 if (EtherCAT_async_send_receive(master) < 0) |
1456 |
1487 { |
1457 if (cmd.working_counter != 1) |
1488 EtherCAT_remove_command(master, cmd); |
1458 { |
1489 |
|
1490 return -1; |
|
1491 } |
|
1492 |
|
1493 if (cmd->working_counter != 1) |
|
1494 { |
|
1495 EtherCAT_remove_command(master, cmd); |
|
1496 |
|
1497 EC_DBG(KERN_ERR "EtherCAT: Resetting FMMUs - Slave %04X did not respond!\n", |
1459 EC_DBG(KERN_ERR "EtherCAT: Resetting FMMUs - Slave %04X did not respond!\n", |
1498 slave->station_address); |
1460 slave->station_address); |
1499 return -2; |
1461 return -2; |
1500 } |
1462 } |
1501 |
1463 |
1502 EtherCAT_remove_command(master, cmd); |
|
1503 |
|
1504 // Resetting Sync Manager channels |
1464 // Resetting Sync Manager channels |
1505 |
1465 |
1506 if (desc->type != ECAT_ST_SIMPLE_NOSYNC) |
1466 if (desc->type != ECAT_ST_SIMPLE_NOSYNC) |
1507 { |
1467 { |
1508 memset(data, 0x00, 256); |
1468 memset(data, 0x00, 256); |
1509 |
1469 |
1510 if ((cmd = EtherCAT_write(master, slave->station_address, 0x0800, 256, data)) == NULL) |
1470 EtherCAT_command_write(&cmd, slave->station_address, 0x0800, 256, data); |
1511 return -1; |
1471 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
1512 |
1472 |
1513 if (EtherCAT_async_send_receive(master) < 0) |
1473 if (cmd.working_counter != 1) |
1514 { |
1474 { |
1515 EtherCAT_remove_command(master, cmd); |
|
1516 |
|
1517 return -1; |
|
1518 } |
|
1519 |
|
1520 if (cmd->working_counter != 1) |
|
1521 { |
|
1522 EtherCAT_remove_command(master, cmd); |
|
1523 |
|
1524 EC_DBG(KERN_ERR "EtherCAT: Resetting SMs - Slave %04X did not respond!\n", |
1475 EC_DBG(KERN_ERR "EtherCAT: Resetting SMs - Slave %04X did not respond!\n", |
1525 slave->station_address); |
1476 slave->station_address); |
1526 return -2; |
1477 return -2; |
1527 } |
1478 } |
1528 |
|
1529 EtherCAT_remove_command(master, cmd); |
|
1530 } |
1479 } |
1531 |
1480 |
1532 // Init Mailbox communication |
1481 // Init Mailbox communication |
1533 |
1482 |
1534 if (desc->type == ECAT_ST_MAILBOX) |
1483 if (desc->type == ECAT_ST_MAILBOX) |
1535 { |
1484 { |
1536 if (desc->sm0) |
1485 if (desc->sm0) |
1537 { |
1486 { |
1538 if ((cmd = EtherCAT_write(master, slave->station_address, 0x0800, 8, desc->sm0)) == NULL) |
1487 EtherCAT_command_write(&cmd, slave->station_address, 0x0800, 8, desc->sm0); |
1539 return -1; |
1488 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
1540 |
1489 |
1541 if (EtherCAT_async_send_receive(master) < 0) |
1490 if (cmd.working_counter != 1) |
1542 { |
1491 { |
1543 EtherCAT_remove_command(master, cmd); |
|
1544 |
|
1545 return -1; |
|
1546 } |
|
1547 |
|
1548 if (cmd->working_counter != 1) |
|
1549 { |
|
1550 EtherCAT_remove_command(master, cmd); |
|
1551 |
|
1552 EC_DBG(KERN_ERR "EtherCAT: Setting SM0 - Slave %04X did not respond!\n", |
1492 EC_DBG(KERN_ERR "EtherCAT: Setting SM0 - Slave %04X did not respond!\n", |
1553 slave->station_address); |
1493 slave->station_address); |
1554 return -3; |
1494 return -3; |
1555 } |
1495 } |
1556 |
|
1557 EtherCAT_remove_command(master, cmd); |
|
1558 } |
1496 } |
1559 |
1497 |
1560 if (desc->sm1) |
1498 if (desc->sm1) |
1561 { |
1499 { |
1562 if ((cmd = EtherCAT_write(master, slave->station_address, 0x0808, 8, desc->sm1)) == NULL) |
1500 EtherCAT_command_write(&cmd, slave->station_address, 0x0808, 8, desc->sm1); |
1563 return -1; |
1501 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
1564 |
1502 |
1565 if (EtherCAT_async_send_receive(master) < 0) |
1503 if (cmd.working_counter != 1) |
1566 { |
1504 { |
1567 EtherCAT_remove_command(master, cmd); |
|
1568 |
|
1569 return -1; |
|
1570 } |
|
1571 |
|
1572 if (cmd->working_counter != 1) |
|
1573 { |
|
1574 EtherCAT_remove_command(master, cmd); |
|
1575 |
|
1576 EC_DBG(KERN_ERR "EtherCAT: Setting SM1 - Slave %04X did not respond!\n", |
1505 EC_DBG(KERN_ERR "EtherCAT: Setting SM1 - Slave %04X did not respond!\n", |
1577 slave->station_address); |
1506 slave->station_address); |
1578 return -2; |
1507 return -2; |
1579 } |
1508 } |
1580 |
|
1581 EtherCAT_remove_command(master, cmd); |
|
1582 } |
1509 } |
1583 } |
1510 } |
1584 |
1511 |
1585 // Change state to PREOP |
1512 // Change state to PREOP |
1586 |
1513 |
1598 fmmu[0] = slave->logical_address0 & 0x000000FF; |
1525 fmmu[0] = slave->logical_address0 & 0x000000FF; |
1599 fmmu[1] = (slave->logical_address0 & 0x0000FF00) >> 8; |
1526 fmmu[1] = (slave->logical_address0 & 0x0000FF00) >> 8; |
1600 fmmu[2] = (slave->logical_address0 & 0x00FF0000) >> 16; |
1527 fmmu[2] = (slave->logical_address0 & 0x00FF0000) >> 16; |
1601 fmmu[3] = (slave->logical_address0 & 0xFF000000) >> 24; |
1528 fmmu[3] = (slave->logical_address0 & 0xFF000000) >> 24; |
1602 |
1529 |
1603 if ((cmd = EtherCAT_write(master, slave->station_address, 0x0600, 16, fmmu)) == NULL) |
1530 EtherCAT_command_write(&cmd, slave->station_address, 0x0600, 16, fmmu); |
1604 return -1; |
1531 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
1605 |
1532 |
1606 if (EtherCAT_async_send_receive(master) < 0) |
1533 if (cmd.working_counter != 1) |
1607 { |
1534 { |
1608 EtherCAT_remove_command(master, cmd); |
|
1609 |
|
1610 return -1; |
|
1611 } |
|
1612 |
|
1613 if (cmd->working_counter != 1) |
|
1614 { |
|
1615 EtherCAT_remove_command(master, cmd); |
|
1616 |
|
1617 EC_DBG(KERN_ERR "EtherCAT: Setting FMMU0 - Slave %04X did not respond!\n", |
1535 EC_DBG(KERN_ERR "EtherCAT: Setting FMMU0 - Slave %04X did not respond!\n", |
1618 slave->station_address); |
1536 slave->station_address); |
1619 return -2; |
1537 return -2; |
1620 } |
1538 } |
1621 |
|
1622 EtherCAT_remove_command(master, cmd); |
|
1623 } |
1539 } |
1624 |
1540 |
1625 // Set Sync Managers |
1541 // Set Sync Managers |
1626 |
1542 |
1627 if (desc->type != ECAT_ST_MAILBOX) |
1543 if (desc->type != ECAT_ST_MAILBOX) |
1628 { |
1544 { |
1629 if (desc->sm0) |
1545 if (desc->sm0) |
1630 { |
1546 { |
1631 if ((cmd = EtherCAT_write(master, slave->station_address, 0x0800, 8, desc->sm0)) == NULL) |
1547 EtherCAT_command_write(&cmd, slave->station_address, 0x0800, 8, desc->sm0); |
1632 return -1; |
1548 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
1633 |
1549 |
1634 if (EtherCAT_async_send_receive(master) < 0) |
1550 if (cmd.working_counter != 1) |
1635 { |
1551 { |
1636 EtherCAT_remove_command(master, cmd); |
|
1637 |
|
1638 return -1; |
|
1639 } |
|
1640 |
|
1641 if (cmd->working_counter != 1) |
|
1642 { |
|
1643 EtherCAT_remove_command(master, cmd); |
|
1644 |
|
1645 EC_DBG(KERN_ERR "EtherCAT: Setting SM0 - Slave %04X did not respond!\n", |
1552 EC_DBG(KERN_ERR "EtherCAT: Setting SM0 - Slave %04X did not respond!\n", |
1646 slave->station_address); |
1553 slave->station_address); |
1647 return -3; |
1554 return -3; |
1648 } |
1555 } |
1649 |
|
1650 EtherCAT_remove_command(master, cmd); |
|
1651 } |
1556 } |
1652 |
1557 |
1653 if (desc->sm1) |
1558 if (desc->sm1) |
1654 { |
1559 { |
1655 if ((cmd = EtherCAT_write(master, slave->station_address, 0x0808, 8, desc->sm1)) == NULL) |
1560 EtherCAT_command_write(&cmd, slave->station_address, 0x0808, 8, desc->sm1); |
1656 return -1; |
1561 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
1657 |
1562 |
1658 if (EtherCAT_async_send_receive(master) < 0) |
1563 if (cmd.working_counter != 1) |
1659 { |
1564 { |
1660 EtherCAT_remove_command(master, cmd); |
|
1661 |
|
1662 return -1; |
|
1663 } |
|
1664 |
|
1665 if (cmd->working_counter != 1) |
|
1666 { |
|
1667 EtherCAT_remove_command(master, cmd); |
|
1668 |
|
1669 EC_DBG(KERN_ERR "EtherCAT: Setting SM1 - Slave %04X did not respond!\n", |
1565 EC_DBG(KERN_ERR "EtherCAT: Setting SM1 - Slave %04X did not respond!\n", |
1670 slave->station_address); |
1566 slave->station_address); |
1671 return -3; |
1567 return -3; |
1672 } |
1568 } |
1673 |
|
1674 EtherCAT_remove_command(master, cmd); |
|
1675 } |
1569 } |
1676 } |
1570 } |
1677 |
1571 |
1678 if (desc->sm2) |
1572 if (desc->sm2) |
1679 { |
1573 { |
1680 if ((cmd = EtherCAT_write(master, slave->station_address, 0x0810, 8, desc->sm2)) == NULL) |
1574 EtherCAT_command_write(&cmd, slave->station_address, 0x0810, 8, desc->sm2); |
1681 return -1; |
1575 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
1682 |
1576 |
1683 if (EtherCAT_async_send_receive(master) < 0) |
1577 if (cmd.working_counter != 1) |
1684 { |
1578 { |
1685 EtherCAT_remove_command(master, cmd); |
|
1686 |
|
1687 return -1; |
|
1688 } |
|
1689 |
|
1690 if (cmd->working_counter != 1) |
|
1691 { |
|
1692 EtherCAT_remove_command(master, cmd); |
|
1693 |
|
1694 EC_DBG(KERN_ERR "EtherCAT: Setting SM2 - Slave %04X did not respond!\n", |
1579 EC_DBG(KERN_ERR "EtherCAT: Setting SM2 - Slave %04X did not respond!\n", |
1695 slave->station_address); |
1580 slave->station_address); |
1696 return -3; |
1581 return -3; |
1697 } |
1582 } |
1698 |
|
1699 EtherCAT_remove_command(master, cmd); |
|
1700 } |
1583 } |
1701 |
1584 |
1702 if (desc->sm3) |
1585 if (desc->sm3) |
1703 { |
1586 { |
1704 if ((cmd = EtherCAT_write(master, slave->station_address, 0x0818, 8, desc->sm3)) == NULL) |
1587 EtherCAT_command_write(&cmd, slave->station_address, 0x0818, 8, desc->sm3); |
1705 return -1; |
1588 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
1706 |
1589 |
1707 if (EtherCAT_async_send_receive(master) < 0) |
1590 if (cmd.working_counter != 1) |
1708 { |
1591 { |
1709 EtherCAT_remove_command(master, cmd); |
|
1710 |
|
1711 return -1; |
|
1712 } |
|
1713 |
|
1714 if (cmd->working_counter != 1) |
|
1715 { |
|
1716 EtherCAT_remove_command(master, cmd); |
|
1717 |
|
1718 EC_DBG(KERN_ERR "EtherCAT: Setting SM3 - Slave %04X did not respond!\n", |
1592 EC_DBG(KERN_ERR "EtherCAT: Setting SM3 - Slave %04X did not respond!\n", |
1719 slave->station_address); |
1593 slave->station_address); |
1720 return -3; |
1594 return -3; |
1721 } |
1595 } |
1722 |
|
1723 EtherCAT_remove_command(master, cmd); |
|
1724 } |
1596 } |
1725 |
1597 |
1726 // Change state to SAVEOP |
1598 // Change state to SAVEOP |
1727 if (EtherCAT_state_change(master, slave, ECAT_STATE_SAVEOP) != 0) |
1599 if (EtherCAT_state_change(master, slave, ECAT_STATE_SAVEOP) != 0) |
1728 { |
1600 { |
1872 @return 0 bei Erfolg, sonst < 0 |
1729 @return 0 bei Erfolg, sonst < 0 |
1873 */ |
1730 */ |
1874 |
1731 |
1875 int EtherCAT_read_process_data(EtherCAT_master_t *master) |
1732 int EtherCAT_read_process_data(EtherCAT_master_t *master) |
1876 { |
1733 { |
1877 int ret = -1; |
1734 EtherCAT_device_call_isr(master->dev); |
1878 |
1735 |
1879 if (!master->process_data_command) |
1736 if (EtherCAT_simple_receive(master, &master->process_data_command) < 0) |
1880 { |
1737 { |
1881 EC_DBG(KERN_WARNING "EtherCAT: No process data command available!\n"); |
1738 EC_DBG(KERN_ERR "EtherCAT: Could not receive cyclic command!\n"); |
1882 return -1; |
1739 return -1; |
1883 } |
1740 } |
1884 |
1741 |
|
1742 if (master->process_data_command.state != ECAT_CS_RECEIVED) |
|
1743 { |
|
1744 EC_DBG(KERN_WARNING "EtherCAT: Process data command not received!\n"); |
|
1745 return -1; |
|
1746 } |
|
1747 |
|
1748 // Daten von Kommando in Prozessdaten des Master kopieren |
|
1749 memcpy(master->process_data, master->process_data_command.data, |
|
1750 master->process_data_length); |
|
1751 |
|
1752 return 0; |
|
1753 } |
|
1754 |
|
1755 /***************************************************************/ |
|
1756 |
|
1757 /** |
|
1758 Verwirft das zuletzt gesendete Prozessdatenkommando. |
|
1759 |
|
1760 @param master EtherCAT-Master |
|
1761 */ |
|
1762 |
|
1763 void EtherCAT_clear_process_data(EtherCAT_master_t *master) |
|
1764 { |
1885 EtherCAT_device_call_isr(master->dev); |
1765 EtherCAT_device_call_isr(master->dev); |
1886 |
1766 master->dev->state = ECAT_DS_READY; |
1887 if (EtherCAT_receive(master) < 0) |
|
1888 { |
|
1889 EC_DBG(KERN_ERR "EtherCAT: Could not receive cyclic command!\n"); |
|
1890 } |
|
1891 else if (master->process_data_command->state != ECAT_CS_RECEIVED) |
|
1892 { |
|
1893 EC_DBG(KERN_WARNING "EtherCAT: Process data command not received!\n"); |
|
1894 } |
|
1895 else |
|
1896 { |
|
1897 // Daten von Kommando in Prozessdaten des Master kopieren |
|
1898 memcpy(master->process_data, master->process_data_command->data, master->process_data_length); |
|
1899 ret = 0; |
|
1900 } |
|
1901 |
|
1902 EtherCAT_remove_command(master, master->process_data_command); |
|
1903 EtherCAT_command_clear(master->process_data_command); |
|
1904 master->process_data_command = NULL; |
|
1905 |
|
1906 return ret; |
|
1907 } |
|
1908 |
|
1909 /***************************************************************/ |
|
1910 |
|
1911 /** |
|
1912 Verwirft ein zuvor gesendetes Prozessdatenkommando. |
|
1913 |
|
1914 Diese Funktion sollte nach Ende des zyklischen Betriebes |
|
1915 aufgerufen werden, um das noch wartende Prozessdaten-Kommando |
|
1916 zu entfernen. |
|
1917 |
|
1918 @param master EtherCAT-Master |
|
1919 */ |
|
1920 |
|
1921 void EtherCAT_clear_process_data(EtherCAT_master_t *master) |
|
1922 { |
|
1923 if (!master->process_data_command) return; |
|
1924 |
|
1925 EtherCAT_remove_command(master, master->process_data_command); |
|
1926 EtherCAT_command_clear(master->process_data_command); |
|
1927 master->process_data_command = NULL; |
|
1928 } |
1767 } |
1929 |
1768 |
1930 /***************************************************************/ |
1769 /***************************************************************/ |
1931 |
1770 |
1932 /** |
1771 /** |