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