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.smolik@wo.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,;,root->first_changed=1)
  49
  50 /*****************************************************************************/
  51 UL_HTIMER_IMP(htimerRoot, \
  52       ObjectEntry, \
  53       HTimFncRootNode, \
  54       htimRoot, \
  55       htim)
  56 void
  57 htimerRoot_run_expired(ORTEDomain *d,
  58       ul_htim_time_t *pact_time) {
  59   HTimFncRootNode *timer;
  60
  61   debug(2,10) ("htimerRoot: start\n");
  62   while((timer=htimerRoot_cut_expired(&d->objectEntry, pact_time))){
  63     debug(2,10) ("htimerRoot: AID-0x%x\n",timer->objectEntryAID->aid);
  64     timer->func(d,timer->objectEntryAID,pact_time);
  65   }
  66   debug(2,10) ("htimerRoot: finished\n");
  67 }
  68
  69 /*****************************************************************************/
  70 UL_HTIMER_IMP(htimerUnicastCommon, \
  71       ObjectEntryAID, \
  72       HTimFncUserNode, \
  73       htimUnicast.common, \
  74       htim)
  75
  76 void
  77 htimerUnicastCommon_update_root_timer(ObjectEntry *objectEntry,
  78       ObjectEntryAID *objectEntryAID) {
  79   NtpTime nextExpire;
  80
  81   //if first item is changed -> update root timer queue
  82   if (htimerUnicastCommon_first_changed(objectEntryAID)) {
  83     //deatach old timer from this queue (if any)
  84     htimerRoot_detach(objectEntry,
  85         &objectEntryAID->htimUnicast.commonNode);
  86     if (htimerUnicastCommon_next_expire(objectEntryAID,&nextExpire)!=0) {
  87       //setup new values
  88       objectEntryAID->htimUnicast.commonNode.func=
  89           htimerUnicastCommon_run_expired;
  90       objectEntryAID->htimUnicast.commonNode.objectEntryAID=
  91           objectEntryAID;
  92       htimerRoot_set_expire(
  93           &objectEntryAID->htimUnicast.commonNode,
  94           nextExpire);
  95       //insert new timer to root htimer node
  96       htimerRoot_add(objectEntry,
  97           &objectEntryAID->htimUnicast.commonNode);
  98       debug(2,10) ("htimerUnicastCommon: root updated, wakeup\n");
  99       //wake-up sending thread to process event
 100       ORTEDomainWakeUpSendingThread(objectEntry);
 101     }
 102   }
 103 }
 104
 105 void
 106 htimerUnicastCommon_run_expired(ORTEDomain *d,
 107                        ObjectEntryAID *objectEntryAID,
 108                        ul_htim_time_t *pact_time) {
 109   HTimFncUserNode  *timer;
 110   int              retValue;
 111   pthread_rwlock_t *lock;
 112
 113   while((timer=htimerUnicastCommon_cut_expired(objectEntryAID, pact_time))){
 114     if ((lock=timer->lock)) //after proc. timer->func can be timer freed
 115       pthread_rwlock_wrlock(timer->lock);
 116     debug(2,10) ("htimerUnicastCommon: %s\n",
 117                   timer->name);
 118     retValue=timer->func(d,timer->arg1);
 119     if (lock)
 120       pthread_rwlock_unlock(timer->lock);
 121     if (retValue==2)  //object deleted
 122       return;
 123   }
 124   htimerUnicastCommon_update_root_timer(&d->objectEntry,objectEntryAID);
 125 }
 126
 127 /*****************************************************************************/
 128 UL_HTIMER_IMP(htimerUnicastSendMetatraffic, \
 129       ObjectEntryAID, \
 130       HTimFncUserNode, \
 131       htimUnicast.sendMetatraffic, \
 132       htim)
 133
 134 void
 135 htimerUnicastSendMetatraffic_update_root_timer(ObjectEntry *objectEntry,
 136       ObjectEntryAID *objectEntryAID) {
 137   NtpTime nextExpire;
 138
 139   //if first item is changed -> update root timer queue
 140   if (htimerUnicastSendMetatraffic_first_changed(objectEntryAID)) {
 141     //deatach old timer from this queue (if any)
 142     htimerRoot_detach(objectEntry,
 143         &objectEntryAID->htimUnicast.sendMetatrafficNode);
 144     if (htimerUnicastSendMetatraffic_next_expire(objectEntryAID,&nextExpire)!=0) {
 145       //setup new values
 146       objectEntryAID->htimUnicast.sendMetatrafficNode.func=
 147           htimerUnicastSendMetatraffic_run_expired;
 148       objectEntryAID->htimUnicast.sendMetatrafficNode.objectEntryAID=
 149           objectEntryAID;
 150       htimerRoot_set_expire(
 151           &objectEntryAID->htimUnicast.sendMetatrafficNode,
 152           nextExpire);
 153       //insert new timer to root htimer node
 154       htimerRoot_add(objectEntry,
 155           &objectEntryAID->htimUnicast.sendMetatrafficNode);
 156       debug(2,10) ("htimerUnicastMetatraffic: root updated, wakeup\n");
 157       //wake-up sending thread to process event
 158       ORTEDomainWakeUpSendingThread(objectEntry);
 159     }
 160   }
 161 }
 162
 163 void
 164 htimerUnicastSendMetatraffic_run_expired(ORTEDomain *d,
 165     ObjectEntryAID *objectEntryAID,
 166     ul_htim_time_t *pact_time) {
 167   HTimFncUserNode *timer;
 168   int             retValue;
 169
 170   while((timer=htimerUnicastSendMetatraffic_cut_expired(objectEntryAID, pact_time))){
 171     if (timer->lock)
 172       pthread_rwlock_wrlock(timer->lock);
 173     debug(2,10) ("htimerUnicastMetatraffic: %s\n",
 174                   timer->name);
 175     retValue=timer->func(d,timer->arg1);
 176     while (d->taskSend.mb.needSend) {
 177       ORTESendData(d,objectEntryAID,ORTE_TRUE);
 178       timer->func(d,timer->arg1);
 179     }
 180     if (timer->lock)
 181       pthread_rwlock_unlock(timer->lock);
 182   }
 183   htimerUnicastSendMetatraffic_update_root_timer(&d->objectEntry,objectEntryAID);
 184   if (d->taskSend.mb.cdrCodec.wptr>RTPS_HEADER_LENGTH) {
 185       ORTESendData(d,objectEntryAID,ORTE_TRUE);
 186   }
 187 }
 188
 189 /*****************************************************************************/
 190 UL_HTIMER_IMP(htimerUnicastSendUserData, \
 191       ObjectEntryAID, \
 192       HTimFncUserNode, \
 193       htimUnicast.sendUserData, \
 194       htim)
 195
 196 void
 197 htimerUnicastSendUserData_update_root_timer(ObjectEntry *objectEntry,
 198       ObjectEntryAID *objectEntryAID) {
 199   NtpTime nextExpire;
 200
 201   //if first item is changed -> update root timer queue
 202   if (htimerUnicastSendUserData_first_changed(objectEntryAID)) {
 203     //deatach old timer from this queue (if any)
 204     htimerRoot_detach(objectEntry,
 205         &objectEntryAID->htimUnicast.sendUserDataNode);
 206     if (htimerUnicastSendUserData_next_expire(objectEntryAID,&nextExpire)!=0) {
 207       //setup new values
 208       objectEntryAID->htimUnicast.sendUserDataNode.func=
 209           htimerUnicastSendUserData_run_expired;
 210       objectEntryAID->htimUnicast.sendUserDataNode.objectEntryAID=
 211           objectEntryAID;
 212       htimerRoot_set_expire(
 213           &objectEntryAID->htimUnicast.sendUserDataNode,
 214           nextExpire);
 215       //insert new timer to root htimer node
 216       htimerRoot_add(objectEntry,
 217           &objectEntryAID->htimUnicast.sendUserDataNode);
 218       debug(2,10) ("htimerUnicastUserdata: root updated, wakeup\n");
 219       //wake-up sending thread to process event
 220       ORTEDomainWakeUpSendingThread(objectEntry);
 221     }
 222   }
 223 }
 224
 225 void
 226 htimerUnicastSendUserData_run_expired(ORTEDomain *d,
 227     ObjectEntryAID *objectEntryAID,
 228     ul_htim_time_t *pact_time) {
 229   HTimFncUserNode *timer;
 230   int             retValue;
 231
 232   while((timer=htimerUnicastSendUserData_cut_expired(objectEntryAID, pact_time))){
 233     if (timer->lock)
 234       pthread_rwlock_wrlock(timer->lock);
 235     retValue=timer->func(d,timer->arg1);
 236     while (d->taskSend.mb.needSend) {
 237       ORTESendData(d,objectEntryAID,ORTE_FALSE);
 238       timer->func(d,timer->arg1);
 239     }
 240     if (timer->lock)
 241       pthread_rwlock_unlock(timer->lock);
 242   }
 243   htimerUnicastSendUserData_update_root_timer(&d->objectEntry,objectEntryAID);
 244   if (d->taskSend.mb.cdrCodec.wptr>RTPS_HEADER_LENGTH) {
 245       ORTESendData(d,objectEntryAID,ORTE_FALSE);
 246   }
 247 }
 248
 249 /*********************************************************************/
 250 NtpTime
 251 getActualNtpTime(void) {
 252   NtpTime               result;
 253 #ifndef CONFIG_ORTE_RT
 254   struct timeval        time;
 255
 256   gettimeofday(&time,NULL);
 257   NtpTimeAssembFromUs(result,time.tv_sec,time.tv_usec);
 258   NtpTimeAssembFromUs(result,time.tv_sec,time.tv_usec);
 259 #else
 260   struct timespec        time;
 261
 262   clock_gettime(CLOCK_REALTIME, &time);
 263   time.tv_nsec/=1000;  //conver to us
 264   NtpTimeAssembFromUs(result,time.tv_sec,time.tv_nsec);
 265 #endif
 266   return result;
 267 }