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

** @brief Function to be called from post_SlaveBootup

    55

**

    56

** @param d

    57

** @param nodeId

    58

*/

    59

UNS8 check_and_start_node(CO_Data* d, UNS8 nodeId)

    60

{   

    61

    if(d->dcf_status != DCF_STATUS_INIT)

    62

        return 0;

    63

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

    64

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

    65

        masterSendNMTstateChange (d, nodeId, NMT_Start_Node);

    66

        d->NMTable[nodeId] = Operational;

    67

        return 1;

    68

    }

    69

    d->dcf_status = DCF_STATUS_READ_CHECK;

    70

    return 2;

    71

}

    72

    73

/**

    74

**

    75

**

    76

** @param d

    77

** @param nodeId

    78

*/

    79

static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)

    80

{

    81

    UNS32 abortCode = 0;

    82

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

    83

    UNS32 size=4;

    84

    if(d->dcf_status == DCF_STATUS_READ_CHECK){

    85

        // printf("DCF_STATUS_READ_CHECK \n");

    86

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

    87

            goto dcferror;

    88

        /* Check if data received match the DCF */

    89

        if(size == d->dcf_size){

    90

            match = 1;

    91

            while(size--)

    92

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

    93

                    match = 0;

    94

        }

    95

        if(match) {

    96

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

    97

                masterSendNMTstateChange (d, nodeId, NMT_Start_Node);

    98

                d->NMTable[nodeId] = Operational;

    99

                d->dcf_status = DCF_STATUS_INIT;

   100

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

   101

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

   102

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

   103

                        continue;

   104

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

   105

                        break;

   106

                }

   107

            }

   108

        }

   109

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

   110

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

   111

                goto dcferror;                

   112

            d->dcf_status = DCF_STATUS_WRITE;

   113

        }

   114

    }

   115

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

   116

        // printf("DCF_STATUS_WRITE \n");

   117

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

   118

            goto dcferror;

   119

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

   120

#ifdef DCF_SAVE_NODE

   121

            SaveNode(d, nodeId);

   122

            d->dcf_status = DCF_STATUS_SAVED;

   123

#else //DCF_SAVE_NODE

   124

           masterSendNMTstateChange (d, nodeId, NMT_Start_Node);

   125

           d->NMTable[nodeId] = Operational;

   126

           d->dcf_status = DCF_STATUS_INIT;

   127

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

   128

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

   129

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

   130

                   continue;

   131

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

   132

                   break;

   133

           }

   134

#endif //DCF_SAVE_NODE

   135

        }

   136

    }

   137

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

   138

        // printf("DCF_STATUS_SAVED \n");

   139

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

   140

            goto dcferror;

   141

        masterSendNMTstateChange (d, nodeId, NMT_Reset_Node);

   142

        d->dcf_status = DCF_STATUS_INIT;

   143

        d->NMTable[nodeId] = Unknown_state;

   144

    }

   145

    return;

   146

dcferror:

   147

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

   148

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

   149

    d->NMTable[nodeId] = Unknown_state;

   150

}

   151

   152

/**

   153

**

   154

**

   155

** @param d

   156

** @param nodeId

   157

**

   158

** @return

   159

*/

   160

UNS8 init_consise_dcf(CO_Data* d,UNS8 nodeId)

   161

{

   162

    /* Fetch DCF OD entry */

   163

    UNS32 errorCode;

   164

    ODCallback_t *Callback;

   165

    UNS8* dcf;

   166

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

   167

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

   168

    if (errorCode != OD_SUCCESSFUL) goto DCF_finish;

   169

    /* Fix DCF table overflow */

   170

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

   171

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

   172

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

   173

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

   174

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

   175

    d->dcf_cursor = dcf + 4;

   176

    d->dcf_entries_count = 0;

   177

    d->dcf_status = DCF_STATUS_INIT;

   178

    return 1;

   179

    DCF_finish:

   180

    return 0;

   181

}

   182

   183

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

   184

{

   185

  UNS8* dcfend;

   186

  UNS32 nb_entries;

   187

  UNS32 szData;

   188

  UNS8* dcf;

   189

  if(!d->dcf_odentry)

   190

     return 0;

   191

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

   192

     return 0;

   193

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

   194

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

   195

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

   196

  dcfend = dcf + szData;

   197

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

   198

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

   199

    * we cannot directly dereference d->dcf_cursor 

   200

    * as UNS16 or UNS32 

   201

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

   202

#ifdef CANOPEN_BIG_ENDIAN

   203

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

   204

     	               *(d->dcf_cursor++);

   205

#else

   206

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

   207

       	d->dcf_cursor+=2;

   208

#endif

   209

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

   210

#ifdef CANOPEN_BIG_ENDIAN

   211

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

   212

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

   213

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

   214

        	          *(d->dcf_cursor++);

   215

#else

   216

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

   217

    d->dcf_cursor+=4;

   218

#endif

   219

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

   220

    d->dcf_size = dcf_entry->Size;

   221

    d->dcf_cursor += dcf_entry->Size;

   222

    d->dcf_entries_count++;

   223

    return 1;

   224

  }

   225

  return 0;

   226

}

   227

   228

static UNS8 write_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId)

   229

{

   230

    UNS8 Ret;

   231

    dcf_entry_t dcf_entry;

   232

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

   233

        return 0;

   234

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

   235

                    nodeId, /* UNS8 nodeId*/

   236

                    dcf_entry.Index, /* UNS16 index*/

   237

                    dcf_entry.Subindex, /* UNS8 subindex*/

   238

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

   239

                    0, /* UNS8 dataType*/

   240

                    dcf_entry.Data,/* void *data*/

   241

                    CheckSDOAndContinue,/* Callback*/

   242

                    0,   /* no endianize*/

   243

                    0); /* no block mode */

   244

    if(Ret)

   245

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

   246

    return 1;

   247

}

   248

   249

static UNS8 read_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId)

   250

{

   251

    UNS8 Ret;

   252

    dcf_entry_t dcf_entry;

   253

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

   254

        return 0;

   255

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

   256

                   nodeId, /* UNS8 nodeId*/

   257

                   dcf_entry.Index, /* UNS16 index*/

   258

                   dcf_entry.Subindex, /* UNS8 subindex*/

   259

                   0, /* UNS8 dataType*/

   260

                   CheckSDOAndContinue,/* Callback*/

   261

                   0); /* no block mode */

   262

    if(Ret)

   263

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

   264

    return 1;

   265

}

   266

   267

void SaveNode(CO_Data* d, UNS8 nodeId)

   268

{

   269

    UNS8 Ret;

   270

    UNS32 data=0x65766173;

   271

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

   272

                    nodeId, /* UNS8 nodeId*/

   273

                    0x1010, /* UNS16 index*/

   274

                    1, /* UNS8 subindex*/

   275

                    4, /* UNS8 count*/

   276

                    0, /* UNS8 dataType*/

   277

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

   278

                    CheckSDOAndContinue,/* Callback*/

   279

                    0,   /* no endianize*/

   280

                    0); /* no block mode */

   281

    if(Ret)

   282

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

   283

}