/*
  This file is part of CanFestival, a library implementing CanOpen
  Stack.

  Copyright (C): Edouard TISSERANT and Francis DUPIN

  See COPYING file for copyrights details.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
  USA
*/


/**
** @file   dcf.c
** @author Edouard TISSERANT and Francis DUPIN
** @date   Mon Jun  4 17:06:12 2007
**
** @brief EXEMPLE OF SOMMARY
**
**
*/


#include "data.h"
#include "sysdep.h"
#include "dcf.h"

typedef struct {
    UNS16 Index;
    UNS8 Subindex;
    UNS32 Size;
    UNS8 *Data;
} dcf_entry_t;

void SaveNode(CO_Data* d, UNS8 nodeId);
static UNS8 read_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId);
static UNS8 write_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId);
UNS8 init_consise_dcf(CO_Data* d,UNS8 nodeId);


#ifdef _MSC_VER
#define inline _inline
#endif  /* _MSC_VER */


inline void start_node(CO_Data* d, UNS8 nodeId){
    /* Ask slave node to go in operational mode */
    masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
    d->NMTable[nodeId] = Operational;
}

/**
** @brief Function to be called from post_SlaveBootup
**
** @param d
** @param nodeId
*/
UNS8 check_and_start_node(CO_Data* d, UNS8 nodeId)
{   
    if(d->dcf_status != DCF_STATUS_INIT)
        return 0;
    if((init_consise_dcf(d, nodeId) == 0) || (read_consise_dcf_next_entry(d, nodeId) == 0)){
        start_node(d, nodeId);
        return 1;
    }
    d->dcf_status = DCF_STATUS_READ_CHECK;
    return 2;
}

inline void start_and_seek_node(CO_Data* d, UNS8 nodeId){
   UNS8 node;
   start_node(d,nodeId);
   /* Look for other nodes waiting to be started */
   for(node = 0 ; node<NMT_MAX_NODE_ID ; node++){
       if(d->NMTable[node] != Initialisation)
           continue;
       if(check_and_start_node(d, node) == 2)
           return;
   }
   /* No more node to start. Let's start our own node */
   setState(d, Operational);
}

/**
**
**
** @param d
** @param nodeId
*/
static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
{
    UNS32 abortCode = 0;
    UNS8 buf[4], match = 0, node;
    UNS32 size=4;
    if(d->dcf_status == DCF_STATUS_READ_CHECK){
        // printf("DCF_STATUS_READ_CHECK \n");
        if(getReadResultNetworkDict (d, nodeId, buf, &size, &abortCode) != SDO_FINISHED)
            goto dcferror;
        /* Check if data received match the DCF */
        if(size == d->dcf_size){
            match = 1;
            while(size--)
                if(buf[size] != d->dcf_data[size])
                    match = 0;
        }
        if(match) {
            if(read_consise_dcf_next_entry(d, nodeId) == 0){
                start_and_seek_node(d, nodeId);
            }
        }
        else { /* Data received does not match : start rewriting all */
            if((init_consise_dcf(d, nodeId) == 0) || (write_consise_dcf_next_entry(d, nodeId) == 0))
                goto dcferror;                
            d->dcf_status = DCF_STATUS_WRITE;
        }
    }
    else if(d->dcf_status == DCF_STATUS_WRITE){
        // printf("DCF_STATUS_WRITE \n");
        if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
            goto dcferror;
        if(write_consise_dcf_next_entry(d, nodeId) == 0){
#ifdef DCF_SAVE_NODE
            SaveNode(d, nodeId);
            d->dcf_status = DCF_STATUS_SAVED;
#else //DCF_SAVE_NODE
            d->dcf_status = DCF_STATUS_INIT;
           start_and_seek_node(d,nodeId);
#endif //DCF_SAVE_NODE
        }
    }
    else if(d->dcf_status == DCF_STATUS_SAVED){
        // printf("DCF_STATUS_SAVED \n");
        if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
            goto dcferror;
        masterSendNMTstateChange (d, nodeId, NMT_Reset_Node);
        d->dcf_status = DCF_STATUS_INIT;
        d->NMTable[nodeId] = Unknown_state;
    }
    return;
dcferror:
    MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode);
    MSG_WAR(0x2A02, "server node : ", nodeId);
    d->NMTable[nodeId] = Unknown_state;
}

/**
**
**
** @param d
** @param nodeId
**
** @return
*/
UNS8 init_consise_dcf(CO_Data* d,UNS8 nodeId)
{
    /* Fetch DCF OD entry */
    UNS32 errorCode;
    ODCallback_t *Callback;
    UNS8* dcf;
    d->dcf_odentry = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback);
    /* If DCF entry do not exist... Nothing to do.*/
    if (errorCode != OD_SUCCESSFUL) goto DCF_finish;
    /* Fix DCF table overflow */
    if(nodeId > d->dcf_odentry->bSubCount) goto DCF_finish;
    /* If DCF empty... Nothing to do */
    if(! d->dcf_odentry->pSubindex[nodeId].size) goto DCF_finish;
    dcf = *(UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject;
    // printf("%.2x %.2x %.2x %.2x\n",dcf[0],dcf[1],dcf[2],dcf[3]);
    d->dcf_cursor = dcf + 4;
    d->dcf_entries_count = 0;
    d->dcf_status = DCF_STATUS_INIT;
    return 1;
    DCF_finish:
    return 0;
}

UNS8 get_next_DCF_data(CO_Data* d, dcf_entry_t *dcf_entry, UNS8 nodeId)
{
  UNS8* dcfend;
  UNS32 nb_entries;
  UNS32 szData;
  UNS8* dcf;
  if(!d->dcf_odentry)
     return 0;
  if(nodeId > d->dcf_odentry->bSubCount)
     return 0;
  szData = d->dcf_odentry->pSubindex[nodeId].size;
  dcf = *(UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject;
  nb_entries = UNS32_LE(*((UNS32*)dcf));
  dcfend = dcf + szData;
  if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries){
    /* DCF data may not be 32/16b aligned, 
    * we cannot directly dereference d->dcf_cursor 
    * as UNS16 or UNS32 
    * Do it byte per byte taking care on endianess*/
#ifdef CANOPEN_BIG_ENDIAN
    dcf_entry->Index = *(d->dcf_cursor++) << 8 | 
     	               *(d->dcf_cursor++);
#else
    memcpy(&dcf_entry->Index, d->dcf_cursor,2);
       	d->dcf_cursor+=2;
#endif
    dcf_entry->Subindex = *(d->dcf_cursor++);
#ifdef CANOPEN_BIG_ENDIAN
    dcf_entry->Size = *(d->dcf_cursor++) << 24 | 
     	              *(d->dcf_cursor++) << 16 | 
        	          *(d->dcf_cursor++) << 8 | 
        	          *(d->dcf_cursor++);
#else
    memcpy(&dcf_entry->Size, d->dcf_cursor,4);
    d->dcf_cursor+=4;
#endif
    d->dcf_data = dcf_entry->Data = d->dcf_cursor;
    d->dcf_size = dcf_entry->Size;
    d->dcf_cursor += dcf_entry->Size;
    d->dcf_entries_count++;
    return 1;
  }
  return 0;
}

static UNS8 write_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId)
{
    UNS8 Ret;
    dcf_entry_t dcf_entry;
    if(!get_next_DCF_data(d, &dcf_entry, nodeId))
        return 0;
    Ret = writeNetworkDictCallBackAI(d, /* CO_Data* d*/
                    nodeId, /* UNS8 nodeId*/
                    dcf_entry.Index, /* UNS16 index*/
                    dcf_entry.Subindex, /* UNS8 subindex*/
                    (UNS8)dcf_entry.Size, /* UNS8 count*/
                    0, /* UNS8 dataType*/
                    dcf_entry.Data,/* void *data*/
                    CheckSDOAndContinue,/* Callback*/
                    0,   /* no endianize*/
                    0); /* no block mode */
    if(Ret)
        MSG_ERR(0x1A02,"Erreur writeNetworkDictCallBackAI",Ret);
    return 1;
}

static UNS8 read_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId)
{
    UNS8 Ret;
    dcf_entry_t dcf_entry;
    if(!get_next_DCF_data(d, &dcf_entry, nodeId))
        return 0;
    Ret = readNetworkDictCallbackAI(d, /* CO_Data* d*/
                   nodeId, /* UNS8 nodeId*/
                   dcf_entry.Index, /* UNS16 index*/
                   dcf_entry.Subindex, /* UNS8 subindex*/
                   0, /* UNS8 dataType*/
                   CheckSDOAndContinue,/* Callback*/
                   0); /* no block mode */
    if(Ret)
        MSG_ERR(0x1A03,"Erreur readNetworkDictCallbackAI",Ret);
    return 1;
}

void SaveNode(CO_Data* d, UNS8 nodeId)
{
    UNS8 Ret;
    UNS32 data=0x65766173;
    Ret = writeNetworkDictCallBackAI(d, /* CO_Data* d*/
                    nodeId, /* UNS8 nodeId*/
                    0x1010, /* UNS16 index*/
                    1, /* UNS8 subindex*/
                    4, /* UNS8 count*/
                    0, /* UNS8 dataType*/
                    (void *)&data,/* void *data*/
                    CheckSDOAndContinue,/* Callback*/
                    0,   /* no endianize*/
                    0); /* no block mode */
    if(Ret)
        MSG_ERR(0x1A04,"Erreur writeNetworkDictCallBackAI",Ret);
}