clock.c 2.73 KB
Newer Older
1
2
#include <sys/time.h>
#include <stddef.h>
3
4
5
#include <stdlib.h>
#include "../util/common.h"
#include "../util/dbg.h"
6
#include "clock.h"
7
#include "packet.h"
8

9
prrtTimestamp_t PrrtClock_get_current_time_us()
10
{
11
12
    struct timeval tv;
    gettimeofday(&tv, NULL);
13
    return (prrtTimestamp_t) (1000000 * tv.tv_sec + tv.tv_usec);
14
}
15

Andreas Schmidt's avatar
Andreas Schmidt committed
16
prrtTimestamp_t PrrtClock_get_prrt_time_us(PrrtClock *clock)
17
{
Andreas Schmidt's avatar
Andreas Schmidt committed
18
19
    prrtTimestamp_t currentTime = PrrtClock_get_current_time_us();
    prrtTimestamp_t virtualTime = clock->virtualTime;
20
21

    if(virtualTime) {
Andreas Schmidt's avatar
Andreas Schmidt committed
22
        prrtTimestamp_t lastMeasurement = clock->lastMeasurement;
Andreas Schmidt's avatar
Andreas Schmidt committed
23
24
25
        prrtTimeDifference_t diff = diff_abs_ts(currentTime, lastMeasurement);
        prrtTimeDifference_t skew = (diff * clock->skew) / 400;
        return (prrtTimestamp_t) virtualTime + clock->meanDeviation + skew;
26
27
28
    } else {
        return currentTime;
    }
29
}
30
31


Andreas Schmidt's avatar
Andreas Schmidt committed
32
bool PrrtClock_update(PrrtClock *clock, uint32_t referenceTime, uint32_t rtt)
33
{
Andreas Schmidt's avatar
Andreas Schmidt committed
34
35
    prrtTimestamp_t currentTime = PrrtClock_get_current_time_us();
    prrtTimestamp_t virtualTime = clock->virtualTime;
Andreas Schmidt's avatar
Andreas Schmidt committed
36
    prrtTimeDifference_t clockSkew = clock->skew;
37
    prrtTimedelta_t delay = rtt/2; // half the rtt
Andreas Schmidt's avatar
Andreas Schmidt committed
38
39
    prrtTimeDifference_t phaseError = referenceTime - virtualTime + delay;
    debug("V: %13u, C: %13u, S: %13d, R: %13u, E: %13d, D: %3u", virtualTime, currentTime, clockSkew, referenceTime, phaseError, delay);
40
41
42
43
44
45
46
47

    if(abs(phaseError) > 10000) {
        clock->meanDeviation = 0;
        clock->skew = 0;
        clock->lastMeasurement = currentTime;
        clock->virtualTime = currentTime;
    }

Andreas Schmidt's avatar
Andreas Schmidt committed
48
49
50
    prrtTimeDifference_t deviationSum = (prrtTimeDifference_t) phaseError + 3 * clock->meanDeviation; // TODO: why 3?
    prrtTimeDifference_t meanDeviation = (prrtTimeDifference_t) (deviationSum / 4); // TODO: find out why???
    prrtTimeDifference_t period = diff_abs_ts(currentTime, clock->lastMeasurement);
51
52

    if(period > 0) {
Andreas Schmidt's avatar
Andreas Schmidt committed
53
        clockSkew = (meanDeviation / period) + 15 * clock->skew;
Andreas Schmidt's avatar
Andreas Schmidt committed
54
        clockSkew = (prrtTimeDifference_t) (clockSkew / 16);
55
56
    }

57
    virtualTime = virtualTime + meanDeviation + period * clockSkew / 400;
58
59
60
61
62
63

    clock->meanDeviation = meanDeviation;
    clock->skew = clockSkew;
    clock->lastMeasurement = currentTime;
    clock->virtualTime = virtualTime;

Andreas Schmidt's avatar
Andreas Schmidt committed
64
    //debug("V: %13u, C: %13u; S: %13d; M: %13u", virtualTime, currentTime, clockSkew, meanDeviation);
Andreas Schmidt's avatar
Andreas Schmidt committed
65
    return true;
66
67
}

68
69
70
PrrtClock *PrrtClock_create()
{
    PrrtClock *clock = (PrrtClock *) calloc(1, sizeof(PrrtClock));
71
72
73
74
75
    check_mem(clock);

    clock->meanDeviation = 0;
    clock->virtualTime = 0;
    clock->lastMeasurement = 0;
Andreas Schmidt's avatar
Andreas Schmidt committed
76
    clock->skew = 0;
77
78
79
80

    return clock;

    error:
81
82
    PERROR("Could not create clock.%s", "");
    return NULL;
83
84
}

85
86
bool PrrtClock_destroy(PrrtClock *clock)
{
87
88
89
    free(clock);
    return true;
}