209 if( d->transfers[line].count == 0) |
209 if( d->transfers[line].count == 0) |
210 d->transfers[line].count = d->transfers[line].offset; |
210 d->transfers[line].count = d->transfers[line].offset; |
211 size = d->transfers[line].count; |
211 size = d->transfers[line].count; |
212 |
212 |
213 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
213 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
214 if (size > SDO_MAX_LENGTH_TRANSFERT) |
214 if (size > SDO_MAX_LENGTH_TRANSFER) |
215 { |
215 { |
216 errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, |
216 errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, |
217 (void *) d->transfers[line].dynamicData, &size, 1); |
217 (void *) d->transfers[line].dynamicData, &size, 1); |
218 } |
218 } |
219 else |
219 else |
241 ** |
241 ** |
242 ** @return |
242 ** @return |
243 **/ |
243 **/ |
244 UNS32 objdictToSDOline (CO_Data* d, UNS8 line) |
244 UNS32 objdictToSDOline (CO_Data* d, UNS8 line) |
245 { |
245 { |
246 UNS32 size = SDO_MAX_LENGTH_TRANSFERT; |
246 UNS32 size = SDO_MAX_LENGTH_TRANSFER; |
247 UNS8 dataType; |
247 UNS8 dataType; |
248 UNS32 errorCode; |
248 UNS32 errorCode; |
249 |
249 |
250 MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index); |
250 MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index); |
251 MSG_WAR(0x3A06, " subIndex : ", d->transfers[line].subIndex); |
251 MSG_WAR(0x3A06, " subIndex : ", d->transfers[line].subIndex); |
302 UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS32 nbBytes, UNS8* data) { |
302 UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS32 nbBytes, UNS8* data) { |
303 UNS8 i; |
303 UNS8 i; |
304 UNS32 offset; |
304 UNS32 offset; |
305 |
305 |
306 #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION |
306 #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION |
307 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { |
307 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFER) { |
308 MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
308 MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFER", nbBytes); |
309 return 0xFF; |
309 return 0xFF; |
310 } |
310 } |
311 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
311 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
312 |
312 |
313 if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) { |
313 if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) { |
314 MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes); |
314 MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes); |
315 return 0xFF; |
315 return 0xFF; |
316 } |
316 } |
317 offset = d->transfers[line].offset; |
317 offset = d->transfers[line].offset; |
318 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
318 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
319 if (d->transfers[line].count <= SDO_MAX_LENGTH_TRANSFERT) |
319 if (d->transfers[line].count <= SDO_MAX_LENGTH_TRANSFER) |
320 { |
320 { |
321 for (i = 0 ; i < nbBytes ; i++) |
321 for (i = 0 ; i < nbBytes ; i++) |
322 * (data + i) = d->transfers[line].data[offset + i]; |
322 * (data + i) = d->transfers[line].data[offset + i]; |
323 } |
323 } |
324 else |
324 else |
352 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS32 nbBytes, UNS8* data) |
352 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS32 nbBytes, UNS8* data) |
353 { |
353 { |
354 UNS8 i; |
354 UNS8 i; |
355 UNS32 offset; |
355 UNS32 offset; |
356 #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION |
356 #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION |
357 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { |
357 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFER) { |
358 MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
358 MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFER", nbBytes); |
359 return 0xFF; |
359 return 0xFF; |
360 } |
360 } |
361 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
361 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
362 |
362 |
363 offset = d->transfers[line].offset; |
363 offset = d->transfers[line].offset; |
364 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
364 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
365 { |
365 { |
366 UNS8* lineData = d->transfers[line].data; |
366 UNS8* lineData = d->transfers[line].data; |
367 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { |
367 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFER) { |
368 if (d->transfers[line].dynamicData == NULL) { |
368 if (d->transfers[line].dynamicData == NULL) { |
369 d->transfers[line].dynamicData = (UNS8*) malloc(SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE); |
369 d->transfers[line].dynamicData = (UNS8*) malloc(SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE); |
370 d->transfers[line].dynamicDataSize = SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE; |
370 d->transfers[line].dynamicDataSize = SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE; |
371 |
371 |
372 if (d->transfers[line].dynamicData == NULL) { |
372 if (d->transfers[line].dynamicData == NULL) { |
448 void resetSDOline ( CO_Data* d, UNS8 line ) |
448 void resetSDOline ( CO_Data* d, UNS8 line ) |
449 { |
449 { |
450 UNS32 i; |
450 UNS32 i; |
451 MSG_WAR(0x3A25, "reset SDO line nb : ", line); |
451 MSG_WAR(0x3A25, "reset SDO line nb : ", line); |
452 initSDOline(d, line, 0, 0, 0, SDO_RESET); |
452 initSDOline(d, line, 0, 0, 0, SDO_RESET); |
453 for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++) |
453 for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFER ; i++) |
454 d->transfers[line].data[i] = 0; |
454 d->transfers[line].data[i] = 0; |
455 d->transfers[line].whoami = 0; |
455 d->transfers[line].whoami = 0; |
456 d->transfers[line].abortCode = 0; |
456 d->transfers[line].abortCode = 0; |
457 } |
457 } |
458 |
458 |
540 UNS8 getSDOlineOnUse (CO_Data* d, UNS8 CliServNbr, UNS8 whoami, UNS8 *line) |
540 UNS8 getSDOlineOnUse (CO_Data* d, UNS8 CliServNbr, UNS8 whoami, UNS8 *line) |
541 { |
541 { |
542 |
542 |
543 UNS8 i; |
543 UNS8 i; |
544 |
544 |
545 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){ |
545 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERS ; i++){ |
546 if ( (d->transfers[i].state != SDO_RESET) && |
546 if ( (d->transfers[i].state != SDO_RESET) && |
547 (d->transfers[i].state != SDO_ABORTED_INTERNAL) && |
547 (d->transfers[i].state != SDO_ABORTED_INTERNAL) && |
548 (d->transfers[i].CliServNbr == CliServNbr) && |
548 (d->transfers[i].CliServNbr == CliServNbr) && |
549 (d->transfers[i].whoami == whoami) ) { |
549 (d->transfers[i].whoami == whoami) ) { |
550 if (line) *line = i; |
550 if (line) *line = i; |
567 UNS8 getSDOlineToClose (CO_Data* d, UNS8 CliServNbr, UNS8 whoami, UNS8 *line) |
567 UNS8 getSDOlineToClose (CO_Data* d, UNS8 CliServNbr, UNS8 whoami, UNS8 *line) |
568 { |
568 { |
569 |
569 |
570 UNS8 i; |
570 UNS8 i; |
571 |
571 |
572 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){ |
572 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERS ; i++){ |
573 if ( (d->transfers[i].state != SDO_RESET) && |
573 if ( (d->transfers[i].state != SDO_RESET) && |
574 (d->transfers[i].CliServNbr == CliServNbr) && |
574 (d->transfers[i].CliServNbr == CliServNbr) && |
575 (d->transfers[i].whoami == whoami) ) { |
575 (d->transfers[i].whoami == whoami) ) { |
576 if (line) *line = i; |
576 if (line) *line = i; |
577 return 0; |
577 return 0; |
637 ** @return |
637 ** @return |
638 **/ |
638 **/ |
639 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS32 nbBytes) |
639 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS32 nbBytes) |
640 { |
640 { |
641 #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION |
641 #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION |
642 if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) { |
642 if (nbBytes > SDO_MAX_LENGTH_TRANSFER) { |
643 MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
643 MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFER", nbBytes); |
644 return 0xFF; |
644 return 0xFF; |
645 } |
645 } |
646 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
646 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
647 |
647 |
648 d->transfers[line].count = nbBytes; |
648 d->transfers[line].count = nbBytes; |
759 UNS32 i; |
759 UNS32 i; |
760 UNS8 j; |
760 UNS8 j; |
761 UNS32 *pCobId = NULL; |
761 UNS32 *pCobId = NULL; |
762 UNS16 offset; |
762 UNS16 offset; |
763 UNS16 lastIndex; |
763 UNS16 lastIndex; |
764 UNS8 SubCommand; /* Block transfert only */ |
764 UNS8 SubCommand; /* Block transfer only */ |
765 UNS8 SeqNo; /* Sequence number in block transfert */ |
765 UNS8 SeqNo; /* Sequence number in block transfer */ |
766 UNS8 AckSeq; /* Sequence number of last segment that was received successfully */ |
766 UNS8 AckSeq; /* Sequence number of last segment that was received successfully */ |
767 UNS8 NbBytesNoData; /* Number of bytes that do not contain data in last segment of block transfert */ |
767 UNS8 NbBytesNoData; /* Number of bytes that do not contain data in last segment of block transfer */ |
768 |
768 |
769 MSG_WAR(0x3A60, "proceedSDO ", 0); |
769 MSG_WAR(0x3A60, "proceedSDO ", 0); |
770 whoami = SDO_UNKNOWN; |
770 whoami = SDO_UNKNOWN; |
771 /* Looking for the cobId in the object dictionary. */ |
771 /* Looking for the cobId in the object dictionary. */ |
772 /* Am-I a server ? */ |
772 /* Am-I a server ? */ |
831 } |
831 } |
832 else { |
832 else { |
833 MSG_WAR(0x3A69, "I am SERVER number ", CliServNbr); |
833 MSG_WAR(0x3A69, "I am SERVER number ", CliServNbr); |
834 } |
834 } |
835 |
835 |
836 /* Look for an SDO transfert already initiated. */ |
836 /* Look for an SDO transfer already initiated. */ |
837 err = getSDOlineOnUse( d, CliServNbr, whoami, &line ); |
837 err = getSDOlineOnUse( d, CliServNbr, whoami, &line ); |
838 |
838 |
839 /* Let's find cs value, first it is set as "not valid" */ |
839 /* Let's find cs value, first it is set as "not valid" */ |
840 cs = 0xFF; |
840 cs = 0xFF; |
841 /* Special cases for block transfert : in frames with segment data cs is not spécified */ |
841 /* Special cases for block transfer : in frames with segment data cs is not spécified */ |
842 if (!err) { |
842 if (!err) { |
843 if ((whoami == SDO_SERVER) && (d->transfers[line].state == SDO_BLOCK_DOWNLOAD_IN_PROGRESS) || |
843 if ((whoami == SDO_SERVER) && (d->transfers[line].state == SDO_BLOCK_DOWNLOAD_IN_PROGRESS) || |
844 (whoami == SDO_CLIENT) && (d->transfers[line].state == SDO_BLOCK_UPLOAD_IN_PROGRESS)) { |
844 (whoami == SDO_CLIENT) && (d->transfers[line].state == SDO_BLOCK_UPLOAD_IN_PROGRESS)) { |
845 if(m->data[0] == 0x80) /* If first byte is 0x80 it is an abort frame (seqno = 0 not allowed) */ |
845 if(m->data[0] == 0x80) /* If first byte is 0x80 it is an abort frame (seqno = 0 not allowed) */ |
846 cs = 4; |
846 cs = 4; |
858 switch (cs) { |
858 switch (cs) { |
859 |
859 |
860 case 0: |
860 case 0: |
861 /* I am SERVER */ |
861 /* I am SERVER */ |
862 if (whoami == SDO_SERVER) { |
862 if (whoami == SDO_SERVER) { |
863 /* Receiving a download segment data : an SDO transfert should have been yet initiated. */ |
863 /* Receiving a download segment data : an SDO transfer should have been yet initiated. */ |
864 if (!err) |
864 if (!err) |
865 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
865 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; |
866 if (err) { |
866 if (err) { |
867 MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", |
867 MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", |
868 CliServNbr); |
868 CliServNbr); |
880 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); |
880 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); |
881 return 0xFF; |
881 return 0xFF; |
882 } |
882 } |
883 /* Nb of data to be downloaded */ |
883 /* Nb of data to be downloaded */ |
884 nbBytes = 7 - getSDOn3(m->data[0]); |
884 nbBytes = 7 - getSDOn3(m->data[0]); |
885 /* Store the data in the transfert structure. */ |
885 /* Store the data in the transfer structure. */ |
886 err = SDOtoLine(d, line, nbBytes, (*m).data + 1); |
886 err = SDOtoLine(d, line, nbBytes, (*m).data + 1); |
887 if (err) { |
887 if (err) { |
888 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
888 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
889 return 0xFF; |
889 return 0xFF; |
890 } |
890 } |
971 MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ", |
971 MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ", |
972 CliServNbr); |
972 CliServNbr); |
973 MSG_WAR(0x3A80, "Writing at index : ", index); |
973 MSG_WAR(0x3A80, "Writing at index : ", index); |
974 MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex); |
974 MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex); |
975 |
975 |
976 /* Search if a SDO transfert have been yet initiated */ |
976 /* Search if a SDO transfer have been yet initiated */ |
977 if (! err) { |
977 if (! err) { |
978 MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0); |
978 MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0); |
979 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
979 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
980 return 0xFF; |
980 return 0xFF; |
981 } |
981 } |
999 if (err) { |
999 if (err) { |
1000 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
1000 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
1001 return 0xFF; |
1001 return 0xFF; |
1002 } |
1002 } |
1003 |
1003 |
1004 /* SDO expedited -> transfert finished. Data can be stored in the dictionary. */ |
1004 /* SDO expedited -> transfer finished. Data can be stored in the dictionary. */ |
1005 /*The line will be reseted when it is downloading in the dictionary. */ |
1005 /*The line will be reseted when it is downloading in the dictionary. */ |
1006 MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfert. Finished. ", 0); |
1006 MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfer. Finished. ", 0); |
1007 /* Transfering line data to object dictionary. */ |
1007 /* Transfering line data to object dictionary. */ |
1008 errorCode = SDOlineToObjdict(d, line); |
1008 errorCode = SDOlineToObjdict(d, line); |
1009 if (errorCode) { |
1009 if (errorCode) { |
1010 MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0); |
1010 MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0); |
1011 failedSDO(d, CliServNbr, whoami, index, subIndex, errorCode); |
1011 failedSDO(d, CliServNbr, whoami, index, subIndex, errorCode); |
1012 return 0xFF; |
1012 return 0xFF; |
1013 } |
1013 } |
1014 /* Release of the line. */ |
1014 /* Release of the line. */ |
1015 resetSDOline(d, line); |
1015 resetSDOline(d, line); |
1016 } |
1016 } |
1017 else {/* So, if it is not an expedited transfert */ |
1017 else {/* So, if it is not an expedited transfer */ |
1018 if (getSDOs(m->data[0])) { |
1018 if (getSDOs(m->data[0])) { |
1019 nbBytes = (m->data[4]) + ((UNS32)(m->data[5])<<8) + ((UNS32)(m->data[6])<<16) + ((UNS32)(m->data[7])<<24); |
1019 nbBytes = (m->data[4]) + ((UNS32)(m->data[5])<<8) + ((UNS32)(m->data[6])<<16) + ((UNS32)(m->data[7])<<24); |
1020 err = setSDOlineRestBytes(d, line, nbBytes); |
1020 err = setSDOlineRestBytes(d, line, nbBytes); |
1021 if (err) { |
1021 if (err) { |
1022 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
1022 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
1101 subIndex = getSDOsubIndex(m->data[3]); |
1101 subIndex = getSDOsubIndex(m->data[3]); |
1102 MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", |
1102 MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", |
1103 CliServNbr); |
1103 CliServNbr); |
1104 MSG_WAR(0x3A90, "Reading at index : ", index); |
1104 MSG_WAR(0x3A90, "Reading at index : ", index); |
1105 MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex); |
1105 MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex); |
1106 /* Search if a SDO transfert have been yet initiated*/ |
1106 /* Search if a SDO transfer have been yet initiated*/ |
1107 if (! err) { |
1107 if (! err) { |
1108 MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line); |
1108 MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line); |
1109 MSG_WAR(0x3A93, "Server Nbr = ", CliServNbr); |
1109 MSG_WAR(0x3A93, "Server Nbr = ", CliServNbr); |
1110 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
1110 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
1111 return 0xFF; |
1111 return 0xFF; |
1129 return 0xFF; |
1129 return 0xFF; |
1130 } |
1130 } |
1131 /* Preparing the response.*/ |
1131 /* Preparing the response.*/ |
1132 getSDOlineRestBytes(d, line, &nbBytes); /* Nb bytes to transfer ? */ |
1132 getSDOlineRestBytes(d, line, &nbBytes); /* Nb bytes to transfer ? */ |
1133 if (nbBytes > 4) { |
1133 if (nbBytes > 4) { |
1134 /* normal transfert. (segmented). */ |
1134 /* normal transfer. (segmented). */ |
1135 /* code to send the initiate upload response. (cs = 2) */ |
1135 /* code to send the initiate upload response. (cs = 2) */ |
1136 data[0] = (2 << 5) | 1; |
1136 data[0] = (2 << 5) | 1; |
1137 data[1] = index & 0xFF; /* LSB */ |
1137 data[1] = index & 0xFF; /* LSB */ |
1138 data[2] = (index >> 8) & 0xFF; /* MSB */ |
1138 data[2] = (index >> 8) & 0xFF; /* MSB */ |
1139 data[3] = subIndex; |
1139 data[3] = subIndex; |
1187 err = SDOtoLine(d, line, nbBytes, (*m).data + 4); |
1187 err = SDOtoLine(d, line, nbBytes, (*m).data + 4); |
1188 if (err) { |
1188 if (err) { |
1189 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
1189 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
1190 return 0xFF; |
1190 return 0xFF; |
1191 } |
1191 } |
1192 /* SDO expedited -> transfert finished. data are available via getReadResultNetworkDict(). */ |
1192 /* SDO expedited -> transfer finished. data are available via getReadResultNetworkDict(). */ |
1193 MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId); |
1193 MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId); |
1194 StopSDO_TIMER(line) |
1194 StopSDO_TIMER(line) |
1195 d->transfers[line].count = nbBytes; |
1195 d->transfers[line].count = nbBytes; |
1196 d->transfers[line].state = SDO_FINISHED; |
1196 d->transfers[line].state = SDO_FINISHED; |
1197 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
1197 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); |
1198 return 0; |
1198 return 0; |
1199 } |
1199 } |
1200 else { /* So, if it is not an expedited transfert */ |
1200 else { /* So, if it is not an expedited transfer */ |
1201 /* Storing the nb of data to receive. */ |
1201 /* Storing the nb of data to receive. */ |
1202 if (getSDOs(m->data[0])) { |
1202 if (getSDOs(m->data[0])) { |
1203 nbBytes = m->data[4] + ((UNS32)(m->data[5])<<8) + ((UNS32)(m->data[6])<<16) + ((UNS32)(m->data[7])<<24); |
1203 nbBytes = m->data[4] + ((UNS32)(m->data[5])<<8) + ((UNS32)(m->data[6])<<16) + ((UNS32)(m->data[7])<<24); |
1204 err = setSDOlineRestBytes(d, line, nbBytes); |
1204 err = setSDOlineRestBytes(d, line, nbBytes); |
1205 if (err) { |
1205 if (err) { |
1219 |
1219 |
1220 case 3: |
1220 case 3: |
1221 /* I am SERVER */ |
1221 /* I am SERVER */ |
1222 if (whoami == SDO_SERVER) { |
1222 if (whoami == SDO_SERVER) { |
1223 /* Receiving a upload segment. */ |
1223 /* Receiving a upload segment. */ |
1224 /* A SDO transfert should have been yet initiated. */ |
1224 /* A SDO transfer should have been yet initiated. */ |
1225 if (!err) |
1225 if (!err) |
1226 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
1226 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; |
1227 if (err) { |
1227 if (err) { |
1228 MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", |
1228 MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", |
1229 CliServNbr); |
1229 CliServNbr); |
1364 subIndex = getSDOsubIndex(m->data[3]); |
1364 subIndex = getSDOsubIndex(m->data[3]); |
1365 MSG_WAR(0x3AB2, "Received SDO Initiate block upload defined at index 0x1200 + ", |
1365 MSG_WAR(0x3AB2, "Received SDO Initiate block upload defined at index 0x1200 + ", |
1366 CliServNbr); |
1366 CliServNbr); |
1367 MSG_WAR(0x3AB3, "Reading at index : ", index); |
1367 MSG_WAR(0x3AB3, "Reading at index : ", index); |
1368 MSG_WAR(0x3AB4, "Reading at subIndex : ", subIndex); |
1368 MSG_WAR(0x3AB4, "Reading at subIndex : ", subIndex); |
1369 /* Search if a SDO transfert have been yet initiated */ |
1369 /* Search if a SDO transfer have been yet initiated */ |
1370 if (! err) { |
1370 if (! err) { |
1371 MSG_ERR(0x1A93, "SDO error : Transmission yet started at line : ", line); |
1371 MSG_ERR(0x1A93, "SDO error : Transmission yet started at line : ", line); |
1372 MSG_WAR(0x3AB5, "Server Nbr = ", CliServNbr); |
1372 MSG_WAR(0x3AB5, "Server Nbr = ", CliServNbr); |
1373 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
1373 failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); |
1374 return 0xFF; |
1374 return 0xFF; |
1406 MSG_WAR(0x3A9A, "SDO. Sending normal block upload initiate response defined at index 0x1200 + ", nodeId); |
1406 MSG_WAR(0x3A9A, "SDO. Sending normal block upload initiate response defined at index 0x1200 + ", nodeId); |
1407 sendSDO(d, whoami, CliServNbr, data); |
1407 sendSDO(d, whoami, CliServNbr, data); |
1408 } |
1408 } |
1409 else if (SubCommand == SDO_BCS_END_UPLOAD_REQUEST) { |
1409 else if (SubCommand == SDO_BCS_END_UPLOAD_REQUEST) { |
1410 MSG_WAR(0x3AA2, "Received SDO block END upload request defined at index 0x1200 + ", CliServNbr); |
1410 MSG_WAR(0x3AA2, "Received SDO block END upload request defined at index 0x1200 + ", CliServNbr); |
1411 /* A SDO transfert should have been yet initiated. */ |
1411 /* A SDO transfer should have been yet initiated. */ |
1412 if (!err) |
1412 if (!err) |
1413 err = d->transfers[line].state != SDO_BLOCK_UPLOAD_IN_PROGRESS; |
1413 err = d->transfers[line].state != SDO_BLOCK_UPLOAD_IN_PROGRESS; |
1414 if (err) { |
1414 if (err) { |
1415 MSG_ERR(0x1AA1, "SDO error : Received block upload request for unstarted trans. index 0x1200 + ", |
1415 MSG_ERR(0x1AA1, "SDO error : Received block upload request for unstarted trans. index 0x1200 + ", |
1416 CliServNbr); |
1416 CliServNbr); |
1419 } |
1419 } |
1420 /* Release the line */ |
1420 /* Release the line */ |
1421 resetSDOline(d, line); |
1421 resetSDOline(d, line); |
1422 } |
1422 } |
1423 else if ((SubCommand == SDO_BCS_UPLOAD_RESPONSE) || (SubCommand == SDO_BCS_START_UPLOAD)) { |
1423 else if ((SubCommand == SDO_BCS_UPLOAD_RESPONSE) || (SubCommand == SDO_BCS_START_UPLOAD)) { |
1424 /* A SDO transfert should have been yet initiated. */ |
1424 /* A SDO transfer should have been yet initiated. */ |
1425 if (!err) |
1425 if (!err) |
1426 err = d->transfers[line].state != SDO_BLOCK_UPLOAD_IN_PROGRESS; |
1426 err = d->transfers[line].state != SDO_BLOCK_UPLOAD_IN_PROGRESS; |
1427 if (err) { |
1427 if (err) { |
1428 MSG_ERR(0x1AA1, "SDO error : Received block upload response for unstarted trans. index 0x1200 + ", |
1428 MSG_ERR(0x1AA1, "SDO error : Received block upload response for unstarted trans. index 0x1200 + ", |
1429 CliServNbr); |
1429 CliServNbr); |
1634 d->transfers[line].seqno = 0; |
1634 d->transfers[line].seqno = 0; |
1635 } |
1635 } |
1636 else { |
1636 else { |
1637 if (SeqNo == (d->transfers[line].seqno + 1)) { |
1637 if (SeqNo == (d->transfers[line].seqno + 1)) { |
1638 d->transfers[line].seqno = SeqNo; |
1638 d->transfers[line].seqno = SeqNo; |
1639 /* Store the data in the transfert structure. */ |
1639 /* Store the data in the transfer structure. */ |
1640 err = SDOtoLine(d, line, 7, (*m).data + 1); |
1640 err = SDOtoLine(d, line, 7, (*m).data + 1); |
1641 if (err) { |
1641 if (err) { |
1642 failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR); |
1642 failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR); |
1643 return 0xFF; |
1643 return 0xFF; |
1644 } |
1644 } |
1662 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
1662 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
1663 return 0xFF; |
1663 return 0xFF; |
1664 } |
1664 } |
1665 RestartSDO_TIMER(line) |
1665 RestartSDO_TIMER(line) |
1666 NbBytesNoData = (m->data[0]>>2) & 0x07; |
1666 NbBytesNoData = (m->data[0]>>2) & 0x07; |
1667 /* Store the data in the transfert structure. */ |
1667 /* Store the data in the transfer structure. */ |
1668 err = SDOtoLine(d, line, 7-NbBytesNoData, d->transfers[line].tmpData + 1); |
1668 err = SDOtoLine(d, line, 7-NbBytesNoData, d->transfers[line].tmpData + 1); |
1669 if (err) { |
1669 if (err) { |
1670 failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR); |
1670 failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR); |
1671 return 0xFF; |
1671 return 0xFF; |
1672 } |
1672 } |
1734 d->transfers[line].seqno = 0; |
1734 d->transfers[line].seqno = 0; |
1735 } |
1735 } |
1736 else { |
1736 else { |
1737 if (SeqNo == (d->transfers[line].seqno + 1)) { |
1737 if (SeqNo == (d->transfers[line].seqno + 1)) { |
1738 d->transfers[line].seqno = SeqNo; |
1738 d->transfers[line].seqno = SeqNo; |
1739 /* Store the data in the transfert structure. */ |
1739 /* Store the data in the transfer structure. */ |
1740 err = SDOtoLine(d, line, 7, (*m).data + 1); |
1740 err = SDOtoLine(d, line, 7, (*m).data + 1); |
1741 if (err) { |
1741 if (err) { |
1742 failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR); |
1742 failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR); |
1743 return 0xFF; |
1743 return 0xFF; |
1744 } |
1744 } |
1760 MSG_ERR(0x1AAD, "SDO error block upload : Received wrong subcommand from node id ", nodeId); |
1760 MSG_ERR(0x1AAD, "SDO error block upload : Received wrong subcommand from node id ", nodeId); |
1761 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
1761 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
1762 return 0xFF; |
1762 return 0xFF; |
1763 } |
1763 } |
1764 NbBytesNoData = (m->data[0]>>2) & 0x07; |
1764 NbBytesNoData = (m->data[0]>>2) & 0x07; |
1765 /* Store the data in the transfert structure. */ |
1765 /* Store the data in the transfer structure. */ |
1766 err = SDOtoLine(d, line, 7-NbBytesNoData, d->transfers[line].tmpData + 1); |
1766 err = SDOtoLine(d, line, 7-NbBytesNoData, d->transfers[line].tmpData + 1); |
1767 if (err) { |
1767 if (err) { |
1768 failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR); |
1768 failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR); |
1769 return 0xFF; |
1769 return 0xFF; |
1770 } |
1770 } |
1907 d->transfers[line].count = count; |
1907 d->transfers[line].count = count; |
1908 d->transfers[line].dataType = dataType; |
1908 d->transfers[line].dataType = dataType; |
1909 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
1909 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
1910 { |
1910 { |
1911 UNS8* lineData = d->transfers[line].data; |
1911 UNS8* lineData = d->transfers[line].data; |
1912 if (count > SDO_MAX_LENGTH_TRANSFERT) |
1912 if (count > SDO_MAX_LENGTH_TRANSFER) |
1913 { |
1913 { |
1914 d->transfers[line].dynamicData = (UNS8*) malloc(count); |
1914 d->transfers[line].dynamicData = (UNS8*) malloc(count); |
1915 d->transfers[line].dynamicDataSize = count; |
1915 d->transfers[line].dynamicDataSize = count; |
1916 if (d->transfers[line].dynamicData == NULL) |
1916 if (d->transfers[line].dynamicData == NULL) |
1917 { |
1917 { |
1950 for (i = 0 ; i < 4 ; i++) |
1950 for (i = 0 ; i < 4 ; i++) |
1951 buf[i+4] = (UNS8)((count >> (i<<3))); /* i*8 */ |
1951 buf[i+4] = (UNS8)((count >> (i<<3))); /* i*8 */ |
1952 } |
1952 } |
1953 else { |
1953 else { |
1954 /* Send the SDO to the server. Initiate download, cs=1. */ |
1954 /* Send the SDO to the server. Initiate download, cs=1. */ |
1955 if (count <= 4) { /* Expedited transfert */ |
1955 if (count <= 4) { /* Expedited transfer */ |
1956 buf[0] = (UNS8)((1 << 5) | ((4 - count) << 2) | 3); |
1956 buf[0] = (UNS8)((1 << 5) | ((4 - count) << 2) | 3); |
1957 for (i = 4 ; i < 8 ; i++) |
1957 for (i = 4 ; i < 8 ; i++) |
1958 buf[i] = d->transfers[line].data[i - 4]; |
1958 buf[i] = d->transfers[line].data[i - 4]; |
1959 d->transfers[line].offset = count; |
1959 d->transfers[line].offset = count; |
1960 } |
1960 } |
1961 else { /** Normal transfert */ |
1961 else { /** Normal transfer */ |
1962 buf[0] = (1 << 5) | 1; |
1962 buf[0] = (1 << 5) | 1; |
1963 for (i = 0 ; i < 4 ; i++) |
1963 for (i = 0 ; i < 4 ; i++) |
1964 buf[i+4] = (UNS8)((count >> (i<<3))); /* i*8 */ |
1964 buf[i+4] = (UNS8)((count >> (i<<3))); /* i*8 */ |
1965 } |
1965 } |
1966 } |
1966 } |
2265 MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId); |
2265 MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId); |
2266 *size = 0; |
2266 *size = 0; |
2267 return SDO_ABORTED_INTERNAL; |
2267 return SDO_ABORTED_INTERNAL; |
2268 } |
2268 } |
2269 |
2269 |
2270 /* If transfert not finished just return, but if aborted set abort code and size to 0 */ |
2270 /* If transfer not finished just return, but if aborted set abort code and size to 0 */ |
2271 if (d->transfers[line].state != SDO_FINISHED) { |
2271 if (d->transfers[line].state != SDO_FINISHED) { |
2272 if((d->transfers[line].state == SDO_ABORTED_RCV) || (d->transfers[line].state == SDO_ABORTED_INTERNAL)) { |
2272 if((d->transfers[line].state == SDO_ABORTED_RCV) || (d->transfers[line].state == SDO_ABORTED_INTERNAL)) { |
2273 *abortCode = d->transfers[line].abortCode; |
2273 *abortCode = d->transfers[line].abortCode; |
2274 *size = 0; |
2274 *size = 0; |
2275 } |
2275 } |