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