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"
Mongo@669: #include "dcf.h"
Mongo@669:
Mongo@669: typedef struct {
Mongo@669: UNS16 Index;
Mongo@669: UNS8 Subindex;
Mongo@669: UNS32 Size;
Mongo@669: UNS8 *Data;
Mongo@669: } dcf_entry_t;
Mongo@669:
Mongo@669: void SaveNode(CO_Data* d, UNS8 nodeId);
Mongo@669: static UNS8 read_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId);
Mongo@669: static UNS8 write_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId);
Mongo@669: UNS8 init_consise_dcf(CO_Data* d,UNS8 nodeId);
Mongo@669:
Edouard@742:
Edouard@742: inline void start_node(CO_Data* d, UNS8 nodeId){
Edouard@742: /* Ask slave node to go in operational mode */
Edouard@742: masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
Edouard@742: d->NMTable[nodeId] = Operational;
Edouard@742: }
Edouard@742:
Mongo@669: /**
Mongo@669: ** @brief Function to be called from post_SlaveBootup
nico@207: **
nico@207: ** @param d
nico@207: ** @param nodeId
nico@208: */
Mongo@669: UNS8 check_and_start_node(CO_Data* d, UNS8 nodeId)
Mongo@669: {
Mongo@669: if(d->dcf_status != DCF_STATUS_INIT)
Mongo@669: return 0;
Mongo@669: if((init_consise_dcf(d, nodeId) == 0) || (read_consise_dcf_next_entry(d, nodeId) == 0)){
Edouard@742: start_node(d, nodeId);
Mongo@669: return 1;
Mongo@669: }
Mongo@669: d->dcf_status = DCF_STATUS_READ_CHECK;
Mongo@669: return 2;
Mongo@669: }
groke6@349:
Edouard@742: inline void start_and_seek_node(CO_Data* d, UNS8 nodeId){
Edouard@742: UNS8 node;
Edouard@742: start_node(d,nodeId);
Edouard@742: /* Look for other nodes waiting to be started */
Edouard@742: for(node = 0 ; node<NMT_MAX_NODE_ID ; node++){
Edouard@742: if(d->NMTable[node] != Initialisation)
Edouard@742: continue;
Edouard@742: if(check_and_start_node(d, node) == 2)
Edouard@742: return;
Edouard@742: }
Edouard@742: /* No more node to start. Let's start our own node */
Edouard@742: setState(d, Operational);
Edouard@742: }
Edouard@742:
nico@208: /**
nico@207: **
nico@207: **
nico@207: ** @param d
nico@207: ** @param nodeId
Mongo@669: */
Mongo@669: static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
Mongo@669: {
Mongo@669: UNS32 abortCode = 0;
Mongo@669: UNS8 buf[4], match = 0, node;
Mongo@669: UNS32 size=4;
Mongo@669: if(d->dcf_status == DCF_STATUS_READ_CHECK){
Mongo@669: // printf("DCF_STATUS_READ_CHECK \n");
Mongo@669: if(getReadResultNetworkDict (d, nodeId, buf, &size, &abortCode) != SDO_FINISHED)
Mongo@669: goto dcferror;
Mongo@669: /* Check if data received match the DCF */
Mongo@669: if(size == d->dcf_size){
Mongo@669: match = 1;
Mongo@671: while(size--)
Mongo@669: if(buf[size] != d->dcf_data[size])
Mongo@669: match = 0;
Mongo@669: }
Mongo@669: if(match) {
Mongo@669: if(read_consise_dcf_next_entry(d, nodeId) == 0){
Edouard@742: start_and_seek_node(d, nodeId);
Mongo@669: }
Mongo@669: }
Mongo@669: else { /* Data received does not match : start rewriting all */
Mongo@669: if((init_consise_dcf(d, nodeId) == 0) || (write_consise_dcf_next_entry(d, nodeId) == 0))
Mongo@669: goto dcferror;
Mongo@669: d->dcf_status = DCF_STATUS_WRITE;
Mongo@669: }
Mongo@669: }
Mongo@669: else if(d->dcf_status == DCF_STATUS_WRITE){
Mongo@669: // printf("DCF_STATUS_WRITE \n");
Mongo@669: if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
Mongo@669: goto dcferror;
Mongo@669: if(write_consise_dcf_next_entry(d, nodeId) == 0){
hacking@699: #ifdef DCF_SAVE_NODE
Mongo@669: SaveNode(d, nodeId);
Mongo@669: d->dcf_status = DCF_STATUS_SAVED;
hacking@699: #else //DCF_SAVE_NODE
Edouard@742: start_and_seek_node(d,nodeId);
hacking@699: #endif //DCF_SAVE_NODE
Mongo@669: }
Mongo@669: }
Mongo@669: else if(d->dcf_status == DCF_STATUS_SAVED){
Mongo@669: // printf("DCF_STATUS_SAVED \n");
Mongo@669: if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
Mongo@669: goto dcferror;
Mongo@669: masterSendNMTstateChange (d, nodeId, NMT_Reset_Node);
Mongo@669: d->dcf_status = DCF_STATUS_INIT;
Mongo@669: d->NMTable[nodeId] = Unknown_state;
Mongo@669: }
Mongo@669: return;
Mongo@669: dcferror:
Mongo@669: MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode);
Mongo@669: MSG_WAR(0x2A02, "server node : ", nodeId);
Mongo@669: d->NMTable[nodeId] = Unknown_state;
Mongo@669: }
Mongo@669:
Mongo@669: /**
Mongo@669: **
Mongo@669: **
Mongo@669: ** @param d
Mongo@669: ** @param nodeId
nico@207: **
nico@207: ** @return
nico@208: */
Mongo@669: UNS8 init_consise_dcf(CO_Data* d,UNS8 nodeId)
Mongo@669: {
Mongo@669: /* Fetch DCF OD entry */
etisserant@320: UNS32 errorCode;
etisserant@320: ODCallback_t *Callback;
hacking@699: UNS8* dcf;
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;
Mongo@669: /* Fix DCF table overflow */
Mongo@669: if(nodeId > d->dcf_odentry->bSubCount) goto DCF_finish;
Mongo@669: /* If DCF empty... Nothing to do */
Mongo@669: if(! d->dcf_odentry->pSubindex[nodeId].size) goto DCF_finish;
hacking@699: dcf = *(UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject;
Mongo@669: // printf("%.2x %.2x %.2x %.2x\n",dcf[0],dcf[1],dcf[2],dcf[3]);
Mongo@669: d->dcf_cursor = dcf + 4;
Mongo@669: d->dcf_entries_count = 0;
Mongo@669: d->dcf_status = DCF_STATUS_INIT;
Mongo@669: return 1;
Mongo@669: DCF_finish:
Mongo@669: return 0;
Mongo@669: }
Mongo@669:
Mongo@669: UNS8 get_next_DCF_data(CO_Data* d, dcf_entry_t *dcf_entry, UNS8 nodeId)
Mongo@669: {
Mongo@669: UNS8* dcfend;
Mongo@669: UNS32 nb_entries;
Mongo@669: UNS32 szData;
hacking@699: UNS8* dcf;
Mongo@669: if(!d->dcf_odentry)
Mongo@669: return 0;
Mongo@669: if(nodeId > d->dcf_odentry->bSubCount)
Mongo@669: return 0;
Mongo@669: szData = d->dcf_odentry->pSubindex[nodeId].size;
hacking@699: dcf = *(UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject;
Mongo@669: nb_entries = UNS32_LE(*((UNS32*)dcf));
Mongo@669: dcfend = dcf + szData;
Mongo@669: if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries){
Mongo@669: /* DCF data may not be 32/16b aligned,
Mongo@669: * we cannot directly dereference d->dcf_cursor
Mongo@669: * as UNS16 or UNS32
Mongo@669: * Do it byte per byte taking care on endianess*/
Mongo@669: #ifdef CANOPEN_BIG_ENDIAN
Mongo@669: dcf_entry->Index = *(d->dcf_cursor++) << 8 |
Mongo@669: *(d->dcf_cursor++);
Mongo@669: #else
Mongo@669: memcpy(&dcf_entry->Index, d->dcf_cursor,2);
Mongo@669: d->dcf_cursor+=2;
Mongo@669: #endif
Mongo@669: dcf_entry->Subindex = *(d->dcf_cursor++);
Mongo@669: #ifdef CANOPEN_BIG_ENDIAN
Mongo@669: dcf_entry->Size = *(d->dcf_cursor++) << 24 |
Mongo@669: *(d->dcf_cursor++) << 16 |
Mongo@669: *(d->dcf_cursor++) << 8 |
Mongo@669: *(d->dcf_cursor++);
Mongo@669: #else
Mongo@669: memcpy(&dcf_entry->Size, d->dcf_cursor,4);
Mongo@669: d->dcf_cursor+=4;
Mongo@669: #endif
Mongo@669: d->dcf_data = dcf_entry->Data = d->dcf_cursor;
Mongo@669: d->dcf_size = dcf_entry->Size;
Mongo@669: d->dcf_cursor += dcf_entry->Size;
Mongo@669: d->dcf_entries_count++;
Mongo@669: return 1;
etisserant@320: }
groke6@349: return 0;
groke6@349: }
greg@178:
Mongo@669: static UNS8 write_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId)
Mongo@669: {
Mongo@669: UNS8 Ret;
Mongo@669: dcf_entry_t dcf_entry;
Mongo@669: if(!get_next_DCF_data(d, &dcf_entry, nodeId))
Mongo@669: return 0;
Mongo@669: Ret = writeNetworkDictCallBackAI(d, /* CO_Data* d*/
Mongo@669: nodeId, /* UNS8 nodeId*/
Mongo@669: dcf_entry.Index, /* UNS16 index*/
Mongo@669: dcf_entry.Subindex, /* UNS8 subindex*/
Mongo@669: (UNS8)dcf_entry.Size, /* UNS8 count*/
Mongo@669: 0, /* UNS8 dataType*/
Mongo@669: dcf_entry.Data,/* void *data*/
Mongo@669: CheckSDOAndContinue,/* Callback*/
Mongo@669: 0, /* no endianize*/
Mongo@669: 0); /* no block mode */
Mongo@669: if(Ret)
Mongo@669: MSG_ERR(0x1A02,"Erreur writeNetworkDictCallBackAI",Ret);
Mongo@669: return 1;
Mongo@669: }
Mongo@669:
Mongo@669: static UNS8 read_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId)
Mongo@669: {
Mongo@669: UNS8 Ret;
Mongo@669: dcf_entry_t dcf_entry;
Mongo@669: if(!get_next_DCF_data(d, &dcf_entry, nodeId))
Mongo@669: return 0;
Mongo@669: Ret = readNetworkDictCallbackAI(d, /* CO_Data* d*/
Mongo@669: nodeId, /* UNS8 nodeId*/
Mongo@669: dcf_entry.Index, /* UNS16 index*/
Mongo@669: dcf_entry.Subindex, /* UNS8 subindex*/
Mongo@669: 0, /* UNS8 dataType*/
Mongo@669: CheckSDOAndContinue,/* Callback*/
Mongo@669: 0); /* no block mode */
Mongo@669: if(Ret)
Mongo@669: MSG_ERR(0x1A03,"Erreur readNetworkDictCallbackAI",Ret);
Mongo@669: return 1;
Mongo@669: }
Mongo@669:
Mongo@669: void SaveNode(CO_Data* d, UNS8 nodeId)
Mongo@669: {
Mongo@669: UNS8 Ret;
Mongo@669: UNS32 data=0x65766173;
Mongo@669: Ret = writeNetworkDictCallBackAI(d, /* CO_Data* d*/
Mongo@669: nodeId, /* UNS8 nodeId*/
Mongo@669: 0x1010, /* UNS16 index*/
Mongo@669: 1, /* UNS8 subindex*/
Mongo@669: 4, /* UNS8 count*/
Mongo@669: 0, /* UNS8 dataType*/
Mongo@669: (void *)&data,/* void *data*/
Mongo@669: CheckSDOAndContinue,/* Callback*/
Mongo@669: 0, /* no endianize*/
Mongo@669: 0); /* no block mode */
Mongo@669: if(Ret)
Mongo@669: MSG_ERR(0x1A04,"Erreur writeNetworkDictCallBackAI",Ret);
Mongo@669: }