src/testsuite/clocktest/clocktest.c

   1 /*
   2  * Copyright (C) 2007 Jan Kiszka <jan.kiszka@web.de>.
   3  *
   4  * Xenomai is free software; you can redistribute it and/or modify it
   5  * under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Xenomai is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Xenomai; if not, write to the Free Software Foundation,
  16  * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  17  */
  18
  19 #include <errno.h>
  20 #include <pthread.h>
  21 #include <sched.h>
  22 #include <signal.h>
  23 #include <stdio.h>
  24 #include <stdlib.h>
  25 #include <string.h>
  26 #include <time.h>
  27 #include <unistd.h>
  28 #include <sys/syscall.h>
  29 #include <sys/mman.h>
  30 #include <sys/time.h>
  31 #include <nucleus/vdso.h>
  32
  33 #include <xeno_config.h>
  34
  35 #ifndef HAVE_RECENT_SETAFFINITY
  36 #ifdef HAVE_OLD_SETAFFINITY
  37 #define sched_setaffinity(pid, len, mask)       sched_setaffinity(pid, mask)
  38 #else /* !HAVE_OLD_SETAFFINITY */
  39 #ifndef __cpu_set_t_defined
  40 typedef unsigned long cpu_set_t;
  41 #endif
  42 #define sched_setaffinity(pid, len, mask)       do { } while (0)
  43 #ifndef CPU_ZERO
  44 #define CPU_ZERO(set)                           memset(set, 0, sizeof(*set))
  45 #define CPU_SET(n, set)                         do { } while (0)
  46 #endif
  47 #endif /* !HAVE_OLD_SETAFFINITY */
  48 #endif /* !HAVE_RECENT_SETAFFINITY */
  49
  50 /*
  51  * We can't really trust POSIX headers to check for features, since
  52  * some archs may not implement all of the declared uClibc POSIX
  53  * features (e.g. NIOS2).
  54  */
  55 #ifdef HAVE_PTHREAD_SPIN_LOCK
  56 pthread_spinlock_t lock;
  57 #define init_lock(lock)                         pthread_spin_init(lock, 0)
  58 #define acquire_lock(lock)                      pthread_spin_lock(lock)
  59 #define release_lock(lock)                      pthread_spin_unlock(lock)
  60 #else
  61 pthread_mutex_t lock;
  62 #define init_lock(lock)                         pthread_mutex_init(lock, NULL)
  63 #define acquire_lock(lock)                      pthread_mutex_lock(lock)
  64 #define release_lock(lock)                      pthread_mutex_unlock(lock)
  65 #endif
  66 unsigned long long last_common = 0;
  67 clockid_t clock_id = CLOCK_REALTIME;
  68
  69 struct per_cpu_data {
  70         unsigned long long first_tod, first_clock;
  71         int first_round;
  72         long long offset;
  73         double drift;
  74         unsigned long warps;
  75         unsigned long long max_warp;
  76         pthread_t thread;
  77 } *per_cpu_data;
  78
  79 static void show_hostrt_diagnostics(void)
  80 {
  81         if (!xnvdso_test_feature(XNVDSO_FEAT_HOST_REALTIME)) {
  82                 printf("XNVDSO_FEAT_HOST_REALTIME not available\n");
  83                 return;
  84         }
  85
  86         if (nkvdso->hostrt_data.live)
  87                 printf("hostrt data area is live\n");
  88         else {
  89                 printf("hostrt data area is not live\n");
  90                 return;
  91         }
  92
  93         printf("Sequence counter : %u\n",
  94                nkvdso->hostrt_data.seqcount.sequence);
  95         printf("wall_time_sec    : %ld\n", nkvdso->hostrt_data.wall_time_sec);
  96         printf("wall_time_nsec   : %u\n", nkvdso->hostrt_data.wall_time_nsec);
  97         printf("wall_to_monotonic\n");
  98         printf("          tv_sec : %jd\n",
  99                (intmax_t)nkvdso->hostrt_data.wall_to_monotonic.tv_sec);
 100         printf("         tv_nsec : %ld\n",
 101                nkvdso->hostrt_data.wall_to_monotonic.tv_nsec);
 102         printf("cycle_last       : %Lu\n", nkvdso->hostrt_data.cycle_last);
 103         printf("mask             : 0x%Lx\n", nkvdso->hostrt_data.mask);
 104         printf("mult             : %u\n", nkvdso->hostrt_data.mult);
 105         printf("shift            : %u\n\n", nkvdso->hostrt_data.shift);
 106 }
 107
 108 static inline unsigned long long read_clock(clockid_t clock_id)
 109 {
 110         struct timespec ts;
 111         int res;
 112
 113         res = clock_gettime(clock_id, &ts);
 114         if (res != 0) {
 115                 fprintf(stderr, "clock_gettime failed for clock id %d\n",
 116                         clock_id);
 117                 if (clock_id == CLOCK_HOST_REALTIME)
 118                         show_hostrt_diagnostics();
 119
 120                 exit(-1);
 121         }
 122         return ts.tv_nsec + ts.tv_sec * 1000000000ULL;
 123 }
 124
 125 static inline unsigned long long read_reference_clock(void)
 126 {
 127         struct timeval tv;
 128
 129         /*
 130          * Make sure we do not pick the vsyscall variant. It won't
 131          * switch us into secondary mode and can easily deadlock.
 132          */
 133         syscall(SYS_gettimeofday, &tv, NULL);
 134         return tv.tv_usec * 1000ULL + tv.tv_sec * 1000000000ULL;
 135 }
 136
 137 void check_reference(struct per_cpu_data *per_cpu_data)
 138 {
 139         unsigned long long clock_val[10], tod_val[10];
 140         long long delta, min_delta;
 141         int i, idx;
 142
 143         for (i = 0; i < 10; i++) {
 144                 tod_val[i] = read_reference_clock();
 145                 clock_val[i] = read_clock(clock_id);
 146         }
 147
 148         min_delta = tod_val[1] - tod_val[0];
 149         idx = 1;
 150
 151         for (i = 2; i < 10; i++) {
 152                 delta = tod_val[i] - tod_val[i-1];
 153                 if (delta < min_delta) {
 154                         min_delta = delta;
 155                         idx = i;
 156                 }
 157         }
 158
 159         if (per_cpu_data->first_round) {
 160                 per_cpu_data->first_round = 0;
 161
 162                 per_cpu_data->first_tod = tod_val[idx];
 163                 per_cpu_data->first_clock = clock_val[idx];
 164         } else
 165                 per_cpu_data->drift =
 166                         (clock_val[idx] - per_cpu_data->first_clock) /
 167                         (double)(tod_val[idx] - per_cpu_data->first_tod) - 1;
 168
 169         per_cpu_data->offset = clock_val[idx] - tod_val[idx];
 170 }
 171
 172 void check_time_warps(struct per_cpu_data *per_cpu_data)
 173 {
 174         int i;
 175         unsigned long long last, now;
 176         long long incr;
 177
 178         for (i = 0; i < 100; i++) {
 179                 acquire_lock(&lock);
 180                 now = read_clock(clock_id);
 181                 last = last_common;
 182                 last_common = now;
 183                 release_lock(&lock);
 184
 185                 incr = now - last;
 186                 if (incr < 0) {
 187                         acquire_lock(&lock);
 188                         per_cpu_data->warps++;
 189                         if (-incr > per_cpu_data->max_warp)
 190                                 per_cpu_data->max_warp = -incr;
 191                         release_lock(&lock);
 192                 }
 193         }
 194 }
 195
 196 void *cpu_thread(void *arg)
 197 {
 198         int cpuid = (long)arg;
 199         struct sched_param param = { .sched_priority = 1 };
 200         struct timespec delay = { 0, 0 };
 201         cpu_set_t cpu_set;
 202
 203         srandom(read_reference_clock());
 204
 205         CPU_ZERO(&cpu_set);
 206         CPU_SET(cpuid, &cpu_set);
 207         sched_setaffinity(0, sizeof(cpu_set), &cpu_set);
 208         pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);
 209
 210         while (1) {
 211                 check_reference(&per_cpu_data[cpuid]);
 212
 213                 check_time_warps(&per_cpu_data[cpuid]);
 214
 215                 delay.tv_nsec = 1000000 + random() * (100000.0 / RAND_MAX);
 216                 nanosleep(&delay, NULL);
 217         }
 218 }
 219
 220 void sighand(int signal)
 221 {
 222         exit(0);
 223 }
 224
 225 int main(int argc, char *argv[])
 226 {
 227         int cpus = sysconf(_SC_NPROCESSORS_ONLN);
 228         int i;
 229         int c;
 230         int d = 0;
 231
 232         while ((c = getopt(argc, argv, "C:T:D")) != EOF)
 233                 switch (c) {
 234                 case 'C':
 235                         clock_id = atoi(optarg);
 236                         break;
 237
 238                 case 'T':
 239                         alarm(atoi(optarg));
 240                         break;
 241
 242                 case 'D':
 243                         d = 1;
 244                         break;
 245
 246                 default:
 247                         fprintf(stderr, "usage: clocktest [options]\n"
 248                                 "  [-C <clock_id>]              # tested clock, default=%d (CLOCK_REALTIME)\n"
 249                                 "  [-T <test_duration_seconds>] # default=0, so ^C to end\n"
 250                                 "  [-D]                         # print extra diagnostics for CLOCK_HOST_REALTIME\n",
 251                                 CLOCK_REALTIME);
 252                         exit(2);
 253                 }
 254
 255         mlockall(MCL_CURRENT | MCL_FUTURE);
 256
 257         signal(SIGALRM, sighand);
 258
 259         init_lock(&lock);
 260
 261         if (d && clock_id == CLOCK_HOST_REALTIME)
 262                 show_hostrt_diagnostics();
 263
 264         per_cpu_data = malloc(sizeof(*per_cpu_data) * cpus);
 265         if (!per_cpu_data) {
 266                 fprintf(stderr, "%s\n", strerror(ENOMEM));
 267                 exit(1);
 268         }
 269         memset(per_cpu_data, 0, sizeof(*per_cpu_data) * cpus);
 270
 271         for (i = 0; i < cpus; i++) {
 272                 per_cpu_data[i].first_round = 1;
 273                 pthread_create(&per_cpu_data[i].thread, NULL, cpu_thread,
 274                                (void *)(long)i);
 275         }
 276
 277         printf("== Tested clock: %d (", clock_id);
 278         switch (clock_id) {
 279         case CLOCK_REALTIME:
 280                 printf("CLOCK_REALTIME");
 281                 break;
 282
 283         case CLOCK_MONOTONIC:
 284                 printf("CLOCK_MONOTONIC");
 285                 break;
 286
 287         case CLOCK_HOST_REALTIME:
 288                 printf("CLOCK_HOST_REALTIME");
 289                 break;
 290
 291         default:
 292                 printf("<unknown>");
 293                 break;
 294         }
 295         printf(")\nCPU      ToD offset [us] ToD drift [us/s]      warps max delta [us]\n"
 296                "--- -------------------- ---------------- ---------- --------------\n");
 297
 298         while (1) {
 299                 for (i = 0; i < cpus; i++)
 300                         printf("%3d %20.1f %16.3f %10lu %14.1f\n",
 301                                i,
 302                                per_cpu_data[i].offset/1000.0,
 303                                per_cpu_data[i].drift * 1000000.0,
 304                                per_cpu_data[i].warps,
 305                                per_cpu_data[i].max_warp/1000.0);
 306                 usleep(250000);
 307                 printf("\033[%dA", cpus);
 308         }
 309 }