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