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