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