diff -r 2cb34a4ac65a -r 6b5a4c21e061 doc/doxygen/html/sdo_8c-source.html
--- a/doc/doxygen/html/sdo_8c-source.html Tue Jun 05 17:34:33 2007 +0200
+++ b/doc/doxygen/html/sdo_8c-source.html Tue Jun 05 18:43:21 2007 +0200
@@ -36,1306 +36,1142 @@
00019
00020
00021
-00022
-00023
-00024
-00025
-00026 #include "objacces.h"
-00027 #include "sdo.h"
-00028 #include "canfestival.h"
-00029
-00030
-00031 #define NO_INLINE
-00032
-00033 #ifdef NO_INLINE
-00034 #define INLINE
-00035 #else
-00036 #define INLINE inline
-00037 #endif
-00038
-00039
-00040
-00042 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
-00043 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize);
-00044
-00046 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex,
-00047 UNS8 dataType, SDOCallback_t Callback);
-00048
+00032
+00033
+00034
+00035 #include "objacces.h"
+00036 #include "sdo.h"
+00037 #include "canfestival.h"
+00038
+00039
+00040 #define NO_INLINE
+00041
+00042 #ifdef NO_INLINE
+00043 #define INLINE
+00044 #else
+00045 #define INLINE inline
+00046 #endif
+00047
+00048
00049
-00050
-00051
-00052
-00055 #define getSDOcs(byte) (byte >> 5)
-00056
-00059 #define getSDOn2(byte) ((byte >> 2) & 3)
-00060
-00063 #define getSDOn3(byte) ((byte >> 1) & 7)
-00064
-00067 #define getSDOe(byte) ((byte >> 1) & 1)
-00068
-00071 #define getSDOs(byte) (byte & 1)
-00072
-00075 #define getSDOc(byte) (byte & 1)
-00076
-00079 #define getSDOt(byte) ((byte >> 4) & 1)
-00080
-00083 #define getSDOindex(byte1, byte2) ((byte2 << 8) | (byte1))
-00084
-00088 #define getSDOsubIndex(byte3) (byte3)
-00089
-00090
-00091
-00092
-00093 void SDOTimeoutAlarm(CO_Data* d, UNS32 id)
-00094 {
-00095 MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0);
-00096 MSG_WAR(0x2A02, "server node : ", d->transfers[id].nodeId);
-00097 MSG_WAR(0x2A02, " index : ", d->transfers[id].index);
-00098 MSG_WAR(0x2A02, " subIndex : ", d->transfers[id].subIndex);
-00099
-00100 d->transfers[id].timer = TIMER_NONE;
-00101
-00102 d->transfers[id].state = SDO_ABORTED_INTERNAL;
-00103
-00104 sendSDOabort(d, d->transfers[id].whoami,
-00105 d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT);
-00106 d->transfers[id].abortCode = SDOABT_TIMED_OUT;
-00107
-00108 if(d->transfers[id].Callback)
-00109
-00110 (*d->transfers[id].Callback)(d,d->transfers[id].nodeId);
-00111 else if(d->transfers[id].whoami == SDO_SERVER)
-00112
-00113 resetSDOline(d, (UNS8)id);
-00114 }
-00115
-00116 #define StopSDO_TIMER(id) \
-00117 MSG_WAR(0x3A05, "StopSDO_TIMER for line : ", line);\
-00118 d->transfers[id].timer = DelAlarm(d->transfers[id].timer);
-00119
-00120 #define StartSDO_TIMER(id) \
-00121 MSG_WAR(0x3A06, "StartSDO_TIMER for line : ", line);\
-00122 d->transfers[id].timer = SetAlarm(d,id,&SDOTimeoutAlarm,MS_TO_TIMEVAL(SDO_TIMEOUT_MS),0);
-00123
-00124 #define RestartSDO_TIMER(id) \
-00125 MSG_WAR(0x3A07, "restartSDO_TIMER for line : ", line);\
-00126 if(d->transfers[id].timer != TIMER_NONE) { StopSDO_TIMER(id) StartSDO_TIMER(id) }
-00127
-00128
-00131 void resetSDO (CO_Data* d)
-00132 {
-00133 UNS8 j;
-00134
-00135
-00136 for (j = 0 ; j < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; j++)
-00137 resetSDOline(d, j);
-00138 }
-00139
-00140
-00141 UNS32 SDOlineToObjdict (CO_Data* d, UNS8 line)
-00142 {
-00143 UNS8 size;
-00144 UNS32 errorCode;
-00145 MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line);
-00146 size = (UNS8)d->transfers[line].count;
-00147 errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,
-00148 (void *) d->transfers[line].data, &size, 1);
-00149 if (errorCode != OD_SUCCESSFUL)
-00150 return errorCode;
-00151 MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line);
-00152 return 0;
-00153
-00154 }
-00155
-00156
-00157 UNS32 objdictToSDOline (CO_Data* d, UNS8 line)
-00158 {
-00159 UNS8 size = 0;
-00160 UNS8 dataType;
-00161 UNS32 errorCode;
-00162
-00163 MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index);
-00164 MSG_WAR(0x3A06, " subIndex : ", d->transfers[line].subIndex);
-00165
-00166 errorCode = getODentry(d, d->transfers[line].index,
-00167 d->transfers[line].subIndex,
-00168 (void *)d->transfers[line].data,
-00169 &size, &dataType, 0);
-00170
-00171 if (errorCode != OD_SUCCESSFUL)
-00172 return errorCode;
-00173
-00174 d->transfers[line].count = size;
-00175 d->transfers[line].offset = 0;
-00176 #if 0
-00177
-00178 {
-00179 UNS8 i;
-00180 for (i = 0 ; i < 10 ; i++) {
-00181 MSG_WAR(i, "data= ", d->transfers[line].data[i]);
-00182 }
-00183 }
-00184 #endif
-00185 return 0;
-00186 }
-00187
-00188
-00189 UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data) {
-00190 UNS8 i;
-00191 UNS8 offset;
-00192
-00193 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
-00194 MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
-00195 return 0xFF;
-00196 }
-00197 if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) {
-00198 MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes);
-00199 return 0xFF;
-00200 }
-00201 offset = (UNS8)d->transfers[line].offset;
-00202 for (i = 0 ; i < nbBytes ; i++)
-00203 * (data + i) = d->transfers[line].data[offset + i];
-00204 d->transfers[line].offset = d->transfers[line].offset + nbBytes;
-00205 return 0;
-00206 }
-00207
-00208
-00209
-00210 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data)
-00211 {
-00212 UNS8 i;
-00213 UNS8 offset;
-00214
-00215 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
-00216 MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
-00217 return 0xFF;
-00218 }
-00219 offset = (UNS8)d->transfers[line].offset;
-00220 for (i = 0 ; i < nbBytes ; i++)
-00221 d->transfers[line].data[offset + i] = * (data + i);
-00222 d->transfers[line].offset = d->transfers[line].offset + nbBytes;
-00223 return 0;
-00224 }
+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);
+00082
+00083
+00084
+00085
+00086
+00089 #define getSDOcs(byte) (byte >> 5)
+00090
+00093 #define getSDOn2(byte) ((byte >> 2) & 3)
+00094
+00097 #define getSDOn3(byte) ((byte >> 1) & 7)
+00098
+00101 #define getSDOe(byte) ((byte >> 1) & 1)
+00102
+00105 #define getSDOs(byte) (byte & 1)
+00106
+00109 #define getSDOc(byte) (byte & 1)
+00110
+00113 #define getSDOt(byte) ((byte >> 4) & 1)
+00114
+00117 #define getSDOindex(byte1, byte2) ((byte2 << 8) | (byte1))
+00118
+00121 #define getSDOsubIndex(byte3) (byte3)
+00122
+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);
+00135
+00136 d->transfers[id].timer = TIMER_NONE;
+00137
+00138 d->transfers[id].state = SDO_ABORTED_INTERNAL;
+00139
+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
+00144 if(d->transfers[id].Callback)
+00145
+00146 (*d->transfers[id].Callback)(d,d->transfers[id].nodeId);
+00147 else if(d->transfers[id].whoami == SDO_SERVER)
+00148
+00149 resetSDOline(d, (UNS8)id);
+00150 }
+00151
+00152 #define StopSDO_TIMER(id) \
+00153 MSG_WAR(0x3A05, "StopSDO_TIMER for line : ", line);\
+00154 d->transfers[id].timer = DelAlarm(d->transfers[id].timer);
+00155
+00156 #define StartSDO_TIMER(id) \
+00157 MSG_WAR(0x3A06, "StartSDO_TIMER for line : ", line);\
+00158 d->transfers[id].timer = SetAlarm(d,id,&SDOTimeoutAlarm,MS_TO_TIMEVAL(SDO_TIMEOUT_MS),0);
+00159
+00160 #define RestartSDO_TIMER(id) \
+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)
+00170 {
+00171 UNS8 j;
+00172
+00174 for (j = 0 ; j < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; j++)
+00175 resetSDOline(d, j);
+00176 }
+00177
+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)
+00195 return errorCode;
+00196 MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line);
+00197 return 0;
+00198
+00199 }
+00200
+00209 UNS32 objdictToSDOline (CO_Data* d, UNS8 line)
+00210 {
+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);
+00217
+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)
+00224 return errorCode;
00225
-00226
-00227 UNS8 failedSDO (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS16 index,
-00228 UNS8 subIndex, UNS32 abortCode)
-00229 {
-00230 UNS8 err;
-00231 UNS8 line;
-00232 err = getSDOlineOnUse( d, nodeId, whoami, &line );
-00233 if (!err)
-00234 MSG_WAR(0x3A20, "FailedSDO : line found : ", line);
-00235 if ((! err) && (whoami == SDO_SERVER)) {
-00236 resetSDOline( d, line );
-00237 MSG_WAR(0x3A21, "FailedSDO : line released : ", line);
-00238 }
-00239 if ((! err) && (whoami == SDO_CLIENT)) {
-00240 StopSDO_TIMER(line);
-00241 d->transfers[line].state = SDO_ABORTED_INTERNAL;
-00242 }
-00243 MSG_WAR(0x3A22, "Sending SDO abort ", 0);
-00244 err = sendSDOabort(d, whoami, index, subIndex, abortCode);
-00245 if (err) {
-00246 MSG_WAR(0x3A23, "Unable to send the SDO abort", 0);
-00247 return 0xFF;
-00248 }
-00249 return 0;
-00250 }
-00251
-00252
-00253 void resetSDOline ( CO_Data* d, UNS8 line )
-00254 {
-00255 UNS8 i;
-00256 MSG_WAR(0x3A25, "reset SDO line nb : ", line);
-00257 initSDOline(d, line, 0, 0, 0, SDO_RESET);
-00258 for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++)
-00259 d->transfers[line].data[i] = 0;
-00260 }
-00261
-00262
-00263 UNS8 initSDOline (CO_Data* d, UNS8 line, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 state)
-00264 {
-00265 MSG_WAR(0x3A25, "init SDO line nb : ", line);
-00266 if (state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS){
-00267 StartSDO_TIMER(line)
-00268 }else{
-00269 StopSDO_TIMER(line)
-00270 }
-00271 d->transfers[line].nodeId = nodeId;
-00272 d->transfers[line].index = index;
-00273 d->transfers[line].subIndex = subIndex;
-00274 d->transfers[line].state = state;
-00275 d->transfers[line].toggle = 0;
-00276 d->transfers[line].count = 0;
-00277 d->transfers[line].offset = 0;
-00278 d->transfers[line].dataType = 0;
-00279 d->transfers[line].Callback = NULL;
-00280 return 0;
-00281 }
-00282
-00283
-00284 UNS8 getSDOfreeLine ( CO_Data* d, UNS8 whoami, UNS8 *line )
-00285 {
-00286
-00287 UNS8 i;
-00288
-00289 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
-00290 if ( d->transfers[i].state == SDO_RESET ) {
-00291 *line = i;
-00292 d->transfers[i].whoami = whoami;
-00293 return 0;
-00294 }
-00295 }
-00296 MSG_ERR(0x1A25, "Too many SDO in progress. Aborted.", i);
-00297 return 0xFF;
-00298 }
-00299
-00300
-00301 UNS8 getSDOlineOnUse (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS8 *line)
-00302 {
-00303
-00304 UNS8 i;
-00305
-00306 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
-00307 if ( (d->transfers[i].state != SDO_RESET) &&
-00308 (d->transfers[i].nodeId == nodeId) &&
-00309 (d->transfers[i].whoami == whoami) ) {
-00310 *line = i;
-00311 return 0;
-00312 }
-00313 }
-00314 return 0xFF;
-00315 }
-00316
-00317
-00318 UNS8 closeSDOtransfer (CO_Data* d, UNS8 nodeId, UNS8 whoami)
-00319 {
-00320 UNS8 err;
-00321 UNS8 line;
-00322 err = getSDOlineOnUse(d, nodeId, whoami, &line);
-00323 if (err) {
-00324 MSG_WAR(0x2A30, "No SDO communication to close for node : ", nodeId);
-00325 return 0xFF;
-00326 }
-00327 resetSDOline(d, line);
-00328 return 0;
-00329 }
-00330
-00331
-00332 UNS8 getSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 * nbBytes)
-00333 {
-00334 if (d->transfers[line].count == 0)
-00335 * nbBytes = 0;
-00336 else
-00337 * nbBytes = (UNS8)d->transfers[line].count - (UNS8)d->transfers[line].offset;
-00338 return 0;
-00339 }
-00340
-00341
-00342 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 nbBytes)
-00343 {
-00344 if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) {
-00345 MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
-00346 return 0xFF;
-00347 }
-00348 d->transfers[line].count = nbBytes;
-00349 return 0;
-00350 }
-00351
-00352
-00353
-00354 UNS8 sendSDO (CO_Data* d, UNS8 whoami, s_SDO sdo)
-00355 {
-00356 UNS16 offset;
-00357 UNS16 lastIndex;
-00358 UNS8 found = 0;
-00359 Message m;
-00360 UNS8 i;
-00361 UNS32 * pwCobId = NULL;
-00362 UNS32 * pwNodeId = NULL;
-00363
-00364 MSG_WAR(0x3A38, "sendSDO",0);
-00365 if( !((d->nodeState == Operational) || (d->nodeState == Pre_operational ))) {
-00366 MSG_WAR(0x2A39, "unable to send the SDO (not in op or pre-op mode", d->nodeState);
-00367 return 0xFF;
-00368 }
-00369
-00370
-00371 if ( whoami == SDO_SERVER ) {
-00372 offset = d->firstIndex->SDO_SVR;
-00373 if (offset == 0) {
-00374 MSG_ERR(0x1A42, "SendSDO : No SDO server found", 0);
-00375 return 0xFF;
-00376 }
-00377 pwCobId = d->objdict[offset].pSubindex[2].pObject;
-00378 MSG_WAR(0x3A41, "I am server. cobId : ", *pwCobId);
-00379 }
-00380 else {
-00381
-00382 UNS16 sdoNum = 0;
-00383 offset = d->firstIndex->SDO_CLT;
-00384 lastIndex = d->lastIndex->SDO_CLT;
-00385 if (offset == 0) {
-00386 MSG_ERR(0x1A42, "SendSDO : No SDO client index found", 0);
-00387 return 0xFF;
-00388 }
-00389
-00390 while (offset <= lastIndex){
-00391 MSG_WAR(0x3A43,"Reading index : ", 0x1280 + sdoNum);
-00392 if (d->objdict[offset].bSubCount <= 3) {
-00393 MSG_ERR(0x1A28, "Subindex 3 not found at index ", 0x1280 + sdoNum);
-00394 return 0xFF;
-00395 }
-00396 pwNodeId = d->objdict[offset].pSubindex[3].pObject;
-00397 MSG_WAR(0x3A44, "Found nodeId server = ", *pwNodeId);
-00398 if(*pwNodeId == sdo.nodeId) {
-00399 found = 1;
-00400 break;
-00401 }
-00402 offset ++;
-00403 sdoNum ++;
-00404 }
-00405 if (! found){
-00406 MSG_WAR (0x2A45, "No SDO client corresponds to the mesage to send to node ", sdo.nodeId);
-00407 return 0xFF;
-00408 }
-00409
-00410 pwCobId = d->objdict[offset].pSubindex[1].pObject;
-00411 }
-00412
-00413 m.cob_id.w = *pwCobId;
-00414 m.rtr = NOT_A_REQUEST;
-00415
-00416 m.len = 8;
-00417 for (i = 0 ; i < 8 ; i++) {
-00418 m.data[i] = sdo.body.data[i];
-00419 }
-00420 return canSend(d->canHandle,&m);
-00421 }
-00422
-00423
-00424 UNS8 sendSDOabort (CO_Data* d, UNS8 whoami, UNS16 index, UNS8 subIndex, UNS32 abortCode)
-00425 {
-00426 s_SDO sdo;
-00427 UNS8 ret;
-00428 MSG_WAR(0x2A50,"Sending SDO abort ", abortCode);
-00429 sdo.nodeId = *d->bDeviceNodeId;
-00430 sdo.body.data[0] = 0x80;
-00431
-00432 sdo.body.data[1] = index & 0xFF;
-00433 sdo.body.data[2] = (index >> 8) & 0xFF;
-00434
-00435 sdo.body.data[3] = subIndex;
-00436
-00437 sdo.body.data[4] = (UNS8)(abortCode & 0xFF);
-00438 sdo.body.data[5] = (UNS8)((abortCode >> 8) & 0xFF);
-00439 sdo.body.data[6] = (UNS8)((abortCode >> 16) & 0xFF);
-00440 sdo.body.data[7] = (UNS8)((abortCode >> 24) & 0xFF);
-00441 ret = sendSDO(d, whoami, sdo);
-00442
-00443 return ret;
-00444 }
-00445
-00446
-00447 UNS8 proceedSDO (CO_Data* d, Message *m)
-00448 {
-00449 UNS8 err;
-00450 UNS8 line;
-00451 UNS8 nbBytes;
-00452 UNS8 nodeId = 0;
-00453 UNS32 nodeId_32;
-00454 UNS32 *pNodeId = NULL;
-00455 UNS8 whoami = SDO_UNKNOWN;
-00456 UNS32 errorCode;
-00457 s_SDO sdo;
-00458 UNS16 index;
-00459 UNS8 subIndex;
-00460 UNS32 abortCode;
-00461 UNS8 i,j;
-00462 UNS32 * pCobId = NULL;
-00463 UNS16 offset;
-00464 UNS16 lastIndex;
-00465
-00466 MSG_WAR(0x3A60, "proceedSDO ", 0);
-00467 whoami = SDO_UNKNOWN;
-00468
-00469
-00470 offset = d->firstIndex->SDO_SVR;
-00471 lastIndex = d->lastIndex->SDO_SVR;
-00472 j = 0;
-00473 if(offset) while (offset <= lastIndex) {
-00474 if (d->objdict[offset].bSubCount <= 1) {
-00475 MSG_ERR(0x1A61, "Subindex 1 not found at index ", 0x1200 + j);
-00476 return 0xFF;
-00477 }
-00478 pCobId = d->objdict[offset].pSubindex[1].pObject;
-00479 if ( *pCobId == (*m).cob_id.w ) {
-00480 whoami = SDO_SERVER;
-00481 MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j);
-00482
-00483
-00484 nodeId = j;
-00485 break;
-00486 }
-00487 j++;
-00488 offset++;
-00489 }
-00490 if (whoami == SDO_UNKNOWN) {
-00491
-00492 offset = d->firstIndex->SDO_CLT;
-00493 lastIndex = d->lastIndex->SDO_CLT;
-00494 j = 0;
-00495 if(offset) while (offset <= lastIndex) {
-00496 if (d->objdict[offset].bSubCount <= 3) {
-00497 MSG_ERR(0x1A63, "Subindex 3 not found at index ", 0x1280 + j);
-00498 return 0xFF;
-00499 }
-00500
-00501 pCobId = d->objdict[offset].pSubindex[2].pObject;
-00502 if (*pCobId == (*m).cob_id.w ) {
-00503
-00504 pNodeId = d->objdict[offset].pSubindex[3].pObject;
-00505 whoami = SDO_CLIENT;
-00506 nodeId_32 = *pNodeId;
-00507 nodeId = (UNS8)nodeId_32;
-00508 MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j);
-00509 MSG_WAR(0x3A65, " Server nodeId : ", nodeId);
-00510 break;
-00511 }
-00512 j++;
-00513 offset++;
-00514 }
-00515 }
-00516 if (whoami == SDO_UNKNOWN) {
-00517 return 0xFF;
-00518 }
-00519
-00520
-00521 if ( (*m).len != 8) {
-00522 MSG_ERR(0x1A67, "Error size SDO. CobId : ", (*m).cob_id.w);
-00523 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_GENERAL_ERROR);
-00524 return 0xFF;
-00525 }
-00526
-00527 if (whoami == SDO_CLIENT) {
-00528 MSG_WAR(0x3A68, "I am CLIENT. Received SDO from nodeId : ", nodeId);
-00529 }
-00530 else {
-00531 MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", (*m).cob_id.w);
-00532 }
-00533
-00534
-00535
-00536
-00537 switch (getSDOcs(m->data[0])) {
-00538
-00539 case 0:
-00540
-00541 if (whoami == SDO_SERVER) {
-00542
-00543
-00544 err = getSDOlineOnUse( d, nodeId, whoami, &line );
-00545 if (!err)
-00546 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
-00547 if (err) {
-00548 MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ",
-00549 nodeId);
-00550 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
-00551 return 0xFF;
-00552 }
-00553
-00554 RestartSDO_TIMER(line)
-00555 MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", nodeId);
-00556 index = d->transfers[line].index;
-00557 subIndex = d->transfers[line].subIndex;
-00558
-00559 if (d->transfers[line].toggle != getSDOt(m->data[0])) {
-00560 MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0]));
-00561 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
-00562 return 0xFF;
-00563 }
-00564
-00565 nbBytes = 7 - getSDOn3(m->data[0]);
-00566
-00567 err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
-00568 if (err) {
-00569 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-00570 return 0xFF;
-00571 }
-00572
-00573 sdo.nodeId = *d->bDeviceNodeId;
-00574 sdo.body.data[0] = (1 << 5) | (d->transfers[line].toggle << 4);
-00575 for (i = 1 ; i < 8 ; i++)
-00576 sdo.body.data[i] = 0;
-00577 MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", nodeId);
-00578 sendSDO(d, whoami, sdo);
-00579
-00580 d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
-00581
-00582 if (getSDOc(m->data[0])) {
-00583
-00584
-00585 errorCode = SDOlineToObjdict(d, line);
-00586 if (errorCode) {
-00587 MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0);
-00588 failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
-00589 return 0xFF;
-00590 }
-00591
-00592 resetSDOline(d, line);
-00593 MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", nodeId);
-00594 }
-00595 }
-00596 else {
-00597
-00598
-00599 err = getSDOlineOnUse( d, nodeId, whoami, &line);
-00600 if (!err)
-00601 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
-00602 if (err) {
-00603 MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId);
-00604 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
-00605 return 0xFF;
-00606 }
-00607
-00608 RestartSDO_TIMER(line)
-00609 index = d->transfers[line].index;
-00610 subIndex = d->transfers[line].subIndex;
-00611
-00612 if (d->transfers[line].toggle != getSDOt(m->data[0])) {
-00613 MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId);
-00614 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
-00615 return 0xFF;
-00616 }
-00617
-00618 nbBytes = 7 - getSDOn3(m->data[0]);
-00619
-00620 err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
-00621 if (err) {
-00622 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-00623 return 0xFF;
-00624 }
-00625
-00626 d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
-00627
-00628 if ( getSDOc(m->data[0])) {
-00629
-00630
-00631 StopSDO_TIMER(line)
-00632 d->transfers[line].state = SDO_FINISHED;
-00633 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
-00634
-00635 MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId);
-00636 }
-00637 else {
-00638
-00639 sdo.nodeId = nodeId;
-00640 sdo.body.data[0] = (3 << 5) | (d->transfers[line].toggle << 4);
-00641 for (i = 1 ; i < 8 ; i++)
-00642 sdo.body.data[i] = 0;
-00643 sendSDO(d, whoami, sdo);
-00644 MSG_WAR(0x3A78, "SDO send upload segment request to nodeId", nodeId);
-00645 }
-00646 }
-00647 break;
-00648
-00649 case 1:
-00650
-00651
-00652 if (whoami == SDO_SERVER) {
-00653 index = getSDOindex(m->data[1],m->data[2]);
-00654 subIndex = getSDOsubIndex(m->data[3]);
-00655 MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ",
-00656 nodeId);
-00657 MSG_WAR(0x3A80, "Writing at index : ", index);
-00658 MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex);
-00659
-00660
-00661 err = getSDOlineOnUse( d, nodeId, whoami, &line );
-00662 if (! err) {
-00663 MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0);
-00664 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
-00665 return 0xFF;
-00666 }
-00667
-00668
-00669 err = getSDOfreeLine( d, whoami, &line );
-00670 if (err) {
-00671 MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
-00672 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
-00673 return 0xFF;
-00674 }
-00675 initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);
-00676
-00677 if (getSDOe(m->data[0])) {
-00678
-00679 nbBytes = 4 - getSDOn2(m->data[0]);
-00680
-00681 d->transfers[line].count = nbBytes;
-00682 err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
-00683
-00684 if (err) {
-00685 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-00686 return 0xFF;
-00687 }
-00688
-00689
-00690
-00691 MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfert. Finished.: ", nodeId);
-00692
-00693 errorCode = SDOlineToObjdict(d, line);
-00694 if (errorCode) {
-00695 MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0);
-00696 failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
-00697 return 0xFF;
-00698 }
-00699
-00700 resetSDOline(d, line);
-00701 }
-00702 else {
-00703 if (getSDOs(m->data[0])) {
-00704
-00705 nbBytes = m->data[4];
-00706 err = setSDOlineRestBytes(d, nodeId, nbBytes);
-00707 if (err) {
-00708 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-00709 return 0xFF;
-00710 }
-00711 }
-00712 }
-00713
-00714 sdo.nodeId = *d->bDeviceNodeId;
-00715 sdo.body.data[0] = 3 << 5;
-00716 sdo.body.data[1] = index & 0xFF;
-00717 sdo.body.data[2] = (index >> 8) & 0xFF;
-00718 sdo.body.data[3] = subIndex;
-00719 for (i = 4 ; i < 8 ; i++)
-00720 sdo.body.data[i] = 0;
-00721 sendSDO(d, whoami, sdo);
-00722 }
-00723 else {
-00724
-00725
-00726 err = getSDOlineOnUse( d, nodeId, whoami, &line);
-00727 if (!err)
-00728 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
+00226 d->transfers[line].count = size;
+00227 d->transfers[line].offset = 0;
+00228 #if 0
+00229
+00230 {
+00231 UNS8 i;
+00232 for (i = 0 ; i < 10 ; 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;
+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);
+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);
+00260 return 0xFF;
+00261 }
+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;
+00266 return 0;
+00267 }
+00268
+00279 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data)
+00280 {
+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);
+00286 return 0xFF;
+00287 }
+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;
+00292 return 0;
+00293 }
+00294
+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 );
+00313 if (!err)
+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)) {
+00320 StopSDO_TIMER(line);
+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);
+00325 if (err) {
+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 )
+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;
+00345 }
+00346
+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){
+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;
+00376 return 0;
+00377 }
+00378
+00388 UNS8 getSDOfreeLine ( CO_Data* d, UNS8 whoami, UNS8 *line )
+00389 {
+00390
+00391 UNS8 i;
+00392
+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;
+00397 return 0;
+00398 }
+00399 }
+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)
+00415 {
+00416
+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) ) {
+00423 *line = i;
+00424 return 0;
+00425 }
+00426 }
+00427 return 0xFF;
+00428 }
+00429
+00439 UNS8 closeSDOtransfer (CO_Data* d, UNS8 nodeId, UNS8 whoami)
+00440 {
+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);
+00446 return 0xFF;
+00447 }
+00448 resetSDOline(d, line);
+00449 return 0;
+00450 }
+00451
+00461 UNS8 getSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 * nbBytes)
+00462 {
+00463 if (d->transfers[line].count == 0)
+00464 * nbBytes = 0;
+00465 else
+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)
+00480 {
+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;
+00486 return 0;
+00487 }
+00488
+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;
+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);
+00511 return 0xFF;
+00512 }
+00513
+00515 if ( whoami == SDO_SERVER ) {
+00516 offset = d->firstIndex->SDO_SVR;
+00517 if (offset == 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);
+00523 }
+00524 else {
+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);
+00531 return 0xFF;
+00532 }
+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);
+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) {
+00543 found = 1;
+00544 break;
+00545 }
+00546 offset ++;
+00547 sdoNum ++;
+00548 }
+00549 if (! found){
+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;
+00555 }
+00557 m.cob_id.w = *pwCobId;
+00558 m.rtr = NOT_A_REQUEST;
+00560 m.len = 8;
+00561 for (i = 0 ; i < 8 ; i++) {
+00562 m.data[i] = sdo.body.data[i];
+00563 }
+00564 return canSend(d->canHandle,&m);
+00565 }
+00566
+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;
+00585
+00586 sdo.body.data[1] = index & 0xFF;
+00587 sdo.body.data[2] = (index >> 8) & 0xFF;
+00588
+00589 sdo.body.data[3] = subIndex;
+00590
+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)
+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;
+00626
+00627 MSG_WAR(0x3A60, "proceedSDO ", 0);
+00628 whoami = SDO_UNKNOWN;
+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);
+00637 return 0xFF;
+00638 }
+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);
+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;
+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);
+00659 return 0xFF;
+00660 }
+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;
+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);
+00671 break;
+00672 }
+00673 j++;
+00674 offset++;
+00675 }
+00676 }
+00677 if (whoami == SDO_UNKNOWN) {
+00678 return 0xFF;
+00679 }
+00680
+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);
+00685 return 0xFF;
+00686 }
+00687
+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);
+00693 }
+00694
+00698 switch (getSDOcs(m->data[0])) {
+00699
+00700 case 0:
+00702 if (whoami == SDO_SERVER) {
+00705 err = getSDOlineOnUse( d, nodeId, whoami, &line );
+00706 if (!err)
+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 + ",
+00710 nodeId);
+00711 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
+00712 return 0xFF;
+00713 }
+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);
+00723 return 0xFF;
+00724 }
+00726 nbBytes = 7 - getSDOn3(m->data[0]);
+00728 err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
00729 if (err) {
-00730 MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId);
-00731 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
-00732 return 0xFF;
-00733 }
-00734
-00735 RestartSDO_TIMER(line)
-00736 index = d->transfers[line].index;
-00737 subIndex = d->transfers[line].subIndex;
-00738
-00739 if (d->transfers[line].toggle != getSDOt(m->data[0])) {
-00740 MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId);
-00741 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
-00742 return 0xFF;
-00743 }
-00744
-00745
-00746 getSDOlineRestBytes(d, line, &nbBytes);
-00747 if (nbBytes == 0) {
-00748 MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId);
-00749 StopSDO_TIMER(line)
-00750 d->transfers[line].state = SDO_FINISHED;
-00751 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
-00752 return 0x00;
-00753 }
-00754
-00755 if (nbBytes > 7) {
-00756
-00757
-00758 d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
-00759 sdo.nodeId = nodeId;
-00760 sdo.body.data[0] = (d->transfers[line].toggle << 4);
-00761 err = lineToSDO(d, line, 7, sdo.body.data + 1);
-00762 if (err) {
-00763 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-00764 return 0xFF;
-00765 }
-00766 }
-00767 else {
-00768
-00769
-00770 d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
-00771 sdo.nodeId = nodeId;
-00772 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
-00773 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);
-00774 if (err) {
-00775 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-00776 return 0xFF;
-00777 }
-00778 for (i = nbBytes + 1 ; i < 8 ; i++)
-00779 sdo.body.data[i] = 0;
-00780 }
-00781 MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId);
-00782 sendSDO(d, whoami, sdo);
-00783 }
-00784 break;
-00785
-00786 case 2:
-00787
-00788
-00789 if (whoami == SDO_SERVER) {
-00790 index = getSDOindex(m->data[1],m->data[2]);
-00791 subIndex = getSDOsubIndex(m->data[3]);
-00792 MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ",
-00793 nodeId);
-00794 MSG_WAR(0x3A90, "Reading at index : ", index);
-00795 MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex);
-00796
-00797 err = getSDOlineOnUse( d, nodeId, whoami, &line );
-00798 if (! err) {
-00799 MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line);
-00800 MSG_WAR(0x3A93, "nodeId = ", nodeId);
-00801 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
-00802 return 0xFF;
-00803 }
-00804
-00805
-00806 err = getSDOfreeLine( d, whoami, &line );
-00807 if (err) {
-00808 MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
-00809 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
-00810 return 0xFF;
-00811 }
-00812 initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
-00813
-00814 errorCode = objdictToSDOline(d, line);
-00815
-00816 if (errorCode) {
-00817 MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ",
-00818 errorCode);
-00819 failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
-00820 return 0xFF;
-00821 }
-00822
-00823 getSDOlineRestBytes(d, line, &nbBytes);
-00824 sdo.nodeId = nodeId;
-00825 if (nbBytes > 4) {
-00826
-00827
-00828 sdo.body.data[0] = (2 << 5) | 1;
-00829 sdo.body.data[1] = index & 0xFF;
-00830 sdo.body.data[2] = (index >> 8) & 0xFF;
-00831 sdo.body.data[3] = subIndex;
-00832 sdo.body.data[4] = nbBytes;
-00833
-00834
-00835 for (i = 5 ; i < 8 ; i++)
-00836 sdo.body.data[i] = 0;
-00837 MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId);
-00838 sendSDO(d, whoami, sdo);
-00839 }
-00840 else {
-00841
-00842 sdo.body.data[0] = (2 << 5) | ((4 - nbBytes) << 2) | 3;
-00843 sdo.body.data[1] = index & 0xFF;
-00844 sdo.body.data[2] = (index >> 8) & 0xFF;
-00845 sdo.body.data[3] = subIndex;
-00846 err = lineToSDO(d, line, nbBytes, sdo.body.data + 4);
-00847 if (err) {
-00848 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-00849 return 0xFF;
-00850 }
-00851 for (i = 4 + nbBytes ; i < 8 ; i++)
-00852 sdo.body.data[i] = 0;
-00853 MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ",
-00854 nodeId);
-00855 sendSDO(d, whoami, sdo);
-00856
-00857 resetSDOline(d, line);
-00858 }
-00859 }
-00860 else {
-00861
-00862
-00863
-00864 err = getSDOlineOnUse( d, nodeId, whoami, &line);
-00865 if (!err)
-00866 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
-00867 if (err) {
-00868 MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId);
-00869 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
-00870 return 0xFF;
-00871 }
-00872
-00873 RestartSDO_TIMER(line)
-00874 index = d->transfers[line].index;
-00875 subIndex = d->transfers[line].subIndex;
-00876
-00877 if (getSDOe(m->data[0])) {
-00878
-00879 nbBytes = 4 - getSDOn2(m->data[0]);
-00880
-00881 err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
-00882 if (err) {
-00883 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-00884 return 0xFF;
-00885 }
-00886
-00887 MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId);
-00888 StopSDO_TIMER(line)
-00889 d->transfers[line].count = nbBytes;
-00890 d->transfers[line].state = SDO_FINISHED;
-00891 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
-00892 return 0;
-00893 }
-00894 else {
-00895
-00896 if (getSDOs(m->data[0])) {
-00897 nbBytes = m->data[4];
-00898 err = setSDOlineRestBytes(d, line, nbBytes);
-00899 if (err) {
-00900 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-00901 return 0xFF;
-00902 }
-00903 }
-00904
-00905 sdo.nodeId = nodeId;
-00906 sdo.body.data[0] = 3 << 5;
-00907 for (i = 1 ; i < 8 ; i++)
-00908 sdo.body.data[i] = 0;
-00909 MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId);
-00910 sendSDO(d, whoami, sdo);
-00911 }
-00912 }
-00913 break;
-00914
-00915 case 3:
-00916
-00917 if (whoami == SDO_SERVER) {
-00918
-00919
-00920 err = getSDOlineOnUse( d, nodeId, whoami, &line );
-00921 if (!err)
-00922 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
-00923 if (err) {
-00924 MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ",
-00925 nodeId);
-00926 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
-00927 return 0xFF;
-00928 }
-00929
-00930 RestartSDO_TIMER(line)
-00931 MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", nodeId);
-00932 index = d->transfers[line].index;
-00933 subIndex = d->transfers[line].subIndex;
-00934
-00935 if (d->transfers[line].toggle != getSDOt(m->data[0])) {
-00936 MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0]));
-00937 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
-00938 return 0xFF;
-00939 }
-00940
-00941 getSDOlineRestBytes(d, line, &nbBytes);
-00942 if (nbBytes > 7) {
-00943
-00944
-00945 sdo.nodeId = nodeId;
-00946 sdo.body.data[0] = (d->transfers[line].toggle << 4);
-00947 err = lineToSDO(d, line, 7, sdo.body.data + 1);
-00948 if (err) {
-00949 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-00950 return 0xFF;
-00951 }
-00952
-00953 d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
-00954 MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", nodeId);
-00955 sendSDO(d, whoami, sdo);
-00956 }
-00957 else {
-00958
-00959
-00960 sdo.nodeId = nodeId;
-00961 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
-00962 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);
-00963 if (err) {
-00964 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-00965 return 0xFF;
-00966 }
-00967 for (i = nbBytes + 1 ; i < 8 ; i++)
-00968 sdo.body.data[i] = 0;
-00969 MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", nodeId);
-00970 sendSDO(d, whoami, sdo);
-00971
-00972 resetSDOline(d, line);
-00973 }
-00974 }
-00975 else {
-00976
-00977
-00978
-00979 err = getSDOlineOnUse( d, nodeId, whoami, &line);
-00980 if (!err)
-00981 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
-00982 if (err) {
-00983 MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId);
-00984 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
-00985 return 0xFF;
-00986 }
-00987
-00988 RestartSDO_TIMER(line)
-00989 index = d->transfers[line].index;
-00990 subIndex = d->transfers[line].subIndex;
-00991
-00992 getSDOlineRestBytes(d, line, &nbBytes);
-00993 if (nbBytes == 0) {
-00994 MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId);
-00995 StopSDO_TIMER(line)
-00996 d->transfers[line].state = SDO_FINISHED;
-00997 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
-00998 return 0x00;
-00999 }
-01000 if (nbBytes > 7) {
-01001
-01002
-01003 sdo.nodeId = nodeId;
-01004 sdo.body.data[0] = (d->transfers[line].toggle << 4);
-01005 err = lineToSDO(d, line, 7, sdo.body.data + 1);
-01006 if (err) {
-01007 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-01008 return 0xFF;
-01009 }
-01010 }
-01011 else {
-01012
-01013
-01014 sdo.nodeId = nodeId;
-01015 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
-01016 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);
-01017 if (err) {
-01018 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-01019 return 0xFF;
-01020 }
-01021 for (i = nbBytes + 1 ; i < 8 ; i++)
-01022 sdo.body.data[i] = 0;
-01023 }
-01024 MSG_WAR(0x3AA7, "SDO sending download segment to nodeId", nodeId);
-01025 sendSDO(d, whoami, sdo);
-01026
-01027 }
-01028 break;
-01029
-01030 case 4:
-01031 abortCode = (*m).data[3] |
-01032 ((UNS32)m->data[5] << 8) |
-01033 ((UNS32)m->data[6] << 16) |
-01034 ((UNS32)m->data[7] << 24);
-01035
-01036
-01037 if (whoami == SDO_SERVER) {
-01038 err = getSDOlineOnUse( d, nodeId, whoami, &line );
-01039 if (!err) {
-01040 resetSDOline( d, line );
-01041 MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode);
-01042 }
-01043 else
-01044 MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode);
-01045
-01046
-01047 }
-01048 else {
-01049 err = getSDOlineOnUse( d, nodeId, whoami, &line );
-01050 if (!err) {
-01051
-01052 StopSDO_TIMER(line)
-01053 d->transfers[line].state = SDO_ABORTED_RCV;
-01054 d->transfers[line].abortCode = abortCode;
-01055 MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode);
-01056 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
-01057 }
-01058 else
-01059 MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode);
-01060 }
-01061 break;
-01062 default:
-01063
-01064 MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", getSDOcs(m->data[0]));
-01065 return 0xFF;
-01066
-01067 }
-01068 return 0;
-01069 }
-01070
-01071
-01072 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
-01073 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize)
-01074 {
-01075 UNS8 err;
-01076 UNS8 SDOfound = 0;
-01077 UNS8 line;
-01078 s_SDO sdo;
-01079 UNS8 i, j;
-01080 UNS16 lastIndex;
-01081 UNS16 offset;
-01082 UNS32 *pNodeIdServer;
-01083 UNS32 nodeIdServer;
-01084
-01085 MSG_WAR(0x3AC0, "Send SDO to write in the dictionary of node : ", nodeId);
-01086 MSG_WAR(0x3AC1, " At index : ", index);
-01087 MSG_WAR(0x3AC2, " subIndex : ", subIndex);
-01088 MSG_WAR(0x3AC3, " nb bytes : ", count);
-01089
-01090
-01091 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
-01092 if (!err) {
-01093 MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId);
-01094 return 0xFF;
-01095 }
-01096
-01097 err = getSDOfreeLine( d, SDO_CLIENT, &line );
-01098 if (err) {
-01099 MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId);
-01100 return (0xFF);
-01101 }
-01102
-01103 offset = d->firstIndex->SDO_CLT;
-01104 lastIndex = d->lastIndex->SDO_CLT;
-01105 if (offset == 0) {
-01106 MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0);
-01107 return 0xFF;
-01108 }
-01109 i = 0;
-01110 while (offset <= lastIndex) {
-01111 if (d->objdict[offset].bSubCount <= 3) {
-01112 MSG_ERR(0x1AC8, "Subindex 3 not found at index ", 0x1280 + i);
-01113 return 0xFF;
-01114 }
-01115
-01116 pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
-01117 nodeIdServer = *pNodeIdServer;
-01118 MSG_WAR(0x1AD2, "index : ", 0x1280 + i);
-01119 MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer);
-01120
-01121 if(nodeIdServer == (UNS32)nodeId) {
-01122 SDOfound = 1;
-01123 break;
-01124 }
-01125 offset++;
-01126 i++;
-01127 }
-01128 if (!SDOfound) {
-01129 MSG_ERR(0x1AC9, "SDO. Error. No client found to communicate with node : ", nodeId);
-01130 return 0xFF;
-01131 }
-01132 MSG_WAR(0x3AD0," SDO client defined at index : ", 0x1280 + i);
-01133 initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);
-01134 d->transfers[line].count = count;
-01135 d->transfers[line].dataType = dataType;
-01136
-01137
-01138 for (j = 0 ; j < count ; j++) {
-01139 # ifdef CANOPEN_BIG_ENDIAN
-01140 if (dataType == 0 && endianize)
-01141 d->transfers[line].data[count - 1 - j] = ((char *)data)[j];
-01142 else
-01143 d->transfers[line].data[j] = ((char *)data)[j];
-01144 # else
-01145 d->transfers[line].data[j] = ((char *)data)[j];
-01146 # endif
-01147 }
-01148
-01149 sdo.nodeId = nodeId;
-01150 if (count <= 4) {
-01151 sdo.body.data[0] = (1 << 5) | ((4 - count) << 2) | 3;
-01152 for (i = 4 ; i < 8 ; i++)
-01153 sdo.body.data[i] = d->transfers[line].data[i - 4];
-01154 d->transfers[line].offset = count;
-01155 }
-01156 else {
-01157 sdo.body.data[0] = (1 << 5) | 1;
-01158 sdo.body.data[4] = count;
-01159 for (i = 5 ; i < 8 ; i++)
-01160 sdo.body.data[i] = 0;
-01161 }
-01162 sdo.body.data[1] = index & 0xFF;
-01163 sdo.body.data[2] = (index >> 8) & 0xFF;
-01164 sdo.body.data[3] = subIndex;
-01165
-01166 d->transfers[line].Callback = Callback;
-01167
-01168 err = sendSDO(d, SDO_CLIENT, sdo);
-01169 if (err) {
-01170 MSG_ERR(0x1AD1, "SDO. Error while sending SDO to node : ", nodeId);
-01171
-01172 resetSDOline(d, line);
-01173 return 0xFF;
-01174 }
-01175
-01176
-01177 return 0;
-01178 }
-01179
-01180
-01181
-01182 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
-01183 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data)
-01184 {
-01185 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, NULL, 1);
-01186 }
+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);
+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);
+00741 d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
+00743 if (getSDOc(m->data[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);
+00749 failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
+00750 return 0xFF;
+00751 }
+00753 resetSDOline(d, line);
+00754 MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", nodeId);
+00755 }
+00756 }
+00757 else {
+00760 err = getSDOlineOnUse( d, nodeId, whoami, &line);
+00761 if (!err)
+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);
+00766 return 0xFF;
+00767 }
+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);
+00776 return 0xFF;
+00777 }
+00779 nbBytes = 7 - getSDOn3(m->data[0]);
+00781 err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
+00782 if (err) {
+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])) {
+00792 StopSDO_TIMER(line)
+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);
+00797 }
+00798 else {
+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);
+00806 }
+00807 }
+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 + ",
+00817 nodeId);
+00818 MSG_WAR(0x3A80, "Writing at index : ", index);
+00819 MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex);
+00820
+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);
+00826 return 0xFF;
+00827 }
+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);
+00834 return 0xFF;
+00835 }
+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);
+00844
+00845 if (err) {
+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);
+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);
+00858 return 0xFF;
+00859 }
+00861 resetSDOline(d, line);
+00862 }
+00863 else {
+00864 if (getSDOs(m->data[0])) {
+00866 nbBytes = m->data[4];
+00867 err = setSDOlineRestBytes(d, nodeId, nbBytes);
+00868 if (err) {
+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;
+00878 sdo.body.data[2] = (index >> 8) & 0xFF;
+00879 sdo.body.data[3] = subIndex;
+00880 for (i = 4 ; i < 8 ; i++)
+00881 sdo.body.data[i] = 0;
+00882 sendSDO(d, whoami, sdo);
+00883 }
+00884 else {
+00887 err = getSDOlineOnUse( d, nodeId, whoami, &line);
+00888 if (!err)
+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);
+00893 return 0xFF;
+00894 }
+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);
+00903 return 0xFF;
+00904 }
+00905
+00907 getSDOlineRestBytes(d, line, &nbBytes);
+00908 if (nbBytes == 0) {
+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);
+00913 return 0x00;
+00914 }
+00916 if (nbBytes > 7) {
+00919 d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
+00920 sdo.nodeId = nodeId;
+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);
+00925 return 0xFF;
+00926 }
+00927 }
+00928 else {
+00931 d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
+00932 sdo.nodeId = nodeId;
+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);
+00937 return 0xFF;
+00938 }
+00939 for (i = nbBytes + 1 ; i < 8 ; i++)
+00940 sdo.body.data[i] = 0;
+00941 }
+00942 MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId);
+00943 sendSDO(d, whoami, sdo);
+00944 }
+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 + ",
+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 );
+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);
+00963 return 0xFF;
+00964 }
+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);
+00971 return 0xFF;
+00972 }
+00973 initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
+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 : ",
+00979 errorCode);
+00980 failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
+00981 return 0xFF;
+00982 }
+00984 getSDOlineRestBytes(d, line, &nbBytes);
+00985 sdo.nodeId = nodeId;
+00986 if (nbBytes > 4) {
+00989 sdo.body.data[0] = (2 << 5) | 1;
+00990 sdo.body.data[1] = index & 0xFF;
+00991 sdo.body.data[2] = (index >> 8) & 0xFF;
+00992 sdo.body.data[3] = subIndex;
+00993 sdo.body.data[4] = nbBytes;
+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);
+01000 }
+01001 else {
+01003 sdo.body.data[0] = (2 << 5) | ((4 - nbBytes) << 2) | 3;
+01004 sdo.body.data[1] = index & 0xFF;
+01005 sdo.body.data[2] = (index >> 8) & 0xFF;
+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);
+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 + ",
+01015 nodeId);
+01016 sendSDO(d, whoami, sdo);
+01018 resetSDOline(d, line);
+01019 }
+01020 }
+01021 else {
+01025 err = getSDOlineOnUse( d, nodeId, whoami, &line);
+01026 if (!err)
+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);
+01031 return 0xFF;
+01032 }
+01034 RestartSDO_TIMER(line)
+01035 index = d->transfers[line].index;
+01036 subIndex = d->transfers[line].subIndex;
+01037
+01038 if (getSDOe(m->data[0])) {
+01040 nbBytes = 4 - getSDOn2(m->data[0]);
+01042 err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
+01043 if (err) {
+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);
+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);
+01053 return 0;
+01054 }
+01055 else {
+01057 if (getSDOs(m->data[0])) {
+01058 nbBytes = m->data[4];
+01059 err = setSDOlineRestBytes(d, line, nbBytes);
+01060 if (err) {
+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;
+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);
+01072 }
+01073 }
+01074 break;
+01075
+01076 case 3:
+01078 if (whoami == SDO_SERVER) {
+01081 err = getSDOlineOnUse( d, nodeId, whoami, &line );
+01082 if (!err)
+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 + ",
+01086 nodeId);
+01087 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
+01088 return 0xFF;
+01089 }
+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);
+01099 return 0xFF;
+01100 }
+01102 getSDOlineRestBytes(d, line, &nbBytes);
+01103 if (nbBytes > 7) {
+01106 sdo.nodeId = nodeId;
+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);
+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);
+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);
+01124 if (err) {
+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);
+01133 resetSDOline(d, line);
+01134 }
+01135 }
+01136 else {
+01140 err = getSDOlineOnUse( d, nodeId, whoami, &line);
+01141 if (!err)
+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);
+01146 return 0xFF;
+01147 }
+01149 RestartSDO_TIMER(line)
+01150 index = d->transfers[line].index;
+01151 subIndex = d->transfers[line].subIndex;
+01152
+01153 getSDOlineRestBytes(d, line, &nbBytes);
+01154 if (nbBytes == 0) {
+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);
+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);
+01167 if (err) {
+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);
+01178 if (err) {
+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;
+01184 }
+01185 MSG_WAR(0x3AA7, "SDO sending download segment to nodeId", nodeId);
+01186 sendSDO(d, whoami, sdo);
01187
-01188
-01189
-01190 UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index,
-01191 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback)
-01192 {
-01193 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1);
-01194 }
-01195
-01196
-01197
-01198 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
-01199 {
-01200 UNS8 err;
-01201 UNS8 SDOfound = 0;
-01202 UNS8 i;
-01203 UNS8 line;
-01204 s_SDO sdo;
-01205 UNS32 *pNodeIdServer;
-01206 UNS32 nodeIdServer;
-01207 UNS16 offset;
-01208 UNS16 lastIndex;
-01209 MSG_WAR(0x3AD5, "Send SDO to read in the dictionary of node : ", nodeId);
-01210 MSG_WAR(0x3AD6, " At index : ", index);
-01211 MSG_WAR(0x3AD7, " subIndex : ", subIndex);
-01212
-01213
-01214
-01215 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
-01216 if (!err) {
-01217 MSG_ERR(0x1AD8, "SDO error : Communication yet established. with node : ", nodeId);
-01218 return 0xFF;
-01219 }
-01220
-01221 err = getSDOfreeLine( d, SDO_CLIENT, &line );
-01222 if (err) {
-01223 MSG_ERR(0x1AD9, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId);
-01224 return (0xFF);
-01225 }
-01226 else
-01227 MSG_WAR(0x3AE0, "Transmission on line : ", line);
-01228
-01229
-01230 offset = d->firstIndex->SDO_CLT;
-01231 lastIndex = d->lastIndex->SDO_CLT;
-01232 if (offset == 0) {
-01233 MSG_ERR(0x1AE1, "writeNetworkDict : No SDO client index found", 0);
-01234 return 0xFF;
-01235 }
-01236 i = 0;
-01237 while (offset <= lastIndex) {
-01238 if (d->objdict[offset].bSubCount <= 3) {
-01239 MSG_ERR(0x1AE2, "Subindex 3 not found at index ", 0x1280 + i);
-01240 return 0xFF;
-01241 }
-01242
-01243 pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
-01244 nodeIdServer = *pNodeIdServer;
-01245
-01246 if(nodeIdServer == (UNS32)nodeId) {
-01247 SDOfound = 1;
-01248 break;
-01249 }
-01250 offset++;
-01251 i++;
-01252 }
-01253 if (!SDOfound) {
-01254 MSG_ERR(0x1AE3, "SDO. Error. No client found to communicate with node : ", nodeId);
-01255 return 0xFF;
-01256 }
-01257 MSG_WAR(0x3AE4," SDO client defined at index : ", 0x1280 + i);
-01258 initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
-01259 getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
-01260 sdo.nodeId = nodeId;
-01261
-01262 d->transfers[line].dataType = dataType;
-01263 sdo.body.data[0] = (2 << 5);
-01264 sdo.body.data[1] = index & 0xFF;
-01265 sdo.body.data[2] = (index >> 8) & 0xFF;
-01266 sdo.body.data[3] = subIndex;
-01267 for (i = 4 ; i < 8 ; i++)
-01268 sdo.body.data[i] = 0;
-01269 d->transfers[line].Callback = Callback;
-01270 err = sendSDO(d, SDO_CLIENT, sdo);
-01271 if (err) {
-01272 MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId);
-01273
-01274 resetSDOline(d, line);
-01275 return 0xFF;
-01276 }
-01277 return 0;
-01278 }
-01279
-01280
-01281
-01282 UNS8 readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType)
-01283 {
-01284 return _readNetworkDict (d, nodeId, index, subIndex, dataType, NULL);
-01285 }
-01286
-01287
-01288 UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
-01289 {
-01290 return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback);
-01291 }
-01292
-01293
-01294
-01295 UNS8 getReadResultNetworkDict (CO_Data* d, UNS8 nodeId, void* data, UNS8 *size,
-01296 UNS32 * abortCode)
-01297 {
-01298 UNS8 i;
-01299 UNS8 err;
-01300 UNS8 line;
-01301 * size = 0;
-01302
-01303
-01304 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
-01305 if (err) {
-01306 MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId);
-01307 return SDO_ABORTED_INTERNAL;
-01308 }
-01309 if (d->transfers[line].state != SDO_FINISHED)
-01310 return d->transfers[line].state;
-01311
-01312
-01313 * size = (UNS8)d->transfers[line].count;
-01314 for ( i = 0 ; i < *size ; i++) {
-01315 # ifdef CANOPEN_BIG_ENDIAN
-01316 if (d->transfers[line].dataType != visible_string)
-01317 ( (char *) data)[*size - 1 - i] = d->transfers[line].data[i];
-01318 else
-01319 ( (char *) data)[i] = d->transfers[line].data[i];
-01320 # else
-01321 ( (char *) data)[i] = d->transfers[line].data[i];
-01322 # endif
-01323 }
-01324 return SDO_FINISHED;
-01325 }
-01326
-01327
-01328
-01329 UNS8 getWriteResultNetworkDict (CO_Data* d, UNS8 nodeId, UNS32 * abortCode)
-01330 {
-01331 UNS8 line = 0;
-01332 UNS8 err;
-01333
-01334 * abortCode = 0;
-01335
-01336 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
-01337 if (err) {
-01338 MSG_ERR(0x1AF1, "SDO error : No line found for communication with node : ", nodeId);
-01339 return SDO_ABORTED_INTERNAL;
-01340 }
-01341 * abortCode = d->transfers[line].abortCode;
-01342 return d->transfers[line].state;
-01343 }
-
Generated on Mon Jun 4 17:09:27 2007 for CanFestival by
+01188 }
+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 );
+01200 if (!err) {
+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);
+01208 }
+01209 else {
+01210 err = getSDOlineOnUse( d, nodeId, whoami, &line );
+01211 if (!err) {
+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);
+01218 }
+01219 else
+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]));
+01226 return 0xFF;
+01227
+01228 }
+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)
+01249 {
+01250 UNS8 err;
+01251 UNS8 SDOfound = 0;
+01252 UNS8 line;
+01253 s_SDO sdo;
+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);
+01264
+01266 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
+01267 if (!err) {
+01268 MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId);
+01269 return 0xFF;
+01270 }
+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);
+01275 return (0xFF);
+01276 }
+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);
+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);
+01288 return 0xFF;
+01289 }
+01291 pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
+01292 nodeIdServer = *pNodeIdServer;
+01293 MSG_WAR(0x1AD2, "index : ", 0x1280 + i);
+01294 MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer);
+01295
+01296 if(nodeIdServer == (UNS32)nodeId) {
+01297 SDOfound = 1;
+01298 break;
+01299 }
+01300 offset++;
+01301 i++;
+01302 }
+01303 if (!SDOfound) {
+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;
+01311
+01313 for (j = 0 ; j < count ; j++) {
+01314 # ifdef CANOPEN_BIG_ENDIAN
+01315 if (dataType == 0 && endianize)
+01316 d->transfers[line].data[count - 1 - j] = ((char *)data)[j];
+01317 else
+01318 d->transfers[line].data[j] = ((char *)data)[j];
+01319 # else
+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;
+01327 for (i = 4 ; i < 8 ; i++)
+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;
+01334 for (i = 5 ; i < 8 ; i++)
+01335 sdo.body.data[i] = 0;
+01336 }
+01337 sdo.body.data[1] = index & 0xFF;
+01338 sdo.body.data[2] = (index >> 8) & 0xFF;
+01339 sdo.body.data[3] = subIndex;
+01340
+01341 d->transfers[line].Callback = Callback;
+01342
+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);
+01348 return 0xFF;
+01349 }
+01350
+01351
+01352 return 0;
+01353 }
+01354
+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);
+01372 }
+01373
+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);
+01392 }
+01393
+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;
+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);
+01424 if (!err) {
+01425 MSG_ERR(0x1AD8, "SDO error : Communication yet established. with node : ", nodeId);
+01426 return 0xFF;
+01427 }
+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);
+01432 return (0xFF);
+01433 }
+01434 else
+01435 MSG_WAR(0x3AE0, "Transmission on line : ", line);
+01436
+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);
+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);
+01448 return 0xFF;
+01449 }
+01451 pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
+01452 nodeIdServer = *pNodeIdServer;
+01453
+01454 if(nodeIdServer == (UNS32)nodeId) {
+01455 SDOfound = 1;
+01456 break;
+01457 }
+01458 offset++;
+01459 i++;
+01460 }
+01461 if (!SDOfound) {
+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;
+01473 sdo.body.data[2] = (index >> 8) & 0xFF;
+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);
+01479 if (err) {
+01480 MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId);
+01481
+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)
+01500 {
+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)
+01517 {
+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)
+01534 {
+01535 UNS8 i;
+01536 UNS8 err;
+01537 UNS8 line;
+01538 * size = 0;
+01539
+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;
+01545 }
+01546 if (d->transfers[line].state != SDO_FINISHED)
+01547 return d->transfers[line].state;
+01548
+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];
+01557 # else
+01558 ( (char *) data)[i] = d->transfers[line].data[i];
+01559 # endif
+01560 }
+01561 return SDO_FINISHED;
+01562 }
+01563
+01573 UNS8 getWriteResultNetworkDict (CO_Data* d, UNS8 nodeId, UNS32 * abortCode)
+01574 {
+01575 UNS8 line = 0;
+01576 UNS8 err;
+01577
+01578 * abortCode = 0;
+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;
+01584 }
+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
1.5.1