src/sdo.c

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