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 ConsumerHearbeatAlarm(CO_Data* d, UNS32 id);

    43

    44

    45

void ProducerHearbeatAlarm(CO_Data* d, UNS32 id);

    46

    47

UNS32 OnHearbeatProducerUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex);

    48

    49

/*!

    50

**

    51

**

    52

** @param d

    53

** @param nodeId

    54

**

    55

** @return

    56

**/

    57

e_nodeState getNodeState (CO_Data* d, UNS8 nodeId)

    58

{

    59

  e_nodeState networkNodeState = Unknown_state;

    60

  #if NMT_MAX_NODE_ID>0

    61

  if(nodeId < NMT_MAX_NODE_ID)

    62

    networkNodeState = d->NMTable[nodeId];

    63

  #endif

    64

  return networkNodeState;

    65

}

    66

    67

/*! 

    68

** The Consumer Timer Callback

    69

**

    70

** @param d

    71

** @param id

    72

**/

    73

void ConsumerHearbeatAlarm(CO_Data* d, UNS32 id)

    74

{

    75

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

    76

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

    77

    78

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

    79

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

    80

  d->ConsumerHeartBeatTimers[id]=TIMER_NONE;

    81

    82

  /* set node state */

    83

  d->NMTable[nodeId] = Disconnected;

    84

  /*! call heartbeat error with NodeId */

    85

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

    86

}

    87

    88

/*!

    89

**

    90

**

    91

** @param d

    92

** @param m

    93

**/

    94

void proceedNODE_GUARD(CO_Data* d, Message* m )

    95

{

    96

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

    97

    98

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

    99

    /*!

   100

    ** Notice that only the master can have sent this

   101

    ** node guarding request

   102

    */

   103

    {

   104

      /*!

   105

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

   106

      ** master)

   107

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

   108

      ** not checked (not implemented)

   109

      */

   110

      if (nodeId == *d->bDeviceNodeId )

   111

        {

   112

          Message msg;

   113

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

   114

          msg.cob_id = UNS16_LE(tmp);

   115

          msg.len = (UNS8)0x01;

   116

          msg.rtr = 0;

   117

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

   118

          if (d->toggle)

   119

            {

   120

              msg.data[0] |= 0x80 ;

   121

              d->toggle = 0 ;

   122

            }

   123

          else

   124

            d->toggle = 1 ;

   125

          /* send the nodeguard response. */

   126

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

   127

          canSend(d->canHandle,&msg );

   128

        }

   129

   130

    }else{ /* Not a request CAN */

   131

      /* The state is stored on 7 bit */

   132

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

   133

   134

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

   135

   136

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

   137

      {

   138

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

   139

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

   140

        d->NMTable[nodeId] = newNodeState;

   141

      }

   142

   143

      /* Boot-Up frame reception */

   144

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

   145

      {

   146

          /*

   147

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

   148

          ** to indicate the master that it is entered in

   149

          ** pre_operational mode

   150

          */

   151

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

   152

          /* call post SlaveBootup with NodeId */

   153

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

   154

      }

   155

   156

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

   157

        UNS8 index, ConsummerHeartBeat_nodeId ;

   158

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

   159

          {

   160

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

   161

            if ( nodeId == ConsummerHeartBeat_nodeId )

   162

              {

   163

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

   164

                /* Renew alarm for next heartbeat. */

   165

                DelAlarm(d->ConsumerHeartBeatTimers[index]);

   166

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

   167

              }

   168

          }

   169

      }

   170

    }

   171

}

   172

   173

/*! The Consumer Timer Callback

   174

**

   175

**

   176

** @param d

   177

** @param id

   178

**/

   179

void ProducerHearbeatAlarm(CO_Data* d, UNS32 id)

   180

{

   181

  if(*d->ProducerHeartBeatTime)

   182

    {

   183

      Message msg;

   184

      /* Time expired, the heartbeat must be sent immediately

   185

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

   186

      ** (decimal) and additionaly

   187

      ** the node-id of this device.

   188

      */

   189

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

   190

      msg.cob_id = UNS16_LE(tmp);

   191

      msg.len = (UNS8)0x01;

   192

      msg.rtr = 0;

   193

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

   194

      /* send the heartbeat */

   195

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

   196

      canSend(d->canHandle,&msg );

   197

   198

    }else{

   199

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

   200

    }

   201

}

   202

   203

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

   204

**

   205

**

   206

** @param d

   207

** @param unsused_indextable

   208

** @param unsused_bSubindex

   209

**

   210

** @return

   211

**/

   212

UNS32 OnHeartbeatProducerUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)

   213

{

   214

  heartbeatStop(d);

   215

  heartbeatInit(d);

   216

  return 0;

   217

}

   218

   219

/*!

   220

**

   221

**

   222

** @param d

   223

**/

   224

void heartbeatInit(CO_Data* d)

   225

{

   226

   227

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

   228

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

   229

   230

  d->toggle = 0;

   231

   232

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

   233

    {

   234

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

   235

      /* MSG_WAR(0x3121, "should_time : ", should_time ) ; */

   236

      if ( time )

   237

        {

   238

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

   239

        }

   240

    }

   241

   242

  if ( *d->ProducerHeartBeatTime )

   243

    {

   244

      TIMEVAL time = *d->ProducerHeartBeatTime;

   245

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

   246

    }

   247

}

   248

   249

/*!

   250

**

   251

**

   252

** @param d

   253

**/

   254

void heartbeatStop(CO_Data* d)

   255

{

   256

  UNS8 index;

   257

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

   258

    {

   259

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

   260

    }

   261

   262

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

   263

}

   264

   265

/*!

   266

**

   267

**

   268

** @param heartbeatID

   269

**/

   270

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

   271

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

   272

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

   273