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