sdo.c

Go to the documentation of this 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 }

Generated on Mon Jul 2 19:10:16 2007 for CanFestival by  doxygen 1.5.1