189 MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line); |
189 MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line); |
190 /* if SDO initiated with e=0 and s=0 count is null, offset carry effective size*/ |
190 /* if SDO initiated with e=0 and s=0 count is null, offset carry effective size*/ |
191 if( d->transfers[line].count == 0) |
191 if( d->transfers[line].count == 0) |
192 d->transfers[line].count = d->transfers[line].offset; |
192 d->transfers[line].count = d->transfers[line].offset; |
193 size = d->transfers[line].count; |
193 size = d->transfers[line].count; |
|
194 |
|
195 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
|
196 if (size > SDO_MAX_LENGTH_TRANSFERT) |
|
197 { |
|
198 errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, |
|
199 (void *) d->transfers[line].dynamicData, &size, 1); |
|
200 } |
|
201 else |
|
202 { |
|
203 errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, |
|
204 (void *) d->transfers[line].data, &size, 1); |
|
205 } |
|
206 #else //SDO_DYNAMIC_BUFFER_ALLOCATION |
194 errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, |
207 errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, |
195 (void *) d->transfers[line].data, &size, 1); |
208 (void *) d->transfers[line].data, &size, 1); |
|
209 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
|
210 |
196 if (errorCode != OD_SUCCESSFUL) |
211 if (errorCode != OD_SUCCESSFUL) |
197 return errorCode; |
212 return errorCode; |
198 MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line); |
213 MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line); |
199 return 0; |
214 return 0; |
200 |
215 |
215 UNS32 errorCode; |
230 UNS32 errorCode; |
216 |
231 |
217 MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index); |
232 MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index); |
218 MSG_WAR(0x3A06, " subIndex : ", d->transfers[line].subIndex); |
233 MSG_WAR(0x3A06, " subIndex : ", d->transfers[line].subIndex); |
219 |
234 |
|
235 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
|
236 //TODO: Read the size of the object. Depending o it put data into data or dynamicData |
220 errorCode = getODentry(d, d->transfers[line].index, |
237 errorCode = getODentry(d, d->transfers[line].index, |
221 d->transfers[line].subIndex, |
238 d->transfers[line].subIndex, |
222 (void *)d->transfers[line].data, |
239 (void *)d->transfers[line].data, |
223 &size, &dataType, 1); |
240 &size, &dataType, 1); |
|
241 #else //SDO_DYNAMIC_BUFFER_ALLOCATION |
|
242 errorCode = getODentry(d, d->transfers[line].index, |
|
243 d->transfers[line].subIndex, |
|
244 (void *)d->transfers[line].data, |
|
245 &size, &dataType, 1); |
|
246 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
224 |
247 |
225 if (errorCode != OD_SUCCESSFUL) |
248 if (errorCode != OD_SUCCESSFUL) |
226 return errorCode; |
249 return errorCode; |
227 |
250 |
228 d->transfers[line].count = size; |
251 d->transfers[line].count = size; |
243 **/ |
266 **/ |
244 UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS32 nbBytes, UNS8* data) { |
267 UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS32 nbBytes, UNS8* data) { |
245 UNS8 i; |
268 UNS8 i; |
246 UNS32 offset; |
269 UNS32 offset; |
247 |
270 |
|
271 #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION |
248 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { |
272 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { |
249 MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
273 MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
250 return 0xFF; |
274 return 0xFF; |
251 } |
275 } |
|
276 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
|
277 |
252 if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) { |
278 if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) { |
253 MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes); |
279 MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes); |
254 return 0xFF; |
280 return 0xFF; |
255 } |
281 } |
256 offset = d->transfers[line].offset; |
282 offset = d->transfers[line].offset; |
257 for (i = 0 ; i < nbBytes ; i++) |
283 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
|
284 if (d->transfers[line].count <= SDO_MAX_LENGTH_TRANSFERT) |
|
285 { |
|
286 for (i = 0 ; i < nbBytes ; i++) |
|
287 * (data + i) = d->transfers[line].data[offset + i]; |
|
288 } |
|
289 else |
|
290 { |
|
291 if (d->transfers[line].dynamicData == NULL) |
|
292 { |
|
293 MSG_ERR(0x1A11,"SDO's dynamic buffer not allocated. Line", line); |
|
294 return 0xFF; |
|
295 } |
|
296 for (i = 0 ; i < nbBytes ; i++) |
|
297 * (data + i) = d->transfers[line].dynamicData[offset + i]; |
|
298 } |
|
299 #else //SDO_DYNAMIC_BUFFER_ALLOCATION |
|
300 for (i = 0 ; i < nbBytes ; i++) |
258 * (data + i) = d->transfers[line].data[offset + i]; |
301 * (data + i) = d->transfers[line].data[offset + i]; |
|
302 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
259 d->transfers[line].offset = d->transfers[line].offset + nbBytes; |
303 d->transfers[line].offset = d->transfers[line].offset + nbBytes; |
260 return 0; |
304 return 0; |
261 } |
305 } |
262 |
306 |
263 /*! |
307 /*! |
272 **/ |
316 **/ |
273 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS32 nbBytes, UNS8* data) |
317 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS32 nbBytes, UNS8* data) |
274 { |
318 { |
275 UNS8 i; |
319 UNS8 i; |
276 UNS32 offset; |
320 UNS32 offset; |
277 |
321 #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION |
278 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { |
322 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { |
279 MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
323 MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
280 return 0xFF; |
324 return 0xFF; |
281 } |
325 } |
|
326 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
|
327 |
282 offset = d->transfers[line].offset; |
328 offset = d->transfers[line].offset; |
|
329 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
|
330 { |
|
331 UNS8* lineData = d->transfers[line].data; |
|
332 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { |
|
333 if (d->transfers[line].dynamicData == NULL) { |
|
334 d->transfers[line].dynamicData = (UNS8*) malloc(SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE); |
|
335 d->transfers[line].dynamicDataSize = SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE; |
|
336 |
|
337 if (d->transfers[line].dynamicData == NULL) { |
|
338 MSG_ERR(0x1A15,"SDO allocating dynamic buffer failed, size", SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE); |
|
339 return 0xFF; |
|
340 } |
|
341 //Copy present data |
|
342 memcpy(d->transfers[line].dynamicData, d->transfers[line].data, offset); |
|
343 } |
|
344 else if ((d->transfers[line].offset + nbBytes) > d->transfers[line].dynamicDataSize) |
|
345 { |
|
346 UNS8* newDynamicBuffer = (UNS8*) realloc(d->transfers[line].dynamicData, d->transfers[line].dynamicDataSize + SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE); |
|
347 if (newDynamicBuffer == NULL) { |
|
348 MSG_ERR(0x1A15,"SDO reallocating dynamic buffer failed, size", d->transfers[line].dynamicDataSize + SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE); |
|
349 return 0xFF; |
|
350 } |
|
351 d->transfers[line].dynamicData = newDynamicBuffer; |
|
352 d->transfers[line].dynamicDataSize += SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE; |
|
353 } |
|
354 lineData = d->transfers[line].dynamicData; |
|
355 } |
|
356 |
|
357 for (i = 0 ; i < nbBytes ; i++) |
|
358 lineData[offset + i] = * (data + i); |
|
359 } |
|
360 #else //SDO_DYNAMIC_BUFFER_ALLOCATION |
283 for (i = 0 ; i < nbBytes ; i++) |
361 for (i = 0 ; i < nbBytes ; i++) |
284 d->transfers[line].data[offset + i] = * (data + i); |
362 d->transfers[line].data[offset + i] = * (data + i); |
|
363 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
|
364 |
285 d->transfers[line].offset = d->transfers[line].offset + nbBytes; |
365 d->transfers[line].offset = d->transfers[line].offset + nbBytes; |
286 return 0; |
366 return 0; |
287 } |
367 } |
288 |
368 |
289 /*! |
369 /*! |
336 initSDOline(d, line, 0, 0, 0, SDO_RESET); |
416 initSDOline(d, line, 0, 0, 0, SDO_RESET); |
337 for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++) |
417 for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++) |
338 d->transfers[line].data[i] = 0; |
418 d->transfers[line].data[i] = 0; |
339 d->transfers[line].whoami = 0; |
419 d->transfers[line].whoami = 0; |
340 d->transfers[line].abortCode = 0; |
420 d->transfers[line].abortCode = 0; |
|
421 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
|
422 free(d->transfers[line].dynamicData); |
|
423 d->transfers[line].dynamicData = 0; |
|
424 d->transfers[line].dynamicDataSize = 0; |
|
425 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
341 } |
426 } |
342 |
427 |
343 /*! |
428 /*! |
344 ** |
429 ** |
345 ** |
430 ** |
367 d->transfers[line].toggle = 0; |
452 d->transfers[line].toggle = 0; |
368 d->transfers[line].count = 0; |
453 d->transfers[line].count = 0; |
369 d->transfers[line].offset = 0; |
454 d->transfers[line].offset = 0; |
370 d->transfers[line].dataType = 0; |
455 d->transfers[line].dataType = 0; |
371 d->transfers[line].Callback = NULL; |
456 d->transfers[line].Callback = NULL; |
|
457 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
|
458 free(d->transfers[line].dynamicData); |
|
459 d->transfers[line].dynamicData = 0; |
|
460 d->transfers[line].dynamicDataSize = 0; |
|
461 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
372 return 0; |
462 return 0; |
373 } |
463 } |
374 |
464 |
375 /*! |
465 /*! |
376 ** |
466 ** |
474 ** |
564 ** |
475 ** @return |
565 ** @return |
476 **/ |
566 **/ |
477 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS32 nbBytes) |
567 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS32 nbBytes) |
478 { |
568 { |
|
569 #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION |
479 if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) { |
570 if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) { |
480 MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
571 MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
481 return 0xFF; |
572 return 0xFF; |
482 } |
573 } |
|
574 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
|
575 |
483 d->transfers[line].count = nbBytes; |
576 d->transfers[line].count = nbBytes; |
484 return 0; |
577 return 0; |
485 } |
578 } |
486 |
579 |
487 /*! |
580 /*! |
1313 MSG_WAR(0x3AD0," SDO client defined at index : ", 0x1280 + i); |
1406 MSG_WAR(0x3AD0," SDO client defined at index : ", 0x1280 + i); |
1314 initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS); |
1407 initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS); |
1315 d->transfers[line].count = count; |
1408 d->transfers[line].count = count; |
1316 d->transfers[line].dataType = dataType; |
1409 d->transfers[line].dataType = dataType; |
1317 |
1410 |
|
1411 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
|
1412 { |
|
1413 UNS8* lineData = d->transfers[line].data; |
|
1414 if (count > SDO_MAX_LENGTH_TRANSFERT) |
|
1415 { |
|
1416 d->transfers[line].dynamicData = (UNS8*) malloc(count); |
|
1417 d->transfers[line].dynamicDataSize = count; |
|
1418 if (d->transfers[line].dynamicData == NULL) |
|
1419 { |
|
1420 MSG_ERR(0x1AC9, "SDO. Error. Could not allocate enough bytes : ", count); |
|
1421 return 0xFE; |
|
1422 } |
|
1423 lineData = d->transfers[line].dynamicData; |
|
1424 } |
|
1425 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
|
1426 |
1318 /* Copy data to transfers structure. */ |
1427 /* Copy data to transfers structure. */ |
1319 for (j = 0 ; j < count ; j++) { |
1428 for (j = 0 ; j < count ; j++) { |
|
1429 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
|
1430 # ifdef CANOPEN_BIG_ENDIAN |
|
1431 if (dataType == 0 && endianize) |
|
1432 lineData[count - 1 - j] = ((char *)data)[j]; |
|
1433 else /* String of bytes. */ |
|
1434 lineData[j] = ((char *)data)[j]; |
|
1435 # else |
|
1436 lineData[j] = ((char *)data)[j]; |
|
1437 # endif |
|
1438 } |
|
1439 #else //SDO_DYNAMIC_BUFFER_ALLOCATION |
1320 # ifdef CANOPEN_BIG_ENDIAN |
1440 # ifdef CANOPEN_BIG_ENDIAN |
1321 if (dataType == 0 && endianize) |
1441 if (dataType == 0 && endianize) |
1322 d->transfers[line].data[count - 1 - j] = ((char *)data)[j]; |
1442 d->transfers[line].data[count - 1 - j] = ((char *)data)[j]; |
1323 else /* String of bytes. */ |
1443 else /* String of bytes. */ |
1324 d->transfers[line].data[j] = ((char *)data)[j]; |
1444 d->transfers[line].data[j] = ((char *)data)[j]; |
1325 # else |
1445 # else |
1326 d->transfers[line].data[j] = ((char *)data)[j]; |
1446 d->transfers[line].data[j] = ((char *)data)[j]; |
1327 # endif |
1447 # endif |
|
1448 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
1328 } |
1449 } |
1329 /* Send the SDO to the server. Initiate download, cs=1. */ |
1450 /* Send the SDO to the server. Initiate download, cs=1. */ |
1330 sdo.nodeId = nodeId; |
1451 sdo.nodeId = nodeId; |
1331 if (count <= 4) { /* Expedited transfert */ |
1452 if (count <= 4) { /* Expedited transfert */ |
1332 sdo.body.data[0] = (UNS8)((1 << 5) | ((4 - count) << 2) | 3); |
1453 sdo.body.data[0] = (UNS8)((1 << 5) | ((4 - count) << 2) | 3); |
1653 if( d->transfers[line].count == 0) |
1774 if( d->transfers[line].count == 0) |
1654 d->transfers[line].count = d->transfers[line].offset; |
1775 d->transfers[line].count = d->transfers[line].offset; |
1655 /* use transfers[line].count as max size */ |
1776 /* use transfers[line].count as max size */ |
1656 if( d->transfers[line].count < *size ) |
1777 if( d->transfers[line].count < *size ) |
1657 *size = d->transfers[line].count; |
1778 *size = d->transfers[line].count; |
|
1779 |
1658 /* Copy payload to data pointer */ |
1780 /* Copy payload to data pointer */ |
|
1781 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION |
|
1782 { |
|
1783 UNS8 *lineData = d->transfers[line].data; |
|
1784 |
|
1785 if (d->transfers[line].dynamicData && d->transfers[line].dynamicDataSize) |
|
1786 { |
|
1787 lineData = d->transfers[line].dynamicData; |
|
1788 } |
|
1789 for ( i = 0 ; i < *size ; i++) { |
|
1790 # ifdef CANOPEN_BIG_ENDIAN |
|
1791 if (d->transfers[line].dataType != visible_string) |
|
1792 ( (char *) data)[*size - 1 - i] = lineData[i]; |
|
1793 else /* String of bytes. */ |
|
1794 ( (char *) data)[i] = lineData[i]; |
|
1795 # else |
|
1796 ( (char *) data)[i] = lineData[i]; |
|
1797 # endif |
|
1798 } |
|
1799 } |
|
1800 #else //SDO_DYNAMIC_BUFFER_ALLOCATION |
1659 for ( i = 0 ; i < *size ; i++) { |
1801 for ( i = 0 ; i < *size ; i++) { |
1660 # ifdef CANOPEN_BIG_ENDIAN |
1802 # ifdef CANOPEN_BIG_ENDIAN |
1661 if (d->transfers[line].dataType != visible_string) |
1803 if (d->transfers[line].dataType != visible_string) |
1662 ( (char *) data)[*size - 1 - i] = d->transfers[line].data[i]; |
1804 ( (char *) data)[*size - 1 - i] = d->transfers[line].data[i]; |
1663 else /* String of bytes. */ |
1805 else /* String of bytes. */ |
1664 ( (char *) data)[i] = d->transfers[line].data[i]; |
1806 ( (char *) data)[i] = d->transfers[line].data[i]; |
1665 # else |
1807 # else |
1666 ( (char *) data)[i] = d->transfers[line].data[i]; |
1808 ( (char *) data)[i] = d->transfers[line].data[i]; |
1667 # endif |
1809 # endif |
1668 } |
1810 } |
|
1811 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION |
1669 return SDO_FINISHED; |
1812 return SDO_FINISHED; |
1670 } |
1813 } |
1671 |
1814 |
1672 /*! |
1815 /*! |
1673 ** |
1816 ** |