CanFestival: /home/epimerde/documents/tc11/CanFestival-3/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 */
+00022 
+00023 /* #define DEBUG_WAR_CONSOLE_ON */
+00024 /* #define DEBUG_ERR_CONSOLE_ON */
+00025 
+00026 #include "objacces.h"
+00027 #include "sdo.h"
+00028 #include "canfestival.h"
+00029 
+00030 /* Uncomment if your compiler does not support inline functions */
+00031 #define NO_INLINE 
+00032 
+00033 #ifdef NO_INLINE
+00034   #define INLINE 
+00035 #else
+00036   #define INLINE inline
+00037 #endif
+00038 
+00039 /*Internals prototypes*/
+00040 
+00042 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, 
+00043                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize);
+00044 
+00046 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, 
+00047         UNS8 dataType, SDOCallback_t Callback);
+00048         
+00049 
+00050 /***************************************************************************/
+00051 /* SDO (un)packing macros */
+00052 
+00055 #define getSDOcs(byte) (byte >> 5)
+00056 
+00059 #define getSDOn2(byte) ((byte >> 2) & 3)
+00060 
+00063 #define getSDOn3(byte) ((byte >> 1) & 7)
+00064 
+00067 #define getSDOe(byte) ((byte >> 1) & 1)
+00068 
+00071 #define getSDOs(byte) (byte & 1)
+00072 
+00075 #define getSDOc(byte) (byte & 1)
+00076 
+00079 #define getSDOt(byte) ((byte >> 4) & 1)
+00080 
+00083 #define getSDOindex(byte1, byte2) ((byte2 << 8) | (byte1))
+00084 
+00088 #define getSDOsubIndex(byte3) (byte3)
+00089 
+00090 /***************************************************************************
+00091 **
+00092 */
+00093 void SDOTimeoutAlarm(CO_Data* d, UNS32 id)
+00094 {
+00095     MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0);
+00096     MSG_WAR(0x2A02, "server node : ", d->transfers[id].nodeId);
+00097     MSG_WAR(0x2A02, "      index : ", d->transfers[id].index);
+00098     MSG_WAR(0x2A02, "   subIndex : ", d->transfers[id].subIndex); 
+00099     /* Reset timer handler */
+00100     d->transfers[id].timer = TIMER_NONE;
+00101     /*Set aborted state*/
+00102     d->transfers[id].state = SDO_ABORTED_INTERNAL;
+00103     /* Sending a SDO abort */
+00104     sendSDOabort(d, d->transfers[id].whoami, 
+00105                  d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT);
+00106     d->transfers[id].abortCode = SDOABT_TIMED_OUT;
+00107     /* Call the user function to inform of the problem.*/
+00108     if(d->transfers[id].Callback)
+00109         /*If ther is a callback, it is responsible to close SDO transfer (client)*/
+00110         (*d->transfers[id].Callback)(d,d->transfers[id].nodeId);
+00111     else if(d->transfers[id].whoami == SDO_SERVER)
+00112         /*Else, if server, reset the line*/
+00113         resetSDOline(d, (UNS8)id);
+00114 }
+00115 
+00116 #define StopSDO_TIMER(id) \
+00117 MSG_WAR(0x3A05, "StopSDO_TIMER for line : ", line);\
+00118 d->transfers[id].timer = DelAlarm(d->transfers[id].timer);
+00119 
+00120 #define StartSDO_TIMER(id) \
+00121 MSG_WAR(0x3A06, "StartSDO_TIMER for line : ", line);\
+00122 d->transfers[id].timer = SetAlarm(d,id,&SDOTimeoutAlarm,MS_TO_TIMEVAL(SDO_TIMEOUT_MS),0);
+00123 
+00124 #define RestartSDO_TIMER(id) \
+00125 MSG_WAR(0x3A07, "restartSDO_TIMER for line : ", line);\
+00126 if(d->transfers[id].timer != TIMER_NONE) { StopSDO_TIMER(id) StartSDO_TIMER(id) }
+00127 
+00128 /***************************************************************************/
+00131 void resetSDO (CO_Data* d)
+00132 {
+00133   UNS8 j;
+00134 
+00135   /* transfer structure initialization */
+00136     for (j = 0 ; j < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; j++) 
+00137       resetSDOline(d, j);
+00138 }
+00139 
+00140 /***************************************************************************/
+00141 UNS32 SDOlineToObjdict (CO_Data* d, UNS8 line)
+00142 {
+00143   UNS8      size;
+00144   UNS32 errorCode;
+00145   MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line);
+00146   size = (UNS8)d->transfers[line].count;
+00147   errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, 
+00148                          (void *) d->transfers[line].data, &size, 1);
+00149   if (errorCode != OD_SUCCESSFUL)
+00150     return errorCode;
+00151   MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line);
+00152   return 0;
+00153 
+00154 }
+00155 
+00156 /***************************************************************************/
+00157 UNS32 objdictToSDOline (CO_Data* d, UNS8 line)
+00158 {
+00159   UNS8  size = 0;
+00160   UNS8  dataType;
+00161   UNS32 errorCode;
+00162 
+00163   MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index);
+00164   MSG_WAR(0x3A06, "  subIndex : ", d->transfers[line].subIndex);
+00165 
+00166   errorCode = getODentry(d,     d->transfers[line].index,
+00167                                 d->transfers[line].subIndex,
+00168                                 (void *)d->transfers[line].data,
+00169                                 &size, &dataType, 0);
+00170   
+00171   if (errorCode != OD_SUCCESSFUL)
+00172     return errorCode;
+00173 
+00174   d->transfers[line].count = size;
+00175   d->transfers[line].offset = 0;
+00176 #if 0
+00177    /*Me laisser ça, please ! (FD)*/
+00178   {
+00179     UNS8 i;
+00180     for (i = 0 ; i < 10 ; i++) {
+00181       MSG_WAR(i, "data= ", d->transfers[line].data[i]);
+00182     }     
+00183   }
+00184 #endif
+00185   return 0;
+00186 }
+00187 
+00188 /***************************************************************************/
+00189 UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data) {
+00190   UNS8 i;
+00191   UNS8 offset;
+00192 
+00193   if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
+00194     MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
+00195     return 0xFF;
+00196   }
+00197     if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) {
+00198     MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes);
+00199     return 0xFF;
+00200   }
+00201   offset = (UNS8)d->transfers[line].offset;
+00202   for (i = 0 ; i < nbBytes ; i++) 
+00203     * (data + i) = d->transfers[line].data[offset + i];
+00204   d->transfers[line].offset = d->transfers[line].offset + nbBytes;
+00205   return 0;
+00206 }
+00207 
+00208 
+00209 /***************************************************************************/
+00210 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data)
+00211 {
+00212   UNS8 i;
+00213   UNS8 offset;
+00214   
+00215   if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
+00216     MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
+00217     return 0xFF;
+00218   }
+00219   offset = (UNS8)d->transfers[line].offset;
+00220   for (i = 0 ; i < nbBytes ; i++) 
+00221     d->transfers[line].data[offset + i] = * (data + i);
+00222   d->transfers[line].offset = d->transfers[line].offset + nbBytes;
+00223   return 0;
+00224 }
+00225 
+00226 /***************************************************************************/
+00227 UNS8 failedSDO (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS16 index, 
+00228                 UNS8 subIndex, UNS32 abortCode)
+00229 {
+00230   UNS8 err;
+00231   UNS8 line;
+00232   err = getSDOlineOnUse( d, nodeId, whoami, &line );
+00233   if (!err) /* If a line on use have been found.*/
+00234     MSG_WAR(0x3A20, "FailedSDO : line found : ", line);
+00235   if ((! err) && (whoami == SDO_SERVER)) {
+00236     resetSDOline( d, line );
+00237     MSG_WAR(0x3A21, "FailedSDO : line released : ", line);
+00238   }
+00239   if ((! err) && (whoami == SDO_CLIENT)) {
+00240     StopSDO_TIMER(line);
+00241     d->transfers[line].state = SDO_ABORTED_INTERNAL;
+00242   }
+00243   MSG_WAR(0x3A22, "Sending SDO abort ", 0);
+00244   err = sendSDOabort(d, whoami, index, subIndex, abortCode);
+00245   if (err) {
+00246     MSG_WAR(0x3A23, "Unable to send the SDO abort", 0);
+00247     return 0xFF;
+00248   }
+00249   return 0;
+00250 }
+00251 
+00252 /***************************************************************************/
+00253 void resetSDOline ( CO_Data* d, UNS8 line )
+00254 {
+00255   UNS8 i; 
+00256   MSG_WAR(0x3A25, "reset SDO line nb : ", line); 
+00257   initSDOline(d, line, 0, 0, 0, SDO_RESET);
+00258   for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++)
+00259     d->transfers[line].data[i] = 0;
+00260 }
+00261 
+00262 /***************************************************************************/
+00263 UNS8 initSDOline (CO_Data* d, UNS8 line, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 state)
+00264 {
+00265   MSG_WAR(0x3A25, "init SDO line nb : ", line); 
+00266   if (state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS){
+00267         StartSDO_TIMER(line)
+00268   }else{
+00269         StopSDO_TIMER(line)
+00270   }
+00271   d->transfers[line].nodeId = nodeId; 
+00272   d->transfers[line].index = index;
+00273   d->transfers[line].subIndex = subIndex;
+00274   d->transfers[line].state = state;
+00275   d->transfers[line].toggle = 0;
+00276   d->transfers[line].count = 0;
+00277   d->transfers[line].offset = 0;
+00278   d->transfers[line].dataType = 0;
+00279   d->transfers[line].Callback = NULL;  
+00280   return 0;
+00281 }
+00282 
+00283 /***************************************************************************/
+00284 UNS8 getSDOfreeLine ( CO_Data* d, UNS8 whoami, UNS8 *line )
+00285 {
+00286         
+00287   UNS8 i;
+00288     
+00289   for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
+00290     if ( d->transfers[i].state == SDO_RESET ) {
+00291       *line = i;
+00292       d->transfers[i].whoami = whoami;
+00293       return 0;
+00294     } /* end if */
+00295   } /* end for */
+00296   MSG_ERR(0x1A25, "Too many SDO in progress. Aborted.", i);
+00297   return 0xFF;
+00298 }
+00299 
+00300 /***************************************************************************/
+00301 UNS8 getSDOlineOnUse (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS8 *line)
+00302 {
+00303         
+00304   UNS8 i;
+00305     
+00306   for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
+00307     if ( (d->transfers[i].state != SDO_RESET) &&
+00308          (d->transfers[i].nodeId == nodeId) && 
+00309          (d->transfers[i].whoami == whoami) ) {
+00310       *line = i;
+00311       return 0;
+00312     }
+00313   } 
+00314   return 0xFF;
+00315 }
+00316 
+00317 /***************************************************************************/
+00318 UNS8 closeSDOtransfer (CO_Data* d, UNS8 nodeId, UNS8 whoami)
+00319 {
+00320   UNS8 err;
+00321   UNS8 line;
+00322   err = getSDOlineOnUse(d, nodeId, whoami, &line);
+00323   if (err) {
+00324     MSG_WAR(0x2A30, "No SDO communication to close for node : ", nodeId); 
+00325     return 0xFF;
+00326   }
+00327   resetSDOline(d, line);  
+00328   return 0;
+00329 }
+00330 
+00331 /***************************************************************************/
+00332 UNS8 getSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 * nbBytes)
+00333 {
+00334   if (d->transfers[line].count == 0) /* if received initiate SDO protocol with e=0 and s=0 */
+00335     * nbBytes = 0;
+00336   else
+00337     * nbBytes = (UNS8)d->transfers[line].count - (UNS8)d->transfers[line].offset;
+00338   return 0;
+00339 }
+00340 
+00341 /***************************************************************************/
+00342 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 nbBytes)
+00343 {
+00344   if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) {
+00345     MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
+00346     return 0xFF;
+00347   }
+00348   d->transfers[line].count = nbBytes;
+00349   return 0;
+00350 }
+00351 
+00352 
+00353 /***************************************************************************/
+00354 UNS8 sendSDO (CO_Data* d, UNS8 whoami, s_SDO sdo)
+00355 {       
+00356   UNS16 offset;
+00357   UNS16 lastIndex;
+00358   UNS8 found = 0;
+00359   Message m;
+00360   UNS8 i;
+00361   UNS32 * pwCobId = NULL;
+00362   UNS32 * pwNodeId = NULL;
+00363 
+00364   MSG_WAR(0x3A38, "sendSDO",0);
+00365   if( !((d->nodeState == Operational) ||  (d->nodeState == Pre_operational ))) {
+00366     MSG_WAR(0x2A39, "unable to send the SDO (not in op or pre-op mode", d->nodeState);
+00367     return 0xFF;
+00368   }                             
+00369 
+00370   /*get the server->client cobid*/
+00371   if ( whoami == SDO_SERVER )   {/*case server. Easy because today only one server SDO is authorized in CanFestival*/
+00372     offset = d->firstIndex->SDO_SVR;
+00373     if (offset == 0) {
+00374       MSG_ERR(0x1A42, "SendSDO : No SDO server found", 0); 
+00375       return 0xFF;
+00376     }
+00377     pwCobId = d->objdict[offset].pSubindex[2].pObject;
+00378     MSG_WAR(0x3A41, "I am server. cobId : ", *pwCobId); 
+00379   }
+00380   else {                        /*case client*/
+00381     /* Get the client->server cobid.*/
+00382     UNS16 sdoNum = 0;
+00383     offset = d->firstIndex->SDO_CLT;
+00384     lastIndex = d->lastIndex->SDO_CLT;
+00385     if (offset == 0) {
+00386       MSG_ERR(0x1A42, "SendSDO : No SDO client index found", 0); 
+00387       return 0xFF;
+00388     }
+00389     /* First, have to find at the index where is defined the communication with the server node */
+00390     while (offset <= lastIndex){
+00391       MSG_WAR(0x3A43,"Reading index : ", 0x1280 + sdoNum);
+00392       if (d->objdict[offset].bSubCount <= 3) {
+00393         MSG_ERR(0x1A28, "Subindex 3  not found at index ", 0x1280 + sdoNum);
+00394         return 0xFF;
+00395       }
+00396       pwNodeId = d->objdict[offset].pSubindex[3].pObject;
+00397       MSG_WAR(0x3A44, "Found nodeId server = ", *pwNodeId);     
+00398       if(*pwNodeId == sdo.nodeId) {
+00399         found = 1;
+00400         break;          
+00401       }      
+00402       offset ++;
+00403       sdoNum ++;
+00404     }
+00405     if (! found){
+00406       MSG_WAR (0x2A45, "No SDO client corresponds to the mesage to send to node ", sdo.nodeId);
+00407       return 0xFF;
+00408     }
+00409     /* Second, read the cobid client->server */
+00410     pwCobId = d->objdict[offset].pSubindex[1].pObject;
+00411   }
+00412   /* message copy for sending */
+00413   m.cob_id.w = *pwCobId;
+00414   m.rtr = NOT_A_REQUEST; 
+00415   /* the length of SDO must be 8 */
+00416   m.len = 8;
+00417   for (i = 0 ; i < 8 ; i++) {
+00418     m.data[i] =  sdo.body.data[i];
+00419   }
+00420   return canSend(d->canHandle,&m);
+00421 }
+00422 
+00423 /***************************************************************************/
+00424 UNS8 sendSDOabort (CO_Data* d, UNS8 whoami, UNS16 index, UNS8 subIndex, UNS32 abortCode)
+00425 {
+00426   s_SDO sdo;
+00427   UNS8 ret;
+00428   MSG_WAR(0x2A50,"Sending SDO abort ", abortCode);
+00429   sdo.nodeId = *d->bDeviceNodeId;
+00430   sdo.body.data[0] = 0x80;
+00431   /* Index */
+00432   sdo.body.data[1] = index & 0xFF; /* LSB */
+00433   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
+00434   /* Subindex */
+00435   sdo.body.data[3] = subIndex;
+00436   /* Data */
+00437   sdo.body.data[4] = (UNS8)(abortCode & 0xFF);
+00438   sdo.body.data[5] = (UNS8)((abortCode >> 8) & 0xFF);
+00439   sdo.body.data[6] = (UNS8)((abortCode >> 16) & 0xFF);
+00440   sdo.body.data[7] = (UNS8)((abortCode >> 24) & 0xFF);
+00441   ret = sendSDO(d, whoami, sdo);
+00442 
+00443   return ret;
+00444 }
+00445 
+00446 /***************************************************************************/
+00447 UNS8 proceedSDO (CO_Data* d, Message *m)
+00448 {
+00449   UNS8 err;
+00450   UNS8 line;
+00451   UNS8 nbBytes; /* received or to be transmited. */
+00452   UNS8 nodeId = 0;  /* The node from which the SDO is received */
+00453   UNS32 nodeId_32; /* node id in 32 bits, for temporary use */
+00454   UNS32 *pNodeId = NULL;
+00455   UNS8 whoami = SDO_UNKNOWN;  /* SDO_SERVER or SDO_CLIENT.*/
+00456   UNS32 errorCode; /* while reading or writing in the local object dictionary.*/
+00457   s_SDO sdo;    /* SDO to transmit */
+00458   UNS16 index;
+00459   UNS8 subIndex;
+00460   UNS32 abortCode;
+00461   UNS8 i,j;
+00462   UNS32 *     pCobId = NULL;
+00463   UNS16 offset;
+00464   UNS16 lastIndex;
+00465 
+00466   MSG_WAR(0x3A60, "proceedSDO ", 0);
+00467   whoami = SDO_UNKNOWN;
+00468   /* Looking for the cobId in the object dictionary. */
+00469   /* Am-I a server ? */
+00470   offset = d->firstIndex->SDO_SVR;
+00471   lastIndex = d->lastIndex->SDO_SVR;
+00472   j = 0;
+00473   if(offset) while (offset <= lastIndex) {
+00474      if (d->objdict[offset].bSubCount <= 1) {
+00475           MSG_ERR(0x1A61, "Subindex 1  not found at index ", 0x1200 + j);
+00476           return 0xFF;
+00477         }
+00478       pCobId = d->objdict[offset].pSubindex[1].pObject;
+00479       if ( *pCobId == (*m).cob_id.w ) {
+00480         whoami = SDO_SERVER;
+00481         MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j);
+00482         /* In case of server, the node id of the client may be unknown. So we put the index minus offset */
+00483         /* 0x1200 where the cobid received is defined. */
+00484         nodeId = j;
+00485         break;
+00486       }
+00487       j++;
+00488       offset++;
+00489   } /* end while */
+00490   if (whoami == SDO_UNKNOWN) {
+00491     /* Am-I client ? */
+00492     offset = d->firstIndex->SDO_CLT;
+00493     lastIndex = d->lastIndex->SDO_CLT;
+00494     j = 0;
+00495     if(offset) while (offset <= lastIndex) {
+00496        if (d->objdict[offset].bSubCount <= 3) {
+00497          MSG_ERR(0x1A63, "Subindex 3  not found at index ", 0x1280 + j);
+00498          return 0xFF;
+00499        }
+00500        /* a) Looking for the cobid received. */
+00501        pCobId = d->objdict[offset].pSubindex[2].pObject;
+00502        if (*pCobId == (*m).cob_id.w ) {
+00503          /* b) cobid found, so reading the node id of the server. */
+00504          pNodeId = d->objdict[offset].pSubindex[3].pObject;
+00505          whoami = SDO_CLIENT;
+00506          nodeId_32 = *pNodeId;
+00507          nodeId = (UNS8)nodeId_32;
+00508          MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j);
+00509          MSG_WAR(0x3A65, "                 Server nodeId : ", nodeId);
+00510          break;
+00511         }
+00512        j++;
+00513        offset++;
+00514     } /* end while */
+00515   }
+00516   if (whoami == SDO_UNKNOWN) {
+00517     return 0xFF;/* This SDO was not for us ! */
+00518   }
+00519 
+00520   /* Test if the size of the SDO is ok */
+00521   if ( (*m).len != 8) {
+00522     MSG_ERR(0x1A67, "Error size SDO. CobId  : ", (*m).cob_id.w);
+00523     failedSDO(d, nodeId, whoami, 0, 0, SDOABT_GENERAL_ERROR);
+00524     return 0xFF;
+00525   }
+00526   
+00527   if (whoami == SDO_CLIENT) {
+00528     MSG_WAR(0x3A68, "I am CLIENT. Received SDO from nodeId : ", nodeId);
+00529   }
+00530   else {
+00531     MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", (*m).cob_id.w);
+00532   }
+00533     
+00534   /* Testing the command specifier */
+00535   /* Allowed : cs = 0, 1, 2, 3, 4. (=  all except those for block tranfert). */
+00536   /* cs = other : Not allowed -> abort. */
+00537   switch (getSDOcs(m->data[0])) {
+00538 
+00539   case 0:
+00540     /* I am SERVER */
+00541     if (whoami == SDO_SERVER) {
+00542       /* Receiving a download segment data. */
+00543       /* A SDO transfert should have been yet initiated. */
+00544       err = getSDOlineOnUse( d, nodeId, whoami, &line ); 
+00545       if (!err)
+00546         err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
+00547       if (err) {
+00548         MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", 
+00549                 nodeId); 
+00550         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
+00551         return 0xFF;
+00552       }
+00553       /* Reset the wathdog */
+00554       RestartSDO_TIMER(line)
+00555       MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", nodeId); 
+00556       index = d->transfers[line].index;
+00557       subIndex = d->transfers[line].subIndex;
+00558       /* Toggle test. */
+00559       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
+00560         MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0])); 
+00561         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
+00562         return 0xFF;
+00563       }
+00564       /* Nb of data to be downloaded */
+00565       nbBytes = 7 - getSDOn3(m->data[0]);
+00566       /* Store the data in the transfert structure. */
+00567       err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
+00568       if (err) {
+00569         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
+00570         return 0xFF;
+00571       }
+00572       /* Sending the SDO response, CS = 1 */
+00573       sdo.nodeId = *d->bDeviceNodeId; /* The node id of the server, (here it is the sender). */
+00574       sdo.body.data[0] = (1 << 5) | (d->transfers[line].toggle << 4);
+00575       for (i = 1 ; i < 8 ; i++)
+00576         sdo.body.data[i] = 0;
+00577       MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", nodeId); 
+00578       sendSDO(d, whoami, sdo);
+00579       /* Inverting the toggle for the next segment. */
+00580       d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
+00581       /* If it was the last segment, */
+00582       if (getSDOc(m->data[0])) {
+00583         /* Transfering line data to object dictionary. */
+00584         /* The code does not use the "d" of initiate frame. So it is safe if e=s=0 */
+00585         errorCode = SDOlineToObjdict(d, line);
+00586         if (errorCode) {
+00587           MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0); 
+00588           failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
+00589           return 0xFF;    
+00590         }
+00591         /* Release of the line */
+00592         resetSDOline(d, line);
+00593         MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", nodeId); 
+00594       }
+00595     } /* end if SERVER */
+00596     else { /* if CLIENT */
+00597       /* I am CLIENT */
+00598       /* It is a request for a previous upload segment. We should find a line opened for this.*/
+00599       err = getSDOlineOnUse( d, nodeId, whoami, &line);
+00600       if (!err)
+00601         err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
+00602       if (err) {
+00603         MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); 
+00604         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
+00605         return 0xFF;
+00606       }
+00607       /* Reset the wathdog */
+00608       RestartSDO_TIMER(line)
+00609       index = d->transfers[line].index;
+00610       subIndex = d->transfers[line].subIndex;
+00611       /* test of the toggle; */
+00612       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
+00613         MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId); 
+00614         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
+00615         return 0xFF;
+00616       }
+00617       /* nb of data to be uploaded */
+00618       nbBytes = 7 - getSDOn3(m->data[0]);
+00619       /* Storing the data in the line structure. */
+00620       err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
+00621       if (err) {
+00622         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
+00623         return 0xFF;
+00624       }
+00625       /* Inverting the toggle for the next segment. */
+00626       d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
+00627       /* If it was the last segment,*/
+00628       if ( getSDOc(m->data[0])) {
+00629         /* Put in state finished */
+00630         /* The code is safe for the case e=s=0 in initiate frame. */
+00631         StopSDO_TIMER(line)
+00632         d->transfers[line].state = SDO_FINISHED;
+00633         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
+00634         
+00635         MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId);
+00636       }
+00637       else { /* more segments to receive */
+00638              /* Sending the request for the next segment. */
+00639         sdo.nodeId = nodeId;
+00640         sdo.body.data[0] = (3 << 5) | (d->transfers[line].toggle << 4);
+00641         for (i = 1 ; i < 8 ; i++)
+00642           sdo.body.data[i] = 0;
+00643         sendSDO(d, whoami, sdo);
+00644         MSG_WAR(0x3A78, "SDO send upload segment request to nodeId", nodeId);
+00645       }            
+00646     } /* End if CLIENT */
+00647     break;
+00648 
+00649   case 1:
+00650     /* I am SERVER */
+00651     /* Receive of an initiate download */
+00652     if (whoami == SDO_SERVER) {
+00653       index = getSDOindex(m->data[1],m->data[2]);
+00654       subIndex = getSDOsubIndex(m->data[3]);
+00655       MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ", 
+00656               nodeId); 
+00657       MSG_WAR(0x3A80, "Writing at index : ", index);
+00658       MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex);
+00659       
+00660       /* Search if a SDO transfert have been yet initiated */
+00661       err = getSDOlineOnUse( d, nodeId, whoami, &line );
+00662       if (! err) {
+00663         MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0); 
+00664         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
+00665         return 0xFF;
+00666       }
+00667       /* No line on use. Great ! */
+00668       /* Try to open a new line. */
+00669       err = getSDOfreeLine( d, whoami, &line );
+00670       if (err) {
+00671         MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
+00672         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
+00673         return 0xFF;
+00674       }
+00675       initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);      
+00676 
+00677       if (getSDOe(m->data[0])) { /* If SDO expedited */
+00678         /* nb of data to be downloaded */
+00679         nbBytes = 4 - getSDOn2(m->data[0]);
+00680         /* Storing the data in the line structure. */
+00681         d->transfers[line].count = nbBytes;
+00682         err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
+00683         
+00684         if (err) {
+00685           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
+00686           return 0xFF;
+00687         }         
+00688 
+00689         /* SDO expedited -> transfert finished. Data can be stored in the dictionary. */
+00690         /* The line will be reseted when it is downloading in the dictionary. */
+00691         MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfert. Finished.: ", nodeId);
+00692         /* Transfering line data to object dictionary. */
+00693         errorCode = SDOlineToObjdict(d, line);
+00694         if (errorCode) {
+00695           MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0); 
+00696           failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
+00697           return 0xFF;
+00698         }
+00699         /* Release of the line. */
+00700         resetSDOline(d, line);
+00701       }
+00702       else {/* So, if it is not an expedited transfert */
+00703         if (getSDOs(m->data[0])) {
+00704           /* TODO : if e and s = 0, not reading m->data[4] but put nbBytes = 0 */
+00705           nbBytes = m->data[4]; /* Transfert limited to 255 bytes. */
+00706           err = setSDOlineRestBytes(d, nodeId, nbBytes);
+00707           if (err) {
+00708             failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
+00709             return 0xFF;
+00710           }     
+00711         }
+00712       }
+00713       /*Sending a SDO, cs=3*/
+00714       sdo.nodeId = *d->bDeviceNodeId; /* The node id of the server, (here it is the sender).*/
+00715       sdo.body.data[0] = 3 << 5;
+00716       sdo.body.data[1] = index & 0xFF;        /* LSB */
+00717       sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
+00718       sdo.body.data[3] = subIndex;
+00719       for (i = 4 ; i < 8 ; i++)
+00720                 sdo.body.data[i] = 0;
+00721       sendSDO(d, whoami, sdo);
+00722     } /* end if I am SERVER */
+00723     else {
+00724       /* I am CLIENT */
+00725       /* It is a response for a previous download segment. We should find a line opened for this. */
+00726       err = getSDOlineOnUse( d, nodeId, whoami, &line);
+00727       if (!err)
+00728         err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
+00729       if (err) {
+00730         MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); 
+00731         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
+00732         return 0xFF;
+00733       }
+00734       /* Reset the wathdog */
+00735       RestartSDO_TIMER(line)
+00736       index = d->transfers[line].index;
+00737       subIndex = d->transfers[line].subIndex;
+00738       /* test of the toggle; */
+00739       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
+00740         MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId); 
+00741         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
+00742         return 0xFF;
+00743       }
+00744 
+00745       /* End transmission or downloading next segment. We need to know if it will be the last one. */
+00746       getSDOlineRestBytes(d, line, &nbBytes);
+00747       if (nbBytes == 0) {
+00748         MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId); 
+00749         StopSDO_TIMER(line)
+00750         d->transfers[line].state = SDO_FINISHED;
+00751         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
+00752         return 0x00;
+00753       }
+00754       /* At least one transfer to send. */
+00755       if (nbBytes > 7) {
+00756         /* several segments to download.*/
+00757         /* code to send the next segment. (cs = 0; c = 0) */
+00758         d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
+00759         sdo.nodeId = nodeId; /* The server node Id; */
+00760         sdo.body.data[0] = (d->transfers[line].toggle << 4);
+00761         err = lineToSDO(d, line, 7, sdo.body.data + 1);  
+00762         if (err) {
+00763           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
+00764           return 0xFF;
+00765         }
+00766       } 
+00767       else {
+00768         /* Last segment. */
+00769         /* code to send the last segment. (cs = 0; c = 1)*/
+00770         d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
+00771         sdo.nodeId = nodeId; /* The server node Id; */
+00772         sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
+00773         err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);    
+00774         if (err) {
+00775           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
+00776           return 0xFF;
+00777         }
+00778         for (i = nbBytes + 1 ; i < 8 ; i++)
+00779           sdo.body.data[i] = 0;
+00780       }
+00781       MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId); 
+00782       sendSDO(d, whoami, sdo); 
+00783     } /* end if I am a CLIENT */                          
+00784     break;
+00785 
+00786   case 2:
+00787     /* I am SERVER */
+00788     /* Receive of an initiate upload.*/
+00789     if (whoami == SDO_SERVER) {
+00790       index = getSDOindex(m->data[1],m->data[2]);
+00791       subIndex = getSDOsubIndex(m->data[3]);
+00792       MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", 
+00793               nodeId); 
+00794       MSG_WAR(0x3A90, "Reading at index : ", index);
+00795       MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex);
+00796       /* Search if a SDO transfert have been yet initiated*/
+00797       err = getSDOlineOnUse( d, nodeId, whoami, &line );
+00798       if (! err) {
+00799             MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line); 
+00800         MSG_WAR(0x3A93, "nodeId = ", nodeId); 
+00801             failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
+00802             return 0xFF;
+00803       }
+00804       /* No line on use. Great !*/
+00805       /* Try to open a new line.*/
+00806       err = getSDOfreeLine( d, whoami, &line );
+00807       if (err) {
+00808         MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
+00809         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
+00810         return 0xFF;
+00811       }
+00812       initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
+00813       /* Transfer data from dictionary to the line structure. */
+00814       errorCode = objdictToSDOline(d, line);
+00815      
+00816       if (errorCode) {
+00817         MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ", 
+00818                 errorCode); 
+00819         failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
+00820         return 0xFF;
+00821         }
+00822       /* Preparing the response.*/
+00823       getSDOlineRestBytes(d, line, &nbBytes);   /* Nb bytes to transfer ? */
+00824       sdo.nodeId = nodeId; /* The server node Id; */
+00825       if (nbBytes > 4) {
+00826         /* normal transfert. (segmented). */
+00827         /* code to send the initiate upload response. (cs = 2) */
+00828         sdo.body.data[0] = (2 << 5) | 1;
+00829         sdo.body.data[1] = index & 0xFF;        /* LSB */
+00830         sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
+00831         sdo.body.data[3] = subIndex;
+00832         sdo.body.data[4] = nbBytes; /* Limitation of canfestival2 : Max tranfert is 256 bytes.*/
+00833         /* It takes too much memory to upgrate to 2^32 because the size of data is also coded */
+00834         /* in the object dictionary, at every index and subindex. */
+00835         for (i = 5 ; i < 8 ; i++)
+00836           sdo.body.data[i] = 0;
+00837         MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId); 
+00838         sendSDO(d, whoami, sdo); 
+00839       }
+00840       else {
+00841         /* Expedited upload. (cs = 2 ; e = 1) */
+00842         sdo.body.data[0] = (2 << 5) | ((4 - nbBytes) << 2) | 3;  
+00843         sdo.body.data[1] = index & 0xFF;        /* LSB */
+00844         sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
+00845         sdo.body.data[3] = subIndex;
+00846         err = lineToSDO(d, line, nbBytes, sdo.body.data + 4);    
+00847         if (err) {
+00848           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
+00849           return 0xFF;
+00850         }
+00851         for (i = 4 + nbBytes ; i < 8 ; i++)
+00852           sdo.body.data[i] = 0;
+00853         MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ", 
+00854                 nodeId); 
+00855         sendSDO(d, whoami, sdo); 
+00856         /* Release the line.*/
+00857         resetSDOline(d, line);
+00858       }
+00859     } /* end if I am SERVER*/
+00860     else {
+00861       /* I am CLIENT */
+00862       /* It is the response for the previous initiate upload request.*/
+00863       /* We should find a line opened for this. */
+00864       err = getSDOlineOnUse( d, nodeId, whoami, &line);
+00865       if (!err)
+00866         err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
+00867       if (err) {
+00868         MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId); 
+00869         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
+00870         return 0xFF;
+00871       }
+00872       /* Reset the wathdog */
+00873       RestartSDO_TIMER(line)
+00874       index = d->transfers[line].index;
+00875       subIndex = d->transfers[line].subIndex;
+00876       
+00877       if (getSDOe(m->data[0])) { /* If SDO expedited */
+00878         /* nb of data to be uploaded */
+00879           nbBytes = 4 - getSDOn2(m->data[0]);
+00880         /* Storing the data in the line structure. */
+00881         err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
+00882         if (err) {
+00883           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
+00884           return 0xFF;
+00885         }
+00886         /* SDO expedited -> transfert finished. data are available via  getReadResultNetworkDict(). */
+00887         MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId);
+00888         StopSDO_TIMER(line)
+00889         d->transfers[line].count = nbBytes;
+00890         d->transfers[line].state = SDO_FINISHED;
+00891         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
+00892         return 0;
+00893       }
+00894       else { /* So, if it is not an expedited transfert */
+00895         /* Storing the nb of data to receive. */
+00896         if (getSDOs(m->data[0])) {
+00897           nbBytes = m->data[4]; /* Remember the limitation to 255 bytes to transfert */
+00898           err = setSDOlineRestBytes(d, line, nbBytes);
+00899           if (err) {
+00900             failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
+00901             return 0xFF;
+00902           }     
+00903         }
+00904         /* Requesting next segment. (cs = 3) */
+00905         sdo.nodeId = nodeId;
+00906         sdo.body.data[0] = 3 << 5;
+00907         for (i = 1 ; i < 8 ; i++)
+00908           sdo.body.data[i] = 0;
+00909         MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId); 
+00910         sendSDO(d, whoami, sdo);  
+00911       }
+00912     } /* End if CLIENT */
+00913     break;
+00914 
+00915   case 3:
+00916     /* I am SERVER */
+00917     if (whoami == SDO_SERVER) {
+00918       /* Receiving a upload segment. */
+00919       /* A SDO transfert should have been yet initiated. */
+00920       err = getSDOlineOnUse( d, nodeId, whoami, &line ); 
+00921       if (!err)
+00922         err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
+00923       if (err) {
+00924         MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", 
+00925                 nodeId); 
+00926         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
+00927         return 0xFF;
+00928       }
+00929       /* Reset the wathdog */
+00930       RestartSDO_TIMER(line)
+00931       MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", nodeId); 
+00932       index = d->transfers[line].index;
+00933       subIndex = d->transfers[line].subIndex;
+00934       /* Toggle test.*/
+00935       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
+00936         MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0])); 
+00937         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
+00938         return 0xFF;
+00939       }
+00940       /* Uploading next segment. We need to know if it will be the last one. */
+00941       getSDOlineRestBytes(d, line, &nbBytes);             
+00942       if (nbBytes > 7) {
+00943         /* The segment to transfer is not the last one.*/
+00944         /* code to send the next segment. (cs = 0; c = 0) */
+00945         sdo.nodeId = nodeId; /* The server node Id; */
+00946         sdo.body.data[0] = (d->transfers[line].toggle << 4);
+00947         err = lineToSDO(d, line, 7, sdo.body.data + 1);  
+00948         if (err) {
+00949           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
+00950           return 0xFF;
+00951         }
+00952         /* Inverting the toggle for the next tranfert. */
+00953         d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
+00954         MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", nodeId); 
+00955         sendSDO(d, whoami, sdo); 
+00956       } 
+00957       else {
+00958         /* Last segment. */
+00959         /* code to send the last segment. (cs = 0; c = 1) */        
+00960         sdo.nodeId = nodeId; /* The server node Id; */
+00961         sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
+00962         err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);    
+00963         if (err) {
+00964           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
+00965           return 0xFF;
+00966         }
+00967         for (i = nbBytes + 1 ; i < 8 ; i++)
+00968           sdo.body.data[i] = 0;
+00969         MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", nodeId);      
+00970         sendSDO(d, whoami, sdo);
+00971         /* Release the line */
+00972         resetSDOline(d, line);
+00973       }
+00974     } /* end if SERVER*/
+00975     else {
+00976       /* I am CLIENT */
+00977       /* It is the response for the previous initiate download request. */
+00978       /* We should find a line opened for this. */
+00979       err = getSDOlineOnUse( d, nodeId, whoami, &line);
+00980       if (!err)
+00981         err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
+00982       if (err) {
+00983         MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId); 
+00984         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
+00985         return 0xFF;
+00986       }
+00987       /* Reset the watchdog */
+00988       RestartSDO_TIMER(line)
+00989       index = d->transfers[line].index;
+00990       subIndex = d->transfers[line].subIndex;
+00991       /* End transmission or requesting  next segment. */
+00992       getSDOlineRestBytes(d, line, &nbBytes);
+00993       if (nbBytes == 0) {
+00994         MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId); 
+00995         StopSDO_TIMER(line)
+00996         d->transfers[line].state = SDO_FINISHED;
+00997         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
+00998         return 0x00;
+00999       }   
+01000       if (nbBytes > 7) {
+01001         /* more than one request to send */
+01002         /* code to send the next segment. (cs = 0; c = 0)       */    
+01003         sdo.nodeId = nodeId; /* The server node Id; */
+01004         sdo.body.data[0] = (d->transfers[line].toggle << 4);
+01005         err = lineToSDO(d, line, 7, sdo.body.data + 1);  
+01006         if (err) {
+01007           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
+01008           return 0xFF;
+01009         }
+01010       } 
+01011       else {
+01012         /* Last segment.*/
+01013         /* code to send the last segment. (cs = 0; c = 1)       */   
+01014         sdo.nodeId = nodeId; /* The server node Id; */
+01015         sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
+01016         err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);    
+01017         if (err) {
+01018           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
+01019           return 0xFF;
+01020         }
+01021         for (i = nbBytes + 1 ; i < 8 ; i++)
+01022           sdo.body.data[i] = 0;
+01023       }
+01024       MSG_WAR(0x3AA7, "SDO sending download segment to nodeId", nodeId); 
+01025       sendSDO(d, whoami, sdo); 
+01026 
+01027     } /* end if I am a CLIENT           */        
+01028     break;  
+01029 
+01030    case 4:
+01031      abortCode = (*m).data[3] |
+01032       ((UNS32)m->data[5] << 8) |
+01033       ((UNS32)m->data[6] << 16) |
+01034       ((UNS32)m->data[7] << 24);
+01035     /* Received SDO abort. */
+01036     /* Looking for the line concerned. */
+01037     if (whoami == SDO_SERVER) {
+01038       err = getSDOlineOnUse( d, nodeId, whoami, &line );
+01039       if (!err) {
+01040         resetSDOline( d, line );
+01041         MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode);
+01042       }
+01043       else
+01044         MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode);
+01045       /* Tips : The end user has no way to know that the server node has received an abort SDO. */
+01046       /* Its is ok, I think.*/
+01047     }
+01048     else { /* If I am CLIENT */
+01049       err = getSDOlineOnUse( d, nodeId, whoami, &line );
+01050       if (!err) {
+01051         /* The line *must* be released by the core program. */
+01052         StopSDO_TIMER(line)
+01053         d->transfers[line].state = SDO_ABORTED_RCV;
+01054         d->transfers[line].abortCode = abortCode;
+01055         MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode);
+01056         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
+01057       }
+01058       else
+01059         MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode);
+01060     } 
+01061     break;
+01062   default:
+01063     /* Error : Unknown cs */
+01064     MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", getSDOcs(m->data[0]));
+01065     return 0xFF;
+01066 
+01067   } /* End switch */
+01068   return 0;     
+01069 }
+01070 
+01071 /*******************************************************************)******/
+01072 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, 
+01073                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize)
+01074 {
+01075   UNS8 err;
+01076   UNS8 SDOfound = 0;
+01077   UNS8 line;
+01078   s_SDO sdo;    /* SDO to transmit */
+01079   UNS8 i, j;
+01080   UNS16     lastIndex;
+01081   UNS16     offset;
+01082   UNS32      *pNodeIdServer;
+01083   UNS32      nodeIdServer;
+01084 
+01085   MSG_WAR(0x3AC0, "Send SDO to write in the dictionary of node : ", nodeId);
+01086   MSG_WAR(0x3AC1, "                                   At index : ", index);
+01087   MSG_WAR(0x3AC2, "                                   subIndex : ", subIndex);
+01088   MSG_WAR(0x3AC3, "                                   nb bytes : ", count);
+01089 
+01090   /* Verify that there is no SDO communication yet. */
+01091   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
+01092   if (!err) {
+01093     MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId); 
+01094     return 0xFF;
+01095   }
+01096   /* Taking the line ... */
+01097   err = getSDOfreeLine( d, SDO_CLIENT, &line );
+01098   if (err) {
+01099     MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); 
+01100     return (0xFF);
+01101   }
+01102   /* Check which SDO to use to communicate with the node */
+01103   offset = d->firstIndex->SDO_CLT;
+01104   lastIndex = d->lastIndex->SDO_CLT;
+01105   if (offset == 0) {
+01106     MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0); 
+01107     return 0xFF;
+01108   }
+01109   i = 0;
+01110    while (offset <= lastIndex) {
+01111      if (d->objdict[offset].bSubCount <= 3) {
+01112          MSG_ERR(0x1AC8, "Subindex 3  not found at index ", 0x1280 + i);
+01113          return 0xFF;
+01114      }
+01115      /* looking for the nodeId server */
+01116      pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
+01117      nodeIdServer = *pNodeIdServer;
+01118      MSG_WAR(0x1AD2, "index : ", 0x1280 + i);
+01119      MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer);
+01120    
+01121     if(nodeIdServer == (UNS32)nodeId) {
+01122       SDOfound = 1;
+01123       break;
+01124     }
+01125     offset++;
+01126     i++;
+01127   } /* end while */
+01128   if (!SDOfound) {
+01129     MSG_ERR(0x1AC9, "SDO. Error. No client found to communicate with node : ", nodeId);
+01130     return 0xFF;
+01131   }
+01132   MSG_WAR(0x3AD0,"        SDO client defined at index  : ", 0x1280 + i);
+01133   initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);
+01134   d->transfers[line].count = count;
+01135   d->transfers[line].dataType = dataType;
+01136   
+01137   /* Copy data to transfers structure. */
+01138   for (j = 0 ; j < count ; j++) {
+01139 # ifdef CANOPEN_BIG_ENDIAN
+01140     if (dataType == 0 && endianize)
+01141       d->transfers[line].data[count - 1 - j] = ((char *)data)[j];
+01142     else /* String of bytes. */
+01143       d->transfers[line].data[j] = ((char *)data)[j];
+01144 #  else 
+01145     d->transfers[line].data[j] = ((char *)data)[j];
+01146 #  endif
+01147   }
+01148   /* Send the SDO to the server. Initiate download, cs=1. */
+01149   sdo.nodeId = nodeId;
+01150   if (count <= 4) { /* Expedited transfert */
+01151     sdo.body.data[0] = (1 << 5) | ((4 - count) << 2) | 3;
+01152     for (i = 4 ; i < 8 ; i++)
+01153       sdo.body.data[i] = d->transfers[line].data[i - 4];
+01154     d->transfers[line].offset = count;
+01155   }     
+01156   else { /* Normal transfert */
+01157     sdo.body.data[0] = (1 << 5) | 1;
+01158     sdo.body.data[4] = count; /* nb of byte to transmit. Max = 255. (canfestival2 limitation). */
+01159     for (i = 5 ; i < 8 ; i++)
+01160       sdo.body.data[i] = 0;
+01161   }
+01162   sdo.body.data[1] = index & 0xFF;        /* LSB */
+01163   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
+01164   sdo.body.data[3] = subIndex;
+01165 
+01166   d->transfers[line].Callback = Callback;
+01167     
+01168   err = sendSDO(d, SDO_CLIENT, sdo);
+01169   if (err) {
+01170     MSG_ERR(0x1AD1, "SDO. Error while sending SDO to node : ", nodeId);
+01171     /* release the line */
+01172     resetSDOline(d, line);
+01173     return 0xFF;
+01174   }
+01175 
+01176   
+01177   return 0;
+01178 }
+01179 
+01180 /*--------------------------------------------------------------------------*/
+01181 
+01182 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, 
+01183                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data)
+01184 {
+01185         return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, NULL, 1);
+01186 }
+01187 
+01188 /*--------------------------------------------------------------------------*/
+01189 
+01190 UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index, 
+01191                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback)
+01192 {
+01193         return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1);      
+01194 }
+01195 
+01196 
+01197 /***************************************************************************/
+01198 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
+01199 {
+01200   UNS8 err;
+01201   UNS8 SDOfound = 0;
+01202   UNS8 i;
+01203   UNS8 line;
+01204   s_SDO sdo;    /* SDO to transmit */
+01205   UNS32      *pNodeIdServer;
+01206   UNS32      nodeIdServer;
+01207   UNS16     offset;
+01208   UNS16     lastIndex;
+01209   MSG_WAR(0x3AD5, "Send SDO to read in the dictionary of node : ", nodeId);
+01210   MSG_WAR(0x3AD6, "                                  At index : ", index);
+01211   MSG_WAR(0x3AD7, "                                  subIndex : ", subIndex);
+01212 
+01213 
+01214   /* Verify that there is no SDO communication yet. */
+01215   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
+01216   if (!err) {
+01217     MSG_ERR(0x1AD8, "SDO error : Communication yet established. with node : ", nodeId); 
+01218     return 0xFF;
+01219   }
+01220   /* Taking the line ... */
+01221   err = getSDOfreeLine( d, SDO_CLIENT, &line );
+01222   if (err) {
+01223     MSG_ERR(0x1AD9, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); 
+01224     return (0xFF);
+01225   }
+01226   else
+01227     MSG_WAR(0x3AE0, "Transmission on line : ", line);
+01228 
+01229   /* Check which SDO to use to communicate with the node */
+01230   offset = d->firstIndex->SDO_CLT;
+01231   lastIndex = d->lastIndex->SDO_CLT;
+01232   if (offset == 0) {
+01233     MSG_ERR(0x1AE1, "writeNetworkDict : No SDO client index found", 0); 
+01234     return 0xFF;
+01235   }
+01236   i = 0;
+01237   while (offset <= lastIndex) {
+01238      if (d->objdict[offset].bSubCount <= 3) {
+01239          MSG_ERR(0x1AE2, "Subindex 3  not found at index ", 0x1280 + i);
+01240          return 0xFF;
+01241      }
+01242      /* looking for the nodeId server */
+01243      pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
+01244      nodeIdServer = *pNodeIdServer;
+01245    
+01246     if(nodeIdServer == (UNS32)nodeId) {
+01247       SDOfound = 1;
+01248       break;
+01249     }
+01250     offset++;
+01251     i++;
+01252   } /* end while */
+01253   if (!SDOfound) {
+01254     MSG_ERR(0x1AE3, "SDO. Error. No client found to communicate with node : ", nodeId);
+01255     return 0xFF;
+01256   }
+01257   MSG_WAR(0x3AE4,"        SDO client defined at index  : ", 0x1280 + i);
+01258   initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
+01259   getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
+01260   sdo.nodeId = nodeId;
+01261   /* Send the SDO to the server. Initiate upload, cs=2. */
+01262   d->transfers[line].dataType = dataType;                               
+01263   sdo.body.data[0] = (2 << 5);  
+01264   sdo.body.data[1] = index & 0xFF;        /* LSB */
+01265   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
+01266   sdo.body.data[3] = subIndex;
+01267   for (i = 4 ; i < 8 ; i++)
+01268     sdo.body.data[i] = 0;
+01269   d->transfers[line].Callback = Callback;
+01270   err = sendSDO(d, SDO_CLIENT, sdo);
+01271   if (err) {
+01272     MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId);
+01273     /* release the line */
+01274     resetSDOline(d, line);
+01275     return 0xFF;
+01276   }             
+01277   return 0;
+01278 }
+01279 
+01280 /*--------------------------------------------------------------------------*/
+01281 
+01282 UNS8 readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType)
+01283 {
+01284         return _readNetworkDict (d, nodeId, index, subIndex, dataType, NULL);
+01285 }
+01286 
+01287 /*--------------------------------------------------------------------------*/
+01288 UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
+01289 {
+01290         return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback);
+01291 }
+01292 
+01293 /***************************************************************************/
+01294 
+01295 UNS8 getReadResultNetworkDict (CO_Data* d, UNS8 nodeId, void* data, UNS8 *size, 
+01296                                UNS32 * abortCode)
+01297 {
+01298   UNS8 i;
+01299   UNS8 err;
+01300   UNS8 line;
+01301   * size = 0;
+01302 
+01303   /* Looking for the line tranfert. */
+01304   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
+01305   if (err) {
+01306     MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId); 
+01307     return SDO_ABORTED_INTERNAL;
+01308   }
+01309   if (d->transfers[line].state != SDO_FINISHED)
+01310     return d->transfers[line].state;
+01311 
+01312   /* Transfert is finished. Put the value in the data. */
+01313   * size = (UNS8)d->transfers[line].count;
+01314   for  ( i = 0 ; i < *size ; i++) {
+01315 # ifdef CANOPEN_BIG_ENDIAN
+01316     if (d->transfers[line].dataType != visible_string)
+01317       ( (char *) data)[*size - 1 - i] = d->transfers[line].data[i];
+01318     else /* String of bytes. */
+01319       ( (char *) data)[i] = d->transfers[line].data[i];
+01320 # else 
+01321     ( (char *) data)[i] = d->transfers[line].data[i];
+01322 # endif
+01323   } 
+01324   return SDO_FINISHED;
+01325 }
+01326 
+01327 /***************************************************************************/
+01328 
+01329 UNS8 getWriteResultNetworkDict (CO_Data* d, UNS8 nodeId, UNS32 * abortCode)
+01330 {
+01331   UNS8 line = 0;
+01332   UNS8 err;
+01333 
+01334   * abortCode = 0;
+01335   /* Looking for the line tranfert. */
+01336   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
+01337   if (err) {
+01338     MSG_ERR(0x1AF1, "SDO error : No line found for communication with node : ", nodeId); 
+01339     return SDO_ABORTED_INTERNAL;
+01340   }
+01341   * abortCode = d->transfers[line].abortCode;
+01342   return d->transfers[line].state;
+01343 }
+