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