src/dcf.c

   1 /*
   2   This file is part of CanFestival, a library implementing CanOpen
   3   Stack.
   4
   5   Copyright (C): Edouard TISSERANT and Francis DUPIN
   6
   7   See COPYING file for copyrights details.
   8
   9   This library is free software; you can redistribute it and/or
  10   modify it under the terms of the GNU Lesser General Public
  11   License as published by the Free Software Foundation; either
  12   version 2.1 of the License, or (at your option) any later version.
  13
  14   This library is distributed in the hope that it will be useful,
  15   but WITHOUT ANY WARRANTY; without even the implied warranty of
  16   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17   Lesser General Public License for more details.
  18
  19   You should have received a copy of the GNU Lesser General Public
  20   License along with this library; if not, write to the Free Software
  21   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
  22   USA
  23 */
  24
  25
  26 /**
  27 ** @file   dcf.c
  28 ** @author Edouard TISSERANT and Francis DUPIN
  29 ** @date   Mon Jun  4 17:06:12 2007
  30 **
  31 ** @brief EXEMPLE OF SOMMARY
  32 **
  33 **
  34 */
  35
  36
  37 #include "data.h"
  38 #include "sysdep.h"
  39
  40 extern UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
  41                                UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize);
  42
  43
  44 static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId);
  45
  46 /* Seek to next NodeID's DCF */
  47 #define SEEK_NEXT_DCF() \
  48         nodeId=(nodeId+1) % d->dcf_odentry->bSubCount; \
  49         if(nodeId==0) nodeId=1; \
  50         d->dcf_cursor = NULL;
  51
  52 /**
  53 **
  54 **
  55 ** @param d
  56 ** @param nodeId
  57 */
  58 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
  59 {
  60   UNS32 abortCode = 0;
  61
  62   if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
  63     {
  64       MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode);
  65       MSG_WAR(0x2A02, "server node : ", nodeId);
  66     }
  67
  68   closeSDOtransfer(d, nodeId, SDO_CLIENT);
  69   /* Timedout ? */
  70   if(abortCode == SDOABT_TIMED_OUT){
  71     /* Node may not be ready, try another one */
  72     /* Warning, this might leed to endless attempts */
  73     /* if node does never answer */
  74         SEEK_NEXT_DCF()
  75   }
  76   send_consise_dcf_loop(d,nodeId);
  77 }
  78
  79
  80 /**
  81 **
  82 **
  83 ** @param d
  84 ** @param nodeId
  85 **
  86 ** @return
  87 */
  88 UNS8 send_consise_dcf(CO_Data* d,UNS8 nodeId)
  89 {
  90   UNS8 szData;
  91   /* Fetch DCF OD entry, if not already done */
  92   if(!d->dcf_odentry)
  93   {
  94     UNS32 errorCode;
  95     ODCallback_t *Callback;
  96     d->dcf_odentry = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback);
  97     /* If DCF entry do not exist... Nothing to do.*/
  98     if (errorCode != OD_SUCCESSFUL) goto DCF_finish;
  99   }
 100
 101   szData = d->dcf_odentry->pSubindex[nodeId].size;
 102
 103   /* if the entry for the nodeId is not empty. */
 104   if(szData!=0){
 105         /* if the entry for the nodeId is already been processing, quit.*/
 106         if(d->dcf_odentry->pSubindex[nodeId].bAccessType & DCF_TO_SEND) return 1;
 107
 108         d->dcf_odentry->pSubindex[nodeId].bAccessType|=DCF_TO_SEND;
 109         d->dcf_request++;
 110         if(d->dcf_request==1)
 111                 send_consise_dcf_loop(d,nodeId);
 112         return 1;
 113   }
 114
 115   DCF_finish:
 116   return 0;
 117 }
 118
 119 static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId)
 120 {
 121   /* Loop on all DCF subindexes, corresponding to node ID until there is no request*/
 122   //while (nodeId < d->dcf_odentry->bSubCount){
 123   while (d->dcf_request>0){
 124         if(d->dcf_odentry->pSubindex[nodeId].bAccessType & DCF_TO_SEND){
 125         UNS8* dcfend;
 126                 UNS32 nb_entries;
 127                 UNS8 szData = d->dcf_odentry->pSubindex[nodeId].size;
 128
 129                 {
 130                         UNS8* dcf = *((UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject);
 131                         dcfend = dcf + szData;
 132                         if (!d->dcf_cursor){
 133                         d->dcf_cursor = (UNS8*)dcf + 4;
 134                         d->dcf_entries_count = 0;
 135                         }
 136                         nb_entries = UNS32_LE(*((UNS32*)dcf));
 137                 }
 138
 139         /* condition on consise DCF string for NodeID, if big enough */
 140         if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries){
 141
 142                 UNS16 target_Index;
 143                 UNS8 target_Subindex;
 144                 UNS32 target_Size;
 145
 146                         /* DCF data may not be 32/16b aligned,
 147                          * we cannot directly dereference d->dcf_cursor
 148                          * as UNS16 or UNS32
 149                          * Do it byte per byte taking care on endianess*/
 150 #ifdef CANOPEN_BIG_ENDIAN
 151                 target_Index = *(d->dcf_cursor++) << 8 |
 152                                *(d->dcf_cursor++);
 153 #else
 154                 memcpy(&target_Index, d->dcf_cursor,2);
 155                 d->dcf_cursor+=2;
 156 #endif
 157
 158                 target_Subindex = *(d->dcf_cursor++);
 159
 160 #ifdef CANOPEN_BIG_ENDIAN
 161                 target_Size = *(d->dcf_cursor++) << 24 |
 162                               *(d->dcf_cursor++) << 16 |
 163                               *(d->dcf_cursor++) << 8 |
 164                               *(d->dcf_cursor++);
 165 #else
 166                 memcpy(&target_Size, d->dcf_cursor,4);
 167                 d->dcf_cursor+=4;
 168 #endif
 169
 170             _writeNetworkDict(d, /* CO_Data* d*/
 171                                 nodeId, /* UNS8 nodeId*/
 172                                 target_Index, /* UNS16 index*/
 173                                 target_Subindex, /* UNS8 subindex*/
 174                                 target_Size, /* UNS8 count*/
 175                                 0, /* UNS8 dataType*/
 176                                 d->dcf_cursor,/* void *data*/
 177                                 CheckSDOAndContinue,/* SDOCallback_t
 178                                                       Callback*/
 179                                 0); /* no endianize*/
 180                 /* Push d->dcf_cursor to the end of data*/
 181
 182                 d->dcf_cursor += target_Size;
 183                 d->dcf_entries_count++;
 184
 185                 /* send_consise_dcf_loop will be called by CheckSDOAndContinue for next DCF entry*/
 186                 return;
 187         }
 188         else
 189         {
 190                 /* We have finished with the dcf entry. Change the flag, decrement the request
 191                  *  and execute the bootup callback. */
 192                 d->dcf_odentry->pSubindex[nodeId].bAccessType&=~DCF_TO_SEND;
 193                 d->dcf_request--;
 194                 (*d->post_SlaveBootup)(d, nodeId);
 195         }
 196         }
 197
 198         SEEK_NEXT_DCF()
 199   }
 200
 201 }