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 watchdog */
 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         d->transfers[line].abortCode = abortCode;
1049         MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode);
1050         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
1051       }
1052       else
1053         MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode);
1054     }
1055     break;
1056   default:
1057     /* Error : Unknown cs */
1058     MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", getSDOcs(m->data[0]));
1059     return 0xFF;
1060
1061   } /* End switch */
1062   return 0;
1063 }
1064
1065 /*******************************************************************)******/
1066 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
1067                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback)
1068 {
1069   UNS8 err;
1070   UNS8 SDOfound = 0;
1071   UNS8 line;
1072   s_SDO sdo;    /* SDO to transmit */
1073   UNS8 i, j;
1074   UNS16     lastIndex;
1075   UNS16     offset;
1076   UNS32      *pNodeIdServer;
1077   UNS32      nodeIdServer;
1078
1079   MSG_WAR(0x3AC0, "Send SDO to write in the dictionary of node : ", nodeId);
1080   MSG_WAR(0x3AC1, "                                   At index : ", index);
1081   MSG_WAR(0x3AC2, "                                   subIndex : ", subIndex);
1082   MSG_WAR(0x3AC3, "                                   nb bytes : ", count);
1083
1084   /* Verify that there is no SDO communication yet. */
1085   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1086   if (!err) {
1087     MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId);
1088     return 0xFF;
1089   }
1090   /* Taking the line ... */
1091   err = getSDOfreeLine( d, SDO_CLIENT, &line );
1092   if (err) {
1093     MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId);
1094     return (0xFF);
1095   }
1096   /* Check which SDO to use to communicate with the node */
1097   offset = d->firstIndex->SDO_CLT;
1098   lastIndex = d->lastIndex->SDO_CLT;
1099   if (offset == 0) {
1100     MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0);
1101     return 0xFF;
1102   }
1103   i = 0;
1104    while (offset <= lastIndex) {
1105      if (d->objdict[offset].bSubCount <= 3) {
1106          MSG_ERR(0x1AC8, "Subindex 3  not found at index ", 0x1280 + i);
1107          return 0xFF;
1108      }
1109      /* looking for the nodeId server */
1110      pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
1111      nodeIdServer = *pNodeIdServer;
1112      MSG_WAR(0x1AD2, "index : ", 0x1280 + i);
1113      MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer);
1114
1115     if(nodeIdServer == (UNS32)nodeId) {
1116       SDOfound = 1;
1117       break;
1118     }
1119     offset++;
1120     i++;
1121   } /* end while */
1122   if (!SDOfound) {
1123     MSG_ERR(0x1AC9, "SDO. Error. No client found to communicate with node : ", nodeId);
1124     return 0xFF;
1125   }
1126   MSG_WAR(0x3AD0,"        SDO client defined at index  : ", 0x1280 + i);
1127   initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);
1128   d->transfers[line].count = count;
1129   d->transfers[line].dataType = dataType;
1130
1131   /* Copy data to transfers structure. */
1132   for (j = 0 ; j < count ; j++) {
1133 # ifdef CANOPEN_BIG_ENDIAN
1134     if (dataType == 0)
1135       d->transfers[line].data[count - 1 - j] = ((char *)data)[j];
1136     else /* String of bytes. */
1137       d->transfers[line].data[j] = ((char *)data)[j];
1138 #  else
1139     d->transfers[line].data[j] = ((char *)data)[j];
1140 #  endif
1141   }
1142   /* Send the SDO to the server. Initiate download, cs=1. */
1143   sdo.nodeId = nodeId;
1144   if (count <= 4) { /* Expedited transfert */
1145     sdo.body.data[0] = (1 << 5) | ((4 - count) << 2) | 3;
1146     for (i = 4 ; i < 8 ; i++)
1147       sdo.body.data[i] = d->transfers[line].data[i - 4];
1148     d->transfers[line].offset = count;
1149   }
1150   else { /* Normal transfert */
1151     sdo.body.data[0] = (1 << 5) | 1;
1152     sdo.body.data[4] = count; /* nb of byte to transmit. Max = 255. (canfestival2 limitation). */
1153     for (i = 5 ; i < 8 ; i++)
1154       sdo.body.data[i] = 0;
1155   }
1156   sdo.body.data[1] = index & 0xFF;        /* LSB */
1157   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
1158   sdo.body.data[3] = subIndex;
1159
1160   d->transfers[line].Callback = Callback;
1161
1162   err = sendSDO(d, SDO_CLIENT, sdo);
1163   if (err) {
1164     MSG_ERR(0x1AD1, "SDO. Error while sending SDO to node : ", nodeId);
1165     /* release the line */
1166     resetSDOline(d, line);
1167     return 0xFF;
1168   }
1169
1170
1171   return 0;
1172 }
1173
1174 /*--------------------------------------------------------------------------*/
1175
1176 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
1177                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data)
1178 {
1179         return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, NULL);
1180 }
1181
1182 /*--------------------------------------------------------------------------*/
1183
1184 UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index,
1185                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback)
1186 {
1187         return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback);
1188 }
1189
1190
1191 /***************************************************************************/
1192 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
1193 {
1194   UNS8 err;
1195   UNS8 SDOfound = 0;
1196   UNS8 i;
1197   UNS8 line;
1198   s_SDO sdo;    /* SDO to transmit */
1199   UNS32      *pNodeIdServer;
1200   UNS32      nodeIdServer;
1201   UNS16     offset;
1202   UNS16     lastIndex;
1203   MSG_WAR(0x3AD5, "Send SDO to read in the dictionary of node : ", nodeId);
1204   MSG_WAR(0x3AD6, "                                  At index : ", index);
1205   MSG_WAR(0x3AD7, "                                  subIndex : ", subIndex);
1206
1207
1208   /* Verify that there is no SDO communication yet. */
1209   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1210   if (!err) {
1211     MSG_ERR(0x1AD8, "SDO error : Communication yet established. with node : ", nodeId);
1212     return 0xFF;
1213   }
1214   /* Taking the line ... */
1215   err = getSDOfreeLine( d, SDO_CLIENT, &line );
1216   if (err) {
1217     MSG_ERR(0x1AD9, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId);
1218     return (0xFF);
1219   }
1220   else
1221     MSG_WAR(0x3AE0, "Transmission on line : ", line);
1222
1223   /* Check which SDO to use to communicate with the node */
1224   offset = d->firstIndex->SDO_CLT;
1225   lastIndex = d->lastIndex->SDO_CLT;
1226   if (offset == 0) {
1227     MSG_ERR(0x1AE1, "writeNetworkDict : No SDO client index found", 0);
1228     return 0xFF;
1229   }
1230   i = 0;
1231   while (offset <= lastIndex) {
1232      if (d->objdict[offset].bSubCount <= 3) {
1233          MSG_ERR(0x1AE2, "Subindex 3  not found at index ", 0x1280 + i);
1234          return 0xFF;
1235      }
1236      /* looking for the nodeId server */
1237      pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
1238      nodeIdServer = *pNodeIdServer;
1239
1240     if(nodeIdServer == (UNS32)nodeId) {
1241       SDOfound = 1;
1242       break;
1243     }
1244     offset++;
1245     i++;
1246   } /* end while */
1247   if (!SDOfound) {
1248     MSG_ERR(0x1AE3, "SDO. Error. No client found to communicate with node : ", nodeId);
1249     return 0xFF;
1250   }
1251   MSG_WAR(0x3AE4,"        SDO client defined at index  : ", 0x1280 + i);
1252   initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
1253   getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1254   sdo.nodeId = nodeId;
1255   /* Send the SDO to the server. Initiate upload, cs=2. */
1256   d->transfers[line].dataType = dataType;
1257   sdo.body.data[0] = (2 << 5);
1258   sdo.body.data[1] = index & 0xFF;        /* LSB */
1259   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
1260   sdo.body.data[3] = subIndex;
1261   for (i = 4 ; i < 8 ; i++)
1262     sdo.body.data[i] = 0;
1263   d->transfers[line].Callback = Callback;
1264   err = sendSDO(d, SDO_CLIENT, sdo);
1265   if (err) {
1266     MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId);
1267     /* release the line */
1268     resetSDOline(d, line);
1269     return 0xFF;
1270   }
1271   return 0;
1272 }
1273
1274 /*--------------------------------------------------------------------------*/
1275
1276 UNS8 readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType)
1277 {
1278         return _readNetworkDict (d, nodeId, index, subIndex, dataType, NULL);
1279 }
1280
1281 /*--------------------------------------------------------------------------*/
1282 UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
1283 {
1284         return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback);
1285 }
1286
1287 /***************************************************************************/
1288
1289 UNS8 getReadResultNetworkDict (CO_Data* d, UNS8 nodeId, void* data, UNS8 *size,
1290                                UNS32 * abortCode)
1291 {
1292   UNS8 i;
1293   UNS8 err;
1294   UNS8 line;
1295   * size = 0;
1296
1297   /* Looking for the line tranfert. */
1298   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1299   if (err) {
1300     MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId);
1301     return SDO_ABORTED_INTERNAL;
1302   }
1303   if (d->transfers[line].state != SDO_FINISHED)
1304     return d->transfers[line].state;
1305
1306   /* Transfert is finished. Put the value in the data. */
1307   * size = (UNS8)d->transfers[line].count;
1308   for  ( i = 0 ; i < *size ; i++) {
1309 # ifdef CANOPEN_BIG_ENDIAN
1310     if (d->transfers[line].dataType != visible_string)
1311       ( (char *) data)[*size - 1 - i] = d->transfers[line].data[i];
1312     else /* String of bytes. */
1313       ( (char *) data)[i] = d->transfers[line].data[i];
1314 # else
1315     ( (char *) data)[i] = d->transfers[line].data[i];
1316 # endif
1317   }
1318   return SDO_FINISHED;
1319 }
1320
1321 /***************************************************************************/
1322
1323 UNS8 getWriteResultNetworkDict (CO_Data* d, UNS8 nodeId, UNS32 * abortCode)
1324 {
1325   UNS8 line = 0;
1326   UNS8 err;
1327
1328   * abortCode = 0;
1329   /* Looking for the line tranfert. */
1330   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1331   if (err) {
1332     MSG_ERR(0x1AF1, "SDO error : No line found for communication with node : ", nodeId);
1333     return SDO_ABORTED_INTERNAL;
1334   }
1335   * abortCode = d->transfers[line].abortCode;
1336   return d->transfers[line].state;
1337 }
1338
1339 void _SDOtimeoutError (UNS8 line){}