diff --git a/include/utils.h b/include/utils.h
index d8127db12..babe38bce 100644
--- a/include/utils.h
+++ b/include/utils.h
@@ -57,6 +57,12 @@ int resolve_addr(const char *addr, struct sockaddr_in *sa, int flags);
  */
 cpu_set_t to_cpu_set(int set);
 
+/** Get delta between two timespec structs */
+double timespec_delta(struct timespec *start, struct timespec *end);
+
+/** Get period as timespec from rate */
+struct timespec timespec_rate(double rate);
+
 /** Append an element to a single linked list */
 #define list_add(list, elm) do { \
 		elm->next = list; \
diff --git a/src/test.c b/src/test.c
index df28c40b4..02b3b136d 100644
--- a/src/test.c
+++ b/src/test.c
@@ -71,8 +71,8 @@ int main(int argc, char *argv[])
 		struct timespec *ts1 = (struct timespec *) &m.data;
 		struct timespec *ts2 = malloc(sizeof(struct timespec));
 
-		long long rtt, rtt_max = LLONG_MIN, rtt_min = LLONG_MAX;
-		long long run = 0, avg = 0;
+		double rtt, rtt_max = LLONG_MIN, rtt_min = LLONG_MAX, avg = 0;
+		int run = 0;
 
 		while (1) {
 			clock_gettime(CLOCK_REALTIME, ts1);
@@ -80,20 +80,21 @@ int main(int argc, char *argv[])
 			msg_recv(&m, &n);
 			clock_gettime(CLOCK_REALTIME, ts2);
 
-			rtt = ts2->tv_nsec - ts1->tv_nsec;
+			rtt = timespec_delta(ts1, ts2);
 
 			if (rtt < 0) continue;
 			if (rtt > rtt_max) rtt_max = rtt;
 			if (rtt < rtt_min) rtt_min = rtt;
 
 			avg += rtt;
-
-			info("rtt %.3f min %.3f max %.3f avg %.3f uS", 1e-3 * rtt, 1e-3 * rtt_min, 1e-3 * rtt_max, 1e-3 * avg / run);
-
 			run++;
+
+			info("rtt %.3f min %.3f max %.3f avg %.3f", 1e3 * rtt, 1e3 * rtt_min, 1e3 * rtt_max, 1e3 * avg / run);
+
 			m.sequence++;
-			usleep(1000);
 		}
+
+		free(ts2);
 	}
 
 	return 0;
diff --git a/src/utils.c b/src/utils.c
index 8ea792736..380e855d7 100644
--- a/src/utils.c
+++ b/src/utils.c
@@ -12,6 +12,8 @@
 #include <errno.h>
 #include <unistd.h>
 #include <netdb.h>
+#include <time.h>
+#include <math.h>
 
 #include <netinet/in.h>
 #include <arpa/inet.h>
@@ -94,3 +96,26 @@ cpu_set_t to_cpu_set(int set)
 
 	return cset;
 }
+
+double timespec_delta(struct timespec *start, struct timespec *end)
+{
+	double sec  = end->tv_sec - start->tv_sec;
+	double nsec = end->tv_nsec - start->tv_nsec;
+
+	if (nsec < 0) {
+		sec  -= 1;
+		nsec += 1e9;
+	}
+
+	return sec + nsec * 1e-9;
+}
+
+struct timespec timespec_rate(double rate)
+{
+	struct timespec ts;
+
+	ts.tv_sec  = 1 / rate;
+	ts.tv_nsec = 1.0e9 * (1 / rate - ts.tv_sec);
+
+	return ts;
+}