static const struct sigevent* timerIntHandler(void* data, int id) { struct timespec tp; TimerIntData* myData = (TimerIntData*)data; uint64_t new_tsc = ClockCycles(); clock_gettime(CLOCK_REALTIME, &tp); if(new_tsc > myData->prev_tsc) { myData->cur_delta = new_tsc - myData->prev_tsc; /* when hell freezeth over, thy TSC shall roll over */ } else { myData->cur_delta = myData->prev_delta; } /* 4/6 weighted average */ myData->filtered_delta = (40 * myData->cur_delta + 60 * myData->prev_delta) / 100; myData->prev_delta = myData->cur_delta; myData->prev_tsc = new_tsc; if(myData->counter < 2) { myData->counter++; } myData->last_clock = timespec2nsec(&tp); return NULL; }
int sigtimedwait(const sigset_t *set, siginfo_t *info, const struct timespec *timeout) { int rc; if(timeout) { // Passing a NULL for event is the same as notify = SIGEV_UNBLOCK uint64_t t = timespec2nsec(timeout); //POSIX 3.3.8.4 error checking if(!TIMESPEC_VALID(timeout)) { errno = EINVAL; return -1; } // Passing a NULL for event is the same as notify = SIGEV_UNBLOCK if(TimerTimeout(CLOCK_MONOTONIC, _NTO_TIMEOUT_SIGWAITINFO, 0, &t, 0) == -1) { return -1; } } // POSIX P1003.1-1996 Section 3.3.8.2 line 1995 says we have to do this if (((rc = SignalWaitinfo(set, info)) == -1) && (errno == ETIMEDOUT)) { errno = EAGAIN; } return rc; }
int mq_timedsend_woption(mqd_t mq, const char *buff, size_t nbytes, unsigned msgprio, const struct timespec *timeout, clockid_t clock_choice) { uint64_t t = timespec2nsec(timeout); if(!TIMESPEC_VALID(timeout)) { errno = EINVAL; return -1; } if(TimerTimeout(clock_choice, TIMER_ABSTIME | _NTO_TIMEOUT_SEND | _NTO_TIMEOUT_REPLY, 0, &t, 0) == -1) { return -1; } return (_writex(mq, buff, nbytes, _IO_XTYPE_MQUEUE | (msgprio << 16), 0, 0) == -1) ? -1 : 0; }
int timer_timeout(clockid_t id, int flags, const struct sigevent *notify, const struct timespec *ntime, struct timespec *otime) { int ret; uint64_t o, n; if(ntime) { n = timespec2nsec(ntime); } ret = TimerTimeout(id, flags, notify, ntime ? &n : 0, otime ? &o : 0); if(otime) { nsec2timespec(otime, o); } return ret; }
static Boolean getTime (ClockDriver *self, TimeInternal *time) { #ifdef __QNXNTO__ static TimerIntData tmpData; int ret; uint64_t delta; double tick_delay; uint64_t clock_offset; struct timespec tp; if(!tDataUpdated) { memset(&tData, 0, sizeof(TimerIntData)); if(ThreadCtl(_NTO_TCTL_IO, 0) == -1) { ERROR(THIS_COMPONENT"QNX: could not give process I/O privileges"); return FALSE; } tData.cps = SYSPAGE_ENTRY(qtime)->cycles_per_sec; tData.ns_per_tick = 1000000000.0 / tData.cps; tData.prev_tsc = ClockCycles(); clock_gettime(CLOCK_REALTIME, &tp); tData.last_clock = timespec2nsec(&tp); ret = InterruptAttach(0, timerIntHandler, &tData, sizeof(TimerIntData), _NTO_INTR_FLAGS_END | _NTO_INTR_FLAGS_TRK_MSK); if(ret == -1) { ERROR(THIS_COMPONENT"QNX: could not attach to timer interrupt"); return FALSE; } tDataUpdated = TRUE; time->seconds = tp.tv_sec; time->nanoseconds = tp.tv_nsec; return; } memcpy(&tmpData, &tData, sizeof(TimerIntData)); delta = ClockCycles() - tmpData.prev_tsc; /* compute time since last clock update */ tick_delay = (double)delta / (double)tmpData.filtered_delta; clock_offset = (uint64_t)(tick_delay * tmpData.ns_per_tick * (double)tmpData.filtered_delta); /* not filtered yet */ if(tData.counter < 2) { clock_offset = 0; } DBGV("QNX getTime cps: %lld tick interval: %.09f, time since last tick: %lld\n", tmpData.cps, tmpData.filtered_delta * tmpData.ns_per_tick, clock_offset); nsec2timespec(&tp, tmpData.last_clock + clock_offset); time->seconds = tp.tv_sec; time->nanoseconds = tp.tv_nsec; return TRUE; #else #if defined(_POSIX_TIMERS) && (_POSIX_TIMERS > 0) struct timespec tp; if (clock_gettime(CLOCK_REALTIME, &tp) < 0) { PERROR(THIS_COMPONENT"clock_gettime() failed, exiting."); exit(0); } time->seconds = tp.tv_sec; time->nanoseconds = tp.tv_nsec; #else struct timeval tv; gettimeofday(&tv, 0); time->seconds = tv.tv_sec; time->nanoseconds = tv.tv_usec * 1000; #endif /* _POSIX_TIMERS */ #endif /* __QNXNTO__ */ return TRUE; }
int main(int argc, char** argv) { time_state state = START; struct timespec now; int fd; char* sourceaddr; int sourceport; char buf[1024]; char output[1024]; int rc; int signr; struct sigevent sigev; timer_t timer; struct sched_param schedp; sigset_t sigset; uint64_t superframeStartTime; uint64_t timeOffset; uint64_t timeOffsetExtern; int finished; struct itimerspec tspec; unsigned int frameCounter; uint32_t beaconDelay; /* Parse parameters */ if( argc != ARG_COUNT+1 ){ printf("Usage: clocksync <hostname> <portnummer> <adresse> <slotnummer>\n"); exit(1); } char hostname[128]; strncpy(hostname, argv[1], 127); hostname[127] = '\0'; int port = atoi( argv[2] ); char * adresse = argv[3]; int slotnummer = atoi( argv[4] ); uint64_t timeOffsetMidOwnSlot = (20LL /*Beacon-Fenster*/ + 4 /*Sicherheitspause*/ + (slotnummer-1)*4 /*Zeit bis zu eigenem slot*/ + (4/2) /*halbe slotlaenge*/); printf("starting clocksync as: %s, at %s:%d with slotnummer %d\n",hostname,adresse,port,slotnummer); //Initialisiere Socket. //Trete der Multicast-Gruppe bei //Aktiviere Signal SIGIO fd = initSocket( adresse, port ); if( fd < 0 ){ printf("Failed initializing socket at: %s:%d.", adresse, port); exit(1); } //Definiere Ereignis fuer den Timer //Beim Ablaufen des Timers soll das Signal SIGALRM //an die aktuelle Thread gesendet werden. sigev.sigev_notify = SIGEV_THREAD_ID | SIGEV_SIGNAL; sigev.sigev_signo = SIGALRM; sigev.sigev_notify_thread_id = gettid(); //Erzeuge den Timer timer_create(CLOCK, &sigev, &timer); //Umschaltung auf Real-time Scheduler. //Erfordert besondere Privilegien. //Deshalb hier deaktiviert. /* memset(&schedp, 0, sizeof (schedp)); schedp.sched_priority = PRIO; sched_setscheduler(0, POLICY, &schedp); */ //Lege fest, auf welche Signale beim //Aufruf von sigwaitinfo gewartet werden soll. sigemptyset(&sigset); sigaddset(&sigset, SIGIO); //Socket hat Datagramme empfangen sigaddset(&sigset, SIGALRM); //Timer ist abgelaufen sigaddset(&sigset, SIGINT); //Cntrl-C wurde gedrueckt sigprocmask(SIG_BLOCK, &sigset, NULL); //Framecounter initialisieren frameCounter = 0; superframeStartTime = 0; //Differenz zwischen der realen Zeit und der synchronisierten Anwendungszeit. //Die synchronisierte Anwendungszeit ergibt sich aus der Beaconnummer. //Sie wird gerechnet vom Startzeitpunkt des Superframes mit der Beaconnummer 0 timeOffset = 0; timeOffsetExtern = 0; /* wait 3 superframes for beacon from other units !TODO: check!*/ clock_gettime(CLOCK, &now); tspec.it_interval.tv_sec = 0; tspec.it_interval.tv_nsec = 0; nsec2timespec( &tspec.it_value, TIME_START_WAIT_SUPERFRAMES * 100LL /*msec*/ *1000*1000 + timespec2nsec(&now)); timer_settime(timer, TIMER_ABSTIME, &tspec, NULL); printf("Waiting for first beacon.\n"); //Merker fuer Programmende finished = 0; while( finished == 0 ){ //Lese empfangene Datagramme oder warte auf Signale //Diese Abfrage ist ein wenig tricky, da das I/O-Signal (SIGIO) //flankengesteuert arbeitet. signr=0; while( signr == 0 ){ //Pruefe, ob bereits Datagramme eingetroffen sind. //Die muessen erst gelesen werden, da sonst fuer diese kein SIGIO-Signal ausgeloest wird. //Signal wird erst gesendet beim Uebergang von Non-Ready nach Ready (Flankengesteuert!) //Also muss Socket solange ausgelesen werden, bis es Non-Ready ist. //Beachte: Socket wurde auf nonblocking umgeschaltet. //Wenn keine Nachricht vorhanden ist, kehrt Aufruf sofort mit -1 zurueck. errno ist dann EAGAIN. rc = recvMessage( fd, buf, sizeof(buf), &sourceaddr, &sourceport ); if( rc > 0 ){ //Ok, Datagram empfangen. Beende Schleife signr = SIGIO; break; } //Warte auf ein Signal. //Die entsprechenden Signale sind oben konfiguriert worden. siginfo_t info; if (sigwaitinfo(&sigset, &info) < 0){ perror( "sigwait" ); exit(1); } if( info.si_signo == SIGALRM ){ //Timer ist abgelaufen signr = SIGALRM; break; }else if( info.si_signo == SIGINT ){ //Cntrl-C wurde gedrueckt signr = SIGINT; break; } } //So, gueltiges Ereignis empfangen. //Nun geht es ans auswerten. /* Get current time */ clock_gettime(CLOCK, &now); #ifdef DEBUG printf("received signal %d at state %d.\n",signr,state); #endif // DEBUG switch(state){ case START: switch(signr){ case SIGALRM: // no beacon received, I'm first to arrive printf("no beacon arrived within %d superframes, so this must be the first unit.\n",TIME_START_WAIT_SUPERFRAMES); // configure timer for next slottime tspec.it_interval.tv_sec = 0; tspec.it_interval.tv_nsec = 0; superframeStartTime = timespec2nsec(&now); nsec2timespec( &tspec.it_value, superframeStartTime + timeOffsetMidOwnSlot); timer_settime(timer, TIMER_ABSTIME, &tspec, NULL); timeOffset = timespec2nsec(&now); // init offset time state = AWAIT_SLOT_TIME; break; // case SIGALRM case SIGIO: if( buf[0] == 'B' ){ rc = decodeBeacon( buf, &frameCounter, &beaconDelay, NULL, 0); if( rc < 0 ){ printf( "### Invalid Beacon: '%s'\n", buf ); } else { printf("beacon arrived, this is not the first unit.\n"); //Berechne den Zeitpunkt, an dem der Superframe begann superframeStartTime = timespec2nsec( &now ) - beaconDelay; //Starte Zeitmessung mit dem ersten empfangenen Beacon //Differenz zwischen der realen Zeit und der synchronisierten Anwendungszeit. //Die synchronisierte Anwendungszeit ergibt sich aus der Beaconnummer. //Sie wird gerechnet vom Startzeitpunkt des Superframes mit der Beaconnummer 0 timeOffset = superframeStartTime - frameCounter * 100LL /* msec */ * 1000 * 1000; // configure timer for next slottime tspec.it_interval.tv_sec = 0; tspec.it_interval.tv_nsec = 0; nsec2timespec( &tspec.it_value, superframeStartTime + timeOffsetMidOwnSlot); timer_settime(timer, TIMER_ABSTIME, &tspec, NULL); state = AWAIT_SLOT_TIME; } } else { printf("### Received Message is no beacon: '%s'\n", buf ); } break; // case SIGIO case SIGINT: printf("received SIGINT, shutting down.\n"); finished = 1; break; // case SIGINT } // switch signr break; // case START case AWAIT_BEACON: switch(signr){ case SIGALRM: // send beacon, since no beacon arrived before frameCounter++; // send own beacon encodeBeacon(output, sizeof(output), frameCounter, beaconDelay, hostname); sendMessage(fd, output, adresse, port); superframeStartTime = (frameCounter * 100LL /* msec */*1000*1000) + timeOffset; // configure timer for next slottime tspec.it_interval.tv_sec = 0; tspec.it_interval.tv_nsec = 0; nsec2timespec( &tspec.it_value, superframeStartTime + timeOffsetMidOwnSlot); timer_settime(timer, TIMER_ABSTIME, &tspec, NULL); state = AWAIT_SLOT_TIME; break; // case SIGALRM case SIGIO: if( buf[0] == 'B' ){ rc = decodeBeacon( buf, &frameCounter, &beaconDelay, NULL, 0); if( rc < 0 ){ printf( "### Invalid Beacon: '%s'\n", buf ); } else { printf("beacon arrived before sending own beacon.\n"); timeOffsetExtern = timespec2nsec(&now) - (frameCounter * 100LL /* msec */*1000*1000) - beaconDelay; // check, if other starttime is before own calculated offset and adjust offset if needed if(timeOffsetExtern < timeOffset){ timeOffset = timeOffsetExtern; } //Berechne den Zeitpunkt, an dem der Superframe begann superframeStartTime = (frameCounter * 100LL /* msec */*1000*1000) + timeOffset; // configure timer for next slottime tspec.it_interval.tv_sec = 0; tspec.it_interval.tv_nsec = 0; nsec2timespec( &tspec.it_value, superframeStartTime + timeOffsetMidOwnSlot); timer_settime(timer, TIMER_ABSTIME, &tspec, NULL); state = AWAIT_SLOT_TIME; } } else { #ifdef DEBUG printf("### Received Message is no beacon: '%s'\n",buf); #endif // DEBUG } break; // case SIGIO case SIGINT: printf("received SIGINT, shutting down.\n"); finished = 1; break; // case SIGINT } // switch signr break; // case AWAIT_BEACON case AWAIT_SLOT_TIME: switch(signr){ case SIGALRM: // send own datagram encodeSlotMessage(output, sizeof(output), slotnummer, hostname); sendMessage(fd, output, adresse, port); // calculate own random delay for next beacon beaconDelay = randomNumber(20 /*msec*/ *1000*1000); // configure timer for next beacon send time tspec.it_interval.tv_sec = 0; tspec.it_interval.tv_nsec = 0; nsec2timespec( &tspec.it_value, superframeStartTime + 100LL /*msec*/ *1000*1000 + beaconDelay); timer_settime(timer, TIMER_ABSTIME, &tspec, NULL); state = AWAIT_BEACON; break; // case SIGALRM case SIGIO: //TODO: maybe error message? break; // case SIGIO case SIGINT: printf("received SIGINT, shutting down.\n"); finished = 1; break; // case SIGINT } // switch signr break; // case AWAIT_SLOT_TIME } // switch state } // while finished == 0 //////////////////////////////////////////////////// //und aufraeumen timer_delete(timer); /* switch to normal */ schedp.sched_priority = 0; sched_setscheduler(0, SCHED_OTHER, &schedp); return 0; }