src/sdo.c

   1 /*
   2 This file is part of CanFestival, a library implementing CanOpen Stack.
   3
   4 Copyright (C): Edouard TISSERANT and Francis DUPIN
   5
   6 See COPYING file for copyrights details.
   7
   8 This library is free software; you can redistribute it and/or
   9 modify it under the terms of the GNU Lesser General Public
  10 License as published by the Free Software Foundation; either
  11 version 2.1 of the License, or (at your option) any later version.
  12
  13 This library is distributed in the hope that it will be useful,
  14 but WITHOUT ANY WARRANTY; without even the implied warranty of
  15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16 Lesser General Public License for more details.
  17
  18 You should have received a copy of the GNU Lesser General Public
  19 License along with this library; if not, write to the Free Software
  20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  21 */
  22 /*!
  23 ** @file   sdo.c
  24 ** @author Edouard TISSERANT and Francis DUPIN
  25 ** @date   Tue Jun  5 09:32:32 2007
  26 **
  27 ** @brief
  28 **
  29 **
  30 */
  31
  32 /* #define DEBUG_WAR_CONSOLE_ON */
  33 /* #define DEBUG_ERR_CONSOLE_ON */
  34
  35 #include "objacces.h"
  36 #include "sdo.h"
  37 #include "canfestival.h"
  38
  39 /* Uncomment if your compiler does not support inline functions */
  40 #define NO_INLINE
  41
  42 #ifdef NO_INLINE
  43   #define INLINE
  44 #else
  45   #define INLINE inline
  46 #endif
  47
  48 /*Internals prototypes*/
  49
  50 /*!
  51 ** Called by writeNetworkDict
  52 **
  53 ** @param d
  54 ** @param nodeId
  55 ** @param index
  56 ** @param subIndex
  57 ** @param count
  58 ** @param dataType
  59 ** @param data
  60 ** @param Callback
  61 ** @param endianize
  62 **
  63 ** @return
  64 **/
  65 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
  66                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize);
  67
  68 /*!
  69 ** Called by readNetworkDict
  70 **
  71 ** @param d
  72 ** @param nodeId
  73 ** @param index
  74 ** @param subIndex
  75 ** @param dataType
  76 ** @param Callback
  77 **
  78 ** @return
  79 **/
  80 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex,
  81         UNS8 dataType, SDOCallback_t Callback);
  82
  83
  84 /***************************************************************************/
  85 /* SDO (un)packing macros */
  86
  87 /** Returns the command specifier (cs, ccs, scs) from the first byte of the SDO
  88  */
  89 #define getSDOcs(byte) (byte >> 5)
  90
  91 /** Returns the number of bytes without data from the first byte of the SDO. Coded in 2 bits
  92  */
  93 #define getSDOn2(byte) ((byte >> 2) & 3)
  94
  95 /** Returns the number of bytes without data from the first byte of the SDO. Coded in 3 bits
  96  */
  97 #define getSDOn3(byte) ((byte >> 1) & 7)
  98
  99 /** Returns the transfer type from the first byte of the SDO
 100  */
 101 #define getSDOe(byte) ((byte >> 1) & 1)
 102
 103 /** Returns the size indicator from the first byte of the SDO
 104  */
 105 #define getSDOs(byte) (byte & 1)
 106
 107 /** Returns the indicator of end transmission from the first byte of the SDO
 108  */
 109 #define getSDOc(byte) (byte & 1)
 110
 111 /** Returns the toggle from the first byte of the SDO
 112  */
 113 #define getSDOt(byte) ((byte >> 4) & 1)
 114
 115 /** Returns the index from the bytes 1 and 2 of the SDO
 116  */
 117 #define getSDOindex(byte1, byte2) ((byte2 << 8) | (byte1))
 118
 119 /** Returns the subIndex from the byte 3 of the SDO
 120  */
 121 #define getSDOsubIndex(byte3) (byte3)
 122
 123 /*!
 124 **
 125 **
 126 ** @param d
 127 ** @param id
 128 **/
 129 void SDOTimeoutAlarm(CO_Data* d, UNS32 id)
 130 {
 131     MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0);
 132     MSG_WAR(0x2A02, "server node : ", d->transfers[id].nodeId);
 133     MSG_WAR(0x2A02, "      index : ", d->transfers[id].index);
 134     MSG_WAR(0x2A02, "   subIndex : ", d->transfers[id].subIndex);
 135     /* Reset timer handler */
 136     d->transfers[id].timer = TIMER_NONE;
 137     /*Set aborted state*/
 138     d->transfers[id].state = SDO_ABORTED_INTERNAL;
 139     /* Sending a SDO abort */
 140     sendSDOabort(d, d->transfers[id].whoami,
 141                  d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT);
 142     d->transfers[id].abortCode = SDOABT_TIMED_OUT;
 143     /* Call the user function to inform of the problem.*/
 144     if(d->transfers[id].Callback)
 145         /*If ther is a callback, it is responsible to close SDO transfer (client)*/
 146         (*d->transfers[id].Callback)(d,d->transfers[id].nodeId);
 147     else if(d->transfers[id].whoami == SDO_SERVER)
 148         /*Else, if server, reset the line*/
 149         resetSDOline(d, (UNS8)id);
 150 }
 151
 152 #define StopSDO_TIMER(id) \
 153 MSG_WAR(0x3A05, "StopSDO_TIMER for line : ", line);\
 154 d->transfers[id].timer = DelAlarm(d->transfers[id].timer);
 155
 156 #define StartSDO_TIMER(id) \
 157 MSG_WAR(0x3A06, "StartSDO_TIMER for line : ", line);\
 158 d->transfers[id].timer = SetAlarm(d,id,&SDOTimeoutAlarm,MS_TO_TIMEVAL(SDO_TIMEOUT_MS),0);
 159
 160 #define RestartSDO_TIMER(id) \
 161 MSG_WAR(0x3A07, "restartSDO_TIMER for line : ", line);\
 162 if(d->transfers[id].timer != TIMER_NONE) { StopSDO_TIMER(id) StartSDO_TIMER(id) }
 163
 164 /*!
 165 ** Reset all sdo buffers
 166 **
 167 ** @param d
 168 **/
 169 void resetSDO (CO_Data* d)
 170 {
 171   UNS8 j;
 172
 173   /* transfer structure initialization */
 174     for (j = 0 ; j < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; j++)
 175       resetSDOline(d, j);
 176 }
 177
 178 /*!
 179 **
 180 **
 181 ** @param d
 182 ** @param line
 183 **
 184 ** @return
 185 **/
 186 UNS32 SDOlineToObjdict (CO_Data* d, UNS8 line)
 187 {
 188   UNS8      size;
 189   UNS32 errorCode;
 190   MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line);
 191   size = (UNS8)d->transfers[line].count;
 192   errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,
 193                          (void *) d->transfers[line].data, &size, 1);
 194   if (errorCode != OD_SUCCESSFUL)
 195     return errorCode;
 196   MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line);
 197   return 0;
 198
 199 }
 200
 201 /*!
 202 **
 203 **
 204 ** @param d
 205 ** @param line
 206 **
 207 ** @return
 208 **/
 209 UNS32 objdictToSDOline (CO_Data* d, UNS8 line)
 210 {
 211   UNS8  size = 0;
 212   UNS8  dataType;
 213   UNS32 errorCode;
 214
 215   MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index);
 216   MSG_WAR(0x3A06, "  subIndex : ", d->transfers[line].subIndex);
 217
 218   errorCode = getODentry(d,     d->transfers[line].index,
 219                                 d->transfers[line].subIndex,
 220                                 (void *)d->transfers[line].data,
 221                                 &size, &dataType, 0);
 222
 223   if (errorCode != OD_SUCCESSFUL)
 224     return errorCode;
 225
 226   d->transfers[line].count = size;
 227   d->transfers[line].offset = 0;
 228 #if 0
 229    /*Me laisser a, please ! (FD)*/
 230   {
 231     UNS8 i;
 232     for (i = 0 ; i < 10 ; i++) {
 233       MSG_WAR(i, "data= ", d->transfers[line].data[i]);
 234     }
 235   }
 236 #endif
 237   return 0;
 238 }
 239
 240 /*!
 241 **
 242 **
 243 ** @param d
 244 ** @param line
 245 ** @param nbBytes
 246 ** @param data
 247 **
 248 ** @return
 249 **/
 250 UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data) {
 251   UNS8 i;
 252   UNS8 offset;
 253
 254   if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
 255     MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
 256     return 0xFF;
 257   }
 258     if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) {
 259     MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes);
 260     return 0xFF;
 261   }
 262   offset = (UNS8)d->transfers[line].offset;
 263   for (i = 0 ; i < nbBytes ; i++)
 264     * (data + i) = d->transfers[line].data[offset + i];
 265   d->transfers[line].offset = d->transfers[line].offset + nbBytes;
 266   return 0;
 267 }
 268
 269 /*!
 270 **
 271 **
 272 ** @param d
 273 ** @param line
 274 ** @param nbBytes
 275 ** @param data
 276 **
 277 ** @return
 278 **/
 279 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data)
 280 {
 281   UNS8 i;
 282   UNS8 offset;
 283
 284   if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
 285     MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
 286     return 0xFF;
 287   }
 288   offset = (UNS8)d->transfers[line].offset;
 289   for (i = 0 ; i < nbBytes ; i++)
 290     d->transfers[line].data[offset + i] = * (data + i);
 291   d->transfers[line].offset = d->transfers[line].offset + nbBytes;
 292   return 0;
 293 }
 294
 295 /*!
 296 **
 297 **
 298 ** @param d
 299 ** @param nodeId
 300 ** @param whoami
 301 ** @param index
 302 ** @param subIndex
 303 ** @param abortCode
 304 **
 305 ** @return
 306 **/
 307 UNS8 failedSDO (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS16 index,
 308                 UNS8 subIndex, UNS32 abortCode)
 309 {
 310   UNS8 err;
 311   UNS8 line;
 312   err = getSDOlineOnUse( d, nodeId, whoami, &line );
 313   if (!err) /* If a line on use have been found.*/
 314     MSG_WAR(0x3A20, "FailedSDO : line found : ", line);
 315   if ((! err) && (whoami == SDO_SERVER)) {
 316     resetSDOline( d, line );
 317     MSG_WAR(0x3A21, "FailedSDO : line released : ", line);
 318   }
 319   if ((! err) && (whoami == SDO_CLIENT)) {
 320     StopSDO_TIMER(line);
 321     d->transfers[line].state = SDO_ABORTED_INTERNAL;
 322   }
 323   MSG_WAR(0x3A22, "Sending SDO abort ", 0);
 324   err = sendSDOabort(d, whoami, index, subIndex, abortCode);
 325   if (err) {
 326     MSG_WAR(0x3A23, "Unable to send the SDO abort", 0);
 327     return 0xFF;
 328   }
 329   return 0;
 330 }
 331
 332 /*!
 333 **
 334 **
 335 ** @param d
 336 ** @param line
 337 **/
 338 void resetSDOline ( CO_Data* d, UNS8 line )
 339 {
 340   UNS8 i;
 341   MSG_WAR(0x3A25, "reset SDO line nb : ", line);
 342   initSDOline(d, line, 0, 0, 0, SDO_RESET);
 343   for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++)
 344     d->transfers[line].data[i] = 0;
 345 }
 346
 347 /*!
 348 **
 349 **
 350 ** @param d
 351 ** @param line
 352 ** @param nodeId
 353 ** @param index
 354 ** @param subIndex
 355 ** @param state
 356 **
 357 ** @return
 358 **/
 359 UNS8 initSDOline (CO_Data* d, UNS8 line, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 state)
 360 {
 361   MSG_WAR(0x3A25, "init SDO line nb : ", line);
 362   if (state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS){
 363         StartSDO_TIMER(line)
 364   }else{
 365         StopSDO_TIMER(line)
 366   }
 367   d->transfers[line].nodeId = nodeId;
 368   d->transfers[line].index = index;
 369   d->transfers[line].subIndex = subIndex;
 370   d->transfers[line].state = state;
 371   d->transfers[line].toggle = 0;
 372   d->transfers[line].count = 0;
 373   d->transfers[line].offset = 0;
 374   d->transfers[line].dataType = 0;
 375   d->transfers[line].Callback = NULL;
 376   return 0;
 377 }
 378
 379 /*!
 380 **
 381 **
 382 ** @param d
 383 ** @param whoami
 384 ** @param line
 385 **
 386 ** @return
 387 **/
 388 UNS8 getSDOfreeLine ( CO_Data* d, UNS8 whoami, UNS8 *line )
 389 {
 390
 391   UNS8 i;
 392
 393   for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
 394     if ( d->transfers[i].state == SDO_RESET ) {
 395       *line = i;
 396       d->transfers[i].whoami = whoami;
 397       return 0;
 398     } /* end if */
 399   } /* end for */
 400   MSG_ERR(0x1A25, "Too many SDO in progress. Aborted.", i);
 401   return 0xFF;
 402 }
 403
 404 /*!
 405 **
 406 **
 407 ** @param d
 408 ** @param nodeId
 409 ** @param whoami
 410 ** @param line
 411 **
 412 ** @return
 413 **/
 414 UNS8 getSDOlineOnUse (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS8 *line)
 415 {
 416
 417   UNS8 i;
 418
 419   for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
 420     if ( (d->transfers[i].state != SDO_RESET) &&
 421          (d->transfers[i].nodeId == nodeId) &&
 422          (d->transfers[i].whoami == whoami) ) {
 423       *line = i;
 424       return 0;
 425     }
 426   }
 427   return 0xFF;
 428 }
 429
 430 /*!
 431 **
 432 **
 433 ** @param d
 434 ** @param nodeId
 435 ** @param whoami
 436 **
 437 ** @return
 438 **/
 439 UNS8 closeSDOtransfer (CO_Data* d, UNS8 nodeId, UNS8 whoami)
 440 {
 441   UNS8 err;
 442   UNS8 line;
 443   err = getSDOlineOnUse(d, nodeId, whoami, &line);
 444   if (err) {
 445     MSG_WAR(0x2A30, "No SDO communication to close for node : ", nodeId);
 446     return 0xFF;
 447   }
 448   resetSDOline(d, line);
 449   return 0;
 450 }
 451
 452 /*!
 453 **
 454 **
 455 ** @param d
 456 ** @param line
 457 ** @param nbBytes
 458 **
 459 ** @return
 460 **/
 461 UNS8 getSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 * nbBytes)
 462 {
 463   if (d->transfers[line].count == 0) /* if received initiate SDO protocol with e=0 and s=0 */
 464     * nbBytes = 0;
 465   else
 466     * nbBytes = (UNS8)d->transfers[line].count - (UNS8)d->transfers[line].offset;
 467   return 0;
 468 }
 469
 470 /*!
 471 **
 472 **
 473 ** @param d
 474 ** @param line
 475 ** @param nbBytes
 476 **
 477 ** @return
 478 **/
 479 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 nbBytes)
 480 {
 481   if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) {
 482     MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
 483     return 0xFF;
 484   }
 485   d->transfers[line].count = nbBytes;
 486   return 0;
 487 }
 488
 489 /*!
 490 **
 491 **
 492 ** @param d
 493 ** @param whoami
 494 ** @param sdo
 495 **
 496 ** @return
 497 **/
 498 UNS8 sendSDO (CO_Data* d, UNS8 whoami, s_SDO sdo)
 499 {
 500   UNS16 offset;
 501   UNS16 lastIndex;
 502   UNS8 found = 0;
 503   Message m;
 504   UNS8 i;
 505   UNS32 * pwCobId = NULL;
 506   UNS8 * pwNodeId = NULL;
 507
 508   MSG_WAR(0x3A38, "sendSDO",0);
 509   if( !((d->nodeState == Operational) ||  (d->nodeState == Pre_operational ))) {
 510     MSG_WAR(0x2A39, "unable to send the SDO (not in op or pre-op mode", d->nodeState);
 511     return 0xFF;
 512   }
 513
 514   /*get the server->client cobid*/
 515   if ( whoami == SDO_SERVER )   {/*case server. Easy because today only one server SDO is authorized in CanFestival*/
 516     offset = d->firstIndex->SDO_SVR;
 517     if (offset == 0) {
 518       MSG_ERR(0x1A42, "SendSDO : No SDO server found", 0);
 519       return 0xFF;
 520     }
 521     pwCobId = (UNS32*) d->objdict[offset].pSubindex[2].pObject;
 522     MSG_WAR(0x3A41, "I am server. cobId : ", *pwCobId);
 523   }
 524   else {                        /*case client*/
 525     /* Get the client->server cobid.*/
 526     UNS16 sdoNum = 0;
 527     offset = d->firstIndex->SDO_CLT;
 528     lastIndex = d->lastIndex->SDO_CLT;
 529     if (offset == 0) {
 530       MSG_ERR(0x1A42, "SendSDO : No SDO client index found", 0);
 531       return 0xFF;
 532     }
 533     /* First, have to find at the index where is defined the communication with the server node */
 534     while (offset <= lastIndex){
 535       MSG_WAR(0x3A43,"Reading index : ", 0x1280 + sdoNum);
 536       if (d->objdict[offset].bSubCount <= 3) {
 537         MSG_ERR(0x1A28, "Subindex 3  not found at index ", 0x1280 + sdoNum);
 538         return 0xFF;
 539       }
 540       pwNodeId = (UNS8*) d->objdict[offset].pSubindex[3].pObject;
 541       MSG_WAR(0x3A44, "Found nodeId server = ", *pwNodeId);
 542       if(*pwNodeId == sdo.nodeId) {
 543         found = 1;
 544         break;
 545       }
 546       offset ++;
 547       sdoNum ++;
 548     }
 549     if (! found){
 550       MSG_WAR (0x2A45, "No SDO client corresponds to the mesage to send to node ", sdo.nodeId);
 551       return 0xFF;
 552     }
 553     /* Second, read the cobid client->server */
 554     pwCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject;
 555   }
 556   /* message copy for sending */
 557   m.cob_id = *pwCobId;
 558   m.rtr = NOT_A_REQUEST;
 559   /* the length of SDO must be 8 */
 560   m.len = 8;
 561   for (i = 0 ; i < 8 ; i++) {
 562     m.data[i] =  sdo.body.data[i];
 563   }
 564   return canSend(d->canHandle,&m);
 565 }
 566
 567 /*!
 568 **
 569 **
 570 ** @param d
 571 ** @param whoami
 572 ** @param index
 573 ** @param subIndex
 574 ** @param abortCode
 575 **
 576 ** @return
 577 **/
 578 UNS8 sendSDOabort (CO_Data* d, UNS8 whoami, UNS16 index, UNS8 subIndex, UNS32 abortCode)
 579 {
 580   s_SDO sdo;
 581   UNS8 ret;
 582   MSG_WAR(0x2A50,"Sending SDO abort ", abortCode);
 583   sdo.nodeId = *d->bDeviceNodeId;
 584   sdo.body.data[0] = 0x80;
 585   /* Index */
 586   sdo.body.data[1] = index & 0xFF; /* LSB */
 587   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
 588   /* Subindex */
 589   sdo.body.data[3] = subIndex;
 590   /* Data */
 591   sdo.body.data[4] = (UNS8)(abortCode & 0xFF);
 592   sdo.body.data[5] = (UNS8)((abortCode >> 8) & 0xFF);
 593   sdo.body.data[6] = (UNS8)((abortCode >> 16) & 0xFF);
 594   sdo.body.data[7] = (UNS8)((abortCode >> 24) & 0xFF);
 595   ret = sendSDO(d, whoami, sdo);
 596
 597   return ret;
 598 }
 599
 600 /*!
 601 **
 602 **
 603 ** @param d
 604 ** @param m
 605 **
 606 ** @return
 607 **/
 608 UNS8 proceedSDO (CO_Data* d, Message *m)
 609 {
 610   UNS8 err;
 611   UNS8 line;
 612   UNS8 nbBytes; /* received or to be transmited. */
 613   UNS8 nodeId = 0;  /* The node from which the SDO is received */
 614   UNS8 *pNodeId = NULL;
 615   UNS8 whoami = SDO_UNKNOWN;  /* SDO_SERVER or SDO_CLIENT.*/
 616   UNS32 errorCode; /* while reading or writing in the local object dictionary.*/
 617   s_SDO sdo;    /* SDO to transmit */
 618   UNS16 index;
 619   UNS8 subIndex;
 620   UNS32 abortCode;
 621   UNS8 i,j;
 622   UNS32 *     pCobId = NULL;
 623   UNS16 offset;
 624   UNS16 lastIndex;
 625
 626   MSG_WAR(0x3A60, "proceedSDO ", 0);
 627   whoami = SDO_UNKNOWN;
 628   /* Looking for the cobId in the object dictionary. */
 629   /* Am-I a server ? */
 630   offset = d->firstIndex->SDO_SVR;
 631   lastIndex = d->lastIndex->SDO_SVR;
 632   j = 0;
 633   if(offset) while (offset <= lastIndex) {
 634      if (d->objdict[offset].bSubCount <= 1) {
 635           MSG_ERR(0x1A61, "Subindex 1  not found at index ", 0x1200 + j);
 636           return 0xFF;
 637         }
 638       pCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject;
 639       if ( *pCobId == (*m).cob_id ) {
 640         whoami = SDO_SERVER;
 641         MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j);
 642         /* In case of server, the node id of the client may be unknown. So we put the index minus offset */
 643         /* 0x1200 where the cobid received is defined. */
 644         nodeId = j;
 645         break;
 646       }
 647       j++;
 648       offset++;
 649   } /* end while */
 650   if (whoami == SDO_UNKNOWN) {
 651     /* Am-I client ? */
 652     offset = d->firstIndex->SDO_CLT;
 653     lastIndex = d->lastIndex->SDO_CLT;
 654     j = 0;
 655     if(offset) while (offset <= lastIndex) {
 656        if (d->objdict[offset].bSubCount <= 3) {
 657          MSG_ERR(0x1A63, "Subindex 3  not found at index ", 0x1280 + j);
 658          return 0xFF;
 659        }
 660        /* a) Looking for the cobid received. */
 661        pCobId = (UNS32*) d->objdict[offset].pSubindex[2].pObject;
 662        if (*pCobId == (*m).cob_id ) {
 663          /* b) cobid found, so reading the node id of the server. */
 664          pNodeId = (UNS8*) d->objdict[offset].pSubindex[3].pObject;
 665          whoami = SDO_CLIENT;
 666          nodeId = *pNodeId;
 667          MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j);
 668          MSG_WAR(0x3A65, "                 Server nodeId : ", nodeId);
 669          break;
 670         }
 671        j++;
 672        offset++;
 673     } /* end while */
 674   }
 675   if (whoami == SDO_UNKNOWN) {
 676     return 0xFF;/* This SDO was not for us ! */
 677   }
 678
 679   /* Test if the size of the SDO is ok */
 680   if ( (*m).len != 8) {
 681     MSG_ERR(0x1A67, "Error size SDO. CobId  : ", (*m).cob_id);
 682     failedSDO(d, nodeId, whoami, 0, 0, SDOABT_GENERAL_ERROR);
 683     return 0xFF;
 684   }
 685
 686   if (whoami == SDO_CLIENT) {
 687     MSG_WAR(0x3A68, "I am CLIENT. Received SDO from nodeId : ", nodeId);
 688   }
 689   else {
 690     MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", (*m).cob_id);
 691   }
 692
 693   /* Testing the command specifier */
 694   /* Allowed : cs = 0, 1, 2, 3, 4. (=  all except those for block tranfert). */
 695   /* cs = other : Not allowed -> abort. */
 696   switch (getSDOcs(m->data[0])) {
 697
 698   case 0:
 699     /* I am SERVER */
 700     if (whoami == SDO_SERVER) {
 701       /* Receiving a download segment data. */
 702       /* A SDO transfert should have been yet initiated. */
 703       err = getSDOlineOnUse( d, nodeId, whoami, &line );
 704       if (!err)
 705         err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
 706       if (err) {
 707         MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ",
 708                 nodeId);
 709         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
 710         return 0xFF;
 711       }
 712       /* Reset the wathdog */
 713       RestartSDO_TIMER(line)
 714       MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", nodeId);
 715       index = d->transfers[line].index;
 716       subIndex = d->transfers[line].subIndex;
 717       /* Toggle test. */
 718       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
 719         MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0]));
 720         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
 721         return 0xFF;
 722       }
 723       /* Nb of data to be downloaded */
 724       nbBytes = 7 - getSDOn3(m->data[0]);
 725       /* Store the data in the transfert structure. */
 726       err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
 727       if (err) {
 728         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
 729         return 0xFF;
 730       }
 731       /* Sending the SDO response, CS = 1 */
 732       sdo.nodeId = *d->bDeviceNodeId; /* The node id of the server, (here it is the sender). */
 733       sdo.body.data[0] = (1 << 5) | (d->transfers[line].toggle << 4);
 734       for (i = 1 ; i < 8 ; i++)
 735         sdo.body.data[i] = 0;
 736       MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", nodeId);
 737       sendSDO(d, whoami, sdo);
 738       /* Inverting the toggle for the next segment. */
 739       d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
 740       /* If it was the last segment, */
 741       if (getSDOc(m->data[0])) {
 742         /* Transfering line data to object dictionary. */
 743         /* The code does not use the "d" of initiate frame. So it is safe if e=s=0 */
 744         errorCode = SDOlineToObjdict(d, line);
 745         if (errorCode) {
 746           MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0);
 747           failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
 748           return 0xFF;
 749         }
 750         /* Release of the line */
 751         resetSDOline(d, line);
 752         MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", nodeId);
 753       }
 754     } /* end if SERVER */
 755     else { /* if CLIENT */
 756       /* I am CLIENT */
 757       /* It is a request for a previous upload segment. We should find a line opened for this.*/
 758       err = getSDOlineOnUse( d, nodeId, whoami, &line);
 759       if (!err)
 760         err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
 761       if (err) {
 762         MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId);
 763         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
 764         return 0xFF;
 765       }
 766       /* Reset the wathdog */
 767       RestartSDO_TIMER(line)
 768       index = d->transfers[line].index;
 769       subIndex = d->transfers[line].subIndex;
 770       /* test of the toggle; */
 771       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
 772         MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId);
 773         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
 774         return 0xFF;
 775       }
 776       /* nb of data to be uploaded */
 777       nbBytes = 7 - getSDOn3(m->data[0]);
 778       /* Storing the data in the line structure. */
 779       err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
 780       if (err) {
 781         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
 782         return 0xFF;
 783       }
 784       /* Inverting the toggle for the next segment. */
 785       d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
 786       /* If it was the last segment,*/
 787       if ( getSDOc(m->data[0])) {
 788         /* Put in state finished */
 789         /* The code is safe for the case e=s=0 in initiate frame. */
 790         StopSDO_TIMER(line)
 791         d->transfers[line].state = SDO_FINISHED;
 792         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
 793
 794         MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId);
 795       }
 796       else { /* more segments to receive */
 797              /* Sending the request for the next segment. */
 798         sdo.nodeId = nodeId;
 799         sdo.body.data[0] = (3 << 5) | (d->transfers[line].toggle << 4);
 800         for (i = 1 ; i < 8 ; i++)
 801           sdo.body.data[i] = 0;
 802         sendSDO(d, whoami, sdo);
 803         MSG_WAR(0x3A78, "SDO send upload segment request to nodeId", nodeId);
 804       }
 805     } /* End if CLIENT */
 806     break;
 807
 808   case 1:
 809     /* I am SERVER */
 810     /* Receive of an initiate download */
 811     if (whoami == SDO_SERVER) {
 812       index = getSDOindex(m->data[1],m->data[2]);
 813       subIndex = getSDOsubIndex(m->data[3]);
 814       MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ",
 815               nodeId);
 816       MSG_WAR(0x3A80, "Writing at index : ", index);
 817       MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex);
 818
 819       /* Search if a SDO transfert have been yet initiated */
 820       err = getSDOlineOnUse( d, nodeId, whoami, &line );
 821       if (! err) {
 822         MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0);
 823         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
 824         return 0xFF;
 825       }
 826       /* No line on use. Great ! */
 827       /* Try to open a new line. */
 828       err = getSDOfreeLine( d, whoami, &line );
 829       if (err) {
 830         MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
 831         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
 832         return 0xFF;
 833       }
 834       initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);
 835
 836       if (getSDOe(m->data[0])) { /* If SDO expedited */
 837         /* nb of data to be downloaded */
 838         nbBytes = 4 - getSDOn2(m->data[0]);
 839         /* Storing the data in the line structure. */
 840         d->transfers[line].count = nbBytes;
 841         err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
 842
 843         if (err) {
 844           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
 845           return 0xFF;
 846         }
 847
 848         /* SDO expedited -> transfert finished. Data can be stored in the dictionary. */
 849         /*The line will be reseted when it is downloading in the dictionary. */
 850         MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfert. Finished.: ", nodeId);
 851         /* Transfering line data to object dictionary. */
 852         errorCode = SDOlineToObjdict(d, line);
 853         if (errorCode) {
 854           MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0);
 855           failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
 856           return 0xFF;
 857         }
 858         /* Release of the line. */
 859         resetSDOline(d, line);
 860       }
 861       else {/* So, if it is not an expedited transfert */
 862         if (getSDOs(m->data[0])) {
 863           /* TODO : if e and s = 0, not reading m->data[4] but put nbBytes = 0 */
 864           nbBytes = m->data[4]; /* Transfert limited to 255 bytes. */
 865           err = setSDOlineRestBytes(d, nodeId, nbBytes);
 866           if (err) {
 867             failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
 868             return 0xFF;
 869           }
 870         }
 871       }
 872       /*Sending a SDO, cs=3*/
 873       sdo.nodeId = *d->bDeviceNodeId; /* The node id of the server, (here it is the sender).*/
 874       sdo.body.data[0] = 3 << 5;
 875       sdo.body.data[1] = index & 0xFF;        /* LSB */
 876       sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
 877       sdo.body.data[3] = subIndex;
 878       for (i = 4 ; i < 8 ; i++)
 879                 sdo.body.data[i] = 0;
 880       sendSDO(d, whoami, sdo);
 881     } /* end if I am SERVER */
 882     else {
 883       /* I am CLIENT */
 884       /* It is a response for a previous download segment. We should find a line opened for this. */
 885       err = getSDOlineOnUse( d, nodeId, whoami, &line);
 886       if (!err)
 887         err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
 888       if (err) {
 889         MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId);
 890         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
 891         return 0xFF;
 892       }
 893       /* Reset the wathdog */
 894       RestartSDO_TIMER(line)
 895       index = d->transfers[line].index;
 896       subIndex = d->transfers[line].subIndex;
 897       /* test of the toggle; */
 898       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
 899         MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId);
 900         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
 901         return 0xFF;
 902       }
 903
 904       /* End transmission or downloading next segment. We need to know if it will be the last one. */
 905       getSDOlineRestBytes(d, line, &nbBytes);
 906       if (nbBytes == 0) {
 907         MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId);
 908         StopSDO_TIMER(line)
 909         d->transfers[line].state = SDO_FINISHED;
 910         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
 911         return 0x00;
 912       }
 913       /* At least one transfer to send. */
 914       if (nbBytes > 7) {
 915         /* several segments to download.*/
 916         /* code to send the next segment. (cs = 0; c = 0) */
 917         d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
 918         sdo.nodeId = nodeId; /* The server node Id; */
 919         sdo.body.data[0] = (d->transfers[line].toggle << 4);
 920         err = lineToSDO(d, line, 7, sdo.body.data + 1);
 921         if (err) {
 922           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
 923           return 0xFF;
 924         }
 925       }
 926       else {
 927         /* Last segment. */
 928         /* code to send the last segment. (cs = 0; c = 1)*/
 929         d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
 930         sdo.nodeId = nodeId; /* The server node Id; */
 931         sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
 932         err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);
 933         if (err) {
 934           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
 935           return 0xFF;
 936         }
 937         for (i = nbBytes + 1 ; i < 8 ; i++)
 938           sdo.body.data[i] = 0;
 939       }
 940       MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId);
 941       sendSDO(d, whoami, sdo);
 942     } /* end if I am a CLIENT */
 943     break;
 944
 945   case 2:
 946     /* I am SERVER */
 947     /* Receive of an initiate upload.*/
 948     if (whoami == SDO_SERVER) {
 949       index = getSDOindex(m->data[1],m->data[2]);
 950       subIndex = getSDOsubIndex(m->data[3]);
 951       MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ",
 952               nodeId);
 953       MSG_WAR(0x3A90, "Reading at index : ", index);
 954       MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex);
 955       /* Search if a SDO transfert have been yet initiated*/
 956       err = getSDOlineOnUse( d, nodeId, whoami, &line );
 957       if (! err) {
 958             MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line);
 959         MSG_WAR(0x3A93, "nodeId = ", nodeId);
 960             failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
 961             return 0xFF;
 962       }
 963       /* No line on use. Great !*/
 964       /* Try to open a new line.*/
 965       err = getSDOfreeLine( d, whoami, &line );
 966       if (err) {
 967         MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
 968         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
 969         return 0xFF;
 970       }
 971       initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
 972       /* Transfer data from dictionary to the line structure. */
 973       errorCode = objdictToSDOline(d, line);
 974
 975       if (errorCode) {
 976         MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ",
 977                 errorCode);
 978         failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
 979         return 0xFF;
 980         }
 981       /* Preparing the response.*/
 982       getSDOlineRestBytes(d, line, &nbBytes);   /* Nb bytes to transfer ? */
 983       sdo.nodeId = nodeId; /* The server node Id; */
 984       if (nbBytes > 4) {
 985         /* normal transfert. (segmented). */
 986         /* code to send the initiate upload response. (cs = 2) */
 987         sdo.body.data[0] = (2 << 5) | 1;
 988         sdo.body.data[1] = index & 0xFF;        /* LSB */
 989         sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
 990         sdo.body.data[3] = subIndex;
 991         sdo.body.data[4] = nbBytes; /* Limitation of canfestival2 : Max tranfert is 256 bytes.*/
 992         /* It takes too much memory to upgrate to 2^32 because the size of data is also coded */
 993         /* in the object dictionary, at every index and subindex. */
 994         for (i = 5 ; i < 8 ; i++)
 995           sdo.body.data[i] = 0;
 996         MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId);
 997         sendSDO(d, whoami, sdo);
 998       }
 999       else {
1000         /* Expedited upload. (cs = 2 ; e = 1) */
1001         sdo.body.data[0] = (2 << 5) | ((4 - nbBytes) << 2) | 3;
1002         sdo.body.data[1] = index & 0xFF;        /* LSB */
1003         sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
1004         sdo.body.data[3] = subIndex;
1005         err = lineToSDO(d, line, nbBytes, sdo.body.data + 4);
1006         if (err) {
1007           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
1008           return 0xFF;
1009         }
1010         for (i = 4 + nbBytes ; i < 8 ; i++)
1011           sdo.body.data[i] = 0;
1012         MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ",
1013                 nodeId);
1014         sendSDO(d, whoami, sdo);
1015         /* Release the line.*/
1016         resetSDOline(d, line);
1017       }
1018     } /* end if I am SERVER*/
1019     else {
1020       /* I am CLIENT */
1021       /* It is the response for the previous initiate upload request.*/
1022       /* We should find a line opened for this. */
1023       err = getSDOlineOnUse( d, nodeId, whoami, &line);
1024       if (!err)
1025         err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
1026       if (err) {
1027         MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId);
1028         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
1029         return 0xFF;
1030       }
1031       /* Reset the wathdog */
1032       RestartSDO_TIMER(line)
1033       index = d->transfers[line].index;
1034       subIndex = d->transfers[line].subIndex;
1035
1036       if (getSDOe(m->data[0])) { /* If SDO expedited */
1037         /* nb of data to be uploaded */
1038           nbBytes = 4 - getSDOn2(m->data[0]);
1039         /* Storing the data in the line structure. */
1040         err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
1041         if (err) {
1042           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
1043           return 0xFF;
1044         }
1045         /* SDO expedited -> transfert finished. data are available via  getReadResultNetworkDict(). */
1046         MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId);
1047         StopSDO_TIMER(line)
1048         d->transfers[line].count = nbBytes;
1049         d->transfers[line].state = SDO_FINISHED;
1050         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
1051         return 0;
1052       }
1053       else { /* So, if it is not an expedited transfert */
1054         /* Storing the nb of data to receive. */
1055         if (getSDOs(m->data[0])) {
1056           nbBytes = m->data[4]; /* Remember the limitation to 255 bytes to transfert */
1057           err = setSDOlineRestBytes(d, line, nbBytes);
1058           if (err) {
1059             failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
1060             return 0xFF;
1061           }
1062         }
1063         /* Requesting next segment. (cs = 3) */
1064         sdo.nodeId = nodeId;
1065         sdo.body.data[0] = 3 << 5;
1066         for (i = 1 ; i < 8 ; i++)
1067           sdo.body.data[i] = 0;
1068         MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId);
1069         sendSDO(d, whoami, sdo);
1070       }
1071     } /* End if CLIENT */
1072     break;
1073
1074   case 3:
1075     /* I am SERVER */
1076     if (whoami == SDO_SERVER) {
1077       /* Receiving a upload segment. */
1078       /* A SDO transfert should have been yet initiated. */
1079       err = getSDOlineOnUse( d, nodeId, whoami, &line );
1080       if (!err)
1081         err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
1082       if (err) {
1083         MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ",
1084                 nodeId);
1085         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
1086         return 0xFF;
1087       }
1088       /* Reset the wathdog */
1089       RestartSDO_TIMER(line)
1090       MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", nodeId);
1091       index = d->transfers[line].index;
1092       subIndex = d->transfers[line].subIndex;
1093       /* Toggle test.*/
1094       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
1095         MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0]));
1096         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
1097         return 0xFF;
1098       }
1099       /* Uploading next segment. We need to know if it will be the last one. */
1100       getSDOlineRestBytes(d, line, &nbBytes);
1101       if (nbBytes > 7) {
1102         /* The segment to transfer is not the last one.*/
1103         /* code to send the next segment. (cs = 0; c = 0) */
1104         sdo.nodeId = nodeId; /* The server node Id; */
1105         sdo.body.data[0] = (d->transfers[line].toggle << 4);
1106         err = lineToSDO(d, line, 7, sdo.body.data + 1);
1107         if (err) {
1108           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
1109           return 0xFF;
1110         }
1111         /* Inverting the toggle for the next tranfert. */
1112         d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
1113         MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", nodeId);
1114         sendSDO(d, whoami, sdo);
1115       }
1116       else {
1117         /* Last segment. */
1118         /* code to send the last segment. (cs = 0; c = 1) */
1119         sdo.nodeId = nodeId; /** The server node Id; */
1120         sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
1121         err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);
1122         if (err) {
1123           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
1124           return 0xFF;
1125         }
1126         for (i = nbBytes + 1 ; i < 8 ; i++)
1127           sdo.body.data[i] = 0;
1128         MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", nodeId);
1129         sendSDO(d, whoami, sdo);
1130         /* Release the line */
1131         resetSDOline(d, line);
1132       }
1133     } /* end if SERVER*/
1134     else {
1135       /* I am CLIENT */
1136       /* It is the response for the previous initiate download request. */
1137       /* We should find a line opened for this. */
1138       err = getSDOlineOnUse( d, nodeId, whoami, &line);
1139       if (!err)
1140         err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
1141       if (err) {
1142         MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId);
1143         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
1144         return 0xFF;
1145       }
1146       /* Reset the watchdog */
1147       RestartSDO_TIMER(line)
1148       index = d->transfers[line].index;
1149       subIndex = d->transfers[line].subIndex;
1150       /* End transmission or requesting  next segment. */
1151       getSDOlineRestBytes(d, line, &nbBytes);
1152       if (nbBytes == 0) {
1153         MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId);
1154         StopSDO_TIMER(line)
1155         d->transfers[line].state = SDO_FINISHED;
1156         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
1157         return 0x00;
1158       }
1159       if (nbBytes > 7) {
1160         /* more than one request to send */
1161         /* code to send the next segment. (cs = 0; c = 0)       */
1162         sdo.nodeId = nodeId; /** The server node Id; */
1163         sdo.body.data[0] = (d->transfers[line].toggle << 4);
1164         err = lineToSDO(d, line, 7, sdo.body.data + 1);
1165         if (err) {
1166           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
1167           return 0xFF;
1168         }
1169       }
1170       else {
1171         /* Last segment.*/
1172         /* code to send the last segment. (cs = 0; c = 1)       */
1173         sdo.nodeId = nodeId; /* The server node Id; */
1174         sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
1175         err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);
1176         if (err) {
1177           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
1178           return 0xFF;
1179         }
1180         for (i = nbBytes + 1 ; i < 8 ; i++)
1181           sdo.body.data[i] = 0;
1182       }
1183       MSG_WAR(0x3AA7, "SDO sending download segment to nodeId", nodeId);
1184       sendSDO(d, whoami, sdo);
1185
1186     } /* end if I am a CLIENT           */
1187     break;
1188
1189    case 4:
1190      abortCode = (*m).data[3] |
1191       ((UNS32)m->data[5] << 8) |
1192       ((UNS32)m->data[6] << 16) |
1193       ((UNS32)m->data[7] << 24);
1194     /* Received SDO abort. */
1195     /* Looking for the line concerned. */
1196     if (whoami == SDO_SERVER) {
1197       err = getSDOlineOnUse( d, nodeId, whoami, &line );
1198       if (!err) {
1199         resetSDOline( d, line );
1200         MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode);
1201       }
1202       else
1203         MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode);
1204       /* Tips : The end user has no way to know that the server node has received an abort SDO. */
1205       /* Its is ok, I think.*/
1206     }
1207     else { /* If I am CLIENT */
1208       err = getSDOlineOnUse( d, nodeId, whoami, &line );
1209       if (!err) {
1210         /* The line *must* be released by the core program. */
1211         StopSDO_TIMER(line)
1212         d->transfers[line].state = SDO_ABORTED_RCV;
1213         d->transfers[line].abortCode = abortCode;
1214         MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode);
1215         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
1216       }
1217       else
1218         MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode);
1219     }
1220     break;
1221   default:
1222     /* Error : Unknown cs */
1223     MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", getSDOcs(m->data[0]));
1224     return 0xFF;
1225
1226   } /* End switch */
1227   return 0;
1228 }
1229
1230 /*!
1231 **
1232 **
1233 ** @param d
1234 ** @param nodeId
1235 ** @param index
1236 ** @param subIndex
1237 ** @param count
1238 ** @param dataType
1239 ** @param data
1240 ** @param Callback
1241 ** @param endianize
1242 **
1243 ** @return
1244 **/
1245 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
1246                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize)
1247 {
1248   UNS8 err;
1249   UNS8 SDOfound = 0;
1250   UNS8 line;
1251   s_SDO sdo;    /* SDO to transmit */
1252   UNS8 i, j;
1253   UNS16     lastIndex;
1254   UNS16     offset;
1255   UNS8      *pNodeIdServer;
1256   UNS8      nodeIdServer;
1257
1258   MSG_WAR(0x3AC0, "Send SDO to write in the dictionary of node : ", nodeId);
1259   MSG_WAR(0x3AC1, "                                   At index : ", index);
1260   MSG_WAR(0x3AC2, "                                   subIndex : ", subIndex);
1261   MSG_WAR(0x3AC3, "                                   nb bytes : ", count);
1262
1263   /* Verify that there is no SDO communication yet. */
1264   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1265   if (!err) {
1266     MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId);
1267     return 0xFF;
1268   }
1269   /* Taking the line ... */
1270   err = getSDOfreeLine( d, SDO_CLIENT, &line );
1271   if (err) {
1272     MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId);
1273     return (0xFF);
1274   }
1275   /* Check which SDO to use to communicate with the node */
1276   offset = d->firstIndex->SDO_CLT;
1277   lastIndex = d->lastIndex->SDO_CLT;
1278   if (offset == 0) {
1279     MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0);
1280     return 0xFF;
1281   }
1282   i = 0;
1283    while (offset <= lastIndex) {
1284      if (d->objdict[offset].bSubCount <= 3) {
1285          MSG_ERR(0x1AC8, "Subindex 3  not found at index ", 0x1280 + i);
1286          return 0xFF;
1287      }
1288      /* looking for the nodeId server */
1289      pNodeIdServer = (UNS8*) d->objdict[offset].pSubindex[3].pObject;
1290      nodeIdServer = *pNodeIdServer;
1291      MSG_WAR(0x1AD2, "index : ", 0x1280 + i);
1292      MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer);
1293
1294     if(nodeIdServer == nodeId) {
1295       SDOfound = 1;
1296       break;
1297     }
1298     offset++;
1299     i++;
1300   } /* end while */
1301   if (!SDOfound) {
1302     MSG_ERR(0x1AC9, "SDO. Error. No client found to communicate with node : ", nodeId);
1303     return 0xFF;
1304   }
1305   MSG_WAR(0x3AD0,"        SDO client defined at index  : ", 0x1280 + i);
1306   initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);
1307   d->transfers[line].count = count;
1308   d->transfers[line].dataType = dataType;
1309
1310   /* Copy data to transfers structure. */
1311   for (j = 0 ; j < count ; j++) {
1312 # ifdef CANOPEN_BIG_ENDIAN
1313     if (dataType == 0 && endianize)
1314       d->transfers[line].data[count - 1 - j] = ((char *)data)[j];
1315     else /* String of bytes. */
1316       d->transfers[line].data[j] = ((char *)data)[j];
1317 #  else
1318     d->transfers[line].data[j] = ((char *)data)[j];
1319 #  endif
1320   }
1321   /* Send the SDO to the server. Initiate download, cs=1. */
1322   sdo.nodeId = nodeId;
1323   if (count <= 4) { /* Expedited transfert */
1324     sdo.body.data[0] = (1 << 5) | ((4 - count) << 2) | 3;
1325     for (i = 4 ; i < 8 ; i++)
1326       sdo.body.data[i] = d->transfers[line].data[i - 4];
1327     d->transfers[line].offset = count;
1328   }
1329   else { /** Normal transfert */
1330     sdo.body.data[0] = (1 << 5) | 1;
1331     sdo.body.data[4] = count; /* nb of byte to transmit. Max = 255. (canfestival2 limitation). */
1332     for (i = 5 ; i < 8 ; i++)
1333       sdo.body.data[i] = 0;
1334   }
1335   sdo.body.data[1] = index & 0xFF;        /* LSB */
1336   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
1337   sdo.body.data[3] = subIndex;
1338
1339   d->transfers[line].Callback = Callback;
1340
1341   err = sendSDO(d, SDO_CLIENT, sdo);
1342   if (err) {
1343     MSG_ERR(0x1AD1, "SDO. Error while sending SDO to node : ", nodeId);
1344     /* release the line */
1345     resetSDOline(d, line);
1346     return 0xFF;
1347   }
1348
1349
1350   return 0;
1351 }
1352
1353 /*!
1354 **
1355 **
1356 ** @param d
1357 ** @param nodeId
1358 ** @param index
1359 ** @param subIndex
1360 ** @param count
1361 ** @param dataType
1362 ** @param data
1363 **
1364 ** @return
1365 **/
1366 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
1367                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data)
1368 {
1369         return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, NULL, 1);
1370 }
1371
1372 /*!
1373 **
1374 **
1375 ** @param d
1376 ** @param nodeId
1377 ** @param index
1378 ** @param subIndex
1379 ** @param count
1380 ** @param dataType
1381 ** @param data
1382 ** @param Callback
1383 **
1384 ** @return
1385 **/
1386 UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index,
1387                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback)
1388 {
1389         return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1);
1390 }
1391
1392 /*!
1393 **
1394 **
1395 ** @param d
1396 ** @param nodeId
1397 ** @param index
1398 ** @param subIndex
1399 ** @param dataType
1400 ** @param Callback
1401 **
1402 ** @return
1403 **/
1404 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
1405 {
1406   UNS8 err;
1407   UNS8 SDOfound = 0;
1408   UNS8 i;
1409   UNS8 line;
1410   s_SDO sdo;    /* SDO to transmit */
1411   UNS8      *pNodeIdServer;
1412   UNS8      nodeIdServer;
1413   UNS16     offset;
1414   UNS16     lastIndex;
1415   MSG_WAR(0x3AD5, "Send SDO to read in the dictionary of node : ", nodeId);
1416   MSG_WAR(0x3AD6, "                                  At index : ", index);
1417   MSG_WAR(0x3AD7, "                                  subIndex : ", subIndex);
1418
1419
1420   /* Verify that there is no SDO communication yet. */
1421   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1422   if (!err) {
1423     MSG_ERR(0x1AD8, "SDO error : Communication yet established. with node : ", nodeId);
1424     return 0xFF;
1425   }
1426   /* Taking the line ... */
1427   err = getSDOfreeLine( d, SDO_CLIENT, &line );
1428   if (err) {
1429     MSG_ERR(0x1AD9, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId);
1430     return (0xFF);
1431   }
1432   else
1433     MSG_WAR(0x3AE0, "Transmission on line : ", line);
1434
1435   /* Check which SDO to use to communicate with the node */
1436   offset = d->firstIndex->SDO_CLT;
1437   lastIndex = d->lastIndex->SDO_CLT;
1438   if (offset == 0) {
1439     MSG_ERR(0x1AE1, "writeNetworkDict : No SDO client index found", 0);
1440     return 0xFF;
1441   }
1442   i = 0;
1443   while (offset <= lastIndex) {
1444      if (d->objdict[offset].bSubCount <= 3) {
1445          MSG_ERR(0x1AE2, "Subindex 3  not found at index ", 0x1280 + i);
1446          return 0xFF;
1447      }
1448      /* looking for the nodeId server */
1449      pNodeIdServer = (UNS8*) d->objdict[offset].pSubindex[3].pObject;
1450      nodeIdServer = *pNodeIdServer;
1451
1452     if(nodeIdServer == nodeId) {
1453       SDOfound = 1;
1454       break;
1455     }
1456     offset++;
1457     i++;
1458   } /* end while */
1459   if (!SDOfound) {
1460     MSG_ERR(0x1AE3, "SDO. Error. No client found to communicate with node : ", nodeId);
1461     return 0xFF;
1462   }
1463   MSG_WAR(0x3AE4,"        SDO client defined at index  : ", 0x1280 + i);
1464   initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
1465   getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1466   sdo.nodeId = nodeId;
1467   /* Send the SDO to the server. Initiate upload, cs=2. */
1468   d->transfers[line].dataType = dataType;
1469   sdo.body.data[0] = (2 << 5);
1470   sdo.body.data[1] = index & 0xFF;        /* LSB */
1471   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
1472   sdo.body.data[3] = subIndex;
1473   for (i = 4 ; i < 8 ; i++)
1474     sdo.body.data[i] = 0;
1475   d->transfers[line].Callback = Callback;
1476   err = sendSDO(d, SDO_CLIENT, sdo);
1477   if (err) {
1478     MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId);
1479     /* release the line */
1480     resetSDOline(d, line);
1481     return 0xFF;
1482   }
1483   return 0;
1484 }
1485
1486 /*!
1487 **
1488 **
1489 ** @param d
1490 ** @param nodeId
1491 ** @param index
1492 ** @param subIndex
1493 ** @param dataType
1494 **
1495 ** @return
1496 **/
1497 UNS8 readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType)
1498 {
1499         return _readNetworkDict (d, nodeId, index, subIndex, dataType, NULL);
1500 }
1501
1502 /*!
1503 **
1504 **
1505 ** @param d
1506 ** @param nodeId
1507 ** @param index
1508 ** @param subIndex
1509 ** @param dataType
1510 ** @param Callback
1511 **
1512 ** @return
1513 **/
1514 UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
1515 {
1516         return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback);
1517 }
1518
1519 /*!
1520 **
1521 **
1522 ** @param d
1523 ** @param nodeId
1524 ** @param data
1525 ** @param size
1526 ** @param abortCode
1527 **
1528 ** @return
1529 **/
1530 UNS8 getReadResultNetworkDict (CO_Data* d, UNS8 nodeId, void* data, UNS8 *size,
1531                                UNS32 * abortCode)
1532 {
1533   UNS8 i;
1534   UNS8 err;
1535   UNS8 line;
1536   * size = 0;
1537
1538   /* Looking for the line tranfert. */
1539   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1540   if (err) {
1541     MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId);
1542     return SDO_ABORTED_INTERNAL;
1543   }
1544   if (d->transfers[line].state != SDO_FINISHED)
1545     return d->transfers[line].state;
1546
1547   /* Transfert is finished. Put the value in the data. */
1548   * size = (UNS8)d->transfers[line].count;
1549   for  ( i = 0 ; i < *size ; i++) {
1550 # ifdef CANOPEN_BIG_ENDIAN
1551     if (d->transfers[line].dataType != visible_string)
1552       ( (char *) data)[*size - 1 - i] = d->transfers[line].data[i];
1553     else /* String of bytes. */
1554       ( (char *) data)[i] = d->transfers[line].data[i];
1555 # else
1556     ( (char *) data)[i] = d->transfers[line].data[i];
1557 # endif
1558   }
1559   return SDO_FINISHED;
1560 }
1561
1562 /*!
1563 **
1564 **
1565 ** @param d
1566 ** @param nodeId
1567 ** @param abortCode
1568 **
1569 ** @return
1570 **/
1571 UNS8 getWriteResultNetworkDict (CO_Data* d, UNS8 nodeId, UNS32 * abortCode)
1572 {
1573   UNS8 line = 0;
1574   UNS8 err;
1575
1576   * abortCode = 0;
1577   /* Looking for the line tranfert. */
1578   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1579   if (err) {
1580     MSG_ERR(0x1AF1, "SDO error : No line found for communication with node : ", nodeId);
1581     return SDO_ABORTED_INTERNAL;
1582   }
1583   * abortCode = d->transfers[line].abortCode;
1584   return d->transfers[line].state;
1585 }