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 /**
  47 **
  48 **
  49 ** @param d
  50 ** @param nodeId
  51 */
  52 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
  53 {
  54   UNS32 abortCode;
  55
  56   if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
  57     {
  58       MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode);
  59       MSG_WAR(0x2A02, "server node : ", nodeId);
  60     }
  61
  62   closeSDOtransfer(d, nodeId, SDO_CLIENT);
  63   send_consise_dcf_loop(d,nodeId);
  64 }
  65
  66
  67 /**
  68 **
  69 **
  70 ** @param d
  71 ** @param nodeId
  72 **
  73 ** @return
  74 */
  75 UNS8 send_consise_dcf(CO_Data* d,UNS8 nodeId)
  76 {
  77   UNS8 szData;
  78   /* Fetch DCF OD entry, if not already done */
  79   if(!d->dcf_odentry)
  80   {
  81     UNS32 errorCode;
  82     ODCallback_t *Callback;
  83     d->dcf_odentry = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback);
  84     /* If DCF entry do not exist... Nothing to do.*/
  85     if (errorCode != OD_SUCCESSFUL) goto DCF_finish;
  86   }
  87
  88   szData = d->dcf_odentry->pSubindex[nodeId].size;
  89
  90   /* if the entry for the nodeId is not empty. */
  91   if(szData!=0){
  92         /* if the entry for the nodeId is already been processing, quit.*/
  93         if(d->dcf_odentry->pSubindex[nodeId].bAccessType & DCF_TO_SEND) return 1;
  94
  95         d->dcf_odentry->pSubindex[nodeId].bAccessType|=DCF_TO_SEND;
  96         d->dcf_request++;
  97         if(d->dcf_request==1)
  98                 send_consise_dcf_loop(d,nodeId);
  99         return 1;
 100   }
 101
 102   DCF_finish:
 103   return 0;
 104 }
 105
 106 static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId)
 107 {
 108   /* Loop on all DCF subindexes, corresponding to node ID until there is no request*/
 109   //while (nodeId < d->dcf_odentry->bSubCount){
 110   while (d->dcf_request>0){
 111         if(d->dcf_odentry->pSubindex[nodeId].bAccessType & DCF_TO_SEND){
 112         UNS8* dcfend;
 113                 UNS32 nb_entries;
 114                 UNS8 szData = d->dcf_odentry->pSubindex[nodeId].size;
 115
 116                 {
 117                         UNS8* dcf = *((UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject);
 118                         dcfend = dcf + szData;
 119                         if (!d->dcf_cursor){
 120                         d->dcf_cursor = (UNS8*)dcf + 4;
 121                         d->dcf_entries_count = 0;
 122                         }
 123                         nb_entries = UNS32_LE(*((UNS32*)dcf));
 124                 }
 125
 126         /* condition on consise DCF string for NodeID, if big enough */
 127         if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries){
 128
 129                 UNS16 target_Index;
 130                 UNS8 target_Subindex;
 131                 UNS32 target_Size;
 132
 133                         /* DCF data may not be 32/16b aligned,
 134                          * we cannot directly dereference d->dcf_cursor
 135                          * as UNS16 or UNS32
 136                          * Do it byte per byte taking care on endianess*/
 137 #ifdef CANOPEN_BIG_ENDIAN
 138                 target_Index = *(d->dcf_cursor++) << 8 |
 139                                *(d->dcf_cursor++);
 140 #else
 141                 memcpy(&target_Index, d->dcf_cursor,4);
 142                 d->dcf_cursor+=2;
 143 #endif
 144
 145                 target_Subindex = *(d->dcf_cursor++);
 146
 147 #ifdef CANOPEN_BIG_ENDIAN
 148                 target_Size = *(d->dcf_cursor++) << 24 |
 149                               *(d->dcf_cursor++) << 16 |
 150                               *(d->dcf_cursor++) << 8 |
 151                               *(d->dcf_cursor++);
 152 #else
 153                 memcpy(&target_Size, d->dcf_cursor,4);
 154                 d->dcf_cursor+=4;
 155 #endif
 156
 157             _writeNetworkDict(d, /* CO_Data* d*/
 158                                 nodeId, /* UNS8 nodeId*/
 159                                 target_Index, /* UNS16 index*/
 160                                 target_Subindex, /* UNS8 subindex*/
 161                                 target_Size, /* UNS8 count*/
 162                                 0, /* UNS8 dataType*/
 163                                 d->dcf_cursor,/* void *data*/
 164                                 CheckSDOAndContinue,/* SDOCallback_t
 165                                                       Callback*/
 166                                 0); /* no endianize*/
 167                 /* Push d->dcf_cursor to the end of data*/
 168
 169                 d->dcf_cursor += target_Size;
 170                 d->dcf_entries_count++;
 171
 172                 /* send_consise_dcf_loop will be called by CheckSDOAndContinue for next DCF entry*/
 173                 return;
 174         }
 175         else
 176         {
 177                 /* We have finished with the dcf entry. Change the flag, decrement the request
 178                  *  and execute the bootup callback. */
 179                 d->dcf_odentry->pSubindex[nodeId].bAccessType&=~DCF_TO_SEND;
 180                 d->dcf_request--;
 181                 (*d->post_SlaveBootup)(d, nodeId);
 182         }
 183         }
 184
 185         /* Check the next element*/
 186         nodeId=(nodeId+1) % d->dcf_odentry->bSubCount;
 187         if(nodeId==0)nodeId=1;
 188         d->dcf_cursor = NULL;
 189
 190   }
 191
 192 }