1

/*

     2

  This file is part of CanFestival, a library implementing CanOpen

     3

  Stack.

     4

     5

  Copyright (C): Edouard TISSERANT and Francis DUPIN

     6

     7

  See COPYING file for copyrights details.

     8

     9

  This library is free software; you can redistribute it and/or

    10

  modify it under the terms of the GNU Lesser General Public

    11

  License as published by the Free Software Foundation; either

    12

  version 2.1 of the License, or (at your option) any later version.

    13

    14

  This library is distributed in the hope that it will be useful,

    15

  but WITHOUT ANY WARRANTY; without even the implied warranty of

    16

  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU

    17

  Lesser General Public License for more details.

    18

    19

  You should have received a copy of the GNU Lesser General Public

    20

  License along with this library; if not, write to the Free Software

    21

  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307

    22

  USA

    23

*/

    24

    25

    26

/**

    27

** @file   dcf.c

    28

** @author Edouard TISSERANT and Francis DUPIN

    29

** @date   Mon Jun  4 17:06:12 2007

    30

**

    31

** @brief EXEMPLE OF SOMMARY

    32

**

    33

**

    34

*/

    35

    36

    37

#include "data.h"

    38

#include "sysdep.h"

    39

#include "dcf.h"

    40

    41

typedef struct {

    42

    UNS16 Index;

    43

    UNS8 Subindex;

    44

    UNS32 Size;

    45

    UNS8 *Data;

    46

} dcf_entry_t;

    47

    48

void SaveNode(CO_Data* d, UNS8 nodeId);

    49

static UNS8 read_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId);

    50

static UNS8 write_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId);

    51

UNS8 init_consise_dcf(CO_Data* d,UNS8 nodeId);

    52

    53

    54

inline void start_node(CO_Data* d, UNS8 nodeId){

    55

    /* Ask slave node to go in operational mode */

    56

    masterSendNMTstateChange (d, nodeId, NMT_Start_Node);

    57

    d->NMTable[nodeId] = Operational;

    58

}

    59

    60

/**

    61

** @brief Function to be called from post_SlaveBootup

    62

**

    63

** @param d

    64

** @param nodeId

    65

*/

    66

UNS8 check_and_start_node(CO_Data* d, UNS8 nodeId)

    67

{   

    68

    if(d->dcf_status != DCF_STATUS_INIT)

    69

        return 0;

    70

    if((init_consise_dcf(d, nodeId) == 0) || (read_consise_dcf_next_entry(d, nodeId) == 0)){

    71

        start_node(d, nodeId);

    72

        return 1;

    73

    }

    74

    d->dcf_status = DCF_STATUS_READ_CHECK;

    75

    return 2;

    76

}

    77

    78

inline void start_and_seek_node(CO_Data* d, UNS8 nodeId){

    79

   UNS8 node;

    80

   start_node(d,nodeId);

    81

   /* Look for other nodes waiting to be started */

    82

   for(node = 0 ; node<NMT_MAX_NODE_ID ; node++){

    83

       if(d->NMTable[node] != Initialisation)

    84

           continue;

    85

       if(check_and_start_node(d, node) == 2)

    86

           return;

    87

   }

    88

   /* No more node to start. Let's start our own node */

    89

   setState(d, Operational);

    90

}

    91

    92

/**

    93

**

    94

**

    95

** @param d

    96

** @param nodeId

    97

*/

    98

static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)

    99

{

   100

    UNS32 abortCode = 0;

   101

    UNS8 buf[4], match = 0, node;

   102

    UNS32 size=4;

   103

    if(d->dcf_status == DCF_STATUS_READ_CHECK){

   104

        // printf("DCF_STATUS_READ_CHECK \n");

   105

        if(getReadResultNetworkDict (d, nodeId, buf, &size, &abortCode) != SDO_FINISHED)

   106

            goto dcferror;

   107

        /* Check if data received match the DCF */

   108

        if(size == d->dcf_size){

   109

            match = 1;

   110

            while(size--)

   111

                if(buf[size] != d->dcf_data[size])

   112

                    match = 0;

   113

        }

   114

        if(match) {

   115

            if(read_consise_dcf_next_entry(d, nodeId) == 0){

   116

                start_and_seek_node(d, nodeId);

   117

            }

   118

        }

   119

        else { /* Data received does not match : start rewriting all */

   120

            if((init_consise_dcf(d, nodeId) == 0) || (write_consise_dcf_next_entry(d, nodeId) == 0))

   121

                goto dcferror;                

   122

            d->dcf_status = DCF_STATUS_WRITE;

   123

        }

   124

    }

   125

    else if(d->dcf_status == DCF_STATUS_WRITE){

   126

        // printf("DCF_STATUS_WRITE \n");

   127

        if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)

   128

            goto dcferror;

   129

        if(write_consise_dcf_next_entry(d, nodeId) == 0){

   130

#ifdef DCF_SAVE_NODE

   131

            SaveNode(d, nodeId);

   132

            d->dcf_status = DCF_STATUS_SAVED;

   133

#else //DCF_SAVE_NODE

   135

           start_and_seek_node(d,nodeId);

   136

#endif //DCF_SAVE_NODE

   137

        }

   138

    }

   139

    else if(d->dcf_status == DCF_STATUS_SAVED){

   140

        // printf("DCF_STATUS_SAVED \n");

   141

        if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)

   142

            goto dcferror;

   143

        masterSendNMTstateChange (d, nodeId, NMT_Reset_Node);

   144

        d->dcf_status = DCF_STATUS_INIT;

   145

        d->NMTable[nodeId] = Unknown_state;

   146

    }

   147

    return;

   148

dcferror:

   149

    MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode);

   150

    MSG_WAR(0x2A02, "server node : ", nodeId);

   151

    d->NMTable[nodeId] = Unknown_state;

   152

}

   153

   154

/**

   155

**

   156

**

   157

** @param d

   158

** @param nodeId

   159

**

   160

** @return

   161

*/

   162

UNS8 init_consise_dcf(CO_Data* d,UNS8 nodeId)

   163

{

   164

    /* Fetch DCF OD entry */

   165

    UNS32 errorCode;

   166

    ODCallback_t *Callback;

   167

    UNS8* dcf;

   168

    d->dcf_odentry = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback);

   169

    /* If DCF entry do not exist... Nothing to do.*/

   170

    if (errorCode != OD_SUCCESSFUL) goto DCF_finish;

   171

    /* Fix DCF table overflow */

   172

    if(nodeId > d->dcf_odentry->bSubCount) goto DCF_finish;

   173

    /* If DCF empty... Nothing to do */

   174

    if(! d->dcf_odentry->pSubindex[nodeId].size) goto DCF_finish;

   175

    dcf = *(UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject;

   176

    // printf("%.2x %.2x %.2x %.2x\n",dcf[0],dcf[1],dcf[2],dcf[3]);

   177

    d->dcf_cursor = dcf + 4;

   178

    d->dcf_entries_count = 0;

   179

    d->dcf_status = DCF_STATUS_INIT;

   180

    return 1;

   181

    DCF_finish:

   182

    return 0;

   183

}

   184

   185

UNS8 get_next_DCF_data(CO_Data* d, dcf_entry_t *dcf_entry, UNS8 nodeId)

   186

{

   187

  UNS8* dcfend;

   188

  UNS32 nb_entries;

   189

  UNS32 szData;

   190

  UNS8* dcf;

   191

  if(!d->dcf_odentry)

   192

     return 0;

   193

  if(nodeId > d->dcf_odentry->bSubCount)

   194

     return 0;

   195

  szData = d->dcf_odentry->pSubindex[nodeId].size;

   196

  dcf = *(UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject;

   197

  nb_entries = UNS32_LE(*((UNS32*)dcf));

   198

  dcfend = dcf + szData;

   199

  if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries){

   200

    /* DCF data may not be 32/16b aligned, 

   201

    * we cannot directly dereference d->dcf_cursor 

   202

    * as UNS16 or UNS32 

   203

    * Do it byte per byte taking care on endianess*/

   204

#ifdef CANOPEN_BIG_ENDIAN

   205

    dcf_entry->Index = *(d->dcf_cursor++) << 8 | 

   206

     	               *(d->dcf_cursor++);

   207

#else

   208

    memcpy(&dcf_entry->Index, d->dcf_cursor,2);

   209

       	d->dcf_cursor+=2;

   210

#endif

   211

    dcf_entry->Subindex = *(d->dcf_cursor++);

   212

#ifdef CANOPEN_BIG_ENDIAN

   213

    dcf_entry->Size = *(d->dcf_cursor++) << 24 | 

   214

     	              *(d->dcf_cursor++) << 16 | 

   215

        	          *(d->dcf_cursor++) << 8 | 

   216

        	          *(d->dcf_cursor++);

   217

#else

   218

    memcpy(&dcf_entry->Size, d->dcf_cursor,4);

   219

    d->dcf_cursor+=4;

   220

#endif

   221

    d->dcf_data = dcf_entry->Data = d->dcf_cursor;

   222

    d->dcf_size = dcf_entry->Size;

   223

    d->dcf_cursor += dcf_entry->Size;

   224

    d->dcf_entries_count++;

   225

    return 1;

   226

  }

   227

  return 0;

   228

}

   229

   230

static UNS8 write_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId)

   231

{

   232

    UNS8 Ret;

   233

    dcf_entry_t dcf_entry;

   234

    if(!get_next_DCF_data(d, &dcf_entry, nodeId))

   235

        return 0;

   236

    Ret = writeNetworkDictCallBackAI(d, /* CO_Data* d*/

   237

                    nodeId, /* UNS8 nodeId*/

   238

                    dcf_entry.Index, /* UNS16 index*/

   239

                    dcf_entry.Subindex, /* UNS8 subindex*/

   240

                    (UNS8)dcf_entry.Size, /* UNS8 count*/

   241

                    0, /* UNS8 dataType*/

   242

                    dcf_entry.Data,/* void *data*/

   243

                    CheckSDOAndContinue,/* Callback*/

   244

                    0,   /* no endianize*/

   245

                    0); /* no block mode */

   246

    if(Ret)

   247

        MSG_ERR(0x1A02,"Erreur writeNetworkDictCallBackAI",Ret);

   248

    return 1;

   249

}

   250

   251

static UNS8 read_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId)

   252

{

   253

    UNS8 Ret;

   254

    dcf_entry_t dcf_entry;

   255

    if(!get_next_DCF_data(d, &dcf_entry, nodeId))

   256

        return 0;

   257

    Ret = readNetworkDictCallbackAI(d, /* CO_Data* d*/

   258

                   nodeId, /* UNS8 nodeId*/

   259

                   dcf_entry.Index, /* UNS16 index*/

   260

                   dcf_entry.Subindex, /* UNS8 subindex*/

   261

                   0, /* UNS8 dataType*/

   262

                   CheckSDOAndContinue,/* Callback*/

   263

                   0); /* no block mode */

   264

    if(Ret)

   265

        MSG_ERR(0x1A03,"Erreur readNetworkDictCallbackAI",Ret);

   266

    return 1;

   267

}

   268

   269

void SaveNode(CO_Data* d, UNS8 nodeId)

   270

{

   271

    UNS8 Ret;

   272

    UNS32 data=0x65766173;

   273

    Ret = writeNetworkDictCallBackAI(d, /* CO_Data* d*/

   274

                    nodeId, /* UNS8 nodeId*/

   275

                    0x1010, /* UNS16 index*/

   276

                    1, /* UNS8 subindex*/

   277

                    4, /* UNS8 count*/

   278

                    0, /* UNS8 dataType*/

   279

                    (void *)&data,/* void *data*/

   280

                    CheckSDOAndContinue,/* Callback*/

   281

                    0,   /* no endianize*/

   282

                    0); /* no block mode */

   283

    if(Ret)

   284

        MSG_ERR(0x1A04,"Erreur writeNetworkDictCallBackAI",Ret);

   285

}