src/lifegrd.c

   1 /*
   2 This file is part of CanFestival, a library implementing CanOpen Stack.
   3
   4 Copyright (C): Edouard TISSERANT and Francis DUPIN
   5
   6 See COPYING file for copyrights details.
   7
   8 This library is free software; you can redistribute it and/or
   9 modify it under the terms of the GNU Lesser General Public
  10 License as published by the Free Software Foundation; either
  11 version 2.1 of the License, or (at your option) any later version.
  12
  13 This library is distributed in the hope that it will be useful,
  14 but WITHOUT ANY WARRANTY; without even the implied warranty of
  15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16 Lesser General Public License for more details.
  17
  18 You should have received a copy of the GNU Lesser General Public
  19 License along with this library; if not, write to the Free Software
  20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  21 */
  22
  23 #include <data.h>
  24 #include "lifegrd.h"
  25 #include "canfestival.h"
  26
  27 /* Prototypes for internals functions */
  28 void ConsumerHearbeatAlarm(CO_Data* d, UNS32 id);
  29 void ProducerHearbeatAlarm(CO_Data* d, UNS32 id);
  30
  31
  32 /*****************************************************************************/
  33 e_nodeState getNodeState (CO_Data* d, UNS8 nodeId)
  34 {
  35         e_nodeState networkNodeState = d->NMTable[nodeId];
  36         return networkNodeState;
  37 }
  38
  39 /*****************************************************************************/
  40 /* The Consumer Timer Callback */
  41 void ConsumerHearbeatAlarm(CO_Data* d, UNS32 id)
  42 {
  43         /*MSG_WAR(0x00, "ConsumerHearbeatAlarm", 0x00);*/
  44
  45         /* call heartbeat error with NodeId */
  46         (*d->heartbeatError)((UNS8)( ((d->ConsumerHeartbeatEntries[id]) & (UNS32)0x00FF0000) >> (UNS8)16 ));
  47 }
  48
  49 /*****************************************************************************/
  50 void proceedNODE_GUARD(CO_Data* d, Message* m )
  51 {
  52   UNS8 nodeId = (UNS8) GET_NODE_ID((*m));
  53
  54   if((m->rtr == 1) ) /* Notice that only the master can have sent this node guarding request */
  55   { /* Receiving a NMT NodeGuarding (request of the state by the master) */
  56     /*  only answer to the NMT NodeGuarding request, the master is not checked (not implemented) */
  57     if (nodeId == *d->bDeviceNodeId )
  58     {
  59       Message msg;
  60       msg.cob_id.w = *d->bDeviceNodeId + 0x700;
  61       msg.len = (UNS8)0x01;
  62       msg.rtr = 0;
  63       msg.data[0] = d->nodeState;
  64       if (d->toggle)
  65       {
  66         msg.data[0] |= 0x80 ;
  67         d->toggle = 0 ;
  68       }
  69       else
  70         d->toggle = 1 ;
  71       /* send the nodeguard response. */
  72       MSG_WAR(0x3130, "Sending NMT Nodeguard to master, state: ", d->nodeState);
  73       canSend(d->canHandle,&msg );
  74     }
  75
  76   }else{ /* Not a request CAN */
  77
  78     MSG_WAR(0x3110, "Received NMT nodeId : ", nodeId);
  79     /* the slave's state receievd is stored in the NMTable */
  80       /* The state is stored on 7 bit */
  81     d->NMTable[nodeId] = (e_nodeState) ((*m).data[0] & 0x7F) ;
  82
  83     /* Boot-Up frame reception */
  84     if ( d->NMTable[nodeId] == Initialisation)
  85       {
  86         /* The device send the boot-up message (Initialisation) */
  87         /* to indicate the master that it is entered in pre_operational mode */
  88         /* Because the  device enter automaticaly in pre_operational mode, */
  89         /* the pre_operational mode is stored */
  90 /*        NMTable[bus_id][nodeId] = Pre_operational; */
  91         MSG_WAR(0x3100, "The NMT is a bootup from node : ", nodeId);
  92       }
  93
  94     if( d->NMTable[nodeId] != Unknown_state ) {
  95         UNS8 index, ConsummerHeartBeat_nodeId ;
  96         for( index = (UNS8)0x00; index < *d->ConsumerHeartbeatCount; index++ )
  97         {
  98             ConsummerHeartBeat_nodeId = (UNS8)( ((d->ConsumerHeartbeatEntries[index]) & (UNS32)0x00FF0000) >> (UNS8)16 );
  99             if ( nodeId == ConsummerHeartBeat_nodeId )
 100             {
 101                 TIMEVAL time = ( (d->ConsumerHeartbeatEntries[index]) & (UNS32)0x0000FFFF ) ;
 102                 /* Renew alarm for next heartbeat. */
 103                 DelAlarm(d->ConsumerHeartBeatTimers[index]);
 104                 d->ConsumerHeartBeatTimers[index] = SetAlarm(d, index, &ConsumerHearbeatAlarm, MS_TO_TIMEVAL(time), 0);
 105             }
 106         }
 107     }
 108   }
 109 }
 110
 111 /*****************************************************************************/
 112 /* The Consumer Timer Callback */
 113 void ProducerHearbeatAlarm(CO_Data* d, UNS32 id)
 114 {
 115         if(*d->ProducerHeartBeatTime)
 116         {
 117                 Message msg;
 118                 /* Time expired, the heartbeat must be sent immediately
 119                  * generate the correct node-id: this is done by the offset 1792
 120                  * (decimal) and additionaly
 121                  * the node-id of this device.
 122                  */
 123                 msg.cob_id.w = *d->bDeviceNodeId + 0x700;
 124                 msg.len = (UNS8)0x01;
 125                 msg.rtr = 0;
 126                 msg.data[0] = d->nodeState; /* No toggle for heartbeat !*/
 127                 /* send the heartbeat */
 128                 MSG_WAR(0x3130, "Producing heartbeat: ", d->nodeState);
 129                 canSend(d->canHandle,&msg );
 130         }else{
 131                 d->ProducerHeartBeatTimer = DelAlarm(d->ProducerHeartBeatTimer);
 132         }
 133 }
 134
 135 /*****************************************************************************/
 136 void heartbeatInit(CO_Data* d)
 137 {
 138     UNS8 index; /* Index to scan the table of heartbeat consumers */
 139
 140     d->toggle = 0;
 141
 142     for( index = (UNS8)0x00; index < *d->ConsumerHeartbeatCount; index++ )
 143     {
 144         TIMEVAL time = (UNS16) ( (d->ConsumerHeartbeatEntries[index]) & (UNS32)0x0000FFFF ) ;
 145         /* MSG_WAR(0x3121, "should_time : ", should_time ) ; */
 146         if ( time )
 147         {
 148                 d->ConsumerHeartBeatTimers[index] = SetAlarm(d, index, &ConsumerHearbeatAlarm, MS_TO_TIMEVAL(time), 0);
 149         }
 150     }
 151
 152     if ( *d->ProducerHeartBeatTime )
 153     {
 154         TIMEVAL time = *d->ProducerHeartBeatTime;
 155         d->ProducerHeartBeatTimer = SetAlarm(d, 0, &ProducerHearbeatAlarm, MS_TO_TIMEVAL(time), MS_TO_TIMEVAL(time));
 156     }
 157 }
 158
 159 /*****************************************************************************/
 160 void heartbeatStop(CO_Data* d)
 161 {
 162     UNS8 index;
 163     for( index = (UNS8)0x00; index < *d->ConsumerHeartbeatCount; index++ )
 164     {
 165         d->ConsumerHeartBeatTimers[index + 1] = DelAlarm(d->ConsumerHeartBeatTimers[index + 1]);;
 166     }
 167
 168     d->ProducerHeartBeatTimer = DelAlarm(d->ProducerHeartBeatTimer);;
 169 }
 170
 171 void _heartbeatError(UNS8 heartbeatID){}