Import code from cyclictest

clock-res/src/clockres.c

 #include <errno.h>
 #include <error.h>
+#include <inttypes.h>
 #include <pthread.h>
 #include <sched.h>
+#include <stdint.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
 #include <time.h>
 #include <unistd.h>
-#include <stdint.h>
-#include <inttypes.h>
 
 #define CLOCK_ID CLOCK_MONOTONIC
-#define NSECS_PER_SECOND INT64_C(1000000000)
+#define NSEC_PER_SEC INT64_C(1000000000)
 
 typedef struct thread_stat {
   int nb_cycles;
@@ -30,59 +30,103 @@ typedef struct main_param {
   int refresh_rate;
 } main_param_t;
 
-static inline int64_t diff_ns(struct timespec t1, struct timespec t2) {
+static inline int64_t calcdiff_ns(struct timespec t1, struct timespec t2)
+{
 	int64_t diff;
+	diff = NSEC_PER_SEC * (int64_t)((int) t1.tv_sec - (int) t2.tv_sec);
+	diff += ((int) t1.tv_nsec - (int) t2.tv_nsec);
+	return diff;
+}
+
+static inline int64_t max(int64_t a, int64_t b) { return a > b ? a : b; }
+static inline int64_t min(int64_t a, int64_t b) { return a < b ? a : b; }
+
+// Code from cyclictest
+void compute_resolution() {
+  int clock;
+  uint64_t diff;
+  int k;
+  uint64_t min_non_zero_diff = UINT64_MAX;
+  struct timespec now;
+  struct timespec prev;
+  struct timespec *time;
+  int times;
+
+  clock = CLOCK_ID;
+
+  /*
+   * Calculate how many calls to clock_gettime are needed.
+   * Then call it that many times.
+   * Goal is to collect timestamps for ~ 0.001 sec.
+   * This will reliably capture resolution <= 500 usec.
+   */
+  times = 1000;
+  clock_gettime(clock, &prev);
+  for (k = 0; k < times; k++) {
+    clock_gettime(clock, &now);
+  }
 
-  diff = NSECS_PER_SECOND * (t1.tv_sec - t2.tv_sec);
-  diff += ((int64_t)t1.tv_nsec) - ((int64_t)t2.tv_nsec);
+  diff = calcdiff_ns(now, prev);
+  if (diff == 0) {
+    /*
+     * No clock rollover occurred.
+     * Use the default value for times.
+     */
+    times = -1;
+  } else {
+    int call_time;
+    call_time = diff / times;         /* duration 1 call */
+    times = NSEC_PER_SEC / call_time; /* calls per second */
+    times /= 1000;                    /* calls per msec */
+    if (times < 1000) times = 1000;
+  }
+  /* sanity check */
+  if ((times <= 0) || (times > 100000)) times = 100000;
 
-  if( diff < 0 )
-    diff = -diff;
+  time = calloc(times, sizeof(*time));
 
-  return diff;
-}
+  for (k = 0; k < times; k++) {
+    clock_gettime(clock, &time[k]);
+  }
 
-static inline int64_t max(int64_t a, int64_t b) {
-  return a > b ? a : b;
-}
-static inline int64_t min(int64_t a, int64_t b) {
-  return a < b ? a : b;
+  prev = time[0];
+  for (k = 1; k < times; k++) {
+    diff = calcdiff_ns(time[k], prev);
+    prev = time[k];
+
+    if (diff && (diff < min_non_zero_diff)) {
+      min_non_zero_diff = diff;
+    }
+  }
+
+  free(time);
+
+  printf("measured clock resolution approximately: %llu nsec\n",
+         (unsigned long long)min_non_zero_diff);
 }
 
 // Real-time thread
 // Sends packets at a regular intervall
 static void *timerthread(void *p) {
-
-  struct timespec previous;
-  struct timespec current;
   struct sched_param priority;
 
-  thread_param_t * param = (thread_param_t *)p;
-  thread_stat_t * stat = &param->stat;
+  thread_param_t *param = (thread_param_t *)p;
+  thread_stat_t *stat = &param->stat;
 
   priority.sched_priority = param->priority;
 
   int err = sched_setscheduler(0, SCHED_FIFO, &priority);
 
-  if (err)
-    error(EXIT_FAILURE, errno, "Couldn't set priority");
+  if (err) error(EXIT_FAILURE, errno, "Couldn't set priority");
 
   stat->max_res = 0;
   stat->min_res = 1000000;
 
-  for(stat->nb_cycles = 0;; stat->nb_cycles++) {
-
-    if(param->max_cycles)
-      if(stat->nb_cycles >= param->max_cycles)
-        break;
-
-    clock_gettime(CLOCK_ID, &previous);
-    clock_gettime(CLOCK_ID, &current);
+  for (stat->nb_cycles = 0;; stat->nb_cycles++) {
+    if (param->max_cycles)
+      if (stat->nb_cycles >= param->max_cycles) break;
 
-    int64_t res = diff_ns(previous, current);
-
-    stat->max_res = max(stat->max_res, res);
-    stat->min_res = min(stat->min_res, res);
+    compute_resolution();
 
     usleep(param->interval);
   }
@@ -92,16 +136,14 @@ static void *timerthread(void *p) {
   return NULL;
 }
 
-static void process_options(int argc, char *argv[], thread_param_t * param, main_param_t * main_param) {
-
-  for(;;) {
-
+static void process_options(int argc, char *argv[], thread_param_t *param,
+                            main_param_t *main_param) {
+  for (;;) {
     int c = getopt(argc, argv, "l:p:i:r:");
 
-    if(c == -1)
-      break;
+    if (c == -1) break;
 
-    switch(c) {
+    switch (c) {
       case 'p':
         param->priority = atoi(optarg);
         break;
@@ -121,17 +163,17 @@ static void process_options(int argc, char *argv[], thread_param_t * param, main
   }
 }
 
+
 // Main thread, has non-real time priority
 // Handles the IO and creates real time threads
 int main(int argc, char *argv[]) {
-
   pthread_t thread;
   thread_param_t param;
   main_param_t main_param;
   int err;
 
   // Default values
-  param.interval= 100;
+  param.interval = 100;
   param.max_cycles = 100000;
   param.priority = 99;
 
@@ -143,18 +185,12 @@ int main(int argc, char *argv[]) {
 
   err = pthread_create(&thread, NULL, timerthread, (void *)&param);
 
-  if(err)
-    error(EXIT_FAILURE, errno, "Couldn't create thread");
+  if (err) error(EXIT_FAILURE, errno, "Couldn't create thread");
 
   for (;;) {
-
     usleep(main_param.refresh_rate);
 
-    printf("Maximum res: %" PRIi64 "ns (%d)", (param.stat.max_res), param.stat.nb_cycles);
-    printf(", minimum res: %" PRIi64 "ns (%d)\n", (param.stat.min_res), param.stat.nb_cycles);
-
-    if( param.max_cycles == param.stat.nb_cycles )
-      break;
+    if (param.max_cycles == param.stat.nb_cycles) break;
   }
 
   exit(EXIT_SUCCESS);