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, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize); |
65 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize); |
66 |
66 |
67 /*! |
67 /*! |
68 ** Called by readNetworkDict |
68 ** Called by readNetworkDict |
69 ** |
69 ** |
70 ** @param d |
70 ** @param d |
182 ** |
182 ** |
183 ** @return |
183 ** @return |
184 **/ |
184 **/ |
185 UNS32 SDOlineToObjdict (CO_Data* d, UNS8 line) |
185 UNS32 SDOlineToObjdict (CO_Data* d, UNS8 line) |
186 { |
186 { |
187 UNS8 size; |
187 UNS32 size; |
188 UNS32 errorCode; |
188 UNS32 errorCode; |
189 MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line); |
189 MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line); |
190 size = (UNS8)d->transfers[line].count; |
190 size = d->transfers[line].count; |
191 errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, |
191 errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, |
192 (void *) d->transfers[line].data, &size, 1); |
192 (void *) d->transfers[line].data, &size, 1); |
193 if (errorCode != OD_SUCCESSFUL) |
193 if (errorCode != OD_SUCCESSFUL) |
194 return errorCode; |
194 return errorCode; |
195 MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line); |
195 MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line); |
205 ** |
205 ** |
206 ** @return |
206 ** @return |
207 **/ |
207 **/ |
208 UNS32 objdictToSDOline (CO_Data* d, UNS8 line) |
208 UNS32 objdictToSDOline (CO_Data* d, UNS8 line) |
209 { |
209 { |
210 UNS8 size = 0; |
210 UNS32 size = 0; |
211 UNS8 dataType; |
211 UNS8 dataType; |
212 UNS32 errorCode; |
212 UNS32 errorCode; |
213 |
213 |
214 MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index); |
214 MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index); |
215 MSG_WAR(0x3A06, " subIndex : ", d->transfers[line].subIndex); |
215 MSG_WAR(0x3A06, " subIndex : ", d->transfers[line].subIndex); |
236 ** @param nbBytes |
236 ** @param nbBytes |
237 ** @param data |
237 ** @param data |
238 ** |
238 ** |
239 ** @return |
239 ** @return |
240 **/ |
240 **/ |
241 UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data) { |
241 UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS32 nbBytes, UNS8* data) { |
242 UNS8 i; |
242 UNS8 i; |
243 UNS8 offset; |
243 UNS32 offset; |
244 |
244 |
245 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { |
245 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { |
246 MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
246 MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
247 return 0xFF; |
247 return 0xFF; |
248 } |
248 } |
249 if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) { |
249 if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) { |
250 MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes); |
250 MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes); |
251 return 0xFF; |
251 return 0xFF; |
252 } |
252 } |
253 offset = (UNS8)d->transfers[line].offset; |
253 offset = d->transfers[line].offset; |
254 for (i = 0 ; i < nbBytes ; i++) |
254 for (i = 0 ; i < nbBytes ; i++) |
255 * (data + i) = d->transfers[line].data[offset + i]; |
255 * (data + i) = d->transfers[line].data[offset + i]; |
256 d->transfers[line].offset = d->transfers[line].offset + nbBytes; |
256 d->transfers[line].offset = d->transfers[line].offset + nbBytes; |
257 return 0; |
257 return 0; |
258 } |
258 } |
265 ** @param nbBytes |
265 ** @param nbBytes |
266 ** @param data |
266 ** @param data |
267 ** |
267 ** |
268 ** @return |
268 ** @return |
269 **/ |
269 **/ |
270 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data) |
270 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS32 nbBytes, UNS8* data) |
271 { |
271 { |
272 UNS8 i; |
272 UNS8 i; |
273 UNS8 offset; |
273 UNS32 offset; |
274 |
274 |
275 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { |
275 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { |
276 MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
276 MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
277 return 0xFF; |
277 return 0xFF; |
278 } |
278 } |
279 offset = (UNS8)d->transfers[line].offset; |
279 offset = d->transfers[line].offset; |
280 for (i = 0 ; i < nbBytes ; i++) |
280 for (i = 0 ; i < nbBytes ; i++) |
281 d->transfers[line].data[offset + i] = * (data + i); |
281 d->transfers[line].data[offset + i] = * (data + i); |
282 d->transfers[line].offset = d->transfers[line].offset + nbBytes; |
282 d->transfers[line].offset = d->transfers[line].offset + nbBytes; |
|
283 d->transfers[line].count = d->transfers[line].offset; |
|
284 |
283 return 0; |
285 return 0; |
284 } |
286 } |
285 |
287 |
286 /*! |
288 /*! |
287 ** |
289 ** |
326 ** @param d |
328 ** @param d |
327 ** @param line |
329 ** @param line |
328 **/ |
330 **/ |
329 void resetSDOline ( CO_Data* d, UNS8 line ) |
331 void resetSDOline ( CO_Data* d, UNS8 line ) |
330 { |
332 { |
331 UNS8 i; |
333 UNS32 i; |
332 MSG_WAR(0x3A25, "reset SDO line nb : ", line); |
334 MSG_WAR(0x3A25, "reset SDO line nb : ", line); |
333 initSDOline(d, line, 0, 0, 0, SDO_RESET); |
335 initSDOline(d, line, 0, 0, 0, SDO_RESET); |
334 for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++) |
336 for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++) |
335 d->transfers[line].data[i] = 0; |
337 d->transfers[line].data[i] = 0; |
336 d->transfers[line].whoami = 0; |
338 d->transfers[line].whoami = 0; |
467 ** @param line |
470 ** @param line |
468 ** @param nbBytes |
471 ** @param nbBytes |
469 ** |
472 ** |
470 ** @return |
473 ** @return |
471 **/ |
474 **/ |
472 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 nbBytes) |
475 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS32 nbBytes) |
473 { |
476 { |
474 if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) { |
477 if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) { |
475 MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
478 MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); |
476 return 0xFF; |
479 return 0xFF; |
477 } |
480 } |
611 **/ |
614 **/ |
612 UNS8 proceedSDO (CO_Data* d, Message *m) |
615 UNS8 proceedSDO (CO_Data* d, Message *m) |
613 { |
616 { |
614 UNS8 err; |
617 UNS8 err; |
615 UNS8 line; |
618 UNS8 line; |
616 UNS8 nbBytes; /* received or to be transmited. */ |
619 UNS32 nbBytes; /* received or to be transmited. */ |
617 UNS8 nodeId = 0; /* The node from which the SDO is received */ |
620 UNS8 nodeId = 0; /* The node from which the SDO is received */ |
618 UNS8 *pNodeId = NULL; |
621 UNS8 *pNodeId = NULL; |
619 UNS8 whoami = SDO_UNKNOWN; /* SDO_SERVER or SDO_CLIENT.*/ |
622 UNS8 whoami = SDO_UNKNOWN; /* SDO_SERVER or SDO_CLIENT.*/ |
620 UNS32 errorCode; /* while reading or writing in the local object dictionary.*/ |
623 UNS32 errorCode; /* while reading or writing in the local object dictionary.*/ |
621 s_SDO sdo; /* SDO to transmit */ |
624 s_SDO sdo; /* SDO to transmit */ |
862 /* Release of the line. */ |
865 /* Release of the line. */ |
863 resetSDOline(d, line); |
866 resetSDOline(d, line); |
864 } |
867 } |
865 else {/* So, if it is not an expedited transfert */ |
868 else {/* So, if it is not an expedited transfert */ |
866 if (getSDOs(m->data[0])) { |
869 if (getSDOs(m->data[0])) { |
867 /* TODO : if e and s = 0, not reading m->data[4] but put nbBytes = 0 */ |
870 nbBytes = 0; |
868 nbBytes = m->data[4]; /* Transfert limited to 255 bytes. */ |
|
869 err = setSDOlineRestBytes(d, nodeId, nbBytes); |
871 err = setSDOlineRestBytes(d, nodeId, nbBytes); |
870 if (err) { |
872 if (err) { |
871 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
873 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
872 return 0xFF; |
874 return 0xFF; |
873 } |
875 } |
1246 ** @param endianize |
1248 ** @param endianize |
1247 ** |
1249 ** |
1248 ** @return |
1250 ** @return |
1249 **/ |
1251 **/ |
1250 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, |
1252 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, |
1251 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize) |
1253 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize) |
1252 { |
1254 { |
1253 UNS8 err; |
1255 UNS8 err; |
1254 UNS8 SDOfound = 0; |
1256 UNS8 SDOfound = 0; |
1255 UNS8 line; |
1257 UNS8 line; |
1256 s_SDO sdo; /* SDO to transmit */ |
1258 s_SDO sdo; /* SDO to transmit */ |
1257 UNS8 i, j; |
1259 UNS8 i; |
|
1260 UNS32 j; |
1258 UNS16 lastIndex; |
1261 UNS16 lastIndex; |
1259 UNS16 offset; |
1262 UNS16 offset; |
1260 UNS8 *pNodeIdServer; |
1263 UNS8 *pNodeIdServer; |
1261 UNS8 nodeIdServer; |
1264 UNS8 nodeIdServer; |
1262 |
1265 |
1331 sdo.body.data[i] = d->transfers[line].data[i - 4]; |
1334 sdo.body.data[i] = d->transfers[line].data[i - 4]; |
1332 d->transfers[line].offset = count; |
1335 d->transfers[line].offset = count; |
1333 } |
1336 } |
1334 else { /** Normal transfert */ |
1337 else { /** Normal transfert */ |
1335 sdo.body.data[0] = (1 << 5) | 1; |
1338 sdo.body.data[0] = (1 << 5) | 1; |
1336 sdo.body.data[4] = count; /* nb of byte to transmit. Max = 255. (canfestival2 limitation). */ |
1339 for (i = 4 ; i < 8 ; i++) |
1337 for (i = 5 ; i < 8 ; i++) |
1340 sdo.body.data[i] = 0; |
1338 sdo.body.data[i] = 0; |
|
1339 } |
1341 } |
1340 sdo.body.data[1] = index & 0xFF; /* LSB */ |
1342 sdo.body.data[1] = index & 0xFF; /* LSB */ |
1341 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ |
1343 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ |
1342 sdo.body.data[3] = subIndex; |
1344 sdo.body.data[3] = subIndex; |
1343 |
1345 |
1367 ** @param data |
1369 ** @param data |
1368 ** |
1370 ** |
1369 ** @return |
1371 ** @return |
1370 **/ |
1372 **/ |
1371 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, |
1373 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, |
1372 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data) |
1374 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data) |
1373 { |
1375 { |
1374 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, NULL, 1); |
1376 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, NULL, 1); |
1375 } |
1377 } |
1376 |
1378 |
1377 /*! |
1379 /*! |
1387 ** @param Callback |
1389 ** @param Callback |
1388 ** |
1390 ** |
1389 ** @return |
1391 ** @return |
1390 **/ |
1392 **/ |
1391 UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index, |
1393 UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index, |
1392 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback) |
1394 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback) |
1393 { |
1395 { |
1394 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1); |
1396 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1); |
1395 } |
1397 } |
1396 |
1398 |
1397 UNS8 writeNetworkDictCallBackAI (CO_Data* d, UNS8 nodeId, UNS16 index, |
1399 UNS8 writeNetworkDictCallBackAI (CO_Data* d, UNS8 nodeId, UNS16 index, |
1398 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize) |
1400 UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize) |
1399 { |
1401 { |
1400 UNS8 ret; |
1402 UNS8 ret; |
1401 UNS16 lastIndex; |
1403 UNS16 lastIndex; |
1402 UNS16 offset; |
1404 UNS16 offset; |
1403 UNS8 nodeIdServer; |
1405 UNS8 nodeIdServer; |
1627 ** @param size pointer to expected size, changed into returned size. Expected size will be truncated to transfered data size |
1629 ** @param size pointer to expected size, changed into returned size. Expected size will be truncated to transfered data size |
1628 ** @param abortCode |
1630 ** @param abortCode |
1629 ** |
1631 ** |
1630 ** @return |
1632 ** @return |
1631 **/ |
1633 **/ |
1632 UNS8 getReadResultNetworkDict (CO_Data* d, UNS8 nodeId, void* data, UNS8 *size, |
1634 UNS8 getReadResultNetworkDict (CO_Data* d, UNS8 nodeId, void* data, UNS32 *size, |
1633 UNS32 * abortCode) |
1635 UNS32 * abortCode) |
1634 { |
1636 { |
1635 UNS8 i; |
1637 UNS32 i; |
1636 UNS8 err; |
1638 UNS8 err; |
1637 UNS8 line; |
1639 UNS8 line; |
1638 * abortCode = 0; |
1640 * abortCode = 0; |
1639 |
1641 |
1640 /* Looking for the line tranfert. */ |
1642 /* Looking for the line tranfert. */ |
1647 if (d->transfers[line].state != SDO_FINISHED) |
1649 if (d->transfers[line].state != SDO_FINISHED) |
1648 return d->transfers[line].state; |
1650 return d->transfers[line].state; |
1649 |
1651 |
1650 /* Transfert is finished. Put the value in the data. */ |
1652 /* Transfert is finished. Put the value in the data. */ |
1651 /* use transfers[line].count as max size */ |
1653 /* use transfers[line].count as max size */ |
1652 if( (UNS8)d->transfers[line].count < *size ) |
1654 if( d->transfers[line].count < *size ) |
1653 *size = (UNS8)d->transfers[line].count; |
1655 *size = d->transfers[line].count; |
1654 for ( i = 0 ; i < *size ; i++) { |
1656 for ( i = 0 ; i < *size ; i++) { |
1655 # ifdef CANOPEN_BIG_ENDIAN |
1657 # ifdef CANOPEN_BIG_ENDIAN |
1656 if (d->transfers[line].dataType != visible_string) |
1658 if (d->transfers[line].dataType != visible_string) |
1657 ( (char *) data)[*size - 1 - i] = d->transfers[line].data[i]; |
1659 ( (char *) data)[*size - 1 - i] = d->transfers[line].data[i]; |
1658 else /* String of bytes. */ |
1660 else /* String of bytes. */ |