orte/liborte/htimerNtp.c

   1 /*
   2  *  $Id: htimerNtp.c,v 0.0.0.1          2003/08/21
   3  *
   4  *  DEBUG:  section 2                   HTimer for NtpTime
   5  *
   6  *  -------------------------------------------------------------------
   7  *                                ORTE
   8  *                      Open Real-Time Ethernet
   9  *
  10  *                      Copyright (C) 2001-2006
  11  *  Department of Control Engineering FEE CTU Prague, Czech Republic
  12  *                      http://dce.felk.cvut.cz
  13  *                      http://www.ocera.org
  14  *
  15  *  Author:              Petr Smolik    petr@smoliku.cz
  16  *  Advisor:             Pavel Pisa
  17  *  Project Responsible: Zdenek Hanzalek
  18  *  --------------------------------------------------------------------
  19  *
  20  *  This program is free software; you can redistribute it and/or modify
  21  *  it under the terms of the GNU General Public License as published by
  22  *  the Free Software Foundation; either version 2 of the License, or
  23  *  (at your option) any later version.
  24  *
  25  *  This program is distributed in the hope that it will be useful,
  26  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  27  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  28  *  GNU General Public License for more details.
  29  *
  30  */
  31
  32 #define UL_HTIMER_INC_FROM_BASE
  33
  34 #include "orte_all.h"
  35
  36 GAVL_FLES_INT_DEC(ul_htim_queue,        /* prefix */
  37                   ul_htim_queue_t,      /* root_t */
  38                   ul_htim_node_t,       /* item_t */
  39                   ul_htim_time_t,       /* key_t */
  40                   timers,               /* root_field */
  41                   node,                 /* item_node */
  42                   expires,              /* item_key */
  43                   ul_htimer_cmp_fnc)    /* cmp_fnc */
  44
  45
  46 GAVL_FLES_INT_IMP(ul_htim_queue, ul_htim_queue_t, ul_htim_node_t, ul_htim_time_t,
  47                   timers, node, expires, ul_htimer_cmp_fnc, GAVL_FAFTER,
  48                   root->first_changed = 1,;
  49                   , root->first_changed = 1)
  50
  51 /*****************************************************************************/
  52 UL_HTIMER_IMP(htimerRoot, \
  53               ObjectEntry, \
  54               HTimFncRootNode, \
  55               htimRoot, \
  56               htim)
  57 void
  58 htimerRoot_run_expired(ORTEDomain *d,
  59                        ul_htim_time_t *pact_time)
  60 {
  61   HTimFncRootNode *timer;
  62
  63   debug(2, 10) ("htimerRoot: start\n");
  64   while ((timer = htimerRoot_cut_expired(&d->objectEntry, pact_time))) {
  65     debug(2, 10) ("htimerRoot: AID-0x%x\n", timer->objectEntryAID->aid);
  66     timer->func(d, timer->objectEntryAID, pact_time);
  67   }
  68   debug(2, 10) ("htimerRoot: finished\n");
  69 }
  70
  71 /*****************************************************************************/
  72 UL_HTIMER_IMP(htimerUnicastCommon, \
  73               ObjectEntryAID, \
  74               HTimFncUserNode, \
  75               htimUnicast.common, \
  76               htim)
  77
  78 void
  79 htimerUnicastCommon_update_root_timer(ObjectEntry *objectEntry,
  80                                       ObjectEntryAID *objectEntryAID)
  81 {
  82   NtpTime nextExpire;
  83
  84   //if first item is changed -> update root timer queue
  85   if (htimerUnicastCommon_first_changed(objectEntryAID)) {
  86     //deatach old timer from this queue (if any)
  87     htimerRoot_detach(objectEntry,
  88                       &objectEntryAID->htimUnicast.commonNode);
  89     if (htimerUnicastCommon_next_expire(objectEntryAID, &nextExpire) != 0) {
  90       //setup new values
  91       objectEntryAID->htimUnicast.commonNode.func =
  92         htimerUnicastCommon_run_expired;
  93       objectEntryAID->htimUnicast.commonNode.objectEntryAID =
  94         objectEntryAID;
  95       htimerRoot_set_expire(
  96         &objectEntryAID->htimUnicast.commonNode,
  97         nextExpire);
  98       //insert new timer to root htimer node
  99       htimerRoot_add(objectEntry,
 100                      &objectEntryAID->htimUnicast.commonNode);
 101       debug(2, 10) ("htimerUnicastCommon: root updated, wakeup\n");
 102       //wake-up sending thread to process event
 103       ORTEDomainWakeUpSendingThread(objectEntry);
 104     }
 105   }
 106 }
 107
 108 void
 109 htimerUnicastCommon_run_expired(ORTEDomain *d,
 110                                 ObjectEntryAID *objectEntryAID,
 111                                 ul_htim_time_t *pact_time)
 112 {
 113   HTimFncUserNode  *timer;
 114   int              retValue;
 115   pthread_rwlock_t *lock;
 116
 117   while ((timer = htimerUnicastCommon_cut_expired(objectEntryAID, pact_time))) {
 118     if ((lock = timer->lock)) //after proc. timer->func can be timer freed
 119       pthread_rwlock_wrlock(timer->lock);
 120     debug(2, 10) ("htimerUnicastCommon: %s\n",
 121                   timer->name);
 122     retValue = timer->func(d, timer->arg1);
 123     if (lock)
 124       pthread_rwlock_unlock(timer->lock);
 125     if (retValue == 2) //object deleted
 126       return;
 127   }
 128   htimerUnicastCommon_update_root_timer(&d->objectEntry, objectEntryAID);
 129 }
 130
 131 /*****************************************************************************/
 132 UL_HTIMER_IMP(htimerUnicastSendMetatraffic, \
 133               ObjectEntryAID, \
 134               HTimFncUserNode, \
 135               htimUnicast.sendMetatraffic, \
 136               htim)
 137
 138 void
 139 htimerUnicastSendMetatraffic_update_root_timer(ObjectEntry *objectEntry,
 140                                                ObjectEntryAID *objectEntryAID)
 141 {
 142   NtpTime nextExpire;
 143
 144   //if first item is changed -> update root timer queue
 145   if (htimerUnicastSendMetatraffic_first_changed(objectEntryAID)) {
 146     //deatach old timer from this queue (if any)
 147     htimerRoot_detach(objectEntry,
 148                       &objectEntryAID->htimUnicast.sendMetatrafficNode);
 149     if (htimerUnicastSendMetatraffic_next_expire(objectEntryAID, &nextExpire) != 0) {
 150       //setup new values
 151       objectEntryAID->htimUnicast.sendMetatrafficNode.func =
 152         htimerUnicastSendMetatraffic_run_expired;
 153       objectEntryAID->htimUnicast.sendMetatrafficNode.objectEntryAID =
 154         objectEntryAID;
 155       htimerRoot_set_expire(
 156         &objectEntryAID->htimUnicast.sendMetatrafficNode,
 157         nextExpire);
 158       //insert new timer to root htimer node
 159       htimerRoot_add(objectEntry,
 160                      &objectEntryAID->htimUnicast.sendMetatrafficNode);
 161       debug(2, 10) ("htimerUnicastMetatraffic: root updated, wakeup\n");
 162       //wake-up sending thread to process event
 163       ORTEDomainWakeUpSendingThread(objectEntry);
 164     }
 165   }
 166 }
 167
 168 void
 169 htimerUnicastSendMetatraffic_run_expired(ORTEDomain *d,
 170                                          ObjectEntryAID *objectEntryAID,
 171                                          ul_htim_time_t *pact_time)
 172 {
 173   HTimFncUserNode *timer;
 174   int             retValue;
 175
 176   while ((timer = htimerUnicastSendMetatraffic_cut_expired(objectEntryAID, pact_time))) {
 177     if (timer->lock)
 178       pthread_rwlock_wrlock(timer->lock);
 179     debug(2, 10) ("htimerUnicastMetatraffic: %s\n",
 180                   timer->name);
 181     retValue = timer->func(d, timer->arg1);
 182     while (d->taskSend.mb.needSend) {
 183       ORTESendData(d, objectEntryAID, ORTE_TRUE);
 184       timer->func(d, timer->arg1);
 185     }
 186     if (timer->lock)
 187       pthread_rwlock_unlock(timer->lock);
 188   }
 189   htimerUnicastSendMetatraffic_update_root_timer(&d->objectEntry, objectEntryAID);
 190   if (d->taskSend.mb.cdrCodec.wptr > RTPS_HEADER_LENGTH) {
 191     ORTESendData(d, objectEntryAID, ORTE_TRUE);
 192   }
 193 }
 194
 195 /*****************************************************************************/
 196 UL_HTIMER_IMP(htimerUnicastSendUserData, \
 197               ObjectEntryAID, \
 198               HTimFncUserNode, \
 199               htimUnicast.sendUserData, \
 200               htim)
 201
 202 void
 203 htimerUnicastSendUserData_update_root_timer(ObjectEntry *objectEntry,
 204                                             ObjectEntryAID *objectEntryAID)
 205 {
 206   NtpTime nextExpire;
 207
 208   //if first item is changed -> update root timer queue
 209   if (htimerUnicastSendUserData_first_changed(objectEntryAID)) {
 210     //deatach old timer from this queue (if any)
 211     htimerRoot_detach(objectEntry,
 212                       &objectEntryAID->htimUnicast.sendUserDataNode);
 213     if (htimerUnicastSendUserData_next_expire(objectEntryAID, &nextExpire) != 0) {
 214       //setup new values
 215       objectEntryAID->htimUnicast.sendUserDataNode.func =
 216         htimerUnicastSendUserData_run_expired;
 217       objectEntryAID->htimUnicast.sendUserDataNode.objectEntryAID =
 218         objectEntryAID;
 219       htimerRoot_set_expire(
 220         &objectEntryAID->htimUnicast.sendUserDataNode,
 221         nextExpire);
 222       //insert new timer to root htimer node
 223       htimerRoot_add(objectEntry,
 224                      &objectEntryAID->htimUnicast.sendUserDataNode);
 225       debug(2, 10) ("htimerUnicastUserdata: root updated, wakeup\n");
 226       //wake-up sending thread to process event
 227       ORTEDomainWakeUpSendingThread(objectEntry);
 228     }
 229   }
 230 }
 231
 232 void
 233 htimerUnicastSendUserData_run_expired(ORTEDomain *d,
 234                                       ObjectEntryAID *objectEntryAID,
 235                                       ul_htim_time_t *pact_time)
 236 {
 237   HTimFncUserNode *timer;
 238   int             retValue;
 239
 240   while ((timer = htimerUnicastSendUserData_cut_expired(objectEntryAID, pact_time))) {
 241     if (timer->lock)
 242       pthread_rwlock_wrlock(timer->lock);
 243     retValue = timer->func(d, timer->arg1);
 244     while (d->taskSend.mb.needSend) {
 245       ORTESendData(d, objectEntryAID, ORTE_FALSE);
 246       timer->func(d, timer->arg1);
 247     }
 248     if (timer->lock)
 249       pthread_rwlock_unlock(timer->lock);
 250   }
 251   htimerUnicastSendUserData_update_root_timer(&d->objectEntry, objectEntryAID);
 252   if (d->taskSend.mb.cdrCodec.wptr > RTPS_HEADER_LENGTH) {
 253     ORTESendData(d, objectEntryAID, ORTE_FALSE);
 254   }
 255 }
 256
 257 /*********************************************************************/
 258 NtpTime
 259 getActualNtpTime(void)
 260 {
 261   NtpTime               result;
 262
 263 #ifndef CONFIG_ORTE_RT
 264   struct timeval        time;
 265
 266   gettimeofday(&time, NULL);
 267   NtpTimeAssembFromUs(result, time.tv_sec, time.tv_usec);
 268   NtpTimeAssembFromUs(result, time.tv_sec, time.tv_usec);
 269 #else
 270   struct timespec        time;
 271
 272   clock_gettime(CLOCK_REALTIME, &time);
 273   time.tv_nsec /= 1000;  //conver to us
 274   NtpTimeAssembFromUs(result, time.tv_sec, time.tv_nsec);
 275 #endif
 276   return result;
 277 }