517 } |
527 } |
518 else { |
528 else { |
519 MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", (*m).cob_id.w); |
529 MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", (*m).cob_id.w); |
520 } |
530 } |
521 |
531 |
522 // Testing the command specifier |
532 /* Testing the command specifier */ |
523 // Allowed : cs = 0, 1, 2, 3, 4. (= all except those for block tranfert). |
533 /* Allowed : cs = 0, 1, 2, 3, 4. (= all except those for block tranfert). */ |
524 // cs = other : Not allowed -> abort. |
534 /* cs = other : Not allowed -> abort. */ |
525 switch (getSDOcs(m->data[0])) { |
535 switch (getSDOcs(m->data[0])) { |
526 |
536 |
527 case 0: |
537 case 0: |
528 // I am SERVER |
538 /* I am SERVER */ |
529 if (whoami == SDO_SERVER) { |
539 if (whoami == SDO_SERVER) { |
530 // Receiving a download segment data. |
540 /* Receiving a download segment data. */ |
531 // A SDO transfert should have been yet initiated. |
541 /* A SDO transfert should have been yet initiated. */ |
532 err = getSDOlineOnUse( d, nodeId, whoami, &line ); |
542 err = getSDOlineOnUse( d, nodeId, whoami, &line ); |
533 if (!err) |
543 if (!err) |
534 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
544 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
535 if (err) { |
545 if (err) { |
536 MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", |
546 MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", |
537 nodeId); |
547 nodeId); |
538 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
548 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
539 return 0xFF; |
549 return 0xFF; |
540 } |
550 } |
541 // Reset the wathdog |
551 /* Reset the wathdog */ |
542 RestartSDO_TIMER(line) |
552 RestartSDO_TIMER(line) |
543 MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", nodeId); |
553 MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", nodeId); |
544 index = d->transfers[line].index; |
554 index = d->transfers[line].index; |
545 subIndex = d->transfers[line].subIndex; |
555 subIndex = d->transfers[line].subIndex; |
546 // Toggle test. |
556 /* Toggle test. */ |
547 if (d->transfers[line].toggle != getSDOt(m->data[0])) { |
557 if (d->transfers[line].toggle != getSDOt(m->data[0])) { |
548 MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0])); |
558 MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0])); |
549 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); |
559 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); |
550 return 0xFF; |
560 return 0xFF; |
551 } |
561 } |
552 // Nb of data to be downloaded |
562 /* Nb of data to be downloaded */ |
553 nbBytes = 7 - getSDOn3(m->data[0]); |
563 nbBytes = 7 - getSDOn3(m->data[0]); |
554 // Store the data in the transfert structure. |
564 /* Store the data in the transfert structure. */ |
555 err = SDOtoLine(d, line, nbBytes, (*m).data + 1); |
565 err = SDOtoLine(d, line, nbBytes, (*m).data + 1); |
556 if (err) { |
566 if (err) { |
557 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
567 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
558 return 0xFF; |
568 return 0xFF; |
559 } |
569 } |
560 // Sending the SDO response, CS = 1 |
570 /* Sending the SDO response, CS = 1 */ |
561 sdo.nodeId = *d->bDeviceNodeId; // The node id of the server, (here it is the sender). |
571 sdo.nodeId = *d->bDeviceNodeId; /* The node id of the server, (here it is the sender). */ |
562 sdo.body.data[0] = (1 << 5) | (d->transfers[line].toggle << 4); |
572 sdo.body.data[0] = (1 << 5) | (d->transfers[line].toggle << 4); |
563 for (i = 1 ; i < 8 ; i++) |
573 for (i = 1 ; i < 8 ; i++) |
564 sdo.body.data[i] = 0; |
574 sdo.body.data[i] = 0; |
565 MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", nodeId); |
575 MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", nodeId); |
566 sendSDO(d, whoami, sdo); |
576 sendSDO(d, whoami, sdo); |
567 // Inverting the toggle for the next segment. |
577 /* Inverting the toggle for the next segment. */ |
568 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
578 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
569 // If it was the last segment, |
579 /* If it was the last segment, */ |
570 if (getSDOc(m->data[0])) { |
580 if (getSDOc(m->data[0])) { |
571 // Transfering line data to object dictionary. |
581 /* Transfering line data to object dictionary. */ |
572 // The code does not use the "d" of initiate frame. So it is safe if e=s=0 |
582 /* The code does not use the "d" of initiate frame. So it is safe if e=s=0 */ |
573 errorCode = SDOlineToObjdict(d, line); |
583 errorCode = SDOlineToObjdict(d, line); |
574 if (errorCode) { |
584 if (errorCode) { |
575 MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0); |
585 MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0); |
576 failedSDO(d, nodeId, whoami, index, subIndex, errorCode); |
586 failedSDO(d, nodeId, whoami, index, subIndex, errorCode); |
577 return 0xFF; |
587 return 0xFF; |
578 } |
588 } |
579 // Release of the line |
589 /* Release of the line */ |
580 resetSDOline(d, line); |
590 resetSDOline(d, line); |
581 MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", nodeId); |
591 MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", nodeId); |
582 } |
592 } |
583 } // end if SERVER |
593 } /* end if SERVER */ |
584 else { // if CLIENT |
594 else { /* if CLIENT */ |
585 // I am CLIENT |
595 /* I am CLIENT */ |
586 // It is a request for a previous upload segment. We should find a line opened for this. |
596 /* It is a request for a previous upload segment. We should find a line opened for this.*/ |
587 err = getSDOlineOnUse( d, nodeId, whoami, &line); |
597 err = getSDOlineOnUse( d, nodeId, whoami, &line); |
588 if (!err) |
598 if (!err) |
589 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
599 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
590 if (err) { |
600 if (err) { |
591 MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); |
601 MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); |
592 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
602 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
593 return 0xFF; |
603 return 0xFF; |
594 } |
604 } |
595 // Reset the wathdog |
605 /* Reset the wathdog */ |
596 RestartSDO_TIMER(line) |
606 RestartSDO_TIMER(line) |
597 index = d->transfers[line].index; |
607 index = d->transfers[line].index; |
598 subIndex = d->transfers[line].subIndex; |
608 subIndex = d->transfers[line].subIndex; |
599 // test of the toggle; |
609 /* test of the toggle; */ |
600 if (d->transfers[line].toggle != getSDOt(m->data[0])) { |
610 if (d->transfers[line].toggle != getSDOt(m->data[0])) { |
601 MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId); |
611 MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId); |
602 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); |
612 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); |
603 return 0xFF; |
613 return 0xFF; |
604 } |
614 } |
605 // nb of data to be uploaded |
615 /* nb of data to be uploaded */ |
606 nbBytes = 7 - getSDOn3(m->data[0]); |
616 nbBytes = 7 - getSDOn3(m->data[0]); |
607 // Storing the data in the line structure. |
617 /* Storing the data in the line structure. */ |
608 err = SDOtoLine(d, line, nbBytes, (*m).data + 1); |
618 err = SDOtoLine(d, line, nbBytes, (*m).data + 1); |
609 if (err) { |
619 if (err) { |
610 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
620 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
611 return 0xFF; |
621 return 0xFF; |
612 } |
622 } |
613 // Inverting the toggle for the next segment. |
623 /* Inverting the toggle for the next segment. */ |
614 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
624 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
615 // If it was the last segment, |
625 /* If it was the last segment,*/ |
616 if ( getSDOc(m->data[0])) { |
626 if ( getSDOc(m->data[0])) { |
617 // Put in state finished |
627 /* Put in state finished */ |
618 // The code is safe for the case e=s=0 in initiate frame. |
628 /* The code is safe for the case e=s=0 in initiate frame. */ |
619 StopSDO_TIMER(line) |
629 StopSDO_TIMER(line) |
620 d->transfers[line].state = SDO_FINISHED; |
630 d->transfers[line].state = SDO_FINISHED; |
621 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
631 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
622 |
632 |
623 MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId); |
633 MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId); |
624 } |
634 } |
625 else { // more segments to receive |
635 else { /* more segments to receive */ |
626 // Sending the request for the next segment. |
636 /* Sending the request for the next segment. */ |
627 sdo.nodeId = nodeId; |
637 sdo.nodeId = nodeId; |
628 sdo.body.data[0] = (3 << 5) | (d->transfers[line].toggle << 4); |
638 sdo.body.data[0] = (3 << 5) | (d->transfers[line].toggle << 4); |
629 for (i = 1 ; i < 8 ; i++) |
639 for (i = 1 ; i < 8 ; i++) |
630 sdo.body.data[i] = 0; |
640 sdo.body.data[i] = 0; |
631 sendSDO(d, whoami, sdo); |
641 sendSDO(d, whoami, sdo); |
632 MSG_WAR(0x3A78, "SDO send upload segment request to nodeId", nodeId); |
642 MSG_WAR(0x3A78, "SDO send upload segment request to nodeId", nodeId); |
633 } |
643 } |
634 } // End if CLIENT |
644 } /* End if CLIENT */ |
635 break; |
645 break; |
636 |
646 |
637 case 1: |
647 case 1: |
638 // I am SERVER |
648 /* I am SERVER */ |
639 // Receive of an initiate download |
649 /* Receive of an initiate download */ |
640 if (whoami == SDO_SERVER) { |
650 if (whoami == SDO_SERVER) { |
641 index = getSDOindex(m->data[1],m->data[2]); |
651 index = getSDOindex(m->data[1],m->data[2]); |
642 subIndex = getSDOsubIndex(m->data[3]); |
652 subIndex = getSDOsubIndex(m->data[3]); |
643 MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ", |
653 MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ", |
644 nodeId); |
654 nodeId); |
645 MSG_WAR(0x3A80, "Writing at index : ", index); |
655 MSG_WAR(0x3A80, "Writing at index : ", index); |
646 MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex); |
656 MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex); |
647 |
657 |
648 // Search if a SDO transfert have been yet initiated |
658 /* Search if a SDO transfert have been yet initiated */ |
649 err = getSDOlineOnUse( d, nodeId, whoami, &line ); |
659 err = getSDOlineOnUse( d, nodeId, whoami, &line ); |
650 if (! err) { |
660 if (! err) { |
651 MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0); |
661 MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0); |
652 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
662 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
653 return 0xFF; |
663 return 0xFF; |
654 } |
664 } |
655 // No line on use. Great ! |
665 /* No line on use. Great ! */ |
656 // Try to open a new line. |
666 /* Try to open a new line. */ |
657 err = getSDOfreeLine( d, whoami, &line ); |
667 err = getSDOfreeLine( d, whoami, &line ); |
658 if (err) { |
668 if (err) { |
659 MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0); |
669 MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0); |
660 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
670 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
661 return 0xFF; |
671 return 0xFF; |
662 } |
672 } |
663 initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS); |
673 initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS); |
664 |
674 |
665 if (getSDOe(m->data[0])) { // If SDO expedited |
675 if (getSDOe(m->data[0])) { /* If SDO expedited */ |
666 // nb of data to be downloaded |
676 /* nb of data to be downloaded */ |
667 nbBytes = 4 - getSDOn2(m->data[0]); |
677 nbBytes = 4 - getSDOn2(m->data[0]); |
668 // Storing the data in the line structure. |
678 /* Storing the data in the line structure. */ |
669 d->transfers[line].count = nbBytes; |
679 d->transfers[line].count = nbBytes; |
670 err = SDOtoLine(d, line, nbBytes, (*m).data + 4); |
680 err = SDOtoLine(d, line, nbBytes, (*m).data + 4); |
671 |
681 |
672 if (err) { |
682 if (err) { |
673 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
683 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
674 return 0xFF; |
684 return 0xFF; |
675 } |
685 } |
676 |
686 |
677 // SDO expedited -> transfert finished. Data can be stored in the dictionary. |
687 /* SDO expedited -> transfert finished. Data can be stored in the dictionary. */ |
678 // The line will be reseted when it is downloading in the dictionary. |
688 /* The line will be reseted when it is downloading in the dictionary. */ |
679 MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfert. Finished.: ", nodeId); |
689 MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfert. Finished.: ", nodeId); |
680 // Transfering line data to object dictionary. |
690 /* Transfering line data to object dictionary. */ |
681 errorCode = SDOlineToObjdict(d, line); |
691 errorCode = SDOlineToObjdict(d, line); |
682 if (errorCode) { |
692 if (errorCode) { |
683 MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0); |
693 MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0); |
684 failedSDO(d, nodeId, whoami, index, subIndex, errorCode); |
694 failedSDO(d, nodeId, whoami, index, subIndex, errorCode); |
685 return 0xFF; |
695 return 0xFF; |
686 } |
696 } |
687 // Release of the line. |
697 /* Release of the line. */ |
688 resetSDOline(d, line); |
698 resetSDOline(d, line); |
689 } |
699 } |
690 else {// So, if it is not an expedited transfert |
700 else {/* So, if it is not an expedited transfert */ |
691 if (getSDOs(m->data[0])) { |
701 if (getSDOs(m->data[0])) { |
692 // TODO : if e and s = 0, not reading m->data[4] but put nbBytes = 0 |
702 /* TODO : if e and s = 0, not reading m->data[4] but put nbBytes = 0 */ |
693 nbBytes = m->data[4]; // Transfert limited to 255 bytes. |
703 nbBytes = m->data[4]; /* Transfert limited to 255 bytes. */ |
694 err = setSDOlineRestBytes(d, nodeId, nbBytes); |
704 err = setSDOlineRestBytes(d, nodeId, nbBytes); |
695 if (err) { |
705 if (err) { |
696 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
706 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
697 return 0xFF; |
707 return 0xFF; |
698 } |
708 } |
699 } |
709 } |
700 } |
710 } |
701 //Sending a SDO, cs=3 |
711 /*Sending a SDO, cs=3*/ |
702 sdo.nodeId = *d->bDeviceNodeId; // The node id of the server, (here it is the sender). |
712 sdo.nodeId = *d->bDeviceNodeId; /* The node id of the server, (here it is the sender).*/ |
703 sdo.body.data[0] = 3 << 5; |
713 sdo.body.data[0] = 3 << 5; |
704 sdo.body.data[1] = index & 0xFF; // LSB |
714 sdo.body.data[1] = index & 0xFF; /* LSB */ |
705 sdo.body.data[2] = (index >> 8) & 0xFF; // MSB |
715 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ |
706 sdo.body.data[3] = subIndex; |
716 sdo.body.data[3] = subIndex; |
707 for (i = 4 ; i < 8 ; i++) |
717 for (i = 4 ; i < 8 ; i++) |
708 sdo.body.data[i] = 0; |
718 sdo.body.data[i] = 0; |
709 sendSDO(d, whoami, sdo); |
719 sendSDO(d, whoami, sdo); |
710 } // end if I am SERVER |
720 } /* end if I am SERVER */ |
711 else { |
721 else { |
712 // I am CLIENT |
722 /* I am CLIENT */ |
713 // It is a response for a previous download segment. We should find a line opened for this. |
723 /* It is a response for a previous download segment. We should find a line opened for this. */ |
714 err = getSDOlineOnUse( d, nodeId, whoami, &line); |
724 err = getSDOlineOnUse( d, nodeId, whoami, &line); |
715 if (!err) |
725 if (!err) |
716 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
726 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
717 if (err) { |
727 if (err) { |
718 MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); |
728 MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); |
719 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
729 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
720 return 0xFF; |
730 return 0xFF; |
721 } |
731 } |
722 // Reset the wathdog |
732 /* Reset the wathdog */ |
723 RestartSDO_TIMER(line) |
733 RestartSDO_TIMER(line) |
724 index = d->transfers[line].index; |
734 index = d->transfers[line].index; |
725 subIndex = d->transfers[line].subIndex; |
735 subIndex = d->transfers[line].subIndex; |
726 // test of the toggle; |
736 /* test of the toggle; */ |
727 if (d->transfers[line].toggle != getSDOt(m->data[0])) { |
737 if (d->transfers[line].toggle != getSDOt(m->data[0])) { |
728 MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId); |
738 MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId); |
729 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); |
739 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); |
730 return 0xFF; |
740 return 0xFF; |
731 } |
741 } |
732 |
742 |
733 // End transmission or downloading next segment. We need to know if it will be the last one. |
743 /* End transmission or downloading next segment. We need to know if it will be the last one. */ |
734 getSDOlineRestBytes(d, line, &nbBytes); |
744 getSDOlineRestBytes(d, line, &nbBytes); |
735 if (nbBytes == 0) { |
745 if (nbBytes == 0) { |
736 MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId); |
746 MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId); |
737 StopSDO_TIMER(line) |
747 StopSDO_TIMER(line) |
738 d->transfers[line].state = SDO_FINISHED; |
748 d->transfers[line].state = SDO_FINISHED; |
739 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
749 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
740 return 0x00; |
750 return 0x00; |
741 } |
751 } |
742 // At least one transfer to send. |
752 /* At least one transfer to send. */ |
743 if (nbBytes > 7) { |
753 if (nbBytes > 7) { |
744 // several segments to download. |
754 /* several segments to download.*/ |
745 // code to send the next segment. (cs = 0; c = 0) |
755 /* code to send the next segment. (cs = 0; c = 0) */ |
746 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
756 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
747 sdo.nodeId = nodeId; // The server node Id; |
757 sdo.nodeId = nodeId; /* The server node Id; */ |
748 sdo.body.data[0] = (d->transfers[line].toggle << 4); |
758 sdo.body.data[0] = (d->transfers[line].toggle << 4); |
749 err = lineToSDO(d, line, 7, sdo.body.data + 1); |
759 err = lineToSDO(d, line, 7, sdo.body.data + 1); |
750 if (err) { |
760 if (err) { |
751 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
761 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
752 return 0xFF; |
762 return 0xFF; |
753 } |
763 } |
754 } |
764 } |
755 else { |
765 else { |
756 // Last segment. |
766 /* Last segment. */ |
757 // code to send the last segment. (cs = 0; c = 1) |
767 /* code to send the last segment. (cs = 0; c = 1)*/ |
758 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
768 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
759 sdo.nodeId = nodeId; // The server node Id; |
769 sdo.nodeId = nodeId; /* The server node Id; */ |
760 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1; |
770 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1; |
761 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); |
771 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); |
762 if (err) { |
772 if (err) { |
763 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
773 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
764 return 0xFF; |
774 return 0xFF; |
766 for (i = nbBytes + 1 ; i < 8 ; i++) |
776 for (i = nbBytes + 1 ; i < 8 ; i++) |
767 sdo.body.data[i] = 0; |
777 sdo.body.data[i] = 0; |
768 } |
778 } |
769 MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId); |
779 MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId); |
770 sendSDO(d, whoami, sdo); |
780 sendSDO(d, whoami, sdo); |
771 } // end if I am a CLIENT |
781 } /* end if I am a CLIENT */ |
772 break; |
782 break; |
773 |
783 |
774 case 2: |
784 case 2: |
775 // I am SERVER |
785 /* I am SERVER */ |
776 // Receive of an initiate upload. |
786 /* Receive of an initiate upload.*/ |
777 if (whoami == SDO_SERVER) { |
787 if (whoami == SDO_SERVER) { |
778 index = getSDOindex(m->data[1],m->data[2]); |
788 index = getSDOindex(m->data[1],m->data[2]); |
779 subIndex = getSDOsubIndex(m->data[3]); |
789 subIndex = getSDOsubIndex(m->data[3]); |
780 MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", |
790 MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", |
781 nodeId); |
791 nodeId); |
782 MSG_WAR(0x3A90, "Reading at index : ", index); |
792 MSG_WAR(0x3A90, "Reading at index : ", index); |
783 MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex); |
793 MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex); |
784 // Search if a SDO transfert have been yet initiated |
794 /* Search if a SDO transfert have been yet initiated*/ |
785 err = getSDOlineOnUse( d, nodeId, whoami, &line ); |
795 err = getSDOlineOnUse( d, nodeId, whoami, &line ); |
786 if (! err) { |
796 if (! err) { |
787 MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line); |
797 MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line); |
788 MSG_WAR(0x3A93, "nodeId = ", nodeId); |
798 MSG_WAR(0x3A93, "nodeId = ", nodeId); |
789 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
799 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
790 return 0xFF; |
800 return 0xFF; |
791 } |
801 } |
792 // No line on use. Great ! |
802 /* No line on use. Great !*/ |
793 // Try to open a new line. |
803 /* Try to open a new line.*/ |
794 err = getSDOfreeLine( d, whoami, &line ); |
804 err = getSDOfreeLine( d, whoami, &line ); |
795 if (err) { |
805 if (err) { |
796 MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0); |
806 MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0); |
797 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
807 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
798 return 0xFF; |
808 return 0xFF; |
799 } |
809 } |
800 initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS); |
810 initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS); |
801 // Transfer data from dictionary to the line structure. |
811 /* Transfer data from dictionary to the line structure. */ |
802 errorCode = objdictToSDOline(d, line); |
812 errorCode = objdictToSDOline(d, line); |
803 |
813 |
804 if (errorCode) { |
814 if (errorCode) { |
805 MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ", |
815 MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ", |
806 errorCode); |
816 errorCode); |
807 failedSDO(d, nodeId, whoami, index, subIndex, errorCode); |
817 failedSDO(d, nodeId, whoami, index, subIndex, errorCode); |
808 return 0xFF; |
818 return 0xFF; |
809 } |
819 } |
810 // Preparing the response. |
820 /* Preparing the response.*/ |
811 getSDOlineRestBytes(d, line, &nbBytes); // Nb bytes to transfer ? |
821 getSDOlineRestBytes(d, line, &nbBytes); /* Nb bytes to transfer ? */ |
812 sdo.nodeId = nodeId; // The server node Id; |
822 sdo.nodeId = nodeId; /* The server node Id; */ |
813 if (nbBytes > 4) { |
823 if (nbBytes > 4) { |
814 // normal transfert. (segmented). |
824 /* normal transfert. (segmented). */ |
815 // code to send the initiate upload response. (cs = 2) |
825 /* code to send the initiate upload response. (cs = 2) */ |
816 sdo.body.data[0] = (2 << 5) | 1; |
826 sdo.body.data[0] = (2 << 5) | 1; |
817 sdo.body.data[1] = index & 0xFF; // LSB |
827 sdo.body.data[1] = index & 0xFF; /* LSB */ |
818 sdo.body.data[2] = (index >> 8) & 0xFF; // MSB |
828 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ |
819 sdo.body.data[3] = subIndex; |
829 sdo.body.data[3] = subIndex; |
820 sdo.body.data[4] = nbBytes; // Limitation of canfestival2 : Max tranfert is 256 bytes. |
830 sdo.body.data[4] = nbBytes; /* Limitation of canfestival2 : Max tranfert is 256 bytes.*/ |
821 // It takes too much memory to upgrate to 2^32 because the size of data is also coded |
831 /* It takes too much memory to upgrate to 2^32 because the size of data is also coded */ |
822 // in the object dictionary, at every index and subindex. |
832 /* in the object dictionary, at every index and subindex. */ |
823 for (i = 5 ; i < 8 ; i++) |
833 for (i = 5 ; i < 8 ; i++) |
824 sdo.body.data[i] = 0; |
834 sdo.body.data[i] = 0; |
825 MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId); |
835 MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId); |
826 sendSDO(d, whoami, sdo); |
836 sendSDO(d, whoami, sdo); |
827 } |
837 } |
828 else { |
838 else { |
829 // Expedited upload. (cs = 2 ; e = 1) |
839 /* Expedited upload. (cs = 2 ; e = 1) */ |
830 sdo.body.data[0] = (2 << 5) | ((4 - nbBytes) << 2) | 3; |
840 sdo.body.data[0] = (2 << 5) | ((4 - nbBytes) << 2) | 3; |
831 sdo.body.data[1] = index & 0xFF; // LSB |
841 sdo.body.data[1] = index & 0xFF; /* LSB */ |
832 sdo.body.data[2] = (index >> 8) & 0xFF; // MSB |
842 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ |
833 sdo.body.data[3] = subIndex; |
843 sdo.body.data[3] = subIndex; |
834 err = lineToSDO(d, line, nbBytes, sdo.body.data + 4); |
844 err = lineToSDO(d, line, nbBytes, sdo.body.data + 4); |
835 if (err) { |
845 if (err) { |
836 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
846 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
837 return 0xFF; |
847 return 0xFF; |
839 for (i = 4 + nbBytes ; i < 8 ; i++) |
849 for (i = 4 + nbBytes ; i < 8 ; i++) |
840 sdo.body.data[i] = 0; |
850 sdo.body.data[i] = 0; |
841 MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ", |
851 MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ", |
842 nodeId); |
852 nodeId); |
843 sendSDO(d, whoami, sdo); |
853 sendSDO(d, whoami, sdo); |
844 // Release the line. |
854 /* Release the line.*/ |
845 resetSDOline(d, line); |
855 resetSDOline(d, line); |
846 } |
856 } |
847 } // end if I am SERVER |
857 } /* end if I am SERVER*/ |
848 else { |
858 else { |
849 // I am CLIENT |
859 /* I am CLIENT */ |
850 // It is the response for the previous initiate upload request. |
860 /* It is the response for the previous initiate upload request.*/ |
851 // We should find a line opened for this. |
861 /* We should find a line opened for this. */ |
852 err = getSDOlineOnUse( d, nodeId, whoami, &line); |
862 err = getSDOlineOnUse( d, nodeId, whoami, &line); |
853 if (!err) |
863 if (!err) |
854 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
864 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
855 if (err) { |
865 if (err) { |
856 MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId); |
866 MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId); |
857 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
867 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
858 return 0xFF; |
868 return 0xFF; |
859 } |
869 } |
860 // Reset the wathdog |
870 /* Reset the wathdog */ |
861 RestartSDO_TIMER(line) |
871 RestartSDO_TIMER(line) |
862 index = d->transfers[line].index; |
872 index = d->transfers[line].index; |
863 subIndex = d->transfers[line].subIndex; |
873 subIndex = d->transfers[line].subIndex; |
864 |
874 |
865 if (getSDOe(m->data[0])) { // If SDO expedited |
875 if (getSDOe(m->data[0])) { /* If SDO expedited */ |
866 // nb of data to be uploaded |
876 /* nb of data to be uploaded */ |
867 nbBytes = 4 - getSDOn2(m->data[0]); |
877 nbBytes = 4 - getSDOn2(m->data[0]); |
868 // Storing the data in the line structure. |
878 /* Storing the data in the line structure. */ |
869 err = SDOtoLine(d, line, nbBytes, (*m).data + 4); |
879 err = SDOtoLine(d, line, nbBytes, (*m).data + 4); |
870 if (err) { |
880 if (err) { |
871 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
881 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
872 return 0xFF; |
882 return 0xFF; |
873 } |
883 } |
874 // SDO expedited -> transfert finished. data are available via getReadResultNetworkDict(). |
884 /* SDO expedited -> transfert finished. data are available via getReadResultNetworkDict(). */ |
875 MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId); |
885 MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId); |
876 StopSDO_TIMER(line) |
886 StopSDO_TIMER(line) |
877 d->transfers[line].count = nbBytes; |
887 d->transfers[line].count = nbBytes; |
878 d->transfers[line].state = SDO_FINISHED; |
888 d->transfers[line].state = SDO_FINISHED; |
879 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
889 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
880 return 0; |
890 return 0; |
881 } |
891 } |
882 else { // So, if it is not an expedited transfert |
892 else { /* So, if it is not an expedited transfert */ |
883 // Storing the nb of data to receive. |
893 /* Storing the nb of data to receive. */ |
884 if (getSDOs(m->data[0])) { |
894 if (getSDOs(m->data[0])) { |
885 nbBytes = m->data[4]; // Remember the limitation to 255 bytes to transfert |
895 nbBytes = m->data[4]; /* Remember the limitation to 255 bytes to transfert */ |
886 err = setSDOlineRestBytes(d, line, nbBytes); |
896 err = setSDOlineRestBytes(d, line, nbBytes); |
887 if (err) { |
897 if (err) { |
888 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
898 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
889 return 0xFF; |
899 return 0xFF; |
890 } |
900 } |
891 } |
901 } |
892 // Requesting next segment. (cs = 3) |
902 /* Requesting next segment. (cs = 3) */ |
893 sdo.nodeId = nodeId; |
903 sdo.nodeId = nodeId; |
894 sdo.body.data[0] = 3 << 5; |
904 sdo.body.data[0] = 3 << 5; |
895 for (i = 1 ; i < 8 ; i++) |
905 for (i = 1 ; i < 8 ; i++) |
896 sdo.body.data[i] = 0; |
906 sdo.body.data[i] = 0; |
897 MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId); |
907 MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId); |
898 sendSDO(d, whoami, sdo); |
908 sendSDO(d, whoami, sdo); |
899 } |
909 } |
900 } // End if CLIENT |
910 } /* End if CLIENT */ |
901 break; |
911 break; |
902 |
912 |
903 case 3: |
913 case 3: |
904 // I am SERVER |
914 /* I am SERVER */ |
905 if (whoami == SDO_SERVER) { |
915 if (whoami == SDO_SERVER) { |
906 // Receiving a upload segment. |
916 /* Receiving a upload segment. */ |
907 // A SDO transfert should have been yet initiated. |
917 /* A SDO transfert should have been yet initiated. */ |
908 err = getSDOlineOnUse( d, nodeId, whoami, &line ); |
918 err = getSDOlineOnUse( d, nodeId, whoami, &line ); |
909 if (!err) |
919 if (!err) |
910 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
920 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
911 if (err) { |
921 if (err) { |
912 MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", |
922 MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", |
913 nodeId); |
923 nodeId); |
914 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
924 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
915 return 0xFF; |
925 return 0xFF; |
916 } |
926 } |
917 // Reset the wathdog |
927 /* Reset the wathdog */ |
918 RestartSDO_TIMER(line) |
928 RestartSDO_TIMER(line) |
919 MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", nodeId); |
929 MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", nodeId); |
920 index = d->transfers[line].index; |
930 index = d->transfers[line].index; |
921 subIndex = d->transfers[line].subIndex; |
931 subIndex = d->transfers[line].subIndex; |
922 // Toggle test. |
932 /* Toggle test.*/ |
923 if (d->transfers[line].toggle != getSDOt(m->data[0])) { |
933 if (d->transfers[line].toggle != getSDOt(m->data[0])) { |
924 MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0])); |
934 MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0])); |
925 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); |
935 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); |
926 return 0xFF; |
936 return 0xFF; |
927 } |
937 } |
928 // Uploading next segment. We need to know if it will be the last one. |
938 /* Uploading next segment. We need to know if it will be the last one. */ |
929 getSDOlineRestBytes(d, line, &nbBytes); |
939 getSDOlineRestBytes(d, line, &nbBytes); |
930 if (nbBytes > 7) { |
940 if (nbBytes > 7) { |
931 // The segment to transfer is not the last one. |
941 /* The segment to transfer is not the last one.*/ |
932 // code to send the next segment. (cs = 0; c = 0) |
942 /* code to send the next segment. (cs = 0; c = 0) */ |
933 sdo.nodeId = nodeId; // The server node Id; |
943 sdo.nodeId = nodeId; /* The server node Id; */ |
934 sdo.body.data[0] = (d->transfers[line].toggle << 4); |
944 sdo.body.data[0] = (d->transfers[line].toggle << 4); |
935 err = lineToSDO(d, line, 7, sdo.body.data + 1); |
945 err = lineToSDO(d, line, 7, sdo.body.data + 1); |
936 if (err) { |
946 if (err) { |
937 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
947 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
938 return 0xFF; |
948 return 0xFF; |
939 } |
949 } |
940 // Inverting the toggle for the next tranfert. |
950 /* Inverting the toggle for the next tranfert. */ |
941 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
951 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
942 MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", nodeId); |
952 MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", nodeId); |
943 sendSDO(d, whoami, sdo); |
953 sendSDO(d, whoami, sdo); |
944 } |
954 } |
945 else { |
955 else { |
946 // Last segment. |
956 /* Last segment. */ |
947 // code to send the last segment. (cs = 0; c = 1) |
957 /* code to send the last segment. (cs = 0; c = 1) */ |
948 sdo.nodeId = nodeId; // The server node Id; |
958 sdo.nodeId = nodeId; /* The server node Id; */ |
949 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1; |
959 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1; |
950 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); |
960 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); |
951 if (err) { |
961 if (err) { |
952 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
962 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
953 return 0xFF; |
963 return 0xFF; |
954 } |
964 } |
955 for (i = nbBytes + 1 ; i < 8 ; i++) |
965 for (i = nbBytes + 1 ; i < 8 ; i++) |
956 sdo.body.data[i] = 0; |
966 sdo.body.data[i] = 0; |
957 MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", nodeId); |
967 MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", nodeId); |
958 sendSDO(d, whoami, sdo); |
968 sendSDO(d, whoami, sdo); |
959 // Release the line |
969 /* Release the line */ |
960 resetSDOline(d, line); |
970 resetSDOline(d, line); |
961 } |
971 } |
962 } // end if SERVER |
972 } /* end if SERVER*/ |
963 else { |
973 else { |
964 // I am CLIENT |
974 /* I am CLIENT */ |
965 // It is the response for the previous initiate download request. |
975 /* It is the response for the previous initiate download request. */ |
966 // We should find a line opened for this. |
976 /* We should find a line opened for this. */ |
967 err = getSDOlineOnUse( d, nodeId, whoami, &line); |
977 err = getSDOlineOnUse( d, nodeId, whoami, &line); |
968 if (!err) |
978 if (!err) |
969 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
979 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
970 if (err) { |
980 if (err) { |
971 MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId); |
981 MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId); |
972 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
982 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
973 return 0xFF; |
983 return 0xFF; |
974 } |
984 } |
975 // Reset the wathdog |
985 /* Reset the wathdog */ |
976 RestartSDO_TIMER(line) |
986 RestartSDO_TIMER(line) |
977 index = d->transfers[line].index; |
987 index = d->transfers[line].index; |
978 subIndex = d->transfers[line].subIndex; |
988 subIndex = d->transfers[line].subIndex; |
979 // End transmission or requesting next segment. |
989 /* End transmission or requesting next segment. */ |
980 getSDOlineRestBytes(d, line, &nbBytes); |
990 getSDOlineRestBytes(d, line, &nbBytes); |
981 if (nbBytes == 0) { |
991 if (nbBytes == 0) { |
982 MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId); |
992 MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId); |
983 StopSDO_TIMER(line) |
993 StopSDO_TIMER(line) |
984 d->transfers[line].state = SDO_FINISHED; |
994 d->transfers[line].state = SDO_FINISHED; |
985 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
995 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
986 return 0x00; |
996 return 0x00; |
987 } |
997 } |
988 if (nbBytes > 7) { |
998 if (nbBytes > 7) { |
989 // more than one request to send |
999 /* more than one request to send */ |
990 // code to send the next segment. (cs = 0; c = 0) |
1000 /* code to send the next segment. (cs = 0; c = 0) */ |
991 sdo.nodeId = nodeId; // The server node Id; |
1001 sdo.nodeId = nodeId; /* The server node Id; */ |
992 sdo.body.data[0] = (d->transfers[line].toggle << 4); |
1002 sdo.body.data[0] = (d->transfers[line].toggle << 4); |
993 err = lineToSDO(d, line, 7, sdo.body.data + 1); |
1003 err = lineToSDO(d, line, 7, sdo.body.data + 1); |
994 if (err) { |
1004 if (err) { |
995 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
1005 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
996 return 0xFF; |
1006 return 0xFF; |
997 } |
1007 } |
998 } |
1008 } |
999 else { |
1009 else { |
1000 // Last segment. |
1010 /* Last segment.*/ |
1001 // code to send the last segment. (cs = 0; c = 1) |
1011 /* code to send the last segment. (cs = 0; c = 1) */ |
1002 sdo.nodeId = nodeId; // The server node Id; |
1012 sdo.nodeId = nodeId; /* The server node Id; */ |
1003 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1; |
1013 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1; |
1004 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); |
1014 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); |
1005 if (err) { |
1015 if (err) { |
1006 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
1016 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
1007 return 0xFF; |
1017 return 0xFF; |