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