dcf.c

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