| changeset 664 | a03f0aa7d219 |
| parent 663 | 70fc3603e36f |
| child 665 | 90e6cf84a0d7 |
| 663:70fc3603e36f | 664:a03f0aa7d219 |
|---|---|
60 ** @param endianize |
60 ** @param endianize |
61 ** |
61 ** |
62 ** @return |
62 ** @return |
63 **/ |
63 **/ |
64 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, |
64 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, |
65 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize); |
65 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize, UNS8 useBlockMode); |
66 |
66 |
67 /*! |
67 /*! |
68 ** Called by readNetworkDict |
68 ** Called by readNetworkDict |
69 ** |
69 ** |
70 ** @param d |
70 ** @param d |
75 ** @param Callback |
75 ** @param Callback |
76 ** |
76 ** |
77 ** @return |
77 ** @return |
78 **/ |
78 **/ |
79 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, |
79 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, |
80 UNS8 dataType, SDOCallback_t Callback); |
80 UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode); |
81 |
81 |
82 |
82 |
83 /***************************************************************************/ |
83 /***************************************************************************/ |
84 /* SDO (un)packing macros */ |
84 /* SDO (un)packing macros */ |
85 |
85 |
116 #define getSDOindex(byte1, byte2) (((UNS16)byte2 << 8) | ((UNS16)byte1)) |
116 #define getSDOindex(byte1, byte2) (((UNS16)byte2 << 8) | ((UNS16)byte1)) |
117 |
117 |
118 /** Returns the subIndex from the byte 3 of the SDO |
118 /** Returns the subIndex from the byte 3 of the SDO |
119 */ |
119 */ |
120 #define getSDOsubIndex(byte3) (byte3) |
120 #define getSDOsubIndex(byte3) (byte3) |
121 |
|
122 /** Returns the subcommand in SDO block transfert |
|
123 */ |
|
124 #define getSDOblockSC(byte) (byte & 3) |
|
125 |
|
121 |
126 |
122 /*! |
127 /*! |
123 ** |
128 ** |
124 ** |
129 ** |
125 ** @param d |
130 ** @param d |
134 if ((offset == 0) || ((offset+d->transfers[id].CliServNbr) > d->lastIndex->SDO_CLT)) { |
139 if ((offset == 0) || ((offset+d->transfers[id].CliServNbr) > d->lastIndex->SDO_CLT)) { |
135 return ; |
140 return ; |
136 } |
141 } |
137 nodeId = *((UNS32*) d->objdict[offset+d->transfers[id].CliServNbr].pSubindex[3].pObject); |
142 nodeId = *((UNS32*) d->objdict[offset+d->transfers[id].CliServNbr].pSubindex[3].pObject); |
138 MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0); |
143 MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0); |
139 MSG_WAR(0x2A02, "server node : ", NodeId); |
144 MSG_WAR(0x2A02, "server node id : ", nodeId); |
140 MSG_WAR(0x2A02, " index : ", d->transfers[id].index); |
145 MSG_WAR(0x2A02, " index : ", d->transfers[id].index); |
141 MSG_WAR(0x2A02, " subIndex : ", d->transfers[id].subIndex); |
146 MSG_WAR(0x2A02, " subIndex : ", d->transfers[id].subIndex); |
142 /* Reset timer handler */ |
147 /* Reset timer handler */ |
143 d->transfers[id].timer = TIMER_NONE; |
148 d->transfers[id].timer = TIMER_NONE; |
144 /*Set aborted state*/ |
149 /*Set aborted state*/ |
145 d->transfers[id].state = SDO_ABORTED_INTERNAL; |
150 d->transfers[id].state = SDO_ABORTED_INTERNAL; |
146 /* Sending a SDO abort */ |
151 /* Sending a SDO abort */ |
400 MSG_WAR(0x3A21, "FailedSDO : line released : ", line); |
405 MSG_WAR(0x3A21, "FailedSDO : line released : ", line); |
401 } |
406 } |
402 if ((! err) && (whoami == SDO_CLIENT)) { |
407 if ((! err) && (whoami == SDO_CLIENT)) { |
403 StopSDO_TIMER(line); |
408 StopSDO_TIMER(line); |
404 d->transfers[line].state = SDO_ABORTED_INTERNAL; |
409 d->transfers[line].state = SDO_ABORTED_INTERNAL; |
410 d->transfers[line].abortCode = abortCode; |
|
405 } |
411 } |
406 MSG_WAR(0x3A22, "Sending SDO abort ", 0); |
412 MSG_WAR(0x3A22, "Sending SDO abort ", 0); |
407 err = sendSDOabort(d, whoami, CliServNbr, index, subIndex, abortCode); |
413 err = sendSDOabort(d, whoami, CliServNbr, index, subIndex, abortCode); |
408 if (err) { |
414 if (err) { |
409 MSG_WAR(0x3A23, "Unable to send the SDO abort", 0); |
415 MSG_WAR(0x3A23, "Unable to send the SDO abort", 0); |
425 initSDOline(d, line, 0, 0, 0, SDO_RESET); |
431 initSDOline(d, line, 0, 0, 0, SDO_RESET); |
426 for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++) |
432 for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++) |
427 d->transfers[line].data[i] = 0; |
433 d->transfers[line].data[i] = 0; |
428 d->transfers[line].whoami = 0; |
434 d->transfers[line].whoami = 0; |
429 d->transfers[line].abortCode = 0; |
435 d->transfers[line].abortCode = 0; |
430 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
|
431 free(d->transfers[line].dynamicData); |
|
432 d->transfers[line].dynamicData = 0; |
|
433 d->transfers[line].dynamicDataSize = 0; |
|
434 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
|
435 } |
436 } |
436 |
437 |
437 /*! |
438 /*! |
438 ** |
439 ** |
439 ** |
440 ** |
447 ** @return |
448 ** @return |
448 **/ |
449 **/ |
449 UNS8 initSDOline (CO_Data* d, UNS8 line, UNS8 CliServNbr, UNS16 index, UNS8 subIndex, UNS8 state) |
450 UNS8 initSDOline (CO_Data* d, UNS8 line, UNS8 CliServNbr, UNS16 index, UNS8 subIndex, UNS8 state) |
450 { |
451 { |
451 MSG_WAR(0x3A25, "init SDO line nb : ", line); |
452 MSG_WAR(0x3A25, "init SDO line nb : ", line); |
452 if (state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS){ |
453 if (state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS || |
454 state == SDO_BLOCK_DOWNLOAD_IN_PROGRESS || state == SDO_BLOCK_UPLOAD_IN_PROGRESS){ |
|
453 StartSDO_TIMER(line) |
455 StartSDO_TIMER(line) |
454 }else{ |
456 }else{ |
455 StopSDO_TIMER(line) |
457 StopSDO_TIMER(line) |
456 } |
458 } |
457 d->transfers[line].CliServNbr = CliServNbr; |
459 d->transfers[line].CliServNbr = CliServNbr; |
459 d->transfers[line].subIndex = subIndex; |
461 d->transfers[line].subIndex = subIndex; |
460 d->transfers[line].state = state; |
462 d->transfers[line].state = state; |
461 d->transfers[line].toggle = 0; |
463 d->transfers[line].toggle = 0; |
462 d->transfers[line].count = 0; |
464 d->transfers[line].count = 0; |
463 d->transfers[line].offset = 0; |
465 d->transfers[line].offset = 0; |
466 d->transfers[line].peerCRCsupport = 0; |
|
467 d->transfers[line].blksize = 0; |
|
468 d->transfers[line].ackseq = 0; |
|
469 d->transfers[line].objsize = 0; |
|
470 d->transfers[line].lastblockoffset = 0; |
|
471 d->transfers[line].seqno = 0; |
|
472 d->transfers[line].endfield = 0; |
|
473 d->transfers[line].rxstep = RXSTEP_INIT; |
|
464 d->transfers[line].dataType = 0; |
474 d->transfers[line].dataType = 0; |
465 d->transfers[line].Callback = NULL; |
475 d->transfers[line].Callback = NULL; |
466 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
476 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
467 free(d->transfers[line].dynamicData); |
477 free(d->transfers[line].dynamicData); |
468 d->transfers[line].dynamicData = 0; |
478 d->transfers[line].dynamicData = 0; |
641 if ((offset == 0) || ((offset+CliServNbr) > d->lastIndex->SDO_SVR)) { |
651 if ((offset == 0) || ((offset+CliServNbr) > d->lastIndex->SDO_SVR)) { |
642 MSG_ERR(0x1A42, "SendSDO : SDO server not found", 0); |
652 MSG_ERR(0x1A42, "SendSDO : SDO server not found", 0); |
643 return 0xFF; |
653 return 0xFF; |
644 } |
654 } |
645 m.cob_id = *((UNS32*) d->objdict[offset+CliServNbr].pSubindex[2].pObject); |
655 m.cob_id = *((UNS32*) d->objdict[offset+CliServNbr].pSubindex[2].pObject); |
646 MSG_WAR(0x3A41, "I am server cobId : ", m.cob_id); |
656 MSG_WAR(0x3A41, "I am server Tx cobId : ", m.cob_id); |
647 } |
657 } |
648 else { /*case client*/ |
658 else { /*case client*/ |
649 /* Get the client->server cobid.*/ |
659 /* Get the client->server cobid.*/ |
650 offset = d->firstIndex->SDO_CLT; |
660 offset = d->firstIndex->SDO_CLT; |
651 if ((offset == 0) || ((offset+CliServNbr) > d->lastIndex->SDO_CLT)) { |
661 if ((offset == 0) || ((offset+CliServNbr) > d->lastIndex->SDO_CLT)) { |
652 MSG_ERR(0x1A42, "SendSDO : SDO client not found", 0); |
662 MSG_ERR(0x1A42, "SendSDO : SDO client not found", 0); |
653 return 0xFF; |
663 return 0xFF; |
654 } |
664 } |
655 m.cob_id = *((UNS32*) d->objdict[offset+CliServNbr].pSubindex[1].pObject); |
665 m.cob_id = *((UNS32*) d->objdict[offset+CliServNbr].pSubindex[1].pObject); |
656 MSG_WAR(0x3A41, "I am client cobId : ", m.cob_id); |
666 MSG_WAR(0x3A41, "I am client Tx cobId : ", m.cob_id); |
657 } |
667 } |
658 /* message copy for sending */ |
668 /* message copy for sending */ |
659 m.rtr = NOT_A_REQUEST; |
669 m.rtr = NOT_A_REQUEST; |
660 /* the length of SDO must be 8 */ |
670 /* the length of SDO must be 8 */ |
661 m.len = 8; |
671 m.len = 8; |
707 ** @return |
717 ** @return |
708 **/ |
718 **/ |
709 UNS8 proceedSDO (CO_Data* d, Message *m) |
719 UNS8 proceedSDO (CO_Data* d, Message *m) |
710 { |
720 { |
711 UNS8 err; |
721 UNS8 err; |
722 UNS8 cs; |
|
712 UNS8 line; |
723 UNS8 line; |
713 UNS32 nbBytes; /* received or to be transmited. */ |
724 UNS32 nbBytes; /* received or to be transmited. */ |
714 UNS8 nodeId = 0; /* The node Id of the server if client otherwise unused */ |
725 UNS8 nodeId = 0; /* The node Id of the server if client otherwise unused */ |
715 UNS8 CliServNbr; |
726 UNS8 CliServNbr; |
716 UNS8 whoami = SDO_UNKNOWN; /* SDO_SERVER or SDO_CLIENT.*/ |
727 UNS8 whoami = SDO_UNKNOWN; /* SDO_SERVER or SDO_CLIENT.*/ |
722 UNS32 i; |
733 UNS32 i; |
723 UNS8 j; |
734 UNS8 j; |
724 UNS32 *pCobId = NULL; |
735 UNS32 *pCobId = NULL; |
725 UNS16 offset; |
736 UNS16 offset; |
726 UNS16 lastIndex; |
737 UNS16 lastIndex; |
738 UNS8 SubCommand; /* Block transfert only */ |
|
739 UNS8 SeqNo; /* Sequence number in block transfert */ |
|
740 UNS8 AckSeq; /* Sequence number of last segment that was received successfully */ |
|
741 UNS8 NbBytesNoData; /* Number of bytes that do not contain data in last segment of block transfert */ |
|
727 |
742 |
728 MSG_WAR(0x3A60, "proceedSDO ", 0); |
743 MSG_WAR(0x3A60, "proceedSDO ", 0); |
729 whoami = SDO_UNKNOWN; |
744 whoami = SDO_UNKNOWN; |
730 /* Looking for the cobId in the object dictionary. */ |
745 /* Looking for the cobId in the object dictionary. */ |
731 /* Am-I a server ? */ |
746 /* Am-I a server ? */ |
761 } |
776 } |
762 /* Looking for the cobid received. */ |
777 /* Looking for the cobid received. */ |
763 pCobId = (UNS32*) d->objdict[offset].pSubindex[2].pObject; |
778 pCobId = (UNS32*) d->objdict[offset].pSubindex[2].pObject; |
764 if (*pCobId == UNS16_LE(m->cob_id) ) { |
779 if (*pCobId == UNS16_LE(m->cob_id) ) { |
765 whoami = SDO_CLIENT; |
780 whoami = SDO_CLIENT; |
766 MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j); |
781 MSG_WAR(0x3A64, "proceedSDO. I am client index : ", 0x1280 + j); |
767 /* Defining Client number = index minus 0x1280 where the cobid received is defined. */ |
782 /* Defining Client number = index minus 0x1280 where the cobid received is defined. */ |
768 CliServNbr = j; |
783 CliServNbr = j; |
769 /* Reading the server node ID, if client it is mandatory in the OD */ |
784 /* Reading the server node ID, if client it is mandatory in the OD */ |
770 nodeId = *((UNS8*) d->objdict[offset].pSubindex[3].pObject); |
785 nodeId = *((UNS8*) d->objdict[offset].pSubindex[3].pObject); |
771 break; |
786 break; |
790 } |
805 } |
791 else { |
806 else { |
792 MSG_WAR(0x3A69, "I am SERVER number ", CliServNbr); |
807 MSG_WAR(0x3A69, "I am SERVER number ", CliServNbr); |
793 } |
808 } |
794 |
809 |
810 /* Look for an SDO transfert already initiated. */ |
|
811 err = getSDOlineOnUse( d, CliServNbr, whoami, &line ); |
|
812 |
|
813 /* Let's find cs value, first it is set as "not valid" */ |
|
814 cs = 0xFF; |
|
815 /* Special cases for block transfert : in frames with segment data cs is not spécified */ |
|
816 if (!err) { |
|
817 if ((whoami == SDO_SERVER) && (d->transfers[line].state == SDO_BLOCK_DOWNLOAD_IN_PROGRESS) || |
|
818 (whoami == SDO_CLIENT) && (d->transfers[line].state == SDO_BLOCK_UPLOAD_IN_PROGRESS)) { |
|
819 if(m->data[0] == 0x80) /* If first byte is 0x80 it is an abort frame (seqno = 0 not allowed) */ |
|
820 cs = 4; |
|
821 else |
|
822 cs = 6; |
|
823 } |
|
824 } |
|
825 /* Other cases : cs is specified */ |
|
826 if (cs == 0xFF) |
|
827 cs = getSDOcs(m->data[0]); |
|
828 |
|
795 /* Testing the command specifier */ |
829 /* Testing the command specifier */ |
796 /* Allowed : cs = 0, 1, 2, 3, 4. (= all except those for block tranfert). */ |
830 /* Allowed : cs = 0, 1, 2, 3, 4, 5, 6 */ |
797 /* cs = other : Not allowed -> abort. */ |
831 /* cs = other : Not allowed -> abort. */ |
798 switch (getSDOcs(m->data[0])) { |
832 switch (cs) { |
799 |
833 |
800 case 0: |
834 case 0: |
801 /* I am SERVER */ |
835 /* I am SERVER */ |
802 if (whoami == SDO_SERVER) { |
836 if (whoami == SDO_SERVER) { |
803 /* Receiving a download segment data. */ |
837 /* Receiving a download segment data : an SDO transfert should have been yet initiated. */ |
804 /* A SDO transfert should have been yet initiated. */ |
|
805 err = getSDOlineOnUse( d, CliServNbr, whoami, &line ); |
|
806 if (!err) |
838 if (!err) |
807 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
839 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
808 if (err) { |
840 if (err) { |
809 MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", |
841 MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", |
810 CliServNbr); |
842 CliServNbr); |
854 } |
886 } |
855 } /* end if SERVER */ |
887 } /* end if SERVER */ |
856 else { /* if CLIENT */ |
888 else { /* if CLIENT */ |
857 /* I am CLIENT */ |
889 /* I am CLIENT */ |
858 /* It is a request for a previous upload segment. We should find a line opened for this.*/ |
890 /* It is a request for a previous upload segment. We should find a line opened for this.*/ |
859 err = getSDOlineOnUse( d, CliServNbr, whoami, &line); |
|
860 if (!err) |
891 if (!err) |
861 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
892 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
862 if (err) { |
893 if (err) { |
863 MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); |
894 MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); |
864 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
895 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
915 CliServNbr); |
946 CliServNbr); |
916 MSG_WAR(0x3A80, "Writing at index : ", index); |
947 MSG_WAR(0x3A80, "Writing at index : ", index); |
917 MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex); |
948 MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex); |
918 |
949 |
919 /* Search if a SDO transfert have been yet initiated */ |
950 /* Search if a SDO transfert have been yet initiated */ |
920 err = getSDOlineOnUse( d, CliServNbr, whoami, &line ); |
|
921 if (! err) { |
951 if (! err) { |
922 MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0); |
952 MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0); |
923 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
953 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
924 return 0xFF; |
954 return 0xFF; |
925 } |
955 } |
978 sendSDO(d, whoami, CliServNbr, data); |
1008 sendSDO(d, whoami, CliServNbr, data); |
979 } /* end if I am SERVER */ |
1009 } /* end if I am SERVER */ |
980 else { |
1010 else { |
981 /* I am CLIENT */ |
1011 /* I am CLIENT */ |
982 /* It is a response for a previous download segment. We should find a line opened for this. */ |
1012 /* It is a response for a previous download segment. We should find a line opened for this. */ |
983 err = getSDOlineOnUse( d, CliServNbr, whoami, &line); |
|
984 if (!err) |
1013 if (!err) |
985 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
1014 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
986 if (err) { |
1015 if (err) { |
987 MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); |
1016 MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); |
988 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
1017 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
1047 MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", |
1076 MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", |
1048 CliServNbr); |
1077 CliServNbr); |
1049 MSG_WAR(0x3A90, "Reading at index : ", index); |
1078 MSG_WAR(0x3A90, "Reading at index : ", index); |
1050 MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex); |
1079 MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex); |
1051 /* Search if a SDO transfert have been yet initiated*/ |
1080 /* Search if a SDO transfert have been yet initiated*/ |
1052 err = getSDOlineOnUse( d, CliServNbr, whoami, &line ); |
|
1053 if (! err) { |
1081 if (! err) { |
1054 MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line); |
1082 MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line); |
1055 MSG_WAR(0x3A93, "Server Nbr = ", CliServNbr); |
1083 MSG_WAR(0x3A93, "Server Nbr = ", CliServNbr); |
1056 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
1084 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
1057 return 0xFF; |
1085 return 0xFF; |
1081 /* code to send the initiate upload response. (cs = 2) */ |
1109 /* code to send the initiate upload response. (cs = 2) */ |
1082 data[0] = (2 << 5) | 1; |
1110 data[0] = (2 << 5) | 1; |
1083 data[1] = index & 0xFF; /* LSB */ |
1111 data[1] = index & 0xFF; /* LSB */ |
1084 data[2] = (index >> 8) & 0xFF; /* MSB */ |
1112 data[2] = (index >> 8) & 0xFF; /* MSB */ |
1085 data[3] = subIndex; |
1113 data[3] = subIndex; |
1086 data[4] = (UNS8)nbBytes; /* Limitation of canfestival2 : Max tranfert is 256 bytes.*/ |
1114 data[4] = nbBytes; |
1087 /* It takes too much memory to upgrate to 2^32 because the size of data is also coded */ |
1115 data[5] = nbBytes >> 8; |
1088 /* in the object dictionary, at every index and subindex. */ |
1116 data[6] = nbBytes >> 16; |
1089 for (i = 5 ; i < 8 ; i++) |
1117 data[7] = nbBytes >> 24; |
1090 data[i] = 0; |
1118 MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId); |
1091 MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId); |
|
1092 sendSDO(d, whoami, CliServNbr, data); |
1119 sendSDO(d, whoami, CliServNbr, data); |
1093 } |
1120 } |
1094 else { |
1121 else { |
1095 /* Expedited upload. (cs = 2 ; e = 1) */ |
1122 /* Expedited upload. (cs = 2 ; e = 1) */ |
1096 data[0] = (UNS8)((2 << 5) | ((4 - nbBytes) << 2) | 3); |
1123 data[0] = (UNS8)((2 << 5) | ((4 - nbBytes) << 2) | 3); |
1113 } /* end if I am SERVER*/ |
1140 } /* end if I am SERVER*/ |
1114 else { |
1141 else { |
1115 /* I am CLIENT */ |
1142 /* I am CLIENT */ |
1116 /* It is the response for the previous initiate upload request.*/ |
1143 /* It is the response for the previous initiate upload request.*/ |
1117 /* We should find a line opened for this. */ |
1144 /* We should find a line opened for this. */ |
1118 err = getSDOlineOnUse( d, CliServNbr, whoami, &line); |
|
1119 if (!err) |
1145 if (!err) |
1120 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
1146 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
1121 if (err) { |
1147 if (err) { |
1122 MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId); |
1148 MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId); |
1123 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
1149 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
1168 case 3: |
1194 case 3: |
1169 /* I am SERVER */ |
1195 /* I am SERVER */ |
1170 if (whoami == SDO_SERVER) { |
1196 if (whoami == SDO_SERVER) { |
1171 /* Receiving a upload segment. */ |
1197 /* Receiving a upload segment. */ |
1172 /* A SDO transfert should have been yet initiated. */ |
1198 /* A SDO transfert should have been yet initiated. */ |
1173 err = getSDOlineOnUse( d, CliServNbr, whoami, &line ); |
|
1174 if (!err) |
1199 if (!err) |
1175 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
1200 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
1176 if (err) { |
1201 if (err) { |
1177 MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", |
1202 MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", |
1178 CliServNbr); |
1203 CliServNbr); |
1225 } /* end if SERVER*/ |
1250 } /* end if SERVER*/ |
1226 else { |
1251 else { |
1227 /* I am CLIENT */ |
1252 /* I am CLIENT */ |
1228 /* It is the response for the previous initiate download request. */ |
1253 /* It is the response for the previous initiate download request. */ |
1229 /* We should find a line opened for this. */ |
1254 /* We should find a line opened for this. */ |
1230 err = getSDOlineOnUse( d, CliServNbr, whoami, &line); |
|
1231 if (!err) |
1255 if (!err) |
1232 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
1256 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
1233 if (err) { |
1257 if (err) { |
1234 MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId); |
1258 MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId); |
1235 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
1259 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
1281 (UNS32)m->data[4] | |
1305 (UNS32)m->data[4] | |
1282 ((UNS32)m->data[5] << 8) | |
1306 ((UNS32)m->data[5] << 8) | |
1283 ((UNS32)m->data[6] << 16) | |
1307 ((UNS32)m->data[6] << 16) | |
1284 ((UNS32)m->data[7] << 24); |
1308 ((UNS32)m->data[7] << 24); |
1285 /* Received SDO abort. */ |
1309 /* Received SDO abort. */ |
1286 /* Looking for the line concerned. */ |
|
1287 if (whoami == SDO_SERVER) { |
1310 if (whoami == SDO_SERVER) { |
1288 err = getSDOlineOnUse( d, CliServNbr, whoami, &line ); |
|
1289 if (!err) { |
1311 if (!err) { |
1290 resetSDOline( d, line ); |
1312 resetSDOline( d, line ); |
1291 MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode); |
1313 MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode); |
1292 } |
1314 } |
1293 else |
1315 else |
1294 MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode); |
1316 MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode); |
1295 /* Tips : The end user has no way to know that the server node has received an abort SDO. */ |
1317 /* Tips : The end user has no way to know that the server node has received an abort SDO. */ |
1296 /* Its is ok, I think.*/ |
1318 /* Its is ok, I think.*/ |
1297 } |
1319 } |
1298 else { /* If I am CLIENT */ |
1320 else { /* If I am CLIENT */ |
1299 err = getSDOlineOnUse( d, CliServNbr, whoami, &line ); |
|
1300 if (!err) { |
1321 if (!err) { |
1301 /* The line *must* be released by the core program. */ |
1322 /* The line *must* be released by the core program. */ |
1302 StopSDO_TIMER(line) |
1323 StopSDO_TIMER(line) |
1303 d->transfers[line].state = SDO_ABORTED_RCV; |
1324 d->transfers[line].state = SDO_ABORTED_RCV; |
1304 d->transfers[line].abortCode = abortCode; |
1325 d->transfers[line].abortCode = abortCode; |
1307 } |
1328 } |
1308 else |
1329 else |
1309 MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode); |
1330 MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode); |
1310 } |
1331 } |
1311 break; |
1332 break; |
1333 case 5: /* Command specifier for data transmission - the client or server is the data producer */ |
|
1334 SubCommand = getSDOblockSC(m->data[0]); |
|
1335 if (whoami == SDO_SERVER) { /* Server block upload */ |
|
1336 if (SubCommand == SDO_BCS_INITIATE_UPLOAD_REQUEST) { |
|
1337 index = getSDOindex(m->data[1],m->data[2]); |
|
1338 subIndex = getSDOsubIndex(m->data[3]); |
|
1339 MSG_WAR(0x3AB2, "Received SDO Initiate block upload defined at index 0x1200 + ", |
|
1340 CliServNbr); |
|
1341 MSG_WAR(0x3AB3, "Reading at index : ", index); |
|
1342 MSG_WAR(0x3AB4, "Reading at subIndex : ", subIndex); |
|
1343 /* Search if a SDO transfert have been yet initiated */ |
|
1344 if (! err) { |
|
1345 MSG_ERR(0x1A93, "SDO error : Transmission yet started at line : ", line); |
|
1346 MSG_WAR(0x3AB5, "Server Nbr = ", CliServNbr); |
|
1347 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
|
1348 return 0xFF; |
|
1349 } |
|
1350 /* No line on use. Great !*/ |
|
1351 /* Try to open a new line.*/ |
|
1352 err = getSDOfreeLine( d, whoami, &line ); |
|
1353 if (err) { |
|
1354 MSG_ERR(0x1A73, "SDO error : No line free, too many SDO in progress. Aborted.", 0); |
|
1355 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
|
1356 return 0xFF; |
|
1357 } |
|
1358 initSDOline(d, line, CliServNbr, index, subIndex, SDO_BLOCK_UPLOAD_IN_PROGRESS); |
|
1359 d->transfers[line].peerCRCsupport = ((m->data[0])>>2) & 1; |
|
1360 d->transfers[line].blksize = m->data[4]; |
|
1361 /* Transfer data from dictionary to the line structure. */ |
|
1362 errorCode = objdictToSDOline(d, line); |
|
1363 if (errorCode) { |
|
1364 MSG_ERR(0x1A95, "SDO error : Unable to copy the data from object dictionary. Err code : ", |
|
1365 errorCode); |
|
1366 failedSDO(d, CliServNbr, whoami, index, subIndex, errorCode); |
|
1367 return 0xFF; |
|
1368 } |
|
1369 /* Preparing the response.*/ |
|
1370 getSDOlineRestBytes(d, line, &nbBytes); /* get Nb bytes to transfer */ |
|
1371 d->transfers[line].objsize = nbBytes; |
|
1372 data[0] = (6 << 5) | (1 << 1) | SDO_BSS_INITIATE_UPLOAD_RESPONSE; |
|
1373 data[1] = index & 0xFF; /* LSB */ |
|
1374 data[2] = (index >> 8) & 0xFF; /* MSB */ |
|
1375 data[3] = subIndex; |
|
1376 data[4] = nbBytes; |
|
1377 data[5] = nbBytes >> 8; |
|
1378 data[6] = nbBytes >> 16; |
|
1379 data[7] = nbBytes >> 24; |
|
1380 MSG_WAR(0x3A9A, "SDO. Sending normal block upload initiate response defined at index 0x1200 + ", nodeId); |
|
1381 sendSDO(d, whoami, CliServNbr, data); |
|
1382 } |
|
1383 else if (SubCommand == SDO_BCS_END_UPLOAD_REQUEST) { |
|
1384 MSG_WAR(0x3AA2, "Received SDO block END upload request defined at index 0x1200 + ", CliServNbr); |
|
1385 /* A SDO transfert should have been yet initiated. */ |
|
1386 if (!err) |
|
1387 err = d->transfers[line].state != SDO_BLOCK_UPLOAD_IN_PROGRESS; |
|
1388 if (err) { |
|
1389 MSG_ERR(0x1AA1, "SDO error : Received block upload request for unstarted trans. index 0x1200 + ", |
|
1390 CliServNbr); |
|
1391 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
|
1392 return 0xFF; |
|
1393 } |
|
1394 /* Release the line */ |
|
1395 resetSDOline(d, line); |
|
1396 } |
|
1397 else if ((SubCommand == SDO_BCS_UPLOAD_RESPONSE) || (SubCommand == SDO_BCS_START_UPLOAD)) { |
|
1398 /* A SDO transfert should have been yet initiated. */ |
|
1399 if (!err) |
|
1400 err = d->transfers[line].state != SDO_BLOCK_UPLOAD_IN_PROGRESS; |
|
1401 if (err) { |
|
1402 MSG_ERR(0x1AA1, "SDO error : Received block upload response for unstarted trans. index 0x1200 + ", |
|
1403 CliServNbr); |
|
1404 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
|
1405 return 0xFF; |
|
1406 } |
|
1407 /* Reset the wathdog */ |
|
1408 RestartSDO_TIMER(line); |
|
1409 /* Uploading first or next block */ |
|
1410 index = d->transfers[line].index; |
|
1411 subIndex = d->transfers[line].subIndex; |
|
1412 if (SubCommand == SDO_BCS_UPLOAD_RESPONSE) { |
|
1413 MSG_WAR(0x3AA2, "Received SDO block upload response defined at index 0x1200 + ", CliServNbr); |
|
1414 d->transfers[line].blksize = m->data[2]; |
|
1415 AckSeq = (m->data[1]) & 0x7f; |
|
1416 getSDOlineRestBytes(d, line, &nbBytes); |
|
1417 if((nbBytes == 0) && (AckSeq == d->transfers[line].seqno)){ /* Si tout est envoyé et confirmé reçu on envoi un block end upload response */ |
|
1418 data[0] = (6 << 5) | ((d->transfers[line].endfield) << 2) | SDO_BSS_END_UPLOAD_RESPONSE; |
|
1419 for (i = 1 ; i < 8 ; i++) |
|
1420 data[i] = 0; |
|
1421 MSG_WAR(0x3AA5, "SDO. Sending block END upload response defined at index 0x1200 + ", CliServNbr); |
|
1422 sendSDO(d, whoami, CliServNbr, data); |
|
1423 break; |
|
1424 } |
|
1425 else |
|
1426 d->transfers[line].offset = d->transfers[line].lastblockoffset + 7 * AckSeq; |
|
1427 if(d->transfers[line].offset > d->transfers[line].count) { /* Bad AckSeq reveived (too high) */ |
|
1428 MSG_ERR(0x1AA1, "SDO error : Received upload response with bad ackseq index 0x1200 + ", |
|
1429 CliServNbr); |
|
1430 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
|
1431 return 0xFF; |
|
1432 } |
|
1433 } |
|
1434 else |
|
1435 MSG_WAR(0x3AA2, "Received SDO block START upload defined at index 0x1200 + ", CliServNbr); |
|
1436 d->transfers[line].lastblockoffset = d->transfers[line].offset; |
|
1437 for(SeqNo = 1 ; SeqNo <= d->transfers[line].blksize ; SeqNo++) { |
|
1438 d->transfers[line].seqno = SeqNo; |
|
1439 getSDOlineRestBytes(d, line, &nbBytes); |
|
1440 if (nbBytes > 7) { |
|
1441 /* The segment to transfer is not the last one.*/ |
|
1442 data[0] = SeqNo; |
|
1443 err = lineToSDO(d, line, 7, data + 1); |
|
1444 if (err) { |
|
1445 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1446 return 0xFF; |
|
1447 } |
|
1448 MSG_WAR(0x3AA5, "SDO. Sending upload segment defined at index 0x1200 + ", CliServNbr); |
|
1449 sendSDO(d, whoami, CliServNbr, data); |
|
1450 } |
|
1451 else { |
|
1452 /* Last segment is in this block */ |
|
1453 data[0] = 0x80 | SeqNo; |
|
1454 err = lineToSDO(d, line, nbBytes, data + 1); |
|
1455 if (err) { |
|
1456 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1457 return 0xFF; |
|
1458 } |
|
1459 for (i = nbBytes + 1 ; i < 8 ; i++) |
|
1460 data[i] = 0; |
|
1461 MSG_WAR(0x3AA5, "SDO. Sending last upload segment defined at index 0x1200 + ", CliServNbr); |
|
1462 sendSDO(d, whoami, CliServNbr, data); |
|
1463 d->transfers[line].endfield = 7 - nbBytes; |
|
1464 break; |
|
1465 } |
|
1466 } |
|
1467 } |
|
1468 } /* end if SERVER */ |
|
1469 else { /* if CLIENT (block download) */ |
|
1470 if ((SubCommand == SDO_BSS_INITIATE_DOWNLOAD_RESPONSE) || (SubCommand == SDO_BSS_DOWNLOAD_RESPONSE)) { |
|
1471 /* We should find a line opened for this. */ |
|
1472 if (!err) |
|
1473 err = d->transfers[line].state != SDO_BLOCK_DOWNLOAD_IN_PROGRESS; |
|
1474 if (err) { |
|
1475 MSG_ERR(0x1AAA, "SDO error : Received response for unknown block download request from node id", nodeId); |
|
1476 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
|
1477 return 0xFF; |
|
1478 } |
|
1479 /* Reset the watchdog */ |
|
1480 RestartSDO_TIMER(line) |
|
1481 if (SubCommand == SDO_BSS_INITIATE_DOWNLOAD_RESPONSE) { |
|
1482 index = d->transfers[line].index; |
|
1483 subIndex = d->transfers[line].subIndex; |
|
1484 d->transfers[line].peerCRCsupport = ((m->data[0])>>2) & 1; |
|
1485 d->transfers[line].blksize = m->data[4]; |
|
1486 } |
|
1487 else { |
|
1488 d->transfers[line].blksize = m->data[2]; |
|
1489 AckSeq = (m->data[1]) & 0x7f; |
|
1490 getSDOlineRestBytes(d, line, &nbBytes); |
|
1491 if((nbBytes == 0) && (AckSeq == d->transfers[line].seqno)){ /* Si tout est envoyé et confirmé reçu on envoi un block end download request */ |
|
1492 data[0] = (6 << 5) | ((d->transfers[line].endfield) << 2) | SDO_BCS_END_DOWNLOAD_REQUEST; |
|
1493 for (i = 1 ; i < 8 ; i++) |
|
1494 data[i] = 0; |
|
1495 MSG_WAR(0x3AA5, "SDO. Sending block END download request defined at index 0x1200 + ", CliServNbr); |
|
1496 sendSDO(d, whoami, CliServNbr, data); |
|
1497 break; |
|
1498 } |
|
1499 else |
|
1500 d->transfers[line].offset = d->transfers[line].lastblockoffset + 7 * AckSeq; |
|
1501 if(d->transfers[line].offset > d->transfers[line].count) { /* Bad AckSeq reveived (too high) */ |
|
1502 MSG_ERR(0x1AA1, "SDO error : Received upload segment with bad ackseq index 0x1200 + ", |
|
1503 CliServNbr); |
|
1504 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
|
1505 return 0xFF; |
|
1506 } |
|
1507 } |
|
1508 d->transfers[line].lastblockoffset = d->transfers[line].offset; |
|
1509 for(SeqNo = 1 ; SeqNo <= d->transfers[line].blksize ; SeqNo++) { |
|
1510 d->transfers[line].seqno = SeqNo; |
|
1511 getSDOlineRestBytes(d, line, &nbBytes); |
|
1512 if (nbBytes > 7) { |
|
1513 /* The segment to transfer is not the last one.*/ |
|
1514 data[0] = SeqNo; |
|
1515 err = lineToSDO(d, line, 7, data + 1); |
|
1516 if (err) { |
|
1517 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1518 return 0xFF; |
|
1519 } |
|
1520 MSG_WAR(0x3AAB, "SDO. Sending download segment to node id ", nodeId); |
|
1521 sendSDO(d, whoami, CliServNbr, data); |
|
1522 } |
|
1523 else { |
|
1524 /* Last segment is in this block */ |
|
1525 data[0] = 0x80 | SeqNo; |
|
1526 err = lineToSDO(d, line, nbBytes, data + 1); |
|
1527 if (err) { |
|
1528 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
|
1529 return 0xFF; |
|
1530 } |
|
1531 for (i = nbBytes + 1 ; i < 8 ; i++) |
|
1532 data[i] = 0; |
|
1533 MSG_WAR(0x3AAB, "SDO. Sending last download segment to node id ", nodeId); |
|
1534 sendSDO(d, whoami, CliServNbr, data); |
|
1535 d->transfers[line].endfield = 7 - nbBytes; |
|
1536 break; |
|
1537 } |
|
1538 } |
|
1539 } |
|
1540 else if (SubCommand == SDO_BSS_END_DOWNLOAD_RESPONSE) { |
|
1541 MSG_WAR(0x3AAC, "SDO End block download response from nodeId", nodeId); |
|
1542 StopSDO_TIMER(line) |
|
1543 d->transfers[line].state = SDO_FINISHED; |
|
1544 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
|
1545 return 0x00; |
|
1546 } |
|
1547 else { |
|
1548 MSG_ERR(0x1AAB, "SDO error block download : Received wrong subcommand from nodeId", nodeId); |
|
1549 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
|
1550 return 0xFF; |
|
1551 } |
|
1552 } /* end if CLIENT */ |
|
1553 break; |
|
1554 case 6: /* Command specifier for data reception - the client or server is the data consumer */ |
|
1555 if (whoami == SDO_SERVER) { /* Server block download */ |
|
1556 if (err) { |
|
1557 /* Nothing already started */ |
|
1558 SubCommand = (m->data[0]) & 1; |
|
1559 if (SubCommand != SDO_BCS_INITIATE_DOWNLOAD_REQUEST) { |
|
1560 MSG_ERR(0x1AAC, "SDO error block download : Received wrong subcommand from node id", nodeId); |
|
1561 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
|
1562 return 0xFF; |
|
1563 } |
|
1564 index = getSDOindex(m->data[1],m->data[2]); |
|
1565 subIndex = getSDOsubIndex(m->data[3]); |
|
1566 MSG_WAR(0x3A9B, "Received SDO block download initiate defined at index 0x1200 + ", |
|
1567 CliServNbr); |
|
1568 MSG_WAR(0x3A9B, "Writing at index : ", index); |
|
1569 MSG_WAR(0x3A9B, "Writing at subIndex : ", subIndex); |
|
1570 /* Try to open a new line. */ |
|
1571 err = getSDOfreeLine( d, whoami, &line ); |
|
1572 if (err) { |
|
1573 MSG_ERR(0x1A89, "SDO error : No line free, too many SDO in progress. Aborted.", 0); |
|
1574 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
|
1575 return 0xFF; |
|
1576 } |
|
1577 initSDOline(d, line, CliServNbr, index, subIndex, SDO_BLOCK_DOWNLOAD_IN_PROGRESS); |
|
1578 d->transfers[line].rxstep = RXSTEP_STARTED; |
|
1579 d->transfers[line].peerCRCsupport = ((m->data[0])>>2) & 1; |
|
1580 if ((m->data[0]) & 2) /* if data set size is indicated */ |
|
1581 d->transfers[line].objsize = (UNS32)m->data[4] + (UNS32)m->data[5]*256 + (UNS32)m->data[6]*256*256 + (UNS32)m->data[7]*256*256*256; |
|
1582 data[0] = (5 << 5) | SDO_BSS_INITIATE_DOWNLOAD_RESPONSE; |
|
1583 data[1] = index; /* LSB */ |
|
1584 data[2] = index >> 8; /* MSB */ |
|
1585 data[3] = subIndex; |
|
1586 data[4] = SDO_BLOCK_SIZE; |
|
1587 data[5] = data[6] = data[7] = 0; |
|
1588 MSG_WAR(0x3AAD, "SDO. Sending block download initiate response - index 0x1200 + ", CliServNbr); |
|
1589 sendSDO(d, whoami, CliServNbr, data); |
|
1590 } |
|
1591 else if (d->transfers[line].rxstep == RXSTEP_STARTED) { |
|
1592 MSG_WAR(0x3A9B, "Received SDO block download data segment - index 0x1200 + ", CliServNbr); |
|
1593 RestartSDO_TIMER(line) |
|
1594 SeqNo = m->data[0] & 0x7F; |
|
1595 if (m->data[0] & 0x80) { /* Last segment ? */ |
|
1596 if(SeqNo == (d->transfers[line].seqno + 1)) { |
|
1597 d->transfers[line].rxstep = RXSTEP_END; |
|
1598 d->transfers[line].seqno = SeqNo; |
|
1599 /* Store the data temporary because we don't know yet how many bytes do not contain data */ |
|
1600 memcpy(d->transfers[line].tmpData, m->data, 8); |
|
1601 } |
|
1602 data[0] = (5 << 5) | SDO_BSS_DOWNLOAD_RESPONSE; |
|
1603 data[1] = d->transfers[line].seqno; |
|
1604 data[2] = SDO_BLOCK_SIZE; |
|
1605 data[3] = data[4] = data[5] = data[6] = data[7] = 0; |
|
1606 MSG_WAR(0x3AAE, "SDO. Sending block download response - index 0x1200 + ", CliServNbr); |
|
1607 sendSDO(d, whoami, CliServNbr, data); |
|
1608 d->transfers[line].seqno = 0; |
|
1609 } |
|
1610 else { |
|
1611 if (SeqNo == (d->transfers[line].seqno + 1)) { |
|
1612 d->transfers[line].seqno = SeqNo; |
|
1613 /* Store the data in the transfert structure. */ |
|
1614 err = SDOtoLine(d, line, 7, (*m).data + 1); |
|
1615 if (err) { |
|
1616 failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR); |
|
1617 return 0xFF; |
|
1618 } |
|
1619 } |
|
1620 if (SeqNo == SDO_BLOCK_SIZE) { |
|
1621 data[0] = (5 << 5) | SDO_BSS_DOWNLOAD_RESPONSE; |
|
1622 data[1] = d->transfers[line].seqno; |
|
1623 data[2] = SDO_BLOCK_SIZE; |
|
1624 data[3] = data[4] = data[5] = data[6] = data[7] = 0; |
|
1625 MSG_WAR(0x3AAE, "SDO. Sending block download response - index 0x1200 + ", CliServNbr); |
|
1626 sendSDO(d, whoami, CliServNbr, data); |
|
1627 d->transfers[line].seqno = 0; |
|
1628 } |
|
1629 } |
|
1630 } |
|
1631 else if (d->transfers[line].rxstep == RXSTEP_END) { /* endphase */ |
|
1632 MSG_WAR(0x3A9B, "Received SDO block download end request - index 0x1200 + ", CliServNbr); |
|
1633 /* here store remaining bytes in tmpData to line, check size and confirm or abort */ |
|
1634 if ((m->data[0] & 1) != SDO_BCS_END_DOWNLOAD_REQUEST) { |
|
1635 MSG_ERR(0x1AAD, "SDO error block download : Received wrong subcommand - index 0x1200 + ", CliServNbr); |
|
1636 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
|
1637 return 0xFF; |
|
1638 } |
|
1639 RestartSDO_TIMER(line) |
|
1640 NbBytesNoData = (m->data[0]>>2) & 0x07; |
|
1641 /* Store the data in the transfert structure. */ |
|
1642 err = SDOtoLine(d, line, 7-NbBytesNoData, d->transfers[line].tmpData + 1); |
|
1643 if (err) { |
|
1644 failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR); |
|
1645 return 0xFF; |
|
1646 } |
|
1647 if(d->transfers[line].objsize){ /* If size was indicated in the initiate request */ |
|
1648 if (d->transfers[line].objsize != d->transfers[line].offset){ |
|
1649 MSG_ERR(0x1AAE, "SDO error block download : sizes do not match - index 0x1200 + ", CliServNbr); |
|
1650 failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_LOCAL_CTRL_ERROR); |
|
1651 return 0xFF; |
|
1652 } |
|
1653 } |
|
1654 data[0] = (5 << 5) | SDO_BSS_END_DOWNLOAD_RESPONSE; |
|
1655 for (i = 1 ; i < 8 ; i++) |
|
1656 data[i] = 0; |
|
1657 MSG_WAR(0x3AAF, "SDO. Sending block download end response - index 0x1200 + ", CliServNbr); |
|
1658 sendSDO(d, whoami, CliServNbr, data); |
|
1659 /* Transfering line data to object dictionary. */ |
|
1660 errorCode = SDOlineToObjdict(d, line); |
|
1661 if (errorCode) { |
|
1662 MSG_ERR(0x1AAF, "SDO error : Unable to copy the data in the object dictionary", 0); |
|
1663 failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, errorCode); |
|
1664 return 0xFF; |
|
1665 } |
|
1666 /* Release of the line */ |
|
1667 resetSDOline(d, line); |
|
1668 MSG_WAR(0x3AAF, "SDO. End of block download defined at index 0x1200 + ", CliServNbr); |
|
1669 } |
|
1670 } /* end if SERVER */ |
|
1671 else { /* if CLIENT (block upload) */ |
|
1672 if (err) { |
|
1673 /* Nothing already started */ |
|
1674 MSG_ERR(0x1AAD, "SDO error block upload : no transmission started", nodeId); |
|
1675 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
|
1676 return 0xFF; |
|
1677 } |
|
1678 RestartSDO_TIMER(line) |
|
1679 if (d->transfers[line].rxstep == RXSTEP_INIT) { |
|
1680 if ((m->data[0] & 1) == SDO_BSS_INITIATE_UPLOAD_RESPONSE) { |
|
1681 MSG_WAR(0x3A9C, "Received SDO block upload response from node id ", nodeId); |
|
1682 d->transfers[line].rxstep = RXSTEP_STARTED; |
|
1683 d->transfers[line].peerCRCsupport = ((m->data[0])>>2) & 1; |
|
1684 if ((m->data[0]) & 2) /* if data set size is indicated */ |
|
1685 d->transfers[line].objsize = (UNS32)m->data[4] + (UNS32)m->data[5]*256 + (UNS32)m->data[6]*256*256 + (UNS32)m->data[7]*256*256*256; |
|
1686 data[0] = (5 << 5) | SDO_BCS_START_UPLOAD; |
|
1687 for (i = 1 ; i < 8 ; i++) |
|
1688 data[i] = 0; |
|
1689 MSG_WAR(0x3AB6, "SDO. Sending block upload start to node id ", nodeId); |
|
1690 sendSDO(d, whoami, CliServNbr, data); |
|
1691 } |
|
1692 } |
|
1693 else if (d->transfers[line].rxstep == RXSTEP_STARTED) { |
|
1694 SeqNo = m->data[0] & 0x7F; |
|
1695 if (m->data[0] & 0x80) { /* Last segment ? */ |
|
1696 if(SeqNo == (d->transfers[line].seqno + 1)) { |
|
1697 d->transfers[line].rxstep = RXSTEP_END; |
|
1698 d->transfers[line].seqno = SeqNo; |
|
1699 /* Store the data temporary because we don't know yet how many bytes do not contain data */ |
|
1700 memcpy(d->transfers[line].tmpData, m->data, 8); |
|
1701 } |
|
1702 data[0] = (5 << 5) | SDO_BCS_UPLOAD_RESPONSE; |
|
1703 data[1] = d->transfers[line].seqno; |
|
1704 data[2] = SDO_BLOCK_SIZE; |
|
1705 data[3] = data[4] = data[5] = data[6] = data[7] = 0; |
|
1706 MSG_WAR(0x3AB7, "SDO. Sending block upload response to node id ", nodeId); |
|
1707 sendSDO(d, whoami, CliServNbr, data); |
|
1708 d->transfers[line].seqno = 0; |
|
1709 } |
|
1710 else { |
|
1711 if (SeqNo == (d->transfers[line].seqno + 1)) { |
|
1712 d->transfers[line].seqno = SeqNo; |
|
1713 /* Store the data in the transfert structure. */ |
|
1714 err = SDOtoLine(d, line, 7, (*m).data + 1); |
|
1715 if (err) { |
|
1716 failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR); |
|
1717 return 0xFF; |
|
1718 } |
|
1719 } |
|
1720 if (SeqNo == SDO_BLOCK_SIZE) { |
|
1721 data[0] = (5 << 5) | SDO_BCS_UPLOAD_RESPONSE; |
|
1722 data[1] = d->transfers[line].seqno; |
|
1723 data[2] = SDO_BLOCK_SIZE; |
|
1724 data[3] = data[4] = data[5] = data[6] = data[7] = 0; |
|
1725 MSG_WAR(0x3AAE, "SDO. Sending block upload response to node id ", nodeId); |
|
1726 sendSDO(d, whoami, CliServNbr, data); |
|
1727 d->transfers[line].seqno = 0; |
|
1728 } |
|
1729 } |
|
1730 } |
|
1731 else if (d->transfers[line].rxstep == RXSTEP_END) { /* endphase */ |
|
1732 /* here store remaining bytes in tmpData to line, check size and confirm or abort */ |
|
1733 if ((m->data[0] & 1) != SDO_BSS_END_UPLOAD_RESPONSE) { |
|
1734 MSG_ERR(0x1AAD, "SDO error block upload : Received wrong subcommand from node id ", nodeId); |
|
1735 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
|
1736 return 0xFF; |
|
1737 } |
|
1738 NbBytesNoData = (m->data[0]>>2) & 0x07; |
|
1739 /* Store the data in the transfert structure. */ |
|
1740 err = SDOtoLine(d, line, 7-NbBytesNoData, d->transfers[line].tmpData + 1); |
|
1741 if (err) { |
|
1742 failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR); |
|
1743 return 0xFF; |
|
1744 } |
|
1745 if(d->transfers[line].objsize){ /* If size was indicated in the initiate request */ |
|
1746 if (d->transfers[line].objsize != d->transfers[line].offset){ |
|
1747 MSG_ERR(0x1AAE, "SDO error block download : sizes do not match - from node id ", nodeId); |
|
1748 failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_LOCAL_CTRL_ERROR); |
|
1749 return 0xFF; |
|
1750 } |
|
1751 } |
|
1752 data[0] = (5 << 5) | SDO_BCS_END_UPLOAD_REQUEST; |
|
1753 for (i = 1 ; i < 8 ; i++) |
|
1754 data[i] = 0; |
|
1755 MSG_WAR(0x3AAF, "SDO. Sending block upload end request to node id ", nodeId); |
|
1756 sendSDO(d, whoami, CliServNbr, data); |
|
1757 MSG_WAR(0x3AAF, "SDO. End of block upload request", 0); |
|
1758 StopSDO_TIMER(line) |
|
1759 d->transfers[line].state = SDO_FINISHED; |
|
1760 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
|
1761 } |
|
1762 } /* end if CLIENT */ |
|
1763 break; |
|
1312 default: |
1764 default: |
1313 /* Error : Unknown cs */ |
1765 /* Error : Unknown cs */ |
1314 MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", getSDOcs(m->data[0])); |
1766 MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", cs); |
1315 return 0xFF; |
1767 return 0xFF; |
1316 |
1768 |
1317 } /* End switch */ |
1769 } /* End switch */ |
1318 return 0; |
1770 return 0; |
1319 } |
1771 } |
1386 ** @param endianize |
1838 ** @param endianize |
1387 ** |
1839 ** |
1388 ** @return |
1840 ** @return |
1389 **/ |
1841 **/ |
1390 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, |
1842 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, |
1391 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize) |
1843 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize, UNS8 useBlockMode) |
1392 { |
1844 { |
1393 UNS8 err; |
1845 UNS8 err; |
1394 UNS8 line; |
1846 UNS8 line; |
1395 UNS8 CliNbr; |
1847 UNS8 CliNbr; |
1396 UNS32 j; |
1848 UNS32 j; |
1416 err = getSDOfreeLine( d, SDO_CLIENT, &line ); |
1868 err = getSDOfreeLine( d, SDO_CLIENT, &line ); |
1417 if (err) { |
1869 if (err) { |
1418 MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); |
1870 MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); |
1419 return (0xFF); |
1871 return (0xFF); |
1420 } |
1872 } |
1421 initSDOline(d, line, CliNbr, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS); |
1873 if(useBlockMode) { |
1874 initSDOline(d, line, CliNbr, index, subIndex, SDO_BLOCK_DOWNLOAD_IN_PROGRESS); |
|
1875 d->transfers[line].objsize = count; |
|
1876 } |
|
1877 else |
|
1878 initSDOline(d, line, CliNbr, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS); |
|
1422 d->transfers[line].count = count; |
1879 d->transfers[line].count = count; |
1423 d->transfers[line].dataType = dataType; |
1880 d->transfers[line].dataType = dataType; |
1424 |
|
1425 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
1881 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
1426 { |
1882 { |
1427 UNS8* lineData = d->transfers[line].data; |
1883 UNS8* lineData = d->transfers[line].data; |
1428 if (count > SDO_MAX_LENGTH_TRANSFERT) |
1884 if (count > SDO_MAX_LENGTH_TRANSFERT) |
1429 { |
1885 { |
1459 # else |
1915 # else |
1460 d->transfers[line].data[j] = ((char *)data)[j]; |
1916 d->transfers[line].data[j] = ((char *)data)[j]; |
1461 # endif |
1917 # endif |
1462 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
1918 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
1463 } |
1919 } |
1464 /* Send the SDO to the server. Initiate download, cs=1. */ |
1920 if(useBlockMode) { |
1465 if (count <= 4) { /* Expedited transfert */ |
1921 buf[0] = (6 << 5) | (1 << 1 ); /* CCS = 6 , CC = 0 , S = 1 , CS = 0 */ |
1466 buf[0] = (UNS8)((1 << 5) | ((4 - count) << 2) | 3); |
1922 for (i = 0 ; i < 4 ; i++) |
1467 for (i = 4 ; i < 8 ; i++) |
1923 buf[i+4] = (UNS8)((count >> (i<<3))); /* i*8 */ |
1468 buf[i] = d->transfers[line].data[i - 4]; |
1924 } |
1469 d->transfers[line].offset = count; |
1925 else { |
1470 } |
1926 /* Send the SDO to the server. Initiate download, cs=1. */ |
1471 else { /** Normal transfert */ |
1927 if (count <= 4) { /* Expedited transfert */ |
1472 buf[0] = (1 << 5) | 1; |
1928 buf[0] = (UNS8)((1 << 5) | ((4 - count) << 2) | 3); |
1473 for (i = 0 ; i < 4 ; i++) |
1929 for (i = 4 ; i < 8 ; i++) |
1474 buf[i+4] = (UNS8)((count >> (i<<3))); /* i*8 */ |
1930 buf[i] = d->transfers[line].data[i - 4]; |
1475 } |
1931 d->transfers[line].offset = count; |
1932 } |
|
1933 else { /** Normal transfert */ |
|
1934 buf[0] = (1 << 5) | 1; |
|
1935 for (i = 0 ; i < 4 ; i++) |
|
1936 buf[i+4] = (UNS8)((count >> (i<<3))); /* i*8 */ |
|
1937 } |
|
1938 } |
|
1476 buf[1] = index & 0xFF; /* LSB */ |
1939 buf[1] = index & 0xFF; /* LSB */ |
1477 buf[2] = (index >> 8) & 0xFF; /* MSB */ |
1940 buf[2] = (index >> 8) & 0xFF; /* MSB */ |
1478 buf[3] = subIndex; |
1941 buf[3] = subIndex; |
1479 |
1942 |
1480 d->transfers[line].Callback = Callback; |
1943 d->transfers[line].Callback = Callback; |
1503 ** @param data |
1966 ** @param data |
1504 ** |
1967 ** |
1505 ** @return |
1968 ** @return |
1506 **/ |
1969 **/ |
1507 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, |
1970 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, |
1508 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data) |
1971 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, UNS8 useBlockMode) |
1509 { |
1972 { |
1510 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, NULL, 1); |
1973 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, NULL, 1, useBlockMode); |
1511 } |
1974 } |
1512 |
1975 |
1513 /*! |
1976 /*! |
1514 ** |
1977 ** |
1515 ** |
1978 ** |
1523 ** @param Callback |
1986 ** @param Callback |
1524 ** |
1987 ** |
1525 ** @return |
1988 ** @return |
1526 **/ |
1989 **/ |
1527 UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index, |
1990 UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index, |
1528 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback) |
1991 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 useBlockMode) |
1529 { |
1992 { |
1530 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1); |
1993 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1, useBlockMode); |
1531 } |
1994 } |
1532 |
1995 |
1533 UNS8 writeNetworkDictCallBackAI (CO_Data* d, UNS8 nodeId, UNS16 index, |
1996 UNS8 writeNetworkDictCallBackAI (CO_Data* d, UNS8 nodeId, UNS16 index, |
1534 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize) |
1997 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize, UNS8 useBlockMode) |
1535 { |
1998 { |
1536 UNS8 ret; |
1999 UNS8 ret; |
1537 UNS16 lastIndex; |
2000 UNS16 lastIndex; |
1538 UNS16 offset; |
2001 UNS16 offset; |
1539 UNS8 nodeIdServer; |
2002 UNS8 nodeIdServer; |
1540 UNS8 i; |
2003 UNS8 i; |
1541 |
2004 |
1542 ret = _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, endianize); |
2005 ret = _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, endianize, useBlockMode); |
1543 if(ret == 0xFE) |
2006 if(ret == 0xFE) |
1544 { |
2007 { |
1545 offset = d->firstIndex->SDO_CLT; |
2008 offset = d->firstIndex->SDO_CLT; |
1546 lastIndex = d->lastIndex->SDO_CLT; |
2009 lastIndex = d->lastIndex->SDO_CLT; |
1547 if (offset == 0) |
2010 if (offset == 0) |
1561 if(nodeIdServer == 0) |
2024 if(nodeIdServer == 0) |
1562 { |
2025 { |
1563 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = (UNS32)(0x600 + nodeId); |
2026 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = (UNS32)(0x600 + nodeId); |
1564 *(UNS32*)d->objdict[offset].pSubindex[2].pObject = (UNS32)(0x580 + nodeId); |
2027 *(UNS32*)d->objdict[offset].pSubindex[2].pObject = (UNS32)(0x580 + nodeId); |
1565 *(UNS8*) d->objdict[offset].pSubindex[3].pObject = nodeId; |
2028 *(UNS8*) d->objdict[offset].pSubindex[3].pObject = nodeId; |
1566 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1); |
2029 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1, useBlockMode); |
1567 } |
2030 } |
1568 offset++; |
2031 offset++; |
1569 } |
2032 } |
1570 return 0xFF; |
2033 return 0xFF; |
1571 } |
2034 } |
1589 ** @param dataType |
2052 ** @param dataType |
1590 ** @param Callback |
2053 ** @param Callback |
1591 ** |
2054 ** |
1592 ** @return |
2055 ** @return |
1593 **/ |
2056 **/ |
1594 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback) |
2057 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode) |
1595 { |
2058 { |
1596 UNS8 err; |
2059 UNS8 err; |
1597 UNS8 i; |
2060 UNS8 i; |
1598 UNS8 CliNbr; |
2061 UNS8 CliNbr; |
1599 UNS8 line; |
2062 UNS8 line; |
1621 return (0xFF); |
2084 return (0xFF); |
1622 } |
2085 } |
1623 else |
2086 else |
1624 MSG_WAR(0x3AE0, "Transmission on line : ", line); |
2087 MSG_WAR(0x3AE0, "Transmission on line : ", line); |
1625 |
2088 |
1626 initSDOline(d, line, CliNbr, index, subIndex, SDO_UPLOAD_IN_PROGRESS); |
2089 if(useBlockMode) { |
1627 getSDOlineOnUse(d, CliNbr, SDO_CLIENT, &line); |
2090 initSDOline(d, line, CliNbr, index, subIndex, SDO_BLOCK_UPLOAD_IN_PROGRESS); |
1628 /* Send the SDO to the server. Initiate upload, cs=2. */ |
2091 /* Send the SDO to the server. Initiate block upload, cs=0. */ |
1629 d->transfers[line].dataType = dataType; |
2092 d->transfers[line].dataType = dataType; |
1630 data[0] = (2 << 5); |
2093 data[0] = (5 << 5) | SDO_BCS_INITIATE_UPLOAD_REQUEST; |
1631 data[1] = index & 0xFF; /* LSB */ |
2094 data[1] = index & 0xFF; /* LSB */ |
1632 data[2] = (index >> 8) & 0xFF; /* MSB */ |
2095 data[2] = (index >> 8) & 0xFF; /* MSB */ |
1633 data[3] = subIndex; |
2096 data[3] = subIndex; |
1634 for (i = 4 ; i < 8 ; i++) |
2097 data[4] = SDO_BLOCK_SIZE; |
1635 data[i] = 0; |
2098 for (i = 5 ; i < 8 ; i++) |
2099 data[i] = 0; |
|
2100 } |
|
2101 else { |
|
2102 initSDOline(d, line, CliNbr, index, subIndex, SDO_UPLOAD_IN_PROGRESS); |
|
2103 /* Send the SDO to the server. Initiate upload, cs=2. */ |
|
2104 d->transfers[line].dataType = dataType; |
|
2105 data[0] = (2 << 5); |
|
2106 data[1] = index & 0xFF; /* LSB */ |
|
2107 data[2] = (index >> 8) & 0xFF; /* MSB */ |
|
2108 data[3] = subIndex; |
|
2109 for (i = 4 ; i < 8 ; i++) |
|
2110 data[i] = 0; |
|
2111 } |
|
1636 d->transfers[line].Callback = Callback; |
2112 d->transfers[line].Callback = Callback; |
1637 err = sendSDO(d, SDO_CLIENT, CliNbr, data); |
2113 err = sendSDO(d, SDO_CLIENT, CliNbr, data); |
1638 if (err) { |
2114 if (err) { |
1639 MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId); |
2115 MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId); |
1640 /* release the line */ |
2116 /* release the line */ |
1653 ** @param subIndex |
2129 ** @param subIndex |
1654 ** @param dataType |
2130 ** @param dataType |
1655 ** |
2131 ** |
1656 ** @return |
2132 ** @return |
1657 **/ |
2133 **/ |
1658 UNS8 readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType) |
2134 UNS8 readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, UNS8 useBlockMode) |
1659 { |
2135 { |
1660 return _readNetworkDict (d, nodeId, index, subIndex, dataType, NULL); |
2136 return _readNetworkDict (d, nodeId, index, subIndex, dataType, NULL, useBlockMode); |
1661 } |
2137 } |
1662 |
2138 |
1663 /*! |
2139 /*! |
1664 ** |
2140 ** |
1665 ** |
2141 ** |
1670 ** @param dataType |
2146 ** @param dataType |
1671 ** @param Callback |
2147 ** @param Callback |
1672 ** |
2148 ** |
1673 ** @return |
2149 ** @return |
1674 **/ |
2150 **/ |
1675 UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback) |
2151 UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode) |
1676 { |
2152 { |
1677 return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback); |
2153 return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback, useBlockMode); |
1678 } |
2154 } |
1679 |
2155 |
1680 UNS8 readNetworkDictCallbackAI (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback) |
2156 UNS8 readNetworkDictCallbackAI (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode) |
1681 { |
2157 { |
1682 UNS8 ret; |
2158 UNS8 ret; |
1683 UNS16 lastIndex; |
2159 UNS16 lastIndex; |
1684 UNS16 offset; |
2160 UNS16 offset; |
1685 UNS8 nodeIdServer; |
2161 UNS8 nodeIdServer; |
1686 UNS8 i; |
2162 UNS8 i; |
1687 |
2163 |
1688 ret = _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback); |
2164 ret = _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback, useBlockMode); |
1689 if(ret == 0xFE) |
2165 if(ret == 0xFE) |
1690 { |
2166 { |
1691 offset = d->firstIndex->SDO_CLT; |
2167 offset = d->firstIndex->SDO_CLT; |
1692 lastIndex = d->lastIndex->SDO_CLT; |
2168 lastIndex = d->lastIndex->SDO_CLT; |
1693 if (offset == 0) |
2169 if (offset == 0) |
1707 if(nodeIdServer == 0) |
2183 if(nodeIdServer == 0) |
1708 { |
2184 { |
1709 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = (UNS32)(0x600 + nodeId); |
2185 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = (UNS32)(0x600 + nodeId); |
1710 *(UNS32*)d->objdict[offset].pSubindex[2].pObject = (UNS32)(0x580 + nodeId); |
2186 *(UNS32*)d->objdict[offset].pSubindex[2].pObject = (UNS32)(0x580 + nodeId); |
1711 *(UNS8*) d->objdict[offset].pSubindex[3].pObject = nodeId; |
2187 *(UNS8*) d->objdict[offset].pSubindex[3].pObject = nodeId; |
1712 return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback); |
2188 return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback, useBlockMode); |
1713 } |
2189 } |
1714 offset++; |
2190 offset++; |
1715 } |
2191 } |
1716 return 0xFF; |
2192 return 0xFF; |
1717 } |
2193 } |
1729 ** |
2205 ** |
1730 ** |
2206 ** |
1731 ** @param d |
2207 ** @param d |
1732 ** @param nodeId |
2208 ** @param nodeId |
1733 ** @param data |
2209 ** @param data |
1734 ** @param size pointer to expected size, changed into returned size. Expected size will be truncated to transfered data size |
2210 ** @param size : *size MUST contain the size of *data buffer before calling |
2211 ** The function set it to the actual number of written bytes |
|
1735 ** @param abortCode |
2212 ** @param abortCode |
1736 ** |
2213 ** |
1737 ** @return |
2214 ** @return |
2215 ** SDO_PROVIDED_BUFFER_TOO_SMALL if *data is not big enough |
|
2216 ** or any transmission status value. |
|
1738 **/ |
2217 **/ |
1739 UNS8 getReadResultNetworkDict (CO_Data* d, UNS8 nodeId, void* data, UNS32 *size, |
2218 UNS8 getReadResultNetworkDict (CO_Data* d, UNS8 nodeId, void* data, UNS32 *size, |
1740 UNS32 * abortCode) |
2219 UNS32 * abortCode) |
1741 { |
2220 { |
1742 UNS32 i; |
2221 UNS32 i; |
1745 UNS8 line; |
2224 UNS8 line; |
1746 * abortCode = 0; |
2225 * abortCode = 0; |
1747 |
2226 |
1748 /* First let's find the corresponding SDO client in our OD */ |
2227 /* First let's find the corresponding SDO client in our OD */ |
1749 CliNbr = GetSDOClientFromNodeId(d, nodeId); |
2228 CliNbr = GetSDOClientFromNodeId(d, nodeId); |
1750 if(CliNbr >= 0xFE) |
2229 if(CliNbr >= 0xFE) { |
2230 *size = 0; |
|
1751 return SDO_ABORTED_INTERNAL; |
2231 return SDO_ABORTED_INTERNAL; |
2232 } |
|
1752 |
2233 |
1753 /* Looking for the line tranfert. */ |
2234 /* Looking for the line tranfert. */ |
1754 err = getSDOlineOnUse(d, CliNbr, SDO_CLIENT, &line); |
2235 err = getSDOlineOnUse(d, CliNbr, SDO_CLIENT, &line); |
1755 if (err) { |
2236 if (err) { |
1756 MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId); |
2237 MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId); |
1757 return SDO_ABORTED_INTERNAL; |
2238 *size = 0; |
1758 } |
2239 return SDO_ABORTED_INTERNAL; |
1759 * abortCode = d->transfers[line].abortCode; |
2240 } |
1760 if (d->transfers[line].state != SDO_FINISHED) |
2241 |
2242 /* If transfert not finished just return, but if aborted set abort code and size to 0 */ |
|
2243 if (d->transfers[line].state != SDO_FINISHED) { |
|
2244 if((d->transfers[line].state == SDO_ABORTED_RCV) || (d->transfers[line].state == SDO_ABORTED_INTERNAL)) { |
|
2245 *abortCode = d->transfers[line].abortCode; |
|
2246 *size = 0; |
|
2247 } |
|
1761 return d->transfers[line].state; |
2248 return d->transfers[line].state; |
2249 } |
|
1762 |
2250 |
1763 /* if SDO initiated with e=0 and s=0 count is null, offset carry effective size*/ |
2251 /* if SDO initiated with e=0 and s=0 count is null, offset carry effective size*/ |
1764 if( d->transfers[line].count == 0) |
2252 if( d->transfers[line].count == 0) |
1765 d->transfers[line].count = d->transfers[line].offset; |
2253 d->transfers[line].count = d->transfers[line].offset; |
1766 /* use transfers[line].count as max size */ |
2254 |
1767 if( d->transfers[line].count < *size ) |
2255 /* Check if the provided buffer is big enough */ |
1768 *size = d->transfers[line].count; |
2256 if(*size < d->transfers[line].count) { |
2257 *size = 0; |
|
2258 return SDO_PROVIDED_BUFFER_TOO_SMALL; |
|
2259 } |
|
2260 |
|
2261 /* Give back actual size */ |
|
2262 *size = d->transfers[line].count; |
|
1769 |
2263 |
1770 /* Copy payload to data pointer */ |
2264 /* Copy payload to data pointer */ |
1771 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
2265 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
1772 { |
2266 { |
1773 UNS8 *lineData = d->transfers[line].data; |
2267 UNS8 *lineData = d->transfers[line].data; |
1797 # else |
2291 # else |
1798 ( (char *) data)[i] = d->transfers[line].data[i]; |
2292 ( (char *) data)[i] = d->transfers[line].data[i]; |
1799 # endif |
2293 # endif |
1800 } |
2294 } |
1801 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
2295 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
2296 resetSDOline(d, line); |
|
1802 return SDO_FINISHED; |
2297 return SDO_FINISHED; |
1803 } |
2298 } |
1804 |
2299 |
1805 /*! |
2300 /*! |
1806 ** |
2301 ** |
1828 if (err) { |
2323 if (err) { |
1829 MSG_ERR(0x1AF1, "SDO error : No line found for communication with node : ", nodeId); |
2324 MSG_ERR(0x1AF1, "SDO error : No line found for communication with node : ", nodeId); |
1830 return SDO_ABORTED_INTERNAL; |
2325 return SDO_ABORTED_INTERNAL; |
1831 } |
2326 } |
1832 * abortCode = d->transfers[line].abortCode; |
2327 * abortCode = d->transfers[line].abortCode; |
1833 return d->transfers[line].state; |
2328 if (d->transfers[line].state != SDO_FINISHED) |
1834 } |
2329 return d->transfers[line].state; |
2330 resetSDOline(d, line); |
|
2331 return SDO_FINISHED; |
|
2332 } |