Luis Silvio Cordova Rivadeneira / Mbed 2 deprecated Can_open_masternode

Can_open_masternode

Dependencies:   mbed

framework/src/dcf.c@7:537bae5a6fc6, 2012-09-26 (annotated)

Committer:
sam_grove
Date:
Wed Sep 26 05:48:14 2012 +0000
Revision:
7:537bae5a6fc6
Parent:
0:9dd7c6129683 
Pushing the project into the new repo

Who changed what in which revision?

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