Commit 931eb065 authored by Joanne Hugé's avatar Joanne Hugé

Import code from cyclictest

parent d1170af3
#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) {
int64_t diff;
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;
}
diff = NSECS_PER_SECOND * (t1.tv_sec - t2.tv_sec);
diff += ((int64_t)t1.tv_nsec) - ((int64_t)t2.tv_nsec);
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);
}
if( diff < 0 )
diff = -diff;
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;
return diff;
}
time = calloc(times, sizeof(*time));
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;
for (k = 0; k < times; k++) {
clock_gettime(clock, &time[k]);
}
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++) {
for (stat->nb_cycles = 0;; stat->nb_cycles++) {
if (param->max_cycles)
if (stat->nb_cycles >= param->max_cycles) break;
if(param->max_cycles)
if(stat->nb_cycles >= param->max_cycles)
break;
clock_gettime(CLOCK_ID, &previous);
clock_gettime(CLOCK_ID, &current);
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,46 +136,44 @@ 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;
switch(c) {
case 'p':
param->priority = atoi(optarg);
break;
case 'l':
param->max_cycles = atoi(optarg);
break;
case 'i':
param->interval = atoi(optarg);
break;
case 'r':
main_param->refresh_rate = atoi(optarg);
break;
default:
exit(EXIT_FAILURE);
break;
if (c == -1) break;
switch (c) {
case 'p':
param->priority = atoi(optarg);
break;
case 'l':
param->max_cycles = atoi(optarg);
break;
case 'i':
param->interval = atoi(optarg);
break;
case 'r':
main_param->refresh_rate = atoi(optarg);
break;
default:
exit(EXIT_FAILURE);
break;
}
}
}
// 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);
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment