/*
 * Copyright (C) 2005 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 <linux/module.h>
#include <linux/slab.h>
#include <linux/semaphore.h>
#include <cobalt/kernel/trace.h>
#include <cobalt/kernel/arith.h>
#include <rtdm/testing.h>
#include <rtdm/driver.h>
#include <rtdm/compat.h>

MODULE_DESCRIPTION("Timer latency test helper");
MODULE_AUTHOR("Jan Kiszka <jan.kiszka@web.de>");
MODULE_VERSION("0.2.1");
MODULE_LICENSE("GPL");

struct rt_tmbench_context {
	int mode;
	unsigned int period;
	int freeze_max;
	int warmup_loops;
	int samples_per_sec;
	int32_t *histogram_min;
	int32_t *histogram_max;
	int32_t *histogram_avg;
	int histogram_size;
	int bucketsize;

	rtdm_task_t timer_task;

	rtdm_timer_t timer;
	int warmup;
	uint64_t start_time;
	uint64_t date;
	struct rttst_bench_res curr;

	rtdm_event_t result_event;
	struct rttst_interm_bench_res result;

	struct semaphore nrt_mutex;
};

static inline void add_histogram(struct rt_tmbench_context *ctx,
				 __s32 *histogram, __s32 addval)
{
	/* bucketsize steps */
	int inabs = (addval >= 0 ? addval : -addval) / ctx->bucketsize;
	histogram[inabs < ctx->histogram_size ?
		  inabs : ctx->histogram_size - 1]++;
}

static inline long long slldiv(long long s, unsigned d)
{
	return s >= 0 ? xnarch_ulldiv(s, d, NULL) : -xnarch_ulldiv(-s, d, NULL);
}

static void eval_inner_loop(struct rt_tmbench_context *ctx, __s32 dt)
{
	if (dt > ctx->curr.max)
		ctx->curr.max = dt;
	if (dt < ctx->curr.min)
		ctx->curr.min = dt;
	ctx->curr.avg += dt;

	if (xntrace_enabled() &&
		ctx->freeze_max &&
		(dt > ctx->result.overall.max) &&
		!ctx->warmup) {
		ctx->result.overall.max = dt;
		xntrace_latpeak_freeze(dt);
	}

	ctx->date += ctx->period;

	if (!ctx->warmup && ctx->histogram_size)
		add_histogram(ctx, ctx->histogram_avg, dt);

	/* Evaluate overruns and adjust next release date.
	   Beware of signedness! */
	while (dt > 0 && (unsigned long)dt > ctx->period) {
		ctx->curr.overruns++;
		ctx->date += ctx->period;
		dt -= ctx->period;
	}
}

static void eval_outer_loop(struct rt_tmbench_context *ctx)
{
	if (!ctx->warmup) {
		if (ctx->histogram_size) {
			add_histogram(ctx, ctx->histogram_max, ctx->curr.max);
			add_histogram(ctx, ctx->histogram_min, ctx->curr.min);
		}

		ctx->result.last.min = ctx->curr.min;
		if (ctx->curr.min < ctx->result.overall.min)
			ctx->result.overall.min = ctx->curr.min;

		ctx->result.last.max = ctx->curr.max;
		if (ctx->curr.max > ctx->result.overall.max)
			ctx->result.overall.max = ctx->curr.max;

		ctx->result.last.avg =
		    slldiv(ctx->curr.avg, ctx->samples_per_sec);
		ctx->result.overall.avg += ctx->result.last.avg;
		ctx->result.overall.overruns += ctx->curr.overruns;
		rtdm_event_pulse(&ctx->result_event);
	}

	if (ctx->warmup &&
	    (ctx->result.overall.test_loops == ctx->warmup_loops)) {
		ctx->result.overall.test_loops = 0;
		ctx->warmup = 0;
	}

	ctx->curr.min = 10000000;
	ctx->curr.max = -10000000;
	ctx->curr.avg = 0;
	ctx->curr.overruns = 0;

	ctx->result.overall.test_loops++;
}

static void timer_task_proc(void *arg)
{
	struct rt_tmbench_context *ctx = arg;
	int count, err;
	spl_t s;

	/* first event: one millisecond from now. */
	ctx->date = rtdm_clock_read_monotonic() + 1000000;

	while (1) {
		for (count = 0; count < ctx->samples_per_sec; count++) {
			cobalt_atomic_enter(s);
			ctx->start_time = rtdm_clock_read_monotonic();
			err = rtdm_task_sleep_abs(ctx->date,
						  RTDM_TIMERMODE_ABSOLUTE);
			cobalt_atomic_leave(s);
			if (err)
				return;

			eval_inner_loop(ctx,
					(__s32)(rtdm_clock_read_monotonic() -
						ctx->date));
		}
		eval_outer_loop(ctx);
	}
}

static void timer_proc(rtdm_timer_t *timer)
{
	struct rt_tmbench_context *ctx =
	    container_of(timer, struct rt_tmbench_context, timer);
	int err;

	do {
		eval_inner_loop(ctx, (__s32)(rtdm_clock_read_monotonic() -
					     ctx->date));

		ctx->start_time = rtdm_clock_read_monotonic();
		err = rtdm_timer_start_in_handler(&ctx->timer, ctx->date, 0,
						  RTDM_TIMERMODE_ABSOLUTE);

		if (++ctx->curr.test_loops >= ctx->samples_per_sec) {
			ctx->curr.test_loops = 0;
			eval_outer_loop(ctx);
		}
	} while (err);
}

static int rt_tmbench_open(struct rtdm_fd *fd, int oflags)
{
	struct rt_tmbench_context *ctx;

	ctx = rtdm_fd_to_private(fd);

	ctx->mode = RTTST_TMBENCH_INVALID;
	sema_init(&ctx->nrt_mutex, 1);

	return 0;
}

static void rt_tmbench_close(struct rtdm_fd *fd)
{
	struct rt_tmbench_context *ctx;

	ctx = rtdm_fd_to_private(fd);

	down(&ctx->nrt_mutex);

	if (ctx->mode >= 0) {
		if (ctx->mode == RTTST_TMBENCH_TASK)
			rtdm_task_destroy(&ctx->timer_task);
		else if (ctx->mode == RTTST_TMBENCH_HANDLER)
			rtdm_timer_destroy(&ctx->timer);

		rtdm_event_destroy(&ctx->result_event);

		if (ctx->histogram_size)
			kfree(ctx->histogram_min);

		ctx->mode = RTTST_TMBENCH_INVALID;
		ctx->histogram_size = 0;
	}

	up(&ctx->nrt_mutex);
}

static int rt_tmbench_start(struct rtdm_fd *fd,
			    struct rt_tmbench_context *ctx,
			    struct rttst_tmbench_config __user *user_config)
{
	int err = 0;
	spl_t s;

	struct rttst_tmbench_config config_buf;
	struct rttst_tmbench_config *config =
		(struct rttst_tmbench_config *)user_config;

	if (rtdm_fd_is_user(fd)) {
		if (rtdm_safe_copy_from_user
		    (fd, &config_buf,user_config,
		     sizeof(struct rttst_tmbench_config)) < 0)
			return -EFAULT;

		config = &config_buf;
	}

	down(&ctx->nrt_mutex);

	ctx->period = config->period;
	ctx->warmup_loops = config->warmup_loops;
	ctx->samples_per_sec = 1000000000 / ctx->period;
	ctx->histogram_size = config->histogram_size;
	ctx->freeze_max = config->freeze_max;

	if (ctx->histogram_size > 0) {
		ctx->histogram_min =
		    kmalloc(3 * ctx->histogram_size * sizeof(int32_t),
			    GFP_KERNEL);
		ctx->histogram_max =
		    ctx->histogram_min + config->histogram_size;
		ctx->histogram_avg =
		    ctx->histogram_max + config->histogram_size;

		if (!ctx->histogram_min) {
			up(&ctx->nrt_mutex);
			return -ENOMEM;
		}

		memset(ctx->histogram_min, 0,
		       3 * ctx->histogram_size * sizeof(int32_t));
		ctx->bucketsize = config->histogram_bucketsize;
	}

	ctx->result.overall.min = 10000000;
	ctx->result.overall.max = -10000000;
	ctx->result.overall.avg = 0;
	ctx->result.overall.test_loops = 1;
	ctx->result.overall.overruns = 0;

	ctx->warmup = 1;

	ctx->curr.min = 10000000;
	ctx->curr.max = -10000000;
	ctx->curr.avg = 0;
	ctx->curr.overruns = 0;
	ctx->mode = RTTST_TMBENCH_INVALID;

	rtdm_event_init(&ctx->result_event, 0);

	if (config->mode == RTTST_TMBENCH_TASK) {
		err = rtdm_task_init(&ctx->timer_task, "timerbench",
				timer_task_proc, ctx,
				config->priority, 0);
		if (!err)
			ctx->mode = RTTST_TMBENCH_TASK;
	} else {
		rtdm_timer_init(&ctx->timer, timer_proc,
				rtdm_fd_device(fd)->name);

		ctx->curr.test_loops = 0;

		ctx->mode = RTTST_TMBENCH_HANDLER;

		cobalt_atomic_enter(s);
		ctx->start_time = rtdm_clock_read_monotonic();

		/* first event: one millisecond from now. */
		ctx->date = ctx->start_time + 1000000;

		err = rtdm_timer_start(&ctx->timer, ctx->date, 0,
				RTDM_TIMERMODE_ABSOLUTE);
		cobalt_atomic_leave(s);
	}

	up(&ctx->nrt_mutex);

	return err;
}

static int kernel_copy_results(struct rt_tmbench_context *ctx,
			       struct rttst_overall_bench_res *res)
{
	int size;

	memcpy(&res->result, &ctx->result.overall, sizeof(res->result));

	if (ctx->histogram_size > 0) {
		size = ctx->histogram_size * sizeof(int32_t);
		memcpy(res->histogram_min, ctx->histogram_min, size);
		memcpy(res->histogram_max, ctx->histogram_max, size);
		memcpy(res->histogram_avg, ctx->histogram_avg, size);
		kfree(ctx->histogram_min);
	}

	return 0;
}

static int user_copy_results(struct rt_tmbench_context *ctx,
			     struct rttst_overall_bench_res __user *u_res)
{
	struct rtdm_fd *fd = rtdm_private_to_fd(ctx);
	struct rttst_overall_bench_res res_buf;
	int ret, size;

	ret = rtdm_safe_copy_to_user(fd, &u_res->result,
				     &ctx->result.overall,
				     sizeof(u_res->result));
	if (ret || ctx->histogram_size == 0)
		return ret;

	size = ctx->histogram_size * sizeof(int32_t);

	if (rtdm_safe_copy_from_user(fd, &res_buf, u_res, sizeof(res_buf)) < 0 ||
	    rtdm_safe_copy_to_user(fd, res_buf.histogram_min,
				   ctx->histogram_min, size) < 0 ||
	    rtdm_safe_copy_to_user(fd, res_buf.histogram_max,
				   ctx->histogram_max, size) < 0 ||
	    rtdm_safe_copy_to_user(fd, res_buf.histogram_avg,
				   ctx->histogram_avg, size) < 0)
		return -EFAULT;

	return 0;
}

#ifdef CONFIG_XENO_ARCH_SYS3264

static int compat_user_copy_results(struct rt_tmbench_context *ctx,
				    struct compat_rttst_overall_bench_res __user *u_res)
{
	struct compat_rttst_overall_bench_res res_buf;
	struct rtdm_fd *fd = rtdm_private_to_fd(ctx);
	int ret, size;

	ret = rtdm_safe_copy_to_user(fd, &u_res->result,
				     &ctx->result.overall,
				     sizeof(u_res->result));
	if (ret || ctx->histogram_size == 0)
		return ret;

	size = ctx->histogram_size * sizeof(int32_t);

	if (rtdm_safe_copy_from_user(fd, &res_buf, u_res, sizeof(res_buf)) < 0 ||
	    rtdm_safe_copy_to_user(fd, compat_ptr(res_buf.histogram_min),
				   ctx->histogram_min, size) < 0 ||
	    rtdm_safe_copy_to_user(fd, compat_ptr(res_buf.histogram_max),
				   ctx->histogram_max, size) < 0 ||
	    rtdm_safe_copy_to_user(fd, compat_ptr(res_buf.histogram_avg),
				   ctx->histogram_avg, size) < 0)
		return -EFAULT;

	return 0;
}

#endif /* CONFIG_XENO_ARCH_SYS3264 */

static int rt_tmbench_stop(struct rt_tmbench_context *ctx, void *u_res)
{
	struct rtdm_fd *fd = rtdm_private_to_fd(ctx);
	int ret;

	down(&ctx->nrt_mutex);

	if (ctx->mode < 0) {
		up(&ctx->nrt_mutex);
		return -EINVAL;
	}

	if (ctx->mode == RTTST_TMBENCH_TASK)
		rtdm_task_destroy(&ctx->timer_task);
	else if (ctx->mode == RTTST_TMBENCH_HANDLER)
		rtdm_timer_destroy(&ctx->timer);

	rtdm_event_destroy(&ctx->result_event);

	ctx->mode = RTTST_TMBENCH_INVALID;

	ctx->result.overall.avg =
	    slldiv(ctx->result.overall.avg,
		   ((ctx->result.overall.test_loops) > 1 ?
		    ctx->result.overall.test_loops : 2) - 1);

	if (rtdm_fd_is_user(fd)) {
#ifdef CONFIG_XENO_ARCH_SYS3264
		if (rtdm_fd_is_compat(fd))
			ret = compat_user_copy_results(ctx, u_res);
		else
#endif
			ret = user_copy_results(ctx, u_res);
	} else
		ret = kernel_copy_results(ctx, u_res);

	if (ctx->histogram_size > 0)
		kfree(ctx->histogram_min);

	up(&ctx->nrt_mutex);

	return ret;
}

static int rt_tmbench_ioctl_nrt(struct rtdm_fd *fd,
				unsigned int request, void __user *arg)
{
	struct rt_tmbench_context *ctx;
	int err = 0;

	ctx = rtdm_fd_to_private(fd);

	switch (request) {
	case RTTST_RTIOC_TMBENCH_START:
		err = rt_tmbench_start(fd, ctx, arg);
		break;

	COMPAT_CASE(RTTST_RTIOC_TMBENCH_STOP):
		err = rt_tmbench_stop(ctx, arg);
		break;
	default:
		err = -ENOSYS;
	}

	return err;
}

static int rt_tmbench_ioctl_rt(struct rtdm_fd *fd,
			       unsigned int request, void __user *arg)
{
	struct rt_tmbench_context *ctx;
	int err = 0;

	ctx = rtdm_fd_to_private(fd);

	switch (request) {
	case RTTST_RTIOC_INTERM_BENCH_RES:
		err = rtdm_event_wait(&ctx->result_event);
		if (err)
			return err;

		if (rtdm_fd_is_user(fd)) {
			struct rttst_interm_bench_res __user *user_res = arg;

			err = rtdm_safe_copy_to_user(fd, user_res,
						     &ctx->result,
						     sizeof(*user_res));
		} else {
			struct rttst_interm_bench_res *res = (void *)arg;

			memcpy(res, &ctx->result, sizeof(*res));
		}

		break;

	default:
		err = -ENOSYS;
	}

	return err;
}

static struct rtdm_driver timerbench_driver = {
	.profile_info		= RTDM_PROFILE_INFO(timerbench,
						    RTDM_CLASS_TESTING,
						    RTDM_SUBCLASS_TIMERBENCH,
						    RTTST_PROFILE_VER),
	.device_flags		= RTDM_NAMED_DEVICE,
	.device_count		= 1,
	.context_size		= sizeof(struct rt_tmbench_context),
	.ops = {
		.open		= rt_tmbench_open,
		.close		= rt_tmbench_close,
		.ioctl_rt	= rt_tmbench_ioctl_rt,
		.ioctl_nrt	= rt_tmbench_ioctl_nrt,
	},
};

static struct rtdm_device device = {
	.driver = &timerbench_driver,
	.label = "timerbench",
};

static int __init __timerbench_init(void)
{
	return rtdm_dev_register(&device);
}

static void __timerbench_exit(void)
{
	rtdm_dev_unregister(&device);
}

module_init(__timerbench_init);
module_exit(__timerbench_exit);