greg@178: /* nico@207: This file is part of CanFestival, a library implementing CanOpen nico@207: Stack. greg@178: nico@207: Copyright (C): Edouard TISSERANT and Francis DUPIN greg@178: nico@207: See COPYING file for copyrights details. greg@178: nico@207: This library is free software; you can redistribute it and/or nico@207: modify it under the terms of the GNU Lesser General Public nico@207: License as published by the Free Software Foundation; either nico@207: version 2.1 of the License, or (at your option) any later version. greg@178: nico@207: This library is distributed in the hope that it will be useful, nico@207: but WITHOUT ANY WARRANTY; without even the implied warranty of nico@207: MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU nico@207: Lesser General Public License for more details. greg@178: nico@207: You should have received a copy of the GNU Lesser General Public nico@207: License along with this library; if not, write to the Free Software nico@207: Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 nico@207: USA greg@178: */ greg@178: nico@210: nico@208: /** nico@207: ** @file dcf.c nico@207: ** @author Edouard TISSERANT and Francis DUPIN nico@207: ** @date Mon Jun 4 17:06:12 2007 nico@207: ** nico@208: ** @brief EXEMPLE OF SOMMARY nico@207: ** nico@207: ** nico@207: */ nico@215: nico@215: etisserant@378: #include "data.h" etisserant@200: #include "sysdep.h" greg@178: nico@207: extern UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, nico@207: UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize); etisserant@204: greg@178: groke6@349: static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId); groke6@349: etisserant@478: /* Seek to next NodeID's DCF */ etisserant@478: #define SEEK_NEXT_DCF() \ etisserant@478: nodeId=(nodeId+1) % d->dcf_odentry->bSubCount; \ etisserant@478: if(nodeId==0) nodeId=1; \ etisserant@478: d->dcf_cursor = NULL; etisserant@478: nico@208: /** nico@207: ** nico@207: ** nico@207: ** @param d nico@207: ** @param nodeId nico@208: */ greg@178: static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId) greg@178: { etisserant@478: UNS32 abortCode = 0; etisserant@191: nico@207: if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED) nico@207: { nico@207: MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode); nico@207: MSG_WAR(0x2A02, "server node : ", nodeId); nico@207: } nico@207: nico@207: closeSDOtransfer(d, nodeId, SDO_CLIENT); etisserant@478: /* Timedout ? */ etisserant@478: if(abortCode == SDOABT_TIMED_OUT){ etisserant@478: /* Node may not be ready, try another one */ etisserant@478: /* Warning, this might leed to endless attempts */ etisserant@478: /* if node does never answer */ etisserant@478: SEEK_NEXT_DCF() etisserant@478: } groke6@349: send_consise_dcf_loop(d,nodeId); greg@178: } greg@178: groke6@349: nico@208: /** nico@207: ** nico@207: ** nico@207: ** @param d nico@207: ** @param nodeId nico@207: ** nico@207: ** @return nico@208: */ groke6@349: UNS8 send_consise_dcf(CO_Data* d,UNS8 nodeId) greg@178: { greg@587: UNS32 szData; etisserant@320: /* Fetch DCF OD entry, if not already done */ etisserant@320: if(!d->dcf_odentry) etisserant@320: { etisserant@320: UNS32 errorCode; etisserant@320: ODCallback_t *Callback; etisserant@320: d->dcf_odentry = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback); etisserant@320: /* If DCF entry do not exist... Nothing to do.*/ etisserant@320: if (errorCode != OD_SUCCESSFUL) goto DCF_finish; etisserant@320: } greg@178: groke6@361: szData = d->dcf_odentry->pSubindex[nodeId].size; groke6@361: groke6@349: /* if the entry for the nodeId is not empty. */ groke6@349: if(szData!=0){ groke6@349: /* if the entry for the nodeId is already been processing, quit.*/ groke6@349: if(d->dcf_odentry->pSubindex[nodeId].bAccessType & DCF_TO_SEND) return 1; groke6@349: groke6@349: d->dcf_odentry->pSubindex[nodeId].bAccessType|=DCF_TO_SEND; groke6@349: d->dcf_request++; groke6@349: if(d->dcf_request==1) groke6@349: send_consise_dcf_loop(d,nodeId); groke6@349: return 1; groke6@349: } groke6@349: groke6@349: DCF_finish: groke6@349: return 0; groke6@349: } greg@178: groke6@349: static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId) groke6@349: { greg@609: if(nodeId > d->dcf_odentry->bSubCount) return; greg@609: /* Loop on all DCF subindexes, corresponding to node ID until there is no request*/ groke6@349: //while (nodeId < d->dcf_odentry->bSubCount){ groke6@349: while (d->dcf_request>0){ groke6@349: if(d->dcf_odentry->pSubindex[nodeId].bAccessType & DCF_TO_SEND){ etisserant@368: UNS8* dcfend; groke6@349: UNS32 nb_entries; greg@587: UNS32 szData = d->dcf_odentry->pSubindex[nodeId].size; groke6@349: groke6@349: { groke6@349: UNS8* dcf = *((UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject); groke6@349: dcfend = dcf + szData; groke6@349: if (!d->dcf_cursor){ groke6@349: d->dcf_cursor = (UNS8*)dcf + 4; groke6@349: d->dcf_entries_count = 0; groke6@349: } groke6@349: nb_entries = UNS32_LE(*((UNS32*)dcf)); groke6@349: } nico@207: groke6@349: /* condition on consise DCF string for NodeID, if big enough */ groke6@349: if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries){ groke6@349: groke6@349: UNS16 target_Index; groke6@349: UNS8 target_Subindex; groke6@349: UNS32 target_Size; nico@207: etisserant@474: /* DCF data may not be 32/16b aligned, etisserant@474: * we cannot directly dereference d->dcf_cursor etisserant@474: * as UNS16 or UNS32 etisserant@474: * Do it byte per byte taking care on endianess*/ etisserant@474: #ifdef CANOPEN_BIG_ENDIAN etisserant@474: target_Index = *(d->dcf_cursor++) << 8 | etisserant@474: *(d->dcf_cursor++); etisserant@474: #else etisserant@476: memcpy(&target_Index, d->dcf_cursor,2); etisserant@474: d->dcf_cursor+=2; etisserant@474: #endif etisserant@474: etisserant@474: target_Subindex = *(d->dcf_cursor++); etisserant@474: etisserant@474: #ifdef CANOPEN_BIG_ENDIAN etisserant@474: target_Size = *(d->dcf_cursor++) << 24 | etisserant@474: *(d->dcf_cursor++) << 16 | etisserant@474: *(d->dcf_cursor++) << 8 | etisserant@474: *(d->dcf_cursor++); etisserant@474: #else etisserant@474: memcpy(&target_Size, d->dcf_cursor,4); etisserant@474: d->dcf_cursor+=4; etisserant@474: #endif groke6@349: groke6@349: _writeNetworkDict(d, /* CO_Data* d*/ nico@215: nodeId, /* UNS8 nodeId*/ nico@215: target_Index, /* UNS16 index*/ nico@215: target_Subindex, /* UNS8 subindex*/ greg@587: (UNS8)target_Size, /* UNS8 count*/ nico@215: 0, /* UNS8 dataType*/ nico@215: d->dcf_cursor,/* void *data*/ nico@215: CheckSDOAndContinue,/* SDOCallback_t nico@207: Callback*/ nico@215: 0); /* no endianize*/ groke6@349: /* Push d->dcf_cursor to the end of data*/ nico@207: groke6@349: d->dcf_cursor += target_Size; groke6@349: d->dcf_entries_count++; nico@207: groke6@349: /* send_consise_dcf_loop will be called by CheckSDOAndContinue for next DCF entry*/ groke6@349: return; groke6@349: } groke6@349: else groke6@349: { groke6@349: /* We have finished with the dcf entry. Change the flag, decrement the request groke6@349: * and execute the bootup callback. */ groke6@349: d->dcf_odentry->pSubindex[nodeId].bAccessType&=~DCF_TO_SEND; groke6@349: d->dcf_request--; etisserant@378: (*d->post_SlaveBootup)(d, nodeId); groke6@349: } groke6@349: } groke6@353: etisserant@478: SEEK_NEXT_DCF() nico@207: } etisserant@378: greg@178: }