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