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