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: 
>@760: #ifdef _MSC_VER
>@760: #define inline _inline
>@760: #endif  /* _MSC_VER */
>@760: 
>@760: 
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
Laurent@756:             d->dcf_status = DCF_STATUS_INIT;
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: }