1 files changed, 87 insertions, 27 deletions

diff --git a/src/clocks.c b/src/clocks.c
index c90a07c2f..879ebf911 100644
--- a/src/clocks.c
+++ b/src/clocks.c
@@ -6,64 +6,89 @@ inline int clock_gettime(clockid_t clk_id, struct timespec *ts) {
     if(unlikely(gettimeofday(&tv, NULL) == -1))
         return -1;
     ts->tv_sec = tv.tv_sec;
-    ts->tv_nsec = tv.tv_usec * NSEC_PER_USEC;
+    ts->tv_nsec = (tv.tv_usec % USEC_PER_SEC) * NSEC_PER_USEC;
     return 0;
 }
 #endif
 
-inline time_t now_realtime_sec(void) {
+static inline time_t now_sec(clockid_t clk_id) {
     struct timespec ts;
-    if(unlikely(clock_gettime(CLOCK_REALTIME, &ts) == -1))
+    if(unlikely(clock_gettime(clk_id, &ts) == -1))
         return 0;
     return ts.tv_sec;
 }
 
-inline int now_realtime_timeval(struct timeval *tv) {
+static inline usec_t now_usec(clockid_t clk_id) {
+    struct timespec ts;
+    if(unlikely(clock_gettime(clk_id, &ts) == -1))
+        return 0;
+    return (usec_t)ts.tv_sec * USEC_PER_SEC + (ts.tv_nsec % NSEC_PER_SEC) / NSEC_PER_USEC;
+}
+
+static inline int now_timeval(clockid_t clk_id, struct timeval *tv) {
     struct timespec ts;
-    if(unlikely(clock_gettime(CLOCK_REALTIME, &ts) == -1))
+
+    if(unlikely(clock_gettime(clk_id, &ts) == -1)) {
+        tv->tv_sec = 0;
+        tv->tv_usec = 0;
         return -1;
+    }
+
     tv->tv_sec = ts.tv_sec;
-    tv->tv_usec = ts.tv_nsec / NSEC_PER_USEC;
+    tv->tv_usec = (suseconds_t)((ts.tv_nsec % NSEC_PER_SEC) / NSEC_PER_USEC);
     return 0;
 }
 
+inline time_t now_realtime_sec(void) {
+    return now_sec(CLOCK_REALTIME);
+}
+
 inline usec_t now_realtime_usec(void) {
-    struct timespec ts;
-    if(unlikely(clock_gettime(CLOCK_REALTIME, &ts) == -1))
-        return 0;
-    return (usec_t)ts.tv_sec * USEC_PER_SEC + ts.tv_nsec / NSEC_PER_USEC;
+    return now_usec(CLOCK_REALTIME);
+}
+
+inline int now_realtime_timeval(struct timeval *tv) {
+    return now_timeval(CLOCK_REALTIME, tv);
 }
 
 inline time_t now_monotonic_sec(void) {
-    struct timespec ts;
-    if(unlikely(clock_gettime(CLOCK_MONOTONIC, &ts) == -1))
-        return 0;
-    return ts.tv_sec;
+    return now_sec(CLOCK_MONOTONIC);
 }
 
 inline usec_t now_monotonic_usec(void) {
-    struct timespec ts;
-    if(unlikely(clock_gettime(CLOCK_MONOTONIC, &ts) == -1))
-        return 0;
-    return (usec_t)ts.tv_sec * USEC_PER_SEC + ts.tv_nsec / NSEC_PER_USEC;
+    return now_usec(CLOCK_MONOTONIC);
+}
+
+inline int now_monotonic_timeval(struct timeval *tv) {
+    return now_timeval(CLOCK_MONOTONIC, tv);
 }
 
 inline time_t now_boottime_sec(void) {
-    struct timespec ts;
-    if(unlikely(clock_gettime(CLOCK_BOOTTIME, &ts) == -1))
-        return 0;
-    return ts.tv_sec;
+    return now_sec(CLOCK_BOOTTIME);
 }
 
 inline usec_t now_boottime_usec(void) {
-    struct timespec ts;
-    if(unlikely(clock_gettime(CLOCK_BOOTTIME, &ts) == -1))
-        return 0;
-    return (usec_t)ts.tv_sec * USEC_PER_SEC + ts.tv_nsec / NSEC_PER_USEC;
+    return now_usec(CLOCK_BOOTTIME);
+}
+
+inline int now_boottime_timeval(struct timeval *tv) {
+    return now_timeval(CLOCK_BOOTTIME, tv);
 }
 
 inline usec_t timeval_usec(struct timeval *tv) {
-    return (usec_t)tv->tv_sec * USEC_PER_SEC + tv->tv_usec;
+    return (usec_t)tv->tv_sec * USEC_PER_SEC + (tv->tv_usec % USEC_PER_SEC);
+}
+
+inline msec_t timeval_msec(struct timeval *tv) {
+    return (msec_t)tv->tv_sec * MSEC_PER_SEC + ((tv->tv_usec % USEC_PER_SEC) / MSEC_PER_SEC);
+}
+
+inline susec_t dt_usec_signed(struct timeval *now, struct timeval *old) {
+    usec_t ts1 = timeval_usec(now);
+    usec_t ts2 = timeval_usec(old);
+
+    if(likely(ts1 >= ts2)) return (susec_t)(ts1 - ts2);
+    return -((susec_t)(ts2 - ts1));
 }
 
 inline usec_t dt_usec(struct timeval *now, struct timeval *old) {
@@ -71,3 +96,38 @@ inline usec_t dt_usec(struct timeval *now, struct timeval *old) {
     usec_t ts2 = timeval_usec(old);
     return (ts1 > ts2) ? (ts1 - ts2) : (ts2 - ts1);
 }
+
+inline void heartbeat_init(heartbeat_t *hb)
+{
+    *hb = 0ULL;
+}
+
+usec_t heartbeat_next(heartbeat_t *hb, usec_t tick)
+{
+    heartbeat_t now = now_monotonic_usec();
+    usec_t next = now - (now % tick) + tick;
+
+    while(now < next) {
+        sleep_usec(next - now);
+        now = now_monotonic_usec();
+    }
+
+    if(likely(*hb != 0ULL)) {
+        usec_t dt = now - *hb;
+        *hb = now;
+        if(unlikely(dt / tick > 1))
+            error("heartbeat missed between %llu usec and %llu usec", *hb, now);
+        return dt;
+    }
+    else {
+        *hb = now;
+        return 0ULL;
+    }
+}
+
+inline usec_t heartbeat_dt_usec(heartbeat_t *hb)
+{
+    if(!*hb)
+        return 0ULL;
+    return now_monotonic_usec() - *hb;
+}