sdo.c

00001 /*
00002 This file is part of CanFestival, a library implementing CanOpen Stack.
00003 
00004 Copyright (C): Edouard TISSERANT and Francis DUPIN
00005 
00006 See COPYING file for copyrights details.
00007 
00008 This library is free software; you can redistribute it and/or
00009 modify it under the terms of the GNU Lesser General Public
00010 License as published by the Free Software Foundation; either
00011 version 2.1 of the License, or (at your option) any later version.
00012 
00013 This library is distributed in the hope that it will be useful,
00014 but WITHOUT ANY WARRANTY; without even the implied warranty of
00015 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00016 Lesser General Public License for more details.
00017 
00018 You should have received a copy of the GNU Lesser General Public
00019 License along with this library; if not, write to the Free Software
00020 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00021 */
00022 /*!
00023 ** @file   sdo.c
00024 ** @author Edouard TISSERANT and Francis DUPIN
00025 ** @date   Tue Jun  5 09:32:32 2007
00026 **
00027 ** @brief
00028 **
00029 **
00030 */
00031 
00032 /* #define DEBUG_WAR_CONSOLE_ON */
00033 /* #define DEBUG_ERR_CONSOLE_ON */
00034 
00035 #include "canfestival.h"
00036 #include "sysdep.h"
00037 
00038 /* Uncomment if your compiler does not support inline functions */
00039 #define NO_INLINE
00040 
00041 #ifdef NO_INLINE
00042   #define INLINE
00043 #else
00044   #define INLINE inline
00045 #endif
00046 
00047 /*Internals prototypes*/
00048 
00049 /*!
00050 ** Called by writeNetworkDict
00051 **
00052 ** @param d
00053 ** @param nodeId
00054 ** @param index
00055 ** @param subIndex
00056 ** @param count
00057 ** @param dataType
00058 ** @param data
00059 ** @param Callback
00060 ** @param endianize
00061 **
00062 ** @return
00063 **/
00064 INLINE UNS8 _writeNetworkDict  (CO_Data* d, UNS8 nodeId, UNS16 index,
00065                UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize);
00066 
00067 /*!
00068 ** Called by readNetworkDict
00069 **
00070 ** @param d
00071 ** @param nodeId
00072 ** @param index
00073 ** @param subIndex
00074 ** @param dataType
00075 ** @param Callback
00076 **
00077 ** @return
00078 **/
00079 INLINE UNS8 _readNetworkDict  (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex,
00080     UNS8 dataType, SDOCallback_t Callback);
00081 
00082 
00083 /***************************************************************************/
00084 /* SDO (un)packing macros */
00085 
00086 /** Returns the command specifier (cs, ccs, scs) from the first byte of the SDO
00087  */
00088 #define getSDOcs(byte) (byte >> 5)
00089 
00090 /** Returns the number of bytes without data from the first byte of the SDO. Coded in 2 bits
00091  */
00092 #define getSDOn2(byte) ((byte >> 2) & 3)
00093 
00094 /** Returns the number of bytes without data from the first byte of the SDO. Coded in 3 bits
00095  */
00096 #define getSDOn3(byte) ((byte >> 1) & 7)
00097 
00098 /** Returns the transfer type from the first byte of the SDO
00099  */
00100 #define getSDOe(byte) ((byte >> 1) & 1)
00101 
00102 /** Returns the size indicator from the first byte of the SDO
00103  */
00104 #define getSDOs(byte) (byte & 1)
00105 
00106 /** Returns the indicator of end transmission from the first byte of the SDO
00107  */
00108 #define getSDOc(byte) (byte & 1)
00109 
00110 /** Returns the toggle from the first byte of the SDO
00111  */
00112 #define getSDOt(byte) ((byte >> 4) & 1)
00113 
00114 /** Returns the index from the bytes 1 and 2 of the SDO
00115  */
00116 #define getSDOindex(byte1, byte2) (((UNS16)byte2 << 8) | ((UNS16)byte1))
00117 
00118 /** Returns the subIndex from the byte 3 of the SDO
00119  */
00120 #define getSDOsubIndex(byte3) (byte3)
00121 
00122 /*!
00123 **
00124 **
00125 ** @param d
00126 ** @param id
00127 **/
00128 void SDOTimeoutAlarm(CO_Data* d, UNS32 id)
00129 {
00130     MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0);
00131     MSG_WAR(0x2A02, "server node : ", d->transfers[id].nodeId);
00132     MSG_WAR(0x2A02, "      index : ", d->transfers[id].index);
00133     MSG_WAR(0x2A02, "   subIndex : ", d->transfers[id].subIndex);
00134     /* Reset timer handler */
00135     d->transfers[id].timer = TIMER_NONE;
00136     /*Set aborted state*/
00137     d->transfers[id].state = SDO_ABORTED_INTERNAL;
00138     /* Sending a SDO abort */
00139     sendSDOabort(d, d->transfers[id].whoami, d->transfers[id].nodeId,
00140          d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT);
00141     d->transfers[id].abortCode = SDOABT_TIMED_OUT;
00142     /* Call the user function to inform of the problem.*/
00143     if(d->transfers[id].Callback)
00144         /*If ther is a callback, it is responsible to close SDO transfer (client)*/
00145         (*d->transfers[id].Callback)(d,d->transfers[id].nodeId);
00146     
00147     /*Reset the line if (whoami == SDO_SERVER) or the callback did not close the line.
00148       Otherwise this sdo transfer would never be closed. */
00149     resetSDOline(d, (UNS8)id);
00150 }
00151 
00152 #define StopSDO_TIMER(id) \
00153 MSG_WAR(0x3A05, "StopSDO_TIMER for line : ", line);\
00154 d->transfers[id].timer = DelAlarm(d->transfers[id].timer);
00155 
00156 #define StartSDO_TIMER(id) \
00157 MSG_WAR(0x3A06, "StartSDO_TIMER for line : ", line);\
00158 d->transfers[id].timer = SetAlarm(d,id,&SDOTimeoutAlarm,MS_TO_TIMEVAL(SDO_TIMEOUT_MS),0);
00159 
00160 #define RestartSDO_TIMER(id) \
00161 MSG_WAR(0x3A07, "restartSDO_TIMER for line : ", line);\
00162 if(d->transfers[id].timer != TIMER_NONE) { StopSDO_TIMER(id) StartSDO_TIMER(id) }
00163 
00164 /*!
00165 ** Reset all sdo buffers
00166 **
00167 ** @param d
00168 **/
00169 void resetSDO (CO_Data* d)
00170 {
00171   UNS8 j;
00172 
00173   /* transfer structure initialization */
00174     for (j = 0 ; j < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; j++)
00175       resetSDOline(d, j);
00176 }
00177 
00178 /*!
00179 **
00180 **
00181 ** @param d
00182 ** @param line
00183 **
00184 ** @return
00185 **/
00186 UNS32 SDOlineToObjdict (CO_Data* d, UNS8 line)
00187 {
00188   UNS32 size;
00189   UNS32 errorCode;
00190   MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line);
00191   /* if SDO initiated with e=0 and s=0 count is null, offset carry effective size*/
00192   if( d->transfers[line].count == 0)
00193       d->transfers[line].count = d->transfers[line].offset;
00194   size = d->transfers[line].count;
00195 
00196 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
00197   if (size > SDO_MAX_LENGTH_TRANSFERT)
00198   {
00199     errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,
00200              (void *) d->transfers[line].dynamicData, &size, 1);
00201   }
00202   else
00203   {
00204    errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,
00205              (void *) d->transfers[line].data, &size, 1);
00206   }
00207 #else //SDO_DYNAMIC_BUFFER_ALLOCATION
00208   errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,
00209              (void *) d->transfers[line].data, &size, 1);
00210 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION
00211 
00212   if (errorCode != OD_SUCCESSFUL)
00213     return errorCode;
00214   MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line);
00215   return 0;
00216 
00217 }
00218 
00219 /*!
00220 **
00221 **
00222 ** @param d
00223 ** @param line
00224 **
00225 ** @return
00226 **/
00227 UNS32 objdictToSDOline (CO_Data* d, UNS8 line)
00228 {
00229   UNS32  size = 0;
00230   UNS8  dataType;
00231   UNS32 errorCode;
00232 
00233   MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index);
00234   MSG_WAR(0x3A06, "  subIndex : ", d->transfers[line].subIndex);
00235 
00236 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
00237   //TODO: Read the size of the object. Depending o it put data into data or dynamicData
00238   errorCode = getODentry(d,     d->transfers[line].index,
00239                   d->transfers[line].subIndex,
00240                   (void *)d->transfers[line].data,
00241                   &size, &dataType, 1);
00242 #else //SDO_DYNAMIC_BUFFER_ALLOCATION
00243   errorCode = getODentry(d,     d->transfers[line].index,
00244                   d->transfers[line].subIndex,
00245                   (void *)d->transfers[line].data,
00246                   &size, &dataType, 1);
00247 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION
00248 
00249   if (errorCode != OD_SUCCESSFUL)
00250     return errorCode;
00251 
00252   d->transfers[line].count = size;
00253   d->transfers[line].offset = 0;
00254 
00255   return 0;
00256 }
00257 
00258 /*!
00259 **
00260 **
00261 ** @param d
00262 ** @param line
00263 ** @param nbBytes
00264 ** @param data
00265 **
00266 ** @return
00267 **/
00268 UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS32 nbBytes, UNS8* data) {
00269   UNS8 i;
00270   UNS32 offset;
00271 
00272 #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION
00273   if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
00274     MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
00275     return 0xFF;
00276   }
00277 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION
00278 
00279     if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) {
00280     MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes);
00281     return 0xFF;
00282   }
00283   offset = d->transfers[line].offset;
00284 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
00285   if (d->transfers[line].count <= SDO_MAX_LENGTH_TRANSFERT)
00286   {
00287     for (i = 0 ; i < nbBytes ; i++)
00288       * (data + i) = d->transfers[line].data[offset + i];
00289   }
00290   else
00291   {
00292     if (d->transfers[line].dynamicData == NULL)
00293     {
00294       MSG_ERR(0x1A11,"SDO's dynamic buffer not allocated. Line", line);
00295       return 0xFF;
00296     }
00297     for (i = 0 ; i < nbBytes ; i++)
00298       * (data + i) = d->transfers[line].dynamicData[offset + i];
00299   }
00300 #else //SDO_DYNAMIC_BUFFER_ALLOCATION
00301     for (i = 0 ; i < nbBytes ; i++)
00302     * (data + i) = d->transfers[line].data[offset + i];
00303 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION
00304   d->transfers[line].offset = d->transfers[line].offset + nbBytes;
00305   return 0;
00306 }
00307 
00308 /*!
00309 **
00310 **
00311 ** @param d
00312 ** @param line
00313 ** @param nbBytes
00314 ** @param data
00315 **
00316 ** @return
00317 **/
00318 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS32 nbBytes, UNS8* data)
00319 {
00320   UNS8 i;
00321   UNS32 offset;
00322 #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION
00323   if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
00324     MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
00325     return 0xFF;
00326   }
00327 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION
00328 
00329   offset = d->transfers[line].offset;
00330 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
00331   {
00332     UNS8* lineData = d->transfers[line].data;
00333     if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
00334       if (d->transfers[line].dynamicData == NULL) {
00335         d->transfers[line].dynamicData = (UNS8*) malloc(SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);
00336         d->transfers[line].dynamicDataSize = SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE;
00337 
00338         if (d->transfers[line].dynamicData == NULL) {
00339           MSG_ERR(0x1A15,"SDO allocating dynamic buffer failed, size", SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);
00340           return 0xFF;
00341         }
00342         //Copy present data
00343         memcpy(d->transfers[line].dynamicData, d->transfers[line].data, offset);
00344       }
00345       else if ((d->transfers[line].offset + nbBytes) > d->transfers[line].dynamicDataSize)
00346       {
00347         UNS8* newDynamicBuffer = (UNS8*) realloc(d->transfers[line].dynamicData, d->transfers[line].dynamicDataSize + SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);
00348         if (newDynamicBuffer == NULL) {
00349           MSG_ERR(0x1A15,"SDO reallocating dynamic buffer failed, size", d->transfers[line].dynamicDataSize + SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);
00350           return 0xFF;
00351         }
00352         d->transfers[line].dynamicData = newDynamicBuffer;
00353         d->transfers[line].dynamicDataSize += SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE;
00354       }
00355       lineData = d->transfers[line].dynamicData;
00356     }
00357     
00358     for (i = 0 ; i < nbBytes ; i++)
00359       lineData[offset + i] = * (data + i);
00360   }
00361 #else //SDO_DYNAMIC_BUFFER_ALLOCATION
00362   for (i = 0 ; i < nbBytes ; i++)
00363     d->transfers[line].data[offset + i] = * (data + i);
00364 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION
00365 
00366   d->transfers[line].offset = d->transfers[line].offset + nbBytes;
00367   return 0;
00368 }
00369 
00370 /*!
00371 **
00372 **
00373 ** @param d
00374 ** @param nodeId
00375 ** @param whoami
00376 ** @param index
00377 ** @param subIndex
00378 ** @param abortCode
00379 **
00380 ** @return
00381 **/
00382 UNS8 failedSDO (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS16 index,
00383         UNS8 subIndex, UNS32 abortCode)
00384 {
00385   UNS8 err;
00386   UNS8 line;
00387   err = getSDOlineOnUse( d, nodeId, whoami, &line );
00388   if (!err) /* If a line on use have been found.*/
00389     MSG_WAR(0x3A20, "FailedSDO : line found : ", line);
00390   if ((! err) && (whoami == SDO_SERVER)) {
00391     resetSDOline( d, line );
00392     MSG_WAR(0x3A21, "FailedSDO : line released : ", line);
00393   }
00394   if ((! err) && (whoami == SDO_CLIENT)) {
00395     StopSDO_TIMER(line);
00396     d->transfers[line].state = SDO_ABORTED_INTERNAL;
00397   }
00398   MSG_WAR(0x3A22, "Sending SDO abort ", 0);
00399   err = sendSDOabort(d, whoami, nodeId, index, subIndex, abortCode);
00400   if (err) {
00401     MSG_WAR(0x3A23, "Unable to send the SDO abort", 0);
00402     return 0xFF;
00403   }
00404   return 0;
00405 }
00406 
00407 /*!
00408 **
00409 **
00410 ** @param d
00411 ** @param line
00412 **/
00413 void resetSDOline ( CO_Data* d, UNS8 line )
00414 {
00415   UNS32 i;
00416   MSG_WAR(0x3A25, "reset SDO line nb : ", line);
00417   initSDOline(d, line, 0, 0, 0, SDO_RESET);
00418   for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++)
00419     d->transfers[line].data[i] = 0;
00420   d->transfers[line].whoami = 0;
00421   d->transfers[line].abortCode = 0;
00422 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
00423   free(d->transfers[line].dynamicData);
00424   d->transfers[line].dynamicData = 0;
00425   d->transfers[line].dynamicDataSize = 0;
00426 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION
00427 }
00428 
00429 /*!
00430 **
00431 **
00432 ** @param d
00433 ** @param line
00434 ** @param nodeId
00435 ** @param index
00436 ** @param subIndex
00437 ** @param state
00438 **
00439 ** @return
00440 **/
00441 UNS8 initSDOline (CO_Data* d, UNS8 line, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 state)
00442 {
00443   MSG_WAR(0x3A25, "init SDO line nb : ", line);
00444   if (state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS){
00445       StartSDO_TIMER(line)
00446   }else{
00447       StopSDO_TIMER(line)
00448   }
00449   d->transfers[line].nodeId = nodeId;
00450   d->transfers[line].index = index;
00451   d->transfers[line].subIndex = subIndex;
00452   d->transfers[line].state = state;
00453   d->transfers[line].toggle = 0;
00454   d->transfers[line].count = 0;
00455   d->transfers[line].offset = 0;
00456   d->transfers[line].dataType = 0;
00457   d->transfers[line].Callback = NULL;
00458 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
00459   free(d->transfers[line].dynamicData);
00460   d->transfers[line].dynamicData = 0;
00461   d->transfers[line].dynamicDataSize = 0;
00462 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION
00463   return 0;
00464 }
00465 
00466 /*!
00467 **
00468 **
00469 ** @param d
00470 ** @param whoami
00471 ** @param line
00472 **
00473 ** @return
00474 **/
00475 UNS8 getSDOfreeLine ( CO_Data* d, UNS8 whoami, UNS8 *line )
00476 {
00477 
00478   UNS8 i;
00479 
00480   for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
00481     if ( d->transfers[i].state == SDO_RESET ) {
00482       *line = i;
00483       d->transfers[i].whoami = whoami;
00484       return 0;
00485     } /* end if */
00486   } /* end for */
00487   MSG_ERR(0x1A25, "Too many SDO in progress. Aborted.", i);
00488   return 0xFF;
00489 }
00490 
00491 /*!
00492 **
00493 **
00494 ** @param d
00495 ** @param nodeId
00496 ** @param whoami
00497 ** @param line
00498 **
00499 ** @return
00500 **/
00501 UNS8 getSDOlineOnUse (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS8 *line)
00502 {
00503 
00504   UNS8 i;
00505 
00506   for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
00507     if ( (d->transfers[i].state != SDO_RESET) &&
00508      (d->transfers[i].state != SDO_ABORTED_INTERNAL) &&
00509      (d->transfers[i].nodeId == nodeId) &&
00510      (d->transfers[i].whoami == whoami) ) {
00511       if (line) *line = i;
00512       return 0;
00513     }
00514   }
00515   return 0xFF;
00516 }
00517 
00518 /*!
00519 **
00520 **
00521 ** @param d
00522 ** @param nodeId
00523 ** @param whoami
00524 ** @param line
00525 **
00526 ** @return
00527 **/
00528 UNS8 getSDOlineToClose (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS8 *line)
00529 {
00530 
00531   UNS8 i;
00532 
00533   for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
00534     if ( (d->transfers[i].state != SDO_RESET) &&
00535      (d->transfers[i].nodeId == nodeId) &&
00536      (d->transfers[i].whoami == whoami) ) {
00537       if (line) *line = i;
00538       return 0;
00539     }
00540   }
00541   return 0xFF;
00542 }
00543 
00544 
00545 /*!
00546 **
00547 **
00548 ** @param d
00549 ** @param nodeId
00550 ** @param whoami
00551 **
00552 ** @return
00553 **/
00554 UNS8 closeSDOtransfer (CO_Data* d, UNS8 nodeId, UNS8 whoami)
00555 {
00556   UNS8 err;
00557   UNS8 line;
00558   err = getSDOlineToClose(d, nodeId, whoami, &line);
00559   if (err) {
00560     MSG_WAR(0x2A30, "No SDO communication to close for node : ", nodeId);
00561     return 0xFF;
00562   }
00563   resetSDOline(d, line);
00564   return 0;
00565 }
00566 
00567 /*!
00568 **
00569 **
00570 ** @param d
00571 ** @param line
00572 ** @param nbBytes
00573 **
00574 ** @return
00575 **/
00576 UNS8 getSDOlineRestBytes (CO_Data* d, UNS8 line, UNS32 * nbBytes)
00577 {
00578   /* SDO initiated with e=0 and s=0 have count set to null */
00579   if (d->transfers[line].count == 0)
00580     * nbBytes = 0;
00581   else
00582     * nbBytes = d->transfers[line].count - d->transfers[line].offset;
00583   return 0;
00584 }
00585 
00586 /*!
00587 **
00588 **
00589 ** @param d
00590 ** @param line
00591 ** @param nbBytes
00592 **
00593 ** @return
00594 **/
00595 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS32 nbBytes)
00596 {
00597 #ifndef SDO_DYNAMIC_BUFFER_ALLOCATION
00598   if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) {
00599     MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
00600     return 0xFF;
00601   }
00602 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION
00603 
00604   d->transfers[line].count = nbBytes;
00605   return 0;
00606 }
00607 
00608 /*!
00609 **
00610 **
00611 ** @param d
00612 ** @param whoami
00613 ** @param sdo
00614 **
00615 ** @return
00616 **/
00617 UNS8 sendSDO (CO_Data* d, UNS8 whoami, s_SDO sdo)
00618 {
00619   UNS16 offset;
00620   UNS16 lastIndex;
00621   UNS8 found = 0;
00622   Message m;
00623   UNS8 i;
00624   UNS32 * pwCobId = NULL;
00625   UNS8 * pwNodeId = NULL;
00626 
00627   MSG_WAR(0x3A38, "sendSDO",0);
00628   if( !((d->nodeState == Operational) ||  (d->nodeState == Pre_operational ))) {
00629     MSG_WAR(0x2A39, "unable to send the SDO (not in op or pre-op mode", d->nodeState);
00630     return 0xFF;
00631   }
00632 
00633   /*get the server->client cobid*/
00634   if ( whoami == SDO_SERVER )    {/*case server. only one SDO server*/
00635     offset = d->firstIndex->SDO_SVR;
00636     if (offset == 0) {
00637       MSG_ERR(0x1A42, "SendSDO : No SDO server found", 0);
00638       return 0xFF;
00639     }
00640     pwCobId = (UNS32*) d->objdict[offset].pSubindex[2].pObject;
00641     MSG_WAR(0x3A41, "I am server. cobId : ", *pwCobId);
00642   }
00643   else {            /*case client*/
00644     /* Get the client->server cobid.*/
00645     UNS16 sdoNum = 0;
00646     offset = d->firstIndex->SDO_CLT;
00647     lastIndex = d->lastIndex->SDO_CLT;
00648     if (offset == 0) {
00649       MSG_ERR(0x1A42, "SendSDO : No SDO client index found", 0);
00650       return 0xFF;
00651     }
00652     /* find index for communication server node */
00653     while (offset <= lastIndex){
00654       MSG_WAR(0x3A43,"Reading index : ", 0x1280 + sdoNum);
00655       if (d->objdict[offset].bSubCount <= 3) {
00656     MSG_ERR(0x1A28, "Subindex 3  not found at index ", 0x1280 + sdoNum);
00657     return 0xFF;
00658       }
00659       pwNodeId = (UNS8*) d->objdict[offset].pSubindex[3].pObject;
00660       MSG_WAR(0x3A44, "Found nodeId server = ", *pwNodeId);
00661       if(*pwNodeId == sdo.nodeId) {
00662     found = 1;
00663     break;
00664       }
00665       offset ++;
00666       sdoNum ++;
00667     }
00668     if (! found){
00669       MSG_WAR (0x2A45, "No SDO client corresponds to the mesage to send to node ", sdo.nodeId);
00670       return 0xFF;
00671     }
00672     /* read the client->server cobid */
00673     pwCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject;
00674   }
00675   /* message copy for sending */
00676   m.cob_id = (UNS16)UNS16_LE(*pwCobId);
00677   m.rtr = NOT_A_REQUEST;
00678   /* the length of SDO must be 8 */
00679   m.len = 8;
00680   for (i = 0 ; i < 8 ; i++) {
00681     m.data[i] =  sdo.body.data[i];
00682   }
00683   return canSend(d->canHandle,&m);
00684 }
00685 
00686 /*!
00687 **
00688 **
00689 ** @param d
00690 ** @param whoami
00691 ** @param index
00692 ** @param subIndex
00693 ** @param abortCode
00694 **
00695 ** @return
00696 **/
00697 UNS8 sendSDOabort (CO_Data* d, UNS8 whoami, UNS8 nodeID, UNS16 index, UNS8 subIndex, UNS32 abortCode)
00698 {
00699   s_SDO sdo;
00700   UNS8 ret;
00701   
00702   MSG_WAR(0x2A50,"Sending SDO abort ", abortCode);
00703   if(whoami == SDO_SERVER)
00704   {
00705     sdo.nodeId = *d->bDeviceNodeId;
00706   }
00707   else
00708   {
00709     sdo.nodeId = nodeID;
00710   }
00711   sdo.body.data[0] = 0x80;
00712   /* Index */
00713   sdo.body.data[1] = index & 0xFF; /* LSB */
00714   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
00715   /* Subindex */
00716   sdo.body.data[3] = subIndex;
00717   /* Data */
00718   sdo.body.data[4] = (UNS8)(abortCode & 0xFF);
00719   sdo.body.data[5] = (UNS8)((abortCode >> 8) & 0xFF);
00720   sdo.body.data[6] = (UNS8)((abortCode >> 16) & 0xFF);
00721   sdo.body.data[7] = (UNS8)((abortCode >> 24) & 0xFF);
00722   ret = sendSDO(d, whoami, sdo);
00723 
00724   return ret;
00725 }
00726 
00727 /*!
00728 **
00729 **
00730 ** @param d
00731 ** @param m
00732 **
00733 ** @return
00734 **/
00735 UNS8 proceedSDO (CO_Data* d, Message *m)
00736 {
00737   UNS8 err;
00738   UNS8 line;
00739   UNS32 nbBytes; /* received or to be transmited. */
00740   UNS8 nodeId = 0;  /* The node from which the SDO is received */
00741   UNS8 *pNodeId = NULL;
00742   UNS8 whoami = SDO_UNKNOWN;  /* SDO_SERVER or SDO_CLIENT.*/
00743   UNS32 errorCode; /* while reading or writing in the local object dictionary.*/
00744   s_SDO sdo;    /* SDO to transmit */
00745   UNS16 index;
00746   UNS8 subIndex;
00747   UNS32 abortCode;
00748   UNS32 i;
00749   UNS8    j;
00750   UNS32 *pCobId = NULL;
00751   UNS16 offset;
00752   UNS16 lastIndex;
00753 
00754   MSG_WAR(0x3A60, "proceedSDO ", 0);
00755   whoami = SDO_UNKNOWN;
00756   /* Looking for the cobId in the object dictionary. */
00757   /* Am-I a server ? */
00758   offset = d->firstIndex->SDO_SVR;
00759   lastIndex = d->lastIndex->SDO_SVR;
00760   j = 0;
00761   if(offset) while (offset <= lastIndex) {
00762      if (d->objdict[offset].bSubCount <= 1) {
00763       MSG_ERR(0x1A61, "Subindex 1  not found at index ", 0x1200 + j);
00764       return 0xFF;
00765     }
00766       pCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject;
00767       if ( *pCobId == UNS16_LE(m->cob_id) ) {
00768     whoami = SDO_SERVER;
00769     MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j);
00770     /* In case of server, the node id of the client may be unknown. So we put the index minus offset */
00771     /* 0x1200 where the cobid received is defined. */
00772     nodeId = j;
00773     break;
00774       }
00775       j++;
00776       offset++;
00777   } /* end while */
00778   if (whoami == SDO_UNKNOWN) {
00779     /* Am-I client ? */
00780     offset = d->firstIndex->SDO_CLT;
00781     lastIndex = d->lastIndex->SDO_CLT;
00782     j = 0;
00783     if(offset) while (offset <= lastIndex) {
00784        if (d->objdict[offset].bSubCount <= 3) {
00785      MSG_ERR(0x1A63, "Subindex 3  not found at index ", 0x1280 + j);
00786      return 0xFF;
00787        }
00788        /* a) Looking for the cobid received. */
00789        pCobId = (UNS32*) d->objdict[offset].pSubindex[2].pObject;
00790        if (*pCobId == UNS16_LE(m->cob_id) ) {
00791      /* b) cobid found, so reading the node id of the server. */
00792      pNodeId = (UNS8*) d->objdict[offset].pSubindex[3].pObject;
00793      whoami = SDO_CLIENT;
00794      nodeId = *pNodeId;
00795      MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j);
00796      MSG_WAR(0x3A65, "                 Server nodeId : ", nodeId);
00797      break;
00798     }
00799        j++;
00800        offset++;
00801     } /* end while */
00802   }
00803   if (whoami == SDO_UNKNOWN) {
00804     return 0xFF;/* This SDO was not for us ! */
00805   }
00806 
00807   /* Test if the size of the SDO is ok */
00808   if ( (*m).len != 8) {
00809     MSG_ERR(0x1A67, "Error size SDO. CobId  : ", UNS16_LE(m->cob_id));
00810     failedSDO(d, nodeId, whoami, 0, 0, SDOABT_GENERAL_ERROR);
00811     return 0xFF;
00812   }
00813 
00814   if (whoami == SDO_CLIENT) {
00815     MSG_WAR(0x3A68, "I am CLIENT. Received SDO from nodeId : ", nodeId);
00816   }
00817   else {
00818     MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", UNS16_LE(m->cob_id));
00819   }
00820 
00821   /* Testing the command specifier */
00822   /* Allowed : cs = 0, 1, 2, 3, 4. (=  all except those for block tranfert). */
00823   /* cs = other : Not allowed -> abort. */
00824   switch (getSDOcs(m->data[0])) {
00825 
00826   case 0:
00827     /* I am SERVER */
00828     if (whoami == SDO_SERVER) {
00829       /* Receiving a download segment data. */
00830       /* A SDO transfert should have been yet initiated. */
00831       err = getSDOlineOnUse( d, nodeId, whoami, &line );
00832       if (!err)
00833     err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
00834       if (err) {
00835     MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ",
00836         nodeId);
00837     failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
00838     return 0xFF;
00839       }
00840       /* Reset the wathdog */
00841       RestartSDO_TIMER(line)
00842       MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", nodeId);
00843       index = d->transfers[line].index;
00844       subIndex = d->transfers[line].subIndex;
00845       /* Toggle test. */
00846       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
00847     MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0]));
00848     failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
00849     return 0xFF;
00850       }
00851       /* Nb of data to be downloaded */
00852       nbBytes = 7 - getSDOn3(m->data[0]);
00853       /* Store the data in the transfert structure. */
00854       err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
00855       if (err) {
00856     failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
00857     return 0xFF;
00858       }
00859       /* Sending the SDO response, CS = 1 */
00860       sdo.nodeId = *d->bDeviceNodeId; /* The node id of the server, (here it is the sender). */
00861       sdo.body.data[0] = (1 << 5) | (d->transfers[line].toggle << 4);
00862       for (i = 1 ; i < 8 ; i++)
00863     sdo.body.data[i] = 0;
00864       MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", nodeId);
00865       sendSDO(d, whoami, sdo);
00866       /* Inverting the toggle for the next segment. */
00867       d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
00868       /* If it was the last segment, */
00869       if (getSDOc(m->data[0])) {
00870     /* Transfering line data to object dictionary. */
00871     /* The code does not use the "d" of initiate frame. So it is safe if e=s=0 */
00872     errorCode = SDOlineToObjdict(d, line);
00873     if (errorCode) {
00874       MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0);
00875       failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
00876       return 0xFF;
00877     }
00878     /* Release of the line */
00879     resetSDOline(d, line);
00880     MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", nodeId);
00881       }
00882     } /* end if SERVER */
00883     else { /* if CLIENT */
00884       /* I am CLIENT */
00885       /* It is a request for a previous upload segment. We should find a line opened for this.*/
00886       err = getSDOlineOnUse( d, nodeId, whoami, &line);
00887       if (!err)
00888     err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
00889       if (err) {
00890     MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId);
00891     failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
00892     return 0xFF;
00893       }
00894       /* Reset the wathdog */
00895       RestartSDO_TIMER(line)
00896       index = d->transfers[line].index;
00897       subIndex = d->transfers[line].subIndex;
00898       /* test of the toggle; */
00899       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
00900     MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId);
00901     failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
00902     return 0xFF;
00903       }
00904       /* nb of data to be uploaded */
00905       nbBytes = 7 - getSDOn3(m->data[0]);
00906       /* Storing the data in the line structure. */
00907       err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
00908       if (err) {
00909     failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
00910     return 0xFF;
00911       }
00912       /* Inverting the toggle for the next segment. */
00913       d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
00914       /* If it was the last segment,*/
00915       if ( getSDOc(m->data[0])) {
00916     /* Put in state finished */
00917     /* The code is safe for the case e=s=0 in initiate frame. */
00918     StopSDO_TIMER(line)
00919     d->transfers[line].state = SDO_FINISHED;
00920     if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
00921 
00922     MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId);
00923       }
00924       else { /* more segments to receive */
00925          /* Sending the request for the next segment. */
00926     sdo.nodeId = nodeId;
00927     sdo.body.data[0] = (3 << 5) | (d->transfers[line].toggle << 4);
00928     for (i = 1 ; i < 8 ; i++)
00929       sdo.body.data[i] = 0;
00930     sendSDO(d, whoami, sdo);
00931     MSG_WAR(0x3A78, "SDO send upload segment request to nodeId", nodeId);
00932       }
00933     } /* End if CLIENT */
00934     break;
00935 
00936   case 1:
00937     /* I am SERVER */
00938     /* Receive of an initiate download */
00939     if (whoami == SDO_SERVER) {
00940       index = getSDOindex(m->data[1],m->data[2]);
00941       subIndex = getSDOsubIndex(m->data[3]);
00942       MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ",
00943           nodeId);
00944       MSG_WAR(0x3A80, "Writing at index : ", index);
00945       MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex);
00946 
00947       /* Search if a SDO transfert have been yet initiated */
00948       err = getSDOlineOnUse( d, nodeId, whoami, &line );
00949       if (! err) {
00950     MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0);
00951     failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
00952     return 0xFF;
00953       }
00954       /* No line on use. Great ! */
00955       /* Try to open a new line. */
00956       err = getSDOfreeLine( d, whoami, &line );
00957       if (err) {
00958     MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
00959     failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
00960     return 0xFF;
00961       }
00962       initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);
00963 
00964       if (getSDOe(m->data[0])) { /* If SDO expedited */
00965     /* nb of data to be downloaded */
00966     nbBytes = 4 - getSDOn2(m->data[0]);
00967     /* Storing the data in the line structure. */
00968     d->transfers[line].count = nbBytes;
00969     err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
00970 
00971     if (err) {
00972       failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
00973       return 0xFF;
00974     }
00975 
00976     /* SDO expedited -> transfert finished. Data can be stored in the dictionary. */
00977     /*The line will be reseted when it is downloading in the dictionary. */
00978     MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfert. Finished.: ", nodeId);
00979     /* Transfering line data to object dictionary. */
00980     errorCode = SDOlineToObjdict(d, line);
00981     if (errorCode) {
00982       MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0);
00983       failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
00984       return 0xFF;
00985     }
00986     /* Release of the line. */
00987     resetSDOline(d, line);
00988       }
00989       else {/* So, if it is not an expedited transfert */
00990     if (getSDOs(m->data[0])) {
00991       nbBytes = (m->data[4]) + ((UNS32)(m->data[5])<<8) + ((UNS32)(m->data[6])<<16) + ((UNS32)(m->data[7])<<24);
00992       err = setSDOlineRestBytes(d, nodeId, nbBytes);
00993       if (err) {
00994         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
00995         return 0xFF;
00996       }
00997     }
00998       }
00999       /*Sending a SDO, cs=3*/
01000       sdo.nodeId = *d->bDeviceNodeId; /* The node id of the server, (here it is the sender).*/
01001       sdo.body.data[0] = 3 << 5;
01002       sdo.body.data[1] = index & 0xFF;        /* LSB */
01003       sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
01004       sdo.body.data[3] = subIndex;
01005       for (i = 4 ; i < 8 ; i++)
01006         sdo.body.data[i] = 0;
01007       sendSDO(d, whoami, sdo);
01008     } /* end if I am SERVER */
01009     else {
01010       /* I am CLIENT */
01011       /* It is a response for a previous download segment. We should find a line opened for this. */
01012       err = getSDOlineOnUse( d, nodeId, whoami, &line);
01013       if (!err)
01014     err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
01015       if (err) {
01016     MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId);
01017     failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
01018     return 0xFF;
01019       }
01020       /* Reset the wathdog */
01021       RestartSDO_TIMER(line)
01022       index = d->transfers[line].index;
01023       subIndex = d->transfers[line].subIndex;
01024       /* test of the toggle; */
01025       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
01026     MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId);
01027     failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
01028     return 0xFF;
01029       }
01030 
01031       /* End transmission or downloading next segment. We need to know if it will be the last one. */
01032       getSDOlineRestBytes(d, line, &nbBytes);
01033       if (nbBytes == 0) {
01034     MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId);
01035     StopSDO_TIMER(line)
01036     d->transfers[line].state = SDO_FINISHED;
01037     if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
01038     return 0x00;
01039       }
01040       /* At least one transfer to send.    */
01041       if (nbBytes > 7) {
01042     /* several segments to download.*/
01043     /* code to send the next segment. (cs = 0; c = 0) */
01044     d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
01045     sdo.nodeId = nodeId; /* The server node Id; */
01046     sdo.body.data[0] = (d->transfers[line].toggle << 4);
01047     err = lineToSDO(d, line, 7, sdo.body.data + 1);
01048     if (err) {
01049       failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
01050       return 0xFF;
01051     }
01052       }
01053       else {
01054     /* Last segment. */
01055     /* code to send the last segment. (cs = 0; c = 1)*/
01056     d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
01057     sdo.nodeId = nodeId; /* The server node Id; */
01058     sdo.body.data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1);
01059     err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);
01060     if (err) {
01061       failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
01062       return 0xFF;
01063     }
01064     for (i = nbBytes + 1 ; i < 8 ; i++)
01065       sdo.body.data[i] = 0;
01066       }
01067       MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId);
01068       sendSDO(d, whoami, sdo);
01069     } /* end if I am a CLIENT */
01070     break;
01071 
01072   case 2:
01073     /* I am SERVER */
01074     /* Receive of an initiate upload.*/
01075     if (whoami == SDO_SERVER) {
01076       index = getSDOindex(m->data[1],m->data[2]);
01077       subIndex = getSDOsubIndex(m->data[3]);
01078       MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ",
01079           nodeId);
01080       MSG_WAR(0x3A90, "Reading at index : ", index);
01081       MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex);
01082       /* Search if a SDO transfert have been yet initiated*/
01083       err = getSDOlineOnUse( d, nodeId, whoami, &line );
01084       if (! err) {
01085         MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line);
01086         MSG_WAR(0x3A93, "nodeId = ", nodeId);
01087         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
01088         return 0xFF;
01089       }
01090       /* No line on use. Great !*/
01091       /* Try to open a new line.*/
01092       err = getSDOfreeLine( d, whoami, &line );
01093       if (err) {
01094     MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
01095     failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
01096     return 0xFF;
01097       }
01098       initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
01099       /* Transfer data from dictionary to the line structure. */
01100       errorCode = objdictToSDOline(d, line);
01101 
01102       if (errorCode) {
01103     MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ",
01104         errorCode);
01105     failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
01106     return 0xFF;
01107     }
01108       /* Preparing the response.*/
01109       getSDOlineRestBytes(d, line, &nbBytes);    /* Nb bytes to transfer ? */
01110       sdo.nodeId = nodeId; /* The server node Id; */
01111       if (nbBytes > 4) {
01112     /* normal transfert. (segmented). */
01113     /* code to send the initiate upload response. (cs = 2) */
01114     sdo.body.data[0] = (2 << 5) | 1;
01115     sdo.body.data[1] = index & 0xFF;        /* LSB */
01116     sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
01117     sdo.body.data[3] = subIndex;
01118     sdo.body.data[4] = (UNS8)nbBytes; /* Limitation of canfestival2 : Max tranfert is 256 bytes.*/
01119     /* It takes too much memory to upgrate to 2^32 because the size of data is also coded */
01120     /* in the object dictionary, at every index and subindex. */
01121     for (i = 5 ; i < 8 ; i++)
01122       sdo.body.data[i] = 0;
01123     MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId);
01124     sendSDO(d, whoami, sdo);
01125       }
01126       else {
01127     /* Expedited upload. (cs = 2 ; e = 1) */
01128     sdo.body.data[0] = (UNS8)((2 << 5) | ((4 - nbBytes) << 2) | 3);
01129     sdo.body.data[1] = index & 0xFF;        /* LSB */
01130     sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
01131     sdo.body.data[3] = subIndex;
01132     err = lineToSDO(d, line, nbBytes, sdo.body.data + 4);
01133     if (err) {
01134       failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
01135       return 0xFF;
01136     }
01137     for (i = 4 + nbBytes ; i < 8 ; i++)
01138       sdo.body.data[i] = 0;
01139     MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ",
01140         nodeId);
01141     sendSDO(d, whoami, sdo);
01142     /* Release the line.*/
01143     resetSDOline(d, line);
01144       }
01145     } /* end if I am SERVER*/
01146     else {
01147       /* I am CLIENT */
01148       /* It is the response for the previous initiate upload request.*/
01149       /* We should find a line opened for this. */
01150       err = getSDOlineOnUse( d, nodeId, whoami, &line);
01151       if (!err)
01152     err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
01153       if (err) {
01154     MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId);
01155     failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
01156     return 0xFF;
01157       }
01158       /* Reset the wathdog */
01159       RestartSDO_TIMER(line)
01160       index = d->transfers[line].index;
01161       subIndex = d->transfers[line].subIndex;
01162 
01163       if (getSDOe(m->data[0])) { /* If SDO expedited */
01164     /* nb of data to be uploaded */
01165       nbBytes = 4 - getSDOn2(m->data[0]);
01166     /* Storing the data in the line structure. */
01167     err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
01168     if (err) {
01169       failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
01170       return 0xFF;
01171     }
01172     /* SDO expedited -> transfert finished. data are available via  getReadResultNetworkDict(). */
01173     MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId);
01174     StopSDO_TIMER(line)
01175     d->transfers[line].count = nbBytes;
01176     d->transfers[line].state = SDO_FINISHED;
01177     if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
01178     return 0;
01179       }
01180       else { /* So, if it is not an expedited transfert */
01181     /* Storing the nb of data to receive. */
01182     if (getSDOs(m->data[0])) {
01183       nbBytes = m->data[4] + ((UNS32)(m->data[5])<<8) + ((UNS32)(m->data[6])<<16) + ((UNS32)(m->data[7])<<24);
01184       err = setSDOlineRestBytes(d, line, nbBytes);
01185       if (err) {
01186         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
01187         return 0xFF;
01188       }
01189     }
01190     /* Requesting next segment. (cs = 3) */
01191     sdo.nodeId = nodeId;
01192     sdo.body.data[0] = 3 << 5;
01193     for (i = 1 ; i < 8 ; i++)
01194       sdo.body.data[i] = 0;
01195     MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId);
01196     sendSDO(d, whoami, sdo);
01197       }
01198     } /* End if CLIENT */
01199     break;
01200 
01201   case 3:
01202     /* I am SERVER */
01203     if (whoami == SDO_SERVER) {
01204       /* Receiving a upload segment. */
01205       /* A SDO transfert should have been yet initiated. */
01206       err = getSDOlineOnUse( d, nodeId, whoami, &line );
01207       if (!err)
01208     err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
01209       if (err) {
01210     MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ",
01211         nodeId);
01212     failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
01213     return 0xFF;
01214       }
01215       /* Reset the wathdog */
01216       RestartSDO_TIMER(line)
01217       MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", nodeId);
01218       index = d->transfers[line].index;
01219       subIndex = d->transfers[line].subIndex;
01220       /* Toggle test.*/
01221       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
01222     MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0]));
01223     failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
01224     return 0xFF;
01225       }
01226       /* Uploading next segment. We need to know if it will be the last one. */
01227       getSDOlineRestBytes(d, line, &nbBytes);
01228       if (nbBytes > 7) {
01229     /* The segment to transfer is not the last one.*/
01230     /* code to send the next segment. (cs = 0; c = 0) */
01231     sdo.nodeId = nodeId; /* The server node Id; */
01232     sdo.body.data[0] = (d->transfers[line].toggle << 4);
01233     err = lineToSDO(d, line, 7, sdo.body.data + 1);
01234     if (err) {
01235       failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
01236       return 0xFF;
01237     }
01238     /* Inverting the toggle for the next tranfert. */
01239     d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
01240     MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", nodeId);
01241     sendSDO(d, whoami, sdo);
01242       }
01243       else {
01244     /* Last segment. */
01245     /* code to send the last segment. (cs = 0; c = 1) */
01246     sdo.nodeId = nodeId; /** The server node Id; */
01247     sdo.body.data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1);
01248     err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);
01249     if (err) {
01250       failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
01251       return 0xFF;
01252     }
01253     for (i = nbBytes + 1 ; i < 8 ; i++)
01254       sdo.body.data[i] = 0;
01255     MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", nodeId);
01256     sendSDO(d, whoami, sdo);
01257     /* Release the line */
01258     resetSDOline(d, line);
01259       }
01260     } /* end if SERVER*/
01261     else {
01262       /* I am CLIENT */
01263       /* It is the response for the previous initiate download request. */
01264       /* We should find a line opened for this. */
01265       err = getSDOlineOnUse( d, nodeId, whoami, &line);
01266       if (!err)
01267     err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
01268       if (err) {
01269     MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId);
01270     failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
01271     return 0xFF;
01272       }
01273       /* Reset the watchdog */
01274       RestartSDO_TIMER(line)
01275       index = d->transfers[line].index;
01276       subIndex = d->transfers[line].subIndex;
01277       /* End transmission or requesting  next segment. */
01278       getSDOlineRestBytes(d, line, &nbBytes);
01279       if (nbBytes == 0) {
01280     MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId);
01281     StopSDO_TIMER(line)
01282     d->transfers[line].state = SDO_FINISHED;
01283     if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
01284     return 0x00;
01285       }
01286       if (nbBytes > 7) {
01287     /* more than one request to send */
01288     /* code to send the next segment. (cs = 0; c = 0)    */
01289     sdo.nodeId = nodeId; /** The server node Id; */
01290     sdo.body.data[0] = (d->transfers[line].toggle << 4);
01291     err = lineToSDO(d, line, 7, sdo.body.data + 1);
01292     if (err) {
01293       failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
01294       return 0xFF;
01295     }
01296       }
01297       else {
01298     /* Last segment.*/
01299     /* code to send the last segment. (cs = 0; c = 1)    */
01300     sdo.nodeId = nodeId; /* The server node Id; */
01301     sdo.body.data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1);
01302     err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);
01303     if (err) {
01304       failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
01305       return 0xFF;
01306     }
01307     for (i = nbBytes + 1 ; i < 8 ; i++)
01308       sdo.body.data[i] = 0;
01309       }
01310       MSG_WAR(0x3AA7, "SDO sending download segment to nodeId", nodeId);
01311       sendSDO(d, whoami, sdo);
01312 
01313     } /* end if I am a CLIENT        */
01314     break;
01315 
01316    case 4:
01317      abortCode =
01318       (UNS32)m->data[4] |
01319       ((UNS32)m->data[5] << 8) |
01320       ((UNS32)m->data[6] << 16) |
01321       ((UNS32)m->data[7] << 24);
01322     /* Received SDO abort. */
01323     /* Looking for the line concerned. */
01324     if (whoami == SDO_SERVER) {
01325       err = getSDOlineOnUse( d, nodeId, whoami, &line );
01326       if (!err) {
01327     resetSDOline( d, line );
01328     MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode);
01329       }
01330       else
01331     MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode);
01332       /* Tips : The end user has no way to know that the server node has received an abort SDO. */
01333       /* Its is ok, I think.*/
01334     }
01335     else { /* If I am CLIENT */
01336       err = getSDOlineOnUse( d, nodeId, whoami, &line );
01337       if (!err) {
01338     /* The line *must* be released by the core program. */
01339     StopSDO_TIMER(line)
01340     d->transfers[line].state = SDO_ABORTED_RCV;
01341     d->transfers[line].abortCode = abortCode;
01342     MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode);
01343     if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
01344       }
01345       else
01346     MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode);
01347     }
01348     break;
01349   default:
01350     /* Error : Unknown cs */
01351     MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", getSDOcs(m->data[0]));
01352     return 0xFF;
01353 
01354   } /* End switch */
01355   return 0;
01356 }
01357 
01358 /*!
01359 **
01360 **
01361 ** @param d
01362 ** @param nodeId
01363 ** @param index
01364 ** @param subIndex
01365 ** @param count
01366 ** @param dataType
01367 ** @param data
01368 ** @param Callback
01369 ** @param endianize
01370 **
01371 ** @return
01372 **/
01373 INLINE UNS8 _writeNetworkDict  (CO_Data* d, UNS8 nodeId, UNS16 index,
01374                UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize)
01375 {
01376   UNS8 err;
01377   UNS8 SDOfound = 0;
01378   UNS8 line;
01379   s_SDO sdo;    /* SDO to transmit */
01380   UNS8 i;
01381   UNS32 j;
01382   UNS16     lastIndex;
01383   UNS16     offset;
01384   UNS8      *pNodeIdServer;
01385   UNS8      nodeIdServer;
01386 
01387   MSG_WAR(0x3AC0, "Send SDO to write in the dictionary of node : ", nodeId);
01388   MSG_WAR(0x3AC1, "                                   At index : ", index);
01389   MSG_WAR(0x3AC2, "                                   subIndex : ", subIndex);
01390   MSG_WAR(0x3AC3, "                                   nb bytes : ", count);
01391 
01392   /* Verify that there is no SDO communication yet. */
01393   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
01394   if (!err) {
01395     MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId);
01396     return 0xFF;
01397   }
01398   /* Taking the line ... */
01399   err = getSDOfreeLine( d, SDO_CLIENT, &line );
01400   if (err) {
01401     MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId);
01402     return (0xFF);
01403   }
01404   /* Check which SDO to use to communicate with the node */
01405   offset = d->firstIndex->SDO_CLT;
01406   lastIndex = d->lastIndex->SDO_CLT;
01407   if (offset == 0) {
01408     MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0);
01409     return 0xFF;
01410   }
01411   i = 0;
01412    while (offset <= lastIndex) {
01413      if (d->objdict[offset].bSubCount <= 3) {
01414      MSG_ERR(0x1AC8, "Subindex 3  not found at index ", 0x1280 + i);
01415      return 0xFF;
01416      }
01417      /* looking for the nodeId server */
01418      pNodeIdServer = (UNS8*) d->objdict[offset].pSubindex[3].pObject;
01419      nodeIdServer = *pNodeIdServer;
01420      MSG_WAR(0x1AD2, "index : ", 0x1280 + i);
01421      MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer);
01422 
01423     if(nodeIdServer == nodeId) {
01424       SDOfound = 1;
01425       break;
01426     }
01427     offset++;
01428     i++;
01429   } /* end while */
01430   if (!SDOfound) {
01431     MSG_ERR(0x1AC9, "SDO. Error. No client found to communicate with node : ", nodeId);
01432     return 0xFE;
01433   }
01434   MSG_WAR(0x3AD0,"        SDO client defined at index  : ", 0x1280 + i);
01435   initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);
01436   d->transfers[line].count = count;
01437   d->transfers[line].dataType = dataType;
01438 
01439 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
01440   {
01441     UNS8* lineData = d->transfers[line].data;
01442     if (count > SDO_MAX_LENGTH_TRANSFERT)
01443     {
01444       d->transfers[line].dynamicData = (UNS8*) malloc(count);
01445       d->transfers[line].dynamicDataSize = count;
01446       if (d->transfers[line].dynamicData == NULL)
01447       {
01448         MSG_ERR(0x1AC9, "SDO. Error. Could not allocate enough bytes : ", count);
01449         return 0xFE;
01450       }
01451       lineData = d->transfers[line].dynamicData;
01452     }
01453 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION
01454 
01455   /* Copy data to transfers structure. */
01456   for (j = 0 ; j < count ; j++) {
01457 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
01458 # ifdef CANOPEN_BIG_ENDIAN
01459       if (dataType == 0 && endianize)
01460         lineData[count - 1 - j] = ((char *)data)[j];
01461       else /* String of bytes. */
01462         lineData[j] = ((char *)data)[j];
01463 #  else
01464       lineData[j] = ((char *)data)[j];
01465 #  endif
01466   }
01467 #else //SDO_DYNAMIC_BUFFER_ALLOCATION
01468 # ifdef CANOPEN_BIG_ENDIAN
01469     if (dataType == 0 && endianize)
01470       d->transfers[line].data[count - 1 - j] = ((char *)data)[j];
01471     else /* String of bytes. */
01472       d->transfers[line].data[j] = ((char *)data)[j];
01473 #  else
01474     d->transfers[line].data[j] = ((char *)data)[j];
01475 #  endif
01476 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION
01477   }
01478   /* Send the SDO to the server. Initiate download, cs=1. */
01479   sdo.nodeId = nodeId;
01480   if (count <= 4) { /* Expedited transfert */
01481     sdo.body.data[0] = (UNS8)((1 << 5) | ((4 - count) << 2) | 3);
01482     for (i = 4 ; i < 8 ; i++)
01483       sdo.body.data[i] = d->transfers[line].data[i - 4];
01484     d->transfers[line].offset = count;
01485   }
01486   else { /** Normal transfert */
01487     sdo.body.data[0] = (1 << 5) | 1;
01488     for (i = 0 ; i < 4 ; i++)
01489       sdo.body.data[i+4] = (UNS8)((count >> (i<<3))); /* i*8 */
01490   }
01491   sdo.body.data[1] = index & 0xFF;        /* LSB */
01492   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
01493   sdo.body.data[3] = subIndex;
01494 
01495   d->transfers[line].Callback = Callback;
01496 
01497   err = sendSDO(d, SDO_CLIENT, sdo);
01498   if (err) {
01499     MSG_ERR(0x1AD1, "SDO. Error while sending SDO to node : ", nodeId);
01500     /* release the line */
01501     resetSDOline(d, line);
01502     return 0xFF;
01503   }
01504 
01505 
01506   return 0;
01507 }
01508 
01509 /*!
01510 **
01511 **
01512 ** @param d
01513 ** @param nodeId
01514 ** @param index
01515 ** @param subIndex
01516 ** @param count
01517 ** @param dataType
01518 ** @param data
01519 **
01520 ** @return
01521 **/
01522 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
01523                UNS8 subIndex, UNS32 count, UNS8 dataType, void *data)
01524 {
01525     return _writeNetworkDict  (d, nodeId, index, subIndex, count, dataType, data, NULL, 1);
01526 }
01527 
01528 /*!
01529 **
01530 **
01531 ** @param d
01532 ** @param nodeId
01533 ** @param index
01534 ** @param subIndex
01535 ** @param count
01536 ** @param dataType
01537 ** @param data
01538 ** @param Callback
01539 **
01540 ** @return
01541 **/
01542 UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index,
01543                UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback)
01544 {
01545     return _writeNetworkDict  (d, nodeId, index, subIndex, count, dataType, data, Callback, 1);
01546 }
01547 
01548 UNS8 writeNetworkDictCallBackAI (CO_Data* d, UNS8 nodeId, UNS16 index,
01549                UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize)
01550 {
01551     UNS8 ret;
01552     UNS16 lastIndex;
01553     UNS16 offset;
01554     UNS8 nodeIdServer;
01555     UNS8 i;
01556 
01557     ret = _writeNetworkDict  (d, nodeId, index, subIndex, count, dataType, data, Callback, endianize);
01558     if(ret == 0xFE)
01559     {
01560         offset = d->firstIndex->SDO_CLT;
01561         lastIndex = d->lastIndex->SDO_CLT;
01562         if (offset == 0)
01563         {
01564             MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0);
01565             return 0xFF;
01566         }
01567         i = 0;
01568         while (offset <= lastIndex)
01569         {
01570             if (d->objdict[offset].bSubCount <= 3)
01571             {
01572                 MSG_ERR(0x1AC8, "Subindex 3  not found at index ", 0x1280 + i);
01573                 return 0xFF;
01574             }
01575             nodeIdServer = *(UNS8*) d->objdict[offset].pSubindex[3].pObject;
01576             if(nodeIdServer == 0)
01577             {
01578                 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = (UNS32)(0x600 + nodeId);
01579                 *(UNS32*)d->objdict[offset].pSubindex[2].pObject = (UNS32)(0x580 + nodeId);
01580                 *(UNS8*) d->objdict[offset].pSubindex[3].pObject = nodeId;
01581                 return _writeNetworkDict  (d, nodeId, index, subIndex, count, dataType, data, Callback, 1);
01582             }
01583             offset++;
01584         }
01585         return 0xFF;
01586     }
01587     else if(ret == 0)
01588     {
01589         return 0;
01590     }
01591     else
01592     {
01593         return 0xFF;
01594     }
01595 }
01596 
01597 /*!
01598 **
01599 **
01600 ** @param d
01601 ** @param nodeId
01602 ** @param index
01603 ** @param subIndex
01604 ** @param dataType
01605 ** @param Callback
01606 **
01607 ** @return
01608 **/
01609 INLINE UNS8 _readNetworkDict  (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
01610 {
01611   UNS8 err;
01612   UNS8 SDOfound = 0;
01613   UNS8 i;
01614   UNS8 line;
01615   s_SDO sdo;    /* SDO to transmit */
01616   UNS8      *pNodeIdServer;
01617   UNS8      nodeIdServer;
01618   UNS16     offset;
01619   UNS16     lastIndex;
01620   MSG_WAR(0x3AD5, "Send SDO to read in the dictionary of node : ", nodeId);
01621   MSG_WAR(0x3AD6, "                                  At index : ", index);
01622   MSG_WAR(0x3AD7, "                                  subIndex : ", subIndex);
01623 
01624 
01625   /* Verify that there is no SDO communication yet. */
01626   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
01627   if (!err) {
01628     MSG_ERR(0x1AD8, "SDO error : Communication yet established. with node : ", nodeId);
01629     return 0xFF;
01630   }
01631   /* Taking the line ... */
01632   err = getSDOfreeLine( d, SDO_CLIENT, &line );
01633   if (err) {
01634     MSG_ERR(0x1AD9, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId);
01635     return (0xFF);
01636   }
01637   else
01638     MSG_WAR(0x3AE0, "Transmission on line : ", line);
01639 
01640   /* Check which SDO to use to communicate with the node */
01641   offset = d->firstIndex->SDO_CLT;
01642   lastIndex = d->lastIndex->SDO_CLT;
01643   if (offset == 0) {
01644     MSG_ERR(0x1AE1, "writeNetworkDict : No SDO client index found", 0);
01645     return 0xFF;
01646   }
01647   i = 0;
01648   while (offset <= lastIndex) {
01649      if (d->objdict[offset].bSubCount <= 3) {
01650      MSG_ERR(0x1AE2, "Subindex 3  not found at index ", 0x1280 + i);
01651      return 0xFF;
01652      }
01653      /* looking for the nodeId server */
01654      pNodeIdServer = (UNS8*) d->objdict[offset].pSubindex[3].pObject;
01655      nodeIdServer = *pNodeIdServer;
01656 
01657     if(nodeIdServer == nodeId) {
01658       SDOfound = 1;
01659       break;
01660     }
01661     offset++;
01662     i++;
01663   } /* end while */
01664   if (!SDOfound) {
01665     MSG_ERR(0x1AE3, "SDO. Error. No client found to communicate with node : ", nodeId);
01666     return 0xFE;
01667   }
01668   MSG_WAR(0x3AE4,"        SDO client defined at index  : ", 0x1280 + i);
01669   initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
01670   getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
01671   sdo.nodeId = nodeId;
01672   /* Send the SDO to the server. Initiate upload, cs=2. */
01673   d->transfers[line].dataType = dataType;
01674   sdo.body.data[0] = (2 << 5);
01675   sdo.body.data[1] = index & 0xFF;        /* LSB */
01676   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
01677   sdo.body.data[3] = subIndex;
01678   for (i = 4 ; i < 8 ; i++)
01679     sdo.body.data[i] = 0;
01680   d->transfers[line].Callback = Callback;
01681   err = sendSDO(d, SDO_CLIENT, sdo);
01682   if (err) {
01683     MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId);
01684     /* release the line */
01685     resetSDOline(d, line);
01686     return 0xFF;
01687   }
01688   return 0;
01689 }
01690 
01691 /*!
01692 **
01693 **
01694 ** @param d
01695 ** @param nodeId
01696 ** @param index
01697 ** @param subIndex
01698 ** @param dataType
01699 **
01700 ** @return
01701 **/
01702 UNS8 readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType)
01703 {
01704     return _readNetworkDict  (d, nodeId, index, subIndex, dataType, NULL);
01705 }
01706 
01707 /*!
01708 **
01709 **
01710 ** @param d
01711 ** @param nodeId
01712 ** @param index
01713 ** @param subIndex
01714 ** @param dataType
01715 ** @param Callback
01716 **
01717 ** @return
01718 **/
01719 UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
01720 {
01721     return _readNetworkDict  (d, nodeId, index, subIndex, dataType, Callback);
01722 }
01723 
01724 UNS8 readNetworkDictCallbackAI (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
01725 {
01726     UNS8 ret;
01727     UNS16 lastIndex;
01728     UNS16 offset;
01729     UNS8 nodeIdServer;
01730     UNS8 i;
01731 
01732     ret = _readNetworkDict  (d, nodeId, index, subIndex, dataType, Callback);
01733     if(ret == 0xFE)
01734     {
01735         offset = d->firstIndex->SDO_CLT;
01736         lastIndex = d->lastIndex->SDO_CLT;
01737         if (offset == 0)
01738         {
01739             MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0);
01740             return 0xFF;
01741         }
01742         i = 0;
01743         while (offset <= lastIndex)
01744         {
01745             if (d->objdict[offset].bSubCount <= 3)
01746             {
01747                 MSG_ERR(0x1AC8, "Subindex 3  not found at index ", 0x1280 + i);
01748                 return 0xFF;
01749             }
01750             nodeIdServer = *(UNS8*) d->objdict[offset].pSubindex[3].pObject;
01751             if(nodeIdServer == 0)
01752             {
01753                 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = (UNS32)(0x600 + nodeId);
01754                 *(UNS32*)d->objdict[offset].pSubindex[2].pObject = (UNS32)(0x580 + nodeId);
01755                 *(UNS8*) d->objdict[offset].pSubindex[3].pObject = nodeId;
01756                 return _readNetworkDict  (d, nodeId, index, subIndex, dataType, Callback);
01757             }
01758             offset++;
01759         }
01760         return 0xFF;
01761     }
01762     else if(ret == 0)
01763     {
01764         return 0;
01765     }
01766     else
01767     {
01768         return 0xFF;
01769     }
01770 }
01771 
01772 /*!
01773 **
01774 **
01775 ** @param d
01776 ** @param nodeId
01777 ** @param data
01778 ** @param size pointer to expected size, changed into returned size. Expected size will be truncated to transfered data size
01779 ** @param abortCode
01780 **
01781 ** @return
01782 **/
01783 UNS8 getReadResultNetworkDict (CO_Data* d, UNS8 nodeId, void* data, UNS32 *size,
01784                    UNS32 * abortCode)
01785 {
01786   UNS32 i;
01787   UNS8 err;
01788   UNS8 line;
01789   * abortCode = 0;
01790 
01791   /* Looking for the line tranfert. */
01792   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
01793   if (err) {
01794     MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId);
01795     return SDO_ABORTED_INTERNAL;
01796   }
01797   * abortCode = d->transfers[line].abortCode;
01798   if (d->transfers[line].state != SDO_FINISHED)
01799     return d->transfers[line].state;
01800 
01801   /* if SDO initiated with e=0 and s=0 count is null, offset carry effective size*/
01802   if( d->transfers[line].count == 0)
01803       d->transfers[line].count = d->transfers[line].offset;
01804   /* use transfers[line].count as max size */
01805   if( d->transfers[line].count < *size )
01806       *size = d->transfers[line].count;
01807 
01808   /* Copy payload to data pointer */
01809 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
01810   {
01811     UNS8 *lineData = d->transfers[line].data;
01812 
01813     if (d->transfers[line].dynamicData && d->transfers[line].dynamicDataSize)
01814     {
01815       lineData = d->transfers[line].dynamicData;
01816     }
01817     for  ( i = 0 ; i < *size ; i++) {
01818 # ifdef CANOPEN_BIG_ENDIAN
01819       if (d->transfers[line].dataType != visible_string)
01820         ( (char *) data)[*size - 1 - i] = lineData[i];
01821       else /* String of bytes. */
01822         ( (char *) data)[i] = lineData[i];
01823 # else
01824       ( (char *) data)[i] = lineData[i];
01825 # endif
01826     }
01827   }
01828 #else //SDO_DYNAMIC_BUFFER_ALLOCATION
01829   for  ( i = 0 ; i < *size ; i++) {
01830 # ifdef CANOPEN_BIG_ENDIAN
01831     if (d->transfers[line].dataType != visible_string)
01832       ( (char *) data)[*size - 1 - i] = d->transfers[line].data[i];
01833     else /* String of bytes. */
01834       ( (char *) data)[i] = d->transfers[line].data[i];
01835 # else
01836     ( (char *) data)[i] = d->transfers[line].data[i];
01837 # endif
01838   }
01839 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION
01840   return SDO_FINISHED;
01841 }
01842 
01843 /*!
01844 **
01845 **
01846 ** @param d
01847 ** @param nodeId
01848 ** @param abortCode
01849 **
01850 ** @return
01851 **/
01852 UNS8 getWriteResultNetworkDict (CO_Data* d, UNS8 nodeId, UNS32 * abortCode)
01853 {
01854   UNS8 line = 0;
01855   UNS8 err;
01856 
01857   * abortCode = 0;
01858   /* Looking for the line tranfert. */
01859   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
01860   if (err) {
01861     MSG_ERR(0x1AF1, "SDO error : No line found for communication with node : ", nodeId);
01862     return SDO_ABORTED_INTERNAL;
01863   }
01864   * abortCode = d->transfers[line].abortCode;
01865   return d->transfers[line].state;
01866 }