nico@207: nico@207: nico@207: CanFestival: /home/epimerde/documents/tc11/CanFestival-3/src/sdo.c Source File nico@207: nico@207: nico@207: nico@207: nico@207:
nico@207:
nico@207:
nico@207:
nico@207:

/home/epimerde/documents/tc11/CanFestival-3/src/sdo.c

Go to the documentation of this 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@210: 00035 #include "objacces.h"
nico@210: 00036 #include "sdo.h"
nico@210: 00037 #include "canfestival.h"
nico@210: 00038 
nico@210: 00039 /* Uncomment if your compiler does not support inline functions */
nico@210: 00040 #define NO_INLINE 
nico@210: 00041 
nico@210: 00042 #ifdef NO_INLINE
nico@210: 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 
nico@210: 00065 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, 
nico@210: 00066                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize);
nico@210: 00067 
nico@210: 00080 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, 
nico@210: 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 
nico@210: 00089 #define getSDOcs(byte) (byte >> 5)
nico@210: 00090 
nico@210: 00093 #define getSDOn2(byte) ((byte >> 2) & 3)
nico@210: 00094 
nico@210: 00097 #define getSDOn3(byte) ((byte >> 1) & 7)
nico@210: 00098 
nico@210: 00101 #define getSDOe(byte) ((byte >> 1) & 1)
nico@210: 00102 
nico@210: 00105 #define getSDOs(byte) (byte & 1)
nico@210: 00106 
nico@210: 00109 #define getSDOc(byte) (byte & 1)
nico@210: 00110 
nico@210: 00113 #define getSDOt(byte) ((byte >> 4) & 1)
nico@210: 00114 
nico@210: 00117 #define getSDOindex(byte1, byte2) ((byte2 << 8) | (byte1))
nico@210: 00118 
nico@210: 00121 #define getSDOsubIndex(byte3) (byte3)
nico@210: 00122 
nico@210: 00129 void SDOTimeoutAlarm(CO_Data* d, UNS32 id)
nico@210: 00130 {
nico@210: 00131     MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0);
nico@210: 00132     MSG_WAR(0x2A02, "server node : ", d->transfers[id].nodeId);
nico@210: 00133     MSG_WAR(0x2A02, "      index : ", d->transfers[id].index);
nico@210: 00134     MSG_WAR(0x2A02, "   subIndex : ", d->transfers[id].subIndex); 
nico@210: 00135     /* Reset timer handler */
nico@210: 00136     d->transfers[id].timer = TIMER_NONE;
nico@210: 00137     /*Set aborted state*/
nico@210: 00138     d->transfers[id].state = SDO_ABORTED_INTERNAL;
nico@210: 00139     /* Sending a SDO abort */
nico@210: 00140     sendSDOabort(d, d->transfers[id].whoami, 
nico@210: 00141                  d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT);
nico@210: 00142     d->transfers[id].abortCode = SDOABT_TIMED_OUT;
nico@210: 00143     /* Call the user function to inform of the problem.*/
nico@210: 00144     if(d->transfers[id].Callback)
nico@210: 00145         /*If ther is a callback, it is responsible to close SDO transfer (client)*/
nico@210: 00146         (*d->transfers[id].Callback)(d,d->transfers[id].nodeId);
nico@210: 00147     else if(d->transfers[id].whoami == SDO_SERVER)
nico@210: 00148         /*Else, if server, reset the line*/
nico@210: 00149         resetSDOline(d, (UNS8)id);
nico@210: 00150 }
nico@210: 00151 
nico@210: 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 
nico@210: 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 
nico@210: 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 
nico@210: 00169 void resetSDO (CO_Data* d)
nico@210: 00170 {
nico@210: 00171   UNS8 j;
nico@210: 00172 
nico@210: 00174     for (j = 0 ; j < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; j++) 
nico@210: 00175       resetSDOline(d, j);
nico@210: 00176 }
nico@210: 00177 
nico@210: 00186 UNS32 SDOlineToObjdict (CO_Data* d, UNS8 line)
nico@210: 00187 {
nico@210: 00188   UNS8      size;
nico@210: 00189   UNS32 errorCode;
nico@210: 00190   MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line);
nico@210: 00191   size = (UNS8)d->transfers[line].count;
nico@210: 00192   errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, 
nico@210: 00193                          (void *) d->transfers[line].data, &size, 1);
nico@210: 00194   if (errorCode != OD_SUCCESSFUL)
nico@210: 00195     return errorCode;
nico@210: 00196   MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line);
nico@210: 00197   return 0;
nico@210: 00198 
nico@210: 00199 }
nico@210: 00200 
nico@210: 00209 UNS32 objdictToSDOline (CO_Data* d, UNS8 line)
nico@210: 00210 {
nico@210: 00211   UNS8  size = 0;
nico@210: 00212   UNS8  dataType;
nico@210: 00213   UNS32 errorCode;
nico@210: 00214 
nico@210: 00215   MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index);
nico@210: 00216   MSG_WAR(0x3A06, "  subIndex : ", d->transfers[line].subIndex);
nico@210: 00217 
nico@210: 00218   errorCode = getODentry(d,     d->transfers[line].index,
nico@210: 00219                                 d->transfers[line].subIndex,
nico@210: 00220                                 (void *)d->transfers[line].data,
nico@210: 00221                                 &size, &dataType, 0);
nico@210: 00222   
nico@210: 00223   if (errorCode != OD_SUCCESSFUL)
nico@210: 00224     return errorCode;
nico@207: 00225 
nico@210: 00226   d->transfers[line].count = size;
nico@210: 00227   d->transfers[line].offset = 0;
nico@210: 00228 #if 0
nico@210: 00229    /*Me laisser a, please ! (FD)*/
nico@210: 00230   {
nico@210: 00231     UNS8 i;
nico@210: 00232     for (i = 0 ; i < 10 ; i++) {
nico@210: 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 
nico@210: 00250 UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data) {
nico@210: 00251   UNS8 i;
nico@210: 00252   UNS8 offset;
nico@210: 00253 
nico@210: 00254   if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
nico@210: 00255     MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
nico@210: 00256     return 0xFF;
nico@210: 00257   }
nico@210: 00258     if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) {
nico@210: 00259     MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes);
nico@210: 00260     return 0xFF;
nico@210: 00261   }
nico@210: 00262   offset = (UNS8)d->transfers[line].offset;
nico@210: 00263   for (i = 0 ; i < nbBytes ; i++) 
nico@210: 00264     * (data + i) = d->transfers[line].data[offset + i];
nico@210: 00265   d->transfers[line].offset = d->transfers[line].offset + nbBytes;
nico@210: 00266   return 0;
nico@210: 00267 }
nico@210: 00268 
nico@210: 00279 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data)
nico@210: 00280 {
nico@210: 00281   UNS8 i;
nico@210: 00282   UNS8 offset;
nico@210: 00283   
nico@210: 00284   if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
nico@210: 00285     MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
nico@210: 00286     return 0xFF;
nico@210: 00287   }
nico@210: 00288   offset = (UNS8)d->transfers[line].offset;
nico@210: 00289   for (i = 0 ; i < nbBytes ; i++) 
nico@210: 00290     d->transfers[line].data[offset + i] = * (data + i);
nico@210: 00291   d->transfers[line].offset = d->transfers[line].offset + nbBytes;
nico@210: 00292   return 0;
nico@210: 00293 }
nico@210: 00294 
nico@210: 00307 UNS8 failedSDO (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS16 index, 
nico@210: 00308                 UNS8 subIndex, UNS32 abortCode)
nico@210: 00309 {
nico@210: 00310   UNS8 err;
nico@210: 00311   UNS8 line;
nico@210: 00312   err = getSDOlineOnUse( d, nodeId, whoami, &line );
nico@210: 00313   if (!err) /* If a line on use have been found.*/
nico@210: 00314     MSG_WAR(0x3A20, "FailedSDO : line found : ", line);
nico@210: 00315   if ((! err) && (whoami == SDO_SERVER)) {
nico@210: 00316     resetSDOline( d, line );
nico@210: 00317     MSG_WAR(0x3A21, "FailedSDO : line released : ", line);
nico@210: 00318   }
nico@210: 00319   if ((! err) && (whoami == SDO_CLIENT)) {
nico@210: 00320     StopSDO_TIMER(line);
nico@210: 00321     d->transfers[line].state = SDO_ABORTED_INTERNAL;
nico@210: 00322   }
nico@210: 00323   MSG_WAR(0x3A22, "Sending SDO abort ", 0);
nico@210: 00324   err = sendSDOabort(d, whoami, index, subIndex, abortCode);
nico@210: 00325   if (err) {
nico@210: 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 
nico@210: 00338 void resetSDOline ( CO_Data* d, UNS8 line )
nico@210: 00339 {
nico@210: 00340   UNS8 i; 
nico@210: 00341   MSG_WAR(0x3A25, "reset SDO line nb : ", line); 
nico@210: 00342   initSDOline(d, line, 0, 0, 0, SDO_RESET);
nico@210: 00343   for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++)
nico@210: 00344     d->transfers[line].data[i] = 0;
nico@210: 00345 }
nico@210: 00346 
nico@210: 00359 UNS8 initSDOline (CO_Data* d, UNS8 line, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 state)
nico@210: 00360 {
nico@210: 00361   MSG_WAR(0x3A25, "init SDO line nb : ", line); 
nico@210: 00362   if (state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS){
nico@210: 00363         StartSDO_TIMER(line)
nico@210: 00364   }else{
nico@210: 00365         StopSDO_TIMER(line)
nico@210: 00366   }
nico@210: 00367   d->transfers[line].nodeId = nodeId; 
nico@210: 00368   d->transfers[line].index = index;
nico@210: 00369   d->transfers[line].subIndex = subIndex;
nico@210: 00370   d->transfers[line].state = state;
nico@210: 00371   d->transfers[line].toggle = 0;
nico@210: 00372   d->transfers[line].count = 0;
nico@210: 00373   d->transfers[line].offset = 0;
nico@210: 00374   d->transfers[line].dataType = 0;
nico@210: 00375   d->transfers[line].Callback = NULL;  
nico@210: 00376   return 0;
nico@210: 00377 }
nico@210: 00378 
nico@210: 00388 UNS8 getSDOfreeLine ( CO_Data* d, UNS8 whoami, UNS8 *line )
nico@210: 00389 {
nico@210: 00390         
nico@210: 00391   UNS8 i;
nico@210: 00392     
nico@210: 00393   for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
nico@210: 00394     if ( d->transfers[i].state == SDO_RESET ) {
nico@210: 00395       *line = i;
nico@210: 00396       d->transfers[i].whoami = whoami;
nico@210: 00397       return 0;
nico@210: 00398     } /* end if */
nico@210: 00399   } /* end for */
nico@210: 00400   MSG_ERR(0x1A25, "Too many SDO in progress. Aborted.", i);
nico@210: 00401   return 0xFF;
nico@210: 00402 }
nico@210: 00403 
nico@210: 00414 UNS8 getSDOlineOnUse (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS8 *line)
nico@210: 00415 {
nico@210: 00416         
nico@210: 00417   UNS8 i;
nico@210: 00418     
nico@210: 00419   for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
nico@210: 00420     if ( (d->transfers[i].state != SDO_RESET) &&
nico@210: 00421          (d->transfers[i].nodeId == nodeId) && 
nico@210: 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 
nico@210: 00439 UNS8 closeSDOtransfer (CO_Data* d, UNS8 nodeId, UNS8 whoami)
nico@210: 00440 {
nico@210: 00441   UNS8 err;
nico@210: 00442   UNS8 line;
nico@210: 00443   err = getSDOlineOnUse(d, nodeId, whoami, &line);
nico@210: 00444   if (err) {
nico@210: 00445     MSG_WAR(0x2A30, "No SDO communication to close for node : ", nodeId); 
nico@210: 00446     return 0xFF;
nico@210: 00447   }
nico@210: 00448   resetSDOline(d, line);  
nico@210: 00449   return 0;
nico@210: 00450 }
nico@210: 00451 
nico@210: 00461 UNS8 getSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 * nbBytes)
nico@210: 00462 {
nico@210: 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
nico@210: 00466     * nbBytes = (UNS8)d->transfers[line].count - (UNS8)d->transfers[line].offset;
nico@210: 00467   return 0;
nico@210: 00468 }
nico@210: 00469 
nico@210: 00479 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 nbBytes)
nico@210: 00480 {
nico@210: 00481   if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) {
nico@210: 00482     MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
nico@210: 00483     return 0xFF;
nico@210: 00484   }
nico@210: 00485   d->transfers[line].count = nbBytes;
nico@210: 00486   return 0;
nico@210: 00487 }
nico@210: 00488 
nico@210: 00498 UNS8 sendSDO (CO_Data* d, UNS8 whoami, s_SDO sdo)
nico@210: 00499 {       
nico@210: 00500   UNS16 offset;
nico@210: 00501   UNS16 lastIndex;
nico@210: 00502   UNS8 found = 0;
nico@210: 00503   Message m;
nico@210: 00504   UNS8 i;
nico@210: 00505   UNS32 * pwCobId = NULL;
nico@210: 00506   UNS32 * pwNodeId = NULL;
nico@210: 00507 
nico@210: 00508   MSG_WAR(0x3A38, "sendSDO",0);
nico@210: 00509   if( !((d->nodeState == Operational) ||  (d->nodeState == Pre_operational ))) {
nico@210: 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@210: 00515   if ( whoami == SDO_SERVER )   {
nico@210: 00516     offset = d->firstIndex->SDO_SVR;
nico@210: 00517     if (offset == 0) {
nico@210: 00518       MSG_ERR(0x1A42, "SendSDO : No SDO server found", 0); 
nico@210: 00519       return 0xFF;
nico@210: 00520     }
nico@210: 00521     pwCobId = d->objdict[offset].pSubindex[2].pObject;
nico@210: 00522     MSG_WAR(0x3A41, "I am server. cobId : ", *pwCobId); 
nico@210: 00523   }
nico@210: 00524   else {                        
nico@210: 00526     UNS16 sdoNum = 0;
nico@210: 00527     offset = d->firstIndex->SDO_CLT;
nico@210: 00528     lastIndex = d->lastIndex->SDO_CLT;
nico@210: 00529     if (offset == 0) {
nico@210: 00530       MSG_ERR(0x1A42, "SendSDO : No SDO client index found", 0); 
nico@210: 00531       return 0xFF;
nico@210: 00532     }
nico@210: 00534     while (offset <= lastIndex){
nico@210: 00535       MSG_WAR(0x3A43,"Reading index : ", 0x1280 + sdoNum);
nico@210: 00536       if (d->objdict[offset].bSubCount <= 3) {
nico@210: 00537         MSG_ERR(0x1A28, "Subindex 3  not found at index ", 0x1280 + sdoNum);
nico@210: 00538         return 0xFF;
nico@210: 00539       }
nico@210: 00540       pwNodeId = d->objdict[offset].pSubindex[3].pObject;
nico@210: 00541       MSG_WAR(0x3A44, "Found nodeId server = ", *pwNodeId);     
nico@210: 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){
nico@210: 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@210: 00554     pwCobId = d->objdict[offset].pSubindex[1].pObject;
nico@210: 00555   }
nico@210: 00557   m.cob_id.w = *pwCobId;
nico@210: 00558   m.rtr = NOT_A_REQUEST; 
nico@210: 00560   m.len = 8;
nico@210: 00561   for (i = 0 ; i < 8 ; i++) {
nico@210: 00562     m.data[i] =  sdo.body.data[i];
nico@210: 00563   }
nico@210: 00564   return canSend(d->canHandle,&m);
nico@210: 00565 }
nico@210: 00566 
nico@210: 00578 UNS8 sendSDOabort (CO_Data* d, UNS8 whoami, UNS16 index, UNS8 subIndex, UNS32 abortCode)
nico@210: 00579 {
nico@210: 00580   s_SDO sdo;
nico@210: 00581   UNS8 ret;
nico@210: 00582   MSG_WAR(0x2A50,"Sending SDO abort ", abortCode);
nico@210: 00583   sdo.nodeId = *d->bDeviceNodeId;
nico@210: 00584   sdo.body.data[0] = 0x80;
nico@210: 00585   /* Index */
nico@210: 00586   sdo.body.data[1] = index & 0xFF; /* LSB */
nico@210: 00587   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
nico@210: 00588   /* Subindex */
nico@210: 00589   sdo.body.data[3] = subIndex;
nico@210: 00590   /* Data */
nico@210: 00591   sdo.body.data[4] = (UNS8)(abortCode & 0xFF);
nico@210: 00592   sdo.body.data[5] = (UNS8)((abortCode >> 8) & 0xFF);
nico@210: 00593   sdo.body.data[6] = (UNS8)((abortCode >> 16) & 0xFF);
nico@210: 00594   sdo.body.data[7] = (UNS8)((abortCode >> 24) & 0xFF);
nico@210: 00595   ret = sendSDO(d, whoami, sdo);
nico@210: 00596 
nico@210: 00597   return ret;
nico@210: 00598 }
nico@210: 00599 
nico@210: 00608 UNS8 proceedSDO (CO_Data* d, Message *m)
nico@210: 00609 {
nico@210: 00610   UNS8 err;
nico@210: 00611   UNS8 line;
nico@210: 00612   UNS8 nbBytes; 
nico@210: 00613   UNS8 nodeId = 0;  
nico@210: 00614   UNS32 nodeId_32; 
nico@210: 00615   UNS32 *pNodeId = NULL;
nico@210: 00616   UNS8 whoami = SDO_UNKNOWN;  
nico@210: 00617   UNS32 errorCode; 
nico@210: 00618   s_SDO sdo;    
nico@210: 00619   UNS16 index;
nico@210: 00620   UNS8 subIndex;
nico@210: 00621   UNS32 abortCode;
nico@210: 00622   UNS8 i,j;
nico@210: 00623   UNS32 *     pCobId = NULL;
nico@210: 00624   UNS16 offset;
nico@210: 00625   UNS16 lastIndex;
nico@210: 00626 
nico@210: 00627   MSG_WAR(0x3A60, "proceedSDO ", 0);
nico@210: 00628   whoami = SDO_UNKNOWN;
nico@210: 00631   offset = d->firstIndex->SDO_SVR;
nico@210: 00632   lastIndex = d->lastIndex->SDO_SVR;
nico@210: 00633   j = 0;
nico@210: 00634   if(offset) while (offset <= lastIndex) {
nico@210: 00635      if (d->objdict[offset].bSubCount <= 1) {
nico@210: 00636           MSG_ERR(0x1A61, "Subindex 1  not found at index ", 0x1200 + j);
nico@210: 00637           return 0xFF;
nico@210: 00638         }
nico@210: 00639       pCobId = d->objdict[offset].pSubindex[1].pObject;
nico@210: 00640       if ( *pCobId == (*m).cob_id.w ) {
nico@210: 00641         whoami = SDO_SERVER;
nico@210: 00642         MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j);
nico@210: 00645         nodeId = j;
nico@210: 00646         break;
nico@210: 00647       }
nico@210: 00648       j++;
nico@210: 00649       offset++;
nico@210: 00650   } 
nico@210: 00651   if (whoami == SDO_UNKNOWN) {
nico@210: 00653     offset = d->firstIndex->SDO_CLT;
nico@210: 00654     lastIndex = d->lastIndex->SDO_CLT;
nico@210: 00655     j = 0;
nico@210: 00656     if(offset) while (offset <= lastIndex) {
nico@210: 00657        if (d->objdict[offset].bSubCount <= 3) {
nico@210: 00658          MSG_ERR(0x1A63, "Subindex 3  not found at index ", 0x1280 + j);
nico@210: 00659          return 0xFF;
nico@210: 00660        }
nico@210: 00662        pCobId = d->objdict[offset].pSubindex[2].pObject;
nico@210: 00663        if (*pCobId == (*m).cob_id.w ) {
nico@210: 00665          pNodeId = d->objdict[offset].pSubindex[3].pObject;
nico@210: 00666          whoami = SDO_CLIENT;
nico@210: 00667          nodeId_32 = *pNodeId;
nico@210: 00668          nodeId = (UNS8)nodeId_32;
nico@210: 00669          MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j);
nico@210: 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   }
nico@210: 00677   if (whoami == SDO_UNKNOWN) {
nico@210: 00678     return 0xFF;
nico@210: 00679   }
nico@210: 00680 
nico@210: 00682   if ( (*m).len != 8) {
nico@210: 00683     MSG_ERR(0x1A67, "Error size SDO. CobId  : ", (*m).cob_id.w);
nico@210: 00684     failedSDO(d, nodeId, whoami, 0, 0, SDOABT_GENERAL_ERROR);
nico@210: 00685     return 0xFF;
nico@210: 00686   }
nico@210: 00687   
nico@210: 00688   if (whoami == SDO_CLIENT) {
nico@210: 00689     MSG_WAR(0x3A68, "I am CLIENT. Received SDO from nodeId : ", nodeId);
nico@210: 00690   }
nico@210: 00691   else {
nico@210: 00692     MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", (*m).cob_id.w);
nico@210: 00693   }
nico@210: 00694     
nico@210: 00698   switch (getSDOcs(m->data[0])) {
nico@210: 00699 
nico@210: 00700   case 0:
nico@210: 00702     if (whoami == SDO_SERVER) {
nico@210: 00705       err = getSDOlineOnUse( d, nodeId, whoami, &line ); 
nico@210: 00706       if (!err)
nico@210: 00707         err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
nico@210: 00708       if (err) {
nico@210: 00709         MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", 
nico@210: 00710                 nodeId); 
nico@210: 00711         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
nico@210: 00712         return 0xFF;
nico@210: 00713       }
nico@210: 00715       RestartSDO_TIMER(line)
nico@210: 00716       MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", nodeId); 
nico@210: 00717       index = d->transfers[line].index;
nico@210: 00718       subIndex = d->transfers[line].subIndex;
nico@210: 00720       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
nico@210: 00721         MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0])); 
nico@210: 00722         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
nico@210: 00723         return 0xFF;
nico@210: 00724       }
nico@210: 00726       nbBytes = 7 - getSDOn3(m->data[0]);
nico@210: 00728       err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
nico@207: 00729       if (err) {
nico@210: 00730         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
nico@210: 00731         return 0xFF;
nico@210: 00732       }
nico@210: 00734       sdo.nodeId = *d->bDeviceNodeId; 
nico@210: 00735       sdo.body.data[0] = (1 << 5) | (d->transfers[line].toggle << 4);
nico@210: 00736       for (i = 1 ; i < 8 ; i++)
nico@210: 00737         sdo.body.data[i] = 0;
nico@210: 00738       MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", nodeId); 
nico@210: 00739       sendSDO(d, whoami, sdo);
nico@210: 00741       d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
nico@210: 00743       if (getSDOc(m->data[0])) {
nico@210: 00746         errorCode = SDOlineToObjdict(d, line);
nico@210: 00747         if (errorCode) {
nico@210: 00748           MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0); 
nico@210: 00749           failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
nico@210: 00750           return 0xFF;    
nico@210: 00751         }
nico@210: 00753         resetSDOline(d, line);
nico@210: 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@210: 00760       err = getSDOlineOnUse( d, nodeId, whoami, &line);
nico@210: 00761       if (!err)
nico@210: 00762         err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
nico@210: 00763       if (err) {
nico@210: 00764         MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); 
nico@210: 00765         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
nico@210: 00766         return 0xFF;
nico@210: 00767       }
nico@210: 00769       RestartSDO_TIMER(line)
nico@210: 00770       index = d->transfers[line].index;
nico@210: 00771       subIndex = d->transfers[line].subIndex;
nico@210: 00773       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
nico@210: 00774         MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId); 
nico@210: 00775         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
nico@210: 00776         return 0xFF;
nico@210: 00777       }
nico@210: 00779       nbBytes = 7 - getSDOn3(m->data[0]);
nico@210: 00781       err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
nico@210: 00782       if (err) {
nico@210: 00783         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
nico@210: 00784         return 0xFF;
nico@210: 00785       }
nico@210: 00787       d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
nico@210: 00789       if ( getSDOc(m->data[0])) {
nico@210: 00792         StopSDO_TIMER(line)
nico@210: 00793         d->transfers[line].state = SDO_FINISHED;
nico@210: 00794         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
nico@210: 00795         
nico@210: 00796         MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId);
nico@210: 00797       }
nico@210: 00798       else { 
nico@210: 00800         sdo.nodeId = nodeId;
nico@210: 00801         sdo.body.data[0] = (3 << 5) | (d->transfers[line].toggle << 4);
nico@210: 00802         for (i = 1 ; i < 8 ; i++)
nico@210: 00803           sdo.body.data[i] = 0;
nico@210: 00804         sendSDO(d, whoami, sdo);
nico@210: 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@210: 00813     if (whoami == SDO_SERVER) {
nico@210: 00814       index = getSDOindex(m->data[1],m->data[2]);
nico@210: 00815       subIndex = getSDOsubIndex(m->data[3]);
nico@210: 00816       MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ", 
nico@210: 00817               nodeId); 
nico@210: 00818       MSG_WAR(0x3A80, "Writing at index : ", index);
nico@210: 00819       MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex);
nico@210: 00820       
nico@210: 00822       err = getSDOlineOnUse( d, nodeId, whoami, &line );
nico@210: 00823       if (! err) {
nico@210: 00824         MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0); 
nico@210: 00825         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
nico@210: 00826         return 0xFF;
nico@210: 00827       }
nico@210: 00830       err = getSDOfreeLine( d, whoami, &line );
nico@210: 00831       if (err) {
nico@210: 00832         MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
nico@210: 00833         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
nico@210: 00834         return 0xFF;
nico@210: 00835       }
nico@210: 00836       initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);      
nico@210: 00837 
nico@210: 00838       if (getSDOe(m->data[0])) { 
nico@210: 00840         nbBytes = 4 - getSDOn2(m->data[0]);
nico@210: 00842         d->transfers[line].count = nbBytes;
nico@210: 00843         err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
nico@210: 00844         
nico@210: 00845         if (err) {
nico@210: 00846           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
nico@210: 00847           return 0xFF;
nico@210: 00848         }         
nico@210: 00849 
nico@210: 00852         MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfert. Finished.: ", nodeId);
nico@210: 00854         errorCode = SDOlineToObjdict(d, line);
nico@210: 00855         if (errorCode) {
nico@210: 00856           MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0); 
nico@210: 00857           failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
nico@210: 00858           return 0xFF;
nico@210: 00859         }
nico@210: 00861         resetSDOline(d, line);
nico@210: 00862       }
nico@210: 00863       else {
nico@210: 00864         if (getSDOs(m->data[0])) {
nico@210: 00866           nbBytes = m->data[4]; 
nico@210: 00867           err = setSDOlineRestBytes(d, nodeId, nbBytes);
nico@210: 00868           if (err) {
nico@210: 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@210: 00875       sdo.nodeId = *d->bDeviceNodeId; 
nico@210: 00876       sdo.body.data[0] = 3 << 5;
nico@210: 00877       sdo.body.data[1] = index & 0xFF;        /* LSB */
nico@210: 00878       sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
nico@210: 00879       sdo.body.data[3] = subIndex;
nico@210: 00880       for (i = 4 ; i < 8 ; i++)
nico@210: 00881                 sdo.body.data[i] = 0;
nico@210: 00882       sendSDO(d, whoami, sdo);
nico@210: 00883     } /* end if I am SERVER */
nico@210: 00884     else {
nico@210: 00887       err = getSDOlineOnUse( d, nodeId, whoami, &line);
nico@210: 00888       if (!err)
nico@210: 00889         err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
nico@210: 00890       if (err) {
nico@210: 00891         MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); 
nico@210: 00892         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
nico@210: 00893         return 0xFF;
nico@210: 00894       }
nico@210: 00896       RestartSDO_TIMER(line)
nico@210: 00897       index = d->transfers[line].index;
nico@210: 00898       subIndex = d->transfers[line].subIndex;
nico@210: 00900       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
nico@210: 00901         MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId); 
nico@210: 00902         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
nico@210: 00903         return 0xFF;
nico@210: 00904       }
nico@210: 00905 
nico@210: 00907       getSDOlineRestBytes(d, line, &nbBytes);
nico@210: 00908       if (nbBytes == 0) {
nico@210: 00909         MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId); 
nico@210: 00910         StopSDO_TIMER(line)
nico@210: 00911         d->transfers[line].state = SDO_FINISHED;
nico@210: 00912         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
nico@210: 00913         return 0x00;
nico@210: 00914       }
nico@210: 00916       if (nbBytes > 7) {
nico@210: 00919         d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
nico@210: 00920         sdo.nodeId = nodeId; /* The server node Id; */
nico@210: 00921         sdo.body.data[0] = (d->transfers[line].toggle << 4);
nico@210: 00922         err = lineToSDO(d, line, 7, sdo.body.data + 1);  
nico@210: 00923         if (err) {
nico@210: 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@210: 00931         d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
nico@210: 00932         sdo.nodeId = nodeId; /* The server node Id; */
nico@210: 00933         sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
nico@210: 00934         err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);    
nico@210: 00935         if (err) {
nico@210: 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++)
nico@210: 00940           sdo.body.data[i] = 0;
nico@210: 00941       }
nico@210: 00942       MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId); 
nico@210: 00943       sendSDO(d, whoami, sdo); 
nico@210: 00944     } 
nico@210: 00945     break;
nico@210: 00946 
nico@210: 00947   case 2:
nico@210: 00950     if (whoami == SDO_SERVER) {
nico@210: 00951       index = getSDOindex(m->data[1],m->data[2]);
nico@210: 00952       subIndex = getSDOsubIndex(m->data[3]);
nico@210: 00953       MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", 
nico@210: 00954               nodeId); 
nico@210: 00955       MSG_WAR(0x3A90, "Reading at index : ", index);
nico@210: 00956       MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex);
nico@210: 00958       err = getSDOlineOnUse( d, nodeId, whoami, &line );
nico@210: 00959       if (! err) {
nico@210: 00960             MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line); 
nico@210: 00961         MSG_WAR(0x3A93, "nodeId = ", nodeId); 
nico@210: 00962             failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
nico@210: 00963             return 0xFF;
nico@210: 00964       }
nico@210: 00967       err = getSDOfreeLine( d, whoami, &line );
nico@210: 00968       if (err) {
nico@210: 00969         MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
nico@210: 00970         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
nico@210: 00971         return 0xFF;
nico@210: 00972       }
nico@210: 00973       initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
nico@210: 00975       errorCode = objdictToSDOline(d, line);
nico@210: 00976      
nico@210: 00977       if (errorCode) {
nico@210: 00978         MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ", 
nico@210: 00979                 errorCode); 
nico@210: 00980         failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
nico@210: 00981         return 0xFF;
nico@210: 00982         }
nico@210: 00984       getSDOlineRestBytes(d, line, &nbBytes);   
nico@210: 00985       sdo.nodeId = nodeId; 
nico@210: 00986       if (nbBytes > 4) {
nico@210: 00989         sdo.body.data[0] = (2 << 5) | 1;
nico@210: 00990         sdo.body.data[1] = index & 0xFF;        /* LSB */
nico@210: 00991         sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
nico@210: 00992         sdo.body.data[3] = subIndex;
nico@210: 00993         sdo.body.data[4] = nbBytes; /* Limitation of canfestival2 : Max tranfert is 256 bytes.*/
nico@210: 00996         for (i = 5 ; i < 8 ; i++)
nico@210: 00997           sdo.body.data[i] = 0;
nico@210: 00998         MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId); 
nico@210: 00999         sendSDO(d, whoami, sdo); 
nico@210: 01000       }
nico@210: 01001       else {
nico@210: 01003         sdo.body.data[0] = (2 << 5) | ((4 - nbBytes) << 2) | 3;  
nico@210: 01004         sdo.body.data[1] = index & 0xFF;        /* LSB */
nico@210: 01005         sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
nico@210: 01006         sdo.body.data[3] = subIndex;
nico@210: 01007         err = lineToSDO(d, line, nbBytes, sdo.body.data + 4);    
nico@210: 01008         if (err) {
nico@210: 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++)
nico@210: 01013           sdo.body.data[i] = 0;
nico@210: 01014         MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ", 
nico@210: 01015                 nodeId); 
nico@210: 01016         sendSDO(d, whoami, sdo); 
nico@210: 01018         resetSDOline(d, line);
nico@210: 01019       }
nico@210: 01020     } 
nico@210: 01021     else {
nico@210: 01025       err = getSDOlineOnUse( d, nodeId, whoami, &line);
nico@210: 01026       if (!err)
nico@210: 01027         err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
nico@210: 01028       if (err) {
nico@210: 01029         MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId); 
nico@210: 01030         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
nico@210: 01031         return 0xFF;
nico@210: 01032       }
nico@210: 01034       RestartSDO_TIMER(line)
nico@210: 01035       index = d->transfers[line].index;
nico@210: 01036       subIndex = d->transfers[line].subIndex;
nico@210: 01037       
nico@210: 01038       if (getSDOe(m->data[0])) { /* If SDO expedited */
nico@210: 01040           nbBytes = 4 - getSDOn2(m->data[0]);
nico@210: 01042         err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
nico@210: 01043         if (err) {
nico@210: 01044           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
nico@210: 01045           return 0xFF;
nico@210: 01046         }
nico@210: 01048         MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId);
nico@210: 01049         StopSDO_TIMER(line)
nico@210: 01050         d->transfers[line].count = nbBytes;
nico@210: 01051         d->transfers[line].state = SDO_FINISHED;
nico@210: 01052         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
nico@210: 01053         return 0;
nico@210: 01054       }
nico@210: 01055       else { 
nico@210: 01057         if (getSDOs(m->data[0])) {
nico@210: 01058           nbBytes = m->data[4]; 
nico@210: 01059           err = setSDOlineRestBytes(d, line, nbBytes);
nico@210: 01060           if (err) {
nico@210: 01061             failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
nico@210: 01062             return 0xFF;
nico@210: 01063           }     
nico@210: 01064         }
nico@210: 01066         sdo.nodeId = nodeId;
nico@210: 01067         sdo.body.data[0] = 3 << 5;
nico@210: 01068         for (i = 1 ; i < 8 ; i++)
nico@210: 01069           sdo.body.data[i] = 0;
nico@210: 01070         MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId); 
nico@210: 01071         sendSDO(d, whoami, sdo);  
nico@210: 01072       }
nico@210: 01073     } 
nico@210: 01074     break;
nico@210: 01075 
nico@210: 01076   case 3:
nico@210: 01078     if (whoami == SDO_SERVER) {
nico@210: 01081       err = getSDOlineOnUse( d, nodeId, whoami, &line ); 
nico@210: 01082       if (!err)
nico@210: 01083         err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
nico@210: 01084       if (err) {
nico@210: 01085         MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", 
nico@210: 01086                 nodeId); 
nico@210: 01087         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
nico@210: 01088         return 0xFF;
nico@210: 01089       }
nico@210: 01091       RestartSDO_TIMER(line)
nico@210: 01092       MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", nodeId); 
nico@210: 01093       index = d->transfers[line].index;
nico@210: 01094       subIndex = d->transfers[line].subIndex;
nico@210: 01096       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
nico@210: 01097         MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0])); 
nico@210: 01098         failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
nico@210: 01099         return 0xFF;
nico@210: 01100       }
nico@210: 01102       getSDOlineRestBytes(d, line, &nbBytes);             
nico@210: 01103       if (nbBytes > 7) {
nico@210: 01106         sdo.nodeId = nodeId; /* The server node Id; */
nico@210: 01107         sdo.body.data[0] = (d->transfers[line].toggle << 4);
nico@210: 01108         err = lineToSDO(d, line, 7, sdo.body.data + 1);  
nico@210: 01109         if (err) {
nico@210: 01110           failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
nico@210: 01111           return 0xFF;
nico@210: 01112         }
nico@210: 01114         d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
nico@210: 01115         MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", nodeId); 
nico@210: 01116         sendSDO(d, whoami, sdo); 
nico@210: 01117       } 
nico@210: 01118       else {
nico@210: 01121         sdo.nodeId = nodeId; 
nico@210: 01122         sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
nico@210: 01123         err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);    
nico@210: 01124         if (err) {
nico@210: 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++)
nico@210: 01129           sdo.body.data[i] = 0;
nico@210: 01130         MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", nodeId);      
nico@210: 01131         sendSDO(d, whoami, sdo);
nico@210: 01133         resetSDOline(d, line);
nico@210: 01134       }
nico@210: 01135     } /* end if SERVER*/
nico@210: 01136     else {
nico@210: 01140       err = getSDOlineOnUse( d, nodeId, whoami, &line);
nico@210: 01141       if (!err)
nico@210: 01142         err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
nico@210: 01143       if (err) {
nico@210: 01144         MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId); 
nico@210: 01145         failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
nico@210: 01146         return 0xFF;
nico@210: 01147       }
nico@210: 01149       RestartSDO_TIMER(line)
nico@210: 01150       index = d->transfers[line].index;
nico@210: 01151       subIndex = d->transfers[line].subIndex;
nico@210: 01152       /* End transmission or requesting  next segment. */
nico@210: 01153       getSDOlineRestBytes(d, line, &nbBytes);
nico@210: 01154       if (nbBytes == 0) {
nico@210: 01155         MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId); 
nico@210: 01156         StopSDO_TIMER(line)
nico@210: 01157         d->transfers[line].state = SDO_FINISHED;
nico@210: 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@210: 01164         sdo.nodeId = nodeId; 
nico@210: 01165         sdo.body.data[0] = (d->transfers[line].toggle << 4);
nico@210: 01166         err = lineToSDO(d, line, 7, sdo.body.data + 1);  
nico@210: 01167         if (err) {
nico@210: 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@210: 01175         sdo.nodeId = nodeId; 
nico@210: 01176         sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
nico@210: 01177         err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);    
nico@210: 01178         if (err) {
nico@210: 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++)
nico@210: 01183           sdo.body.data[i] = 0;
nico@210: 01184       }
nico@210: 01185       MSG_WAR(0x3AA7, "SDO sending download segment to nodeId", nodeId); 
nico@210: 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] |
nico@210: 01193       ((UNS32)m->data[5] << 8) |
nico@210: 01194       ((UNS32)m->data[6] << 16) |
nico@210: 01195       ((UNS32)m->data[7] << 24);
nico@210: 01198     if (whoami == SDO_SERVER) {
nico@210: 01199       err = getSDOlineOnUse( d, nodeId, whoami, &line );
nico@210: 01200       if (!err) {
nico@210: 01201         resetSDOline( d, line );
nico@210: 01202         MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode);
nico@210: 01203       }
nico@210: 01204       else
nico@210: 01205         MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode);
nico@210: 01208     }
nico@210: 01209     else { 
nico@210: 01210       err = getSDOlineOnUse( d, nodeId, whoami, &line );
nico@210: 01211       if (!err) {
nico@210: 01213         StopSDO_TIMER(line)
nico@210: 01214         d->transfers[line].state = SDO_ABORTED_RCV;
nico@210: 01215         d->transfers[line].abortCode = abortCode;
nico@210: 01216         MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode);
nico@210: 01217         if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
nico@210: 01218       }
nico@210: 01219       else
nico@210: 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@210: 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 
nico@210: 01247 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, 
nico@210: 01248                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize)
nico@210: 01249 {
nico@210: 01250   UNS8 err;
nico@210: 01251   UNS8 SDOfound = 0;
nico@210: 01252   UNS8 line;
nico@210: 01253   s_SDO sdo;    /* SDO to transmit */
nico@210: 01254   UNS8 i, j;
nico@210: 01255   UNS16     lastIndex;
nico@210: 01256   UNS16     offset;
nico@210: 01257   UNS32      *pNodeIdServer;
nico@210: 01258   UNS32      nodeIdServer;
nico@210: 01259 
nico@210: 01260   MSG_WAR(0x3AC0, "Send SDO to write in the dictionary of node : ", nodeId);
nico@210: 01261   MSG_WAR(0x3AC1, "                                   At index : ", index);
nico@210: 01262   MSG_WAR(0x3AC2, "                                   subIndex : ", subIndex);
nico@210: 01263   MSG_WAR(0x3AC3, "                                   nb bytes : ", count);
nico@210: 01264 
nico@210: 01266   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
nico@210: 01267   if (!err) {
nico@210: 01268     MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId); 
nico@210: 01269     return 0xFF;
nico@210: 01270   }
nico@210: 01272   err = getSDOfreeLine( d, SDO_CLIENT, &line );
nico@210: 01273   if (err) {
nico@210: 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@210: 01278   offset = d->firstIndex->SDO_CLT;
nico@210: 01279   lastIndex = d->lastIndex->SDO_CLT;
nico@210: 01280   if (offset == 0) {
nico@210: 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) {
nico@210: 01286      if (d->objdict[offset].bSubCount <= 3) {
nico@210: 01287          MSG_ERR(0x1AC8, "Subindex 3  not found at index ", 0x1280 + i);
nico@210: 01288          return 0xFF;
nico@210: 01289      }
nico@210: 01291      pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
nico@210: 01292      nodeIdServer = *pNodeIdServer;
nico@210: 01293      MSG_WAR(0x1AD2, "index : ", 0x1280 + i);
nico@210: 01294      MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer);
nico@210: 01295    
nico@210: 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) {
nico@210: 01304     MSG_ERR(0x1AC9, "SDO. Error. No client found to communicate with node : ", nodeId);
nico@210: 01305     return 0xFF;
nico@210: 01306   }
nico@210: 01307   MSG_WAR(0x3AD0,"        SDO client defined at index  : ", 0x1280 + i);
nico@210: 01308   initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);
nico@210: 01309   d->transfers[line].count = count;
nico@210: 01310   d->transfers[line].dataType = dataType;
nico@210: 01311   
nico@210: 01313   for (j = 0 ; j < count ; j++) {
nico@210: 01314 # ifdef CANOPEN_BIG_ENDIAN
nico@210: 01315     if (dataType == 0 && endianize)
nico@210: 01316       d->transfers[line].data[count - 1 - j] = ((char *)data)[j];
nico@210: 01317     else /* String of bytes. */
nico@210: 01318       d->transfers[line].data[j] = ((char *)data)[j];
nico@210: 01319 #  else 
nico@210: 01320     d->transfers[line].data[j] = ((char *)data)[j];
nico@210: 01321 #  endif
nico@210: 01322   }
nico@210: 01324   sdo.nodeId = nodeId;
nico@210: 01325   if (count <= 4) { 
nico@210: 01326     sdo.body.data[0] = (1 << 5) | ((4 - count) << 2) | 3;
nico@210: 01327     for (i = 4 ; i < 8 ; i++)
nico@210: 01328       sdo.body.data[i] = d->transfers[line].data[i - 4];
nico@210: 01329     d->transfers[line].offset = count;
nico@210: 01330   }     
nico@210: 01331   else { 
nico@210: 01332     sdo.body.data[0] = (1 << 5) | 1;
nico@210: 01333     sdo.body.data[4] = count; 
nico@210: 01334     for (i = 5 ; i < 8 ; i++)
nico@210: 01335       sdo.body.data[i] = 0;
nico@210: 01336   }
nico@210: 01337   sdo.body.data[1] = index & 0xFF;        /* LSB */
nico@210: 01338   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
nico@210: 01339   sdo.body.data[3] = subIndex;
nico@210: 01340 
nico@210: 01341   d->transfers[line].Callback = Callback;
nico@210: 01342     
nico@210: 01343   err = sendSDO(d, SDO_CLIENT, sdo);
nico@210: 01344   if (err) {
nico@210: 01345     MSG_ERR(0x1AD1, "SDO. Error while sending SDO to node : ", nodeId);
nico@210: 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 
nico@210: 01368 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, 
nico@210: 01369                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data)
nico@210: 01370 {
nico@210: 01371         return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, NULL, 1);
nico@210: 01372 }
nico@210: 01373 
nico@210: 01388 UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index, 
nico@210: 01389                        UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback)
nico@210: 01390 {
nico@210: 01391         return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1);      
nico@210: 01392 }
nico@210: 01393 
nico@210: 01406 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
nico@210: 01407 {
nico@210: 01408   UNS8 err;
nico@210: 01409   UNS8 SDOfound = 0;
nico@210: 01410   UNS8 i;
nico@210: 01411   UNS8 line;
nico@210: 01412   s_SDO sdo;    /* SDO to transmit */
nico@210: 01413   UNS32      *pNodeIdServer;
nico@210: 01414   UNS32      nodeIdServer;
nico@210: 01415   UNS16     offset;
nico@210: 01416   UNS16     lastIndex;
nico@210: 01417   MSG_WAR(0x3AD5, "Send SDO to read in the dictionary of node : ", nodeId);
nico@210: 01418   MSG_WAR(0x3AD6, "                                  At index : ", index);
nico@210: 01419   MSG_WAR(0x3AD7, "                                  subIndex : ", subIndex);
nico@210: 01420 
nico@210: 01421 
nico@210: 01423   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
nico@210: 01424   if (!err) {
nico@210: 01425     MSG_ERR(0x1AD8, "SDO error : Communication yet established. with node : ", nodeId); 
nico@210: 01426     return 0xFF;
nico@210: 01427   }
nico@210: 01429   err = getSDOfreeLine( d, SDO_CLIENT, &line );
nico@210: 01430   if (err) {
nico@210: 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
nico@210: 01435     MSG_WAR(0x3AE0, "Transmission on line : ", line);
nico@210: 01436 
nico@210: 01438   offset = d->firstIndex->SDO_CLT;
nico@210: 01439   lastIndex = d->lastIndex->SDO_CLT;
nico@210: 01440   if (offset == 0) {
nico@210: 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) {
nico@210: 01446      if (d->objdict[offset].bSubCount <= 3) {
nico@210: 01447          MSG_ERR(0x1AE2, "Subindex 3  not found at index ", 0x1280 + i);
nico@210: 01448          return 0xFF;
nico@210: 01449      }
nico@210: 01451      pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
nico@210: 01452      nodeIdServer = *pNodeIdServer;
nico@210: 01453    
nico@210: 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) {
nico@210: 01462     MSG_ERR(0x1AE3, "SDO. Error. No client found to communicate with node : ", nodeId);
nico@210: 01463     return 0xFF;
nico@210: 01464   }
nico@210: 01465   MSG_WAR(0x3AE4,"        SDO client defined at index  : ", 0x1280 + i);
nico@210: 01466   initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
nico@210: 01467   getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
nico@210: 01468   sdo.nodeId = nodeId;
nico@210: 01470   d->transfers[line].dataType = dataType;                               
nico@210: 01471   sdo.body.data[0] = (2 << 5);  
nico@210: 01472   sdo.body.data[1] = index & 0xFF;        /* LSB */
nico@210: 01473   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
nico@210: 01474   sdo.body.data[3] = subIndex;
nico@210: 01475   for (i = 4 ; i < 8 ; i++)
nico@210: 01476     sdo.body.data[i] = 0;
nico@210: 01477   d->transfers[line].Callback = Callback;
nico@210: 01478   err = sendSDO(d, SDO_CLIENT, sdo);
nico@210: 01479   if (err) {
nico@210: 01480     MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId);
nico@210: 01481     /* release the line */
nico@210: 01482     resetSDOline(d, line);
nico@210: 01483     return 0xFF;
nico@210: 01484   }             
nico@210: 01485   return 0;
nico@210: 01486 }
nico@210: 01487 
nico@210: 01499 UNS8 readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType)
nico@210: 01500 {
nico@210: 01501         return _readNetworkDict (d, nodeId, index, subIndex, dataType, NULL);
nico@210: 01502 }
nico@210: 01503 
nico@210: 01516 UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
nico@210: 01517 {
nico@210: 01518         return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback);
nico@210: 01519 }
nico@210: 01520 
nico@210: 01532 UNS8 getReadResultNetworkDict (CO_Data* d, UNS8 nodeId, void* data, UNS8 *size, 
nico@210: 01533                                UNS32 * abortCode)
nico@210: 01534 {
nico@210: 01535   UNS8 i;
nico@210: 01536   UNS8 err;
nico@210: 01537   UNS8 line;
nico@210: 01538   * size = 0;
nico@210: 01539 
nico@210: 01541   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
nico@210: 01542   if (err) {
nico@210: 01543     MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId); 
nico@210: 01544     return SDO_ABORTED_INTERNAL;
nico@210: 01545   }
nico@210: 01546   if (d->transfers[line].state != SDO_FINISHED)
nico@210: 01547     return d->transfers[line].state;
nico@210: 01548 
nico@210: 01550   * size = (UNS8)d->transfers[line].count;
nico@210: 01551   for  ( i = 0 ; i < *size ; i++) {
nico@210: 01552 # ifdef CANOPEN_BIG_ENDIAN
nico@210: 01553     if (d->transfers[line].dataType != visible_string)
nico@210: 01554       ( (char *) data)[*size - 1 - i] = d->transfers[line].data[i];
nico@210: 01555     else 
nico@210: 01556       ( (char *) data)[i] = d->transfers[line].data[i];
nico@210: 01557 # else 
nico@210: 01558     ( (char *) data)[i] = d->transfers[line].data[i];
nico@210: 01559 # endif
nico@210: 01560   } 
nico@210: 01561   return SDO_FINISHED;
nico@210: 01562 }
nico@210: 01563 
nico@210: 01573 UNS8 getWriteResultNetworkDict (CO_Data* d, UNS8 nodeId, UNS32 * abortCode)
nico@210: 01574 {
nico@210: 01575   UNS8 line = 0;
nico@210: 01576   UNS8 err;
nico@210: 01577 
nico@210: 01578   * abortCode = 0;
nico@210: 01580   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
nico@210: 01581   if (err) {
nico@210: 01582     MSG_ERR(0x1AF1, "SDO error : No line found for communication with node : ", nodeId); 
nico@210: 01583     return SDO_ABORTED_INTERNAL;
nico@210: 01584   }
nico@210: 01585   * abortCode = d->transfers[line].abortCode;
nico@210: 01586   return d->transfers[line].state;
nico@210: 01587 }
nico@210:
Generated on Tue Jun 5 18:32:05 2007 for CanFestival by  nico@207: nico@207: doxygen 1.5.1
nico@207: nico@207: