src/dcf.c
author Christian Taedcke <hacking@taedcke.com>
Thu, 22 Dec 2011 11:43:08 +0100
changeset 678 c1b9455929e6
parent 671 06b30f679531
child 699 c26ea35559f8
permissions -rw-r--r--
FIXED: - adapted paths for current vscom driver
- removed chmod +x vs_can_api.dll;
- added option -o to unzip command to overwrite existing files
178
01d81faa3350 Add concise DCF support
greg
parents:
diff changeset
     1
/*
207
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
     2
  This file is part of CanFestival, a library implementing CanOpen
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
     3
  Stack.
178
01d81faa3350 Add concise DCF support
greg
parents:
diff changeset
     4
207
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
     5
  Copyright (C): Edouard TISSERANT and Francis DUPIN
178
01d81faa3350 Add concise DCF support
greg
parents:
diff changeset
     6
207
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
     7
  See COPYING file for copyrights details.
178
01d81faa3350 Add concise DCF support
greg
parents:
diff changeset
     8
207
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
     9
  This library is free software; you can redistribute it and/or
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    10
  modify it under the terms of the GNU Lesser General Public
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    11
  License as published by the Free Software Foundation; either
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    12
  version 2.1 of the License, or (at your option) any later version.
178
01d81faa3350 Add concise DCF support
greg
parents:
diff changeset
    13
207
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    14
  This library is distributed in the hope that it will be useful,
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    15
  but WITHOUT ANY WARRANTY; without even the implied warranty of
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    16
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    17
  Lesser General Public License for more details.
178
01d81faa3350 Add concise DCF support
greg
parents:
diff changeset
    18
207
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    19
  You should have received a copy of the GNU Lesser General Public
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    20
  License along with this library; if not, write to the Free Software
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    21
  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    22
  USA
178
01d81faa3350 Add concise DCF support
greg
parents:
diff changeset
    23
*/
01d81faa3350 Add concise DCF support
greg
parents:
diff changeset
    24
210
6b5a4c21e061 Manual and Documentation finish
nico
parents: 208
diff changeset
    25
208
05d95c45b388 Manual convertion -> latex -> pdf
nico
parents: 207
diff changeset
    26
/**
207
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    27
** @file   dcf.c
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    28
** @author Edouard TISSERANT and Francis DUPIN
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    29
** @date   Mon Jun  4 17:06:12 2007
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    30
**
208
05d95c45b388 Manual convertion -> latex -> pdf
nico
parents: 207
diff changeset
    31
** @brief EXEMPLE OF SOMMARY
207
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    32
**
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    33
**
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    34
*/
215
f49e5a6b7804 Manual and Documentation finish
nico
parents: 212
diff changeset
    35
f49e5a6b7804 Manual and Documentation finish
nico
parents: 212
diff changeset
    36
378
d2abf6c8c27b As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents: 368
diff changeset
    37
#include "data.h"
200
930be51ddc24 Big Endian fixes.
etisserant
parents: 191
diff changeset
    38
#include "sysdep.h"
669
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    39
#include "dcf.h"
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    40
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    41
typedef struct {
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    42
    UNS16 Index;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    43
    UNS8 Subindex;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    44
    UNS32 Size;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    45
    UNS8 *Data;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    46
} dcf_entry_t;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    47
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    48
void SaveNode(CO_Data* d, UNS8 nodeId);
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    49
static UNS8 read_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId);
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    50
static UNS8 write_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId);
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    51
UNS8 init_consise_dcf(CO_Data* d,UNS8 nodeId);
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    52
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    53
/**
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    54
** @brief Function to be called from post_SlaveBootup
207
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    55
**
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    56
** @param d
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    57
** @param nodeId
208
05d95c45b388 Manual convertion -> latex -> pdf
nico
parents: 207
diff changeset
    58
*/
669
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    59
UNS8 check_and_start_node(CO_Data* d, UNS8 nodeId)
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    60
{   
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    61
    if(d->dcf_status != DCF_STATUS_INIT)
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    62
        return 0;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    63
    if((init_consise_dcf(d, nodeId) == 0) || (read_consise_dcf_next_entry(d, nodeId) == 0)){
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    64
	    /* Ask slave node to go in operational mode */
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    65
        masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    66
        d->NMTable[nodeId] = Operational;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    67
        return 1;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    68
    }
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    69
    d->dcf_status = DCF_STATUS_READ_CHECK;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    70
    return 2;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    71
}
349
1e6dd4ef46b9 fixed the DCF-LSS conflict
groke6
parents: 320
diff changeset
    72
208
05d95c45b388 Manual convertion -> latex -> pdf
nico
parents: 207
diff changeset
    73
/**
207
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    74
**
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    75
**
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    76
** @param d
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
    77
** @param nodeId
669
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    78
*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    79
static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    80
{
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    81
    UNS32 abortCode = 0;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    82
    UNS8 buf[4], match = 0, node;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    83
    UNS32 size=4;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    84
    if(d->dcf_status == DCF_STATUS_READ_CHECK){
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    85
        // printf("DCF_STATUS_READ_CHECK \n");
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    86
        if(getReadResultNetworkDict (d, nodeId, buf, &size, &abortCode) != SDO_FINISHED)
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    87
            goto dcferror;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    88
        /* Check if data received match the DCF */
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    89
        if(size == d->dcf_size){
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    90
            match = 1;
671
06b30f679531 bug correction in dcf.c, an entry of only one byte was not checked
Mongo
parents: 669
diff changeset
    91
            while(size--)
669
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    92
                if(buf[size] != d->dcf_data[size])
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    93
                    match = 0;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    94
        }
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    95
        if(match) {
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    96
            if(read_consise_dcf_next_entry(d, nodeId) == 0){
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    97
                masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    98
                d->NMTable[nodeId] = Operational;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
    99
                d->dcf_status = DCF_STATUS_INIT;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   100
                /* Look for other nodes waiting to be started */
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   101
                for(node = 0 ; node<NMT_MAX_NODE_ID ; node++){
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   102
                    if(d->NMTable[node] != Initialisation)
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   103
                        continue;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   104
                    if(check_and_start_node(d, node) == 2)
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   105
                        break;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   106
                }
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   107
            }
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   108
        }
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   109
        else { /* Data received does not match : start rewriting all */
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   110
            if((init_consise_dcf(d, nodeId) == 0) || (write_consise_dcf_next_entry(d, nodeId) == 0))
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   111
                goto dcferror;                
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   112
            d->dcf_status = DCF_STATUS_WRITE;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   113
        }
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   114
    }
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   115
    else if(d->dcf_status == DCF_STATUS_WRITE){
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   116
        // printf("DCF_STATUS_WRITE \n");
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   117
        if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   118
            goto dcferror;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   119
        if(write_consise_dcf_next_entry(d, nodeId) == 0){
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   120
            SaveNode(d, nodeId);
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   121
            d->dcf_status = DCF_STATUS_SAVED;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   122
        }
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   123
    }
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   124
    else if(d->dcf_status == DCF_STATUS_SAVED){
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   125
        // printf("DCF_STATUS_SAVED \n");
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   126
        if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   127
            goto dcferror;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   128
        masterSendNMTstateChange (d, nodeId, NMT_Reset_Node);
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   129
        d->dcf_status = DCF_STATUS_INIT;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   130
        d->NMTable[nodeId] = Unknown_state;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   131
    }
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   132
    return;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   133
dcferror:
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   134
    MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode);
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   135
    MSG_WAR(0x2A02, "server node : ", nodeId);
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   136
    d->NMTable[nodeId] = Unknown_state;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   137
}
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   138
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   139
/**
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   140
**
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   141
**
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   142
** @param d
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   143
** @param nodeId
207
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
   144
**
b6572d0336c3 First doxygen implementation
nico
parents: 204
diff changeset
   145
** @return
208
05d95c45b388 Manual convertion -> latex -> pdf
nico
parents: 207
diff changeset
   146
*/
669
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   147
UNS8 init_consise_dcf(CO_Data* d,UNS8 nodeId)
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   148
{
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   149
    /* Fetch DCF OD entry */
320
f82e758840bd Some fixes suggested by Luis Jim?nez, plus some minor enhancements in DCF.
etisserant
parents: 251
diff changeset
   150
    UNS32 errorCode;
f82e758840bd Some fixes suggested by Luis Jim?nez, plus some minor enhancements in DCF.
etisserant
parents: 251
diff changeset
   151
    ODCallback_t *Callback;
f82e758840bd Some fixes suggested by Luis Jim?nez, plus some minor enhancements in DCF.
etisserant
parents: 251
diff changeset
   152
    d->dcf_odentry = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback);
f82e758840bd Some fixes suggested by Luis Jim?nez, plus some minor enhancements in DCF.
etisserant
parents: 251
diff changeset
   153
    /* If DCF entry do not exist... Nothing to do.*/
f82e758840bd Some fixes suggested by Luis Jim?nez, plus some minor enhancements in DCF.
etisserant
parents: 251
diff changeset
   154
    if (errorCode != OD_SUCCESSFUL) goto DCF_finish;
669
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   155
    /* Fix DCF table overflow */
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   156
    if(nodeId > d->dcf_odentry->bSubCount) goto DCF_finish;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   157
    /* If DCF empty... Nothing to do */
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   158
    if(! d->dcf_odentry->pSubindex[nodeId].size) goto DCF_finish;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   159
    UNS8* dcf = (UNS8*)d->dcf_odentry->pSubindex[nodeId].pObject;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   160
    // printf("%.2x %.2x %.2x %.2x\n",dcf[0],dcf[1],dcf[2],dcf[3]);
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   161
    d->dcf_cursor = dcf + 4;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   162
    d->dcf_entries_count = 0;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   163
    d->dcf_status = DCF_STATUS_INIT;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   164
    return 1;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   165
    DCF_finish:
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   166
    return 0;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   167
}
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   168
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   169
UNS8 get_next_DCF_data(CO_Data* d, dcf_entry_t *dcf_entry, UNS8 nodeId)
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   170
{
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   171
  UNS8* dcfend;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   172
  UNS32 nb_entries;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   173
  UNS32 szData;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   174
  if(!d->dcf_odentry)
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   175
     return 0;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   176
  if(nodeId > d->dcf_odentry->bSubCount)
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   177
     return 0;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   178
  szData = d->dcf_odentry->pSubindex[nodeId].size;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   179
  UNS8* dcf = (UNS8*)d->dcf_odentry->pSubindex[nodeId].pObject;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   180
  nb_entries = UNS32_LE(*((UNS32*)dcf));
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   181
  dcfend = dcf + szData;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   182
  if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries){
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   183
    /* DCF data may not be 32/16b aligned, 
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   184
    * we cannot directly dereference d->dcf_cursor 
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   185
    * as UNS16 or UNS32 
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   186
    * Do it byte per byte taking care on endianess*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   187
#ifdef CANOPEN_BIG_ENDIAN
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   188
    dcf_entry->Index = *(d->dcf_cursor++) << 8 | 
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   189
     	               *(d->dcf_cursor++);
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   190
#else
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   191
    memcpy(&dcf_entry->Index, d->dcf_cursor,2);
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   192
       	d->dcf_cursor+=2;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   193
#endif
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   194
    dcf_entry->Subindex = *(d->dcf_cursor++);
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   195
#ifdef CANOPEN_BIG_ENDIAN
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   196
    dcf_entry->Size = *(d->dcf_cursor++) << 24 | 
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   197
     	              *(d->dcf_cursor++) << 16 | 
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   198
        	          *(d->dcf_cursor++) << 8 | 
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   199
        	          *(d->dcf_cursor++);
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   200
#else
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   201
    memcpy(&dcf_entry->Size, d->dcf_cursor,4);
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   202
    d->dcf_cursor+=4;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   203
#endif
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   204
    d->dcf_data = dcf_entry->Data = d->dcf_cursor;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   205
    d->dcf_size = dcf_entry->Size;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   206
    d->dcf_cursor += dcf_entry->Size;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   207
    d->dcf_entries_count++;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   208
    return 1;
320
f82e758840bd Some fixes suggested by Luis Jim?nez, plus some minor enhancements in DCF.
etisserant
parents: 251
diff changeset
   209
  }
349
1e6dd4ef46b9 fixed the DCF-LSS conflict
groke6
parents: 320
diff changeset
   210
  return 0;
1e6dd4ef46b9 fixed the DCF-LSS conflict
groke6
parents: 320
diff changeset
   211
}
178
01d81faa3350 Add concise DCF support
greg
parents:
diff changeset
   212
669
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   213
static UNS8 write_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId)
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   214
{
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   215
    UNS8 Ret;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   216
    dcf_entry_t dcf_entry;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   217
    if(!get_next_DCF_data(d, &dcf_entry, nodeId))
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   218
        return 0;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   219
    Ret = writeNetworkDictCallBackAI(d, /* CO_Data* d*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   220
                    nodeId, /* UNS8 nodeId*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   221
                    dcf_entry.Index, /* UNS16 index*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   222
                    dcf_entry.Subindex, /* UNS8 subindex*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   223
                    (UNS8)dcf_entry.Size, /* UNS8 count*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   224
                    0, /* UNS8 dataType*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   225
                    dcf_entry.Data,/* void *data*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   226
                    CheckSDOAndContinue,/* Callback*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   227
                    0,   /* no endianize*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   228
                    0); /* no block mode */
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   229
    if(Ret)
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   230
        MSG_ERR(0x1A02,"Erreur writeNetworkDictCallBackAI",Ret);
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   231
    return 1;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   232
}
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   233
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   234
static UNS8 read_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId)
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   235
{
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   236
    UNS8 Ret;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   237
    dcf_entry_t dcf_entry;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   238
    if(!get_next_DCF_data(d, &dcf_entry, nodeId))
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   239
        return 0;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   240
    Ret = readNetworkDictCallbackAI(d, /* CO_Data* d*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   241
                   nodeId, /* UNS8 nodeId*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   242
                   dcf_entry.Index, /* UNS16 index*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   243
                   dcf_entry.Subindex, /* UNS8 subindex*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   244
                   0, /* UNS8 dataType*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   245
                   CheckSDOAndContinue,/* Callback*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   246
                   0); /* no block mode */
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   247
    if(Ret)
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   248
        MSG_ERR(0x1A03,"Erreur readNetworkDictCallbackAI",Ret);
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   249
    return 1;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   250
}
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   251
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   252
void SaveNode(CO_Data* d, UNS8 nodeId)
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   253
{
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   254
    UNS8 Ret;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   255
    UNS32 data=0x65766173;
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   256
    Ret = writeNetworkDictCallBackAI(d, /* CO_Data* d*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   257
                    nodeId, /* UNS8 nodeId*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   258
                    0x1010, /* UNS16 index*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   259
                    1, /* UNS8 subindex*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   260
                    4, /* UNS8 count*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   261
                    0, /* UNS8 dataType*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   262
                    (void *)&data,/* void *data*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   263
                    CheckSDOAndContinue,/* Callback*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   264
                    0,   /* no endianize*/
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   265
                    0); /* no block mode */
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   266
    if(Ret)
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   267
        MSG_ERR(0x1A04,"Erreur writeNetworkDictCallBackAI",Ret);
50da44ebaf00 Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
Mongo
parents: 668
diff changeset
   268
}