/*
 * Copyright (C) 2007 Jan Kiszka <jan.kiszka@web.de>.
 *
 * Xenomai is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * Xenomai is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Xenomai; if not, write to the Free Software Foundation,
 * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */
#include <errno.h>
#include <pthread.h>
#include <sched.h>
#include <signal.h>
#include <stdio.h>
#include <error.h>
#include <stdlib.h>
#include <stdint.h>
#include <ctype.h>
#include <string.h>
#include <time.h>
#include <unistd.h>
#include <sys/syscall.h>
#include <sys/time.h>
#include <boilerplate/ancillaries.h>
#include <boilerplate/atomic.h>
#include <cobalt/uapi/kernel/vdso.h>
#include <xeno_config.h>

extern struct xnvdso *cobalt_vdso;

/*
 * We can't really trust POSIX headers to check for features, since
 * some archs may not implement all of the declared uClibc POSIX
 * features (e.g. NIOS2).
 */
#ifdef HAVE_PTHREAD_SPIN_LOCK
pthread_spinlock_t lock;
#define init_lock(lock)				pthread_spin_init(lock, 0)
#define acquire_lock(lock)			pthread_spin_lock(lock)
#define release_lock(lock)			pthread_spin_unlock(lock)
#else
pthread_mutex_t lock;
#define init_lock(lock)				pthread_mutex_init(lock, NULL)
#define acquire_lock(lock)			pthread_mutex_lock(lock)
#define release_lock(lock)			pthread_mutex_unlock(lock)
#endif

static uint64_t last_common = 0;
static clockid_t clock_id = CLOCK_REALTIME;
static cpu_set_t cpu_realtime_set, cpu_online_set;

struct per_cpu_data {
	uint64_t first_tod, first_clock;
	int first_round;
	int64_t offset;
	double drift;
	unsigned long warps;
	uint64_t max_warp;
	pthread_t thread;
} *per_cpu_data;

static void show_hostrt_diagnostics(void)
{
	if (!xnvdso_test_feature(cobalt_vdso, XNVDSO_FEAT_HOST_REALTIME)) {
		printf("XNVDSO_FEAT_HOST_REALTIME not available\n");
		return;
	}

	if (cobalt_vdso->hostrt_data.live)
		printf("hostrt data area is live\n");
	else {
		printf("hostrt data area is not live\n");
		return;
	}

	printf("sequence counter : %u\n",
	       cobalt_vdso->hostrt_data.lock.sequence);
	printf("wall_time_sec    : %lld\n",
	       (unsigned long long)cobalt_vdso->hostrt_data.wall_sec);
	printf("wall_time_nsec   : %u\n", cobalt_vdso->hostrt_data.wall_nsec);
	printf("wall_to_monotonic_sec    : %lld\n",
	       (unsigned long long)cobalt_vdso->hostrt_data.wtom_sec);
	printf("wall_to_monotonic_nsec   : %u\n", cobalt_vdso->hostrt_data.wtom_nsec);
	printf("cycle_last       : %Lu\n", (long long)cobalt_vdso->hostrt_data.cycle_last);
	printf("mask             : 0x%Lx\n", (long long)cobalt_vdso->hostrt_data.mask);
	printf("mult             : %u\n", cobalt_vdso->hostrt_data.mult);
	printf("shift            : %u\n\n", cobalt_vdso->hostrt_data.shift);
}

static void show_realtime_offset(void)
{
	if (!xnvdso_test_feature(cobalt_vdso, XNVDSO_FEAT_HOST_REALTIME)) {
		printf("XNVDSO_FEAT_WALLCLOCK_OFFSET not available\n");
		return;
	}

	printf("Wallclock offset : %llu\n", (long long)cobalt_vdso->wallclock_offset);
}

static inline uint64_t read_clock(clockid_t clock_id)
{
	struct timespec ts;
	int res;

	res = clock_gettime(clock_id, &ts);
	if (res != 0) {
		fprintf(stderr, "clock_gettime failed for clock id %d\n",
			clock_id);
		if (clock_id == CLOCK_HOST_REALTIME)
			show_hostrt_diagnostics();
		else if (clock_id == CLOCK_REALTIME)
			show_realtime_offset();

		exit(-1);
	}
	return ts.tv_nsec + ts.tv_sec * 1000000000ULL;
}

static inline uint64_t read_reference_clock(void)
{
	struct timeval tv;

	/*
	 * Make sure we do not pick the vsyscall variant. It won't
	 * switch us into secondary mode and can easily deadlock.
	 */
	syscall(SYS_gettimeofday, &tv, NULL);
	return tv.tv_usec * 1000ULL + tv.tv_sec * 1000000000ULL;
}

static void check_reference(struct per_cpu_data *per_cpu_data)
{
	uint64_t clock_val[10], tod_val[10];
	int64_t delta, min_delta;
	int i, idx;

	for (i = 0; i < 10; i++) {
		tod_val[i] = read_reference_clock();
		clock_val[i] = read_clock(clock_id);
	}

	min_delta = tod_val[1] - tod_val[0];
	idx = 1;

	for (i = 2; i < 10; i++) {
		delta = tod_val[i] - tod_val[i-1];
		if (delta < min_delta) {
			min_delta = delta;
			idx = i;
		}
	}

	if (per_cpu_data->first_round) {
		per_cpu_data->first_round = 0;

		per_cpu_data->first_tod = tod_val[idx];
		per_cpu_data->first_clock = clock_val[idx];
	} else
		per_cpu_data->drift =
			(clock_val[idx] - per_cpu_data->first_clock) /
			(double)(tod_val[idx] - per_cpu_data->first_tod) - 1;

	per_cpu_data->offset = clock_val[idx] - tod_val[idx];
}

static void check_time_warps(struct per_cpu_data *per_cpu_data)
{
	int i;
	uint64_t last, now;
	int64_t incr;

	for (i = 0; i < 100; i++) {
		acquire_lock(&lock);
		now = read_clock(clock_id);
		last = last_common;
		last_common = now;
		release_lock(&lock);

		incr = now - last;
		if (incr < 0) {
			acquire_lock(&lock);
			per_cpu_data->warps++;
			if (-incr > per_cpu_data->max_warp)
				per_cpu_data->max_warp = -incr;
			release_lock(&lock);
		}
	}
}

static void *cpu_thread(void *arg)
{
	int cpuid = (long)arg;
	struct sched_param param = { .sched_priority = 1 };
	struct timespec delay = { 0, 0 };
	cpu_set_t cpu_set;

	srandom(read_reference_clock());

	CPU_ZERO(&cpu_set);
	CPU_SET(cpuid, &cpu_set);
	sched_setaffinity(0, sizeof(cpu_set), &cpu_set);
	pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);

	while (1) {
		check_reference(&per_cpu_data[cpuid]);

		check_time_warps(&per_cpu_data[cpuid]);

		delay.tv_nsec = 1000000 + random() * (100000.0 / RAND_MAX);
		nanosleep(&delay, NULL);
	}

	return NULL;
}

static void sighand(int signal)
{
	exit(0);
}

static clockid_t resolve_clock_name(const char *name,
				    const char **real_name, int ext)
{
	char *path, buf[BUFSIZ];
	clockid_t clock_id;
	FILE *fp;
	int id;

	*real_name = name;

	if (!ext) {
		if (isdigit(*name)) {
			clock_id = (clockid_t)atoi(name);
			switch (clock_id) {
			case CLOCK_REALTIME:
				*real_name = "CLOCK_REALTIME";
				break;
			case CLOCK_MONOTONIC:
				*real_name = "CLOCK_MONOTONIC";
				break;
			case CLOCK_MONOTONIC_RAW:
				*real_name = "CLOCK_MONOTONIC_RAW";
				break;
			case CLOCK_HOST_REALTIME:
				*real_name = "CLOCK_HOST_REALTIME";
				break;
			default:
				error(1, EINVAL, "bad built-in clock id '%d'",
				      clock_id);
			}
			return clock_id;
		}
		if (strcmp(name, "CLOCK_REALTIME") == 0)
			return CLOCK_REALTIME;
		if (strcmp(name, "CLOCK_MONOTONIC") == 0)
			return CLOCK_MONOTONIC;
		if (strcmp(name, "CLOCK_MONOTONIC_RAW") == 0)
			return CLOCK_MONOTONIC_RAW;
		if (strcmp(name, "CLOCK_HOST_REALTIME") == 0)
			return CLOCK_HOST_REALTIME;
		if (strcmp(name, "coreclk") == 0) {
			/* Read the core clock as CLOCK_MONOTONIC_RAW */
			*real_name = "CLOCK_MONOTONIC_RAW";
			return CLOCK_MONOTONIC_RAW;
		}
	}

	if (!isdigit(*name)) {
		if (asprintf(&path, "/proc/xenomai/clock/%s", name) < 0)
			error(1, ENOMEM, "resolve_clock_name");
		fp = fopen(path, "r");
		if (fp == NULL)
			error(1, EINVAL, "bad extension clock name '%s'", name);
		if (fgets(buf, sizeof(buf), fp) == NULL)
			error(1, EIO, "cannot read %s", path);
		free(path);
		if (sscanf(buf, "%*[ ]id: %d\n", &id) != 1)
			error(1, EINVAL, "bad extension clock name '%s'", name);
		return __COBALT_CLOCK_EXT(id);
	}

	id = atoi(name);
	if (!__COBALT_CLOCK_EXT_P(id) ||
	    __COBALT_CLOCK_EXT_INDEX(id) >= COBALT_MAX_EXTCLOCKS)
		error(1, EINVAL, "bad extension clock id '%d'", id);
		
	*real_name = "CLOCK_UNKNOWN";

	return __COBALT_CLOCK_EXT(id);
}

int main(int argc, char *argv[])
{
	const char *clock_name = NULL, *real_clock_name = "CLOCK_REALTIME";
	int max_cpu, cpus;
	int i;
	int c;
	int d = 0;
	int ext = 0;

	while ((c = getopt(argc, argv, "C:ET:D")) != EOF)
		switch (c) {
		case 'C':
			clock_name = optarg;
			break;

		case 'E':
			ext = 1;
			break;

		case 'T':
			alarm(atoi(optarg));
			break;

		case 'D':
			d = 1;
			break;

		default:
			fprintf(stderr, "usage: clocktest [options]\n"
				"  [-C <clock_id|clock_name>]   # tested clock, defaults to CLOCK_REALTIME\n"
				"  [-E]                         # -C specifies extension clock\n"
				"  [-T <test_duration_seconds>] # default=0, so ^C to end\n"
				"  [-D]                         # print extra diagnostics for CLOCK_HOST_REALTIME\n");
			exit(2);
		}

	if (clock_name)
		clock_id = resolve_clock_name(clock_name, &real_clock_name, ext);

	signal(SIGALRM, sighand);

	init_lock(&lock);

	if (d && clock_id == CLOCK_HOST_REALTIME)
		show_hostrt_diagnostics();

	if (get_realtime_cpu_set(&cpu_realtime_set) != 0)
		error(1, ENOSYS, "get_realtime_cpu_set");

	if (get_online_cpu_set(&cpu_online_set) != 0)
		error(1, ENOSYS, "get_online_cpu_set");

	CPU_AND(&cpu_realtime_set, &cpu_realtime_set, &cpu_online_set);

	max_cpu = 0;
	cpus = 0;
	for (i = 0; i < CPU_SETSIZE; i++) {
		if (!CPU_ISSET(i, &cpu_realtime_set))
			continue;
		cpus++;
		if (i > max_cpu)
			max_cpu = i;
	}

	per_cpu_data = malloc(sizeof(*per_cpu_data) * (max_cpu + 1));
	if (per_cpu_data == NULL)
		error(1, ENOMEM, "malloc");

	memset(per_cpu_data, 0, sizeof(*per_cpu_data) * (max_cpu + 1));

	for (i = 0; i <= max_cpu; i++) {
		if (!CPU_ISSET(i, &cpu_realtime_set))
			continue;
		per_cpu_data[i].first_round = 1;
		pthread_create(&per_cpu_data[i].thread, NULL, cpu_thread,
			       (void *)(long)i);
	}

	printf("== Testing %s %s (%d)\n",
	       ext ? "extension" : "built-in", real_clock_name, clock_id);
	printf("CPU      ToD offset [us] ToD drift [us/s]      warps max delta [us]\n"
	       "--- -------------------- ---------------- ---------- --------------\n");

	while (1) {
		for (i = 0; i <= max_cpu; i++)
			if (CPU_ISSET(i, &cpu_realtime_set))
				printf("%3d %20.1f %16.3f %10lu %14.1f\n",
				       i,
				       per_cpu_data[i].offset/1000.0,
				       per_cpu_data[i].drift * 1000000.0,
				       per_cpu_data[i].warps,
				       per_cpu_data[i].max_warp/1000.0);
		usleep(250000);
		printf("\033[%dA", cpus);
	}
}