CanFestival: src/sdo.c Source File

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