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