diff -r 003cc3c63855 -r f49e5a6b7804 doc/doxygen/html/sdo_8c-source.html --- a/doc/doxygen/html/sdo_8c-source.html Fri Jun 08 09:02:43 2007 +0200 +++ b/doc/doxygen/html/sdo_8c-source.html Fri Jun 08 09:23:56 2007 +0200 @@ -1,6 +1,6 @@
-00001 /* + +sdo.c
Go to the documentation of this file.00001 /* 00002 This file is part of CanFestival, a library implementing CanOpen Stack. 00003 00004 Copyright (C): Edouard TISSERANT and Francis DUPIN @@ -39,9 +43,9 @@ 00032 /* #define DEBUG_WAR_CONSOLE_ON */ 00033 /* #define DEBUG_ERR_CONSOLE_ON */ 00034 -00035 #include "objacces.h" -00036 #include "sdo.h" -00037 #include "canfestival.h" +00035 #include "objacces.h" +00036 #include "sdo.h" +00037 #include "canfestival.h" 00038 00039 /* Uncomment if your compiler does not support inline functions */ 00040 #define NO_INLINE @@ -54,11 +58,11 @@ 00047 00048 /*Internals prototypes*/ 00049 -00065 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, -00066 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize); +00065 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, +00066 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize); 00067 -00080 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, -00081 UNS8 dataType, SDOCallback_t Callback); +00080 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, +00081 UNS8 dataType, SDOCallback_t Callback); 00082 00083 00084 /***************************************************************************/ @@ -82,27 +86,27 @@ 00118 00121 #define getSDOsubIndex(byte3) (byte3) 00122 -00129 void SDOTimeoutAlarm(CO_Data* d, UNS32 id) +00129 void SDOTimeoutAlarm(CO_Data* d, UNS32 id) 00130 { -00131 MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0); -00132 MSG_WAR(0x2A02, "server node : ", d->transfers[id].nodeId); -00133 MSG_WAR(0x2A02, " index : ", d->transfers[id].index); -00134 MSG_WAR(0x2A02, " subIndex : ", d->transfers[id].subIndex); +00131 MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0); +00132 MSG_WAR(0x2A02, "server node : ", d->transfers[id].nodeId); +00133 MSG_WAR(0x2A02, " index : ", d->transfers[id].index); +00134 MSG_WAR(0x2A02, " subIndex : ", d->transfers[id].subIndex); 00135 /* Reset timer handler */ -00136 d->transfers[id].timer = TIMER_NONE; +00136 d->transfers[id].timer = TIMER_NONE; 00137 /*Set aborted state*/ -00138 d->transfers[id].state = SDO_ABORTED_INTERNAL; +00138 d->transfers[id].state = SDO_ABORTED_INTERNAL; 00139 /* Sending a SDO abort */ -00140 sendSDOabort(d, d->transfers[id].whoami, -00141 d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT); -00142 d->transfers[id].abortCode = SDOABT_TIMED_OUT; +00140 sendSDOabort(d, d->transfers[id].whoami, +00141 d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT); +00142 d->transfers[id].abortCode = SDOABT_TIMED_OUT; 00143 /* Call the user function to inform of the problem.*/ -00144 if(d->transfers[id].Callback) +00144 if(d->transfers[id].Callback) 00145 /*If ther is a callback, it is responsible to close SDO transfer (client)*/ -00146 (*d->transfers[id].Callback)(d,d->transfers[id].nodeId); -00147 else if(d->transfers[id].whoami == SDO_SERVER) +00146 (*d->transfers[id].Callback)(d,d->transfers[id].nodeId); +00147 else if(d->transfers[id].whoami == SDO_SERVER) 00148 /*Else, if server, reset the line*/ -00149 resetSDOline(d, (UNS8)id); +00149 resetSDOline(d, (UNS8)id); 00150 } 00151 00152 #define StopSDO_TIMER(id) \ @@ -117,174 +121,175 @@ 00161 MSG_WAR(0x3A07, "restartSDO_TIMER for line : ", line);\ 00162 if(d->transfers[id].timer != TIMER_NONE) { StopSDO_TIMER(id) StartSDO_TIMER(id) } 00163 -00169 void resetSDO (CO_Data* d) +00169 void resetSDO (CO_Data* d) 00170 { -00171 UNS8 j; +00171 UNS8 j; 00172 -00174 for (j = 0 ; j < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; j++) -00175 resetSDOline(d, j); +00173 /* transfer structure initialization */ +00174 for (j = 0 ; j < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; j++) +00175 resetSDOline(d, j); 00176 } 00177 -00186 UNS32 SDOlineToObjdict (CO_Data* d, UNS8 line) +00186 UNS32 SDOlineToObjdict (CO_Data* d, UNS8 line) 00187 { -00188 UNS8 size; -00189 UNS32 errorCode; -00190 MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line); -00191 size = (UNS8)d->transfers[line].count; -00192 errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, -00193 (void *) d->transfers[line].data, &size, 1); -00194 if (errorCode != OD_SUCCESSFUL) +00188 UNS8 size; +00189 UNS32 errorCode; +00190 MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line); +00191 size = (UNS8)d->transfers[line].count; +00192 errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, +00193 (void *) d->transfers[line].data, &size, 1); +00194 if (errorCode != OD_SUCCESSFUL) 00195 return errorCode; -00196 MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line); +00196 MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line); 00197 return 0; 00198 00199 } 00200 -00209 UNS32 objdictToSDOline (CO_Data* d, UNS8 line) +00209 UNS32 objdictToSDOline (CO_Data* d, UNS8 line) 00210 { -00211 UNS8 size = 0; -00212 UNS8 dataType; -00213 UNS32 errorCode; +00211 UNS8 size = 0; +00212 UNS8 dataType; +00213 UNS32 errorCode; 00214 -00215 MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index); -00216 MSG_WAR(0x3A06, " subIndex : ", d->transfers[line].subIndex); +00215 MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index); +00216 MSG_WAR(0x3A06, " subIndex : ", d->transfers[line].subIndex); 00217 -00218 errorCode = getODentry(d, d->transfers[line].index, -00219 d->transfers[line].subIndex, -00220 (void *)d->transfers[line].data, +00218 errorCode = getODentry(d, d->transfers[line].index, +00219 d->transfers[line].subIndex, +00220 (void *)d->transfers[line].data, 00221 &size, &dataType, 0); 00222 -00223 if (errorCode != OD_SUCCESSFUL) +00223 if (errorCode != OD_SUCCESSFUL) 00224 return errorCode; 00225 -00226 d->transfers[line].count = size; -00227 d->transfers[line].offset = 0; +00226 d->transfers[line].count = size; +00227 d->transfers[line].offset = 0; 00228 #if 0 00229 /*Me laisser a, please ! (FD)*/ 00230 { -00231 UNS8 i; +00231 UNS8 i; 00232 for (i = 0 ; i < 10 ; i++) { -00233 MSG_WAR(i, "data= ", d->transfers[line].data[i]); +00233 MSG_WAR(i, "data= ", d->transfers[line].data[i]); 00234 } 00235 } 00236 #endif 00237 return 0; 00238 } 00239 -00250 UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data) { -00251 UNS8 i; -00252 UNS8 offset; +00250 UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data) { +00251 UNS8 i; +00252 UNS8 offset; 00253 -00254 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { -00255 MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); +00254 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { +00255 MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); 00256 return 0xFF; 00257 } -00258 if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) { -00259 MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes); +00258 if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) { +00259 MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes); 00260 return 0xFF; 00261 } -00262 offset = (UNS8)d->transfers[line].offset; +00262 offset = (UNS8)d->transfers[line].offset; 00263 for (i = 0 ; i < nbBytes ; i++) -00264 * (data + i) = d->transfers[line].data[offset + i]; -00265 d->transfers[line].offset = d->transfers[line].offset + nbBytes; +00264 * (data + i) = d->transfers[line].data[offset + i]; +00265 d->transfers[line].offset = d->transfers[line].offset + nbBytes; 00266 return 0; 00267 } 00268 -00279 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data) +00279 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data) 00280 { -00281 UNS8 i; -00282 UNS8 offset; +00281 UNS8 i; +00282 UNS8 offset; 00283 -00284 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { -00285 MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); +00284 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { +00285 MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); 00286 return 0xFF; 00287 } -00288 offset = (UNS8)d->transfers[line].offset; +00288 offset = (UNS8)d->transfers[line].offset; 00289 for (i = 0 ; i < nbBytes ; i++) -00290 d->transfers[line].data[offset + i] = * (data + i); -00291 d->transfers[line].offset = d->transfers[line].offset + nbBytes; +00290 d->transfers[line].data[offset + i] = * (data + i); +00291 d->transfers[line].offset = d->transfers[line].offset + nbBytes; 00292 return 0; 00293 } 00294 -00307 UNS8 failedSDO (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS16 index, -00308 UNS8 subIndex, UNS32 abortCode) +00307 UNS8 failedSDO (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS16 index, +00308 UNS8 subIndex, UNS32 abortCode) 00309 { -00310 UNS8 err; -00311 UNS8 line; -00312 err = getSDOlineOnUse( d, nodeId, whoami, &line ); +00310 UNS8 err; +00311 UNS8 line; +00312 err = getSDOlineOnUse( d, nodeId, whoami, &line ); 00313 if (!err) /* If a line on use have been found.*/ -00314 MSG_WAR(0x3A20, "FailedSDO : line found : ", line); -00315 if ((! err) && (whoami == SDO_SERVER)) { -00316 resetSDOline( d, line ); -00317 MSG_WAR(0x3A21, "FailedSDO : line released : ", line); +00314 MSG_WAR(0x3A20, "FailedSDO : line found : ", line); +00315 if ((! err) && (whoami == SDO_SERVER)) { +00316 resetSDOline( d, line ); +00317 MSG_WAR(0x3A21, "FailedSDO : line released : ", line); 00318 } -00319 if ((! err) && (whoami == SDO_CLIENT)) { +00319 if ((! err) && (whoami == SDO_CLIENT)) { 00320 StopSDO_TIMER(line); -00321 d->transfers[line].state = SDO_ABORTED_INTERNAL; +00321 d->transfers[line].state = SDO_ABORTED_INTERNAL; 00322 } -00323 MSG_WAR(0x3A22, "Sending SDO abort ", 0); -00324 err = sendSDOabort(d, whoami, index, subIndex, abortCode); +00323 MSG_WAR(0x3A22, "Sending SDO abort ", 0); +00324 err = sendSDOabort(d, whoami, index, subIndex, abortCode); 00325 if (err) { -00326 MSG_WAR(0x3A23, "Unable to send the SDO abort", 0); +00326 MSG_WAR(0x3A23, "Unable to send the SDO abort", 0); 00327 return 0xFF; 00328 } 00329 return 0; 00330 } 00331 -00338 void resetSDOline ( CO_Data* d, UNS8 line ) +00338 void resetSDOline ( CO_Data* d, UNS8 line ) 00339 { -00340 UNS8 i; -00341 MSG_WAR(0x3A25, "reset SDO line nb : ", line); -00342 initSDOline(d, line, 0, 0, 0, SDO_RESET); -00343 for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++) -00344 d->transfers[line].data[i] = 0; +00340 UNS8 i; +00341 MSG_WAR(0x3A25, "reset SDO line nb : ", line); +00342 initSDOline(d, line, 0, 0, 0, SDO_RESET); +00343 for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++) +00344 d->transfers[line].data[i] = 0; 00345 } 00346 -00359 UNS8 initSDOline (CO_Data* d, UNS8 line, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 state) +00359 UNS8 initSDOline (CO_Data* d, UNS8 line, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 state) 00360 { -00361 MSG_WAR(0x3A25, "init SDO line nb : ", line); -00362 if (state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS){ +00361 MSG_WAR(0x3A25, "init SDO line nb : ", line); +00362 if (state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS){ 00363 StartSDO_TIMER(line) 00364 }else{ 00365 StopSDO_TIMER(line) 00366 } -00367 d->transfers[line].nodeId = nodeId; -00368 d->transfers[line].index = index; -00369 d->transfers[line].subIndex = subIndex; -00370 d->transfers[line].state = state; -00371 d->transfers[line].toggle = 0; -00372 d->transfers[line].count = 0; -00373 d->transfers[line].offset = 0; -00374 d->transfers[line].dataType = 0; -00375 d->transfers[line].Callback = NULL; +00367 d->transfers[line].nodeId = nodeId; +00368 d->transfers[line].index = index; +00369 d->transfers[line].subIndex = subIndex; +00370 d->transfers[line].state = state; +00371 d->transfers[line].toggle = 0; +00372 d->transfers[line].count = 0; +00373 d->transfers[line].offset = 0; +00374 d->transfers[line].dataType = 0; +00375 d->transfers[line].Callback = NULL; 00376 return 0; 00377 } 00378 -00388 UNS8 getSDOfreeLine ( CO_Data* d, UNS8 whoami, UNS8 *line ) +00388 UNS8 getSDOfreeLine ( CO_Data* d, UNS8 whoami, UNS8 *line ) 00389 { 00390 -00391 UNS8 i; +00391 UNS8 i; 00392 -00393 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){ -00394 if ( d->transfers[i].state == SDO_RESET ) { +00393 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){ +00394 if ( d->transfers[i].state == SDO_RESET ) { 00395 *line = i; -00396 d->transfers[i].whoami = whoami; +00396 d->transfers[i].whoami = whoami; 00397 return 0; 00398 } /* end if */ 00399 } /* end for */ -00400 MSG_ERR(0x1A25, "Too many SDO in progress. Aborted.", i); +00400 MSG_ERR(0x1A25, "Too many SDO in progress. Aborted.", i); 00401 return 0xFF; 00402 } 00403 -00414 UNS8 getSDOlineOnUse (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS8 *line) +00414 UNS8 getSDOlineOnUse (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS8 *line) 00415 { 00416 -00417 UNS8 i; +00417 UNS8 i; 00418 -00419 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){ -00420 if ( (d->transfers[i].state != SDO_RESET) && -00421 (d->transfers[i].nodeId == nodeId) && -00422 (d->transfers[i].whoami == whoami) ) { +00419 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){ +00420 if ( (d->transfers[i].state != SDO_RESET) && +00421 (d->transfers[i].nodeId == nodeId) && +00422 (d->transfers[i].whoami == whoami) ) { 00423 *line = i; 00424 return 0; 00425 } @@ -292,80 +297,83 @@ 00427 return 0xFF; 00428 } 00429 -00439 UNS8 closeSDOtransfer (CO_Data* d, UNS8 nodeId, UNS8 whoami) +00439 UNS8 closeSDOtransfer (CO_Data* d, UNS8 nodeId, UNS8 whoami) 00440 { -00441 UNS8 err; -00442 UNS8 line; -00443 err = getSDOlineOnUse(d, nodeId, whoami, &line); +00441 UNS8 err; +00442 UNS8 line; +00443 err = getSDOlineOnUse(d, nodeId, whoami, &line); 00444 if (err) { -00445 MSG_WAR(0x2A30, "No SDO communication to close for node : ", nodeId); +00445 MSG_WAR(0x2A30, "No SDO communication to close for node : ", nodeId); 00446 return 0xFF; 00447 } -00448 resetSDOline(d, line); +00448 resetSDOline(d, line); 00449 return 0; 00450 } 00451 -00461 UNS8 getSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 * nbBytes) +00461 UNS8 getSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 * nbBytes) 00462 { -00463 if (d->transfers[line].count == 0) /* if received initiate SDO protocol with e=0 and s=0 */ +00463 if (d->transfers[line].count == 0) /* if received initiate SDO protocol with e=0 and s=0 */ 00464 * nbBytes = 0; 00465 else -00466 * nbBytes = (UNS8)d->transfers[line].count - (UNS8)d->transfers[line].offset; +00466 * nbBytes = (UNS8)d->transfers[line].count - (UNS8)d->transfers[line].offset; 00467 return 0; 00468 } 00469 -00479 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 nbBytes) +00479 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 nbBytes) 00480 { -00481 if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) { -00482 MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); +00481 if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) { +00482 MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); 00483 return 0xFF; 00484 } -00485 d->transfers[line].count = nbBytes; +00485 d->transfers[line].count = nbBytes; 00486 return 0; 00487 } 00488 -00498 UNS8 sendSDO (CO_Data* d, UNS8 whoami, s_SDO sdo) +00498 UNS8 sendSDO (CO_Data* d, UNS8 whoami, s_SDO sdo) 00499 { -00500 UNS16 offset; -00501 UNS16 lastIndex; -00502 UNS8 found = 0; -00503 Message m; -00504 UNS8 i; -00505 UNS32 * pwCobId = NULL; -00506 UNS32 * pwNodeId = NULL; +00500 UNS16 offset; +00501 UNS16 lastIndex; +00502 UNS8 found = 0; +00503 Message m; +00504 UNS8 i; +00505 UNS32 * pwCobId = NULL; +00506 UNS32 * pwNodeId = NULL; 00507 -00508 MSG_WAR(0x3A38, "sendSDO",0); -00509 if( !((d->nodeState == Operational) || (d->nodeState == Pre_operational ))) { -00510 MSG_WAR(0x2A39, "unable to send the SDO (not in op or pre-op mode", d->nodeState); +00508 MSG_WAR(0x3A38, "sendSDO",0); +00509 if( !((d->nodeState == Operational) || (d->nodeState == Pre_operational ))) { +00510 MSG_WAR(0x2A39, "unable to send the SDO (not in op or pre-op mode", d->nodeState); 00511 return 0xFF; 00512 } 00513 -00515 if ( whoami == SDO_SERVER ) { -00516 offset = d->firstIndex->SDO_SVR; +00514 /*get the server->client cobid*/ +00515 if ( whoami == SDO_SERVER ) {/*case server. Easy because today only one server SDO is authorized in CanFestival*/ +00516 offset = d->firstIndex->SDO_SVR; 00517 if (offset == 0) { -00518 MSG_ERR(0x1A42, "SendSDO : No SDO server found", 0); +00518 MSG_ERR(0x1A42, "SendSDO : No SDO server found", 0); 00519 return 0xFF; 00520 } -00521 pwCobId = d->objdict[offset].pSubindex[2].pObject; -00522 MSG_WAR(0x3A41, "I am server. cobId : ", *pwCobId); +00521 pwCobId = d->objdict[offset].pSubindex[2].pObject; +00522 MSG_WAR(0x3A41, "I am server. cobId : ", *pwCobId); 00523 } -00524 else { -00526 UNS16 sdoNum = 0; -00527 offset = d->firstIndex->SDO_CLT; -00528 lastIndex = d->lastIndex->SDO_CLT; +00524 else { /*case client*/ +00525 /* Get the client->server cobid.*/ +00526 UNS16 sdoNum = 0; +00527 offset = d->firstIndex->SDO_CLT; +00528 lastIndex = d->lastIndex->SDO_CLT; 00529 if (offset == 0) { -00530 MSG_ERR(0x1A42, "SendSDO : No SDO client index found", 0); +00530 MSG_ERR(0x1A42, "SendSDO : No SDO client index found", 0); 00531 return 0xFF; 00532 } +00533 /* First, have to find at the index where is defined the communication with the server node */ 00534 while (offset <= lastIndex){ -00535 MSG_WAR(0x3A43,"Reading index : ", 0x1280 + sdoNum); -00536 if (d->objdict[offset].bSubCount <= 3) { -00537 MSG_ERR(0x1A28, "Subindex 3 not found at index ", 0x1280 + sdoNum); +00535 MSG_WAR(0x3A43,"Reading index : ", 0x1280 + sdoNum); +00536 if (d->objdict[offset].bSubCount <= 3) { +00537 MSG_ERR(0x1A28, "Subindex 3 not found at index ", 0x1280 + sdoNum); 00538 return 0xFF; 00539 } -00540 pwNodeId = d->objdict[offset].pSubindex[3].pObject; -00541 MSG_WAR(0x3A44, "Found nodeId server = ", *pwNodeId); -00542 if(*pwNodeId == sdo.nodeId) { +00540 pwNodeId = d->objdict[offset].pSubindex[3].pObject; +00541 MSG_WAR(0x3A44, "Found nodeId server = ", *pwNodeId); +00542 if(*pwNodeId == sdo.nodeId) { 00543 found = 1; 00544 break; 00545 } @@ -373,606 +381,719 @@ 00547 sdoNum ++; 00548 } 00549 if (! found){ -00550 MSG_WAR (0x2A45, "No SDO client corresponds to the mesage to send to node ", sdo.nodeId); +00550 MSG_WAR (0x2A45, "No SDO client corresponds to the mesage to send to node ", sdo.nodeId); 00551 return 0xFF; 00552 } -00554 pwCobId = d->objdict[offset].pSubindex[1].pObject; +00553 /* Second, read the cobid client->server */ +00554 pwCobId = d->objdict[offset].pSubindex[1].pObject; 00555 } -00557 m.cob_id.w = *pwCobId; -00558 m.rtr = NOT_A_REQUEST; -00560 m.len = 8; +00556 /* message copy for sending */ +00557 m.cob_id.w = *pwCobId; +00558 m.rtr = NOT_A_REQUEST; +00559 /* the length of SDO must be 8 */ +00560 m.len = 8; 00561 for (i = 0 ; i < 8 ; i++) { -00562 m.data[i] = sdo.body.data[i]; +00562 m.data[i] = sdo.body.data[i]; 00563 } -00564 return canSend(d->canHandle,&m); +00564 return canSend(d->canHandle,&m); 00565 } 00566 -00578 UNS8 sendSDOabort (CO_Data* d, UNS8 whoami, UNS16 index, UNS8 subIndex, UNS32 abortCode) +00578 UNS8 sendSDOabort (CO_Data* d, UNS8 whoami, UNS16 index, UNS8 subIndex, UNS32 abortCode) 00579 { -00580 s_SDO sdo; -00581 UNS8 ret; -00582 MSG_WAR(0x2A50,"Sending SDO abort ", abortCode); -00583 sdo.nodeId = *d->bDeviceNodeId; -00584 sdo.body.data[0] = 0x80; +00580 s_SDO sdo; +00581 UNS8 ret; +00582 MSG_WAR(0x2A50,"Sending SDO abort ", abortCode); +00583 sdo.nodeId = *d->bDeviceNodeId; +00584 sdo.body.data[0] = 0x80; 00585 /* Index */ -00586 sdo.body.data[1] = index & 0xFF; /* LSB */ -00587 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ +00586 sdo.body.data[1] = index & 0xFF; /* LSB */ +00587 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ 00588 /* Subindex */ -00589 sdo.body.data[3] = subIndex; +00589 sdo.body.data[3] = subIndex; 00590 /* Data */ -00591 sdo.body.data[4] = (UNS8)(abortCode & 0xFF); -00592 sdo.body.data[5] = (UNS8)((abortCode >> 8) & 0xFF); -00593 sdo.body.data[6] = (UNS8)((abortCode >> 16) & 0xFF); -00594 sdo.body.data[7] = (UNS8)((abortCode >> 24) & 0xFF); -00595 ret = sendSDO(d, whoami, sdo); +00591 sdo.body.data[4] = (UNS8)(abortCode & 0xFF); +00592 sdo.body.data[5] = (UNS8)((abortCode >> 8) & 0xFF); +00593 sdo.body.data[6] = (UNS8)((abortCode >> 16) & 0xFF); +00594 sdo.body.data[7] = (UNS8)((abortCode >> 24) & 0xFF); +00595 ret = sendSDO(d, whoami, sdo); 00596 00597 return ret; 00598 } 00599 -00608 UNS8 proceedSDO (CO_Data* d, Message *m) +00608 UNS8 proceedSDO (CO_Data* d, Message *m) 00609 { -00610 UNS8 err; -00611 UNS8 line; -00612 UNS8 nbBytes; -00613 UNS8 nodeId = 0; -00614 UNS32 nodeId_32; -00615 UNS32 *pNodeId = NULL; -00616 UNS8 whoami = SDO_UNKNOWN; -00617 UNS32 errorCode; -00618 s_SDO sdo; -00619 UNS16 index; -00620 UNS8 subIndex; -00621 UNS32 abortCode; -00622 UNS8 i,j; -00623 UNS32 * pCobId = NULL; -00624 UNS16 offset; -00625 UNS16 lastIndex; +00610 UNS8 err; +00611 UNS8 line; +00612 UNS8 nbBytes; /* received or to be transmited. */ +00613 UNS8 nodeId = 0; /* The node from which the SDO is received */ +00614 UNS32 nodeId_32; /* node id in 32 bits, for temporary use */ +00615 UNS32 *pNodeId = NULL; +00616 UNS8 whoami = SDO_UNKNOWN; /* SDO_SERVER or SDO_CLIENT.*/ +00617 UNS32 errorCode; /* while reading or writing in the local object dictionary.*/ +00618 s_SDO sdo; /* SDO to transmit */ +00619 UNS16 index; +00620 UNS8 subIndex; +00621 UNS32 abortCode; +00622 UNS8 i,j; +00623 UNS32 * pCobId = NULL; +00624 UNS16 offset; +00625 UNS16 lastIndex; 00626 -00627 MSG_WAR(0x3A60, "proceedSDO ", 0); -00628 whoami = SDO_UNKNOWN; -00631 offset = d->firstIndex->SDO_SVR; -00632 lastIndex = d->lastIndex->SDO_SVR; +00627 MSG_WAR(0x3A60, "proceedSDO ", 0); +00628 whoami = SDO_UNKNOWN; +00629 /* Looking for the cobId in the object dictionary. */ +00630 /* Am-I a server ? */ +00631 offset = d->firstIndex->SDO_SVR; +00632 lastIndex = d->lastIndex->SDO_SVR; 00633 j = 0; 00634 if(offset) while (offset <= lastIndex) { -00635 if (d->objdict[offset].bSubCount <= 1) { -00636 MSG_ERR(0x1A61, "Subindex 1 not found at index ", 0x1200 + j); +00635 if (d->objdict[offset].bSubCount <= 1) { +00636 MSG_ERR(0x1A61, "Subindex 1 not found at index ", 0x1200 + j); 00637 return 0xFF; 00638 } -00639 pCobId = d->objdict[offset].pSubindex[1].pObject; +00639 pCobId = d->objdict[offset].pSubindex[1].pObject; 00640 if ( *pCobId == (*m).cob_id.w ) { -00641 whoami = SDO_SERVER; -00642 MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j); +00641 whoami = SDO_SERVER; +00642 MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j); +00643 /* In case of server, the node id of the client may be unknown. So we put the index minus offset */ +00644 /* 0x1200 where the cobid received is defined. */ 00645 nodeId = j; 00646 break; 00647 } 00648 j++; 00649 offset++; -00650 } -00651 if (whoami == SDO_UNKNOWN) { -00653 offset = d->firstIndex->SDO_CLT; -00654 lastIndex = d->lastIndex->SDO_CLT; +00650 } /* end while */ +00651 if (whoami == SDO_UNKNOWN) { +00652 /* Am-I client ? */ +00653 offset = d->firstIndex->SDO_CLT; +00654 lastIndex = d->lastIndex->SDO_CLT; 00655 j = 0; 00656 if(offset) while (offset <= lastIndex) { -00657 if (d->objdict[offset].bSubCount <= 3) { -00658 MSG_ERR(0x1A63, "Subindex 3 not found at index ", 0x1280 + j); +00657 if (d->objdict[offset].bSubCount <= 3) { +00658 MSG_ERR(0x1A63, "Subindex 3 not found at index ", 0x1280 + j); 00659 return 0xFF; 00660 } -00662 pCobId = d->objdict[offset].pSubindex[2].pObject; +00661 /* a) Looking for the cobid received. */ +00662 pCobId = d->objdict[offset].pSubindex[2].pObject; 00663 if (*pCobId == (*m).cob_id.w ) { -00665 pNodeId = d->objdict[offset].pSubindex[3].pObject; -00666 whoami = SDO_CLIENT; +00664 /* b) cobid found, so reading the node id of the server. */ +00665 pNodeId = d->objdict[offset].pSubindex[3].pObject; +00666 whoami = SDO_CLIENT; 00667 nodeId_32 = *pNodeId; -00668 nodeId = (UNS8)nodeId_32; -00669 MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j); -00670 MSG_WAR(0x3A65, " Server nodeId : ", nodeId); +00668 nodeId = (UNS8)nodeId_32; +00669 MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j); +00670 MSG_WAR(0x3A65, " Server nodeId : ", nodeId); 00671 break; 00672 } 00673 j++; 00674 offset++; 00675 } /* end while */ 00676 } -00677 if (whoami == SDO_UNKNOWN) { -00678 return 0xFF; +00677 if (whoami == SDO_UNKNOWN) { +00678 return 0xFF;/* This SDO was not for us ! */ 00679 } 00680 +00681 /* Test if the size of the SDO is ok */ 00682 if ( (*m).len != 8) { -00683 MSG_ERR(0x1A67, "Error size SDO. CobId : ", (*m).cob_id.w); -00684 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_GENERAL_ERROR); +00683 MSG_ERR(0x1A67, "Error size SDO. CobId : ", (*m).cob_id.w); +00684 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_GENERAL_ERROR); 00685 return 0xFF; 00686 } 00687 -00688 if (whoami == SDO_CLIENT) { -00689 MSG_WAR(0x3A68, "I am CLIENT. Received SDO from nodeId : ", nodeId); +00688 if (whoami == SDO_CLIENT) { +00689 MSG_WAR(0x3A68, "I am CLIENT. Received SDO from nodeId : ", nodeId); 00690 } 00691 else { -00692 MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", (*m).cob_id.w); +00692 MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", (*m).cob_id.w); 00693 } 00694 -00698 switch (getSDOcs(m->data[0])) { +00695 /* Testing the command specifier */ +00696 /* Allowed : cs = 0, 1, 2, 3, 4. (= all except those for block tranfert). */ +00697 /* cs = other : Not allowed -> abort. */ +00698 switch (getSDOcs(m->data[0])) { 00699 00700 case 0: -00702 if (whoami == SDO_SERVER) { -00705 err = getSDOlineOnUse( d, nodeId, whoami, &line ); +00701 /* I am SERVER */ +00702 if (whoami == SDO_SERVER) { +00703 /* Receiving a download segment data. */ +00704 /* A SDO transfert should have been yet initiated. */ +00705 err = getSDOlineOnUse( d, nodeId, whoami, &line ); 00706 if (!err) -00707 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; +00707 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; 00708 if (err) { -00709 MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", +00709 MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", 00710 nodeId); -00711 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); +00711 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); 00712 return 0xFF; 00713 } +00714 /* Reset the wathdog */ 00715 RestartSDO_TIMER(line) -00716 MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", nodeId); -00717 index = d->transfers[line].index; -00718 subIndex = d->transfers[line].subIndex; -00720 if (d->transfers[line].toggle != getSDOt(m->data[0])) { -00721 MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0])); -00722 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); +00716 MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", nodeId); +00717 index = d->transfers[line].index; +00718 subIndex = d->transfers[line].subIndex; +00719 /* Toggle test. */ +00720 if (d->transfers[line].toggle != getSDOt(m->data[0])) { +00721 MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0])); +00722 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); 00723 return 0xFF; 00724 } -00726 nbBytes = 7 - getSDOn3(m->data[0]); -00728 err = SDOtoLine(d, line, nbBytes, (*m).data + 1); +00725 /* Nb of data to be downloaded */ +00726 nbBytes = 7 - getSDOn3(m->data[0]); +00727 /* Store the data in the transfert structure. */ +00728 err = SDOtoLine(d, line, nbBytes, (*m).data + 1); 00729 if (err) { -00730 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); +00730 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); 00731 return 0xFF; 00732 } -00734 sdo.nodeId = *d->bDeviceNodeId; -00735 sdo.body.data[0] = (1 << 5) | (d->transfers[line].toggle << 4); +00733 /* Sending the SDO response, CS = 1 */ +00734 sdo.nodeId = *d->bDeviceNodeId; /* The node id of the server, (here it is the sender). */ +00735 sdo.body.data[0] = (1 << 5) | (d->transfers[line].toggle << 4); 00736 for (i = 1 ; i < 8 ; i++) -00737 sdo.body.data[i] = 0; -00738 MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", nodeId); -00739 sendSDO(d, whoami, sdo); +00737 sdo.body.data[i] = 0; +00738 MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", nodeId); +00739 sendSDO(d, whoami, sdo); +00740 /* Inverting the toggle for the next segment. */ 00741 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; -00743 if (getSDOc(m->data[0])) { +00742 /* If it was the last segment, */ +00743 if (getSDOc(m->data[0])) { +00744 /* Transfering line data to object dictionary. */ +00745 /* The code does not use the "d" of initiate frame. So it is safe if e=s=0 */ 00746 errorCode = SDOlineToObjdict(d, line); 00747 if (errorCode) { -00748 MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0); +00748 MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0); 00749 failedSDO(d, nodeId, whoami, index, subIndex, errorCode); 00750 return 0xFF; 00751 } +00752 /* Release of the line */ 00753 resetSDOline(d, line); -00754 MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", nodeId); +00754 MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", nodeId); 00755 } 00756 } /* end if SERVER */ 00757 else { /* if CLIENT */ -00760 err = getSDOlineOnUse( d, nodeId, whoami, &line); +00758 /* I am CLIENT */ +00759 /* It is a request for a previous upload segment. We should find a line opened for this.*/ +00760 err = getSDOlineOnUse( d, nodeId, whoami, &line); 00761 if (!err) -00762 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; +00762 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; 00763 if (err) { -00764 MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); -00765 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); +00764 MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); +00765 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); 00766 return 0xFF; 00767 } +00768 /* Reset the wathdog */ 00769 RestartSDO_TIMER(line) -00770 index = d->transfers[line].index; -00771 subIndex = d->transfers[line].subIndex; -00773 if (d->transfers[line].toggle != getSDOt(m->data[0])) { -00774 MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId); -00775 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); +00770 index = d->transfers[line].index; +00771 subIndex = d->transfers[line].subIndex; +00772 /* test of the toggle; */ +00773 if (d->transfers[line].toggle != getSDOt(m->data[0])) { +00774 MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId); +00775 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); 00776 return 0xFF; 00777 } -00779 nbBytes = 7 - getSDOn3(m->data[0]); -00781 err = SDOtoLine(d, line, nbBytes, (*m).data + 1); +00778 /* nb of data to be uploaded */ +00779 nbBytes = 7 - getSDOn3(m->data[0]); +00780 /* Storing the data in the line structure. */ +00781 err = SDOtoLine(d, line, nbBytes, (*m).data + 1); 00782 if (err) { -00783 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); +00783 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); 00784 return 0xFF; 00785 } -00787 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; -00789 if ( getSDOc(m->data[0])) { +00786 /* Inverting the toggle for the next segment. */ +00787 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; +00788 /* If it was the last segment,*/ +00789 if ( getSDOc(m->data[0])) { +00790 /* Put in state finished */ +00791 /* The code is safe for the case e=s=0 in initiate frame. */ 00792 StopSDO_TIMER(line) -00793 d->transfers[line].state = SDO_FINISHED; -00794 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); +00793 d->transfers[line].state = SDO_FINISHED; +00794 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); 00795 -00796 MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId); +00796 MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId); 00797 } -00798 else { -00800 sdo.nodeId = nodeId; -00801 sdo.body.data[0] = (3 << 5) | (d->transfers[line].toggle << 4); +00798 else { /* more segments to receive */ +00799 /* Sending the request for the next segment. */ +00800 sdo.nodeId = nodeId; +00801 sdo.body.data[0] = (3 << 5) | (d->transfers[line].toggle << 4); 00802 for (i = 1 ; i < 8 ; i++) -00803 sdo.body.data[i] = 0; -00804 sendSDO(d, whoami, sdo); -00805 MSG_WAR(0x3A78, "SDO send upload segment request to nodeId", nodeId); +00803 sdo.body.data[i] = 0; +00804 sendSDO(d, whoami, sdo); +00805 MSG_WAR(0x3A78, "SDO send upload segment request to nodeId", nodeId); 00806 } 00807 } /* End if CLIENT */ 00808 break; 00809 00810 case 1: -00813 if (whoami == SDO_SERVER) { -00814 index = getSDOindex(m->data[1],m->data[2]); -00815 subIndex = getSDOsubIndex(m->data[3]); -00816 MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ", +00811 /* I am SERVER */ +00812 /* Receive of an initiate download */ +00813 if (whoami == SDO_SERVER) { +00814 index = getSDOindex(m->data[1],m->data[2]); +00815 subIndex = getSDOsubIndex(m->data[3]); +00816 MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ", 00817 nodeId); -00818 MSG_WAR(0x3A80, "Writing at index : ", index); -00819 MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex); +00818 MSG_WAR(0x3A80, "Writing at index : ", index); +00819 MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex); 00820 -00822 err = getSDOlineOnUse( d, nodeId, whoami, &line ); +00821 /* Search if a SDO transfert have been yet initiated */ +00822 err = getSDOlineOnUse( d, nodeId, whoami, &line ); 00823 if (! err) { -00824 MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0); -00825 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); +00824 MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0); +00825 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); 00826 return 0xFF; 00827 } -00830 err = getSDOfreeLine( d, whoami, &line ); +00828 /* No line on use. Great ! */ +00829 /* Try to open a new line. */ +00830 err = getSDOfreeLine( d, whoami, &line ); 00831 if (err) { -00832 MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0); -00833 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); +00832 MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0); +00833 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); 00834 return 0xFF; 00835 } -00836 initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS); +00836 initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS); 00837 -00838 if (getSDOe(m->data[0])) { -00840 nbBytes = 4 - getSDOn2(m->data[0]); -00842 d->transfers[line].count = nbBytes; -00843 err = SDOtoLine(d, line, nbBytes, (*m).data + 4); +00838 if (getSDOe(m->data[0])) { /* If SDO expedited */ +00839 /* nb of data to be downloaded */ +00840 nbBytes = 4 - getSDOn2(m->data[0]); +00841 /* Storing the data in the line structure. */ +00842 d->transfers[line].count = nbBytes; +00843 err = SDOtoLine(d, line, nbBytes, (*m).data + 4); 00844 00845 if (err) { -00846 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); +00846 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); 00847 return 0xFF; 00848 } 00849 -00852 MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfert. Finished.: ", nodeId); -00854 errorCode = SDOlineToObjdict(d, line); +00850 /* SDO expedited -> transfert finished. Data can be stored in the dictionary. */ +00851 /*The line will be reseted when it is downloading in the dictionary. */ +00852 MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfert. Finished.: ", nodeId); +00853 /* Transfering line data to object dictionary. */ +00854 errorCode = SDOlineToObjdict(d, line); 00855 if (errorCode) { -00856 MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0); -00857 failedSDO(d, nodeId, whoami, index, subIndex, errorCode); +00856 MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0); +00857 failedSDO(d, nodeId, whoami, index, subIndex, errorCode); 00858 return 0xFF; 00859 } -00861 resetSDOline(d, line); +00860 /* Release of the line. */ +00861 resetSDOline(d, line); 00862 } -00863 else { -00864 if (getSDOs(m->data[0])) { -00866 nbBytes = m->data[4]; -00867 err = setSDOlineRestBytes(d, nodeId, nbBytes); +00863 else {/* So, if it is not an expedited transfert */ +00864 if (getSDOs(m->data[0])) { +00865 /* TODO : if e and s = 0, not reading m->data[4] but put nbBytes = 0 */ +00866 nbBytes = m->data[4]; /* Transfert limited to 255 bytes. */ +00867 err = setSDOlineRestBytes(d, nodeId, nbBytes); 00868 if (err) { -00869 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); +00869 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); 00870 return 0xFF; 00871 } 00872 } 00873 } -00875 sdo.nodeId = *d->bDeviceNodeId; -00876 sdo.body.data[0] = 3 << 5; -00877 sdo.body.data[1] = index & 0xFF; /* LSB */ -00878 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ -00879 sdo.body.data[3] = subIndex; +00874 /*Sending a SDO, cs=3*/ +00875 sdo.nodeId = *d->bDeviceNodeId; /* The node id of the server, (here it is the sender).*/ +00876 sdo.body.data[0] = 3 << 5; +00877 sdo.body.data[1] = index & 0xFF; /* LSB */ +00878 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ +00879 sdo.body.data[3] = subIndex; 00880 for (i = 4 ; i < 8 ; i++) -00881 sdo.body.data[i] = 0; -00882 sendSDO(d, whoami, sdo); +00881 sdo.body.data[i] = 0; +00882 sendSDO(d, whoami, sdo); 00883 } /* end if I am SERVER */ 00884 else { -00887 err = getSDOlineOnUse( d, nodeId, whoami, &line); +00885 /* I am CLIENT */ +00886 /* It is a response for a previous download segment. We should find a line opened for this. */ +00887 err = getSDOlineOnUse( d, nodeId, whoami, &line); 00888 if (!err) -00889 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; +00889 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; 00890 if (err) { -00891 MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); -00892 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); +00891 MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); +00892 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); 00893 return 0xFF; 00894 } +00895 /* Reset the wathdog */ 00896 RestartSDO_TIMER(line) -00897 index = d->transfers[line].index; -00898 subIndex = d->transfers[line].subIndex; -00900 if (d->transfers[line].toggle != getSDOt(m->data[0])) { -00901 MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId); -00902 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); +00897 index = d->transfers[line].index; +00898 subIndex = d->transfers[line].subIndex; +00899 /* test of the toggle; */ +00900 if (d->transfers[line].toggle != getSDOt(m->data[0])) { +00901 MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId); +00902 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); 00903 return 0xFF; 00904 } 00905 -00907 getSDOlineRestBytes(d, line, &nbBytes); +00906 /* End transmission or downloading next segment. We need to know if it will be the last one. */ +00907 getSDOlineRestBytes(d, line, &nbBytes); 00908 if (nbBytes == 0) { -00909 MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId); +00909 MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId); 00910 StopSDO_TIMER(line) -00911 d->transfers[line].state = SDO_FINISHED; -00912 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); +00911 d->transfers[line].state = SDO_FINISHED; +00912 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); 00913 return 0x00; 00914 } +00915 /* At least one transfer to send. */ 00916 if (nbBytes > 7) { -00919 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; -00920 sdo.nodeId = nodeId; /* The server node Id; */ -00921 sdo.body.data[0] = (d->transfers[line].toggle << 4); -00922 err = lineToSDO(d, line, 7, sdo.body.data + 1); +00917 /* several segments to download.*/ +00918 /* code to send the next segment. (cs = 0; c = 0) */ +00919 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; +00920 sdo.nodeId = nodeId; /* The server node Id; */ +00921 sdo.body.data[0] = (d->transfers[line].toggle << 4); +00922 err = lineToSDO(d, line, 7, sdo.body.data + 1); 00923 if (err) { -00924 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); +00924 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); 00925 return 0xFF; 00926 } 00927 } 00928 else { -00931 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; -00932 sdo.nodeId = nodeId; /* The server node Id; */ -00933 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1; -00934 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); +00929 /* Last segment. */ +00930 /* code to send the last segment. (cs = 0; c = 1)*/ +00931 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; +00932 sdo.nodeId = nodeId; /* The server node Id; */ +00933 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1; +00934 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); 00935 if (err) { -00936 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); +00936 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); 00937 return 0xFF; 00938 } 00939 for (i = nbBytes + 1 ; i < 8 ; i++) -00940 sdo.body.data[i] = 0; +00940 sdo.body.data[i] = 0; 00941 } -00942 MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId); -00943 sendSDO(d, whoami, sdo); -00944 } +00942 MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId); +00943 sendSDO(d, whoami, sdo); +00944 } /* end if I am a CLIENT */ 00945 break; 00946 00947 case 2: -00950 if (whoami == SDO_SERVER) { -00951 index = getSDOindex(m->data[1],m->data[2]); -00952 subIndex = getSDOsubIndex(m->data[3]); -00953 MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", +00948 /* I am SERVER */ +00949 /* Receive of an initiate upload.*/ +00950 if (whoami == SDO_SERVER) { +00951 index = getSDOindex(m->data[1],m->data[2]); +00952 subIndex = getSDOsubIndex(m->data[3]); +00953 MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", 00954 nodeId); -00955 MSG_WAR(0x3A90, "Reading at index : ", index); -00956 MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex); -00958 err = getSDOlineOnUse( d, nodeId, whoami, &line ); +00955 MSG_WAR(0x3A90, "Reading at index : ", index); +00956 MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex); +00957 /* Search if a SDO transfert have been yet initiated*/ +00958 err = getSDOlineOnUse( d, nodeId, whoami, &line ); 00959 if (! err) { -00960 MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line); -00961 MSG_WAR(0x3A93, "nodeId = ", nodeId); -00962 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); +00960 MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line); +00961 MSG_WAR(0x3A93, "nodeId = ", nodeId); +00962 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); 00963 return 0xFF; 00964 } -00967 err = getSDOfreeLine( d, whoami, &line ); +00965 /* No line on use. Great !*/ +00966 /* Try to open a new line.*/ +00967 err = getSDOfreeLine( d, whoami, &line ); 00968 if (err) { -00969 MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0); -00970 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); +00969 MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0); +00970 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); 00971 return 0xFF; 00972 } -00973 initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS); -00975 errorCode = objdictToSDOline(d, line); +00973 initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS); +00974 /* Transfer data from dictionary to the line structure. */ +00975 errorCode = objdictToSDOline(d, line); 00976 00977 if (errorCode) { -00978 MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ", +00978 MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ", 00979 errorCode); -00980 failedSDO(d, nodeId, whoami, index, subIndex, errorCode); +00980 failedSDO(d, nodeId, whoami, index, subIndex, errorCode); 00981 return 0xFF; 00982 } -00984 getSDOlineRestBytes(d, line, &nbBytes); -00985 sdo.nodeId = nodeId; +00983 /* Preparing the response.*/ +00984 getSDOlineRestBytes(d, line, &nbBytes); /* Nb bytes to transfer ? */ +00985 sdo.nodeId = nodeId; /* The server node Id; */ 00986 if (nbBytes > 4) { -00989 sdo.body.data[0] = (2 << 5) | 1; -00990 sdo.body.data[1] = index & 0xFF; /* LSB */ -00991 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ -00992 sdo.body.data[3] = subIndex; -00993 sdo.body.data[4] = nbBytes; /* Limitation of canfestival2 : Max tranfert is 256 bytes.*/ +00987 /* normal transfert. (segmented). */ +00988 /* code to send the initiate upload response. (cs = 2) */ +00989 sdo.body.data[0] = (2 << 5) | 1; +00990 sdo.body.data[1] = index & 0xFF; /* LSB */ +00991 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ +00992 sdo.body.data[3] = subIndex; +00993 sdo.body.data[4] = nbBytes; /* Limitation of canfestival2 : Max tranfert is 256 bytes.*/ +00994 /* It takes too much memory to upgrate to 2^32 because the size of data is also coded */ +00995 /* in the object dictionary, at every index and subindex. */ 00996 for (i = 5 ; i < 8 ; i++) -00997 sdo.body.data[i] = 0; -00998 MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId); -00999 sendSDO(d, whoami, sdo); +00997 sdo.body.data[i] = 0; +00998 MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId); +00999 sendSDO(d, whoami, sdo); 01000 } 01001 else { -01003 sdo.body.data[0] = (2 << 5) | ((4 - nbBytes) << 2) | 3; -01004 sdo.body.data[1] = index & 0xFF; /* LSB */ -01005 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ -01006 sdo.body.data[3] = subIndex; -01007 err = lineToSDO(d, line, nbBytes, sdo.body.data + 4); +01002 /* Expedited upload. (cs = 2 ; e = 1) */ +01003 sdo.body.data[0] = (2 << 5) | ((4 - nbBytes) << 2) | 3; +01004 sdo.body.data[1] = index & 0xFF; /* LSB */ +01005 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ +01006 sdo.body.data[3] = subIndex; +01007 err = lineToSDO(d, line, nbBytes, sdo.body.data + 4); 01008 if (err) { -01009 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); +01009 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); 01010 return 0xFF; 01011 } 01012 for (i = 4 + nbBytes ; i < 8 ; i++) -01013 sdo.body.data[i] = 0; -01014 MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ", +01013 sdo.body.data[i] = 0; +01014 MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ", 01015 nodeId); -01016 sendSDO(d, whoami, sdo); +01016 sendSDO(d, whoami, sdo); +01017 /* Release the line.*/ 01018 resetSDOline(d, line); 01019 } -01020 } +01020 } /* end if I am SERVER*/ 01021 else { -01025 err = getSDOlineOnUse( d, nodeId, whoami, &line); +01022 /* I am CLIENT */ +01023 /* It is the response for the previous initiate upload request.*/ +01024 /* We should find a line opened for this. */ +01025 err = getSDOlineOnUse( d, nodeId, whoami, &line); 01026 if (!err) -01027 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; +01027 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; 01028 if (err) { -01029 MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId); -01030 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); +01029 MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId); +01030 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); 01031 return 0xFF; 01032 } +01033 /* Reset the wathdog */ 01034 RestartSDO_TIMER(line) -01035 index = d->transfers[line].index; -01036 subIndex = d->transfers[line].subIndex; +01035 index = d->transfers[line].index; +01036 subIndex = d->transfers[line].subIndex; 01037 -01038 if (getSDOe(m->data[0])) { /* If SDO expedited */ -01040 nbBytes = 4 - getSDOn2(m->data[0]); -01042 err = SDOtoLine(d, line, nbBytes, (*m).data + 4); +01038 if (getSDOe(m->data[0])) { /* If SDO expedited */ +01039 /* nb of data to be uploaded */ +01040 nbBytes = 4 - getSDOn2(m->data[0]); +01041 /* Storing the data in the line structure. */ +01042 err = SDOtoLine(d, line, nbBytes, (*m).data + 4); 01043 if (err) { -01044 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); +01044 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); 01045 return 0xFF; 01046 } -01048 MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId); +01047 /* SDO expedited -> transfert finished. data are available via getReadResultNetworkDict(). */ +01048 MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId); 01049 StopSDO_TIMER(line) -01050 d->transfers[line].count = nbBytes; -01051 d->transfers[line].state = SDO_FINISHED; -01052 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); +01050 d->transfers[line].count = nbBytes; +01051 d->transfers[line].state = SDO_FINISHED; +01052 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); 01053 return 0; 01054 } -01055 else { -01057 if (getSDOs(m->data[0])) { -01058 nbBytes = m->data[4]; -01059 err = setSDOlineRestBytes(d, line, nbBytes); +01055 else { /* So, if it is not an expedited transfert */ +01056 /* Storing the nb of data to receive. */ +01057 if (getSDOs(m->data[0])) { +01058 nbBytes = m->data[4]; /* Remember the limitation to 255 bytes to transfert */ +01059 err = setSDOlineRestBytes(d, line, nbBytes); 01060 if (err) { -01061 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); +01061 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); 01062 return 0xFF; 01063 } 01064 } -01066 sdo.nodeId = nodeId; -01067 sdo.body.data[0] = 3 << 5; +01065 /* Requesting next segment. (cs = 3) */ +01066 sdo.nodeId = nodeId; +01067 sdo.body.data[0] = 3 << 5; 01068 for (i = 1 ; i < 8 ; i++) -01069 sdo.body.data[i] = 0; -01070 MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId); -01071 sendSDO(d, whoami, sdo); +01069 sdo.body.data[i] = 0; +01070 MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId); +01071 sendSDO(d, whoami, sdo); 01072 } -01073 } +01073 } /* End if CLIENT */ 01074 break; 01075 01076 case 3: -01078 if (whoami == SDO_SERVER) { -01081 err = getSDOlineOnUse( d, nodeId, whoami, &line ); +01077 /* I am SERVER */ +01078 if (whoami == SDO_SERVER) { +01079 /* Receiving a upload segment. */ +01080 /* A SDO transfert should have been yet initiated. */ +01081 err = getSDOlineOnUse( d, nodeId, whoami, &line ); 01082 if (!err) -01083 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; +01083 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; 01084 if (err) { -01085 MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", +01085 MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", 01086 nodeId); -01087 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); +01087 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); 01088 return 0xFF; 01089 } +01090 /* Reset the wathdog */ 01091 RestartSDO_TIMER(line) -01092 MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", nodeId); -01093 index = d->transfers[line].index; -01094 subIndex = d->transfers[line].subIndex; -01096 if (d->transfers[line].toggle != getSDOt(m->data[0])) { -01097 MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0])); -01098 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); +01092 MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", nodeId); +01093 index = d->transfers[line].index; +01094 subIndex = d->transfers[line].subIndex; +01095 /* Toggle test.*/ +01096 if (d->transfers[line].toggle != getSDOt(m->data[0])) { +01097 MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0])); +01098 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); 01099 return 0xFF; 01100 } -01102 getSDOlineRestBytes(d, line, &nbBytes); +01101 /* Uploading next segment. We need to know if it will be the last one. */ +01102 getSDOlineRestBytes(d, line, &nbBytes); 01103 if (nbBytes > 7) { -01106 sdo.nodeId = nodeId; /* The server node Id; */ -01107 sdo.body.data[0] = (d->transfers[line].toggle << 4); -01108 err = lineToSDO(d, line, 7, sdo.body.data + 1); +01104 /* The segment to transfer is not the last one.*/ +01105 /* code to send the next segment. (cs = 0; c = 0) */ +01106 sdo.nodeId = nodeId; /* The server node Id; */ +01107 sdo.body.data[0] = (d->transfers[line].toggle << 4); +01108 err = lineToSDO(d, line, 7, sdo.body.data + 1); 01109 if (err) { -01110 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); +01110 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); 01111 return 0xFF; 01112 } -01114 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; -01115 MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", nodeId); -01116 sendSDO(d, whoami, sdo); +01113 /* Inverting the toggle for the next tranfert. */ +01114 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; +01115 MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", nodeId); +01116 sendSDO(d, whoami, sdo); 01117 } 01118 else { -01121 sdo.nodeId = nodeId; -01122 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1; -01123 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); +01119 /* Last segment. */ +01120 /* code to send the last segment. (cs = 0; c = 1) */ +01121 sdo.nodeId = nodeId; +01122 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1; +01123 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); 01124 if (err) { -01125 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); +01125 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); 01126 return 0xFF; 01127 } 01128 for (i = nbBytes + 1 ; i < 8 ; i++) -01129 sdo.body.data[i] = 0; -01130 MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", nodeId); -01131 sendSDO(d, whoami, sdo); +01129 sdo.body.data[i] = 0; +01130 MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", nodeId); +01131 sendSDO(d, whoami, sdo); +01132 /* Release the line */ 01133 resetSDOline(d, line); 01134 } 01135 } /* end if SERVER*/ 01136 else { -01140 err = getSDOlineOnUse( d, nodeId, whoami, &line); +01137 /* I am CLIENT */ +01138 /* It is the response for the previous initiate download request. */ +01139 /* We should find a line opened for this. */ +01140 err = getSDOlineOnUse( d, nodeId, whoami, &line); 01141 if (!err) -01142 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; +01142 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; 01143 if (err) { -01144 MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId); -01145 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); +01144 MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId); +01145 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); 01146 return 0xFF; 01147 } +01148 /* Reset the watchdog */ 01149 RestartSDO_TIMER(line) -01150 index = d->transfers[line].index; -01151 subIndex = d->transfers[line].subIndex; +01150 index = d->transfers[line].index; +01151 subIndex = d->transfers[line].subIndex; 01152 /* End transmission or requesting next segment. */ -01153 getSDOlineRestBytes(d, line, &nbBytes); +01153 getSDOlineRestBytes(d, line, &nbBytes); 01154 if (nbBytes == 0) { -01155 MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId); +01155 MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId); 01156 StopSDO_TIMER(line) -01157 d->transfers[line].state = SDO_FINISHED; -01158 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); +01157 d->transfers[line].state = SDO_FINISHED; +01158 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); 01159 return 0x00; 01160 } 01161 if (nbBytes > 7) { -01164 sdo.nodeId = nodeId; -01165 sdo.body.data[0] = (d->transfers[line].toggle << 4); -01166 err = lineToSDO(d, line, 7, sdo.body.data + 1); +01162 /* more than one request to send */ +01163 /* code to send the next segment. (cs = 0; c = 0) */ +01164 sdo.nodeId = nodeId; +01165 sdo.body.data[0] = (d->transfers[line].toggle << 4); +01166 err = lineToSDO(d, line, 7, sdo.body.data + 1); 01167 if (err) { -01168 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); +01168 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); 01169 return 0xFF; 01170 } 01171 } 01172 else { -01175 sdo.nodeId = nodeId; -01176 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1; -01177 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); +01173 /* Last segment.*/ +01174 /* code to send the last segment. (cs = 0; c = 1) */ +01175 sdo.nodeId = nodeId; /* The server node Id; */ +01176 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1; +01177 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); 01178 if (err) { -01179 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); +01179 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); 01180 return 0xFF; 01181 } 01182 for (i = nbBytes + 1 ; i < 8 ; i++) -01183 sdo.body.data[i] = 0; +01183 sdo.body.data[i] = 0; 01184 } -01185 MSG_WAR(0x3AA7, "SDO sending download segment to nodeId", nodeId); -01186 sendSDO(d, whoami, sdo); +01185 MSG_WAR(0x3AA7, "SDO sending download segment to nodeId", nodeId); +01186 sendSDO(d, whoami, sdo); 01187 01188 } /* end if I am a CLIENT */ 01189 break; 01190 01191 case 4: 01192 abortCode = (*m).data[3] | -01193 ((UNS32)m->data[5] << 8) | -01194 ((UNS32)m->data[6] << 16) | -01195 ((UNS32)m->data[7] << 24); -01198 if (whoami == SDO_SERVER) { -01199 err = getSDOlineOnUse( d, nodeId, whoami, &line ); +01193 ((UNS32)m->data[5] << 8) | +01194 ((UNS32)m->data[6] << 16) | +01195 ((UNS32)m->data[7] << 24); +01196 /* Received SDO abort. */ +01197 /* Looking for the line concerned. */ +01198 if (whoami == SDO_SERVER) { +01199 err = getSDOlineOnUse( d, nodeId, whoami, &line ); 01200 if (!err) { -01201 resetSDOline( d, line ); -01202 MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode); +01201 resetSDOline( d, line ); +01202 MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode); 01203 } 01204 else -01205 MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode); +01205 MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode); +01206 /* Tips : The end user has no way to know that the server node has received an abort SDO. */ +01207 /* Its is ok, I think.*/ 01208 } -01209 else { -01210 err = getSDOlineOnUse( d, nodeId, whoami, &line ); +01209 else { /* If I am CLIENT */ +01210 err = getSDOlineOnUse( d, nodeId, whoami, &line ); 01211 if (!err) { +01212 /* The line *must* be released by the core program. */ 01213 StopSDO_TIMER(line) -01214 d->transfers[line].state = SDO_ABORTED_RCV; -01215 d->transfers[line].abortCode = abortCode; -01216 MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode); -01217 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); +01214 d->transfers[line].state = SDO_ABORTED_RCV; +01215 d->transfers[line].abortCode = abortCode; +01216 MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode); +01217 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); 01218 } 01219 else -01220 MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode); +01220 MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode); 01221 } 01222 break; 01223 default: -01225 MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", getSDOcs(m->data[0])); +01224 /* Error : Unknown cs */ +01225 MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", getSDOcs(m->data[0])); 01226 return 0xFF; 01227 01228 } /* End switch */ 01229 return 0; 01230 } 01231 -01247 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, -01248 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize) +01247 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, +01248 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize) 01249 { -01250 UNS8 err; -01251 UNS8 SDOfound = 0; -01252 UNS8 line; -01253 s_SDO sdo; /* SDO to transmit */ -01254 UNS8 i, j; -01255 UNS16 lastIndex; -01256 UNS16 offset; -01257 UNS32 *pNodeIdServer; -01258 UNS32 nodeIdServer; +01250 UNS8 err; +01251 UNS8 SDOfound = 0; +01252 UNS8 line; +01253 s_SDO sdo; /* SDO to transmit */ +01254 UNS8 i, j; +01255 UNS16 lastIndex; +01256 UNS16 offset; +01257 UNS32 *pNodeIdServer; +01258 UNS32 nodeIdServer; 01259 -01260 MSG_WAR(0x3AC0, "Send SDO to write in the dictionary of node : ", nodeId); -01261 MSG_WAR(0x3AC1, " At index : ", index); -01262 MSG_WAR(0x3AC2, " subIndex : ", subIndex); -01263 MSG_WAR(0x3AC3, " nb bytes : ", count); +01260 MSG_WAR(0x3AC0, "Send SDO to write in the dictionary of node : ", nodeId); +01261 MSG_WAR(0x3AC1, " At index : ", index); +01262 MSG_WAR(0x3AC2, " subIndex : ", subIndex); +01263 MSG_WAR(0x3AC3, " nb bytes : ", count); 01264 -01266 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line); +01265 /* Verify that there is no SDO communication yet. */ +01266 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line); 01267 if (!err) { -01268 MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId); +01268 MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId); 01269 return 0xFF; 01270 } -01272 err = getSDOfreeLine( d, SDO_CLIENT, &line ); +01271 /* Taking the line ... */ +01272 err = getSDOfreeLine( d, SDO_CLIENT, &line ); 01273 if (err) { -01274 MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); +01274 MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); 01275 return (0xFF); 01276 } -01278 offset = d->firstIndex->SDO_CLT; -01279 lastIndex = d->lastIndex->SDO_CLT; +01277 /* Check which SDO to use to communicate with the node */ +01278 offset = d->firstIndex->SDO_CLT; +01279 lastIndex = d->lastIndex->SDO_CLT; 01280 if (offset == 0) { -01281 MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0); +01281 MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0); 01282 return 0xFF; 01283 } 01284 i = 0; 01285 while (offset <= lastIndex) { -01286 if (d->objdict[offset].bSubCount <= 3) { -01287 MSG_ERR(0x1AC8, "Subindex 3 not found at index ", 0x1280 + i); +01286 if (d->objdict[offset].bSubCount <= 3) { +01287 MSG_ERR(0x1AC8, "Subindex 3 not found at index ", 0x1280 + i); 01288 return 0xFF; 01289 } -01291 pNodeIdServer = d->objdict[offset].pSubindex[3].pObject; +01290 /* looking for the nodeId server */ +01291 pNodeIdServer = d->objdict[offset].pSubindex[3].pObject; 01292 nodeIdServer = *pNodeIdServer; -01293 MSG_WAR(0x1AD2, "index : ", 0x1280 + i); -01294 MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer); +01293 MSG_WAR(0x1AD2, "index : ", 0x1280 + i); +01294 MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer); 01295 -01296 if(nodeIdServer == (UNS32)nodeId) { +01296 if(nodeIdServer == (UNS32)nodeId) { 01297 SDOfound = 1; 01298 break; 01299 } @@ -980,47 +1101,50 @@ 01301 i++; 01302 } /* end while */ 01303 if (!SDOfound) { -01304 MSG_ERR(0x1AC9, "SDO. Error. No client found to communicate with node : ", nodeId); +01304 MSG_ERR(0x1AC9, "SDO. Error. No client found to communicate with node : ", nodeId); 01305 return 0xFF; 01306 } -01307 MSG_WAR(0x3AD0," SDO client defined at index : ", 0x1280 + i); -01308 initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS); -01309 d->transfers[line].count = count; -01310 d->transfers[line].dataType = dataType; +01307 MSG_WAR(0x3AD0," SDO client defined at index : ", 0x1280 + i); +01308 initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS); +01309 d->transfers[line].count = count; +01310 d->transfers[line].dataType = dataType; 01311 +01312 /* Copy data to transfers structure. */ 01313 for (j = 0 ; j < count ; j++) { -01314 # ifdef CANOPEN_BIG_ENDIAN +01314 # ifdef CANOPEN_BIG_ENDIAN 01315 if (dataType == 0 && endianize) -01316 d->transfers[line].data[count - 1 - j] = ((char *)data)[j]; +01316 d->transfers[line].data[count - 1 - j] = ((char *)data)[j]; 01317 else /* String of bytes. */ -01318 d->transfers[line].data[j] = ((char *)data)[j]; +01318 d->transfers[line].data[j] = ((char *)data)[j]; 01319 # else -01320 d->transfers[line].data[j] = ((char *)data)[j]; +01320 d->transfers[line].data[j] = ((char *)data)[j]; 01321 # endif 01322 } -01324 sdo.nodeId = nodeId; -01325 if (count <= 4) { -01326 sdo.body.data[0] = (1 << 5) | ((4 - count) << 2) | 3; +01323 /* Send the SDO to the server. Initiate download, cs=1. */ +01324 sdo.nodeId = nodeId; +01325 if (count <= 4) { /* Expedited transfert */ +01326 sdo.body.data[0] = (1 << 5) | ((4 - count) << 2) | 3; 01327 for (i = 4 ; i < 8 ; i++) -01328 sdo.body.data[i] = d->transfers[line].data[i - 4]; -01329 d->transfers[line].offset = count; +01328 sdo.body.data[i] = d->transfers[line].data[i - 4]; +01329 d->transfers[line].offset = count; 01330 } 01331 else { -01332 sdo.body.data[0] = (1 << 5) | 1; -01333 sdo.body.data[4] = count; +01332 sdo.body.data[0] = (1 << 5) | 1; +01333 sdo.body.data[4] = count; /* nb of byte to transmit. Max = 255. (canfestival2 limitation). */ 01334 for (i = 5 ; i < 8 ; i++) -01335 sdo.body.data[i] = 0; +01335 sdo.body.data[i] = 0; 01336 } -01337 sdo.body.data[1] = index & 0xFF; /* LSB */ -01338 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ -01339 sdo.body.data[3] = subIndex; +01337 sdo.body.data[1] = index & 0xFF; /* LSB */ +01338 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ +01339 sdo.body.data[3] = subIndex; 01340 -01341 d->transfers[line].Callback = Callback; +01341 d->transfers[line].Callback = Callback; 01342 -01343 err = sendSDO(d, SDO_CLIENT, sdo); +01343 err = sendSDO(d, SDO_CLIENT, sdo); 01344 if (err) { -01345 MSG_ERR(0x1AD1, "SDO. Error while sending SDO to node : ", nodeId); -01347 resetSDOline(d, line); +01345 MSG_ERR(0x1AD1, "SDO. Error while sending SDO to node : ", nodeId); +01346 /* release the line */ +01347 resetSDOline(d, line); 01348 return 0xFF; 01349 } 01350 @@ -1028,63 +1152,67 @@ 01352 return 0; 01353 } 01354 -01368 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, -01369 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data) +01368 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, +01369 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data) 01370 { -01371 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, NULL, 1); +01371 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, NULL, 1); 01372 } 01373 -01388 UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index, -01389 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback) +01388 UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index, +01389 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback) 01390 { -01391 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1); +01391 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1); 01392 } 01393 -01406 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback) +01406 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback) 01407 { -01408 UNS8 err; -01409 UNS8 SDOfound = 0; -01410 UNS8 i; -01411 UNS8 line; -01412 s_SDO sdo; /* SDO to transmit */ -01413 UNS32 *pNodeIdServer; -01414 UNS32 nodeIdServer; -01415 UNS16 offset; -01416 UNS16 lastIndex; -01417 MSG_WAR(0x3AD5, "Send SDO to read in the dictionary of node : ", nodeId); -01418 MSG_WAR(0x3AD6, " At index : ", index); -01419 MSG_WAR(0x3AD7, " subIndex : ", subIndex); +01408 UNS8 err; +01409 UNS8 SDOfound = 0; +01410 UNS8 i; +01411 UNS8 line; +01412 s_SDO sdo; /* SDO to transmit */ +01413 UNS32 *pNodeIdServer; +01414 UNS32 nodeIdServer; +01415 UNS16 offset; +01416 UNS16 lastIndex; +01417 MSG_WAR(0x3AD5, "Send SDO to read in the dictionary of node : ", nodeId); +01418 MSG_WAR(0x3AD6, " At index : ", index); +01419 MSG_WAR(0x3AD7, " subIndex : ", subIndex); 01420 01421 -01423 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line); +01422 /* Verify that there is no SDO communication yet. */ +01423 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line); 01424 if (!err) { -01425 MSG_ERR(0x1AD8, "SDO error : Communication yet established. with node : ", nodeId); +01425 MSG_ERR(0x1AD8, "SDO error : Communication yet established. with node : ", nodeId); 01426 return 0xFF; 01427 } -01429 err = getSDOfreeLine( d, SDO_CLIENT, &line ); +01428 /* Taking the line ... */ +01429 err = getSDOfreeLine( d, SDO_CLIENT, &line ); 01430 if (err) { -01431 MSG_ERR(0x1AD9, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); +01431 MSG_ERR(0x1AD9, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); 01432 return (0xFF); 01433 } 01434 else -01435 MSG_WAR(0x3AE0, "Transmission on line : ", line); +01435 MSG_WAR(0x3AE0, "Transmission on line : ", line); 01436 -01438 offset = d->firstIndex->SDO_CLT; -01439 lastIndex = d->lastIndex->SDO_CLT; +01437 /* Check which SDO to use to communicate with the node */ +01438 offset = d->firstIndex->SDO_CLT; +01439 lastIndex = d->lastIndex->SDO_CLT; 01440 if (offset == 0) { -01441 MSG_ERR(0x1AE1, "writeNetworkDict : No SDO client index found", 0); +01441 MSG_ERR(0x1AE1, "writeNetworkDict : No SDO client index found", 0); 01442 return 0xFF; 01443 } 01444 i = 0; 01445 while (offset <= lastIndex) { -01446 if (d->objdict[offset].bSubCount <= 3) { -01447 MSG_ERR(0x1AE2, "Subindex 3 not found at index ", 0x1280 + i); +01446 if (d->objdict[offset].bSubCount <= 3) { +01447 MSG_ERR(0x1AE2, "Subindex 3 not found at index ", 0x1280 + i); 01448 return 0xFF; 01449 } -01451 pNodeIdServer = d->objdict[offset].pSubindex[3].pObject; +01450 /* looking for the nodeId server */ +01451 pNodeIdServer = d->objdict[offset].pSubindex[3].pObject; 01452 nodeIdServer = *pNodeIdServer; 01453 -01454 if(nodeIdServer == (UNS32)nodeId) { +01454 if(nodeIdServer == (UNS32)nodeId) { 01455 SDOfound = 1; 01456 break; 01457 } @@ -1092,86 +1220,90 @@ 01459 i++; 01460 } /* end while */ 01461 if (!SDOfound) { -01462 MSG_ERR(0x1AE3, "SDO. Error. No client found to communicate with node : ", nodeId); +01462 MSG_ERR(0x1AE3, "SDO. Error. No client found to communicate with node : ", nodeId); 01463 return 0xFF; 01464 } -01465 MSG_WAR(0x3AE4," SDO client defined at index : ", 0x1280 + i); -01466 initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS); -01467 getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line); -01468 sdo.nodeId = nodeId; -01470 d->transfers[line].dataType = dataType; -01471 sdo.body.data[0] = (2 << 5); -01472 sdo.body.data[1] = index & 0xFF; /* LSB */ -01473 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ -01474 sdo.body.data[3] = subIndex; +01465 MSG_WAR(0x3AE4," SDO client defined at index : ", 0x1280 + i); +01466 initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS); +01467 getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line); +01468 sdo.nodeId = nodeId; +01469 /* Send the SDO to the server. Initiate upload, cs=2. */ +01470 d->transfers[line].dataType = dataType; +01471 sdo.body.data[0] = (2 << 5); +01472 sdo.body.data[1] = index & 0xFF; /* LSB */ +01473 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ +01474 sdo.body.data[3] = subIndex; 01475 for (i = 4 ; i < 8 ; i++) -01476 sdo.body.data[i] = 0; -01477 d->transfers[line].Callback = Callback; -01478 err = sendSDO(d, SDO_CLIENT, sdo); +01476 sdo.body.data[i] = 0; +01477 d->transfers[line].Callback = Callback; +01478 err = sendSDO(d, SDO_CLIENT, sdo); 01479 if (err) { -01480 MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId); +01480 MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId); 01481 /* release the line */ -01482 resetSDOline(d, line); +01482 resetSDOline(d, line); 01483 return 0xFF; 01484 } 01485 return 0; 01486 } 01487 -01499 UNS8 readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType) +01499 UNS8 readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType) 01500 { -01501 return _readNetworkDict (d, nodeId, index, subIndex, dataType, NULL); +01501 return _readNetworkDict (d, nodeId, index, subIndex, dataType, NULL); 01502 } 01503 -01516 UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback) +01516 UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback) 01517 { -01518 return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback); +01518 return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback); 01519 } 01520 -01532 UNS8 getReadResultNetworkDict (CO_Data* d, UNS8 nodeId, void* data, UNS8 *size, -01533 UNS32 * abortCode) +01532 UNS8 getReadResultNetworkDict (CO_Data* d, UNS8 nodeId, void* data, UNS8 *size, +01533 UNS32 * abortCode) 01534 { -01535 UNS8 i; -01536 UNS8 err; -01537 UNS8 line; +01535 UNS8 i; +01536 UNS8 err; +01537 UNS8 line; 01538 * size = 0; 01539 -01541 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line); +01540 /* Looking for the line tranfert. */ +01541 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line); 01542 if (err) { -01543 MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId); -01544 return SDO_ABORTED_INTERNAL; +01543 MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId); +01544 return SDO_ABORTED_INTERNAL; 01545 } -01546 if (d->transfers[line].state != SDO_FINISHED) -01547 return d->transfers[line].state; +01546 if (d->transfers[line].state != SDO_FINISHED) +01547 return d->transfers[line].state; 01548 -01550 * size = (UNS8)d->transfers[line].count; +01549 /* Transfert is finished. Put the value in the data. */ +01550 * size = (UNS8)d->transfers[line].count; 01551 for ( i = 0 ; i < *size ; i++) { -01552 # ifdef CANOPEN_BIG_ENDIAN -01553 if (d->transfers[line].dataType != visible_string) -01554 ( (char *) data)[*size - 1 - i] = d->transfers[line].data[i]; -01555 else -01556 ( (char *) data)[i] = d->transfers[line].data[i]; +01552 # ifdef CANOPEN_BIG_ENDIAN +01553 if (d->transfers[line].dataType != visible_string) +01554 ( (char *) data)[*size - 1 - i] = d->transfers[line].data[i]; +01555 else /* String of bytes. */ +01556 ( (char *) data)[i] = d->transfers[line].data[i]; 01557 # else -01558 ( (char *) data)[i] = d->transfers[line].data[i]; +01558 ( (char *) data)[i] = d->transfers[line].data[i]; 01559 # endif 01560 } -01561 return SDO_FINISHED; +01561 return SDO_FINISHED; 01562 } 01563 -01573 UNS8 getWriteResultNetworkDict (CO_Data* d, UNS8 nodeId, UNS32 * abortCode) +01573 UNS8 getWriteResultNetworkDict (CO_Data* d, UNS8 nodeId, UNS32 * abortCode) 01574 { -01575 UNS8 line = 0; -01576 UNS8 err; +01575 UNS8 line = 0; +01576 UNS8 err; 01577 01578 * abortCode = 0; -01580 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line); +01579 /* Looking for the line tranfert. */ +01580 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line); 01581 if (err) { -01582 MSG_ERR(0x1AF1, "SDO error : No line found for communication with node : ", nodeId); -01583 return SDO_ABORTED_INTERNAL; +01582 MSG_ERR(0x1AF1, "SDO error : No line found for communication with node : ", nodeId); +01583 return SDO_ABORTED_INTERNAL; 01584 } -01585 * abortCode = d->transfers[line].abortCode; -01586 return d->transfers[line].state; +01585 * abortCode = d->transfers[line].abortCode; +01586 return d->transfers[line].state; 01587 } -
Generated on Tue Jun 5 18:32:05 2007 for CanFestival by +