src/sdo.c

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