CanFestival: src/sdo.c Source File

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 */
-00032 /* #define DEBUG_WAR_CONSOLE_ON */
-00033 /* #define DEBUG_ERR_CONSOLE_ON */
-00034 
-00035 #include "objacces.h"
-00036 #include "sdo.h"
-00037 #include "canfestival.h"
-00038 
-00039 /* Uncomment if your compiler does not support inline functions */
-00040 #define NO_INLINE 
-00041 
-00042 #ifdef NO_INLINE
-00043   #define INLINE 
-00044 #else
-00045   #define INLINE inline
-00046 #endif
-00047 
-00048 /*Internals prototypes*/
-00049 
-00065 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, 
-00066                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize);
-00067 
-00080 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, 
-00081         UNS8 dataType, SDOCallback_t Callback);
-00082         
-00083 
-00084 /***************************************************************************/
-00085 /* SDO (un)packing macros */
-00086 
-00089 #define getSDOcs(byte) (byte >> 5)
-00090 
-00093 #define getSDOn2(byte) ((byte >> 2) & 3)
-00094 
-00097 #define getSDOn3(byte) ((byte >> 1) & 7)
-00098 
-00101 #define getSDOe(byte) ((byte >> 1) & 1)
-00102 
-00105 #define getSDOs(byte) (byte & 1)
-00106 
-00109 #define getSDOc(byte) (byte & 1)
-00110 
-00113 #define getSDOt(byte) ((byte >> 4) & 1)
-00114 
-00117 #define getSDOindex(byte1, byte2) ((byte2 << 8) | (byte1))
-00118 
-00121 #define getSDOsubIndex(byte3) (byte3)
-00122 
-00129 void SDOTimeoutAlarm(CO_Data* d, UNS32 id)
-00130 {
-00131     MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0);
-00132     MSG_WAR(0x2A02, "server node : ", d->transfers[id].nodeId);
-00133     MSG_WAR(0x2A02, "      index : ", d->transfers[id].index);
-00134     MSG_WAR(0x2A02, "   subIndex : ", d->transfers[id].subIndex); 
-00135     /* Reset timer handler */
-00136     d->transfers[id].timer = TIMER_NONE;
-00137     /*Set aborted state*/
-00138     d->transfers[id].state = SDO_ABORTED_INTERNAL;
-00139     /* Sending a SDO abort */
-00140     sendSDOabort(d, d->transfers[id].whoami, 
-00141                  d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT);
-00142     d->transfers[id].abortCode = SDOABT_TIMED_OUT;
-00143     /* Call the user function to inform of the problem.*/
-00144     if(d->transfers[id].Callback)
-00145         /*If ther is a callback, it is responsible to close SDO transfer (client)*/
-00146         (*d->transfers[id].Callback)(d,d->transfers[id].nodeId);
-00147     else if(d->transfers[id].whoami == SDO_SERVER)
-00148         /*Else, if server, reset the line*/
-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 
-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 
-00186 UNS32 SDOlineToObjdict (CO_Data* d, UNS8 line)
-00187 {
-00188   UNS8      size;
-00189   UNS32 errorCode;
-00190   MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line);
-00191   size = (UNS8)d->transfers[line].count;
-00192   errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, 
-00193                          (void *) d->transfers[line].data, &size, 1);
-00194   if (errorCode != OD_SUCCESSFUL)
-00195     return errorCode;
-00196   MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line);
-00197   return 0;
-00198 
-00199 }
-00200 
-00209 UNS32 objdictToSDOline (CO_Data* d, UNS8 line)
-00210 {
-00211   UNS8  size = 0;
-00212   UNS8  dataType;
-00213   UNS32 errorCode;
-00214 
-00215   MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index);
-00216   MSG_WAR(0x3A06, "  subIndex : ", d->transfers[line].subIndex);
-00217 
-00218   errorCode = getODentry(d,     d->transfers[line].index,
-00219                                 d->transfers[line].subIndex,
-00220                                 (void *)d->transfers[line].data,
-00221                                 &size, &dataType, 0);
-00222   
-00223   if (errorCode != OD_SUCCESSFUL)
-00224     return errorCode;
-00225 
-00226   d->transfers[line].count = size;
-00227   d->transfers[line].offset = 0;
-00228 #if 0
-00229    /*Me laisser a, please ! (FD)*/
-00230   {
-00231     UNS8 i;
-00232     for (i = 0 ; i < 10 ; i++) {
-00233       MSG_WAR(i, "data= ", d->transfers[line].data[i]);
-00234     }     
-00235   }
-00236 #endif
-00237   return 0;
-00238 }
-00239 
-00250 UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data) {
-00251   UNS8 i;
-00252   UNS8 offset;
-00253 
-00254   if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
-00255     MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
-00256     return 0xFF;
-00257   }
-00258     if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) {
-00259     MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes);
-00260     return 0xFF;
-00261   }
-00262   offset = (UNS8)d->transfers[line].offset;
-00263   for (i = 0 ; i < nbBytes ; i++) 
-00264     * (data + i) = d->transfers[line].data[offset + i];
-00265   d->transfers[line].offset = d->transfers[line].offset + nbBytes;
-00266   return 0;
-00267 }
-00268 
-00279 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data)
-00280 {
-00281   UNS8 i;
-00282   UNS8 offset;
-00283   
-00284   if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
-00285     MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
-00286     return 0xFF;
-00287   }
-00288   offset = (UNS8)d->transfers[line].offset;
-00289   for (i = 0 ; i < nbBytes ; i++) 
-00290     d->transfers[line].data[offset + i] = * (data + i);
-00291   d->transfers[line].offset = d->transfers[line].offset + nbBytes;
-00292   return 0;
-00293 }
-00294 
-00307 UNS8 failedSDO (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS16 index, 
-00308                 UNS8 subIndex, UNS32 abortCode)
-00309 {
-00310   UNS8 err;
-00311   UNS8 line;
-00312   err = getSDOlineOnUse( d, nodeId, whoami, &line );
-00313   if (!err) /* If a line on use have been found.*/
-00314     MSG_WAR(0x3A20, "FailedSDO : line found : ", line);
-00315   if ((! err) && (whoami == SDO_SERVER)) {
-00316     resetSDOline( d, line );
-00317     MSG_WAR(0x3A21, "FailedSDO : line released : ", line);
-00318   }
-00319   if ((! err) && (whoami == SDO_CLIENT)) {
-00320     StopSDO_TIMER(line);
-00321     d->transfers[line].state = SDO_ABORTED_INTERNAL;
-00322   }
-00323   MSG_WAR(0x3A22, "Sending SDO abort ", 0);
-00324   err = sendSDOabort(d, whoami, index, subIndex, abortCode);
-00325   if (err) {
-00326     MSG_WAR(0x3A23, "Unable to send the SDO abort", 0);
-00327     return 0xFF;
-00328   }
-00329   return 0;
-00330 }
-00331 
-00338 void resetSDOline ( CO_Data* d, UNS8 line )
-00339 {
-00340   UNS8 i; 
-00341   MSG_WAR(0x3A25, "reset SDO line nb : ", line); 
-00342   initSDOline(d, line, 0, 0, 0, SDO_RESET);
-00343   for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++)
-00344     d->transfers[line].data[i] = 0;
-00345 }
-00346 
-00359 UNS8 initSDOline (CO_Data* d, UNS8 line, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 state)
-00360 {
-00361   MSG_WAR(0x3A25, "init SDO line nb : ", line); 
-00362   if (state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS){
-00363         StartSDO_TIMER(line)
-00364   }else{
-00365         StopSDO_TIMER(line)
-00366   }
-00367   d->transfers[line].nodeId = nodeId; 
-00368   d->transfers[line].index = index;
-00369   d->transfers[line].subIndex = subIndex;
-00370   d->transfers[line].state = state;
-00371   d->transfers[line].toggle = 0;
-00372   d->transfers[line].count = 0;
-00373   d->transfers[line].offset = 0;
-00374   d->transfers[line].dataType = 0;
-00375   d->transfers[line].Callback = NULL;  
-00376   return 0;
-00377 }
-00378 
-00388 UNS8 getSDOfreeLine ( CO_Data* d, UNS8 whoami, UNS8 *line )
-00389 {
-00390         
-00391   UNS8 i;
-00392     
-00393   for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
-00394     if ( d->transfers[i].state == SDO_RESET ) {
-00395       *line = i;
-00396       d->transfers[i].whoami = whoami;
-00397       return 0;
-00398     } /* end if */
-00399   } /* end for */
-00400   MSG_ERR(0x1A25, "Too many SDO in progress. Aborted.", i);
-00401   return 0xFF;
-00402 }
-00403 
-00414 UNS8 getSDOlineOnUse (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS8 *line)
-00415 {
-00416         
-00417   UNS8 i;
-00418     
-00419   for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
-00420     if ( (d->transfers[i].state != SDO_RESET) &&
-00421          (d->transfers[i].nodeId == nodeId) && 
-00422          (d->transfers[i].whoami == whoami) ) {
-00423       *line = i;
-00424       return 0;
-00425     }
-00426   } 
-00427   return 0xFF;
-00428 }
-00429 
-00439 UNS8 closeSDOtransfer (CO_Data* d, UNS8 nodeId, UNS8 whoami)
-00440 {
-00441   UNS8 err;
-00442   UNS8 line;
-00443   err = getSDOlineOnUse(d, nodeId, whoami, &line);
-00444   if (err) {
-00445     MSG_WAR(0x2A30, "No SDO communication to close for node : ", nodeId); 
-00446     return 0xFF;
-00447   }
-00448   resetSDOline(d, line);  
-00449   return 0;
-00450 }
-00451 
-00461 UNS8 getSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 * nbBytes)
-00462 {
-00463   if (d->transfers[line].count == 0) /* if received initiate SDO protocol with e=0 and s=0 */
-00464     * nbBytes = 0;
-00465   else
-00466     * nbBytes = (UNS8)d->transfers[line].count - (UNS8)d->transfers[line].offset;
-00467   return 0;
-00468 }
-00469 
-00479 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 nbBytes)
-00480 {
-00481   if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) {
-00482     MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
-00483     return 0xFF;
-00484   }
-00485   d->transfers[line].count = nbBytes;
-00486   return 0;
-00487 }
-00488 
-00498 UNS8 sendSDO (CO_Data* d, UNS8 whoami, s_SDO sdo)
-00499 {       
-00500   UNS16 offset;
-00501   UNS16 lastIndex;
-00502   UNS8 found = 0;
-00503   Message m;
-00504   UNS8 i;
-00505   UNS32 * pwCobId = NULL;
-00506   UNS32 * pwNodeId = NULL;
-00507 
-00508   MSG_WAR(0x3A38, "sendSDO",0);
-00509   if( !((d->nodeState == Operational) ||  (d->nodeState == Pre_operational ))) {
-00510     MSG_WAR(0x2A39, "unable to send the SDO (not in op or pre-op mode", d->nodeState);
-00511     return 0xFF;
-00512   }                             
-00513 
-00514   /*get the server->client cobid*/
-00515   if ( whoami == SDO_SERVER )   {/*case server. Easy because today only one server SDO is authorized in CanFestival*/
-00516     offset = d->firstIndex->SDO_SVR;
-00517     if (offset == 0) {
-00518       MSG_ERR(0x1A42, "SendSDO : No SDO server found", 0); 
-00519       return 0xFF;
-00520     }
-00521     pwCobId = (UNS32*) d->objdict[offset].pSubindex[2].pObject;
-00522     MSG_WAR(0x3A41, "I am server. cobId : ", *pwCobId); 
-00523   }
-00524   else {                        /*case client*/
-00525     /* Get the client->server cobid.*/
-00526     UNS16 sdoNum = 0;
-00527     offset = d->firstIndex->SDO_CLT;
-00528     lastIndex = d->lastIndex->SDO_CLT;
-00529     if (offset == 0) {
-00530       MSG_ERR(0x1A42, "SendSDO : No SDO client index found", 0); 
-00531       return 0xFF;
-00532     }
-00533     /* First, have to find at the index where is defined the communication with the server node */
-00534     while (offset <= lastIndex){
-00535       MSG_WAR(0x3A43,"Reading index : ", 0x1280 + sdoNum);
-00536       if (d->objdict[offset].bSubCount <= 3) {
-00537         MSG_ERR(0x1A28, "Subindex 3  not found at index ", 0x1280 + sdoNum);
-00538         return 0xFF;
-00539       }
-00540       pwNodeId = (UNS32*) d->objdict[offset].pSubindex[3].pObject;
-00541       MSG_WAR(0x3A44, "Found nodeId server = ", *pwNodeId);     
-00542       if(*pwNodeId == sdo.nodeId) {
-00543         found = 1;
-00544         break;          
-00545       }      
-00546       offset ++;
-00547       sdoNum ++;
-00548     }
-00549     if (! found){
-00550       MSG_WAR (0x2A45, "No SDO client corresponds to the mesage to send to node ", sdo.nodeId);
-00551       return 0xFF;
-00552     }
-00553     /* Second, read the cobid client->server */
-00554     pwCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject;
-00555   }
-00556   /* message copy for sending */
-00557   m.cob_id.w = *pwCobId;
-00558   m.rtr = NOT_A_REQUEST; 
-00559   /* the length of SDO must be 8 */
-00560   m.len = 8;
-00561   for (i = 0 ; i < 8 ; i++) {
-00562     m.data[i] =  sdo.body.data[i];
-00563   }
-00564   return canSend(d->canHandle,&m);
-00565 }
-00566 
-00578 UNS8 sendSDOabort (CO_Data* d, UNS8 whoami, UNS16 index, UNS8 subIndex, UNS32 abortCode)
-00579 {
-00580   s_SDO sdo;
-00581   UNS8 ret;
-00582   MSG_WAR(0x2A50,"Sending SDO abort ", abortCode);
-00583   sdo.nodeId = *d->bDeviceNodeId;
-00584   sdo.body.data[0] = 0x80;
-00585   /* Index */
-00586   sdo.body.data[1] = index & 0xFF; /* LSB */
-00587   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
-00588   /* Subindex */
-00589   sdo.body.data[3] = subIndex;
-00590   /* Data */
-00591   sdo.body.data[4] = (UNS8)(abortCode & 0xFF);
-00592   sdo.body.data[5] = (UNS8)((abortCode >> 8) & 0xFF);
-00593   sdo.body.data[6] = (UNS8)((abortCode >> 16) & 0xFF);
-00594   sdo.body.data[7] = (UNS8)((abortCode >> 24) & 0xFF);
-00595   ret = sendSDO(d, whoami, sdo);
-00596 
-00597   return ret;
-00598 }
-00599 
-00608 UNS8 proceedSDO (CO_Data* d, Message *m)
-00609 {
-00610   UNS8 err;
-00611   UNS8 line;
-00612   UNS8 nbBytes; /* received or to be transmited. */
-00613   UNS8 nodeId = 0;  /* The node from which the SDO is received */
-00614   UNS32 nodeId_32; /* node id in 32 bits, for temporary use */
-00615   UNS32 *pNodeId = NULL;
-00616   UNS8 whoami = SDO_UNKNOWN;  /* SDO_SERVER or SDO_CLIENT.*/
-00617   UNS32 errorCode; /* while reading or writing in the local object dictionary.*/
-00618   s_SDO sdo;    /* SDO to transmit */
-00619   UNS16 index;
-00620   UNS8 subIndex;
-00621   UNS32 abortCode;
-00622   UNS8 i,j;
-00623   UNS32 *     pCobId = NULL;
-00624   UNS16 offset;
-00625   UNS16 lastIndex;
-00626 
-00627   MSG_WAR(0x3A60, "proceedSDO ", 0);
-00628   whoami = SDO_UNKNOWN;
-00629   /* Looking for the cobId in the object dictionary. */
-00630   /* Am-I a server ? */
-00631   offset = d->firstIndex->SDO_SVR;
-00632   lastIndex = d->lastIndex->SDO_SVR;
-00633   j = 0;
-00634   if(offset) while (offset <= lastIndex) {
-00635      if (d->objdict[offset].bSubCount <= 1) {
-00636           MSG_ERR(0x1A61, "Subindex 1  not found at index ", 0x1200 + j);
-00637           return 0xFF;
-00638         }
-00639       pCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject;
-00640       if ( *pCobId == (*m).cob_id.w ) {
-00641         whoami = SDO_SERVER;
-00642         MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j);
-00643         /* In case of server, the node id of the client may be unknown. So we put the index minus offset */
-00644         /* 0x1200 where the cobid received is defined. */
-00645         nodeId = j;
-00646         break;
-00647       }
-00648       j++;
-00649       offset++;
-00650   } /* end while */
-00651   if (whoami == SDO_UNKNOWN) {
-00652     /* Am-I client ? */
-00653     offset = d->firstIndex->SDO_CLT;
-00654     lastIndex = d->lastIndex->SDO_CLT;
-00655     j = 0;
-00656     if(offset) while (offset <= lastIndex) {
-00657        if (d->objdict[offset].bSubCount <= 3) {
-00658          MSG_ERR(0x1A63, "Subindex 3  not found at index ", 0x1280 + j);
-00659          return 0xFF;
-00660        }
-00661        /* a) Looking for the cobid received. */
-00662        pCobId = (UNS32*) d->objdict[offset].pSubindex[2].pObject;
-00663        if (*pCobId == (*m).cob_id.w ) {
-00664          /* b) cobid found, so reading the node id of the server. */
-00665          pNodeId = (UNS32*) d->objdict[offset].pSubindex[3].pObject;
-00666          whoami = SDO_CLIENT;
-00667          nodeId_32 = *pNodeId;
-00668          nodeId = (UNS8)nodeId_32;
-00669          MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j);
-00670          MSG_WAR(0x3A65, "                 Server nodeId : ", nodeId);
-00671          break;
-00672         }
-00673        j++;
-00674        offset++;
-00675     } /* end while */
-00676   }
-00677   if (whoami == SDO_UNKNOWN) {
-00678     return 0xFF;/* This SDO was not for us ! */
-00679   }
-00680 
-00681   /* Test if the size of the SDO is ok */
-00682   if ( (*m).len != 8) {
-00683     MSG_ERR(0x1A67, "Error size SDO. CobId  : ", (*m).cob_id.w);
-00684     failedSDO(d, nodeId, whoami, 0, 0, SDOABT_GENERAL_ERROR);
-00685     return 0xFF;
-00686   }
-00687   
-00688   if (whoami == SDO_CLIENT) {
-00689     MSG_WAR(0x3A68, "I am CLIENT. Received SDO from nodeId : ", nodeId);
-00690   }
-00691   else {
-00692     MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", (*m).cob_id.w);
-00693   }
-00694     
-00695   /* Testing the command specifier */
-00696   /* Allowed : cs = 0, 1, 2, 3, 4. (=  all except those for block tranfert). */
-00697   /* cs = other : Not allowed -> abort. */
-00698   switch (getSDOcs(m->data[0])) {
-00699 
-00700   case 0:
-00701     /* I am SERVER */
-00702     if (whoami == SDO_SERVER) {
-00703       /* Receiving a download segment data. */
-00704       /* A SDO transfert should have been yet initiated. */
-00705       err = getSDOlineOnUse( d, nodeId, whoami, &line ); 
-00706       if (!err)
-00707         err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
-00708       if (err) {
-00709         MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", 
-00710                 nodeId); 
-00711         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
-00712         return 0xFF;
-00713       }
-00714       /* Reset the wathdog */
-00715       RestartSDO_TIMER(line)
-00716       MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", nodeId); 
-00717       index = d->transfers[line].index;
-00718       subIndex = d->transfers[line].subIndex;
-00719       /* Toggle test. */
-00720       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
-00721         MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0])); 
-00722         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
-00723         return 0xFF;
-00724       }
-00725       /* Nb of data to be downloaded */
-00726       nbBytes = 7 - getSDOn3(m->data[0]);
-00727       /* Store the data in the transfert structure. */
-00728       err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
-00729       if (err) {
-00730         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-00731         return 0xFF;
-00732       }
-00733       /* Sending the SDO response, CS = 1 */
-00734       sdo.nodeId = *d->bDeviceNodeId; /* The node id of the server, (here it is the sender). */
-00735       sdo.body.data[0] = (1 << 5) | (d->transfers[line].toggle << 4);
-00736       for (i = 1 ; i < 8 ; i++)
-00737         sdo.body.data[i] = 0;
-00738       MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", nodeId); 
-00739       sendSDO(d, whoami, sdo);
-00740       /* Inverting the toggle for the next segment. */
-00741       d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
-00742       /* If it was the last segment, */
-00743       if (getSDOc(m->data[0])) {
-00744         /* Transfering line data to object dictionary. */
-00745         /* The code does not use the "d" of initiate frame. So it is safe if e=s=0 */
-00746         errorCode = SDOlineToObjdict(d, line);
-00747         if (errorCode) {
-00748           MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0); 
-00749           failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
-00750           return 0xFF;    
-00751         }
-00752         /* Release of the line */
-00753         resetSDOline(d, line);
-00754         MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", nodeId); 
-00755       }
-00756     } /* end if SERVER */
-00757     else { /* if CLIENT */
-00758       /* I am CLIENT */
-00759       /* It is a request for a previous upload segment. We should find a line opened for this.*/
-00760       err = getSDOlineOnUse( d, nodeId, whoami, &line);
-00761       if (!err)
-00762         err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
-00763       if (err) {
-00764         MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); 
-00765         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
-00766         return 0xFF;
-00767       }
-00768       /* Reset the wathdog */
-00769       RestartSDO_TIMER(line)
-00770       index = d->transfers[line].index;
-00771       subIndex = d->transfers[line].subIndex;
-00772       /* test of the toggle; */
-00773       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
-00774         MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId); 
-00775         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
-00776         return 0xFF;
-00777       }
-00778       /* nb of data to be uploaded */
-00779       nbBytes = 7 - getSDOn3(m->data[0]);
-00780       /* Storing the data in the line structure. */
-00781       err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
-00782       if (err) {
-00783         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-00784         return 0xFF;
-00785       }
-00786       /* Inverting the toggle for the next segment. */
-00787       d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
-00788       /* If it was the last segment,*/
-00789       if ( getSDOc(m->data[0])) {
-00790         /* Put in state finished */
-00791         /* The code is safe for the case e=s=0 in initiate frame. */
-00792         StopSDO_TIMER(line)
-00793         d->transfers[line].state = SDO_FINISHED;
-00794         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
-00795         
-00796         MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId);
-00797       }
-00798       else { /* more segments to receive */
-00799              /* Sending the request for the next segment. */
-00800         sdo.nodeId = nodeId;
-00801         sdo.body.data[0] = (3 << 5) | (d->transfers[line].toggle << 4);
-00802         for (i = 1 ; i < 8 ; i++)
-00803           sdo.body.data[i] = 0;
-00804         sendSDO(d, whoami, sdo);
-00805         MSG_WAR(0x3A78, "SDO send upload segment request to nodeId", nodeId);
-00806       }            
-00807     } /* End if CLIENT */
-00808     break;
-00809 
-00810   case 1:
-00811     /* I am SERVER */
-00812     /* Receive of an initiate download */
-00813     if (whoami == SDO_SERVER) {
-00814       index = getSDOindex(m->data[1],m->data[2]);
-00815       subIndex = getSDOsubIndex(m->data[3]);
-00816       MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ", 
-00817               nodeId); 
-00818       MSG_WAR(0x3A80, "Writing at index : ", index);
-00819       MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex);
-00820       
-00821       /* Search if a SDO transfert have been yet initiated */
-00822       err = getSDOlineOnUse( d, nodeId, whoami, &line );
-00823       if (! err) {
-00824         MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0); 
-00825         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
-00826         return 0xFF;
-00827       }
-00828       /* No line on use. Great ! */
-00829       /* Try to open a new line. */
-00830       err = getSDOfreeLine( d, whoami, &line );
-00831       if (err) {
-00832         MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
-00833         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
-00834         return 0xFF;
-00835       }
-00836       initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);      
-00837 
-00838       if (getSDOe(m->data[0])) { /* If SDO expedited */
-00839         /* nb of data to be downloaded */
-00840         nbBytes = 4 - getSDOn2(m->data[0]);
-00841         /* Storing the data in the line structure. */
-00842         d->transfers[line].count = nbBytes;
-00843         err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
-00844         
-00845         if (err) {
-00846           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-00847           return 0xFF;
-00848         }         
-00849 
-00850         /* SDO expedited -> transfert finished. Data can be stored in the dictionary. */
-00851         /*The line will be reseted when it is downloading in the dictionary. */
-00852         MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfert. Finished.: ", nodeId);
-00853         /* Transfering line data to object dictionary. */
-00854         errorCode = SDOlineToObjdict(d, line);
-00855         if (errorCode) {
-00856           MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0); 
-00857           failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
-00858           return 0xFF;
-00859         }
-00860         /* Release of the line. */
-00861         resetSDOline(d, line);
-00862       }
-00863       else {/* So, if it is not an expedited transfert */
-00864         if (getSDOs(m->data[0])) {
-00865           /* TODO : if e and s = 0, not reading m->data[4] but put nbBytes = 0 */
-00866           nbBytes = m->data[4]; /* Transfert limited to 255 bytes. */
-00867           err = setSDOlineRestBytes(d, nodeId, nbBytes);
-00868           if (err) {
-00869             failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-00870             return 0xFF;
-00871           }     
-00872         }
-00873       }
-00874       /*Sending a SDO, cs=3*/
-00875       sdo.nodeId = *d->bDeviceNodeId; /* The node id of the server, (here it is the sender).*/
-00876       sdo.body.data[0] = 3 << 5;
-00877       sdo.body.data[1] = index & 0xFF;        /* LSB */
-00878       sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
-00879       sdo.body.data[3] = subIndex;
-00880       for (i = 4 ; i < 8 ; i++)
-00881                 sdo.body.data[i] = 0;
-00882       sendSDO(d, whoami, sdo);
-00883     } /* end if I am SERVER */
-00884     else {
-00885       /* I am CLIENT */
-00886       /* It is a response for a previous download segment. We should find a line opened for this. */
-00887       err = getSDOlineOnUse( d, nodeId, whoami, &line);
-00888       if (!err)
-00889         err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
-00890       if (err) {
-00891         MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); 
-00892         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
-00893         return 0xFF;
-00894       }
-00895       /* Reset the wathdog */
-00896       RestartSDO_TIMER(line)
-00897       index = d->transfers[line].index;
-00898       subIndex = d->transfers[line].subIndex;
-00899       /* test of the toggle; */
-00900       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
-00901         MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId); 
-00902         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
-00903         return 0xFF;
-00904       }
-00905 
-00906       /* End transmission or downloading next segment. We need to know if it will be the last one. */
-00907       getSDOlineRestBytes(d, line, &nbBytes);
-00908       if (nbBytes == 0) {
-00909         MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId); 
-00910         StopSDO_TIMER(line)
-00911         d->transfers[line].state = SDO_FINISHED;
-00912         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
-00913         return 0x00;
-00914       }
-00915       /* At least one transfer to send. */
-00916       if (nbBytes > 7) {
-00917         /* several segments to download.*/
-00918         /* code to send the next segment. (cs = 0; c = 0) */
-00919         d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
-00920         sdo.nodeId = nodeId; /* The server node Id; */
-00921         sdo.body.data[0] = (d->transfers[line].toggle << 4);
-00922         err = lineToSDO(d, line, 7, sdo.body.data + 1);  
-00923         if (err) {
-00924           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-00925           return 0xFF;
-00926         }
-00927       } 
-00928       else {
-00929         /* Last segment. */
-00930         /* code to send the last segment. (cs = 0; c = 1)*/
-00931         d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
-00932         sdo.nodeId = nodeId; /* The server node Id; */
-00933         sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
-00934         err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);    
-00935         if (err) {
-00936           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-00937           return 0xFF;
-00938         }
-00939         for (i = nbBytes + 1 ; i < 8 ; i++)
-00940           sdo.body.data[i] = 0;
-00941       }
-00942       MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId); 
-00943       sendSDO(d, whoami, sdo); 
-00944     } /* end if I am a CLIENT */                          
-00945     break;
-00946 
-00947   case 2:
-00948     /* I am SERVER */
-00949     /* Receive of an initiate upload.*/
-00950     if (whoami == SDO_SERVER) {
-00951       index = getSDOindex(m->data[1],m->data[2]);
-00952       subIndex = getSDOsubIndex(m->data[3]);
-00953       MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", 
-00954               nodeId); 
-00955       MSG_WAR(0x3A90, "Reading at index : ", index);
-00956       MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex);
-00957       /* Search if a SDO transfert have been yet initiated*/
-00958       err = getSDOlineOnUse( d, nodeId, whoami, &line );
-00959       if (! err) {
-00960             MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line); 
-00961         MSG_WAR(0x3A93, "nodeId = ", nodeId); 
-00962             failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
-00963             return 0xFF;
-00964       }
-00965       /* No line on use. Great !*/
-00966       /* Try to open a new line.*/
-00967       err = getSDOfreeLine( d, whoami, &line );
-00968       if (err) {
-00969         MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
-00970         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
-00971         return 0xFF;
-00972       }
-00973       initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
-00974       /* Transfer data from dictionary to the line structure. */
-00975       errorCode = objdictToSDOline(d, line);
-00976      
-00977       if (errorCode) {
-00978         MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ", 
-00979                 errorCode); 
-00980         failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
-00981         return 0xFF;
-00982         }
-00983       /* Preparing the response.*/
-00984       getSDOlineRestBytes(d, line, &nbBytes);   /* Nb bytes to transfer ? */
-00985       sdo.nodeId = nodeId; /* The server node Id; */
-00986       if (nbBytes > 4) {
-00987         /* normal transfert. (segmented). */
-00988         /* code to send the initiate upload response. (cs = 2) */
-00989         sdo.body.data[0] = (2 << 5) | 1;
-00990         sdo.body.data[1] = index & 0xFF;        /* LSB */
-00991         sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
-00992         sdo.body.data[3] = subIndex;
-00993         sdo.body.data[4] = nbBytes; /* Limitation of canfestival2 : Max tranfert is 256 bytes.*/
-00994         /* It takes too much memory to upgrate to 2^32 because the size of data is also coded */
-00995         /* in the object dictionary, at every index and subindex. */
-00996         for (i = 5 ; i < 8 ; i++)
-00997           sdo.body.data[i] = 0;
-00998         MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId); 
-00999         sendSDO(d, whoami, sdo); 
-01000       }
-01001       else {
-01002         /* Expedited upload. (cs = 2 ; e = 1) */
-01003         sdo.body.data[0] = (2 << 5) | ((4 - nbBytes) << 2) | 3;  
-01004         sdo.body.data[1] = index & 0xFF;        /* LSB */
-01005         sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
-01006         sdo.body.data[3] = subIndex;
-01007         err = lineToSDO(d, line, nbBytes, sdo.body.data + 4);    
-01008         if (err) {
-01009           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-01010           return 0xFF;
-01011         }
-01012         for (i = 4 + nbBytes ; i < 8 ; i++)
-01013           sdo.body.data[i] = 0;
-01014         MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ", 
-01015                 nodeId); 
-01016         sendSDO(d, whoami, sdo); 
-01017         /* Release the line.*/
-01018         resetSDOline(d, line);
-01019       }
-01020     } /* end if I am SERVER*/
-01021     else {
-01022       /* I am CLIENT */
-01023       /* It is the response for the previous initiate upload request.*/
-01024       /* We should find a line opened for this. */
-01025       err = getSDOlineOnUse( d, nodeId, whoami, &line);
-01026       if (!err)
-01027         err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
-01028       if (err) {
-01029         MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId); 
-01030         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
-01031         return 0xFF;
-01032       }
-01033       /* Reset the wathdog */
-01034       RestartSDO_TIMER(line)
-01035       index = d->transfers[line].index;
-01036       subIndex = d->transfers[line].subIndex;
-01037       
-01038       if (getSDOe(m->data[0])) { /* If SDO expedited */
-01039         /* nb of data to be uploaded */
-01040           nbBytes = 4 - getSDOn2(m->data[0]);
-01041         /* Storing the data in the line structure. */
-01042         err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
-01043         if (err) {
-01044           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-01045           return 0xFF;
-01046         }
-01047         /* SDO expedited -> transfert finished. data are available via  getReadResultNetworkDict(). */
-01048         MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId);
-01049         StopSDO_TIMER(line)
-01050         d->transfers[line].count = nbBytes;
-01051         d->transfers[line].state = SDO_FINISHED;
-01052         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
-01053         return 0;
-01054       }
-01055       else { /* So, if it is not an expedited transfert */
-01056         /* Storing the nb of data to receive. */
-01057         if (getSDOs(m->data[0])) {
-01058           nbBytes = m->data[4]; /* Remember the limitation to 255 bytes to transfert */
-01059           err = setSDOlineRestBytes(d, line, nbBytes);
-01060           if (err) {
-01061             failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-01062             return 0xFF;
-01063           }     
-01064         }
-01065         /* Requesting next segment. (cs = 3) */
-01066         sdo.nodeId = nodeId;
-01067         sdo.body.data[0] = 3 << 5;
-01068         for (i = 1 ; i < 8 ; i++)
-01069           sdo.body.data[i] = 0;
-01070         MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId); 
-01071         sendSDO(d, whoami, sdo);  
-01072       }
-01073     } /* End if CLIENT */
-01074     break;
-01075 
-01076   case 3:
-01077     /* I am SERVER */
-01078     if (whoami == SDO_SERVER) {
-01079       /* Receiving a upload segment. */
-01080       /* A SDO transfert should have been yet initiated. */
-01081       err = getSDOlineOnUse( d, nodeId, whoami, &line ); 
-01082       if (!err)
-01083         err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
-01084       if (err) {
-01085         MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", 
-01086                 nodeId); 
-01087         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
-01088         return 0xFF;
-01089       }
-01090       /* Reset the wathdog */
-01091       RestartSDO_TIMER(line)
-01092       MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", nodeId); 
-01093       index = d->transfers[line].index;
-01094       subIndex = d->transfers[line].subIndex;
-01095       /* Toggle test.*/
-01096       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
-01097         MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0])); 
-01098         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
-01099         return 0xFF;
-01100       }
-01101       /* Uploading next segment. We need to know if it will be the last one. */
-01102       getSDOlineRestBytes(d, line, &nbBytes);             
-01103       if (nbBytes > 7) {
-01104         /* The segment to transfer is not the last one.*/
-01105         /* code to send the next segment. (cs = 0; c = 0) */
-01106         sdo.nodeId = nodeId; /* The server node Id; */
-01107         sdo.body.data[0] = (d->transfers[line].toggle << 4);
-01108         err = lineToSDO(d, line, 7, sdo.body.data + 1);  
-01109         if (err) {
-01110           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-01111           return 0xFF;
-01112         }
-01113         /* Inverting the toggle for the next tranfert. */
-01114         d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
-01115         MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", nodeId); 
-01116         sendSDO(d, whoami, sdo); 
-01117       } 
-01118       else {
-01119         /* Last segment. */
-01120         /* code to send the last segment. (cs = 0; c = 1) */        
-01121         sdo.nodeId = nodeId; 
-01122         sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
-01123         err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);    
-01124         if (err) {
-01125           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-01126           return 0xFF;
-01127         }
-01128         for (i = nbBytes + 1 ; i < 8 ; i++)
-01129           sdo.body.data[i] = 0;
-01130         MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", nodeId);      
-01131         sendSDO(d, whoami, sdo);
-01132         /* Release the line */
-01133         resetSDOline(d, line);
-01134       }
-01135     } /* end if SERVER*/
-01136     else {
-01137       /* I am CLIENT */
-01138       /* It is the response for the previous initiate download request. */
-01139       /* We should find a line opened for this. */
-01140       err = getSDOlineOnUse( d, nodeId, whoami, &line);
-01141       if (!err)
-01142         err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
-01143       if (err) {
-01144         MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId); 
-01145         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
-01146         return 0xFF;
-01147       }
-01148       /* Reset the watchdog */
-01149       RestartSDO_TIMER(line)
-01150       index = d->transfers[line].index;
-01151       subIndex = d->transfers[line].subIndex;
-01152       /* End transmission or requesting  next segment. */
-01153       getSDOlineRestBytes(d, line, &nbBytes);
-01154       if (nbBytes == 0) {
-01155         MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId); 
-01156         StopSDO_TIMER(line)
-01157         d->transfers[line].state = SDO_FINISHED;
-01158         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
-01159         return 0x00;
-01160       }   
-01161       if (nbBytes > 7) {
-01162         /* more than one request to send */
-01163         /* code to send the next segment. (cs = 0; c = 0)       */    
-01164         sdo.nodeId = nodeId; 
-01165         sdo.body.data[0] = (d->transfers[line].toggle << 4);
-01166         err = lineToSDO(d, line, 7, sdo.body.data + 1);  
-01167         if (err) {
-01168           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-01169           return 0xFF;
-01170         }
-01171       } 
-01172       else {
-01173         /* Last segment.*/
-01174         /* code to send the last segment. (cs = 0; c = 1)       */   
-01175         sdo.nodeId = nodeId; /* The server node Id; */
-01176         sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
-01177         err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);    
-01178         if (err) {
-01179           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
-01180           return 0xFF;
-01181         }
-01182         for (i = nbBytes + 1 ; i < 8 ; i++)
-01183           sdo.body.data[i] = 0;
-01184       }
-01185       MSG_WAR(0x3AA7, "SDO sending download segment to nodeId", nodeId); 
-01186       sendSDO(d, whoami, sdo); 
-01187 
-01188     } /* end if I am a CLIENT           */        
-01189     break;  
-01190 
-01191    case 4:
-01192      abortCode = (*m).data[3] |
-01193       ((UNS32)m->data[5] << 8) |
-01194       ((UNS32)m->data[6] << 16) |
-01195       ((UNS32)m->data[7] << 24);
-01196     /* Received SDO abort. */
-01197     /* Looking for the line concerned. */
-01198     if (whoami == SDO_SERVER) {
-01199       err = getSDOlineOnUse( d, nodeId, whoami, &line );
-01200       if (!err) {
-01201         resetSDOline( d, line );
-01202         MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode);
-01203       }
-01204       else
-01205         MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode);
-01206       /* Tips : The end user has no way to know that the server node has received an abort SDO. */
-01207       /* Its is ok, I think.*/
-01208     }
-01209     else { /* If I am CLIENT */
-01210       err = getSDOlineOnUse( d, nodeId, whoami, &line );
-01211       if (!err) {
-01212         /* The line *must* be released by the core program. */
-01213         StopSDO_TIMER(line)
-01214         d->transfers[line].state = SDO_ABORTED_RCV;
-01215         d->transfers[line].abortCode = abortCode;
-01216         MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode);
-01217         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
-01218       }
-01219       else
-01220         MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode);
-01221     } 
-01222     break;
-01223   default:
-01224     /* Error : Unknown cs */
-01225     MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", getSDOcs(m->data[0]));
-01226     return 0xFF;
-01227 
-01228   } /* End switch */
-01229   return 0;     
-01230 }
-01231 
-01247 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, 
-01248                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize)
-01249 {
-01250   UNS8 err;
-01251   UNS8 SDOfound = 0;
-01252   UNS8 line;
-01253   s_SDO sdo;    /* SDO to transmit */
-01254   UNS8 i, j;
-01255   UNS16     lastIndex;
-01256   UNS16     offset;
-01257   UNS32      *pNodeIdServer;
-01258   UNS32      nodeIdServer;
-01259 
-01260   MSG_WAR(0x3AC0, "Send SDO to write in the dictionary of node : ", nodeId);
-01261   MSG_WAR(0x3AC1, "                                   At index : ", index);
-01262   MSG_WAR(0x3AC2, "                                   subIndex : ", subIndex);
-01263   MSG_WAR(0x3AC3, "                                   nb bytes : ", count);
-01264 
-01265   /* Verify that there is no SDO communication yet. */
-01266   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
-01267   if (!err) {
-01268     MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId); 
-01269     return 0xFF;
-01270   }
-01271   /* Taking the line ... */
-01272   err = getSDOfreeLine( d, SDO_CLIENT, &line );
-01273   if (err) {
-01274     MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); 
-01275     return (0xFF);
-01276   }
-01277   /* Check which SDO to use to communicate with the node */
-01278   offset = d->firstIndex->SDO_CLT;
-01279   lastIndex = d->lastIndex->SDO_CLT;
-01280   if (offset == 0) {
-01281     MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0); 
-01282     return 0xFF;
-01283   }
-01284   i = 0;
-01285    while (offset <= lastIndex) {
-01286      if (d->objdict[offset].bSubCount <= 3) {
-01287          MSG_ERR(0x1AC8, "Subindex 3  not found at index ", 0x1280 + i);
-01288          return 0xFF;
-01289      }
-01290      /* looking for the nodeId server */
-01291      pNodeIdServer = (UNS32*) d->objdict[offset].pSubindex[3].pObject;
-01292      nodeIdServer = *pNodeIdServer;
-01293      MSG_WAR(0x1AD2, "index : ", 0x1280 + i);
-01294      MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer);
-01295    
-01296     if(nodeIdServer == (UNS32)nodeId) {
-01297       SDOfound = 1;
-01298       break;
-01299     }
-01300     offset++;
-01301     i++;
-01302   } /* end while */
-01303   if (!SDOfound) {
-01304     MSG_ERR(0x1AC9, "SDO. Error. No client found to communicate with node : ", nodeId);
-01305     return 0xFF;
-01306   }
-01307   MSG_WAR(0x3AD0,"        SDO client defined at index  : ", 0x1280 + i);
-01308   initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);
-01309   d->transfers[line].count = count;
-01310   d->transfers[line].dataType = dataType;
-01311   
-01312   /* Copy data to transfers structure. */
-01313   for (j = 0 ; j < count ; j++) {
-01314 # ifdef CANOPEN_BIG_ENDIAN
-01315     if (dataType == 0 && endianize)
-01316       d->transfers[line].data[count - 1 - j] = ((char *)data)[j];
-01317     else /* String of bytes. */
-01318       d->transfers[line].data[j] = ((char *)data)[j];
-01319 #  else 
-01320     d->transfers[line].data[j] = ((char *)data)[j];
-01321 #  endif
-01322   }
-01323   /* Send the SDO to the server. Initiate download, cs=1. */
-01324   sdo.nodeId = nodeId;
-01325   if (count <= 4) { /* Expedited transfert */
-01326     sdo.body.data[0] = (1 << 5) | ((4 - count) << 2) | 3;
-01327     for (i = 4 ; i < 8 ; i++)
-01328       sdo.body.data[i] = d->transfers[line].data[i - 4];
-01329     d->transfers[line].offset = count;
-01330   }     
-01331   else { 
-01332     sdo.body.data[0] = (1 << 5) | 1;
-01333     sdo.body.data[4] = count; /* nb of byte to transmit. Max = 255. (canfestival2 limitation). */
-01334     for (i = 5 ; i < 8 ; i++)
-01335       sdo.body.data[i] = 0;
-01336   }
-01337   sdo.body.data[1] = index & 0xFF;        /* LSB */
-01338   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
-01339   sdo.body.data[3] = subIndex;
-01340 
-01341   d->transfers[line].Callback = Callback;
-01342     
-01343   err = sendSDO(d, SDO_CLIENT, sdo);
-01344   if (err) {
-01345     MSG_ERR(0x1AD1, "SDO. Error while sending SDO to node : ", nodeId);
-01346     /* release the line */
-01347     resetSDOline(d, line);
-01348     return 0xFF;
-01349   }
-01350 
-01351   
-01352   return 0;
-01353 }
-01354 
-01368 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, 
-01369                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data)
-01370 {
-01371         return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, NULL, 1);
-01372 }
-01373 
-01388 UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index, 
-01389                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback)
-01390 {
-01391         return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1);      
-01392 }
-01393 
-01406 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
-01407 {
-01408   UNS8 err;
-01409   UNS8 SDOfound = 0;
-01410   UNS8 i;
-01411   UNS8 line;
-01412   s_SDO sdo;    /* SDO to transmit */
-01413   UNS32      *pNodeIdServer;
-01414   UNS32      nodeIdServer;
-01415   UNS16     offset;
-01416   UNS16     lastIndex;
-01417   MSG_WAR(0x3AD5, "Send SDO to read in the dictionary of node : ", nodeId);
-01418   MSG_WAR(0x3AD6, "                                  At index : ", index);
-01419   MSG_WAR(0x3AD7, "                                  subIndex : ", subIndex);
-01420 
-01421 
-01422   /* Verify that there is no SDO communication yet. */
-01423   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
-01424   if (!err) {
-01425     MSG_ERR(0x1AD8, "SDO error : Communication yet established. with node : ", nodeId); 
-01426     return 0xFF;
-01427   }
-01428   /* Taking the line ... */
-01429   err = getSDOfreeLine( d, SDO_CLIENT, &line );
-01430   if (err) {
-01431     MSG_ERR(0x1AD9, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); 
-01432     return (0xFF);
-01433   }
-01434   else
-01435     MSG_WAR(0x3AE0, "Transmission on line : ", line);
-01436 
-01437   /* Check which SDO to use to communicate with the node */
-01438   offset = d->firstIndex->SDO_CLT;
-01439   lastIndex = d->lastIndex->SDO_CLT;
-01440   if (offset == 0) {
-01441     MSG_ERR(0x1AE1, "writeNetworkDict : No SDO client index found", 0); 
-01442     return 0xFF;
-01443   }
-01444   i = 0;
-01445   while (offset <= lastIndex) {
-01446      if (d->objdict[offset].bSubCount <= 3) {
-01447          MSG_ERR(0x1AE2, "Subindex 3  not found at index ", 0x1280 + i);
-01448          return 0xFF;
-01449      }
-01450      /* looking for the nodeId server */
-01451      pNodeIdServer = (UNS32*) d->objdict[offset].pSubindex[3].pObject;
-01452      nodeIdServer = *pNodeIdServer;
-01453    
-01454     if(nodeIdServer == (UNS32)nodeId) {
-01455       SDOfound = 1;
-01456       break;
-01457     }
-01458     offset++;
-01459     i++;
-01460   } /* end while */
-01461   if (!SDOfound) {
-01462     MSG_ERR(0x1AE3, "SDO. Error. No client found to communicate with node : ", nodeId);
-01463     return 0xFF;
-01464   }
-01465   MSG_WAR(0x3AE4,"        SDO client defined at index  : ", 0x1280 + i);
-01466   initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
-01467   getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
-01468   sdo.nodeId = nodeId;
-01469   /* Send the SDO to the server. Initiate upload, cs=2. */
-01470   d->transfers[line].dataType = dataType;                               
-01471   sdo.body.data[0] = (2 << 5);  
-01472   sdo.body.data[1] = index & 0xFF;        /* LSB */
-01473   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
-01474   sdo.body.data[3] = subIndex;
-01475   for (i = 4 ; i < 8 ; i++)
-01476     sdo.body.data[i] = 0;
-01477   d->transfers[line].Callback = Callback;
-01478   err = sendSDO(d, SDO_CLIENT, sdo);
-01479   if (err) {
-01480     MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId);
-01481     /* release the line */
-01482     resetSDOline(d, line);
-01483     return 0xFF;
-01484   }             
-01485   return 0;
-01486 }
-01487 
-01499 UNS8 readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType)
-01500 {
-01501         return _readNetworkDict (d, nodeId, index, subIndex, dataType, NULL);
-01502 }
-01503 
-01516 UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
-01517 {
-01518         return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback);
-01519 }
-01520 
-01532 UNS8 getReadResultNetworkDict (CO_Data* d, UNS8 nodeId, void* data, UNS8 *size, 
-01533                                UNS32 * abortCode)
-01534 {
-01535   UNS8 i;
-01536   UNS8 err;
-01537   UNS8 line;
-01538   * size = 0;
-01539 
-01540   /* Looking for the line tranfert. */
-01541   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
-01542   if (err) {
-01543     MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId); 
-01544     return SDO_ABORTED_INTERNAL;
-01545   }
-01546   if (d->transfers[line].state != SDO_FINISHED)
-01547     return d->transfers[line].state;
-01548 
-01549   /* Transfert is finished. Put the value in the data. */
-01550   * size = (UNS8)d->transfers[line].count;
-01551   for  ( i = 0 ; i < *size ; i++) {
-01552 # ifdef CANOPEN_BIG_ENDIAN
-01553     if (d->transfers[line].dataType != visible_string)
-01554       ( (char *) data)[*size - 1 - i] = d->transfers[line].data[i];
-01555     else /* String of bytes. */
-01556       ( (char *) data)[i] = d->transfers[line].data[i];
-01557 # else 
-01558     ( (char *) data)[i] = d->transfers[line].data[i];
-01559 # endif
-01560   } 
-01561   return SDO_FINISHED;
-01562 }
-01563 
-01573 UNS8 getWriteResultNetworkDict (CO_Data* d, UNS8 nodeId, UNS32 * abortCode)
-01574 {
-01575   UNS8 line = 0;
-01576   UNS8 err;
-01577 
-01578   * abortCode = 0;
-01579   /* Looking for the line tranfert. */
-01580   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
-01581   if (err) {
-01582     MSG_ERR(0x1AF1, "SDO error : No line found for communication with node : ", nodeId); 
-01583     return SDO_ABORTED_INTERNAL;
-01584   }
-01585   * abortCode = d->transfers[line].abortCode;
-01586   return d->transfers[line].state;
-01587 }
-