dcf.c

00001 /*
00002   This file is part of CanFestival, a library implementing CanOpen
00003   Stack.
00004 
00005   Copyright (C): Edouard TISSERANT and Francis DUPIN
00006 
00007   See COPYING file for copyrights details.
00008 
00009   This library is free software; you can redistribute it and/or
00010   modify it under the terms of the GNU Lesser General Public
00011   License as published by the Free Software Foundation; either
00012   version 2.1 of the License, or (at your option) any later version.
00013 
00014   This library is distributed in the hope that it will be useful,
00015   but WITHOUT ANY WARRANTY; without even the implied warranty of
00016   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00017   Lesser General Public License for more details.
00018 
00019   You should have received a copy of the GNU Lesser General Public
00020   License along with this library; if not, write to the Free Software
00021   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
00022   USA
00023 */
00024 
00025 
00026 /**
00027 ** @file   dcf.c
00028 ** @author Edouard TISSERANT and Francis DUPIN
00029 ** @date   Mon Jun  4 17:06:12 2007
00030 **
00031 ** @brief EXEMPLE OF SOMMARY
00032 **
00033 **
00034 */
00035 
00036 
00037 #include "data.h"
00038 #include "sysdep.h"
00039 //#include "sdo.h"
00040 
00041 //extern UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
00042 //                             UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize);
00043 
00044 
00045 static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId);
00046 
00047 /* Seek to next NodeID's DCF */
00048 #define SEEK_NEXT_DCF() \
00049        nodeId=(nodeId+1) % d->dcf_odentry->bSubCount; \
00050        if(nodeId==0) nodeId=1; \
00051        d->dcf_cursor = NULL;
00052 
00053 /**
00054 **
00055 **
00056 ** @param d
00057 ** @param nodeId
00058 */
00059 static void CheckSDOAndContinue (CO_Data* d, UNS8 nodeId)
00060 {
00061   UNS32 abortCode = 0;
00062 
00063   if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
00064     {
00065       MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode);
00066       MSG_WAR(0x2A02, "server node : ", nodeId);
00067     }
00068 
00069   closeSDOtransfer(d, nodeId, SDO_CLIENT);
00070   /* Timedout ? */
00071   if(abortCode == SDOABT_TIMED_OUT){
00072     /* Node may not be ready, try another one */
00073     /* Warning, this might leed to endless attempts */
00074     /* if node does never answer */
00075     SEEK_NEXT_DCF()
00076   }
00077   send_consise_dcf_loop(d,nodeId);
00078 }
00079 
00080 
00081 /**
00082 **
00083 **
00084 ** @param d
00085 ** @param nodeId
00086 **
00087 ** @return
00088 */
00089 UNS8 send_consise_dcf(CO_Data* d,UNS8 nodeId)
00090 {
00091   UNS32 szData;
00092   /* Fetch DCF OD entry, if not already done */
00093   if(!d->dcf_odentry)
00094   {
00095     UNS32 errorCode;
00096     ODCallback_t *Callback;
00097     d->dcf_odentry = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback);
00098     /* If DCF entry do not exist... Nothing to do.*/
00099     if (errorCode != OD_SUCCESSFUL) goto DCF_finish;
00100   }
00101 
00102   szData = d->dcf_odentry->pSubindex[nodeId].size;
00103   
00104   /* if the entry for the nodeId is not empty. */
00105   if(szData!=0){
00106       /* if the entry for the nodeId is already been processing, quit.*/
00107       if(d->dcf_odentry->pSubindex[nodeId].bAccessType & DCF_TO_SEND) return 1;
00108       
00109       d->dcf_odentry->pSubindex[nodeId].bAccessType|=DCF_TO_SEND;
00110       d->dcf_request++;
00111       if(d->dcf_request==1)
00112           send_consise_dcf_loop(d,nodeId);
00113       return 1;
00114   }
00115   
00116   DCF_finish:
00117   return 0;
00118 }
00119 
00120 static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId)
00121 {
00122   if(nodeId > d->dcf_odentry->bSubCount) return;
00123 /* Loop on all DCF subindexes, corresponding to node ID until there is no request*/
00124   //while (nodeId < d->dcf_odentry->bSubCount){
00125   while (d->dcf_request>0){
00126       if(d->dcf_odentry->pSubindex[nodeId].bAccessType & DCF_TO_SEND){        
00127         UNS8* dcfend;
00128           UNS32 nb_entries;
00129           UNS32 szData = d->dcf_odentry->pSubindex[nodeId].size;
00130            
00131            {
00132                UNS8* dcf = *((UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject);
00133                dcfend = dcf + szData;
00134                if (!d->dcf_cursor){
00135                   d->dcf_cursor = (UNS8*)dcf + 4;
00136                    d->dcf_entries_count = 0;
00137                }
00138                nb_entries = UNS32_LE(*((UNS32*)dcf));
00139            }
00140 
00141         /* condition on consise DCF string for NodeID, if big enough */
00142         if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries){
00143         
00144             UNS16 target_Index;
00145             UNS8 target_Subindex;
00146             UNS32 target_Size;
00147 
00148             /* DCF data may not be 32/16b aligned, 
00149              * we cannot directly dereference d->dcf_cursor 
00150              * as UNS16 or UNS32 
00151              * Do it byte per byte taking care on endianess*/
00152 #ifdef CANOPEN_BIG_ENDIAN
00153             target_Index = *(d->dcf_cursor++) << 8 | 
00154                            *(d->dcf_cursor++);
00155 #else
00156             memcpy(&target_Index, d->dcf_cursor,2);
00157             d->dcf_cursor+=2;
00158 #endif
00159 
00160             target_Subindex = *(d->dcf_cursor++);
00161 
00162 #ifdef CANOPEN_BIG_ENDIAN
00163             target_Size = *(d->dcf_cursor++) << 24 | 
00164                           *(d->dcf_cursor++) << 16 | 
00165                           *(d->dcf_cursor++) << 8 | 
00166                           *(d->dcf_cursor++);
00167 #else
00168             memcpy(&target_Size, d->dcf_cursor,4);
00169             d->dcf_cursor+=4;
00170 #endif
00171     
00172            _writeNetworkDict (d, /* CO_Data* d*/
00173                                 nodeId, /* UNS8 nodeId*/
00174                                 target_Index, /* UNS16 index*/
00175                                 target_Subindex, /* UNS8 subindex*/
00176                                 (UNS8)target_Size, /* UNS8 count*/
00177                                 0, /* UNS8 dataType*/
00178                                 d->dcf_cursor,/* void *data*/
00179                                 CheckSDOAndContinue ,/* SDOCallback_t
00180                                                       Callback*/
00181                                 0); /* no endianize*/
00182             /* Push d->dcf_cursor to the end of data*/
00183 
00184             d->dcf_cursor += target_Size;
00185             d->dcf_entries_count++;
00186 
00187             /* send_consise_dcf_loop will be called by CheckSDOAndContinue for next DCF entry*/
00188             return;
00189           }
00190           else
00191           {
00192               /* We have finished with the dcf entry. Change the flag, decrement the request
00193                *  and execute the bootup callback. */
00194               d->dcf_odentry->pSubindex[nodeId].bAccessType&=~DCF_TO_SEND;
00195               d->dcf_request--;
00196               (*d->post_SlaveBootup)(d, nodeId);
00197           }
00198      }
00199      
00200     SEEK_NEXT_DCF()
00201   }
00202 
00203 }