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