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