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