src/sdo.c

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