/*-
 * Copyright (c) 2010 Andriy Gapon
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 * $FreeBSD$
 */

#include <sys/param.h>
#include <sys/types.h>
#include <sys/systm.h>
#include <sys/kernel.h>
#include <sys/conf.h>
#include <sys/linker.h>
#include <sys/limits.h>
#include <sys/module.h>
#include <sys/pcpu.h>
#include <sys/proc.h>
#include <sys/sched.h>
#include <sys/smp.h>

#include <machine/cpufunc.h>

volatile static uint64_t hst_cpu_tsc;
volatile static uint64_t tgt_cpu_tsc;
static uint64_t tsc_after;


static void
tsc_sync_init(void *arg)
{
	uintptr_t cpu = (uintptr_t) arg;
	uint64_t t;

	if (cpu != curcpu)
		return;

	/* Re-fill the cache line */
	t = hst_cpu_tsc;
	mfence();
}

static void
tsc_sync_master(void)
{

	hst_cpu_tsc = rdtsc();
	mfence();
	tsc_after = rdtsc();
}

static void
tsc_sync_slave(void)
{
	uint64_t t;

	/* XXX CDDL start*/
	do {
		/*
		 * Do not put an cpu_spinwait() here. For instance,
		 * if the master and slave are really the same
		 * hyper-threaded CPU, then you want the master
		 * to yield to the slave as quickly as possible here,
		 * but not the other way.
		 */
		t = rdtsc();
	} while (hst_cpu_tsc == 0);
	/* XXX CDDL end*/
	tgt_cpu_tsc = t;
	mfence();
}

static void
tsc_sync(void *arg)
{
	uintptr_t cpu = (uintptr_t) arg;

	if (cpu == curcpu)
		tsc_sync_slave();
	else
		tsc_sync_master();
}

static void
tsc_check(void)
{
	struct pcpu *pc;
	cpumask_t map;
	int64_t write_time;
	int64_t min_write_time;
	int64_t tdelta;
	int64_t x;
	int i, j;

	/* The current CPU is the reference one. */
	sched_pin();
	CPU_FOREACH(i) {
		if (i == curcpu)
			continue;

		pc = pcpu_find(i);
		map = PCPU_GET(cpumask) | pc->pc_cpumask;

		min_write_time = LONG_MAX;
		tdelta = 0;
		for (j = 0; j < 100; j++) {
			smp_rendezvous_cpus(map, tsc_sync_init,
			    tsc_sync,
			    smp_no_rendevous_barrier, (void *)(uintptr_t) i);
			if (bootverbose) {
				uprintf("cpus %d-%d, iteration %d, master %ju, slave %ju, after %ju\n",
				    curcpu, i, j, (uintmax_t)hst_cpu_tsc, (uintmax_t)tgt_cpu_tsc,
				    (uintmax_t)tsc_after);
				uprintf("cpus %d-%d, iteration %d, write_time %ju, delta %jd\n",
				    curcpu, i, j, (uintmax_t)(tsc_after - hst_cpu_tsc),
				    (intmax_t)(int64_t)tgt_cpu_tsc - tsc_after);
			}
			/* XXX CDDL start*/
			write_time =  tsc_after - hst_cpu_tsc;
			if (write_time <= min_write_time) {
				min_write_time = write_time;
				/*
				 * Apply heuristic adjustment only if the calculated
				 * delta is > 1/4th of the write time.
				 */
				x = tgt_cpu_tsc - tsc_after;
				if (x < 0)
					x = -x;
				if (x > (min_write_time/4))
					/*
					 * Subtract 1/4th of the measured write time
					 * from the master's TSC value, as an estimate
					 * of how late the mfence completion came
					 * after the slave noticed the cache line
					 * change.
					 */
					tdelta = tgt_cpu_tsc -
					    (tsc_after - (min_write_time/4));
				else
					tdelta = tgt_cpu_tsc - tsc_after;
			}
			/* XXX CDDL end*/
			tgt_cpu_tsc = hst_cpu_tsc = tsc_after = 0;
			mfence();
		}
		uprintf("cpus %d-%d, min_write_time = %ju, tdelta = %jd\n",
		    curcpu, i,
		    (uintmax_t)min_write_time, (intmax_t)tdelta);
		if (tdelta < 0)
			tdelta = -tdelta;
		if (tdelta > min_write_time)
			uprintf("cpus %d-%d, TSCs are considered to be OUT of sync\n",
			    curcpu, i);
		else
			uprintf("cpus %d-%d, TSCs are considered to be IN sync\n",
			    curcpu, i);
	}
	sched_unpin();

}

static int
tsc_modevent(module_t mod __unused, int type, void *data __unused)
{
	switch (type) {
	case MOD_LOAD:
		tsc_check();
		break;
	case MOD_UNLOAD:
	case MOD_SHUTDOWN:
		break;
	default:
		return (EOPNOTSUPP);
	}
	return (0);
}

DEV_MODULE(tsc, tsc_modevent, NULL);