src/sdo.c

   1 /*
   2 This file is part of CanFestival, a library implementing CanOpen Stack.
   3
   4 Copyright (C): Edouard TISSERANT and Francis DUPIN
   5
   6 See COPYING file for copyrights details.
   7
   8 This library is free software; you can redistribute it and/or
   9 modify it under the terms of the GNU Lesser General Public
  10 License as published by the Free Software Foundation; either
  11 version 2.1 of the License, or (at your option) any later version.
  12
  13 This library is distributed in the hope that it will be useful,
  14 but WITHOUT ANY WARRANTY; without even the implied warranty of
  15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16 Lesser General Public License for more details.
  17
  18 You should have received a copy of the GNU Lesser General Public
  19 License along with this library; if not, write to the Free Software
  20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  21 */
  22 /*!
  23 ** @file   sdo.c
  24 ** @author Edouard TISSERANT and Francis DUPIN
  25 ** @date   Tue Jun  5 09:32:32 2007
  26 **
  27 ** @brief
  28 **
  29 **
  30 */
  31
  32 /* #define DEBUG_WAR_CONSOLE_ON */
  33 /* #define DEBUG_ERR_CONSOLE_ON */
  34
  35 #include "objacces.h"
  36 #include "sdo.h"
  37 #include "canfestival.h"
  38
  39 /* Uncomment if your compiler does not support inline functions */
  40 #define NO_INLINE
  41
  42 #ifdef NO_INLINE
  43   #define INLINE
  44 #else
  45   #define INLINE inline
  46 #endif
  47
  48 /*Internals prototypes*/
  49
  50 /*!
  51 ** Called by writeNetworkDict
  52 **
  53 ** @param d
  54 ** @param nodeId
  55 ** @param index
  56 ** @param subIndex
  57 ** @param count
  58 ** @param dataType
  59 ** @param data
  60 ** @param Callback
  61 ** @param endianize
  62 **
  63 ** @return
  64 **/
  65 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
  66                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize);
  67
  68 /*!
  69 ** Called by readNetworkDict
  70 **
  71 ** @param d
  72 ** @param nodeId
  73 ** @param index
  74 ** @param subIndex
  75 ** @param dataType
  76 ** @param Callback
  77 **
  78 ** @return
  79 **/
  80 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex,
  81         UNS8 dataType, SDOCallback_t Callback);
  82
  83
  84 /***************************************************************************/
  85 /* SDO (un)packing macros */
  86
  87 /** Returns the command specifier (cs, ccs, scs) from the first byte of the SDO
  88  */
  89 #define getSDOcs(byte) (byte >> 5)
  90
  91 /** Returns the number of bytes without data from the first byte of the SDO. Coded in 2 bits
  92  */
  93 #define getSDOn2(byte) ((byte >> 2) & 3)
  94
  95 /** Returns the number of bytes without data from the first byte of the SDO. Coded in 3 bits
  96  */
  97 #define getSDOn3(byte) ((byte >> 1) & 7)
  98
  99 /** Returns the transfer type from the first byte of the SDO
 100  */
 101 #define getSDOe(byte) ((byte >> 1) & 1)
 102
 103 /** Returns the size indicator from the first byte of the SDO
 104  */
 105 #define getSDOs(byte) (byte & 1)
 106
 107 /** Returns the indicator of end transmission from the first byte of the SDO
 108  */
 109 #define getSDOc(byte) (byte & 1)
 110
 111 /** Returns the toggle from the first byte of the SDO
 112  */
 113 #define getSDOt(byte) ((byte >> 4) & 1)
 114
 115 /** Returns the index from the bytes 1 and 2 of the SDO
 116  */
 117 #define getSDOindex(byte1, byte2) ((byte2 << 8) | (byte1))
 118
 119 /** Returns the subIndex from the byte 3 of the SDO
 120  */
 121 #define getSDOsubIndex(byte3) (byte3)
 122
 123 /*!
 124 **
 125 **
 126 ** @param d
 127 ** @param id
 128 **/
 129 void SDOTimeoutAlarm(CO_Data* d, UNS32 id)
 130 {
 131     MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0);
 132     MSG_WAR(0x2A02, "server node : ", d->transfers[id].nodeId);
 133     MSG_WAR(0x2A02, "      index : ", d->transfers[id].index);
 134     MSG_WAR(0x2A02, "   subIndex : ", d->transfers[id].subIndex);
 135     /* Reset timer handler */
 136     d->transfers[id].timer = TIMER_NONE;
 137     /*Set aborted state*/
 138     d->transfers[id].state = SDO_ABORTED_INTERNAL;
 139     /* Sending a SDO abort */
 140     sendSDOabort(d, d->transfers[id].whoami,
 141                  d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT);
 142     d->transfers[id].abortCode = SDOABT_TIMED_OUT;
 143     /* Call the user function to inform of the problem.*/
 144     if(d->transfers[id].Callback)
 145         /*If ther is a callback, it is responsible to close SDO transfer (client)*/
 146         (*d->transfers[id].Callback)(d,d->transfers[id].nodeId);
 147     else if(d->transfers[id].whoami == SDO_SERVER)
 148         /*Else, if server, reset the line*/
 149         resetSDOline(d, (UNS8)id);
 150 }
 151
 152 #define StopSDO_TIMER(id) \
 153 MSG_WAR(0x3A05, "StopSDO_TIMER for line : ", line);\
 154 d->transfers[id].timer = DelAlarm(d->transfers[id].timer);
 155
 156 #define StartSDO_TIMER(id) \
 157 MSG_WAR(0x3A06, "StartSDO_TIMER for line : ", line);\
 158 d->transfers[id].timer = SetAlarm(d,id,&SDOTimeoutAlarm,MS_TO_TIMEVAL(SDO_TIMEOUT_MS),0);
 159
 160 #define RestartSDO_TIMER(id) \
 161 MSG_WAR(0x3A07, "restartSDO_TIMER for line : ", line);\
 162 if(d->transfers[id].timer != TIMER_NONE) { StopSDO_TIMER(id) StartSDO_TIMER(id) }
 163
 164 /*!
 165 ** Reset all sdo buffers
 166 **
 167 ** @param d
 168 **/
 169 void resetSDO (CO_Data* d)
 170 {
 171   UNS8 j;
 172
 173   /** transfer structure initialization */
 174     for (j = 0 ; j < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; j++)
 175       resetSDOline(d, j);
 176 }
 177
 178 /*!
 179 **
 180 **
 181 ** @param d
 182 ** @param line
 183 **
 184 ** @return
 185 **/
 186 UNS32 SDOlineToObjdict (CO_Data* d, UNS8 line)
 187 {
 188   UNS8      size;
 189   UNS32 errorCode;
 190   MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line);
 191   size = (UNS8)d->transfers[line].count;
 192   errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,
 193                          (void *) d->transfers[line].data, &size, 1);
 194   if (errorCode != OD_SUCCESSFUL)
 195     return errorCode;
 196   MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line);
 197   return 0;
 198
 199 }
 200
 201 /*!
 202 **
 203 **
 204 ** @param d
 205 ** @param line
 206 **
 207 ** @return
 208 **/
 209 UNS32 objdictToSDOline (CO_Data* d, UNS8 line)
 210 {
 211   UNS8  size = 0;
 212   UNS8  dataType;
 213   UNS32 errorCode;
 214
 215   MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index);
 216   MSG_WAR(0x3A06, "  subIndex : ", d->transfers[line].subIndex);
 217
 218   errorCode = getODentry(d,     d->transfers[line].index,
 219                                 d->transfers[line].subIndex,
 220                                 (void *)d->transfers[line].data,
 221                                 &size, &dataType, 0);
 222
 223   if (errorCode != OD_SUCCESSFUL)
 224     return errorCode;
 225
 226   d->transfers[line].count = size;
 227   d->transfers[line].offset = 0;
 228 #if 0
 229    /*Me laisser a, please ! (FD)*/
 230   {
 231     UNS8 i;
 232     for (i = 0 ; i < 10 ; i++) {
 233       MSG_WAR(i, "data= ", d->transfers[line].data[i]);
 234     }
 235   }
 236 #endif
 237   return 0;
 238 }
 239
 240 /*!
 241 **
 242 **
 243 ** @param d
 244 ** @param line
 245 ** @param nbBytes
 246 ** @param data
 247 **
 248 ** @return
 249 **/
 250 UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data) {
 251   UNS8 i;
 252   UNS8 offset;
 253
 254   if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
 255     MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
 256     return 0xFF;
 257   }
 258     if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) {
 259     MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes);
 260     return 0xFF;
 261   }
 262   offset = (UNS8)d->transfers[line].offset;
 263   for (i = 0 ; i < nbBytes ; i++)
 264     * (data + i) = d->transfers[line].data[offset + i];
 265   d->transfers[line].offset = d->transfers[line].offset + nbBytes;
 266   return 0;
 267 }
 268
 269 /*!
 270 **
 271 **
 272 ** @param d
 273 ** @param line
 274 ** @param nbBytes
 275 ** @param data
 276 **
 277 ** @return
 278 **/
 279 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data)
 280 {
 281   UNS8 i;
 282   UNS8 offset;
 283
 284   if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
 285     MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
 286     return 0xFF;
 287   }
 288   offset = (UNS8)d->transfers[line].offset;
 289   for (i = 0 ; i < nbBytes ; i++)
 290     d->transfers[line].data[offset + i] = * (data + i);
 291   d->transfers[line].offset = d->transfers[line].offset + nbBytes;
 292   return 0;
 293 }
 294
 295 /*!
 296 **
 297 **
 298 ** @param d
 299 ** @param nodeId
 300 ** @param whoami
 301 ** @param index
 302 ** @param subIndex
 303 ** @param abortCode
 304 **
 305 ** @return
 306 **/
 307 UNS8 failedSDO (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS16 index,
 308                 UNS8 subIndex, UNS32 abortCode)
 309 {
 310   UNS8 err;
 311   UNS8 line;
 312   err = getSDOlineOnUse( d, nodeId, whoami, &line );
 313   if (!err) /* If a line on use have been found.*/
 314     MSG_WAR(0x3A20, "FailedSDO : line found : ", line);
 315   if ((! err) && (whoami == SDO_SERVER)) {
 316     resetSDOline( d, line );
 317     MSG_WAR(0x3A21, "FailedSDO : line released : ", line);
 318   }
 319   if ((! err) && (whoami == SDO_CLIENT)) {
 320     StopSDO_TIMER(line);
 321     d->transfers[line].state = SDO_ABORTED_INTERNAL;
 322   }
 323   MSG_WAR(0x3A22, "Sending SDO abort ", 0);
 324   err = sendSDOabort(d, whoami, index, subIndex, abortCode);
 325   if (err) {
 326     MSG_WAR(0x3A23, "Unable to send the SDO abort", 0);
 327     return 0xFF;
 328   }
 329   return 0;
 330 }
 331
 332 /*!
 333 **
 334 **
 335 ** @param d
 336 ** @param line
 337 **/
 338 void resetSDOline ( CO_Data* d, UNS8 line )
 339 {
 340   UNS8 i;
 341   MSG_WAR(0x3A25, "reset SDO line nb : ", line);
 342   initSDOline(d, line, 0, 0, 0, SDO_RESET);
 343   for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++)
 344     d->transfers[line].data[i] = 0;
 345 }
 346
 347 /*!
 348 **
 349 **
 350 ** @param d
 351 ** @param line
 352 ** @param nodeId
 353 ** @param index
 354 ** @param subIndex
 355 ** @param state
 356 **
 357 ** @return
 358 **/
 359 UNS8 initSDOline (CO_Data* d, UNS8 line, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 state)
 360 {
 361   MSG_WAR(0x3A25, "init SDO line nb : ", line);
 362   if (state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS){
 363         StartSDO_TIMER(line)
 364   }else{
 365         StopSDO_TIMER(line)
 366   }
 367   d->transfers[line].nodeId = nodeId;
 368   d->transfers[line].index = index;
 369   d->transfers[line].subIndex = subIndex;
 370   d->transfers[line].state = state;
 371   d->transfers[line].toggle = 0;
 372   d->transfers[line].count = 0;
 373   d->transfers[line].offset = 0;
 374   d->transfers[line].dataType = 0;
 375   d->transfers[line].Callback = NULL;
 376   return 0;
 377 }
 378
 379 /*!
 380 **
 381 **
 382 ** @param d
 383 ** @param whoami
 384 ** @param line
 385 **
 386 ** @return
 387 **/
 388 UNS8 getSDOfreeLine ( CO_Data* d, UNS8 whoami, UNS8 *line )
 389 {
 390
 391   UNS8 i;
 392
 393   for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
 394     if ( d->transfers[i].state == SDO_RESET ) {
 395       *line = i;
 396       d->transfers[i].whoami = whoami;
 397       return 0;
 398     } /* end if */
 399   } /* end for */
 400   MSG_ERR(0x1A25, "Too many SDO in progress. Aborted.", i);
 401   return 0xFF;
 402 }
 403
 404 /*!
 405 **
 406 **
 407 ** @param d
 408 ** @param nodeId
 409 ** @param whoami
 410 ** @param line
 411 **
 412 ** @return
 413 **/
 414 UNS8 getSDOlineOnUse (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS8 *line)
 415 {
 416
 417   UNS8 i;
 418
 419   for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
 420     if ( (d->transfers[i].state != SDO_RESET) &&
 421          (d->transfers[i].nodeId == nodeId) &&
 422          (d->transfers[i].whoami == whoami) ) {
 423       *line = i;
 424       return 0;
 425     }
 426   }
 427   return 0xFF;
 428 }
 429
 430 /*!
 431 **
 432 **
 433 ** @param d
 434 ** @param nodeId
 435 ** @param whoami
 436 **
 437 ** @return
 438 **/
 439 UNS8 closeSDOtransfer (CO_Data* d, UNS8 nodeId, UNS8 whoami)
 440 {
 441   UNS8 err;
 442   UNS8 line;
 443   err = getSDOlineOnUse(d, nodeId, whoami, &line);
 444   if (err) {
 445     MSG_WAR(0x2A30, "No SDO communication to close for node : ", nodeId);
 446     return 0xFF;
 447   }
 448   resetSDOline(d, line);
 449   return 0;
 450 }
 451
 452 /*!
 453 **
 454 **
 455 ** @param d
 456 ** @param line
 457 ** @param nbBytes
 458 **
 459 ** @return
 460 **/
 461 UNS8 getSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 * nbBytes)
 462 {
 463   if (d->transfers[line].count == 0) /* if received initiate SDO protocol with e=0 and s=0 */
 464     * nbBytes = 0;
 465   else
 466     * nbBytes = (UNS8)d->transfers[line].count - (UNS8)d->transfers[line].offset;
 467   return 0;
 468 }
 469
 470 /*!
 471 **
 472 **
 473 ** @param d
 474 ** @param line
 475 ** @param nbBytes
 476 **
 477 ** @return
 478 **/
 479 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 nbBytes)
 480 {
 481   if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) {
 482     MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
 483     return 0xFF;
 484   }
 485   d->transfers[line].count = nbBytes;
 486   return 0;
 487 }
 488
 489 /*!
 490 **
 491 **
 492 ** @param d
 493 ** @param whoami
 494 ** @param sdo
 495 **
 496 ** @return
 497 **/
 498 UNS8 sendSDO (CO_Data* d, UNS8 whoami, s_SDO sdo)
 499 {
 500   UNS16 offset;
 501   UNS16 lastIndex;
 502   UNS8 found = 0;
 503   Message m;
 504   UNS8 i;
 505   UNS32 * pwCobId = NULL;
 506   UNS32 * pwNodeId = NULL;
 507
 508   MSG_WAR(0x3A38, "sendSDO",0);
 509   if( !((d->nodeState == Operational) ||  (d->nodeState == Pre_operational ))) {
 510     MSG_WAR(0x2A39, "unable to send the SDO (not in op or pre-op mode", d->nodeState);
 511     return 0xFF;
 512   }
 513
 514   /**get the server->client cobid*/
 515   if ( whoami == SDO_SERVER )   {/**case server. Easy because today only one server SDO is authorized in CanFestival*/
 516     offset = d->firstIndex->SDO_SVR;
 517     if (offset == 0) {
 518       MSG_ERR(0x1A42, "SendSDO : No SDO server found", 0);
 519       return 0xFF;
 520     }
 521     pwCobId = d->objdict[offset].pSubindex[2].pObject;
 522     MSG_WAR(0x3A41, "I am server. cobId : ", *pwCobId);
 523   }
 524   else {                        /**case client*/
 525     /** Get the client->server cobid.*/
 526     UNS16 sdoNum = 0;
 527     offset = d->firstIndex->SDO_CLT;
 528     lastIndex = d->lastIndex->SDO_CLT;
 529     if (offset == 0) {
 530       MSG_ERR(0x1A42, "SendSDO : No SDO client index found", 0);
 531       return 0xFF;
 532     }
 533     /** First, have to find at the index where is defined the communication with the server node */
 534     while (offset <= lastIndex){
 535       MSG_WAR(0x3A43,"Reading index : ", 0x1280 + sdoNum);
 536       if (d->objdict[offset].bSubCount <= 3) {
 537         MSG_ERR(0x1A28, "Subindex 3  not found at index ", 0x1280 + sdoNum);
 538         return 0xFF;
 539       }
 540       pwNodeId = d->objdict[offset].pSubindex[3].pObject;
 541       MSG_WAR(0x3A44, "Found nodeId server = ", *pwNodeId);
 542       if(*pwNodeId == sdo.nodeId) {
 543         found = 1;
 544         break;
 545       }
 546       offset ++;
 547       sdoNum ++;
 548     }
 549     if (! found){
 550       MSG_WAR (0x2A45, "No SDO client corresponds to the mesage to send to node ", sdo.nodeId);
 551       return 0xFF;
 552     }
 553     /** Second, read the cobid client->server */
 554     pwCobId = d->objdict[offset].pSubindex[1].pObject;
 555   }
 556   /** message copy for sending */
 557   m.cob_id.w = *pwCobId;
 558   m.rtr = NOT_A_REQUEST;
 559   /** the length of SDO must be 8 */
 560   m.len = 8;
 561   for (i = 0 ; i < 8 ; i++) {
 562     m.data[i] =  sdo.body.data[i];
 563   }
 564   return canSend(d->canHandle,&m);
 565 }
 566
 567 /*!
 568 **
 569 **
 570 ** @param d
 571 ** @param whoami
 572 ** @param index
 573 ** @param subIndex
 574 ** @param abortCode
 575 **
 576 ** @return
 577 **/
 578 UNS8 sendSDOabort (CO_Data* d, UNS8 whoami, UNS16 index, UNS8 subIndex, UNS32 abortCode)
 579 {
 580   s_SDO sdo;
 581   UNS8 ret;
 582   MSG_WAR(0x2A50,"Sending SDO abort ", abortCode);
 583   sdo.nodeId = *d->bDeviceNodeId;
 584   sdo.body.data[0] = 0x80;
 585   /* Index */
 586   sdo.body.data[1] = index & 0xFF; /* LSB */
 587   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
 588   /* Subindex */
 589   sdo.body.data[3] = subIndex;
 590   /* Data */
 591   sdo.body.data[4] = (UNS8)(abortCode & 0xFF);
 592   sdo.body.data[5] = (UNS8)((abortCode >> 8) & 0xFF);
 593   sdo.body.data[6] = (UNS8)((abortCode >> 16) & 0xFF);
 594   sdo.body.data[7] = (UNS8)((abortCode >> 24) & 0xFF);
 595   ret = sendSDO(d, whoami, sdo);
 596
 597   return ret;
 598 }
 599
 600 /*!
 601 **
 602 **
 603 ** @param d
 604 ** @param m
 605 **
 606 ** @return
 607 **/
 608 UNS8 proceedSDO (CO_Data* d, Message *m)
 609 {
 610   UNS8 err;
 611   UNS8 line;
 612   UNS8 nbBytes; /** received or to be transmited. */
 613   UNS8 nodeId = 0;  /** The node from which the SDO is received */
 614   UNS32 nodeId_32; /** node id in 32 bits, for temporary use */
 615   UNS32 *pNodeId = NULL;
 616   UNS8 whoami = SDO_UNKNOWN;  /** SDO_SERVER or SDO_CLIENT.*/
 617   UNS32 errorCode; /** while reading or writing in the local object dictionary.*/
 618   s_SDO sdo;    /** SDO to transmit */
 619   UNS16 index;
 620   UNS8 subIndex;
 621   UNS32 abortCode;
 622   UNS8 i,j;
 623   UNS32 *     pCobId = NULL;
 624   UNS16 offset;
 625   UNS16 lastIndex;
 626
 627   MSG_WAR(0x3A60, "proceedSDO ", 0);
 628   whoami = SDO_UNKNOWN;
 629   /** Looking for the cobId in the object dictionary. */
 630   /** Am-I a server ? */
 631   offset = d->firstIndex->SDO_SVR;
 632   lastIndex = d->lastIndex->SDO_SVR;
 633   j = 0;
 634   if(offset) while (offset <= lastIndex) {
 635      if (d->objdict[offset].bSubCount <= 1) {
 636           MSG_ERR(0x1A61, "Subindex 1  not found at index ", 0x1200 + j);
 637           return 0xFF;
 638         }
 639       pCobId = d->objdict[offset].pSubindex[1].pObject;
 640       if ( *pCobId == (*m).cob_id.w ) {
 641         whoami = SDO_SERVER;
 642         MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j);
 643         /** In case of server, the node id of the client may be unknown. So we put the index minus offset */
 644         /** 0x1200 where the cobid received is defined. */
 645         nodeId = j;
 646         break;
 647       }
 648       j++;
 649       offset++;
 650   } /** end while */
 651   if (whoami == SDO_UNKNOWN) {
 652     /** Am-I client ? */
 653     offset = d->firstIndex->SDO_CLT;
 654     lastIndex = d->lastIndex->SDO_CLT;
 655     j = 0;
 656     if(offset) while (offset <= lastIndex) {
 657        if (d->objdict[offset].bSubCount <= 3) {
 658          MSG_ERR(0x1A63, "Subindex 3  not found at index ", 0x1280 + j);
 659          return 0xFF;
 660        }
 661        /** a) Looking for the cobid received. */
 662        pCobId = d->objdict[offset].pSubindex[2].pObject;
 663        if (*pCobId == (*m).cob_id.w ) {
 664          /** b) cobid found, so reading the node id of the server. */
 665          pNodeId = d->objdict[offset].pSubindex[3].pObject;
 666          whoami = SDO_CLIENT;
 667          nodeId_32 = *pNodeId;
 668          nodeId = (UNS8)nodeId_32;
 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  : ", (*m).cob_id.w);
 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 : ", (*m).cob_id.w);
 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 = (*m).data[3] |
1193       ((UNS32)m->data[5] << 8) |
1194       ((UNS32)m->data[6] << 16) |
1195       ((UNS32)m->data[7] << 24);
1196     /** Received SDO abort. */
1197     /** Looking for the line concerned. */
1198     if (whoami == SDO_SERVER) {
1199       err = getSDOlineOnUse( d, nodeId, whoami, &line );
1200       if (!err) {
1201         resetSDOline( d, line );
1202         MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode);
1203       }
1204       else
1205         MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode);
1206       /** Tips : The end user has no way to know that the server node has received an abort SDO. */
1207       /** Its is ok, I think.*/
1208     }
1209     else { /** If I am CLIENT */
1210       err = getSDOlineOnUse( d, nodeId, whoami, &line );
1211       if (!err) {
1212         /** The line *must* be released by the core program. */
1213         StopSDO_TIMER(line)
1214         d->transfers[line].state = SDO_ABORTED_RCV;
1215         d->transfers[line].abortCode = abortCode;
1216         MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode);
1217         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
1218       }
1219       else
1220         MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode);
1221     }
1222     break;
1223   default:
1224     /** Error : Unknown cs */
1225     MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", getSDOcs(m->data[0]));
1226     return 0xFF;
1227
1228   } /* End switch */
1229   return 0;
1230 }
1231
1232 /*!
1233 **
1234 **
1235 ** @param d
1236 ** @param nodeId
1237 ** @param index
1238 ** @param subIndex
1239 ** @param count
1240 ** @param dataType
1241 ** @param data
1242 ** @param Callback
1243 ** @param endianize
1244 **
1245 ** @return
1246 **/
1247 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
1248                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize)
1249 {
1250   UNS8 err;
1251   UNS8 SDOfound = 0;
1252   UNS8 line;
1253   s_SDO sdo;    /* SDO to transmit */
1254   UNS8 i, j;
1255   UNS16     lastIndex;
1256   UNS16     offset;
1257   UNS32      *pNodeIdServer;
1258   UNS32      nodeIdServer;
1259
1260   MSG_WAR(0x3AC0, "Send SDO to write in the dictionary of node : ", nodeId);
1261   MSG_WAR(0x3AC1, "                                   At index : ", index);
1262   MSG_WAR(0x3AC2, "                                   subIndex : ", subIndex);
1263   MSG_WAR(0x3AC3, "                                   nb bytes : ", count);
1264
1265   /** Verify that there is no SDO communication yet. */
1266   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1267   if (!err) {
1268     MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId);
1269     return 0xFF;
1270   }
1271   /** Taking the line ... */
1272   err = getSDOfreeLine( d, SDO_CLIENT, &line );
1273   if (err) {
1274     MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId);
1275     return (0xFF);
1276   }
1277   /** Check which SDO to use to communicate with the node */
1278   offset = d->firstIndex->SDO_CLT;
1279   lastIndex = d->lastIndex->SDO_CLT;
1280   if (offset == 0) {
1281     MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0);
1282     return 0xFF;
1283   }
1284   i = 0;
1285    while (offset <= lastIndex) {
1286      if (d->objdict[offset].bSubCount <= 3) {
1287          MSG_ERR(0x1AC8, "Subindex 3  not found at index ", 0x1280 + i);
1288          return 0xFF;
1289      }
1290      /** looking for the nodeId server */
1291      pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
1292      nodeIdServer = *pNodeIdServer;
1293      MSG_WAR(0x1AD2, "index : ", 0x1280 + i);
1294      MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer);
1295
1296     if(nodeIdServer == (UNS32)nodeId) {
1297       SDOfound = 1;
1298       break;
1299     }
1300     offset++;
1301     i++;
1302   } /* end while */
1303   if (!SDOfound) {
1304     MSG_ERR(0x1AC9, "SDO. Error. No client found to communicate with node : ", nodeId);
1305     return 0xFF;
1306   }
1307   MSG_WAR(0x3AD0,"        SDO client defined at index  : ", 0x1280 + i);
1308   initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);
1309   d->transfers[line].count = count;
1310   d->transfers[line].dataType = dataType;
1311
1312   /** Copy data to transfers structure. */
1313   for (j = 0 ; j < count ; j++) {
1314 # ifdef CANOPEN_BIG_ENDIAN
1315     if (dataType == 0 && endianize)
1316       d->transfers[line].data[count - 1 - j] = ((char *)data)[j];
1317     else /* String of bytes. */
1318       d->transfers[line].data[j] = ((char *)data)[j];
1319 #  else
1320     d->transfers[line].data[j] = ((char *)data)[j];
1321 #  endif
1322   }
1323   /** Send the SDO to the server. Initiate download, cs=1. */
1324   sdo.nodeId = nodeId;
1325   if (count <= 4) { /** Expedited transfert */
1326     sdo.body.data[0] = (1 << 5) | ((4 - count) << 2) | 3;
1327     for (i = 4 ; i < 8 ; i++)
1328       sdo.body.data[i] = d->transfers[line].data[i - 4];
1329     d->transfers[line].offset = count;
1330   }
1331   else { /** Normal transfert */
1332     sdo.body.data[0] = (1 << 5) | 1;
1333     sdo.body.data[4] = count; /** nb of byte to transmit. Max = 255. (canfestival2 limitation). */
1334     for (i = 5 ; i < 8 ; i++)
1335       sdo.body.data[i] = 0;
1336   }
1337   sdo.body.data[1] = index & 0xFF;        /* LSB */
1338   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
1339   sdo.body.data[3] = subIndex;
1340
1341   d->transfers[line].Callback = Callback;
1342
1343   err = sendSDO(d, SDO_CLIENT, sdo);
1344   if (err) {
1345     MSG_ERR(0x1AD1, "SDO. Error while sending SDO to node : ", nodeId);
1346     /** release the line */
1347     resetSDOline(d, line);
1348     return 0xFF;
1349   }
1350
1351
1352   return 0;
1353 }
1354
1355 /*!
1356 **
1357 **
1358 ** @param d
1359 ** @param nodeId
1360 ** @param index
1361 ** @param subIndex
1362 ** @param count
1363 ** @param dataType
1364 ** @param data
1365 **
1366 ** @return
1367 **/
1368 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
1369                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data)
1370 {
1371         return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, NULL, 1);
1372 }
1373
1374 /*!
1375 **
1376 **
1377 ** @param d
1378 ** @param nodeId
1379 ** @param index
1380 ** @param subIndex
1381 ** @param count
1382 ** @param dataType
1383 ** @param data
1384 ** @param Callback
1385 **
1386 ** @return
1387 **/
1388 UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index,
1389                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback)
1390 {
1391         return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1);
1392 }
1393
1394 /*!
1395 **
1396 **
1397 ** @param d
1398 ** @param nodeId
1399 ** @param index
1400 ** @param subIndex
1401 ** @param dataType
1402 ** @param Callback
1403 **
1404 ** @return
1405 **/
1406 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
1407 {
1408   UNS8 err;
1409   UNS8 SDOfound = 0;
1410   UNS8 i;
1411   UNS8 line;
1412   s_SDO sdo;    /* SDO to transmit */
1413   UNS32      *pNodeIdServer;
1414   UNS32      nodeIdServer;
1415   UNS16     offset;
1416   UNS16     lastIndex;
1417   MSG_WAR(0x3AD5, "Send SDO to read in the dictionary of node : ", nodeId);
1418   MSG_WAR(0x3AD6, "                                  At index : ", index);
1419   MSG_WAR(0x3AD7, "                                  subIndex : ", subIndex);
1420
1421
1422   /** Verify that there is no SDO communication yet. */
1423   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1424   if (!err) {
1425     MSG_ERR(0x1AD8, "SDO error : Communication yet established. with node : ", nodeId);
1426     return 0xFF;
1427   }
1428   /** Taking the line ... */
1429   err = getSDOfreeLine( d, SDO_CLIENT, &line );
1430   if (err) {
1431     MSG_ERR(0x1AD9, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId);
1432     return (0xFF);
1433   }
1434   else
1435     MSG_WAR(0x3AE0, "Transmission on line : ", line);
1436
1437   /** Check which SDO to use to communicate with the node */
1438   offset = d->firstIndex->SDO_CLT;
1439   lastIndex = d->lastIndex->SDO_CLT;
1440   if (offset == 0) {
1441     MSG_ERR(0x1AE1, "writeNetworkDict : No SDO client index found", 0);
1442     return 0xFF;
1443   }
1444   i = 0;
1445   while (offset <= lastIndex) {
1446      if (d->objdict[offset].bSubCount <= 3) {
1447          MSG_ERR(0x1AE2, "Subindex 3  not found at index ", 0x1280 + i);
1448          return 0xFF;
1449      }
1450      /** looking for the nodeId server */
1451      pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
1452      nodeIdServer = *pNodeIdServer;
1453
1454     if(nodeIdServer == (UNS32)nodeId) {
1455       SDOfound = 1;
1456       break;
1457     }
1458     offset++;
1459     i++;
1460   } /* end while */
1461   if (!SDOfound) {
1462     MSG_ERR(0x1AE3, "SDO. Error. No client found to communicate with node : ", nodeId);
1463     return 0xFF;
1464   }
1465   MSG_WAR(0x3AE4,"        SDO client defined at index  : ", 0x1280 + i);
1466   initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
1467   getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1468   sdo.nodeId = nodeId;
1469   /** Send the SDO to the server. Initiate upload, cs=2. */
1470   d->transfers[line].dataType = dataType;
1471   sdo.body.data[0] = (2 << 5);
1472   sdo.body.data[1] = index & 0xFF;        /* LSB */
1473   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
1474   sdo.body.data[3] = subIndex;
1475   for (i = 4 ; i < 8 ; i++)
1476     sdo.body.data[i] = 0;
1477   d->transfers[line].Callback = Callback;
1478   err = sendSDO(d, SDO_CLIENT, sdo);
1479   if (err) {
1480     MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId);
1481     /* release the line */
1482     resetSDOline(d, line);
1483     return 0xFF;
1484   }
1485   return 0;
1486 }
1487
1488 /*!
1489 **
1490 **
1491 ** @param d
1492 ** @param nodeId
1493 ** @param index
1494 ** @param subIndex
1495 ** @param dataType
1496 **
1497 ** @return
1498 **/
1499 UNS8 readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType)
1500 {
1501         return _readNetworkDict (d, nodeId, index, subIndex, dataType, NULL);
1502 }
1503
1504 /*!
1505 **
1506 **
1507 ** @param d
1508 ** @param nodeId
1509 ** @param index
1510 ** @param subIndex
1511 ** @param dataType
1512 ** @param Callback
1513 **
1514 ** @return
1515 **/
1516 UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
1517 {
1518         return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback);
1519 }
1520
1521 /*!
1522 **
1523 **
1524 ** @param d
1525 ** @param nodeId
1526 ** @param data
1527 ** @param size
1528 ** @param abortCode
1529 **
1530 ** @return
1531 **/
1532 UNS8 getReadResultNetworkDict (CO_Data* d, UNS8 nodeId, void* data, UNS8 *size,
1533                                UNS32 * abortCode)
1534 {
1535   UNS8 i;
1536   UNS8 err;
1537   UNS8 line;
1538   * size = 0;
1539
1540   /** Looking for the line tranfert. */
1541   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1542   if (err) {
1543     MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId);
1544     return SDO_ABORTED_INTERNAL;
1545   }
1546   if (d->transfers[line].state != SDO_FINISHED)
1547     return d->transfers[line].state;
1548
1549   /** Transfert is finished. Put the value in the data. */
1550   * size = (UNS8)d->transfers[line].count;
1551   for  ( i = 0 ; i < *size ; i++) {
1552 # ifdef CANOPEN_BIG_ENDIAN
1553     if (d->transfers[line].dataType != visible_string)
1554       ( (char *) data)[*size - 1 - i] = d->transfers[line].data[i];
1555     else /** String of bytes. */
1556       ( (char *) data)[i] = d->transfers[line].data[i];
1557 # else
1558     ( (char *) data)[i] = d->transfers[line].data[i];
1559 # endif
1560   }
1561   return SDO_FINISHED;
1562 }
1563
1564 /*!
1565 **
1566 **
1567 ** @param d
1568 ** @param nodeId
1569 ** @param abortCode
1570 **
1571 ** @return
1572 **/
1573 UNS8 getWriteResultNetworkDict (CO_Data* d, UNS8 nodeId, UNS32 * abortCode)
1574 {
1575   UNS8 line = 0;
1576   UNS8 err;
1577
1578   * abortCode = 0;
1579   /** Looking for the line tranfert. */
1580   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1581   if (err) {
1582     MSG_ERR(0x1AF1, "SDO error : No line found for communication with node : ", nodeId);
1583     return SDO_ABORTED_INTERNAL;
1584   }
1585   * abortCode = d->transfers[line].abortCode;
1586   return d->transfers[line].state;
1587 }