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