CanFestival: /home/epimerde/documents/tc11/CanFestival-3/src/dcf.c Source File

00001 /*
nico@210: 00002   This file is part of CanFestival, a library implementing CanOpen
nico@210: 00003   Stack.
nico@210: 00004 
nico@210: 00005   Copyright (C): Edouard TISSERANT and Francis DUPIN
nico@210: 00006 
nico@210: 00007   See COPYING file for copyrights details.
nico@210: 00008 
nico@210: 00009   This library is free software; you can redistribute it and/or
nico@210: 00010   modify it under the terms of the GNU Lesser General Public
nico@210: 00011   License as published by the Free Software Foundation; either
nico@210: 00012   version 2.1 of the License, or (at your option) any later version.
nico@210: 00013 
nico@210: 00014   This library is distributed in the hope that it will be useful,
nico@210: 00015   but WITHOUT ANY WARRANTY; without even the implied warranty of
nico@210: 00016   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
nico@210: 00017   Lesser General Public License for more details.
nico@210: 00018 
nico@210: 00019   You should have received a copy of the GNU Lesser General Public
nico@210: 00020   License along with this library; if not, write to the Free Software
nico@210: 00021   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
nico@210: 00022   USA
nico@210: 00023 */
nico@210: 00024 
nico@210: 00025 
nico@210: 00035 #include "objacces.h"
nico@210: 00036 #include "sdo.h"
nico@210: 00037 #include "dcf.h"
nico@210: 00038 #include "sysdep.h"
nico@210: 00039 
nico@210: 00040 
nico@210: 00054 extern UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
nico@210: 00055                                UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize);
nico@210: 00056 
nico@210: 00057 const indextable *ptrTable;
nico@210: 00058 
nico@210: 00065 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
nico@210: 00066 {
nico@210: 00067   UNS32 abortCode;
nico@210: 00068 
nico@210: 00069   if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
nico@210: 00070     {
nico@210: 00071       MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode);
nico@210: 00072       MSG_WAR(0x2A02, "server node : ", nodeId);
nico@210: 00073     }
nico@210: 00074 
nico@210: 00075   closeSDOtransfer(d, nodeId, SDO_CLIENT);
nico@210: 00076   decompo_dcf(d,nodeId);
nico@210: 00077 }
nico@210: 00078 
nico@210: 00087 UNS32 decompo_dcf(CO_Data* d,UNS8 nodeId)
nico@210: 00088 {
nico@210: 00089   UNS32 errorCode;
nico@210: 00090   UNS16 target_Index;
nico@210: 00091   UNS8 target_Subindex;
nico@210: 00092   UNS32 target_Size;
nico@210: 00093   UNS32 res;
nico@210: 00094   ODCallback_t *Callback;
nico@210: 00095 
nico@210: 00096   ptrTable = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback);
nico@210: 00097   if (errorCode != OD_SUCCESSFUL)
nico@210: 00098     {
nico@210: 00099       return errorCode;
nico@210: 00100     }
nico@210: 00101 
nico@210: 00103   while (nodeId < ptrTable->bSubCount){
nico@210: 00104     UNS32 nb_targets;
nico@210: 00105 
nico@210: 00106     UNS8 szData = ptrTable->pSubindex[nodeId].size;
nico@210: 00107     UNS8* dcfend;
nico@210: 00108 
nico@210: 00109     {
nico@210: 00110       UNS8* dcf = *((UNS8**)ptrTable->pSubindex[nodeId].pObject);
nico@210: 00111       dcfend = dcf + szData;
nico@210: 00112       if (!d->dcf_cursor){
nico@210: 00113         d->dcf_cursor = (UNS8*)dcf + 4;
nico@210: 00114         d->dcf_count_targets = 0;
nico@210: 00115       }
nico@210: 00116       nb_targets = UNS32_LE(*((UNS32*)dcf));
nico@210: 00117     }
nico@210: 00118 
nico@210: 00120     if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_count_targets < nb_targets)
nico@210: 00121       {
nico@210: 00123         target_Index = UNS16_LE(*((UNS16*)(d->dcf_cursor))); d->dcf_cursor += 2;
nico@210: 00124         target_Subindex = *((UNS8*)(((UNS8*)d->dcf_cursor)++));
nico@210: 00125         target_Size = UNS32_LE(*((UNS32*)(d->dcf_cursor))); d->dcf_cursor += 4;
nico@210: 00126 
nico@210: 00127         /* printf("Master : ConfigureSlaveNode %2.2x (Concise
nico@210: 00128           DCF)\n",nodeId);*/
nico@210: 00129         res = _writeNetworkDict(d, 
nico@210: 00130                                 nodeId, 
nico@210: 00131                                 target_Index, 
nico@210: 00132                                 target_Subindex, 
nico@210: 00133                                 target_Size, 
nico@210: 00134                                 0, 
nico@210: 00135                                 d->dcf_cursor,
nico@210: 00136                                 CheckSDOAndContinue,
nico@210: 00138                                 0); 
nico@210: 00141         d->dcf_cursor += target_Size;
nico@210: 00142         d->dcf_count_targets++;
nico@210: 00143 
nico@210: 00144         return ;
nico@210: 00145       }
nico@210: 00146     nodeId++;
nico@210: 00147     d->dcf_cursor = NULL;
nico@210: 00148   }
nico@210: 00150   (*d->preOperational)();
nico@210: 00151 
nico@210: 00152 }
nico@210: