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 = d->NMTable[nodeId];

    60

  return networkNodeState;

    61

}

    62

    63

/*! 

    64

** The Consumer Timer Callback

    65

**

    66

** @param d

    67

** @param id

    68

**/

    69

void ConsumerHearbeatAlarm(CO_Data* d, UNS32 id)

    70

{

    71

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

    72

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

    73

    74

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

    75

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

    76

  d->ConsumerHeartBeatTimers[id]=TIMER_NONE;

    77

    78

  /* set node state */

    79

  d->NMTable[nodeId] = Disconnected;

    80

  /*! call heartbeat error with NodeId */

    81

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

    82

}

    83

    84

/*!

    85

**

    86

**

    87

** @param d

    88

** @param m

    89

**/

    90

void proceedNODE_GUARD(CO_Data* d, Message* m )

    91

{

    92

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

    93

    94

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

    95

    /*!

    96

    ** Notice that only the master can have sent this

    97

    ** node guarding request

    98

    */

    99

    {

   100

      /*!

   101

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

   102

      ** master)

   103

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

   104

      ** not checked (not implemented)

   105

      */

   106

      if (nodeId == *d->bDeviceNodeId )

   107

        {

   108

          Message msg;

   109

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

   110

          msg.cob_id = UNS16_LE(tmp);

   111

          msg.len = (UNS8)0x01;

   112

          msg.rtr = 0;

   113

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

   114

          if (d->toggle)

   115

            {

   116

              msg.data[0] |= 0x80 ;

   117

              d->toggle = 0 ;

   118

            }

   119

          else

   120

            d->toggle = 1 ;

   121

          /* send the nodeguard response. */

   122

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

   123

          canSend(d->canHandle,&msg );

   124

        }

   125

   126

    }else{ /* Not a request CAN */

   127

      /* The state is stored on 7 bit */

   128

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

   129

   130

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

   131

   132

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

   133

      {

   134

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

   135

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

   136

        d->NMTable[nodeId] = newNodeState;

   137

      }

   138

   139

      /* Boot-Up frame reception */

   140

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

   141

        {

   142

          /*

   143

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

   144

          ** to indicate the master that it is entered in

   145

          ** pre_operational mode

   146

          ** Because the  device enter automaticaly in pre_operational

   147

          ** mode,

   148

          ** the pre_operational mode is stored

   149

          ** NMTable[bus_id][nodeId] = Pre_operational

   150

          */

   151

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

   152

   153

          if(!send_consise_dcf(d,nodeId)){

   154

             /* call post SlaveBootup with NodeId */

   155

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

   156

          }

   157

        }

   158

   159

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

   160

        UNS8 index, ConsummerHeartBeat_nodeId ;

   161

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

   162

          {

   163

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

   164

            if ( nodeId == ConsummerHeartBeat_nodeId )

   165

              {

   166

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

   167

                /* Renew alarm for next heartbeat. */

   168

                DelAlarm(d->ConsumerHeartBeatTimers[index]);

   169

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

   170

              }

   171

          }

   172

      }

   173

    }

   174

}

   175

   176

/*! The Consumer Timer Callback

   177

**

   178

**

   179

** @param d

   180

** @param id

   181

**/

   182

void ProducerHearbeatAlarm(CO_Data* d, UNS32 id)

   183

{

   184

  if(*d->ProducerHeartBeatTime)

   185

    {

   186

      Message msg;

   187

      /* Time expired, the heartbeat must be sent immediately

   188

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

   189

      ** (decimal) and additionaly

   190

      ** the node-id of this device.

   191

      */

   192

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

   193

      msg.cob_id = UNS16_LE(tmp);

   194

      msg.len = (UNS8)0x01;

   195

      msg.rtr = 0;

   196

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

   197

      /* send the heartbeat */

   198

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

   199

      canSend(d->canHandle,&msg );

   200

   201

    }else{

   202

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

   203

    }

   204

}

   205

   206

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

   207

**

   208

**

   209

** @param d

   210

** @param unsused_indextable

   211

** @param unsused_bSubindex

   212

**

   213

** @return

   214

**/

   215

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

   216

{

   217

  heartbeatStop(d);

   218

  heartbeatInit(d);

   219

  return 0;

   220

}

   221

   222

/*!

   223

**

   224

**

   225

** @param d

   226

**/

   227

void heartbeatInit(CO_Data* d)

   228

{

   229

   230

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

   231

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

   232

   233

  d->toggle = 0;

   234

   235

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

   236

    {

   237

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

   238

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

   239

      if ( time )

   240

        {

   241

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

   242

        }

   243

    }

   244

   245

  if ( *d->ProducerHeartBeatTime )

   246

    {

   247

      TIMEVAL time = *d->ProducerHeartBeatTime;

   248

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

   249

    }

   250

}

   251

   252

/*!

   253

**

   254

**

   255

** @param d

   256

**/

   257

void heartbeatStop(CO_Data* d)

   258

{

   259

  UNS8 index;

   260

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

   261

    {

   262

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

   263

    }

   264

   265

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

   266

}

   267

   268

/*!

   269

**

   270

**

   271

** @param heartbeatID

   272

**/

   273

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

   274

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

   275

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

   276