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

** @file   lifegrd.c

    27

** @author Edouard TISSERANT

    28

** @date   Mon Jun  4 17:19:24 2007

    29

**

    30

** @brief

    31

**

    32

**

    33

*/

    34

    35

#include <data.h>

    36

#include "lifegrd.h"

    37

#include "canfestival.h"

    38

#include "dcf.h"

    39

#include "sysdep.h"

    40

    41

    42

void ConsumerHeartbeatAlarm(CO_Data* d, UNS32 id);

    43

void ProducerHeartbeatAlarm(CO_Data* d, UNS32 id);

    44

UNS32 OnHearbeatProducerUpdate(CO_Data* d, const indextable * unused_indextable, UNS8 unused_bSubindex);

    45

    46

void GuardTimeAlarm(CO_Data* d, UNS32 id);

    47

UNS32 OnNodeGuardUpdate(CO_Data* d, const indextable * unused_indextable, UNS8 unused_bSubindex);

    48

    49

    50

e_nodeState getNodeState (CO_Data* d, UNS8 nodeId)

    51

{

    52

  e_nodeState networkNodeState = Unknown_state;

    53

  #if NMT_MAX_NODE_ID>0

    54

  if(nodeId < NMT_MAX_NODE_ID)

    55

    networkNodeState = d->NMTable[nodeId];

    56

  #endif

    57

  return networkNodeState;

    58

}

    59

    60

/*! 

    61

** The Consumer Timer Callback

    62

**

    63

** @param d

    64

** @param id

    65

 * @ingroup heartbeato

    66

**/

    67

void ConsumerHeartbeatAlarm(CO_Data* d, UNS32 id)

    68

{

    69

  UNS8 nodeId = (UNS8)(((d->ConsumerHeartbeatEntries[id]) & (UNS32)0x00FF0000) >> (UNS8)16);

    70

  /*MSG_WAR(0x00, "ConsumerHearbeatAlarm", 0x00);*/

    71

    72

  /* timer have been notified and is now free (non periodic)*/

    73

  /* -> avoid deleting re-assigned timer if message is received too late*/

    74

  d->ConsumerHeartBeatTimers[id]=TIMER_NONE;

    75

    76

  /* set node state */

    77

  d->NMTable[nodeId] = Disconnected;

    78

  /*! call heartbeat error with NodeId */

    79

  (*d->heartbeatError)(d, nodeId);

    80

}

    81

    82

void proceedNODE_GUARD(CO_Data* d, Message* m )

    83

{

    84

  UNS8 nodeId = (UNS8) GET_NODE_ID((*m));

    85

    86

  if((m->rtr == 1) )

    87

    /*!

    88

    ** Notice that only the master can have sent this

    89

    ** node guarding request

    90

    */

    91

    {

    92

      /*!

    93

      ** Receiving a NMT NodeGuarding (request of the state by the

    94

      ** master)

    95

      ** Only answer to the NMT NodeGuarding request, the master is

    96

      ** not checked (not implemented)

    97

      */

    98

      if (nodeId == *d->bDeviceNodeId )

    99

        {

   100

          Message msg;

   101

          UNS16 tmp = *d->bDeviceNodeId + 0x700;

   102

          msg.cob_id = UNS16_LE(tmp);

   103

          msg.len = (UNS8)0x01;

   104

          msg.rtr = 0;

   105

          msg.data[0] = d->nodeState;

   106

          if (d->toggle)

   107

            {

   108

              msg.data[0] |= 0x80 ;

   109

              d->toggle = 0 ;

   110

            }

   111

          else

   112

            d->toggle = 1 ;

   113

          /* send the nodeguard response. */

   114

          MSG_WAR(0x3130, "Sending NMT Nodeguard to master, state: ", d->nodeState);

   115

          canSend(d->canHandle,&msg );

   116

        }

   117

   118

    }else{ /* Not a request CAN */

   119

      /* The state is stored on 7 bit */

   120

      e_nodeState newNodeState = (e_nodeState) ((*m).data[0] & 0x7F);

   121

   122

      MSG_WAR(0x3110, "Received NMT nodeId : ", nodeId);

   123

   124

      /*!

   125

      ** Record node response for node guarding service

   126

      */

   127

      d->nodeGuardStatus[nodeId] = *d->LifeTimeFactor;

   128

   129

      if (d->NMTable[nodeId] != newNodeState)

   130

      {

   131

        (*d->post_SlaveStateChange)(d, nodeId, newNodeState);

   132

        /* the slave's state receievd is stored in the NMTable */

   133

        d->NMTable[nodeId] = newNodeState;

   134

      }

   135

   136

      /* Boot-Up frame reception */

   137

      if ( d->NMTable[nodeId] == Initialisation)

   138

      {

   139

          /*

   140

          ** The device send the boot-up message (Initialisation)

   141

          ** to indicate the master that it is entered in

   142

          ** pre_operational mode

   143

          */

   144

          MSG_WAR(0x3100, "The NMT is a bootup from node : ", nodeId);

   145

          /* call post SlaveBootup with NodeId */

   146

		  (*d->post_SlaveBootup)(d, nodeId);

   147

      }

   148

   149

      if( d->NMTable[nodeId] != Unknown_state ) {

   150

        UNS8 index, ConsumerHeartBeat_nodeId ;

   151

        for( index = (UNS8)0x00; index < *d->ConsumerHeartbeatCount; index++ )

   152

          {

   153

            ConsumerHeartBeat_nodeId = (UNS8)( ((d->ConsumerHeartbeatEntries[index]) & (UNS32)0x00FF0000) >> (UNS8)16 );

   154

            if ( nodeId == ConsumerHeartBeat_nodeId )

   155

              {

   156

                TIMEVAL time = ( (d->ConsumerHeartbeatEntries[index]) & (UNS32)0x0000FFFF ) ;

   157

                /* Renew alarm for next heartbeat. */

   158

                DelAlarm(d->ConsumerHeartBeatTimers[index]);

   159

                d->ConsumerHeartBeatTimers[index] = SetAlarm(d, index, &ConsumerHeartbeatAlarm, MS_TO_TIMEVAL(time), 0);

   160

              }

   161

          }

   162

      }

   163

    }

   164

}

   165

   166

/*! The Producer Timer Callback

   167

**

   168

**

   169

** @param d

   170

** @param id

   171

 * @ingroup heartbeato

   172

**/

   173

void ProducerHeartbeatAlarm(CO_Data* d, UNS32 id)

   174

{

   175

  if(*d->ProducerHeartBeatTime)

   176

    {

   177

      Message msg;

   178

      /* Time expired, the heartbeat must be sent immediately

   179

      ** generate the correct node-id: this is done by the offset 1792

   180

      ** (decimal) and additionaly

   181

      ** the node-id of this device.

   182

      */

   183

      UNS16 tmp = *d->bDeviceNodeId + 0x700;

   184

      msg.cob_id = UNS16_LE(tmp);

   185

      msg.len = (UNS8)0x01;

   186

      msg.rtr = 0;

   187

      msg.data[0] = d->nodeState; /* No toggle for heartbeat !*/

   188

      /* send the heartbeat */

   189

      MSG_WAR(0x3130, "Producing heartbeat: ", d->nodeState);

   190

      canSend(d->canHandle,&msg );

   191

   192

    }else{

   193

      d->ProducerHeartBeatTimer = DelAlarm(d->ProducerHeartBeatTimer);

   194

    }

   195

}

   196

   197

/**

   198

 * @brief The guardTime - Timer Callback.

   199

 * 

   200

 * This function is called every GuardTime (OD 0x100C) ms <br>

   201

 * On every call, a NodeGuard-Request is sent to all nodes which have a

   202

 * node-state not equal to "Unknown" (according to NMTable). If the node has

   203

 * not responded within the lifetime, the nodeguardError function is called and

   204

 * the status of this node is set to "Disconnected"

   205

 *

   206

 * @param d 	Pointer on a CAN object data structure 

   207

 * @param id

   208

 * @ingroup nodeguardo

   209

 */

   210

void GuardTimeAlarm(CO_Data* d, UNS32 id)

   211

{

   212

  if (*d->GuardTime) {

   213

    UNS8 i;

   214

   215

    MSG_WAR(0x00, "Producing nodeguard-requests: ", 0);

   216

   217

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

   218

      /** Send node guard request to all nodes except this node, if the 

   219

      * node state is not "Unknown_state"

   220

      */

   221

      if (d->NMTable[i] != Unknown_state && i != *d->bDeviceNodeId) {

   222

   223

        /** Check if the node has confirmed the guarding request within

   224

        * the LifeTime (GuardTime x LifeTimeFactor)

   225

        */

   226

        if (d->nodeGuardStatus[i] <= 0) {

   227

   228

          MSG_WAR(0x00, "Node Guard alarm for nodeId : ", i);

   229

   230

          // Call error-callback function

   231

          if (*d->nodeguardError) {

   232

            (*d->nodeguardError)(d, i);

   233

          }

   234

   235

          // Mark node as disconnected

   236

          d->NMTable[i] = Disconnected;

   237

   238

        }

   239

   240

        d->nodeGuardStatus[i]--;

   241

   242

        masterSendNMTnodeguard(d, i);

   243

   244

      }

   245

    }

   246

  } else {

   247

    d->GuardTimeTimer = DelAlarm(d->GuardTimeTimer);

   248

  }

   249

   250

   251

   252

}

   253

   254

/**

   255

 * This function is called, if index 0x100C or 0x100D is updated to

   256

 * restart the node-guarding service with the new parameters

   257

 *

   258

 * @param d 	Pointer on a CAN object data structure 

   259

 * @param unused_indextable

   260

 * @param unused_bSubindex

   261

 * @ingroup nodeguardo

   262

 */

   263

UNS32 OnNodeGuardUpdate(CO_Data* d, const indextable * unused_indextable, UNS8 unused_bSubindex)

   264

{

   265

  nodeguardStop(d);

   266

  nodeguardInit(d);

   267

  return 0;

   268

}

   269

   270

   271

/*! This is called when Index 0x1017 is updated.

   272

**

   273

**

   274

** @param d

   275

** @param unused_indextable

   276

** @param unused_bSubindex

   277

**

   278

** @return

   279

 * @ingroup heartbeato

   280

**/

   281

UNS32 OnHeartbeatProducerUpdate(CO_Data* d, const indextable * unused_indextable, UNS8 unused_bSubindex)

   282

{

   283

  heartbeatStop(d);

   284

  heartbeatInit(d);

   285

  return 0;

   286

}

   287

   288

void heartbeatInit(CO_Data* d)

   289

{

   290

   291

  UNS8 index; /* Index to scan the table of heartbeat consumers */

   292

  RegisterSetODentryCallBack(d, 0x1017, 0x00, &OnHeartbeatProducerUpdate);

   293

   294

  d->toggle = 0;

   295

   296

  for( index = (UNS8)0x00; index < *d->ConsumerHeartbeatCount; index++ )

   297

    {

   298

      TIMEVAL time = (UNS16) ( (d->ConsumerHeartbeatEntries[index]) & (UNS32)0x0000FFFF ) ;

   299

      if ( time )

   300

        {

   301

          d->ConsumerHeartBeatTimers[index] = SetAlarm(d, index, &ConsumerHeartbeatAlarm, MS_TO_TIMEVAL(time), 0);

   302

        }

   303

    }

   304

   305

  if ( *d->ProducerHeartBeatTime )

   306

    {

   307

      TIMEVAL time = *d->ProducerHeartBeatTime;

   308

      d->ProducerHeartBeatTimer = SetAlarm(d, 0, &ProducerHeartbeatAlarm, MS_TO_TIMEVAL(time), MS_TO_TIMEVAL(time));

   309

    }

   310

}

   311

   312

   313

void nodeguardInit(CO_Data* d)

   314

{

   315

   316

  RegisterSetODentryCallBack(d, 0x100C, 0x00, &OnNodeGuardUpdate);

   317

  RegisterSetODentryCallBack(d, 0x100D, 0x00, &OnNodeGuardUpdate);

   318

   319

  if (*d->GuardTime && *d->LifeTimeFactor) {

   320

    UNS8 i;

   321

   322

    TIMEVAL time = *d->GuardTime;

   323

    d->GuardTimeTimer = SetAlarm(d, 0, &GuardTimeAlarm, MS_TO_TIMEVAL(time), MS_TO_TIMEVAL(time));

   324

    MSG_WAR(0x0, "GuardTime: ", time);

   325

   326

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

   327

      /** Set initial value for the nodes */

   328

      if (d->NMTable[i] != Unknown_state && i != *d->bDeviceNodeId) { 

   329

        d->nodeGuardStatus[i] = *d->LifeTimeFactor;

   330

      }

   331

    }

   332

   333

    MSG_WAR(0x0, "Timer for node-guarding startet", 0);

   334

  }

   335

   336

}

   337

   338

void heartbeatStop(CO_Data* d)

   339

{

   340

  UNS8 index;

   341

  for( index = (UNS8)0x00; index < *d->ConsumerHeartbeatCount; index++ )

   342

    {

   343

      d->ConsumerHeartBeatTimers[index] = DelAlarm(d->ConsumerHeartBeatTimers[index]);

   344

    }

   345

   346

  d->ProducerHeartBeatTimer = DelAlarm(d->ProducerHeartBeatTimer);

   347

}

   348

   349

void nodeguardStop(CO_Data* d)

   350

{

   351

  d->GuardTimeTimer = DelAlarm(d->GuardTimeTimer);

   352

}

   353

   354

   355

void lifeGuardInit(CO_Data* d)

   356

{

   357

  heartbeatInit(d);

   358

  nodeguardInit(d);

   359

}

   360

   361

   362

void lifeGuardStop(CO_Data* d)

   363

{

   364

  heartbeatStop(d);

   365

  nodeguardStop(d);

   366

}

   367

   368

   369

void _heartbeatError(CO_Data* d, UNS8 heartbeatID){}

   370

void _post_SlaveBootup(CO_Data* d, UNS8 SlaveID){}

   371

void _post_SlaveStateChange(CO_Data* d, UNS8 nodeId, e_nodeState newNodeState){}

   372

void _nodeguardError(CO_Data* d, UNS8 id){}

   373